CN111399023A - 基于李群非线性状态误差的惯性基组合导航滤波方法 - Google Patents

基于李群非线性状态误差的惯性基组合导航滤波方法 Download PDF

Info

Publication number
CN111399023A
CN111399023A CN202010312638.4A CN202010312638A CN111399023A CN 111399023 A CN111399023 A CN 111399023A CN 202010312638 A CN202010312638 A CN 202010312638A CN 111399023 A CN111399023 A CN 111399023A
Authority
CN
China
Prior art keywords
inertial
coordinate system
satellite
combined navigation
eci
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.)
Granted
Application number
CN202010312638.4A
Other languages
English (en)
Other versions
CN111399023B (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.)
National University of Defense Technology
Original Assignee
National University of Defense Technology
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 National University of Defense Technology filed Critical National University of Defense Technology
Priority to CN202010312638.4A priority Critical patent/CN111399023B/zh
Publication of CN111399023A publication Critical patent/CN111399023A/zh
Application granted granted Critical
Publication of CN111399023B publication Critical patent/CN111399023B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)

Abstract

一种基于李群非线性状态误差的惯性基组合导航滤波方法，包括：惯性/卫星组合导航系统中的卫星接收机和捷联惯导分别接收各自的数据信息；根据卫星接收机和捷联惯导接收的数据信息，建立惯性/卫星初始对准模型，完成基于李群非线性状态误差的惯性基组合导航滤波方法的初始对准；建立惯性/卫星组合导航模型，完成基于李群非线性状态误差的惯性基组合导航滤波方法的组合导航，将组合导航的姿态、速度、位置信息输出。本发明旨在通过严格李群意义下的非线性状态误差定义，进而解决传统的滤波方法在复杂环境下鲁棒性差的问题，以提高惯性基组合导航系统的精度。

Description

