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

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

Info

Publication number
CN115265588B
CN115265588B CN202210835644.7A CN202210835644A CN115265588B CN 115265588 B CN115265588 B CN 115265588B CN 202210835644 A CN202210835644 A CN 202210835644A CN 115265588 B CN115265588 B CN 115265588B
Authority
CN
China
Prior art keywords
navigation
zero
land
reverse
error
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202210835644.7A
Other languages
English (en)
Other versions
CN115265588A (zh
Inventor
涂勇强
蔡庆中
杨功流
李晶
尹洪亮
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beihang University
Original Assignee
Beihang University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Beihang University filed Critical Beihang University
Priority to CN202210835644.7A priority Critical patent/CN115265588B/zh
Publication of CN115265588A publication Critical patent/CN115265588A/zh
Application granted granted Critical
Publication of CN115265588B publication Critical patent/CN115265588B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T90/00—Enabling technologies or technologies with a potential or indirect contribution to GHG emissions mitigation

Landscapes

  • Engineering & Computer Science (AREA)
  • Manufacturing & Machinery (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Navigation (AREA)

Abstract

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

Description

陆用捷联惯导基于逆向导航的零速修正在线平滑方法
技术领域
本发明涉及陆用捷联惯导导航误差抑制技术领域，特别涉及一种陆用捷联惯导基于逆向导航的零速修正在线平滑方法。
背景技术
陆用捷联惯导能够自主地提供实时的包括姿态、位置、速度在内的全方位导航信息，被广泛应用于陆地作战车辆的实时导航中，如测地车、自行火炮、远程火箭炮、侦察车等陆地车辆。对于增强地面部队的机动能力、生存能力、快速反应能力、目标捕获能力、协同作战能力以及远程精确打击能力都具有重要的作用和意义。
然而，受陆用捷联惯导积分式导航解算原理和惯性器件不可避免的零偏误差的影响，陆用捷联惯导的导航误差随着导航时间增加而增加。为了抑制陆用捷联惯导的导航误差，目前常用的方法有逆向导航、零速修正和数据平滑。逆向导航的原理是将整个导航过程的原始数据存储下来并对存储的数据按时间逆序处理，在实时性无要求的情况下，通过逆向导航有利于提高导航精度，例如，授权发明专利CN111024065B公开了一种用于最优估计精对准的严格逆向导航方法，但该方法无法实时地对导航误差进行抑制。零速修正的原理是充分利用捷联惯导在停车时其速度真实值为零作为观测值对导航误差进行估计和补偿，例如，授权发明专利CN105806340B提供了一种基于窗口平滑的自适应零速修正算法，但该方法只能在停车时候进行修正，对导航误差抑制有限。数据平滑的原理是通过最后停车点的位置信息利用平滑算法离线对全程的数据进行平滑以提高导航精度，但该方法必须离线进行。
综上，现有的陆用捷联惯导导航误差抑制方法存在着实时性差、对全程导航误差抑制有限和需要离线处理的缺点。
发明内容
本发明的目的是提供一种解决现有的陆用捷联惯导导航误差抑制方法存在的实时性差、对导航误差抑制有限和需要离线处理的缺点的陆用捷联惯导基于逆向导航的零速修正在线平滑方法。
为此，本发明技术方案如下：
一种陆用捷联惯导基于逆向导航的零速修正在线平滑方法，步骤如下：
S1、对陆用捷联惯导实时输出的原始数据进行正向导航解算与零速修正；该步骤的具体实施过程为：
S101、采用适合于计算机解算的离散化递推算法对陆用捷联惯导实时输出的原始数据进行正向导航解算；
S102、构建用于陆用捷联惯导正向导航解算的零速修正卡尔曼滤波器，包括：
①定义用于正向导航零速修正卡尔曼滤波器的状态量X15：式中，/>为陆用捷联惯导的姿态误差，δV为陆用捷联惯导的速度误差，δP为陆用捷联惯导的位置误差，ε为陆用捷联惯导中的陀螺零偏误差，/>为陆用捷联惯导中的加速度计零偏误差；
②建立用于正向导航零速修正卡尔曼滤波器的状态方程：式中，/>F11、F12、F13、F21、F22、F13、F32、F33为非零矩阵元素，其具体表达式为：
其中，L为当地地理纬度，λ为当地地理经度，h为当地地理高度；VE、VN和VU分别为陆用捷联惯导的东向速度、北向速度和天向速度；RM和RN分别为当地地球子午圈半径和卯酉圈半径；ωie为地球自转角速率；和/>分别为陆用捷联惯导测量的东向比力、北向比力和天向比力；/>为陆用捷联惯导的姿态矩阵；G15为测量噪声输入矩阵，其表达式为：u为测量噪声，其表达式为：/>其中，ug为陀螺的测量噪声，ug＝[ugx ugy ugz]T，ugx为X向陀螺的测量噪声，ugy为Y向陀螺的测量噪声，ugz为Z向陀螺的测量噪声；ua为加速度计的测量噪声，ua＝[uax uay uaz]T，uax为X向加速度计的测量噪声、uay为Y向加速度计的测量噪声，uaz为Z向加速度计的测量噪声；
③建立用于正向导航零速修正的卡尔曼滤波器的观测方程：Z15＝H15X15+v，式中，Z15＝[01×3 VT 01×9]T为正向导航零速修正的观测向量，V＝[VE VN VU]T为正向导航零速修正的速度向量；v为观测噪声；H15为观测矩阵，H15＝[03×3 I3 03×9]，I3为三行三列的单位矩阵，即03×3为三行三列的零矩阵，03×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′15：
式中，δVr为逆向速度误差，δVr＝[δVEr δVNr δVUr]T，δVEr、δVNr和δVUr分别为逆向东向速度误差、逆向北向速度误差和逆向天向速度误差；
②建立用于逆向导航零速修正卡尔曼滤波器的状态方程：其中，/>F′15矩阵中：
其中，L为当地地理纬度，λ为当地地理经度，h为当地地理高度；VEr、VNr和VUr分别为陆用捷联惯导逆向导航的东向速度、北向速度和天向速度；RM和RN分别为当地地球子午圈半径和卯酉圈半径；ωie为地球自转角速率；和/>分别为陆用捷联惯导测量的东向比力、北向比力和天向比力；/>为陆用捷联惯导的姿态矩阵；G′15为逆向导航零速修正测量噪声输入矩阵，其表达式为：/>u为测量噪声，其表达式为：/>其中，ug为陀螺的测量噪声，ug＝[ugx ugy ugz]T，ugx为X向陀螺的测量噪声，ugy为Y向陀螺的测量噪声，ugz为Z向陀螺的测量噪声；ua为加速度计的测量噪声，ua＝[uax uay uaz]T，uax为X向加速度计的测量噪声、uay为Y向加速度计的测量噪声，uaz为Z向加速度计的测量噪声；
③建立用于逆向导航零速修正的卡尔曼滤波器的观测方程：Z′15＝H′15X′15+v，式中，Z′15＝[01×3 Vr T 01×9]T为逆向导航零速修正的观测向量，Vr＝[VEr VNr VUr]T为逆向导航零速修正的速度向量；v为观测噪声；H′15为逆向导航零速修正的观测矩阵，H′15＝[03×3 I303×9]，I3为三行三列的单位矩阵，即03×3为三行三列的零矩阵，03×9为三行九列的零矩阵；
S203、利用构建的逆向导航零速修正的卡尔曼滤波器对逆向导航结果的导航误差和惯性器件误差中的陀螺零偏误差和加速度计零偏误差进行预测，包括：状态1：对于陆用运载体开车前的静止过程且陆用捷联惯导处于对准阶段，利用步骤S202设计的逆向导航零速修正的卡尔曼滤波器离线解算得到陆用捷联惯导对准阶段下的逆向导航零速修正的卡尔曼滤波器的状态量；状态2：对于陆用运载体行驶的过程中且陆用捷联惯导处于导航阶段，利用步骤S202设计的逆向导航零速修正的卡尔曼滤波器离线解算得到陆用运载体行驶中陆用捷联惯导导航阶段下的逆向导航零速修正的卡尔曼滤波器的状态量；状态3：对于陆用运载体开车后的暂停行驶的过程中且陆用捷联惯导处于导航阶段，利用步骤S202设计的用于逆向导航零速修正的卡尔曼滤波器离线解算得到陆用运载体开车后暂停行驶中陆用捷联惯导导航阶段下的逆向导航零速修正的卡尔曼滤波器的状态量；
S3、采用固定区域最优平滑估计方法，对由步骤S2得到的逆向导航零速修正的卡尔曼滤波器状态量中的导航误差进行处理，估算得到当前时刻导航误差并进行导航误差补偿；该步骤的具体实施过程为：
S301、对当前时刻之前所有时刻的逆向导航零速修正的卡尔曼滤波器状态量的导航误差进行固定区域最优平滑估计，得到当前时刻下经过逆向导航零速修正的导航误差的推算值；
S302、在由步骤S1得到的当前时刻下经过正向导航零速修正后的导航结果中扣除由步骤S301得到的当前时刻下经过逆向导航零速修正的导航误差的推算值，得到最终导航结果。
进一步地，步骤S101中正向导航解算方法如下：
Lk＝Lk-1+TsVNk-1/(RM+hk-1)，
λk＝λk-1+TsVEk-1secLk-1/(RN+hk-1)，
hk＝hk-1+TsVUk-1，
其中，
gn＝[0 0 -g]T，/>
式中，Ts为原始数据的实时采样周期；k为离散化的时刻；每个右下角带有符号k的物理量表示为k时刻下该物理量的状态值，每个右下角带符号k-1的物理量表示为k-1时刻下该物理量的状态值；V，L，λ，h为导航解算结果，/>为陆用捷联惯导的姿态矩阵；V为陆用捷联惯导在导航坐标系下的速度向量，V＝[VE VN VU]T，VE、VN和VU分别为陆用捷联惯导的东向速度、北向速度和天向速度；L、λ和h分别为陆用捷联惯导在地球表面的纬度、经度和高度；/>和fm为原始数据，/>为陆用捷联惯导中陀螺仪组件测量的角速率原始数据，fm为陆用捷联惯导中加速度计组件测量的比力原始数据；ωie和g分别为地球自转角速度和重力加速度；RM和RN分别为当地地球的子午圈半径和卯酉圈半径。
进一步地，步骤S202中逆向导航解算方法如下：
Lk-1＝Lk+TsVNrk/(RM+hk)，
λk-1＝λk+TsVErk secLk/(RN+hk)，
hk-1＝hk+TsVUrk，
其中，
式中，Vr为陆用捷联惯导在导航坐标系下的逆向速度向量，Vr＝[VEr VNr VUr]T，VEr、VNr和VUr分别为陆用捷联惯导的逆向东向速度、逆向北向速度和逆向天向速度；其它符号表征的物理量与步骤S101中的定义一致：Vr，L，λ，h为逆向导航解算结果，/>为陆用捷联惯导的姿态矩阵；Vr为陆用捷联惯导在导航坐标系下逆向导航的速度向量，Vr＝[VEr VNrVUr]T，VEr、VNr和VUr分别为陆用捷联惯导逆向导航的东向速度、北向速度和天向速度；L、λ和h分别为陆用捷联惯导在地球表面的纬度、经度和高度；/>和fm为原始数据，/>为陆用捷联惯导中陀螺仪组件测量的角速率原始数据，fm为陆用捷联惯导中加速度计组件测量的比力原始数据；ωie和g分别为地球自转角速度和重力加速度；RM和RN分别为当地地球的子午圈半径和卯酉圈半径。
进一步地，步骤S301中，采用固定区域最优平滑估计方法推算得到当前时刻导航误差估计值的具体操作方法：
设当前时刻为k时刻，则在逆向导航零速修正的卡尔曼滤波器的工作过程中，k时刻下的状态量为状态一步预测阵为/>状态误差均方差阵为Pk，一步预测误差均方差阵为Pk/k-1；
记在k时刻前进行逆向导航中解算的有M个时刻，以j表示逆向导航过程中的任一时刻，则固定区域最优平滑估计的迭代方程的表达式为：
式中，为由第M时刻的状态量推算得到的第j时刻的状态量，/>为由第j-1时刻的状态量推算得到的第j时刻的状态量，Aj-1为构造的j-1时刻的中间矩阵，/>为第j-1时刻的状态量，/>为由第M时刻的状态量推算得到的第j-1时刻的状态量，Pj-1为第j-1时刻的状态误差均方差阵，/>为第j-1时刻到第j时刻的状态一步转移矩阵的转置矩阵，为Pj/j-1的逆矩阵，Pj-1/M为第M时刻到第j-1时刻的一步预测误差均方差阵，Pj/M为第M时刻到第j时刻的一步预测误差均方差阵，Pj/j-1为第j-1时刻到第j时刻的一步预测误差均方差阵，/>为Aj-1的转置矩阵；
令j＝M,M-1,…,1，通过上述固定区域最优平滑估计的迭代方程得到停车时正向导航经零速修正的当前时刻的逆向导航零速修正滤波器状态量在当前时刻的固定区域最优平滑估计即停车时正向导航经零速修正的当前时刻的逆向零速修正滤波器估计的状态量中的导航误差。
与现有技术相比，该陆用捷联惯导基于逆向导航的零速修正在线平滑方法解决了现有的陆用捷联惯导导航误差抑制方法存在的实时性差、对导航误差抑制有限和需要离线处理的缺点，充分发挥逆向导航、零速修正和数据平滑三种方法的优点，提供一种实时性好、能充分抑制导航误差且能在线处理的陆用捷联惯导基于逆向导航的零速修正在线平滑方法。
附图说明
图1为本发明的陆用捷联惯导基于逆向导航的零速修正在线平滑方法的流程示意图；
图2(a)为本发明实施例中陆用捷联惯导利用本发明提出的基于逆向导航的零速修正在线平滑方法进行导航误差抑制前的位置误差示意图；
图2(b)为本发明实施例中陆用捷联惯导利用本发明提出的基于逆向导航的零速修正在线平滑方法进行导航误差抑制后的实时位置误差示意图。
具体实施方式
下面结合附图及具体实施例对本发明做进一步的说明，但下述实施例绝非对本发明有任何限制。
如图1所示，该陆用捷联惯导基于逆向导航的零速修正在线平滑方法的具体实施步骤如下：
S1、对陆用捷联惯导实时输出的原始数据进行正向导航解算与零速修正；
具体地，该步骤S1的实施步骤为：
S101、对陆用捷联惯导实时输出的原始数据进行正向导航解算；
具体地，对实时输出的原始数据采用适合于计算机解算的离散化递推算法进行正向导航解算：
Lk＝Lk-1+TsVNk-1/(RM+hk-1)，
λk＝λk-1+TsVEk-1secLk-1/(RN+hk-1)，
hk＝hk-1+TsVUk-1，
其中，
gn＝[0 0 -g]T，/>
式中，Ts为原始数据的实时采样周期；k为离散化的时刻；每个右下角带有符号k的物理量表示为k时刻下该物理量的状态值，每个右下角带符号k-1的物理量表示为k-1时刻下该物理量的状态值；V，L，λ，h为导航解算结果，/>为陆用捷联惯导的姿态矩阵；V为陆用捷联惯导在导航坐标系下的速度向量，V＝[VE VN VU]T，VE、VN和VU分别为陆用捷联惯导的东向速度、北向速度和天向速度；L、λ和h分别为陆用捷联惯导在地球表面的纬度、经度和高度；/>和fm为原始数据，/>为陆用捷联惯导中陀螺仪组件测量的角速率原始数据，fm为陆用捷联惯导中加速度计组件测量的比力原始数据；ωie和g分别为地球自转角速度和重力加速度；RM和RN分别为当地地球的子午圈半径和卯酉圈半径；
因此，当k≥1时，由陆用捷联惯导在k时刻输出的实时原始数据和/>进行正向导航解算获得k时刻正向导航解算结果/>Vk、Lk、λk和hk；导航解算结果在k＝0时的初值由陆用捷联惯导的初始对准过程得到；
S102、构建用于陆用捷联惯导正向导航解算的零速修正卡尔曼滤波器；
具体地，步骤S102的实施步骤如下：
S1021、定义用于正向导航零速修正卡尔曼滤波器的状态量：
基于陆用捷联惯导的导航误差包括位置误差、速度误差和姿态误差；导航误差来源于惯性器件误差，其中，惯性器件误差中的陀螺零偏误差和加速度计零偏误差是影响陆用捷联惯导精度最重要的误差，因此，构建以导航误差和惯性器件误差中的陀螺零偏误差和加速度计零偏误差为集合的向量作为正向导航零速修正卡尔曼滤波器的状态量；
定义正向导航零速修正卡尔曼滤波器的状态量为一个十五维的向量，即X15，相应表达式为：
式中，X15为所构建的十五维正向导航零速修正卡尔曼滤波器状态量；为陆用捷联惯导的姿态误差，/>和/>分别为陆用捷联惯导的东向误差角、北向误差角和天向误差角；δV为陆用捷联惯导的速度误差，δV＝[δVE δVN δVU]T，δVE、δVN和δVU分别为陆用捷联惯导的东向速度误差、北向速度误差和天向速度误差；δP为陆用捷联惯导的位置误差，δP＝[δL δλ δh]T，δL、δλ和δh分别为陆用捷联惯导的纬度误差、经度误差和高度误差；ε为陆用捷联惯导中的陀螺零偏误差，ε＝[εx εy εz]T，εx、εy和εz分别为陆用捷联惯导中的X向陀螺零偏误差、Y向陀螺零偏误差和Z向陀螺零偏误差；/>为陆用捷联惯导中的加速度计零偏误差，/>和/>分别为陆用捷联惯导中的X向加速度计零偏误差、Y向加速度计零偏误差和Z向加速度计零偏误差；
S1022、建立用于正向导航零速修正卡尔曼滤波器的状态方程：
根据步骤S1021构建的十五维正向导航零速修正卡尔曼滤波器状态量、以及已知的惯导误差方程，建立卡尔曼滤波器的状态方程为：
式中，在F15中，F11、F12、F13、F21、F22、F13、F32、F33为非零矩阵元素，其具体表达式如下：
/>
其中，L为当地地理纬度，λ为当地地理经度，h为当地地理高度；VE、VN和VU分别为陆用捷联惯导的东向速度、北向速度和天向速度；RM和RN分别为当地地球子午圈半径和卯酉圈半径；ωie为地球自转角速率；和/>分别为陆用捷联惯导测量的东向比力、北向比力和天向比力；/>为陆用捷联惯导的姿态矩阵；
G15为测量噪声输入矩阵，其表达式为：
u为测量噪声，其表达式为：其中，ug为陀螺的测量噪声，ug＝[ugxugy ugz]T，ugx为X向陀螺的测量噪声，ugy为Y向陀螺的测量噪声，ugz为Z向陀螺的测量噪声；ua为加速度计的测量噪声，ua＝[uax uay uaz]T，uax为X向加速度计的测量噪声、uay为Y向加速度计的测量噪声，uaz为Z向加速度计的测量噪声；
S1023、建立用于正向导航零速修正的卡尔曼滤波器的观测方程：
正向导航零速修正的卡尔曼滤波器的观测方程为：
Z15＝H15X15+v，
式中，Z15＝[01×3 VT 01×9]T为正向导航零速修正的观测向量，V＝[VE VN VU]T为正向导航零速修正的速度向量；v为观测噪声；H15为观测矩阵，由于选用陆用捷联惯导的速度解算结果作为观测量，因此，H15的表达式为：
H15＝[03×3 I3 03×9]，
式中，I3为三行三列的单位矩阵，即03×3为三行三列的零矩阵，03×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保存的原始数据进行解算：
Lk-1＝Lk+TsVNrk/(RM+hk)，
λk-1＝λk+TsVErk secLk/(RN+hk)，
hk-1＝hk+TsVUrk，
其中，
式中，Vr为陆用捷联惯导在导航坐标系下的逆向速度向量，Vr＝[VEr VNr VUr]T，VEr、VNr和VUr分别为陆用捷联惯导的逆向东向速度、逆向北向速度和逆向天向速度；其它符号表征的物理量与步骤S101中的定义一致：Vr，L，λ，h为逆向导航解算结果，/>为陆用捷联惯导的姿态矩阵；Vr为陆用捷联惯导在导航坐标系下逆向导航的速度向量，Vr＝[VEr VNrVUr]T，VEr、VNr和VUr分别为陆用捷联惯导逆向导航的东向速度、北向速度和天向速度；L、λ和h分别为陆用捷联惯导在地球表面的纬度、经度和高度；/>和fm为原始数据，/>为陆用捷联惯导中陀螺仪组件测量的角速率原始数据，fm为陆用捷联惯导中加速度计组件测量的比力原始数据；ωie和g分别为地球自转角速度和重力加速度；RM和RN分别为当地地球的子午圈半径和卯酉圈半径；
其中，逆向导航解算结果在k＝0时的初值由陆用捷联惯导在停车状态开始逆向导航时刻的正向导航经零速修正后的导航结果赋予，即经过步骤S1得到当前时刻的正向导航经零速修正的导航数据；
S203、构建用于陆用捷联惯导逆向导航解算的零速修正卡尔曼滤波器；
具体地，该步骤S203的实施方式为：
S2031、定义用于逆向导航零速修正卡尔曼滤波器的状态量；
由于逆向导航算法中的姿态和位置的定义与正向导航的相同，但逆向导航的速度的定义是与正向导航的定义在符号上相反的，记为逆向速度Vr，因此，将步骤S1021中构建的正向导航零速修正卡尔曼滤波器的状态量中的δV替换为δVr，即得到逆向导航零速修正卡尔曼滤波器的状态量：
式中，δVr为逆向速度误差，δVr＝[δVEr δVNr δVUr]T，δVEr、δVNr和δVUr分别为逆向东向速度误差、逆向北向速度误差和逆向天向速度误差；
S2032、建立用于逆向导航零速修正卡尔曼滤波器的状态方程：
逆向导航零速修正卡尔曼滤波器的状态方程：
其中，F′15矩阵中：
/>
其中，L为当地地理纬度，λ为当地地理经度，h为当地地理高度；VEr、VNr和VUr分别为陆用捷联惯导逆向导航的东向速度、北向速度和天向速度；RM和RN分别为当地地球子午圈半径和卯酉圈半径；ωie为地球自转角速率；和/>分别为陆用捷联惯导测量的东向比力、北向比力和天向比力；/>为陆用捷联惯导的姿态矩阵；
G′15为逆向导航零速修正测量噪声输入矩阵，其表达式为：
u为测量噪声，其表达式为：其中，ug为陀螺的测量噪声，ug＝[ugxugy ugz]T，ugx为X向陀螺的测量噪声，ugy为Y向陀螺的测量噪声，ugz为Z向陀螺的测量噪声；ua为加速度计的测量噪声，ua＝[uax uay uaz]T，uax为X向加速度计的测量噪声、uay为Y向加速度计的测量噪声，uaz为Z向加速度计的测量噪声；
S2033、建立用于逆向导航零速修正的卡尔曼滤波器的观测方程：
用于逆向导航零速修正的卡尔曼滤波器的观测方程为：
Z′15＝H′15X′15+v
式中，Z′15＝[01×3 Vr T 01×9]T为逆向导航零速修正的观测向量，Vr＝[VEr VNr VUr]T为逆向导航零速修正的速度向量；v为观测噪声；H′15为逆向导航零速修正的观测矩阵，H′15的表达式为：
H′15＝[03×3 I3 03×9]，
式中，I3为三行三列的单位矩阵，即03×3为三行三列的零矩阵，03×9为三行九列的零矩阵；
S203、利用构建的逆向导航零速修正的卡尔曼滤波器对逆向导航结果的导航误差和惯性器件误差中的陀螺零偏误差和加速度计零偏误差进行预测；
步骤S203与步骤S103的操作思路一致，二者区别仅在于在步骤S203中，卡尔曼滤波器采用步骤S202设计的用于逆向导航零速修正的卡尔曼滤波器；
具体地，步骤S203的具体实施方式为：
状态1：对于陆用运载体开车前的静止过程且陆用捷联惯导处于对准阶段，利用步骤S202设计的逆向导航零速修正的卡尔曼滤波器离线解算得到陆用捷联惯导对准阶段下的逆向导航零速修正的卡尔曼滤波器的状态量；
状态2：对于陆用运载体行驶的过程中且陆用捷联惯导处于导航阶段，利用步骤S202设计的逆向导航零速修正的卡尔曼滤波器离线解算得到陆用运载体行驶中陆用捷联惯导导航阶段下的逆向导航零速修正的卡尔曼滤波器的状态量；
状态3：对于陆用运载体开车后的暂停行驶的过程中且陆用捷联惯导处于导航阶段，利用步骤S202设计的用于逆向导航零速修正的卡尔曼滤波器离线解算得到陆用运载体开车后暂停行驶中陆用捷联惯导导航阶段下的逆向导航零速修正的卡尔曼滤波器的状态量；
S3、对由步骤S3得到的逆向导航零速修正的卡尔曼滤波器状态量中的导航误差进行固定区域最优平滑估计，以得到对当前时刻的导航误差并进行导航误差补偿；
具体地，该步骤S3的实施方式如下：
S301、对逆向导航零速修正的卡尔曼滤波器状态量中的导航误差进行固定区域最优平滑估计，以得到逆向导航零速修正的导航误差在当前时刻的推算值；
在该步骤中，由于逆向导航零速修正的卡尔曼滤波器无法估计停车时刻的导航误差，即无法进行实时的误差补偿；因此，首先通过对逆向导航零速修正的卡尔曼滤波器估计的状态量中的导航误差进行固定区域最优平滑估计，获得逆向导航零速修正估计的导航误差在当前时刻的推算值，进而利用推算值再进行当前时刻的导航误差补偿；
该固定区域最优平滑估计的具体操作的具体实施步骤如下：
设当前时刻为k时刻，则在逆向导航零速修正的卡尔曼滤波器的工作过程中，k时刻下的状态量为状态一步预测阵为/>状态误差均方差阵为Pk，一步预测误差均方差阵为Pk/k-1；
记在k时刻前进行逆向导航中解算的有M个时刻，以j表示逆向导航过程中的任一时刻，则固定区域最优平滑估计的迭代方程的表达式为：
式中，为由第M时刻的状态量推算得到的第j时刻的状态量，/>为由第j-1时刻的状态量推算得到的第j时刻的状态量，Aj-1为构造的j-1时刻的中间矩阵，/>为第j-1时刻的状态量，/>为由第M时刻的状态量推算得到的第j-1时刻的状态量，Pj-1为第j-1时刻的状态误差均方差阵，/>为第j-1时刻到第j时刻的状态一步转移矩阵的转置矩阵，为Pj/j-1的逆矩阵，Pj-1/M为第M时刻到第j-1时刻的一步预测误差均方差阵，Pj/M为第M时刻到第j时刻的一步预测误差均方差阵，Pj/j-1为第j-1时刻到第j时刻的一步预测误差均方差阵，/>为Aj-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、构建用于陆用捷联惯导正向导航解算的零速修正卡尔曼滤波器，包括：
①定义用于正向导航零速修正卡尔曼滤波器的状态量X15：式中，/>为陆用捷联惯导的姿态误差，δV为陆用捷联惯导的速度误差，δP为陆用捷联惯导的位置误差，ε为陆用捷联惯导中的陀螺零偏误差，/>为陆用捷联惯导中的加速度计零偏误差；
②建立用于正向导航零速修正卡尔曼滤波器的状态方程：式中，F11、F12、F13、F21、F22、F13、F32、F33为非零矩阵元素，其具体表达式为：
其中，L为当地地理纬度，λ为当地地理经度，h为当地地理高度；VE、VN和VU分别为陆用捷联惯导的东向速度、北向速度和天向速度；RM和RN分别为当地地球子午圈半径和卯酉圈半径；ωie为地球自转角速率；和/>分别为陆用捷联惯导测量的东向比力、北向比力和天向比力；/>为陆用捷联惯导的姿态矩阵；G15为测量噪声输入矩阵，其表达式为：u为测量噪声，其表达式为：/>其中，ug为陀螺的测量噪声，ug＝[ugx ugy ugz]T，ugx为X向陀螺的测量噪声，ugy为Y向陀螺的测量噪声，ugz为Z向陀螺的测量噪声；ua为加速度计的测量噪声，ua＝[uax uay uaz]T，uax为X向加速度计的测量噪声、uay为Y向加速度计的测量噪声，uaz为Z向加速度计的测量噪声；
③建立用于正向导航零速修正的卡尔曼滤波器的观测方程：Z15＝H15X15+v，式中，Z15＝[01×3 VT 01×9]T为正向导航零速修正的观测向量，V＝[VE VN VU]T为正向导航零速修正的速度向量；v为观测噪声；H15为观测矩阵，H15＝[03×3 I3 03×9]，I3为三行三列的单位矩阵，即03×3为三行三列的零矩阵，03×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′15：式中，δVr为逆向速度误差，δVr＝[δVEr δVNr δVUr]T，δVEr、δVNr和δVUr分别为逆向东向速度误差、逆向北向速度误差和逆向天向速度误差；
②建立用于逆向导航零速修正卡尔曼滤波器的状态方程：其中，F′15矩阵中：
其中，L为当地地理纬度，λ为当地地理经度，h为当地地理高度；VEr、VNr和VUr分别为陆用捷联惯导逆向导航的东向速度、北向速度和天向速度；RM和RN分别为当地地球子午圈半径和卯酉圈半径；ωie为地球自转角速率；和/>分别为陆用捷联惯导测量的东向比力、北向比力和天向比力；/>为陆用捷联惯导的姿态矩阵；G′15为逆向导航零速修正测量噪声输入矩阵，其表达式为：/>u为测量噪声，其表达式为：/>其中，ug为陀螺的测量噪声，ug＝[ugx ugy ugz]T，ugx为X向陀螺的测量噪声，ugy为Y向陀螺的测量噪声，ugz为Z向陀螺的测量噪声；ua为加速度计的测量噪声，ua＝[uax uay uaz]T，uax为X向加速度计的测量噪声、uay为Y向加速度计的测量噪声，uaz为Z向加速度计的测量噪声；
③建立用于逆向导航零速修正的卡尔曼滤波器的观测方程：Z′15＝H′15X′15+v，式中，Z′15＝[01×3 Vr T 01×9]T为逆向导航零速修正的观测向量，Vr＝[VEr VNr VUr]T为逆向导航零速修正的速度向量；v为观测噪声；H′15为逆向导航零速修正的观测矩阵，H′15＝[03×3 I3 03×9]，I3为三行三列的单位矩阵，即03×3为三行三列的零矩阵，03×9为三行九列的零矩阵；
S203、利用构建的逆向导航零速修正的卡尔曼滤波器对逆向导航结果的导航误差和惯性器件误差中的陀螺零偏误差和加速度计零偏误差进行预测，包括：状态1：对于陆用运载体开车前的静止过程且陆用捷联惯导处于对准阶段，利用步骤S202设计的逆向导航零速修正的卡尔曼滤波器离线解算得到陆用捷联惯导对准阶段下的逆向导航零速修正的卡尔曼滤波器的状态量；状态2：对于陆用运载体行驶的过程中且陆用捷联惯导处于导航阶段，利用步骤S202设计的逆向导航零速修正的卡尔曼滤波器离线解算得到陆用运载体行驶中陆用捷联惯导导航阶段下的逆向导航零速修正的卡尔曼滤波器的状态量；状态3：对于陆用运载体开车后的暂停行驶的过程中且陆用捷联惯导处于导航阶段，利用步骤S202设计的用于逆向导航零速修正的卡尔曼滤波器离线解算得到陆用运载体开车后暂停行驶中陆用捷联惯导导航阶段下的逆向导航零速修正的卡尔曼滤波器的状态量；
S3、采用固定区域最优平滑估计方法，对由步骤S2得到的逆向导航零速修正的卡尔曼滤波器状态量中的导航误差进行处理，估算得到当前时刻导航误差并进行导航误差补偿；该步骤的具体实施过程为：
S301、对当前时刻之前所有时刻的逆向导航零速修正的卡尔曼滤波器状态量的导航误差进行固定区域最优平滑估计，得到当前时刻下经过逆向导航零速修正的导航误差的推算值；
S302、在由步骤S1得到的当前时刻下经过正向导航零速修正后的导航结果中扣除由步骤S301得到的当前时刻下经过逆向导航零速修正的导航误差的推算值，得到最终导航结果。
2.根据权利要求1所述的陆用捷联惯导基于逆向导航的零速修正在线平滑方法，其特征在于，步骤S101中正向导航解算方法如下：
Lk＝Lk-1+TsVNk-1/(RM+hk-1)，
λk＝λk-1+TsVEk-1secLk-1/(RN+hk-1)，
hk＝hk-1+TsVUk-1，
其中，
gn＝[0 0 -g]T，/>
式中，Ts为原始数据的实时采样周期；k为离散化的时刻；每个右下角带有符号k的物理量表示为k时刻下该物理量的状态值，每个右下角带符号k-1的物理量表示为k-1时刻下该物理量的状态值；V，L，λ，h为导航解算结果，/>为陆用捷联惯导的姿态矩阵；V为陆用捷联惯导在导航坐标系下的速度向量，V＝[VE VN VU]T，VE、VN和VU分别为陆用捷联惯导的东向速度、北向速度和天向速度；L、λ和h分别为陆用捷联惯导在地球表面的纬度、经度和高度；/>和fm为原始数据，/>为陆用捷联惯导中陀螺仪组件测量的角速率原始数据，fm为陆用捷联惯导中加速度计组件测量的比力原始数据；ωie和g分别为地球自转角速度和重力加速度；RM和RN分别为当地地球的子午圈半径和卯酉圈半径。
3.根据权利要求1所述的陆用捷联惯导基于逆向导航的零速修正在线平滑方法，其特征在于，步骤S202中逆向导航解算方法如下：
Lk-1＝Lk+TsVNrk/(RM+hk)，
λk-1＝λk+TsVErksecLk/(RN+hk)，
hk-1＝hk+TsVUrk，
其中，
式中，Vr为陆用捷联惯导在导航坐标系下的逆向速度向量，Vr＝[VEr VNr VUr]T，VEr、VNr和VUr分别为陆用捷联惯导的逆向东向速度、逆向北向速度和逆向天向速度；其它符号表征的物理量与步骤S101中的定义一致：Vr，L，λ，h为逆向导航解算结果，/>为陆用捷联惯导的姿态矩阵；Vr为陆用捷联惯导在导航坐标系下逆向导航的速度向量，Vr＝[VEr VNr VUr]T，VEr、VNr和VUr分别为陆用捷联惯导逆向导航的东向速度、北向速度和天向速度；L、λ和h分别为陆用捷联惯导在地球表面的纬度、经度和高度；/>和fm为原始数据，/>为陆用捷联惯导中陀螺仪组件测量的角速率原始数据，fm为陆用捷联惯导中加速度计组件测量的比力原始数据；ωie和g分别为地球自转角速度和重力加速度；RM和RN分别为当地地球的子午圈半径和卯酉圈半径。
4.根据权利要求1所述的陆用捷联惯导基于逆向导航的零速修正在线平滑方法，其特征在于，步骤S301中，采用固定区域最优平滑估计方法推算得到当前时刻导航误差估计值的具体操作方法：
设当前时刻为k时刻，则在逆向导航零速修正的卡尔曼滤波器的工作过程中，k时刻下的状态量为状态一步预测阵为/>状态误差均方差阵为Pk，一步预测误差均方差阵为Pk/k-1；
记在k时刻前进行逆向导航中解算的有M个时刻，以j表示逆向导航过程中的任一时刻，则固定区域最优平滑估计的迭代方程的表达式为：
式中，为由第M时刻的状态量推算得到的第j时刻的状态量，/>为由第j-1时刻的状态量推算得到的第j时刻的状态量，Aj-1为构造的j-1时刻的中间矩阵，/>为第j-1时刻的状态量，/>为由第M时刻的状态量推算得到的第j-1时刻的状态量，Pj-1为第j-1时刻的状态误差均方差阵，/>为第j-1时刻到第j时刻的状态一步转移矩阵的转置矩阵，为Pj/j-1的逆矩阵，Pj-1/M为第M时刻到第j-1时刻的一步预测误差均方差阵，Pj/M为第M时刻到第j时刻的一步预测误差均方差阵，Pj/j-1为第j-1时刻到第j时刻的一步预测误差均方差阵，/>为Aj-1的转置矩阵；
令j＝M,M-1,…,1，通过上述固定区域最优平滑估计的迭代方程得到停车时正向导航经零速修正的当前时刻的逆向导航零速修正滤波器状态量在当前时刻的固定区域最优平滑估计即停车时正向导航经零速修正的当前时刻的逆向零速修正滤波器估计的状态量中的导航误差。
CN202210835644.7A 2022-07-15 2022-07-15 陆用捷联惯导基于逆向导航的零速修正在线平滑方法 Active CN115265588B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210835644.7A CN115265588B (zh) 2022-07-15 2022-07-15 陆用捷联惯导基于逆向导航的零速修正在线平滑方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210835644.7A CN115265588B (zh) 2022-07-15 2022-07-15 陆用捷联惯导基于逆向导航的零速修正在线平滑方法

Publications (2)

Publication Number Publication Date
CN115265588A CN115265588A (zh) 2022-11-01
CN115265588B true CN115265588B (zh) 2024-04-09

Family

ID=83765577

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210835644.7A Active CN115265588B (zh) 2022-07-15 2022-07-15 陆用捷联惯导基于逆向导航的零速修正在线平滑方法

Country Status (1)

Country Link
CN (1) CN115265588B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117570976B (zh) * 2024-01-16 2024-05-03 广东奥斯诺工业有限公司 基于逆向推算的超低速载体惯性定向方法

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2014178195A (ja) * 2013-03-14 2014-09-25 Mitsubishi Precision Co Ltd バイアス補正機能を備えた振動型ジャイロ
KR101737950B1 (ko) * 2015-11-20 2017-05-19 한국과학기술원 지형참조항법에서 영상 기반 항법해 추정 시스템 및 방법
CN109959374A (zh) * 2018-04-19 2019-07-02 北京理工大学 一种行人惯性导航全时全程逆向平滑滤波方法
CN112378400A (zh) * 2020-10-30 2021-02-19 湖南航天机电设备与特种材料研究所 一种双天线gnss辅助的捷联惯导组合导航方法
CN112697141A (zh) * 2020-12-16 2021-04-23 北京航空航天大学 基于逆向导航的惯导/里程计动基座姿态与位置对准方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2014178195A (ja) * 2013-03-14 2014-09-25 Mitsubishi Precision Co Ltd バイアス補正機能を備えた振動型ジャイロ
KR101737950B1 (ko) * 2015-11-20 2017-05-19 한국과학기술원 지형참조항법에서 영상 기반 항법해 추정 시스템 및 방법
CN109959374A (zh) * 2018-04-19 2019-07-02 北京理工大学 一种行人惯性导航全时全程逆向平滑滤波方法
CN112378400A (zh) * 2020-10-30 2021-02-19 湖南航天机电设备与特种材料研究所 一种双天线gnss辅助的捷联惯导组合导航方法
CN112697141A (zh) * 2020-12-16 2021-04-23 北京航空航天大学 基于逆向导航的惯导/里程计动基座姿态与位置对准方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
基于逆向导航解算和数据融合的SINS传递对准方法;孙进;徐晓苏;刘义亭;张涛;李瑶;;中国惯性技术学报;20151215(06);全文 *
正逆向滤波在惯性卫星组合导航中的应用;蒋鑫;刘海颖;岳亚洲;张墨起;;压电与声光;20170415(02);全文 *

Also Published As

Publication number Publication date
CN115265588A (zh) 2022-11-01

Similar Documents

Publication Publication Date Title
CN111156994B (zh) 一种基于mems惯性组件的ins/dr&gnss松组合导航方法
CN109974697B (zh) 一种基于惯性***的高精度测绘方法
CN112629538B (zh) 基于融合互补滤波和卡尔曼滤波的舰船水平姿态测量方法
US7328104B2 (en) Systems and methods for improved inertial navigation
CN110044376B (zh) 一种惯性导航设备的校正方法及装置
CN104764467B (zh) 空天飞行器惯性传感器误差在线自适应标定方法
CN112432642B (zh) 一种重力灯塔与惯性导航融合定位方法及***
CN111024074B (zh) 一种基于递推最小二乘参数辨识的惯导速度误差确定方法
CN111141273A (zh) 基于多传感器融合的组合导航方法及***
CN104215262A (zh) 一种惯性导航***惯性传感器误差在线动态辨识方法
CN107677292B (zh) 基于重力场模型的垂线偏差补偿方法
CN109708663B (zh) 基于空天飞机sins辅助的星敏感器在线标定方法
CN115265588B (zh) 陆用捷联惯导基于逆向导航的零速修正在线平滑方法
CN111722295B (zh) 一种水下捷联式重力测量数据处理方法
CN108303120B (zh) 一种机载分布式pos的实时传递对准的方法及装置
CN108225312B (zh) 一种gnss/ins松组合中杆臂估计以及补偿方法
CN113959462A (zh) 一种基于四元数的惯性导航***自对准方法
CN114777812B (zh) 一种水下组合导航***行进间对准与姿态估计方法
CN116105730A (zh) 基于合作目标卫星甚短弧观测的仅测角光学组合导航方法
Zheng et al. Compensation for stochastic error of gyros in a dual-axis rotational inertial navigation system
CN113566850A (zh) 惯性测量单元的安装角度标定方法、装置和计算机设备
CN114674313B (zh) 一种基于ckf算法的gps/bds和sins融合的无人配送车导航定位方法
CN113932803B (zh) 适用于高动态飞行器的惯性/地磁/卫星组合导航***
CN115950447A (zh) 基于磁罗盘与测速仪的水下动基座高精度对准方法及***
CN112197767B (zh) 一种在线改进滤波误差的滤波器设计方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant