CN115265588A - 陆用捷联惯导基于逆向导航的零速修正在线平滑方法 - Google Patents

Abstract

本发明公开了一种陆用捷联惯导基于逆向导航的零速修正在线平滑方法，步骤为：S1、对陆用捷联惯导实时输出的原始数据进行正向导航解算与零速修正；S2、将陆用捷联惯导的原始数据保存在存储器中，在停车阶段，对保存的陆用捷联惯导的原始数据进行逆向导航解算与零速修正；S3、采用固定区域最优平滑估计方法，对逆向导航零速修正的卡尔曼滤波器状态量中的导航误差进行处理，估算得到当前时刻导航误差并进行导航误差补偿；该方法充分发挥逆向导航、零速修正和数据平滑三种方法的优点，提供一种实时性好、能充分抑制导航误差且能在线处理的陆用捷联惯导基于逆向导航的零速修正在线平滑方法。

Description

陆用捷联惯导基于逆向导航的零速修正在线平滑方法

技术领域

本发明涉及陆用捷联惯导导航误差抑制技术领域，特别涉及一种陆用捷联惯导基于逆向导航的零速修正在线平滑方法。

背景技术

陆用捷联惯导能够自主地提供实时的包括姿态、位置、速度在内的全方位导航信息，被广泛应用于陆地作战车辆的实时导航中，如测地车、自行火炮、远程火箭炮、侦察车等陆地车辆。对于增强地面部队的机动能力、生存能力、快速反应能力、目标捕获能力、协同作战能力以及远程精确打击能力都具有重要的作用和意义。

然而，受陆用捷联惯导积分式导航解算原理和惯性器件不可避免的零偏误差的影响，陆用捷联惯导的导航误差随着导航时间增加而增加。为了抑制陆用捷联惯导的导航误差，目前常用的方法有逆向导航、零速修正和数据平滑。逆向导航的原理是将整个导航过程的原始数据存储下来并对存储的数据按时间逆序处理，在实时性无要求的情况下，通过逆向导航有利于提高导航精度，例如，授权发明专利CN111024065B公开了一种用于最优估计精对准的严格逆向导航方法，但该方法无法实时地对导航误差进行抑制。零速修正的原理是充分利用捷联惯导在停车时其速度真实值为零作为观测值对导航误差进行估计和补偿，例如，授权发明专利CN105806340B提供了一种基于窗口平滑的自适应零速修正算法，但该方法只能在停车时候进行修正，对导航误差抑制有限。数据平滑的原理是通过最后停车点的位置信息利用平滑算法离线对全程的数据进行平滑以提高导航精度，但该方法必须离线进行。

综上，现有的陆用捷联惯导导航误差抑制方法存在着实时性差、对全程导航误差抑制有限和需要离线处理的缺点。

发明内容

本发明的目的是提供一种解决现有的陆用捷联惯导导航误差抑制方法存在的实时性差、对导航误差抑制有限和需要离线处理的缺点的陆用捷联惯导基于逆向导航的零速修正在线平滑方法。

为此，本发明技术方案如下：

一种陆用捷联惯导基于逆向导航的零速修正在线平滑方法，步骤如下：

S1、对陆用捷联惯导实时输出的原始数据进行正向导航解算与零速修正；该步骤的具体实施过程为：

S101、采用适合于计算机解算的离散化递推算法对陆用捷联惯导实时输出的原始数据进行正向导航解算；

S102、构建用于陆用捷联惯导正向导航解算的零速修正卡尔曼滤波器，包括：

①定义用于正向导航零速修正卡尔曼滤波器的状态量X¹⁵：

式中，

为陆用捷联惯导的姿态误差，δV为陆用捷联惯导的速度误差，δP为陆用捷联惯导的位置误差，ε为陆用捷联惯导中的陀螺零偏误差，

为陆用捷联惯导中的加速度计零偏误差；

②建立用于正向导航零速修正卡尔曼滤波器的状态方程：

式中，

F¹¹、F¹²、F¹³、F²¹、F²²、F¹³、F³²、F³³为非零矩阵元素，其具体表达式为：

其中，L为当地地理纬度，λ为当地地理经度，h为当地地理高度；V_E、V_N和V_U分别为陆用捷联惯导的东向速度、北向速度和天向速度；R_M和R_N分别为当地地球子午圈半径和卯酉圈半径；ω_ie为地球自转角速率；

和

分别为陆用捷联惯导测量的东向比力、北向比力和天向比力；

为陆用捷联惯导的姿态矩阵；G¹⁵为测量噪声输入矩阵，其表达式为：

u为测量噪声，其表达式为：

其中，u_g为陀螺的测量噪声，u_g＝[u_gx u_gy u_gz]^T，u_gx为X向陀螺的测量噪声，u_gy为Y向陀螺的测量噪声，u_gz为Z向陀螺的测量噪声；u_a为加速度计的测量噪声，u_a＝[u_ax u_ay u_az]^T，u_ax为X向加速度计的测量噪声、u_ay为Y向加速度计的测量噪声，u_az为Z向加速度计的测量噪声；

③建立用于正向导航零速修正的卡尔曼滤波器的观测方程：Z¹⁵＝H¹⁵X¹⁵+v，式中，Z¹⁵＝[0_1×3 V^T 0_1×9]^T为正向导航零速修正的观测向量，V＝[V_E V_N V_U]^T为正向导航零速修正的速度向量；v为观测噪声；H¹⁵为观测矩阵，H¹⁵＝[0_3×3 I₃ 0_3×9]，I₃为三行三列的单位矩阵，即

0_3×3为三行三列的零矩阵，0_3×9为三行九列的零矩阵；

S103、利用步骤S102构建的用于正向导航零速修正的卡尔曼滤波器对正向导航结果的导航误差和惯性器件误差中的陀螺零偏误差和加速度计零偏误差进行实时预测，包括：状态1：在陆用运载体开车前的静止过程且陆用捷联惯导处于对准阶段，利用步骤S102设计的正向导航零速修正的卡尔曼滤波器实时解算得到陆用捷联惯导对准阶段下的正向导航零速修正的卡尔曼滤波器的状态量；状态2：在陆用运载体行驶的过程中且陆用捷联惯导处于导航阶段，利用步骤S102设计的正向导航零速修正的卡尔曼滤波器实时解算得到陆用运载体行驶中陆用捷联惯导导航阶段下的正向导航零速修正的卡尔曼滤波器的状态量；状态3：在陆用运载体开车后的暂停行驶的过程中且陆用捷联惯导处于导航阶段，利用步骤S102设计的卡正向导航零速修正的卡尔曼滤波器实时解算得到陆用运载体开车后暂停行驶中陆用捷联惯导导航阶段下的正向导航零速修正的卡尔曼滤波器的状态量；

