[发明专利]一种实现陆用惯性导航系统运动对准的方法

摘要

本发明涉及一种实现陆用惯性导航系统运动对准的方法，属于惯性导航系统初始对准技术领域。本发明在惯性导航系统动态误差模型和观测方程的基础上，以GPS输出速度作为观测量，采用卡尔曼滤波对载车的加加速度进行估计，并判别GPS速度输出是否为野值。若GPS速度输出为野值，采用卡尔曼滤波平滑方法对当前状态进行递推；否则，采用主、从卡尔曼滤波器来实现惯性导航系统状态和噪声统计特性的同步估计，主滤波器对惯性导航系统的状态进行估计，其新息和方差作为从滤波器的观测量；从滤波器对主滤波器观测噪声的均值和方差进行估计，估计结果为下一次主滤波器的滤波提供噪声特性输入；从而实现陆用惯性导航系统的运动对准。

主权项

1.一种实现陆用惯性导航系统运动对准的方法，其特征在于：包括如下步骤：步骤一、建立包括位置误差、速度误差、失准角和惯性器件误差漂移的惯性导航系统动态误差模型；在东北天坐标系下，动态误差模型表示如公式1所示；x·(t)=F(t)x(t)+w(t)---(1)]]>式中，t为时间值，是正实数；x(t)表示惯性导航系统动态误差模型的状态向量，由位置误差δP、速度误差δVⁿ、失准角陀螺仪零偏δ_g和加速度计零偏组成；w(t)表示惯性导航系统动态误差模型的系统噪声；F(t)为转移矩阵；表示惯性导航系统动态误差模型的状态向量的变化量；对该动态误差模型进行离散化处理，可得：x_k＝F_k-1x_k-1+w_k                                           (2)式中，k表示时间值，为正整数；F_k-1表示离散化的惯性导航系统的一步转移矩阵，x_k为离散化后的k时刻惯性导航系统的状态向量，w_k表示k时刻的惯性导航系统动态误差模型的系统噪声，是均值为零，方差为Q的白噪声序列，Q值根据实际应用环境人为设定，Q为正实数；步骤二、建立步骤一所述的惯性导航系统动态误差模型的观测方程，如公式3所示；Yk=VkINS-VkGPS=Hkxk+ηk---(3)]]>式中，Y_k表示k时刻的观测量；表示惯性导航系统的速度输出；表示经杆臂补偿后GPS的速度输出；H_k为k时刻的观测矩阵，H_k＝[O_3×3  I_3×3  O_3×9]；O_3×3表示3阶O矩阵；I_3×3表示3阶单位阵，O_3×9表示3行9列的O矩阵；η_k表示k时刻的惯性导航系统动态误差模型的观测方程噪声；步骤三、对GPS输出进行实时野值检测；通过GPS测量得到的当前时刻载车东向速度和北向速度计算水平合速度Z_k，并以此作为观测量，建立加加速度跟踪模型，如公式4所示；ddtv(t)a(t)a·(t)=010001000v(t)a(t)a·(t)+ζ(t)---(4)]]>式中，t为时间值，为正实数；v(t)表示水平合速度，a(t)和分别表示水平加速度和水平加加速度；ζ(t)表示加加速度跟踪模型的系统噪声，是均值为零，方差为的白噪声序列，值根据实际应用环境人为设定，为正实数；对公式4进行离散化，离散化后的系统方程表示为：vkaka·k=Gk-1vk-1ak-1a·k-1+ζk---(5)]]>式中，v_k表示k时刻水平合速度，a_k和分别表示k时刻水平加速度和水平加加速度，G_k-1表示离散化的加加速度跟踪模型的一步转移矩阵，ζ_k表示离散后加加速度跟踪模型的系统噪声；加加速度跟踪模型的观测方程表示为：Zk=100vkaka·k+ξk---(6)]]>式中，Z_k表示k时刻水平合速度的观测量，其计算公式为：ξ_k表示加加速度跟踪模型的观测方程噪声，是均值为零，方差为的白噪声序列，值根据实际应用环境人为设定，为正实数；根据公式5和公式6，采用卡尔曼滤波器即可估计出k时刻水平合速度v_k、相应的加速度a_k和加加速度和根据载车的类型和对准动态情况，预先设定阈值J_max，如果成立，则判断当前的观测量为有效值，执行步骤四；否则，判断其为野值，在该时刻做卡尔曼平滑，同时将k+1的值赋给k，然后重复步骤三；步骤四、对惯性导航系统动态误差模型的观测方程噪声η_k的均值和方差进行估计；根据步骤二中的惯性导航系统动态误差模型的观测方程得到的k时刻的观测量Y_k以及观测矩阵H_k，采用主、从两个卡尔曼滤波器分别对k时刻惯性导航系统的状态x_k、k时刻的观测噪声η_k的均值和方差进行估计；具体为：和的状态方程为：xk+1R=I100I2xkR+wkR---(7)]]>式中，表示主对角线元素；该状态方程的系统噪声是均值为零，方差为Q_R的白噪声序列，Q_R值根据实际应用环境人为设定，Q_R为正实数；为m₁行1列的向量；I₁为m₁行m₁列的单位阵；为m₂行1列的向量；I₂为m₂行m₂列的单位阵；公式7对应的观测方程为：zkR=Yk-Hkx^kdiag(Ck-HkPk|k-1HkT)---(8)]]>式中，C_k表示残差序列的方差，满足和P_k|k-1的初始值分别为和P₀，均为人为设定值；此后，和P_k|k-1由主卡尔曼滤波器提供；公式8可进一步整理为：zkR=I00IxkR+ηkR---(9)]]>式中，量测噪声是均值为零，方差为R_R的白噪声序列，R_R值根据实际应用环境人为设定，R_R为正实数；根据公式7和公式9，采用从卡尔曼滤波器即可估计出k时刻的观测噪声η_k的均值和方差从卡尔曼滤波器为经典卡尔曼滤波器；步骤五、对惯性导航系统的状态x_k进行估计；根据步骤一建立的惯性导航系统动态误差模型及步骤二中的观测方程，结合步骤四给出的观测噪声η_k的均值和方差的估计值，采用主卡尔曼滤波器对惯性导航系统动态误差模型的状态向量x_k进行估计；具体为：主卡尔曼滤波器为经典卡尔曼滤波器的改进，计算过程如公式10～14所示：x^k|k-1=Fk-1x^k-1---(10)]]>Pk|k-1=Fk-1Pk-1Fk-1T+Qk---(11)]]>Kk=Pk|k-1HkT[HkPk|k-1HkT+x^kR(1)]-1---(12)]]>x^k=x^k|k-1+Kk[Yk-Fk-1x^k|k-1-x^kR(2)]---(13)]]>P_k＝P_k|k-1-K_kH_kP_k|k-1                                (14)其中，表示惯性导航系统动态误差模型的状态向量x_k的一步预测；P_k|k-1表示一步预测方差；表示惯性导航系统动态误差模型的状态向量x_k的估计值；P_k表示估计方差；K_k表示滤波增益；表示步骤四中的估计值；表示的估计值，其主对角线元素等于步骤四中的估计值，非主对角线的元素为0；经过上述步骤即可得到k时刻惯性导航系统动态误差模型状态向量x_k的估计值包含位置误差(δP)_k，速度误差(δVⁿ)_k和失准角利用这些误差估计结果对惯性导航系统的位置输出速度输出和姿态矩阵输出进行校正，通过公式15～17即可得到修正后的载车的位置速度姿态矩阵P~k=PkINS-(δP)k---(15)]]>V~k=VkINS-(δVn)k---(16)]]>其中，和分别表示k时刻惯性导航系统输出的位置、速度和姿态矩阵，是已知量；I_3×3表示3阶单位阵；表示由构成的斜负对称阵；步骤六、对步骤一中的惯性导航系统动态误差模型一步转移矩阵F_k-1进行更新，同时将k+1的值赋给k，然后返回到步骤二。