基于李群非线性状态误差的惯性基组合导航滤波方法
技术领域
本发明涉及组合导航非线性滤波技术领域，特别是涉及一种可以适用于复杂环境的基于李群非线性状态误差的惯性基组合导航滤波方法。
背景技术
随着无人载体，如无人车的应用场合越来越多，其行驶的场景越发多样，不仅局限于城市道路场景，更可能在复杂的场景下，如乡村道路，牧场草原，海滩沙漠等；这些环境相较于城市来说没有平坦的路面，没有固定不变的特征结构，没有地面基站外源信息，再加之野外气候变化较大，无人车特别是军用无人车如何实现在复杂的地形和恶劣的环境中完成高精度的导航与定位是一个值得研究的问题。
无人车常用的导航定位方法是多传感器信息融合。多传感器组合导航有不同的组合模式，比如惯性/卫星组合导航，惯性/视觉组合导航和惯性/轮式里程计组合导航等。相比于图优化的方法，扩展卡尔曼滤波在实时性上具有较大优势，所以其仍然是多传感器进行信息融合的常用手段。
扩展卡尔曼滤波其从最优估计的角度通过一系列含有噪声的传感器测量信息来对速度姿态位置等状态进行估计。然而，对于无人车特别是军用无人车在野外复杂的环境中进行长航时高精度导航与定位来说，常规的扩展卡尔曼滤波框架下的多源信息融合方案仍然存在一些问题亟待解决。其中一个重要的问题就是传统的惯性基组合导航系统导航状态误差定义为线性误差，进而导致在复杂恶劣的环境下的滤波鲁棒性较差。
因此，亟需设计一种能够解决传统的滤波方法在复杂环境下鲁棒性差的问题的技术方案。
发明内容
针对现有技术中惯性基组合导航系统的滤波方法在复杂环境下鲁棒性差的问题，本发明提供一种基于李群非线性状态误差的惯性基组合导航滤波方法。本发明旨在通过严格李群意义下的非线性状态误差定义，进而解决传统的滤波方法在复杂环境下鲁棒性差的问题，以提高惯性基组合导航系统的精度。
为了实现上述技术目的，本发明采用的技术方案是：
一种基于李群非线性状态误差的惯性基组合导航滤波方法，包括：
卫星接收机和捷联惯导分别接收各自的数据信息；
根据卫星接收机和捷联惯导接收的数据信息，建立惯性/卫星初始对准模型，完成基于李群非线性状态误差的惯性基组合导航滤波方法的初始对准；
建立惯性/卫星组合导航模型，完成基于李群非线性状态误差的惯性基组合导航滤波方法的组合导航，将组合导航的姿态、速度、位置信息输出。
本发明中，卫星接收机接收的数据信息包括：经度、纬度、高度和三维速度信息。捷联惯导接收的数据信息包括：三轴陀螺仪的角增量或角速度信息和三轴加速度计的比力或比力积分增量信息。
本发明中，惯性/卫星初始对准模型包括基于李群非线性状态误差的惯性/卫星组合导航系统模型和惯性/卫星组合导航系统观测模型。根据以上模型即可完成基于李群非线性状态误差的惯性基组合导航滤波方法的初始对准。
本发明中，惯性/卫星初始对准模型的建立方法为：
(1)惯性导航状态方程建立；
姿态表达采用
Figure BDA0002458418980000031
即载体系到地心地固坐标系ECEF的方向余弦矩阵，速度表达和位置的表达均采用载体相对于惯性系ECI的速度和位置在ECEF坐标系的投影；
方向余弦矩阵
Figure BDA0002458418980000032
对时间的导数为
Figure BDA0002458418980000033
其中，
Figure BDA0002458418980000034
为载体相对于ECI系的角速度在ECI系下的投影，
Figure BDA0002458418980000035
为地球自转角速度在ECEF系下的投影；
载体相对ECI坐标系的速度在ECI坐标系的投影
Figure BDA0002458418980000036
对时间的导数为：
Figure BDA0002458418980000037
其中，
Figure BDA0002458418980000038
为在ECI系下投影的引力加速度，其为ECI坐标系中笛卡尔位置的函数，fb为比力值，
Figure BDA0002458418980000039
为载体系到ECI坐标系的方向余弦矩阵；
将公式(2)定义的速度在ECEF系下投影，得到：
Figure BDA00024584189800000310
其中，公式(3)的推导中用到了
Figure BDA00024584189800000311
为ECEF坐标系到ECI坐标系的方向余弦矩阵；
Figure BDA00024584189800000312
为引力加速度在ECEF坐标系下的投影；
Figure BDA00024584189800000313
为载体相对ECI坐标系的速度在ECEF坐标系下的投影。
ECEF系下定义的载体位置随时间的导数为：
Figure BDA00024584189800000314
其中，
Figure BDA00024584189800000315
为ECI坐标系到ECEF坐标系的方向余弦矩阵，
Figure BDA00024584189800000316
为载体相对ECI坐标系的位置在ECEF坐标系的投影，
Figure BDA00024584189800000317
为载体相对ECI坐标系的位置在ECI坐标系的投影。
(2)状态误差定义及其微分方程；
传统的组合导航卡尔曼滤波中，通常直接估计导航参数的误差，卡尔曼滤波完之后，用估计出的误差直接对导航参数进行校正。基于李群非线性状态误差的滤波方法与传统卡尔曼滤波的标准流程一致，但是，卡尔曼滤波的方差预测更新，观测量的使用，以及导航参数的校正需要根据新的状态误差定义做相应调整。
定义李群下的与姿态、速度、位置对应的状态误差如下：
Figure BDA0002458418980000041
其中，
Figure BDA0002458418980000042
为ECEF坐标系到载体系的方向余弦矩阵的估计值，exp(·)代表矩阵的指数，φe为姿态误差；
Figure BDA0002458418980000043
为载体相对ECI坐标系的速度在载体系下投影的估计值，
Figure BDA0002458418980000044
为载体相对ECI坐标系的速度在ECEF坐标系下投影的估计值；
Figure BDA0002458418980000045
为载体相对ECI坐标系的位置在载体系下投影的估计值，
Figure BDA0002458418980000046
为载体相对ECI坐标系的位置在ECEF坐标系下投影的估计值；I为3乘3的单位矩阵。
设带有测量噪声的三轴陀螺仪测量的角速率
Figure BDA0002458418980000047
和三轴加速度计测量的比力
Figure BDA0002458418980000048
为：
Figure BDA0002458418980000049
其中，
Figure BDA00024584189800000410
为三轴陀螺仪测量误差，δfb为三轴加速度计测量误差，εb为三轴陀螺仪零偏，
Figure BDA00024584189800000411
为三轴加速度计零偏，wg＝[wgx wgy wgz]T和wa＝[wax way waz]T均为白噪声过程；
利用一阶近似
Figure BDA00024584189800000412
得到李群下的非线性姿态误差φe的微分方程为：
Figure BDA0002458418980000051
李群下的非线性速度误差
Figure BDA0002458418980000052
的微分方程为:
Figure BDA0002458418980000053
其中，
Figure BDA0002458418980000054
μ为引力常数；
李群下的非线性位置误差
Figure BDA0002458418980000055
的微分方程为：
Figure BDA0002458418980000056
(3)构建基于李群非线性状态误差的惯性/卫星组合导航系统模型；
三轴陀螺仪测量误差和三轴加速度计测量误差可以建模为随机游走过程:
Figure BDA0002458418980000057
根据公式(7)-(10)，得到基于李群非线性状态误差的惯性/卫星组合导航系统模型为：
Figure BDA0002458418980000058
其各变量的定义如下：
Figure BDA0002458418980000059
w＝[wgx wgy wgz wax way waz]T (13)
Figure BDA0002458418980000061
Figure BDA0002458418980000062
(4)构建基于李群非线性状态误差的惯性/卫星组合导航系统观测模型：
基于李群非线性状态误差的惯性/卫星组合导航，由惯导递推得到的导航状态
Figure BDA0002458418980000063
分别为载体相对ECI系并投影到ECEF坐标系的速度和位置，而由卫星接收机解算得到的速度和位置通常是载体相对ECEF坐标系并投影到ECEF坐标系或者投影到当地地理坐标系下的速度和位置。因此，为了将惯导与卫星的速度、位置求差作为观测，需要用到如下的ECEF和ECI坐标系之间状态转换公式：
Figure BDA0002458418980000064
其中，
Figure BDA0002458418980000065
为载体相对ECEF坐标系的位置在ECEF坐标系下投影的估计值，
Figure BDA0002458418980000066
为载体相对ECI坐标系的位置在ECI坐标系下投影的估计值，
Figure BDA0002458418980000067
为载体相对ECEF坐标系的位置在ECI坐标系下投影的估计值；
Figure BDA0002458418980000071
和
Figure BDA0002458418980000072
分别为载体相对ECI系的线性位置误差估计值和载体相对ECEF系的线性位置误差估计值在ECEF系的投影，二者相同，因为ECI和ECEF的坐标原点相同。
Figure BDA0002458418980000073
为载体相对ECI坐标系的速度在ECI坐标系下投影的估计值，
Figure BDA0002458418980000074
为载体相对ECEF坐标系的速度在ECI坐标系下投影的估计值；ωi ie为地球自转角速度在ECI坐标系下的投影；
Figure BDA0002458418980000075
为载体相对ECEF坐标系的速度在ECEF坐标系下投影的估计值；
Figure BDA0002458418980000076
和
Figure BDA0002458418980000077
分别为载体相对ECI系的线性速度误差估计值和载体相对ECEF系的线性速度误差估计值在ECEF系的投影。
基于李群非线性状态误差的惯性/卫星松组合观测模型为：
δz＝HxLGEKF-INS-ECEF+υ (17)
其中：
Figure BDA0002458418980000078
δzv和δzr分别为速度观测误差向量和位置观测误差向量，υv,3×1和υr,3×1分别为速度和位置观测白噪声。
公式(18)中的H矩阵是根据公式(19)得到，
Figure BDA0002458418980000079
进一步地，惯性/卫星组合导航系统在初始对准过程中，组合导航状态根据公式(20)进行更新。
Figure BDA00024584189800000710
根据以上模型即可完成惯性/卫星组合导航系统的初始对准。
在初始对准完成之后，进行本发明的第三步，建立惯性/卫星组合导航模型，完成基于李群非线性状态误差的惯性/卫星组合导航，将组合导航的姿态、速度、位置信息输出。本发明中，惯性/卫星组合导航模型与惯性/卫星初始对准模型完全相同，同样包括基于李群非线性状态误差的惯性/卫星组合导航系统模型和观测模型。初始对准过程和组合导航过程无需进行状态的切换，惯性/卫星组合导航系统一直保持15状态不变，这使得从初始对准到组合导航无需进行协方差的过渡，提高组合导航的收敛速度。惯性/卫星组合导航系统组合导航过程中每一次卡尔曼滤波完成之后，组合导航状态应根据公式(20)更新。通过以上方法，实现了复杂环境下捷联惯导的初始对准和惯性/卫星组合导航。本发明所涉及的滤波方法并不局限于惯性/卫星组合导航，可以根据量测信息的不同(比如里程计、视觉信息)灵活的修改观测矩阵。
与现有的绝大部分惯性基组合导航系统滤波方法不同的是，本发明所定义的惯性导航状态误差是李群意义下的非线性误差，因而比传统的线性状态误差定义更为严格。
本发明所涉及的非线性滤波方法适用于惯性与任何传感器的组合导航系统，如惯性/卫星、惯性/里程计和惯性/视觉组合导航系统等等。
与现有技术相比，本发明具有以下优点：
1)本发明中惯性导航状态的定义形式满足法国学者最新提出的不变性理论，同时，惯性导航的状态误差定义更为严格，进而可以克服某些不利环境如不完全可观对组合导航精度的损害；
2)本发明中的组合导航系统模型中不再还有比力项，而是被引力相关项替代，使得组合导航系统可以更好的适用于复杂的动态环境，克服比力剧烈变化对滤波的不利影响。
3)本发明方法实现容易，并且计算量小。传统的卡尔曼滤波系统矩阵需要以高频率进行预测使得系统可以跟踪比力的动态变化，而基于李群非线性状态误差下的滤波预测和更新过程可以同时进行，提高运算效率。
本发明所提出的方法具有很强的工程应用灵活性和适用性，可以灵活的扩展于惯性与其它传感器组合导航中，如惯性/视觉、惯性/里程计组合导航等。
附图说明
图1为本发明的流程图。
图2为STIM300 MEMS-IMU/卫星组合导航实验轨迹图。
图3为对比试验中分别采用传统卡尔曼滤波方法和基于李群非线性状态误差的惯性基组合导航滤波方法得到的组合导航航向角结果图。
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚明白，下面将以附图及详细叙述清楚说明本发明所揭示内容的精神，任何所属技术领域技术人员在了解本发明内容的实施例后，当可由本发明内容所教示的技术，加以改变及修饰，其并不脱离本发明内容的精神与范围。本发明的示意性实施例及其说明用于解释本发明，但并不作为对本发明的限定。
参照图1是本实施例的流程图，一种基于李群非线性状态误差的惯性基组合导航滤波方法，具体包括如下步骤：
步骤一，初始化数据。
惯性/卫星组合导航系统开始输出捷联惯导和卫星接收机的数据。捷联惯导系统输出载体系(前-右-下)的陀螺和加表数据，以北-东-地(N-E-D)当地地理坐标系为导航系，卫星接收机输出经度、纬度、高度、北向、东向、地向的速度信息。
步骤二，完成基于李群非线性状态误差的惯性基组合导航滤波方法的初始对准。
由于MEMS-IMU无法完成自对准，这里利用2分钟的MEMS静止测量数据进行水平姿态角的初始估计，航向角初值由偏振光航向得到。这里需要说明的是，如果所使用的组合导航系统在正式进入组合导航之前需要完成高精度的自对准，则初始自对准过程与步骤三中的基于李群非线性状态误差的惯性基组合导航滤波方法的组合导航过程完全一致，即初始对准过程中，在每一次卡尔曼滤波完成之后，组合导航状态应根据式(20)进行更新。
步骤三，基于李群非线性状态误差的惯性基组合导航滤波方法的组合导航。
惯性/卫星组合导航模型包括基于李群非线性状态误差的惯性/卫星组合导航系统模型和惯性/卫星组合导航观测模型。对于高精度惯导的组合导航，初始对准过程和组合导航过程无需进行滤波状态的切换，惯性/卫星组合导航系统一直保持15状态不变。
卡尔曼滤波的15状态选择为：
Figure BDA0002458418980000101
分别对应非线性的姿态误差向量，非线性的速度误差向量，非线性的位置误差向量，陀螺零偏向量，加表零偏向量。
基于李群非线性状态误差的组合导航系统方程为：
Figure BDA0002458418980000111
其中各分量已经在公式(12)-(15)进行了定义。注意，公式(12)中的陀螺零偏和加速度计零偏可以建模为常值过程亦可以建模为一阶马尔科夫过程，这取决于陀螺和加速度计的性能。本发明仅以将陀螺零偏和加速度计零偏建模为常值过程为例。同时，从公式(14)可以看出，系统矩阵中不存在比力项，而是引力相关项，这对于区域导航来说，特别是在有强振动时，可以保证滤波的鲁棒性。
基于李群非线性状态误差的观测方程为：δz＝HxLGEKF-INS-ECEF+υ
观测状态为惯导与卫星的速度误差和位置误差，共6个状态，观测矩阵为：
Figure BDA0002458418980000112
卡尔曼滤波具体实施即为标准的五公式的卡尔曼滤波，这里不再赘述。
惯性/卫星组合导航系统组合导航过程中每一次卡尔曼滤波完成之后，组合导航状态应根据下式进行更新：
Figure BDA0002458418980000113
步骤四，将组合导航的姿态、速度、位置信息输出。
为验证本发明所提供方法的有效性，以某次Stim300 MEMS-IMU与卫星组合导航实验为例，对比本发明提供的新的组合导航滤波方案与传统的组合导航滤波方案性能。IMU输出频率为200Hz,GPS输出位置速度频率为1Hz。跑车轨迹如图2所示。卡尔曼滤波的预测频率和更新频率均设置为1Hz。对比两种滤波方案在大初始航向角误差环境下的滤波性能，将初始航向角误差设置为30°，航向角估计结果如图3所示。
从图3可以看出，在初始的准静态环境下(0-1040s)，EKF和LG-EKF的航向角估计性能差别不大，因为航向角在准静态环境下不可观，所以滤波无法收敛到正确的航向值附近。但是从1040s载车开始出发后，航向角变的可观，从图3左下角的局部图可以看出，LG-EKF的航向角收敛速度要比EKF的航向角收敛速度快，而且估计精度高。从右下角航向角估计的局部图1780s-1860s，亦可以看出LG-EKF的航向角估计精度比EKF的航向角估计精度更高。显然，基于李群非线性误差的卡尔曼滤波具有在不利环境下更好的航向角估计性能。
以上所述仅为本发明的优选实施例，并非因此限制本发明的专利范围，凡是在本发明的发明构思下，利用本发明说明书及附图内容所作的等效结构变换，或直接/间接运用在其他相关的技术领域均包括在本发明的专利保护范围内。