S104、将不同阶段下获得的正向导航零速修正的卡尔曼滤波器的状态量用于正向导航的导航误差的补偿；补偿1：将经过步骤S103的状态1下得到的正向导航零速修正的卡尔曼滤波器的状态量中的导航误差补偿至步骤S101实时输出的正向导航结果中，得到经过正向导航零速修正的导航结果；将经过步骤S103的状态1下得到的正向导航零速修正的卡尔曼滤波器的状态量中的惯性器件误差中的陀螺零偏误差和加速度计零偏误差则以写入陆用捷联惯导上导航计算机板上内嵌的惯性导航解算方程中的方式进行补偿；补偿2：将经过步骤S103的状态3下得到的正向导航零速修正的卡尔曼滤波器的状态量中的导航误差补偿至步骤S101实时输出的正向导航结果中，即得到经过正向导航零速修正的导航结果；将经过步骤S103的状态3下得到的正向导航零速修正的卡尔曼滤波器的状态量中的惯性器件误差中的陀螺零偏误差和加速度计零偏误差则以写入陆用捷联惯导上导航计算机板上内嵌的惯性导航解算方程中的方式进行补偿；

S2、将陆用捷联惯导的原始数据保存在存储器中，在停车阶段，对保存的陆用捷联惯导的原始数据进行逆向导航解算与零速修正；该步骤的具体实施过程为：

S201、保存的陆用捷联惯导的原始数据，包括陀螺仪组件测量的角速率和由加速度计组件测量的比力；

S202、以步骤S1得到的当前时刻正向导航经零速修正的导航结果作为逆向导航解算的初值，采用适合于计算机解算的离散化逆向递推算法，对当前时刻之前所有时刻的原始数据进行逆向导航解算；

S203、构建用于陆用捷联惯导逆向导航解算的零速修正卡尔曼滤波器，包括：

①定义用于逆向导航零速修正卡尔曼滤波器的状态量X′¹⁵：

式中，δV_r为逆向速度误差，δV_r＝[δV_Er δV_Nr δV_Ur]^T，δV_Er、δV_Nr和δV_Ur分别为逆向东向速度误差、逆向北向速度误差和逆向天向速度误差；

②建立用于逆向导航零速修正卡尔曼滤波器的状态方程：

其中，

F′¹⁵矩阵中：

其中，L为当地地理纬度，λ为当地地理经度，h为当地地理高度；V_Er、V_Nr和V_Ur分别为陆用捷联惯导逆向导航的东向速度、北向速度和天向速度；R_M和R_N分别为当地地球子午圈半径和卯酉圈半径；ω_ie为地球自转角速率；

和

分别为陆用捷联惯导测量的东向比力、北向比力和天向比力；

为陆用捷联惯导的姿态矩阵；G′¹⁵为逆向导航零速修正测量噪声输入矩阵，其表达式为：

u为测量噪声，其表达式为：

其中，u_g为陀螺的测量噪声，u_g＝[u_gx u_gy u_gz]^T，u_gx为X向陀螺的测量噪声，u_gy为Y向陀螺的测量噪声，u_gz为Z向陀螺的测量噪声；u_a为加速度计的测量噪声，u_a＝[u_ax u_ay u_az]^T，u_ax为X向加速度计的测量噪声、u_ay为Y向加速度计的测量噪声，u_az为Z向加速度计的测量噪声；

③建立用于逆向导航零速修正的卡尔曼滤波器的观测方程：Z′¹⁵＝H′¹⁵X′¹⁵+v，式中，Z′¹⁵＝[0_1×3 V_r ^T 0_1×9]^T为逆向导航零速修正的观测向量，V_r＝[V_Er V_Nr V_Ur]^T为逆向导航零速修正的速度向量；v为观测噪声；H^′15为逆向导航零速修正的观测矩阵，H^′15＝[0_3×3 I₃0_3×9]，I₃为三行三列的单位矩阵，即

0_3×3为三行三列的零矩阵，0_3×9为三行九列的零矩阵；

S203、利用构建的逆向导航零速修正的卡尔曼滤波器对逆向导航结果的导航误差和惯性器件误差中的陀螺零偏误差和加速度计零偏误差进行预测，包括：状态1：对于陆用运载体开车前的静止过程且陆用捷联惯导处于对准阶段，利用步骤S202设计的逆向导航零速修正的卡尔曼滤波器离线解算得到陆用捷联惯导对准阶段下的逆向导航零速修正的卡尔曼滤波器的状态量；状态2：对于陆用运载体行驶的过程中且陆用捷联惯导处于导航阶段，利用步骤S202设计的逆向导航零速修正的卡尔曼滤波器离线解算得到陆用运载体行驶中陆用捷联惯导导航阶段下的逆向导航零速修正的卡尔曼滤波器的状态量；状态3：对于陆用运载体开车后的暂停行驶的过程中且陆用捷联惯导处于导航阶段，利用步骤S202设计的用于逆向导航零速修正的卡尔曼滤波器离线解算得到陆用运载体开车后暂停行驶中陆用捷联惯导导航阶段下的逆向导航零速修正的卡尔曼滤波器的状态量；

S3、采用固定区域最优平滑估计方法，对由步骤S2得到的逆向导航零速修正的卡尔曼滤波器状态量中的导航误差进行处理，估算得到当前时刻导航误差并进行导航误差补偿；该步骤的具体实施过程为：

S301、对当前时刻之前所有时刻的逆向导航零速修正的卡尔曼滤波器状态量的导航误差进行固定区域最优平滑估计，得到当前时刻下经过逆向导航零速修正的导航误差的推算值；

S302、在由步骤S1得到的当前时刻下经过正向导航零速修正后的导航结果中扣除由步骤S301得到的当前时刻下经过逆向导航零速修正的导航误差的推算值，得到最终导航结果。

进一步地，步骤S101中正向导航解算方法如下：

L_k＝L_k-1+T_sV_Nk-1/(R_M+h_k-1)，

λ_k＝λ_k-1+T_sV_Ek-1secL_k-1/(R_N+h_k-1)，

h_k＝h_k-1+T_sV_Uk-1，

其中，

gⁿ＝[0 0 -g]^T，

式中，T_s为原始数据的实时采样周期；k为离散化的时刻；每个右下角带有符号k的物理量表示为k时刻下该物理量的状态值，每个右下角带符号k-1的物理量表示为k-1时刻下该物理量的状态值；

V，L，λ，h为导航解算结果，

为陆用捷联惯导的姿态矩阵；V为陆用捷联惯导在导航坐标系下的速度向量，V＝[V_E V_N V_U]^T，V_E、V_N和V_U分别为陆用捷联惯导的东向速度、北向速度和天向速度；L、λ和h分别为陆用捷联惯导在地球表面的纬度、经度和高度；

