CN116499493A - 一种基于低频逆向滤波的惯导快速对准方法 - Google Patents
一种基于低频逆向滤波的惯导快速对准方法 Download PDFInfo
- Publication number
- CN116499493A CN116499493A CN202310247417.7A CN202310247417A CN116499493A CN 116499493 A CN116499493 A CN 116499493A CN 202310247417 A CN202310247417 A CN 202310247417A CN 116499493 A CN116499493 A CN 116499493A
- Authority
- CN
- China
- Prior art keywords
- navigation
- inertial
- matrix
- alignment
- inertial navigation
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 84
- 238000001914 filtration Methods 0.000 title claims abstract description 25
- 230000008569 process Effects 0.000 claims abstract description 25
- 230000002441 reversible effect Effects 0.000 claims abstract description 24
- 239000011159 matrix material Substances 0.000 claims description 62
- 230000001133 acceleration Effects 0.000 claims description 27
- 238000004364 calculation method Methods 0.000 claims description 23
- 230000005484 gravity Effects 0.000 claims description 18
- 238000005070 sampling Methods 0.000 claims description 10
- 230000009466 transformation Effects 0.000 claims description 4
- 238000005452 bending Methods 0.000 claims description 3
- 238000007906 compression Methods 0.000 claims description 3
- 230000006835 compression Effects 0.000 claims description 3
- 238000013178 mathematical model Methods 0.000 claims description 3
- 238000005259 measurement Methods 0.000 claims description 3
- 238000007711 solidification Methods 0.000 claims description 3
- 230000008023 solidification Effects 0.000 claims description 3
- 238000003491 array Methods 0.000 claims description 2
- 238000013144 data compression Methods 0.000 abstract description 2
- 230000009467 reduction Effects 0.000 abstract description 2
- 239000000284 extract Substances 0.000 abstract 1
- 238000010586 diagram Methods 0.000 description 3
- 230000007704 transition Effects 0.000 description 2
- 238000013500 data storage Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000006870 function Effects 0.000 description 1
- 230000008092 positive effect Effects 0.000 description 1
- 238000002360 preparation method Methods 0.000 description 1
- 230000002035 prolonged effect Effects 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 230000026676 system process Effects 0.000 description 1
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
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
本发明涉及一种基于低频逆向滤波的惯导快速对准方法，构建了基于低频逆向滤波来实现机载振动环境下的捷联惯导快速对准，通过设计低频数据压缩方法，在正向粗对准过称中将短时间内的高频数据压缩成1Hz数据，占用空间<3KB，然后粗对准完成后，提取存储的1Hz数据进行数据还原，同时基于存储的数据在短时间进行低频的(1Hz)正向、逆向无阻尼解算和正向和逆向滤波，借助复用数据来间接“延长”对准时间，解决了由于振动环境导致滤波器难以在短时间内收敛的问题，同时由于低频回溯过程需要的运算资源和存储空间很小，可以在不改变现有的硬件水平缩短对准时间和提高对准精度。
Description
技术领域
本发明属于捷联惯性导航系统快速初始对准技术领域，尤其是一种基于低频逆向滤波的惯导快速对准方法。
背景技术
在车载、机载军事装备在投入使用前，其搭载的导航系统在执行任务之前需要进行初始准备过程，包括初始化姿态角，速度、位置信息，初始对准过程。对于传统的初始对准一般包括粗对准和精对准两个过程，对准时间一般不少于5min，且对准精度与对准环境和惯导系统装备的惯性元件如加速度计和陀螺的精度和噪声水平有直接关系，为了达到对准精度和时间的要求，对准需要在静基座下进行，如果存在扰动和惯性元件噪声增大，会导致对准精度降低甚至对准失败。
为了实现短时间快速对准功能，许多文献采用了逆向导航的算法，将惯性元件数据存储起来实现数据复用，由于存储惯性元件频率一般100Hz～1kHz，占用存储空间较大，同时在逆向导航同时需要进行正常导航计算过程，对导航计算机处理能力提出了较高的要求，为此有的文献采用了1Hz存储然后回溯导航滤波方案，但只是通过节省了粗对准时间来有限的延长精对准滤波器时间，在粗对准结果较差或总对准时间较少(小于或等于3min)情况下，未能充分发挥存储数据复用的优势。
发明内容
本发明的目的在于克服现有技术的不足，提出一种基于低频逆向滤波的惯导快速对准方法，通过低频存储结合低频导航解算和正向逆向滤波器的快速初始对准算法，能将对准算法中精对准时间有效延长，从而实现快速对准算法。
本发明解决其技术问题是采取以下技术方案实现的：
一种基于低频逆向滤波的惯导快速对准方法，包括以下步骤：
步骤1、t0时刻设备通电，启动正向粗对准过程，正向粗对准以200Hz陀螺和加速度计采样频率计算t0时刻的惯导系统的初始姿态矩阵
步骤2、在步骤1进行的同时，对陀螺和加速度计200Hz的信息进行压缩处理，并存储到导航计算机的存储空间内，存储速率为7float/s；
步骤3、在对准第180s时，锁存步骤1中t0时刻的始姿态矩阵同时读取步骤2存储的压缩数据并进行还原，然后启动回溯正向和逆向无阻尼惯导解算；
步骤4、在步骤3启动回溯正向和逆向无阻尼惯导解算的同时，启动回溯卡尔曼滤波器，对步骤1计算得到的进行误差估计；
步骤5、在完成回溯滤波后，对估计的误差进行修正，并利用步骤1存储t＝181s～184s的数据进行正向导航追溯，完成整个初始对准过程。
而且，所述步骤1的具体方法为：以惯性坐标系作为参考基准，初始固定两个惯性坐标系，之后更新载体相对惯性坐标系的角运动四元数，并利用重力和载体线运动在两个惯性坐标系间的变换关系求取两个凝固惯性坐标系的变换阵，进而得到
而且，所述的具体计算方法为：
使用姿态更新算法计算
其中，为惯导系统对准t时刻的导航坐标系n系相对初始时刻导航惯性系in0系的姿态矩阵，初始t0时刻时，/>为3x3的单位矩阵，λt为惯导系统对准t时刻的所处的位置的经度；λ0为惯导系统对准t0时刻的所处的位置的经度；ωie为地球自转角速率；t为惯导系统对准时长；Lt为惯导系统对准t时刻的所处的位置的纬度；
姿态四元数更新计算方法计算
其中：[a×]符号表示的是矢量a的反对称矩阵；为惯导载体系b系相对初始时刻载体惯性系(ib0)的姿态矩阵；/>为陀螺敏感的载体角速度，对准开始时刻ib0坐标系与b坐标系重合，/>I3×3为3x3的单位阵；
将载体系b系下的比力投影到导航坐标系n系下有：
其中：fb为动基座下加速度计输出；gb为重力在b坐标系上投影；为载体线运动加速度；/>为惯导加速度计输出比力在ib0上的投影；
设上式两边同时取积分得到：
其中：为比力矢量的积分在ib0系下的投影的矢量；tk为对准k时刻的时间；/>为初始时刻载体惯性系ib0系相对初始时刻导航惯性系in0系的姿态矩阵；/>为惯导载体系b系相对初始时刻载体惯性系ib0系的姿态矩阵；/>为当地重力加速度矢量在初始时刻导航惯性系in0下的投影；
在载体做匀速运动或等幅摇摆运动条件下，得到：
其中； 为当地重力加速度矢量在初始时刻导航惯性系in0系下的投影；/>为/>的转置矩阵，/>gn为当地重力加速度在导航坐标系n系的投影；g为当地重力加速度大小；
根据惯导解算速度基本方程得到：
其中：为惯导系统导航坐标系n系的速度增量；vn为惯导系统导航坐标系n系的速度；/>为载体运动角速率在导航坐标系n系下的投影；/>地球自转角速率在导航坐标系n系下的投影；fn载体敏感的加速度在导航坐标系n系下的投影；
上式两边积分同时乘以得到：
其中，为步骤1所要求的姿态矩阵，/>为加速度在导航惯性系in0系下的投影，有/> 为外参考对地速度，在静基座条件下，/>
分别设：
得到：
任意取对准过程中的两个时刻点得到
而且，所述步骤2使用低频存储数据处理算法压缩存储数据。
而且，所述低频存储数据处理算法的具体实现方法为：间隔1s分别存储加速度输出的在ib0坐标系投影的比力之和以及t时刻对应的四元数q：
其中：为第k秒储存的比力和；/>为第k秒存储的四元数大小；Δt为存储周期1s，并将要存储的数据转换成浮点型，其中/>占用3个浮点型空间，q占用4个浮点型空间。
而且，所述步骤3的具体实现方法为：先进行低频正向导航算法，每个采样周期读取导航计算机存储的加速计和陀螺姿态更新的数据，通过地理坐标系的机械编排计算地理系下的速度和位置信息，待数据读到末尾，将正向导航输出的速度信息取反，同时将地球自转角速率取反：
其中，为逆向导航解算的速度，/>为正向导航解算的速度，/>代表导航系下地球自转角速率；其中基于1Hz数据进行当地地理系机械编排过程为：
速度更新：
其中，为k时刻无阻尼解算的速度；/>为采样周期内读取存储的加速度大小；Ts为计算周期，Ts＝1s；
位置更新：
其中:pk为k时刻的位置矢量，L,λ,h分别为载体所处的纬度、经度和高度；Mpv为位置矢量矩阵，/>RM,RN分别为地球子午圈主曲率半径和卯酉圈主曲率半径；
惯性系姿态更新：惯性系姿态更新为更新初始时刻in0系相对地理坐标系n系之间的姿态矩阵对准初始时刻矩阵为/>为单位阵，初始时刻四元数/>迭代计算/>的四元数过程为：
计算每个存储时间间隔的
其中:表示导航系相对于惯性系的旋转矩阵，它包含两部分：地球自转引起的导航系旋转，以及系统在地球表面附近移动因地球表面弯曲引起的导航系旋转；vN、vE为惯导速度，通过上一节速度更新得到；计算得/>后采用四元数更新算法更新/>
令求取四元数有：
其中，将上式进行泰勒展开，得四阶近似：
根据t-1时刻计算t时刻的/>
设将上式得到的四元数转换成姿态矩阵求得/>
而且，所述步骤4的回溯卡尔曼滤波器具体实现方法为：
选取精对准中回溯卡尔曼滤波器的状态变量为：
选择的状态变量包括姿态角误差量φE、φN、φU，导航通道的东向、北向和垂向速度误差δvE、δvN、δvU，位置坐标点纬度、经度和高度误差δL、δλ、δh，陀螺常值漂移误差εx、εy、εz，加速度计测量常值偏差
回溯卡尔曼滤波的状态方程为：
其中，和/>为陀螺漂移和加速度计偏置噪声方差；flag为正向逆向导航标志位取值，正向导航时flag＝1，逆向导航时flag＝-1；
快速初始对准选取零速或者北斗速度作为观测量则回溯卡尔曼滤波的量测方程为：
其中，H＝[03×3flag·I3×303×9]；V为速度观测白噪声；flag为正向逆向导航标志位取值；
此时回溯卡尔曼滤波器的状态方程为：
观测方程为：
Zp＝HX+V
其中F、G状态转移矩阵和噪声矩阵采用惯导算法通用的数学模型。
本发明的优点和积极效果是：
本发明构建了基于低频逆向滤波来实现机载振动环境下的捷联惯导快速对准，通过设计低频数据压缩方法，在正向粗对准过称中将短时间内的高频数据压缩成1Hz数据，占用空间<3KB，然后粗对准完成后，提取存储的1Hz数据进行数据还原，同时基于存储的数据在短时间进行低频的(1Hz)正向、逆向无阻尼解算和正向和逆向滤波，借助复用数据来间接“延长”对准时间，解决了由于振动环境导致滤波器难以在短时间内收敛的问题，同时由于低频回溯过程需要的运算资源和存储空间很小，可以在不改变现有的硬件水平缩短对准时间和提高对准精度。
附图说明
图1为本发明的流程图；
图2为本发明的算法流程图；
图3为本发明的正向粗对准流程图；
图4为本发明低频正向逆向导航算法示意图；
图5为本发明回溯卡尔曼滤波基本原理示意图。
具体实施方式
以下结合附图对本发明做进一步详述。
一种基于低频逆向滤波的惯导快速对准方法，如图1所示，包括四部分：①正向粗对准、②低频数据存储算法、③正向导航和逆向导航机械编排和④正向和逆向卡尔曼滤波算法。
其具体实现方法，如图2所示，包括以下步骤：
步骤1、t0时刻设备通电，启动正向粗对准过程，正向粗对准以200Hz陀螺和加速度计采样频率计算t0时刻的惯导系统的初始姿态矩阵步骤1如图1所示的线程1。
本步骤的具体实现方法为：以惯性坐标系作为参考基准，初始固定两个惯性坐标系，之后更新载体相对惯性坐标系的角运动四元数，并利用重力和载体线运动在两个惯性坐标系间的变换关系求取两个凝固惯性坐标系的变换阵，进而得到如图3所示，为基于惯性凝固坐标系的粗对准流程。
的具体计算方法为：
使用姿态更新算法计算
其中，为惯导系统对准t时刻的导航坐标系n系相对初始时刻导航惯性系in0系的姿态矩阵，初始t0时刻时，/>为3x3的单位矩阵，λt为惯导系统对准t时刻的所处的位置的经度；λ0为惯导系统对准t0时刻的所处的位置的经度；ωie为地球自转角速率；t为惯导系统对准时长；Lt为惯导系统对准t时刻的所处的位置的纬度；
姿态四元数更新计算方法计算
其中：[a×]符号表示的是矢量a的反对称矩阵；为惯导载体系b系相对初始时刻载体惯性系(ib0)的姿态矩阵；/>为陀螺敏感的载体角速度，对准开始时刻ib0坐标系与b坐标系重合，/>I3×3为3x3的单位阵；
将载体系b系下的比力投影到导航坐标系n系下有：
其中：fb为动基座下加速度计输出；gb为重力在b坐标系上投影；为载体线运动加速度；/>为惯导加速度计输出比力在ib0上的投影；
设上式两边同时取积分得到：
其中：为比力矢量的积分在ib0系下的投影的矢量；tk为对准k时刻的时间；/>为初始时刻载体惯性系ib0系相对初始时刻导航惯性系in0系的姿态矩阵；/>为惯导载体系b系相对初始时刻载体惯性系ib0系的姿态矩阵；/>为当地重力加速度矢量在初始时刻导航惯性系in0下的投影；
在载体做匀速运动或等幅摇摆运动条件下，得到：
其中；gn＝[0 0 -g]T，/>为当地重力加速度矢量在初始时刻导航惯性系in0系下的投影；/>为/>的转置矩阵，/>gn为当地重力加速度在导航坐标系n系的投影；g为当地重力加速度大小；
根据惯导解算速度基本方程得到：
其中：为惯导系统导航坐标系n系的速度增量；vn为惯导系统导航坐标系n系的速度；/>为载体运动角速率在导航坐标系n系下的投影；/>地球自转角速率在导航坐标系n系下的投影；fn载体敏感的加速度在导航坐标系n系下的投影；
上式两边积分同时乘以得到：
其中，为步骤1所要求的姿态矩阵，/>为加速度在导航惯性系in0系下的投影，有/> 为外参考对地速度，在静基座条件下，/>
分别设：
得到：
任意取对准过程中的两个时刻点得到
步骤2、在步骤1进行的同时，对陀螺和加速度计200Hz的信息进行压缩处理，并存储到导航计算机的存储空间内，存储速率为7float/s。步骤2如图2所示的数据流中缓存寄存器存储过程。
为了间接延长初始对准时间，同时考虑到硬件和软件存储和处理能力有限，若存储惯性元件的原始脉冲，内存占用和处理脉冲的时间都不满足快速初始对准的要求，为此设计了低频存储数据处理算法，即将陀螺和加速度计输出的脉冲投影在ib0系下，并累加成1s存储的算法，在不丢失载体运动信息的同时，存储空间占比小，一般嵌入式计算机内存都足够使用，同时软件在每200Hz循环计算存储的1Hz的数据相当于计算速度也提升了200～1000倍(与导航计算计算机采样频率大小相关)，具体算法如下：
间隔1s分别存储加速度输出的在ib0坐标系投影的比力之和以及t时刻对应的四元数q：
其中：为第k秒储存的比力和；/>为第k秒存储的四元数大小；Δt为存储周期1s，并将要存储的数据转换成浮点型，其中/>占用3个浮点型空间，q占用4个浮点型空间。
步骤3、在对准第180s时，锁存步骤1中t0时刻的始姿态矩阵同时读取步骤2存储的压缩数据并进行还原，然后启动回溯正向和逆向无阻尼惯导解算。步骤3如图2所示的线程2。
如图4所示，通过缓存寄存器将压缩的数据进行读取，然后利用读取的数据进行正向和逆向导航的具体实现方法为：
正向粗对准完成后，先进行低频正向导航算法，每个采样周期读取导航计算机存储的加速计和陀螺姿态更新的数据，通过地理坐标系的机械编排计算地理系下的速度和位置信息，待数据读到末尾，将正向导航输出的速度信息取反，同时将地球自转角速率取反：
其中，为逆向导航解算的速度，/>为正向导航解算的速度，/>代表导航系下地球自转角速率；其中基于1Hz数据进行当地地理系机械编排过程为：
速度更新：
其中，为k时刻无阻尼解算的速度；/>为采样周期内读取存储的加速度大小；Ts为计算周期，Ts＝1s；
位置更新：
其中：pk为k时刻的位置矢量，L,λ,h分别为载体所处的纬度、经度和高度；Mpv为位置矢量矩阵，/>RM,RN分别为地球子午圈主曲率半径和卯酉圈主曲率半径；
惯性系姿态更新：惯性系姿态更新为更新初始时刻in0系相对地理坐标系n系之间的姿态矩阵对准初始时刻矩阵为/>为单位阵，初始时刻四元数/>迭代计算/>的四元数过程为：
计算每个存储时间间隔的/>
其中：表示导航系相对于惯性系的旋转矩阵，它包含两部分：地球自转引起的导航系旋转，以及系统在地球表面附近移动因地球表面弯曲引起的导航系旋转；vN、vE为惯导速度，通过上一节速度更新得到；计算得/>后采用四元数更新算法更新/>
令求取四元数有：
其中，将上式进行泰勒展开，得四阶近似：
根据t-1时刻计算t时刻的/>
设将上式得到的四元数转换成姿态矩阵求得/>
同时，所有解算都是基于1Hz存储的信息进行，计算时间间隔Ts＝1s。
步骤4、在步骤3启动回溯正向和逆向无阻尼惯导解算的同时，启动回溯卡尔曼滤波器，对步骤1计算得到的进行误差估计。低频回溯滤波是利用步骤3的正向导航和逆向导航机械编排，建立基于正向和逆向导航的系统状态误差方程，采用零速或GPS信息，来估计低频导航的姿态误差，实现初始对准的过程，这样可以多次利用过去时间的信息，克服卡尔曼滤波器由于载体扰动或者惯性元件噪声影响，收敛时间不足的问题。其基本原理框图如图5所示：
回溯卡尔曼滤波与通用的卡尔曼滤波基本原理相同，卡尔曼滤波器作为迭代线性最小二乘估计器被广泛应用，将陀螺仪漂移和加速度计偏置均可看做随机常数过程，得到：
选取精对准中回溯卡尔曼滤波器的状态变量为：
选择的状态变量包括姿态角误差量φE、φN、φU，导航通道的东向、北向和垂向速度误差δvE、δvN、δvU，位置坐标点纬度、经度和高度误差δL、δλ、δh，陀螺常值漂移误差εx、εy、εz，加速度计测量常值偏差
回溯卡尔曼滤波的状态方程为：
/>
其中，和/>为陀螺漂移和加速度计偏置噪声方差；flag为正向逆向导航标志位取值，正向导航时flag＝1，逆向导航时flag＝-1；
快速初始对准选取零速或者北斗速度作为观测量则回溯卡尔曼滤波的量测方程为：
其中，H＝[03×3flag·I3×303×9]；V为速度观测白噪声；flag为正向逆向导航标志位取值；
此时回溯卡尔曼滤波器的状态方程为：
观测方程为：
Zp＝HX+V
其中F、G状态转移矩阵和噪声矩阵采用惯导算法通用的数学模型。
步骤5、在完成回溯滤波后，对估计的误差进行修正，并利用步骤1存储t＝181s～184s的数据进行正向导航追溯，完成整个初始对准过程。
以上建立了连续系统误差模型，在程序中实现需进行离散化。离散化实质上是根据连续系统的系统矩阵计算出离散系统的转移阵，以及根据连续系统的系统过程方差强度阵计算出离散系统的噪声方差阵。
连续系统的状态方程模型如下：
其中：X为系统的n维状态向量；F为n×n维系统矩阵；W为p维系统过程噪声；G为n×p维噪声输入矩阵；Z为系统的m维观测向量；V为m维观测噪声；H为m×n维观测矩阵。
计算Φk,k-1：
利用定常系统的计算方法，状态转移阵Φk,k-1与系统矩阵F由如下关系：
其中，(tk,tk+1]为预测周期，记h＝tk+1-tk。
预测周期h一般较短，进行泰勒展开，得：
计算
连续系统的系统噪声向量W(t)的协方差阵为Q(t)，则输入噪声的方差阵为：
Qq＝G(t)Q(t)GT(t)
Kalman滤波的输入噪声方差与连续系统的输入噪声方差Qq有如下关系：
需要强调的是，本发明所述的实施例是说明性的，而不是限定性的，因此本发明包括并不限于具体实施方式中所述的实施例，凡是由本领域技术人员根据本发明的技术方案得出的其他实施方式，同样属于本发明保护的范围。
Claims (7)
1.一种基于低频逆向滤波的惯导快速对准方法，其特征在于：包括以下步骤：
步骤1、t0时刻设备通电，启动正向粗对准过程，正向粗对准以200Hz陀螺和加速度计采样频率计算t0时刻的惯导系统的初始姿态矩阵
步骤2、在步骤1进行的同时，对陀螺和加速度计200Hz的信息进行压缩处理，并存储到导航计算机的存储空间内，存储速率为7float/s；
步骤3、在对准第180s时，锁存步骤1中t0时刻的始姿态矩阵同时读取步骤2存储的压缩数据并进行还原，然后启动回溯正向和逆向无阻尼惯导解算；
步骤4、在步骤3启动回溯正向和逆向无阻尼惯导解算的同时，启动回溯卡尔曼滤波器，对步骤1计算得到的进行误差估计；
步骤5、在完成回溯滤波后，对估计的误差进行修正，并利用步骤1存储t＝181s～184s的数据进行正向导航追溯，完成整个初始对准过程。
2.根据权利要求1所述的一种基于低频逆向滤波的惯导快速对准方法，其特征在于：所述步骤1的具体方法为：以惯性坐标系作为参考基准，初始固定两个惯性坐标系，之后更新载体相对惯性坐标系的角运动四元数，并利用重力和载体线运动在两个惯性坐标系间的变换关系求取两个凝固惯性坐标系的变换阵，进而得到
3.根据权利要求2所述的一种基于低频逆向滤波的惯导快速对准方法，其特征在于：所述的具体计算方法为：
使用姿态更新算法计算
其中，为惯导系统对准t时刻的导航坐标系n系相对初始时刻导航惯性系in0系的姿态矩阵，初始t0时刻时，/>为3x3的单位矩阵，λt为惯导系统对准t时刻的所处的位置的经度；λ0为惯导系统对准t0时刻的所处的位置的经度；ωie为地球自转角速率；t为惯导系统对准时长；Lt为惯导系统对准t时刻的所处的位置的纬度；
姿态四元数更新计算方法计算
其中：[a×]符号表示的是矢量a的反对称矩阵；为惯导载体系b系相对初始时刻载体惯性系(ib0)的姿态矩阵；/>为陀螺敏感的载体角速度，对准开始时刻ib0坐标系与b坐标系重合，/>I3×3为3x3的单位阵；
将载体系b系下的比力投影到导航坐标系n系下有：
其中：fb为动基座下加速度计输出；gb为重力在b坐标系上投影；为载体线运动加速度；fib0为惯导加速度计输出比力在ib0上的投影；
设上式两边同时取积分得到：
其中：vib0为比力矢量的积分在ib0系下的投影的矢量；tk为对准k时刻的时间；为初始时刻载体惯性系ib0系相对初始时刻导航惯性系in0系的姿态矩阵；/>为惯导载体系b系相对初始时刻载体惯性系ib0系的姿态矩阵；/>为当地重力加速度矢量在初始时刻导航惯性系in0下的投影；
在载体做匀速运动或等幅摇摆运动条件下，得到：
其中；gn＝[0 0 -g]T，/>为当地重力加速度矢量在初始时刻导航惯性系in0系下的投影；/>为/>的转置矩阵，/>gn为当地重力加速度在导航坐标系n系的投影；g为当地重力加速度大小；
根据惯导解算速度基本方程得到：
其中：为惯导系统导航坐标系n系的速度增量；vn为惯导系统导航坐标系n系的速度；为载体运动角速率在导航坐标系n系下的投影；/>地球自转角速率在导航坐标系n系下的投影；fn载体敏感的加速度在导航坐标系n系下的投影；
上式两边积分同时乘以得到：
其中，为步骤1所要求的姿态矩阵，/>为加速度在导航惯性系in0系下的投影，有 为外参考对地速度，在静基座条件下，/>
分别设：
得到：
任意取对准过程中的两个时刻点得到/>
4.根据权利要求1所述的一种基于低频逆向滤波的惯导快速对准方法，其特征在于：所述步骤2使用低频存储数据处理算法压缩存储数据。
5.根据权利要求4所述的一种基于低频逆向滤波的惯导快速对准方法，其特征在于：所述低频存储数据处理算法的具体实现方法为：间隔1s分别存储加速度输出的在ib0坐标系投影的比力之和以及t时刻对应的四元数q：
其中：为第k秒储存的比力和；/>为第k秒存储的四元数大小；Δt为存储周期1s，并将要存储的数据转换成浮点型，其中/>占用3个浮点型空间，q占用4个浮点型空间。
6.根据权利要求1所述的一种基于低频逆向滤波的惯导快速对准方法，其特征在于：所述步骤3的具体实现方法为：先进行低频正向导航算法，每个采样周期读取导航计算机存储的加速计和陀螺姿态更新的数据，通过地理坐标系的机械编排计算地理系下的速度和位置信息，待数据读到末尾，将正向导航输出的速度信息取反，同时将地球自转角速率取反：
其中，为逆向导航解算的速度，/>为正向导航解算的速度，/>代表导航系下地球自转角速率；其中基于1Hz数据进行当地地理系机械编排过程为：
速度更新：
其中，为k时刻无阻尼解算的速度；/>为采样周期内读取存储的加速度大小；Ts为计算周期，Ts＝1s；
位置更新：
其中：pk为k时刻的位置矢量，L,λ,h分别为载体所处的纬度、经度和高度；Mpv为位置矢量矩阵，/>RM,RN分别为地球子午圈主曲率半径和卯酉圈主曲率半径；
惯性系姿态更新：惯性系姿态更新为更新初始时刻in0系相对地理坐标系n系之间的姿态矩阵对准初始时刻矩阵为/>为单位阵，初始时刻四元数/>迭代计算/>的四元数过程为：
计算每个存储时间间隔的
其中：表示导航系相对于惯性系的旋转矩阵，它包含两部分：地球自转引起的导航系旋转，以及系统在地球表面附近移动因地球表面弯曲引起的导航系旋转；vN、vE为惯导速度，通过上一节速度更新得到；计算得/>后采用四元数更新算法更新/>
令求取四元数有：
其中，将上式进行泰勒展开，得四阶近似：
根据t-1时刻计算t时刻的/>
设将上式得到的四元数转换成姿态矩阵求得/>
7.根据权利要求1所述的一种基于低频存储的惯导快速对准方法，其特征在于：所述步骤4的回溯卡尔曼滤波器具体实现方法为：
选取精对准中回溯卡尔曼滤波器的状态变量为：
选择的状态变量包括姿态角误差量φE、φN、φU，导航通道的东向、北向和垂向速度误差δvE、δvN、δvU，位置坐标点纬度、经度和高度误差δL、δλ、δh，陀螺常值漂移误差εx、εy、εz，加速度计测量常值偏差
回溯卡尔曼滤波的状态方程为：
其中，和/>为陀螺漂移和加速度计偏置噪声方差；flag为正向逆向导航标志位取值，正向导航时flag＝1，逆向导航时flag＝-1；
快速初始对准选取零速或者北斗速度作为观测量则回溯卡尔曼滤波的量测方程为：
其中，H＝[03×3flag·I3×303×9]；V为速度观测白噪声；flag为正向逆向导航标志位取值；
此时回溯卡尔曼滤波器的状态方程为：
观测方程为：
Zp＝HX+V
其中F、G状态转移矩阵和噪声矩阵采用惯导算法通用的数学模型。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310247417.7A CN116499493A (zh) | 2023-03-15 | 2023-03-15 | 一种基于低频逆向滤波的惯导快速对准方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310247417.7A CN116499493A (zh) | 2023-03-15 | 2023-03-15 | 一种基于低频逆向滤波的惯导快速对准方法 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116499493A true CN116499493A (zh) | 2023-07-28 |
Family
ID=87315633
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310247417.7A Pending CN116499493A (zh) | 2023-03-15 | 2023-03-15 | 一种基于低频逆向滤波的惯导快速对准方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116499493A (zh) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117570976A (zh) * | 2024-01-16 | 2024-02-20 | 广东奥斯诺工业有限公司 | 基于逆向推算的超低速载体惯性定向方法 |
-
2023
- 2023-03-15 CN CN202310247417.7A patent/CN116499493A/zh active Pending
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117570976A (zh) * | 2024-01-16 | 2024-02-20 | 广东奥斯诺工业有限公司 | 基于逆向推算的超低速载体惯性定向方法 |
| CN117570976B (zh) * | 2024-01-16 | 2024-05-03 | 广东奥斯诺工业有限公司 | 基于逆向推算的超低速载体惯性定向方法 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Noureldin et al. | Performance enhancement of MEMS-based INS/GPS integration for low-cost navigation applications | |
| JP6255924B2 (ja) | センサー用ｉｃ、センサーデバイス、電子機器及び移動体 | |
| WO2004063669A2 (en) | Attitude change kalman filter measurement apparatus and method | |
| Rad et al. | Optimal attitude and position determination by integration of INS, star tracker, and horizon sensor | |
| RU2762143C2 (ru) | Система определения курса и углового пространственного положения, выполненная с возможностью функционирования в полярной области | |
| CN110285815A (zh) | 一种可在轨全程应用的微纳卫星多源信息姿态确定方法 | |
| WO2016015140A2 (en) | Method and system for improving inertial measurement unit sensor signals | |
| CN109489661B (zh) | 一种卫星初始入轨时陀螺组合常值漂移估计方法 | |
| CN116499493A (zh) | 一种基于低频逆向滤波的惯导快速对准方法 | |
| CN100588905C (zh) | 陀螺的虚拟实现方法 | |
| CN114001731B (zh) | 虚拟圆球模型下极区惯性导航相位调制阻尼方法及系统 | |
| US11150090B2 (en) | Machine learning zero-rate level calibration | |
| Zhu et al. | A small low-cost hybrid orientation system and its error analysis | |
| CN112665570B (zh) | 一种基于星敏感器的mems陀螺零偏在轨简化工程计算方法 | |
| CN113959462A (zh) | 一种基于四元数的惯性导航系统自对准方法 | |
| CN106918828B (zh) | 一种飞行器自主导航方法及系统 | |
| JP2015094631A (ja) | 位置算出装置及び位置算出方法 | |
| Zhe et al. | Adaptive complementary filtering algorithm for imu based on mems | |
| Xue et al. | MEMS-based multi-sensor integrated attitude estimation technology for MAV applications | |
| CN113916261B (zh) | 基于惯性导航优化对准的姿态误差评估方法 | |
| Shang et al. | Design and implementation of MIMU/GPS integrated navigation systems | |
| CN114526729A (zh) | 一种基于冗余技术的mems惯性定位系统航向优化方法 | |
| Yang et al. | Fast SINS initial alignment method based on iterative algorithms in inertial frame | |
| Edwan et al. | GPS/INS integration for GF-IMU of twelve mono-axial accelerometers configurations | |
| CN117346823B (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 |