Claims (8)

1.一种基于李群非线性状态误差的惯性基组合导航滤波方法，其特征在于，包括：
惯性/卫星组合导航系统中的卫星接收机和捷联惯导分别接收各自的数据信息；
根据卫星接收机和捷联惯导接收的数据信息，建立惯性/卫星初始对准模型，完成基于李群非线性状态误差的惯性基组合导航滤波方法的初始对准；
建立惯性/卫星组合导航模型，完成基于李群非线性状态误差的惯性基组合导航滤波方法的组合导航，将组合导航的姿态、速度、位置信息输出。
2.根据权利要求1所述的基于李群非线性状态误差的惯性基组合导航滤波方法，其特征在于，卫星接收机接收的数据信息包括：经度、纬度、高度和三维速度信息；捷联惯导接收的数据信息包括：三轴陀螺仪的角增量或角速度信息和三轴加速度计的比力或比力积分增量信息。
3.根据权利要求1所述的基于李群非线性状态误差的惯性基组合导航滤波方法，其特征在于，惯性/卫星初始对准模型包括基于李群非线性状态误差的惯性/卫星组合导航系统模型和惯性/卫星组合导航系统观测模型，根据惯性/卫星初始对准模型即可完成基于李群非线性状态误差的惯性基组合导航滤波方法的初始对准。
4.根据权利要求3所述的基于李群非线性状态误差的惯性基组合导航滤波方法，其特征在于，基于李群非线性状态误差的惯性/卫星组合导航系统模型的建立方法为：
(1)惯性导航状态方程建立；
姿态表达采用
Figure FDA0002458418970000011
即载体系到地心地固坐标系ECEF的方向余弦矩阵，速度表达和位置的表达均采用载体相对于惯性系ECI的速度和位置在ECEF坐标系的投影；
方向余弦矩阵
Figure FDA0002458418970000021
对时间的导数为
Figure FDA0002458418970000022
其中，
Figure FDA0002458418970000023
为载体相对于ECI系的角速度在ECI系下的投影，
Figure FDA0002458418970000024
为地球自转角速度在ECEF系下的投影；
载体相对ECI坐标系的速度在ECI坐标系的投影
Figure FDA0002458418970000025
对时间的导数为：
Figure FDA0002458418970000026
其中，
Figure FDA0002458418970000027
为在ECI系下投影的引力加速度，其为ECI坐标系中笛卡尔位置的函数，fb为比力值，
Figure FDA0002458418970000028
为载体系到ECI坐标系的方向余弦矩阵；
将公式(2)定义的速度在ECEF系下投影，得到：
Figure FDA0002458418970000029
其中，公式(3)的推导中用到了
Figure FDA00024584189700000210
Figure FDA00024584189700000211
为ECEF坐标系到ECI坐标系的方向余弦矩阵；
Figure FDA00024584189700000212
为引力加速度在ECEF坐标系下的投影；
Figure FDA00024584189700000213
为载体相对ECI坐标系的速度在ECEF坐标系下的投影；
ECEF系下定义的载体位置随时间的导数为：
Figure FDA00024584189700000214
其中，
Figure FDA00024584189700000215
为ECI坐标系到ECEF坐标系的方向余弦矩阵，
Figure FDA00024584189700000216
为载体相对ECI坐标系的位置在ECEF坐标系的投影，
Figure FDA00024584189700000217
为载体相对ECI坐标系的位置在ECI坐标系的投影；
(2)状态误差定义及其微分方程；
定义李群下的与姿态、速度、位置对应的状态误差如下：
Figure FDA0002458418970000031
其中，
Figure FDA0002458418970000032
为ECEF坐标系到载体系的方向余弦矩阵的估计值，exp(·)代表矩阵的指数，φe为姿态误差；
Figure FDA0002458418970000033
为载体相对ECI坐标系的速度在载体系下投影的估计值，
Figure FDA0002458418970000034
为载体相对ECI坐标系的速度在ECEF坐标系下投影的估计值；
Figure FDA0002458418970000035
为载体相对ECI坐标系的位置在载体系下投影的估计值，
Figure FDA0002458418970000036
为载体相对ECI坐标系的位置在ECEF坐标系下投影的估计值；I为3乘3的单位矩阵；
设带有测量噪声的三轴陀螺仪测量的角速率
Figure FDA0002458418970000037
和三轴加速度计测量的比力
Figure FDA0002458418970000038
为：
Figure FDA0002458418970000039
其中，
Figure FDA00024584189700000310
为三轴陀螺仪测量误差，δfb为三轴加速度计测量误差，εb为三轴陀螺仪零偏，
Figure FDA00024584189700000311
为三轴加速度计零偏，wg＝[wgx wgy wgz]T和wa＝[wax way waz]T均为白噪声过程；
利用一阶近似
Figure FDA00024584189700000312
得到李群下的非线性姿态误差φe的微分方程为：
Figure FDA00024584189700000313
李群下的非线性速度误差
Figure FDA00024584189700000314
的微分方程为:
Figure FDA00024584189700000315
其中，
Figure FDA0002458418970000041
μ为引力常数；
李群下的非线性位置误差
Figure FDA0002458418970000042
的微分方程为：
Figure FDA0002458418970000043
(3)构建基于李群非线性状态误差的惯性/卫星组合导航系统模型；
三轴陀螺仪测量误差和三轴加速度计测量误差可以建模为随机游走过程:
Figure FDA0002458418970000044
根据公式(7)-(10)，得到基于李群非线性状态误差的惯性/卫星组合导航系统模型为：
Figure FDA0002458418970000045
其各变量的定义如下：
Figure FDA0002458418970000046
w＝[wgx wgy wgz wax way waz]T (13)
Figure FDA0002458418970000047
Figure FDA0002458418970000048
5.根据权利要求4所述的基于李群非线性状态误差的惯性基组合导航滤波方法，其特征在于，构建基于李群非线性状态误差的惯性/卫星组合导航系统观测模型方法为：
将惯导与卫星的速度、位置求差作为观测，需要用到如下的ECEF和ECI坐标系之间状态转换公式：
Figure FDA0002458418970000051
其中，
Figure FDA0002458418970000052
为载体相对ECEF坐标系的位置在ECEF坐标系下投影的估计值，
Figure FDA0002458418970000053
为载体相对ECI坐标系的位置在ECI坐标系下投影的估计值，
Figure FDA0002458418970000054
为载体相对ECEF坐标系的位置在ECI坐标系下投影的估计值；
Figure FDA0002458418970000055
和
Figure FDA0002458418970000056
分别为载体相对ECI系的线性位置误差估计值和载体相对ECEF系的线性位置误差估计值在ECEF系的投影，二者相同，因为ECI和ECEF的坐标原点相同；
Figure FDA0002458418970000057
为载体相对ECI坐标系的速度在ECI坐标系下投影的估计值，
Figure FDA0002458418970000058
为载体相对ECEF坐标系的速度在ECI坐标系下投影的估计值；
Figure FDA0002458418970000059
为地球自转角速度在ECI坐标系下的投影；
Figure FDA00024584189700000510
为载体相对ECEF坐标系的速度在ECEF坐标系下投影的估计值；
Figure FDA00024584189700000511
和
Figure FDA00024584189700000512
分别为载体相对ECI系的线性速度误差估计值和载体相对ECEF系的线性速度误差估计值在ECEF系的投影；
基于李群非线性状态误差的惯性/卫星松组合观测模型为：
δz＝HxLGEKF-INS-ECEF+υ (17)
其中：
Figure FDA00024584189700000513
δzv和δzr分别为速度观测误差向量和位置观测误差向量，υv,3×1和υr,3×1分别为速度和位置观测白噪声；
公式(18)中的H矩阵是根据公式(19)得到：
Figure FDA0002458418970000061
6.根据权利要求5所述的基于李群非线性状态误差的惯性基组合导航滤波方法，其特征在于，惯性/卫星组合导航系统在初始对准过程中，组合导航状态根据公式(20)进行更新：
Figure FDA0002458418970000062
7.根据权利要求5或6所述的基于李群非线性状态误差的惯性基组合导航滤波方法，其特征在于，惯性/卫星组合导航模型与惯性/卫星初始对准模型相同，包括基于李群非线性状态误差的惯性/卫星组合导航系统模型和惯性/卫星组合导航系统观测模型；初始对准过程和组合导航过程无需进行状态的切换，惯性/卫星组合导航系统一直保持15状态不变。
8.根据权利要求6所述的基于李群非线性状态误差的惯性基组合导航滤波方法，其特征在于，惯性/卫星组合导航系统在组合导航过程中，组合导航状态根据公式(20)进行更新。
CN202010312638.4A 2020-04-20 2020-04-20 基于李群非线性状态误差的惯性基组合导航滤波方法 Active CN111399023B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010312638.4A CN111399023B (zh) 2020-04-20 2020-04-20 基于李群非线性状态误差的惯性基组合导航滤波方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010312638.4A CN111399023B (zh) 2020-04-20 2020-04-20 基于李群非线性状态误差的惯性基组合导航滤波方法