和f^m为原始数据，

为陆用捷联惯导中陀螺仪组件测量的角速率原始数据，f^m为陆用捷联惯导中加速度计组件测量的比力原始数据；ω_ie和g分别为地球自转角速度和重力加速度；R_M和R_N分别为当地地球的子午圈半径和卯酉圈半径。

进一步地，步骤S202中逆向导航解算方法如下：

L_k-1＝L_k+T_sV_Nrk/(R_M+h_k)，

λ_k-1＝λ_k+T_sV_Erk secL_k/(R_N+h_k)，

h_k-1＝h_k+T_sV_Urk，

其中，

式中，V_r为陆用捷联惯导在导航坐标系下的逆向速度向量，V_r＝[V_Er V_Nr V_Ur]^T，V_Er、V_Nr和V_Ur分别为陆用捷联惯导的逆向东向速度、逆向北向速度和逆向天向速度；其它符号表征的物理量与步骤S101中的定义一致：

V_r，L，λ，h为逆向导航解算结果，

为陆用捷联惯导的姿态矩阵；V_r为陆用捷联惯导在导航坐标系下逆向导航的速度向量，V_r＝[V_Er V_NrV_Ur]^T，V_Er、V_Nr和V_Ur分别为陆用捷联惯导逆向导航的东向速度、北向速度和天向速度；L、λ和h分别为陆用捷联惯导在地球表面的纬度、经度和高度；

和f^m为原始数据，

为陆用捷联惯导中陀螺仪组件测量的角速率原始数据，f^m为陆用捷联惯导中加速度计组件测量的比力原始数据；ω_ie和g分别为地球自转角速度和重力加速度；R_M和R_N分别为当地地球的子午圈半径和卯酉圈半径。

进一步地，步骤S301中，采用固定区域最优平滑估计方法推算得到当前时刻导航误差估计值的具体操作方法：

设当前时刻为k时刻，则在逆向导航零速修正的卡尔曼滤波器的工作过程中，k时刻下的状态量为

状态一步预测阵为

状态误差均方差阵为P_k，一步预测误差均方差阵为P_k/k-1；

记在k时刻前进行逆向导航中解算的有M个时刻，以j表示逆向导航过程中的任一时刻，则固定区域最优平滑估计的迭代方程的表达式为：

式中，

为由第M时刻的状态量推算得到的第j时刻的状态量，

为由第j-1时刻的状态量推算得到的第j时刻的状态量，A_j-1为构造的j-1时刻的中间矩阵，

为第j-1时刻的状态量，

为由第M时刻的状态量推算得到的第j-1时刻的状态量，P_j-1为第j-1时刻的状态误差均方差阵，

为第j-1时刻到第j时刻的状态一步转移矩阵的转置矩阵，

为P_j/j-1的逆矩阵，P_j-1/M为第M时刻到第j-1时刻的一步预测误差均方差阵，P_j/M为第M时刻到第j时刻的一步预测误差均方差阵，P_j/j-1为第j-1时刻到第j时刻的一步预测误差均方差阵，

为A_j-1的转置矩阵；

令j＝M,M-1,…,1，通过上述固定区域最优平滑估计的迭代方程得到停车时正向导航经零速修正的当前时刻的逆向导航零速修正滤波器状态量在当前时刻的固定区域最优平滑估计

即停车时正向导航经零速修正的当前时刻的逆向零速修正滤波器估计的状态量中的导航误差。

与现有技术相比，该陆用捷联惯导基于逆向导航的零速修正在线平滑方法解决了现有的陆用捷联惯导导航误差抑制方法存在的实时性差、对导航误差抑制有限和需要离线处理的缺点，充分发挥逆向导航、零速修正和数据平滑三种方法的优点，提供一种实时性好、能充分抑制导航误差且能在线处理的陆用捷联惯导基于逆向导航的零速修正在线平滑方法。

附图说明

图1为本发明的陆用捷联惯导基于逆向导航的零速修正在线平滑方法的流程示意图；

图2(a)为本发明实施例中陆用捷联惯导利用本发明提出的基于逆向导航的零速修正在线平滑方法进行导航误差抑制前的位置误差示意图；

图2(b)为本发明实施例中陆用捷联惯导利用本发明提出的基于逆向导航的零速修正在线平滑方法进行导航误差抑制后的实时位置误差示意图。

具体实施方式

下面结合附图及具体实施例对本发明做进一步的说明，但下述实施例绝非对本发明有任何限制。

如图1所示，该陆用捷联惯导基于逆向导航的零速修正在线平滑方法的具体实施步骤如下：

S1、对陆用捷联惯导实时输出的原始数据进行正向导航解算与零速修正；

具体地，该步骤S1的实施步骤为：

S101、对陆用捷联惯导实时输出的原始数据进行正向导航解算；

具体地，对实时输出的原始数据采用适合于计算机解算的离散化递推算法进行正向导航解算：

L_k＝L_k-1+T_sV_Nk-1/(R_M+h_k-1)，

λ_k＝λ_k-1+T_sV_Ek-1secL_k-1/(R_N+h_k-1)，

h_k＝h_k-1+T_sV_Uk-1，

其中，

gⁿ＝[0 0 -g]^T，

式中，T_s为原始数据的实时采样周期；k为离散化的时刻；每个右下角带有符号k的物理量表示为k时刻下该物理量的状态值，每个右下角带符号k-1的物理量表示为k-1时刻下该物理量的状态值；

V，L，λ，h为导航解算结果，

为陆用捷联惯导的姿态矩阵；V为陆用捷联惯导在导航坐标系下的速度向量，V＝[V_E V_N V_U]^T，V_E、V_N和V_U分别为陆用捷联惯导的东向速度、北向速度和天向速度；L、λ和h分别为陆用捷联惯导在地球表面的纬度、经度和高度；

和f^m为原始数据，

为陆用捷联惯导中陀螺仪组件测量的角速率原始数据，f^m为陆用捷联惯导中加速度计组件测量的比力原始数据；ω_ie和g分别为地球自转角速度和重力加速度；R_M和R_N分别为当地地球的子午圈半径和卯酉圈半径；

因此，当k≥1时，由陆用捷联惯导在k时刻输出的实时原始数据

和

进行正向导航解算获得k时刻正向导航解算结果

V_k、L_k、λ_k和h_k；导航解算结果在k＝0时的初值由陆用捷联惯导的初始对准过程得到；

S102、构建用于陆用捷联惯导正向导航解算的零速修正卡尔曼滤波器；

具体地，步骤S102的实施步骤如下：

S1021、定义用于正向导航零速修正卡尔曼滤波器的状态量：

