[发明专利]基于伪距/伪距率的多传感器紧组合列车组合定位方法

权利要求书

1.一种基于伪距/伪距率的多传感器紧组合列车组合定位方法，其特征在于，包括：

由INS根据列车初始位置、速度和姿态信息，结合INS中加速度计和陀螺仪量测的列车加速度和角速率，自身递推计算出列车的位置、速度和姿态信息；

在卫星信号有效环境下，根据INS自身解算的列车位置与GNSS卫星位置计算卫星与列车之间的几何距离，计算GNSS量测的伪距与INS解算的卫星与列车的几何距离之间的伪距差值，计算GNSS量测的伪距率与INS解算的列车的速度之间的伪距率差值，计算INS解算的列车的速度与OVS量测的速度之间的速度差值；

将所述计算出的卫星与列车之间的几何距离，以及所述伪距差值、伪距率差值和速度差值作为系统滤波的量测输入，采用标准卡尔曼滤波的滤波结果修正惯性导航系统的误差，得到融合后的列车位置、速度信息和姿态信息；

还包括：

在卫星信号失效环境下，采用标准卡尔曼滤波算法利用OVS来修正INS系统的误差，系统的量测向量、系统量测矩阵和系统噪声协方差矩阵的计算公式如下：

系统的量测矩阵Z为：

Z＝[v_OVS-v_INS]^T

其中：v_INS表示惯导自身解算速度信息，v_OVS表示在导航坐标系下的OVS量测速度；

系统的量测矩阵H为：

H＝[0_3×3 I_3×3 0_3×9 0_3×1 0_3×1]

惯性导航/光电速度传感器组合系统的量测噪声协方差为：

R＝diag(0.8²,0.8²,0.8²)；

所述标准卡尔曼滤波算法包括：时间更新和量测更新；

时间更新包括状态一步预测和一步预测均方误差的计算，计算公式分别如下：

P(k/k-1)＝F(k-1)P(k-1)F(k-1)^T+Q(k-1)

其中，F(k-1)表示状态转移矩阵，表示k-1时刻的估计状态；表示k时刻的一步预测状态，即惯性导航系统自身递推得到的状态，包括位置、速度、姿态、加速度计和陀螺仪的偏移；Q(k-1)表示系统噪声协方差矩阵，P(k-1)表示k-1时刻估计状态误差的协方差矩阵；

量测更新包括滤波增益、状态估计和估计均方误差的计算，计算公式分别如下：

K(k)＝P(k/k-1)H(k)^T[H(k)P(k/k-1)H(k)^T+R(k)]^-1

P(k)＝[I-K(k)H(k/k-1)]P(k/k-1)

其中，H(k)表示系统的量测矩阵，R(k)表示系统量测噪声协方差矩阵，K(k)表示滤波增益，P(k)表示状态估计的均方误差，表示估计的状态，即为融合后的列车状态，由一步预测状态和量测更新得到的状态两部分组成，列车的状态包括列车的位置、速度、姿态、加速度计和陀螺仪的偏移。