Publications (2)

Publication Number Publication Date
CN111399023A true CN111399023A (zh) 2020-07-10
CN111399023B CN111399023B (zh) 2022-02-08

Family

ID=71431632

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010312638.4A Active CN111399023B (zh) 2020-04-20 2020-04-20 基于李群非线性状态误差的惯性基组合导航滤波方法

Country Status (1)

Country Link
CN (1) CN111399023B (zh)

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112082548A (zh) * 2020-09-10 2020-12-15 中国人民解放军海军航空大学 一种无人机惯导与gps混合高度测量方法
CN112212889A (zh) * 2020-09-16 2021-01-12 北京工业大学 基于特殊正交群最优估计的sins捷联惯性导航系统晃动基座粗对准方法
CN113157683A (zh) * 2021-05-12 2021-07-23 北京冠群信息技术股份有限公司 一种定位纠偏的方法及系统
CN113253325A (zh) * 2021-06-24 2021-08-13 中国人民解放军国防科技大学 惯性卫星序贯紧组合李群滤波方法
CN114413892A (zh) * 2022-01-19 2022-04-29 东南大学 一种新型果园机器人组合导航方法

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101561496A (zh) * 2009-05-21 2009-10-21 北京航空航天大学 一种伪卫星和惯性组合导航系统的非线性补偿方法
CN106871928A (zh) * 2017-01-18 2017-06-20 北京工业大学 基于李群滤波的捷联惯性导航初始对准方法
CN107588771A (zh) * 2017-08-28 2018-01-16 北京工业大学 基于李群描述的捷联惯性导航解算方法
CN107690567A (zh) * 2015-04-01 2018-02-13 赛峰电子与防务公司 利用扩展卡尔曼滤波器用于对移动载体设备的航行进行追踪的方法
CN108318027A (zh) * 2017-01-18 2018-07-24 腾讯科技（深圳）有限公司 载体的姿态数据的确定方法和装置
CN110595503A (zh) * 2019-08-05 2019-12-20 北京工业大学 基于李群最优估计的sins捷联惯性导航系统晃动基座自对准方法

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101561496A (zh) * 2009-05-21 2009-10-21 北京航空航天大学 一种伪卫星和惯性组合导航系统的非线性补偿方法
CN107690567A (zh) * 2015-04-01 2018-02-13 赛峰电子与防务公司 利用扩展卡尔曼滤波器用于对移动载体设备的航行进行追踪的方法
US20180095159A1 (en) * 2015-04-01 2018-04-05 Safran Electronics & Defense Method for tracking the navigation of a mobile carrier with an extended kalman filter
CN106871928A (zh) * 2017-01-18 2017-06-20 北京工业大学 基于李群滤波的捷联惯性导航初始对准方法
CN108318027A (zh) * 2017-01-18 2018-07-24 腾讯科技（深圳）有限公司 载体的姿态数据的确定方法和装置
CN107588771A (zh) * 2017-08-28 2018-01-16 北京工业大学 基于李群描述的捷联惯性导航解算方法
CN110595503A (zh) * 2019-08-05 2019-12-20 北京工业大学 基于李群最优估计的sins捷联惯性导航系统晃动基座自对准方法

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
MAOSONG WANG ET AL.: "State Transformation Extended Kalman Filter for SINS based Integrated Navigation System", 《2019 DGON INERTIAL SENSORS AND SYSTEMS (ISS)》 *
王茂松等: "状态变换卡尔曼滤波的进一步解释及应用", 《中国惯性技术学报》 *
袁治晴: "李群李代数在数据融合算法中的应用分析", 《电脑知识与技术》 *
陈雨等: "基于惯性参考系线性滤波传递对准方法", 《系统工程与电子技术》 *

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112082548A (zh) * 2020-09-10 2020-12-15 中国人民解放军海军航空大学 一种无人机惯导与gps混合高度测量方法
CN112212889A (zh) * 2020-09-16 2021-01-12 北京工业大学 基于特殊正交群最优估计的sins捷联惯性导航系统晃动基座粗对准方法
CN113157683A (zh) * 2021-05-12 2021-07-23 北京冠群信息技术股份有限公司 一种定位纠偏的方法及系统
CN113253325A (zh) * 2021-06-24 2021-08-13 中国人民解放军国防科技大学 惯性卫星序贯紧组合李群滤波方法
CN113253325B (zh) * 2021-06-24 2021-09-17 中国人民解放军国防科技大学 惯性卫星序贯紧组合李群滤波方法
CN114413892A (zh) * 2022-01-19 2022-04-29 东南大学 一种新型果园机器人组合导航方法
CN114413892B (zh) * 2022-01-19 2024-01-02 东南大学 一种新型果园机器人组合导航方法