基于陆用捷联惯导的导航误差包括位置误差、速度误差和姿态误差；导航误差来源于惯性器件误差，其中，惯性器件误差中的陀螺零偏误差和加速度计零偏误差是影响陆用捷联惯导精度最重要的误差，因此，构建以导航误差和惯性器件误差中的陀螺零偏误差和加速度计零偏误差为集合的向量作为正向导航零速修正卡尔曼滤波器的状态量；

定义正向导航零速修正卡尔曼滤波器的状态量为一个十五维的向量，即X¹⁵，相应表达式为：

式中，X¹⁵为所构建的十五维正向导航零速修正卡尔曼滤波器状态量；

为陆用捷联惯导的姿态误差，

和

分别为陆用捷联惯导的东向误差角、北向误差角和天向误差角；δV为陆用捷联惯导的速度误差，δV＝[δV_E δV_N δV_U]^T，δV_E、δV_N和δV_U分别为陆用捷联惯导的东向速度误差、北向速度误差和天向速度误差；δP为陆用捷联惯导的位置误差，δP＝[δL δλ δh]^T，δL、δλ和δh分别为陆用捷联惯导的纬度误差、经度误差和高度误差；ε为陆用捷联惯导中的陀螺零偏误差，ε＝[ε_x ε_y ε_z]^T，ε_x、ε_y和ε_z分别为陆用捷联惯导中的X向陀螺零偏误差、Y向陀螺零偏误差和Z向陀螺零偏误差；

为陆用捷联惯导中的加速度计零偏误差，

和

分别为陆用捷联惯导中的X向加速度计零偏误差、Y向加速度计零偏误差和Z向加速度计零偏误差；

S1022、建立用于正向导航零速修正卡尔曼滤波器的状态方程：

根据步骤S1021构建的十五维正向导航零速修正卡尔曼滤波器状态量、以及已知的惯导误差方程，建立卡尔曼滤波器的状态方程为：

式中，

在F¹⁵中，F¹¹、F¹²、F¹³、F²¹、F²²、F¹³、F³²、F³³为非零矩阵元素，其具体表达式如下：

其中，L为当地地理纬度，λ为当地地理经度，h为当地地理高度；V_E、V_N和V_U分别为陆用捷联惯导的东向速度、北向速度和天向速度；R_M和R_N分别为当地地球子午圈半径和卯酉圈半径；ω_ie为地球自转角速率；

和

分别为陆用捷联惯导测量的东向比力、北向比力和天向比力；

为陆用捷联惯导的姿态矩阵；

G¹⁵为测量噪声输入矩阵，其表达式为：

u为测量噪声，其表达式为：

其中，u_g为陀螺的测量噪声，u_g＝[u_gxu_gy u_gz]^T，u_gx为X向陀螺的测量噪声，u_gy为Y向陀螺的测量噪声，u_gz为Z向陀螺的测量噪声；u_a为加速度计的测量噪声，u_a＝[u_ax u_ay u_az]^T，u_ax为X向加速度计的测量噪声、u_ay为Y向加速度计的测量噪声，u_az为Z向加速度计的测量噪声；

S1023、建立用于正向导航零速修正的卡尔曼滤波器的观测方程：

正向导航零速修正的卡尔曼滤波器的观测方程为：

Z¹⁵＝H¹⁵X¹⁵+v，

式中，Z¹⁵＝[0_1×3 V^T 0_1×9]^T为正向导航零速修正的观测向量，V＝[V_E V_N V_U]^T为正向导航零速修正的速度向量；v为观测噪声；H¹⁵为观测矩阵，由于选用陆用捷联惯导的速度解算结果作为观测量，因此，H¹⁵的表达式为：

H¹⁵＝[0_3×3 I₃ 0_3×9]，

式中，I₃为三行三列的单位矩阵，即

0_3×3为三行三列的零矩阵，0_3×9为三行九列的零矩阵；

S103、利用步骤S102构建的用于正向导航零速修正的卡尔曼滤波器对正向导航结果的导航误差和惯性器件误差中的陀螺零偏误差和加速度计零偏误差进行实时预测，包括：

状态1：在陆用运载体开车前的静止过程且陆用捷联惯导处于对准阶段，利用步骤S102设计的正向导航零速修正的卡尔曼滤波器实时解算得到陆用捷联惯导对准阶段下的正向导航零速修正的卡尔曼滤波器的状态量；

状态2：在陆用运载体行驶的过程中且陆用捷联惯导处于导航阶段，利用步骤S102设计的正向导航零速修正的卡尔曼滤波器实时解算得到陆用运载体行驶中陆用捷联惯导导航阶段下的正向导航零速修正的卡尔曼滤波器的状态量；

状态3：在陆用运载体开车后的暂停行驶的过程中且陆用捷联惯导处于导航阶段，利用步骤S102设计的卡正向导航零速修正的卡尔曼滤波器实时解算得到陆用运载体开车后暂停行驶中陆用捷联惯导导航阶段下的正向导航零速修正的卡尔曼滤波器的状态量；

S104、将不同阶段下获得的正向导航零速修正的卡尔曼滤波器的状态量用于正向导航的导航误差的补偿；

补偿1：将经过步骤S103的状态1下得到的正向导航零速修正的卡尔曼滤波器的状态量中的导航误差(位置误差、速度误差和姿态误差)补偿至步骤S101实时输出的正向导航结果(位置、速度和姿态)中，得到经过正向导航零速修正的导航结果；与此同时，将经过步骤S103的状态1下得到的正向导航零速修正的卡尔曼滤波器的状态量中的惯性器件误差中的陀螺零偏误差和加速度计零偏误差则以写入陆用捷联惯导上导航计算机板上内嵌的惯性导航解算方程中的方式进行补偿；

补偿2：将经过步骤S103的状态3下得到的正向导航零速修正的卡尔曼滤波器的状态量中的导航误差(位置误差、速度误差和姿态误差)补偿至步骤S101实时输出的正向导航结果(位置、速度和姿态)中，即得到经过正向导航零速修正的导航结果；与此同时，将经过步骤S103的状态3下得到的正向导航零速修正的卡尔曼滤波器的状态量中的惯性器件误差中的陀螺零偏误差和加速度计零偏误差则以写入陆用捷联惯导上导航计算机板上内嵌的惯性导航解算方程中的方式进行补偿；