Also Published As

Publication number Publication date
CN111399023B (zh) 2022-02-08

Similar Documents

Publication Publication Date Title
CN111399023B (zh) 基于李群非线性状态误差的惯性基组合导航滤波方法
Wang et al. Consistent ST-EKF for long distance land vehicle navigation based on SINS/OD integration
CN109813311B (zh) 一种无人机编队协同导航方法
CN109459044B (zh) 一种gnss双天线辅助的车载mems惯导组合导航方法
US8374783B2 (en) Systems and methods for improved position determination of vehicles
CN108180925A (zh) 一种里程计辅助车载动态对准方法
CN103196445B (zh) 基于匹配技术的地磁辅助惯性的载体姿态测量方法
CN112629538A (zh) 基于融合互补滤波和卡尔曼滤波的舰船水平姿态测量方法
CN109596144B (zh) Gnss位置辅助sins行进间初始对准方法
Li et al. Observability analysis of non-holonomic constraints for land-vehicle navigation systems
CN103822633A (zh) 一种基于二阶量测更新的低成本姿态估计方法
CN111982106A (zh) 导航方法、装置、存储介质及电子装置
CN110006421B (zh) 一种基于mems和gps的车载导航方法及系统
CN110207691A (zh) 一种基于数据链测距的多无人车协同导航方法
CN110221331B (zh) 基于状态变换的惯性/卫星组合导航动态滤波方法
CN111207745A (zh) 一种适用于大机动无人机垂直陀螺仪的惯性测量方法
Gao et al. An integrated land vehicle navigation system based on context awareness
CN109959374A (zh) 一种行人惯性导航全时全程逆向平滑滤波方法
CN111220151B (zh) 载体系下考虑温度模型的惯性和里程计组合导航方法
CN106646569B (zh) 一种导航定位方法及设备
Kubo et al. DGPS/INS/VVheelSensor Integrationfor High Accuracy Land-Vehicle Positioning
CN112229421A (zh) 基于李群最优估计的捷联惯性导航晃动基座粗对准方法
CN114111792B (zh) 一种车载gnss/ins/里程计组合导航方法
CN111256708A (zh) 一种基于射频识别的车载组合导航方法
CN114994732B (zh) 基于gnss载波相位的车载航向快速初始化装置及方法

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