其中，正向导航零速修正的卡尔曼滤波器状态量的导航误差直接补导航误差是直接补到陆用捷联惯导的导航输出结果，又称为即时补偿；而正向导航零速修正的卡尔曼滤波器状态量的惯性器件误差中的陀螺零偏误差和加速度计零偏误差则是写入陆用捷联惯导上导航计算机板上内嵌的惯性导航解算方程，在陆用运载体重新回到行驶状态下，陆用捷联惯导的导航结果由于是采用补偿了惯性器件误差中的陀螺零偏误差和加速度计零偏误差后的惯性导航解算方程计算的，因此也得到了补偿，该种补偿方式又称为长久补偿；而经过步骤S103的状态2得到的正向导航零速修正的卡尔曼滤波器的状态量中的导航误差和惯性器件误差中的陀螺零偏误差和加速度计零偏误差不用于补偿；

S2、将陆用捷联惯导的原始数据保存在存储器中，并在停车阶段，对保存的原始数据进行逆向导航解算与零速修正；

具体地，步骤S2的实施步骤为：

S201、将陆用捷联惯导的原始数据保存在存储器中，原始数据包括由陀螺仪组件测量的角速率和由加速度计组件测量的比力；

S202、以步骤S1得到的当前时刻正向导航经零速修正的导航结果作为逆向导航解算的初值，对步骤S201中保存在存储器内的原始数据进行逆向导航解算；

具体地，在该步骤中，逆向导航解算为采用适合于计算机解算的离散化逆向递推算法对步骤S201保存的原始数据进行解算：

L_k-1＝L_k+T_sV_Nrk/(R_M+h_k)，

λ_k-1＝λ_k+T_sV_Erk secL_k/(R_N+h_k)，

h_k-1＝h_k+T_sV_Urk，

其中，

式中，V_r为陆用捷联惯导在导航坐标系下的逆向速度向量，V_r＝[V_Er V_Nr V_Ur]^T，V_Er、V_Nr和V_Ur分别为陆用捷联惯导的逆向东向速度、逆向北向速度和逆向天向速度；其它符号表征的物理量与步骤S101中的定义一致：

V_r，L，λ，h为逆向导航解算结果，

为陆用捷联惯导的姿态矩阵；V_r为陆用捷联惯导在导航坐标系下逆向导航的速度向量，V_r＝[V_Er V_NrV_Ur]^T，V_Er、V_Nr和V_Ur分别为陆用捷联惯导逆向导航的东向速度、北向速度和天向速度；L、λ和h分别为陆用捷联惯导在地球表面的纬度、经度和高度；

和f^m为原始数据，

为陆用捷联惯导中陀螺仪组件测量的角速率原始数据，f^m为陆用捷联惯导中加速度计组件测量的比力原始数据；ω_ie和g分别为地球自转角速度和重力加速度；R_M和R_N分别为当地地球的子午圈半径和卯酉圈半径；

其中，逆向导航解算结果在k＝0时的初值由陆用捷联惯导在停车状态开始逆向导航时刻的正向导航经零速修正后的导航结果赋予，即经过步骤S1得到当前时刻的正向导航经零速修正的导航数据；

S203、构建用于陆用捷联惯导逆向导航解算的零速修正卡尔曼滤波器；

具体地，该步骤S203的实施方式为：

S2031、定义用于逆向导航零速修正卡尔曼滤波器的状态量；

由于逆向导航算法中的姿态和位置的定义与正向导航的相同，但逆向导航的速度的定义是与正向导航的定义在符号上相反的，记为逆向速度V_r，因此，将步骤S1021中构建的正向导航零速修正卡尔曼滤波器的状态量中的δV替换为δV_r，即得到逆向导航零速修正卡尔曼滤波器的状态量：

式中，δV_r为逆向速度误差，δV_r＝[δV_Er δV_Nr δV_Ur]^T，δV_Er、δV_Nr和δV_Ur分别为逆向东向速度误差、逆向北向速度误差和逆向天向速度误差；

S2032、建立用于逆向导航零速修正卡尔曼滤波器的状态方程：

逆向导航零速修正卡尔曼滤波器的状态方程：

其中，

F′¹⁵矩阵中：

其中，L为当地地理纬度，λ为当地地理经度，h为当地地理高度；V_Er、V_Nr和V_Ur分别为陆用捷联惯导逆向导航的东向速度、北向速度和天向速度；R_M和R_N分别为当地地球子午圈半径和卯酉圈半径；ω_ie为地球自转角速率；

和

分别为陆用捷联惯导测量的东向比力、北向比力和天向比力；

为陆用捷联惯导的姿态矩阵；

G′¹⁵为逆向导航零速修正测量噪声输入矩阵，其表达式为：

u为测量噪声，其表达式为：

其中，u_g为陀螺的测量噪声，u_g＝[u_gxu_gy u_gz]^T，u_gx为X向陀螺的测量噪声，u_gy为Y向陀螺的测量噪声，u_gz为Z向陀螺的测量噪声；u_a为加速度计的测量噪声，u_a＝[u_ax u_ay u_az]^T，u_ax为X向加速度计的测量噪声、u_ay为Y向加速度计的测量噪声，u_az为Z向加速度计的测量噪声；

S2033、建立用于逆向导航零速修正的卡尔曼滤波器的观测方程：

用于逆向导航零速修正的卡尔曼滤波器的观测方程为：

Z′¹⁵＝H′¹⁵X′¹⁵+v

式中，Z′¹⁵＝[0_1×3 V_r ^T 0_1×9]^T为逆向导航零速修正的观测向量，V_r＝[V_Er V_Nr V_Ur]^T为逆向导航零速修正的速度向量；v为观测噪声；H^′15为逆向导航零速修正的观测矩阵，H^′15的表达式为：

H′¹⁵＝[0_3×3 I₃ 0_3×9]，

式中，I₃为三行三列的单位矩阵，即

0_3×3为三行三列的零矩阵，0_3×9为三行九列的零矩阵；

S203、利用构建的逆向导航零速修正的卡尔曼滤波器对逆向导航结果的导航误差和惯性器件误差中的陀螺零偏误差和加速度计零偏误差进行预测；

步骤S203与步骤S103的操作思路一致，二者区别仅在于在步骤S203中，卡尔曼滤波器采用步骤S202设计的用于逆向导航零速修正的卡尔曼滤波器；

具体地，步骤S203的具体实施方式为：

状态1：对于陆用运载体开车前的静止过程且陆用捷联惯导处于对准阶段，利用步骤S202设计的逆向导航零速修正的卡尔曼滤波器离线解算得到陆用捷联惯导对准阶段下的逆向导航零速修正的卡尔曼滤波器的状态量；

状态2：对于陆用运载体行驶的过程中且陆用捷联惯导处于导航阶段，利用步骤S202设计的逆向导航零速修正的卡尔曼滤波器离线解算得到陆用运载体行驶中陆用捷联惯导导航阶段下的逆向导航零速修正的卡尔曼滤波器的状态量；

状态3：对于陆用运载体开车后的暂停行驶的过程中且陆用捷联惯导处于导航阶段，利用步骤S202设计的用于逆向导航零速修正的卡尔曼滤波器离线解算得到陆用运载体开车后暂停行驶中陆用捷联惯导导航阶段下的逆向导航零速修正的卡尔曼滤波器的状态量；

S3、对由步骤S3得到的逆向导航零速修正的卡尔曼滤波器状态量中的导航误差进行固定区域最优平滑估计，以得到对当前时刻的导航误差并进行导航误差补偿；

具体地，该步骤S3的实施方式如下：

S301、对逆向导航零速修正的卡尔曼滤波器状态量中的导航误差进行固定区域最优平滑估计，以得到逆向导航零速修正的导航误差在当前时刻的推算值；

在该步骤中，由于逆向导航零速修正的卡尔曼滤波器无法估计停车时刻的导航误差，即无法进行实时的误差补偿；因此，首先通过对逆向导航零速修正的卡尔曼滤波器估计的状态量中的导航误差进行固定区域最优平滑估计，获得逆向导航零速修正估计的导航误差在当前时刻的推算值，进而利用推算值再进行当前时刻的导航误差补偿；

该固定区域最优平滑估计的具体操作的具体实施步骤如下：

设当前时刻为k时刻，则在逆向导航零速修正的卡尔曼滤波器的工作过程中，k时刻下的状态量为

状态一步预测阵为

状态误差均方差阵为P_k，一步预测误差均方差阵为P_k/k-1；

记在k时刻前进行逆向导航中解算的有M个时刻，以j表示逆向导航过程中的任一时刻，则固定区域最优平滑估计的迭代方程的表达式为：

式中，

为由第M时刻的状态量推算得到的第j时刻的状态量，

为由第j-1时刻的状态量推算得到的第j时刻的状态量，A_j-1为构造的j-1时刻的中间矩阵，

为第j-1时刻的状态量，

为由第M时刻的状态量推算得到的第j-1时刻的状态量，P_j-1为第j-1时刻的状态误差均方差阵，

为第j-1时刻到第j时刻的状态一步转移矩阵的转置矩阵，

为P_j/j-1的逆矩阵，P_j-1/M为第M时刻到第j-1时刻的一步预测误差均方差阵，P_j/M为第M时刻到第j时刻的一步预测误差均方差阵，P_j/j-1为第j-1时刻到第j时刻的一步预测误差均方差阵，

为A_j-1的转置矩阵；

令j＝M,M-1,…,1，通过上述固定区域最优平滑估计的迭代方程得到停车时正向导航经零速修正的当前时刻的逆向导航零速修正滤波器状态量在当前时刻的固定区域最优平滑估计

即停车时正向导航经零速修正的当前时刻的逆向零速修正滤波器估计的状态量中的导航误差；

S302、在经过步骤S1得到的对当前时刻下经过正向导航零速修正后的导航结果(位置、速度和姿态)中扣除由步骤S301得到的当前时刻下经过逆向导航零速修正的导航误差的推算值(包括位置误差、速度误差和姿态误差)，得到最终导航结果，即综合利用逆向导航、零速修正和固定区域最优平滑估计的当前时刻的补偿后的导航结果。

综上所述，采用本申请的方法，通过在每次停车阶段均依次进行如上所述的逆向导航解算、零速修正和固定区域最优平滑估计，即能够使陆用捷联惯导输出位置解算结果的误差得到大幅的减小，实现导航精度的大幅提高。

为验证本发明提出的陆用捷联惯导基于逆向导航的零速修正在线平滑方法的正确性和有效性，选用一套陆用捷联惯导进行了车载试验，选用的陆用捷联惯导的惯性器件由三个零偏稳定性为0.003°/h的激光陀螺仪和三个零偏稳定性为10μg的加速度计组成，原始数据的采样频率为100Hz；在试验车上安装选用的陆用捷联惯导以及一套GPS。

车载试验的具体实施方案设计为：首先，试验车静止，陆用捷联惯导开机对准10分钟后进入导航状态，在陆用捷联惯导进入导航状态后，试验车开动并在行驶过程中随机停车，试验时间为90分钟；采集试验过程的陆用捷联惯导的陀螺仪和加速度计输出，并利用本发明提出的基于逆向导航的零速修正在线平滑方法进行实时在线的导航误差抑制得到实时的位置解算结果，并在试验后对采集的陀螺仪和加速度计输出进行不利用本发明提出的基于逆向导航的零速修正在线平滑方法的正向导航解算，以安装于试验车上的GPS提供的位置作为参照，分别得到未进行导航误差抑制和利用本申请的基于逆向导航的零速修正在线平滑方法进行导航误差抑制的位置解算结果的误差。

如图2(a)所示为在该实验中陆用捷联惯导未进行导航误差抑制得到位置误差示意图；如图2(b)所示为在该实验中陆用捷联惯导利用本申请提出的基于逆向导航的零速修正在线平滑方法进行导航误差抑制后的实时位置误差示意图。

如图2(a)和图2(b)中可以看出，利用本发明提出的基于逆向导航的零速修正在线平滑方法进行导航误差抑制后合位置误差从228m减小到4.8m，合位置的导航精度提高97.89％，且能够实时在线地实现导航误差抑制，进而证明了本申请提供的陆用捷联惯导基于逆向导航的零速修正在线平滑方法的正确性和有效性，能很好地实时在线地抑制陆用捷联惯导的导航误差，有很好的实用性。

本发明未详细公开的部分属于本领域的公知技术。尽管上面对本发明说明性的具体实施方式进行了描述，以便于本技术领域的技术人员理解本发明，但应该清楚，本发明不限于具体实施方式的范围，对本技术领域的普通技术人员来讲，只要各种变化在所附的权利要求限定和确定的本发明的精神和范围内，这些变化时显而易见的，一切利用本发明构思的发明创造均为保护之列。

Claims (4)

1.一种陆用捷联惯导基于逆向导航的零速修正在线平滑方法，其特征在于，步骤如下：

S1、对陆用捷联惯导实时输出的原始数据进行正向导航解算与零速修正；该步骤的具体实施过程为：

S101、采用适合于计算机解算的离散化递推算法对陆用捷联惯导实时输出的原始数据进行正向导航解算；

S102、构建用于陆用捷联惯导正向导航解算的零速修正卡尔曼滤波器，包括：

①定义用于正向导航零速修正卡尔曼滤波器的状态量X¹⁵：

式中，

为陆用捷联惯导的姿态误差，δV为陆用捷联惯导的速度误差，δP为陆用捷联惯导的位置误差，ε为陆用捷联惯导中的陀螺零偏误差，

为陆用捷联惯导中的加速度计零偏误差；

②建立用于正向导航零速修正卡尔曼滤波器的状态方程：

式中，

F¹¹、F¹²、F¹³、F²¹、F²²、F¹³、F³²、F³³为非零矩阵元素，其具体表达式为：

其中，L为当地地理纬度，λ为当地地理经度，h为当地地理高度；V_E、V_N和V_U分别为陆用捷联惯导的东向速度、北向速度和天向速度；R_M和R_N分别为当地地球子午圈半径和卯酉圈半径；ω_ie为地球自转角速率；

和

分别为陆用捷联惯导测量的东向比力、北向比力和天向比力；

为陆用捷联惯导的姿态矩阵；G¹⁵为测量噪声输入矩阵，其表达式为：

u为测量噪声，其表达式为：

其中，u_g为陀螺的测量噪声，u_g＝[u_gx u_gy u_gz]^T，u_gx为X向陀螺的测量噪声，u_gy为Y向陀螺的测量噪声，u_gz为Z向陀螺的测量噪声；u_a为加速度计的测量噪声，u_a＝[u_ax u_ay u_az]^T，u_ax为X向加速度计的测量噪声、u_ay为Y向加速度计的测量噪声，u_az为Z向加速度计的测量噪声；

③建立用于正向导航零速修正的卡尔曼滤波器的观测方程：Z¹⁵＝H¹⁵X¹⁵+v，式中，Z¹⁵＝[0_1×3 V^T 0_1×9]^T为正向导航零速修正的观测向量，V＝[V_E V_N V_U]^T为正向导航零速修正的速度向量；v为观测噪声；H¹⁵为观测矩阵，H¹⁵＝[0_3×3 I₃ 0_3×9]，I₃为三行三列的单位矩阵，即

0_3×3为三行三列的零矩阵，0_3×9为三行九列的零矩阵；

S103、利用步骤S102构建的用于正向导航零速修正的卡尔曼滤波器对正向导航结果的导航误差和惯性器件误差中的陀螺零偏误差和加速度计零偏误差进行实时预测，包括：状态1：在陆用运载体开车前的静止过程且陆用捷联惯导处于对准阶段，利用步骤S102设计的正向导航零速修正的卡尔曼滤波器实时解算得到陆用捷联惯导对准阶段下的正向导航零速修正的卡尔曼滤波器的状态量；状态2：在陆用运载体行驶的过程中且陆用捷联惯导处于导航阶段，利用步骤S102设计的正向导航零速修正的卡尔曼滤波器实时解算得到陆用运载体行驶中陆用捷联惯导导航阶段下的正向导航零速修正的卡尔曼滤波器的状态量；状态3：在陆用运载体开车后的暂停行驶的过程中且陆用捷联惯导处于导航阶段，利用步骤S102设计的卡正向导航零速修正的卡尔曼滤波器实时解算得到陆用运载体开车后暂停行驶中陆用捷联惯导导航阶段下的正向导航零速修正的卡尔曼滤波器的状态量；

S104、将不同阶段下获得的正向导航零速修正的卡尔曼滤波器的状态量用于正向导航的导航误差的补偿；补偿1：将经过步骤S103的状态1下得到的正向导航零速修正的卡尔曼滤波器的状态量中的导航误差补偿至步骤S101实时输出的正向导航结果中，得到经过正向导航零速修正的导航结果；将经过步骤S103的状态1下得到的正向导航零速修正的卡尔曼滤波器的状态量中的惯性器件误差中的陀螺零偏误差和加速度计零偏误差则以写入陆用捷联惯导上导航计算机板上内嵌的惯性导航解算方程中的方式进行补偿；补偿2：将经过步骤S103的状态3下得到的正向导航零速修正的卡尔曼滤波器的状态量中的导航误差补偿至步骤S101实时输出的正向导航结果中，即得到经过正向导航零速修正的导航结果；将经过步骤S103的状态3下得到的正向导航零速修正的卡尔曼滤波器的状态量中的惯性器件误差中的陀螺零偏误差和加速度计零偏误差则以写入陆用捷联惯导上导航计算机板上内嵌的惯性导航解算方程中的方式进行补偿；

S2、将陆用捷联惯导的原始数据保存在存储器中，在停车阶段，对保存的陆用捷联惯导的原始数据进行逆向导航解算与零速修正；该步骤的具体实施过程为：

S201、保存的陆用捷联惯导的原始数据，包括陀螺仪组件测量的角速率和由加速度计组件测量的比力；

S202、以步骤S1得到的当前时刻正向导航经零速修正的导航结果作为逆向导航解算的初值，采用适合于计算机解算的离散化逆向递推算法，对当前时刻之前所有时刻的原始数据进行逆向导航解算；

S203、构建用于陆用捷联惯导逆向导航解算的零速修正卡尔曼滤波器，包括：

①定义用于逆向导航零速修正卡尔曼滤波器的状态量X′¹⁵：

式中，δV_r为逆向速度误差，δV_r＝[δV_Er δV_Nr δV_Ur]^T，δV_Er、δV_Nr和δV_Ur分别为逆向东向速度误差、逆向北向速度误差和逆向天向速度误差；

②建立用于逆向导航零速修正卡尔曼滤波器的状态方程：

其中，

F′¹⁵矩阵中：

其中，L为当地地理纬度，λ为当地地理经度，h为当地地理高度；V_Er、V_Nr和V_Ur分别为陆用捷联惯导逆向导航的东向速度、北向速度和天向速度；R_M和R_N分别为当地地球子午圈半径和卯酉圈半径；ω_ie为地球自转角速率；

和

分别为陆用捷联惯导测量的东向比力、北向比力和天向比力；

为陆用捷联惯导的姿态矩阵；G^′15为逆向导航零速修正测量噪声输入矩阵，其表达式为：

u为测量噪声，其表达式为：

其中，u_g为陀螺的测量噪声，u_g＝[u_gx u_gy u_gz]^T，u_gx为X向陀螺的测量噪声，u_gy为Y向陀螺的测量噪声，u_gz为Z向陀螺的测量噪声；u_a为加速度计的测量噪声，u_a＝[u_ax u_ay u_az]^T，u_ax为X向加速度计的测量噪声、u_ay为Y向加速度计的测量噪声，u_az为Z向加速度计的测量噪声；

③建立用于逆向导航零速修正的卡尔曼滤波器的观测方程：Z′¹⁵＝H′¹⁵X′¹⁵+v，式中，Z′¹⁵＝[0_1×3 V_r ^T 0_1×9]^T为逆向导航零速修正的观测向量，V_r＝[V_Er V_Nr V_Ur]^T为逆向导航零速修正的速度向量；v为观测噪声；H^′15为逆向导航零速修正的观测矩阵，H^′15＝[0_3×3 I₃ 0_3×9]，I₃为三行三列的单位矩阵，即

0_3×3为三行三列的零矩阵，0_3×9为三行九列的零矩阵；

S203、利用构建的逆向导航零速修正的卡尔曼滤波器对逆向导航结果的导航误差和惯性器件误差中的陀螺零偏误差和加速度计零偏误差进行预测，包括：状态1：对于陆用运载体开车前的静止过程且陆用捷联惯导处于对准阶段，利用步骤S202设计的逆向导航零速修正的卡尔曼滤波器离线解算得到陆用捷联惯导对准阶段下的逆向导航零速修正的卡尔曼滤波器的状态量；状态2：对于陆用运载体行驶的过程中且陆用捷联惯导处于导航阶段，利用步骤S202设计的逆向导航零速修正的卡尔曼滤波器离线解算得到陆用运载体行驶中陆用捷联惯导导航阶段下的逆向导航零速修正的卡尔曼滤波器的状态量；状态3：对于陆用运载体开车后的暂停行驶的过程中且陆用捷联惯导处于导航阶段，利用步骤S202设计的用于逆向导航零速修正的卡尔曼滤波器离线解算得到陆用运载体开车后暂停行驶中陆用捷联惯导导航阶段下的逆向导航零速修正的卡尔曼滤波器的状态量；

S3、采用固定区域最优平滑估计方法，对由步骤S2得到的逆向导航零速修正的卡尔曼滤波器状态量中的导航误差进行处理，估算得到当前时刻导航误差并进行导航误差补偿；该步骤的具体实施过程为：

S301、对当前时刻之前所有时刻的逆向导航零速修正的卡尔曼滤波器状态量的导航误差进行固定区域最优平滑估计，得到当前时刻下经过逆向导航零速修正的导航误差的推算值；

S302、在由步骤S1得到的当前时刻下经过正向导航零速修正后的导航结果中扣除由步骤S301得到的当前时刻下经过逆向导航零速修正的导航误差的推算值，得到最终导航结果。

2.根据权利要求1所述的陆用捷联惯导基于逆向导航的零速修正在线平滑方法，其特征在于，步骤S101中正向导航解算方法如下：

L_k＝L_k-1+T_sV_Nk-1/(R_M+h_k-1)，

λ_k＝λ_k-1+T_sV_Ek-1secL_k-1/(R_N+h_k-1)，

h_k＝h_k-1+T_sV_Uk-1，

其中，

gⁿ＝[0 0 -g]^T，

式中，T_s为原始数据的实时采样周期；k为离散化的时刻；每个右下角带有符号k的物理量表示为k时刻下该物理量的状态值，每个右下角带符号k-1的物理量表示为k-1时刻下该物理量的状态值；

V，L，λ，h为导航解算结果，

为陆用捷联惯导的姿态矩阵；V为陆用捷联惯导在导航坐标系下的速度向量，V＝[V_E V_N V_U]^T，V_E、V_N和V_U分别为陆用捷联惯导的东向速度、北向速度和天向速度；L、λ和h分别为陆用捷联惯导在地球表面的纬度、经度和高度；

和f^m为原始数据，

为陆用捷联惯导中陀螺仪组件测量的角速率原始数据，f^m为陆用捷联惯导中加速度计组件测量的比力原始数据；ω_ie和g分别为地球自转角速度和重力加速度；R_M和R_N分别为当地地球的子午圈半径和卯酉圈半径。

3.根据权利要求1所述的陆用捷联惯导基于逆向导航的零速修正在线平滑方法，其特征在于，步骤S202中逆向导航解算方法如下：

L_k-1＝L_k+T_sV_Nrk/(R_M+h_k)，

λ_k-1＝λ_k+T_sV_ErksecL_k/(R_N+h_k)，

h_k-1＝h_k+T_sV_Urk，

其中，

式中，V_r为陆用捷联惯导在导航坐标系下的逆向速度向量，V_r＝[V_Er V_Nr V_Ur]^T，V_Er、V_Nr和V_Ur分别为陆用捷联惯导的逆向东向速度、逆向北向速度和逆向天向速度；其它符号表征的物理量与步骤S101中的定义一致：

V_r，L，λ，h为逆向导航解算结果，

为陆用捷联惯导的姿态矩阵；V_r为陆用捷联惯导在导航坐标系下逆向导航的速度向量，V_r＝[V_Er V_Nr V_Ur]^T，V_Er、V_Nr和V_Ur分别为陆用捷联惯导逆向导航的东向速度、北向速度和天向速度；L、λ和h分别为陆用捷联惯导在地球表面的纬度、经度和高度；

和f^m为原始数据，

为陆用捷联惯导中陀螺仪组件测量的角速率原始数据，f^m为陆用捷联惯导中加速度计组件测量的比力原始数据；ω_ie和g分别为地球自转角速度和重力加速度；R_M和R_N分别为当地地球的子午圈半径和卯酉圈半径。

4.根据权利要求1所述的陆用捷联惯导基于逆向导航的零速修正在线平滑方法，其特征在于，步骤S301中，采用固定区域最优平滑估计方法推算得到当前时刻导航误差估计值的具体操作方法：

设当前时刻为k时刻，则在逆向导航零速修正的卡尔曼滤波器的工作过程中，k时刻下的状态量为

状态一步预测阵为

状态误差均方差阵为P_k，一步预测误差均方差阵为P_k/k-1；

记在k时刻前进行逆向导航中解算的有M个时刻，以j表示逆向导航过程中的任一时刻，则固定区域最优平滑估计的迭代方程的表达式为：

式中，

为由第M时刻的状态量推算得到的第j时刻的状态量，

为由第j-1时刻的状态量推算得到的第j时刻的状态量，A_j-1为构造的j-1时刻的中间矩阵，

为第j-1时刻的状态量，

为由第M时刻的状态量推算得到的第j-1时刻的状态量，P_j-1为第j-1时刻的状态误差均方差阵，

为第j-1时刻到第j时刻的状态一步转移矩阵的转置矩阵，

为P_j/j-1的逆矩阵，P_j-1/M为第M时刻到第j-1时刻的一步预测误差均方差阵，P_j/M为第M时刻到第j时刻的一步预测误差均方差阵，P_j/j-1为第j-1时刻到第j时刻的一步预测误差均方差阵，

为A_j-1的转置矩阵；

令j＝M,M-1,…,1，通过上述固定区域最优平滑估计的迭代方程得到停车时正向导航经零速修正的当前时刻的逆向导航零速修正滤波器状态量在当前时刻的固定区域最优平滑估计

即停车时正向导航经零速修正的当前时刻的逆向零速修正滤波器估计的状态量中的导航误差。

Images