一种基于低频逆向滤波的惯导快速对准方法与流程

1.本发明属于捷联惯性导航系统快速初始对准技术领域，尤其是一种基于低频逆向滤波的惯导快速对准方法。


背景技术：

2.在车载、机载军事装备在投入使用前，其搭载的导航系统在执行任务之前需要进行初始准备过程，包括初始化姿态角，速度、位置信息，初始对准过程。对于传统的初始对准一般包括粗对准和精对准两个过程，对准时间一般不少于5min，且对准精度与对准环境和惯导系统装备的惯性元件如加速度计和陀螺的精度和噪声水平有直接关系，为了达到对准精度和时间的要求，对准需要在静基座下进行，如果存在扰动和惯性元件噪声增大，会导致对准精度降低甚至对准失败。
3.为了实现短时间快速对准功能，许多文献采用了逆向导航的算法，将惯性元件数据存储起来实现数据复用，由于存储惯性元件频率一般100hz～1khz，占用存储空间较大，同时在逆向导航同时需要进行正常导航计算过程，对导航计算机处理能力提出了较高的要求，为此有的文献采用了1hz存储然后回溯导航滤波方案，但只是通过节省了粗对准时间来有限的延长精对准滤波器时间，在粗对准结果较差或总对准时间较少(小于或等于3min)情况下，未能充分发挥存储数据复用的优势。


技术实现要素：

4.本发明的目的在于克服现有技术的不足，提出一种基于低频逆向滤波的惯导快速对准方法，通过低频存储结合低频导航解算和正向逆向滤波器的快速初始对准算法，能将对准算法中精对准时间有效延长，从而实现快速对准算法。
5.本发明解决其技术问题是采取以下技术方案实现的：
6.一种基于低频逆向滤波的惯导快速对准方法，包括以下步骤：
7.步骤1、t0时刻设备通电，启动正向粗对准过程，正向粗对准以200hz陀螺和加速度计采样频率计算t0时刻的惯导系统的初始姿态矩阵
8.步骤2、在步骤1进行的同时，对陀螺和加速度计200hz的信息进行压缩处理，并存储到导航计算机的存储空间内，存储速率为7float/s；
9.步骤3、在对准第180s时，锁存步骤1中t0时刻的始姿态矩阵同时读取步骤2存储的压缩数据并进行还原，然后启动回溯正向和逆向无阻尼惯导解算；
10.步骤4、在步骤3启动回溯正向和逆向无阻尼惯导解算的同时，启动回溯卡尔曼滤波器，对步骤1计算得到的进行误差估计；
11.步骤5、在完成回溯滤波后，对估计的误差进行修正，并利用步骤1存储t＝181s～184s的数据进行正向导航追溯，完成整个初始对准过程。
12.而且，所述步骤1的具体方法为：以惯性坐标系作为参考基准，初始固定两个惯性
坐标系，之后更新载体相对惯性坐标系的角运动四元数，并利用重力和载体线运动在两个惯性坐标系间的变换关系求取两个凝固惯性坐标系的变换阵，进而得到
13.而且，所述的具体计算方法为：
14.使用姿态更新算法计算
[0015][0016]
其中，为惯导系统对准t时刻的导航坐标系n系相对初始时刻导航惯性系i
n0
系的姿态矩阵，初始t0时刻时，为3x3的单位矩阵，λ
t
为惯导系统对准t时刻的所处的位置的经度；λ0为惯导系统对准t0时刻的所处的位置的经度；ω
ie
为地球自转角速率；t为惯导系统对准时长；l
t
为惯导系统对准t时刻的所处的位置的纬度；
[0017]
姿态四元数更新计算方法计算
[0018][0019]
其中：[a
×
]符号表示的是矢量a的反对称矩阵；为惯导载体系b系相对初始时刻载体惯性系(i
b0
)的姿态矩阵；为陀螺敏感的载体角速度，对准开始时刻i
b0
坐标系与b坐标系重合，i3×3为3x3的单位阵；
[0020]
将载体系b系下的比力投影到导航坐标系n系下有：
[0021][0022][0023]
其中：fb为动基座下加速度计输出；gb为重力在b坐标系上投影；为载体线运动加速度；为惯导加速度计输出比力在i
b0
上的投影；
[0024]
设上式两边同时取积分得到：
[0025][0026]
其中：为比力矢量的积分在i
b0
系下的投影的矢量；tk为对准k时刻的时间；为初始时刻载体惯性系i
b0
系相对初始时刻导航惯性系i
n0
系的姿态矩阵；为惯导载体系b系相对初始时刻载体惯性系i
b0
系的姿态矩阵；为当地重力加速度矢量在初始时刻导航惯性系i
n0
下的投影；
[0027]
在载体做匀速运动或等幅摇摆运动条件下，得到：
[0028]
[0029]
其中；其中；为当地重力加速度矢量在初始时刻导航惯性系i
n0
系下的投影；为的转置矩阵，gn为当地重力加速度在导航坐标系n系的投影；g为当地重力加速度大小；
[0030]
根据惯导解算速度基本方程得到：
[0031][0032]
其中：为惯导系统导航坐标系n系的速度增量；vn为惯导系统导航坐标系n系的速度；为载体运动角速率在导航坐标系n系下的投影；地球自转角速率在导航坐标系n系下的投影；fn载体敏感的加速度在导航坐标系n系下的投影；
[0033]
上式两边积分同时乘以得到：
[0034][0035]
其中，为步骤1所要求的姿态矩阵，为加速度在导航惯性系i
n0
系下的投影，有有为外参考对地速度，在静基座条件下，
[0036]
分别设：
[0037][0038][0039]
得到：
[0040][0041]
任意取对准过程中的两个时刻点得到
[0042][0043]
而且，所述步骤2使用低频存储数据处理算法压缩存储数据。
[0044]
而且，所述低频存储数据处理算法的具体实现方法为：间隔1s分别存储加速度输出的在i
b0
坐标系投影的比力之和以及t时刻对应的四元数q：
[0045][0046][0047]
其中：为第k秒储存的比力和；为第k秒存储的四元数大小；δt为存储周期
1s，并将要存储的数据转换成浮点型，其中占用3个浮点型空间，q占用4个浮点型空间。
[0048]
而且，所述步骤3的具体实现方法为：先进行低频正向导航算法，每个采样周期读取导航计算机存储的加速计和陀螺姿态更新的数据，通过地理坐标系的机械编排计算地理系下的速度和位置信息，待数据读到末尾，将正向导航输出的速度信息取反，同时将地球自转角速率取反：
[0049][0050][0051]
其中，为逆向导航解算的速度，为正向导航解算的速度，代表导航系下地球自转角速率；其中基于1hz数据进行当地地理系机械编排过程为：
[0052]
速度更新：
[0053][0054]
其中，为k时刻无阻尼解算的速度；为采样周期内读取存储的加速度大小；ts为计算周期，ts＝1s；
[0055]
位置更新：
[0056][0057]
其中:pk为k时刻的位置矢量，l,λ,h分别为载体所处的纬度、经度和高度；m
pv
为位置矢量矩阵，rm,rn分别为地球子午圈主曲率半径和卯酉圈主曲率半径；
[0058]
惯性系姿态更新：惯性系姿态更新为更新初始时刻i
n0
系相对地理坐标系n系之间的姿态矩阵对准初始时刻矩阵为为单位阵，初始时刻四元数迭代计算的四元数过程为：
[0059]
计算每个存储时间间隔的
[0060][0061][0062][0063]
其中:表示导航系相对于惯性系的旋转矩阵，它包含两部分：地球自转引起的导航系旋转，以及系统在地球表面附近移动因地球表面弯曲引起的导航系旋转；vn、ve为惯
导速度，通过上一节速度更新得到；计算得后采用四元数更新算法更新
[0064]
令求取四元数有：
[0065][0066]
其中，将上式进行泰勒展开，得四阶近似：
[0067][0068]
根据t-1时刻计算t时刻的
[0069][0070]
设将上式得到的四元数转换成姿态矩阵求得
[0071][0072]
而且，所述步骤4的回溯卡尔曼滤波器具体实现方法为：
[0073]
选取精对准中回溯卡尔曼滤波器的状态变量为：
[0074][0075]
选择的状态变量包括姿态角误差量φe、φn、φu，导航通道的东向、北向和垂向速度误差δve、δvn、δvu，位置坐标点纬度、经度和高度误差δl、δλ、δh，陀螺常值漂移误差ε
x
、εy、εz，加速度计测量常值偏差
[0076]
回溯卡尔曼滤波的状态方程为：
[0077][0078][0079]
[0080][0081][0082][0083][0084][0085][0086]
其中，和为陀螺漂移和加速度计偏置噪声方差；flag为正向逆向导航标志位取值，正向导航时flag＝1，逆向导航时flag＝-1；
[0087]
快速初始对准选取零速或者北斗速度作为观测量则回溯卡尔曼滤波的量测方程为：
[0088][0089]
其中，h＝[03×3flag
·
i3×
303
×9]；v为速度观测白噪声；flag为正向逆向导航标志位
取值；
[0090]
此时回溯卡尔曼滤波器的状态方程为：
[0091][0092]
观测方程为：
[0093]zp
＝hx+v
[0094]
其中f、g状态转移矩阵和噪声矩阵采用惯导算法通用的数学模型。
[0095]
本发明的优点和积极效果是：
[0096]
本发明构建了基于低频逆向滤波来实现机载振动环境下的捷联惯导快速对准，通过设计低频数据压缩方法，在正向粗对准过称中将短时间内的高频数据压缩成1hz数据，占用空间《3kb，然后粗对准完成后，提取存储的1hz数据进行数据还原，同时基于存储的数据在短时间进行低频的(1hz)正向、逆向无阻尼解算和正向和逆向滤波，借助复用数据来间接“延长”对准时间，解决了由于振动环境导致滤波器难以在短时间内收敛的问题，同时由于低频回溯过程需要的运算资源和存储空间很小，可以在不改变现有的硬件水平缩短对准时间和提高对准精度。
附图说明
[0097]
图1为本发明的流程图；
[0098]
图2为本发明的算法流程图；
[0099]
图3为本发明的正向粗对准流程图；
[0100]
图4为本发明低频正向逆向导航算法示意图；
[0101]
图5为本发明回溯卡尔曼滤波基本原理示意图。
具体实施方式
[0102]
以下结合附图对本发明做进一步详述。
[0103]
一种基于低频逆向滤波的惯导快速对准方法，如图1所示，包括四部分：
①
正向粗对准、
②
低频数据存储算法、
③
正向导航和逆向导航机械编排和
④
正向和逆向卡尔曼滤波算法。
[0104]
其具体实现方法，如图2所示，包括以下步骤：
[0105]
步骤1、t0时刻设备通电，启动正向粗对准过程，正向粗对准以200hz陀螺和加速度计采样频率计算t0时刻的惯导系统的初始姿态矩阵步骤1如图1所示的线程1。
[0106]
本步骤的具体实现方法为：以惯性坐标系作为参考基准，初始固定两个惯性坐标系，之后更新载体相对惯性坐标系的角运动四元数，并利用重力和载体线运动在两个惯性坐标系间的变换关系求取两个凝固惯性坐标系的变换阵，进而得到如图3所示，为基于惯性凝固坐标系的粗对准流程。
[0107]
的具体计算方法为：
[0108]
使用姿态更新算法计算
[0109][0110]
其中，为惯导系统对准t时刻的导航坐标系n系相对初始时刻导航惯性系i
n0
系的姿态矩阵，初始t0时刻时，为3x3的单位矩阵，λ
t
为惯导系统对准t时刻的所处的位置的经度；λ0为惯导系统对准t0时刻的所处的位置的经度；ω
ie
为地球自转角速率；t为惯导系统对准时长；l
t
为惯导系统对准t时刻的所处的位置的纬度；
[0111]
姿态四元数更新计算方法计算
[0112][0113]
其中：[a
×
]符号表示的是矢量a的反对称矩阵；为惯导载体系b系相对初始时刻载体惯性系(i
b0
)的姿态矩阵；为陀螺敏感的载体角速度，对准开始时刻i
b0
坐标系与b坐标系重合，i3×3为3x3的单位阵；
[0114]
将载体系b系下的比力投影到导航坐标系n系下有：
[0115][0116][0117]
其中：fb为动基座下加速度计输出；gb为重力在b坐标系上投影；为载体线运动加速度；为惯导加速度计输出比力在i
b0
上的投影；
[0118]
设上式两边同时取积分得到：
[0119][0120]
其中：为比力矢量的积分在i
b0
系下的投影的矢量；tk为对准k时刻的时间；为初始时刻载体惯性系i
b0
系相对初始时刻导航惯性系i
n0
系的姿态矩阵；为惯导载体系b系相对初始时刻载体惯性系i
b0
系的姿态矩阵；为当地重力加速度矢量在初始时刻导航惯性系i
n0
下的投影；
[0121]
在载体做匀速运动或等幅摇摆运动条件下，得到：
[0122][0123]
其中；gn＝[0 0
ꢀ‑
g]
t
，为当地重力加速度矢量在初始时刻导航惯性系i
n0
系下的投影；为的转置矩阵，gn为当地重力加速度在导航坐标系n系的投影；g为当地重力加速度大小；
[0124]
根据惯导解算速度基本方程得到：
[0125][0126]
其中：为惯导系统导航坐标系n系的速度增量；vn为惯导系统导航坐标系n系的速度；为载体运动角速率在导航坐标系n系下的投影；地球自转角速率在导航坐标系n系下的投影；fn载体敏感的加速度在导航坐标系n系下的投影；
[0127]
上式两边积分同时乘以得到：
[0128][0129]
其中，为步骤1所要求的姿态矩阵，为加速度在导航惯性系i
n0
系下的投影，有有为外参考对地速度，在静基座条件下，
[0130]
分别设：
[0131][0132][0133]
得到：
[0134][0135]
任意取对准过程中的两个时刻点得到
[0136][0137]
步骤2、在步骤1进行的同时，对陀螺和加速度计200hz的信息进行压缩处理，并存储到导航计算机的存储空间内，存储速率为7float/s。步骤2如图2所示的数据流中缓存寄存器存储过程。
[0138]
为了间接延长初始对准时间，同时考虑到硬件和软件存储和处理能力有限，若存储惯性元件的原始脉冲，内存占用和处理脉冲的时间都不满足快速初始对准的要求，为此设计了低频存储数据处理算法，即将陀螺和加速度计输出的脉冲投影在i
b0
系下，并累加成1s存储的算法，在不丢失载体运动信息的同时，存储空间占比小，一般嵌入式计算机内存都足够使用，同时软件在每200hz循环计算存储的1hz的数据相当于计算速度也提升了200～1000倍(与导航计算计算机采样频率大小相关)，具体算法如下：
[0139]
间隔1s分别存储加速度输出的在i
b0
坐标系投影的比力之和以及t时刻对应的四元数q：
[0140]
[0141][0142]
其中：为第k秒储存的比力和；为第k秒存储的四元数大小；δt为存储周期1s，并将要存储的数据转换成浮点型，其中占用3个浮点型空间，q占用4个浮点型空间。
[0143]
步骤3、在对准第180s时，锁存步骤1中t0时刻的始姿态矩阵同时读取步骤2存储的压缩数据并进行还原，然后启动回溯正向和逆向无阻尼惯导解算。步骤3如图2所示的线程2。
[0144]
如图4所示，通过缓存寄存器将压缩的数据进行读取，然后利用读取的数据进行正向和逆向导航的具体实现方法为：
[0145]
正向粗对准完成后，先进行低频正向导航算法，每个采样周期读取导航计算机存储的加速计和陀螺姿态更新的数据，通过地理坐标系的机械编排计算地理系下的速度和位置信息，待数据读到末尾，将正向导航输出的速度信息取反，同时将地球自转角速率取反：
[0146][0147][0148]
其中，为逆向导航解算的速度，为正向导航解算的速度，代表导航系下地球自转角速率；其中基于1hz数据进行当地地理系机械编排过程为：
[0149]
速度更新：
[0150][0151]
其中，为k时刻无阻尼解算的速度；为采样周期内读取存储的加速度大小；ts为计算周期，ts＝1s；
[0152]
位置更新：
[0153][0154]
其中：pk为k时刻的位置矢量，l,λ,h分别为载体所处的纬度、经度和高度；m
pv
为位置矢量矩阵，rm,rn分别为地球子午圈主曲率半径和卯酉圈主曲率半径；
[0155]
惯性系姿态更新：惯性系姿态更新为更新初始时刻i
n0
系相对地理坐标系n系之间的姿态矩阵对准初始时刻矩阵为为单位阵，初始时刻四元数迭代计算的四元数过程为：
[0156]
计算每个存储时间间隔的
[0157][0158][0159][0160]
其中：表示导航系相对于惯性系的旋转矩阵，它包含两部分：地球自转引起的导航系旋转，以及系统在地球表面附近移动因地球表面弯曲引起的导航系旋转；vn、ve为惯导速度，通过上一节速度更新得到；计算得后采用四元数更新算法更新
[0161]
令求取四元数有：
[0162][0163]
其中，将上式进行泰勒展开，得四阶近似：
[0164][0165]
根据t-1时刻计算t时刻的
[0166][0167]
设将上式得到的四元数转换成姿态矩阵求得将上式得到的四元数转换成姿态矩阵求得
[0168]
同时，所有解算都是基于1hz存储的信息进行，计算时间间隔ts＝1s。
[0169]
步骤4、在步骤3启动回溯正向和逆向无阻尼惯导解算的同时，启动回溯卡尔曼滤波器，对步骤1计算得到的进行误差估计。低频回溯滤波是利用步骤3的正向导航和逆向导航机械编排，建立基于正向和逆向导航的系统状态误差方程，采用零速或gps信息，来估计低频导航的姿态误差，实现初始对准的过程，这样可以多次利用过去时间的信息，克服卡尔曼滤波器由于载体扰动或者惯性元件噪声影响，收敛时间不足的问题。其基本原理框图如图5所示：
[0170]
回溯卡尔曼滤波与通用的卡尔曼滤波基本原理相同，卡尔曼滤波器作为迭代线性最小二乘估计器被广泛应用，将陀螺仪漂移和加速度计偏置均可看做随机常数过程，得到：
[0171][0172]
选取精对准中回溯卡尔曼滤波器的状态变量为：
[0173][0174]
选择的状态变量包括姿态角误差量φe、φn、φu，导航通道的东向、北向和垂向速
度误差δve、δvn、δvu，位置坐标点纬度、经度和高度误差δl、δλ、δh，陀螺常值漂移误差ε
x
、εy、εz，加速度计测量常值偏差
[0175]
回溯卡尔曼滤波的状态方程为：
[0176][0177][0178][0179][0180][0181]
[0182][0183][0184][0185]
其中，和为陀螺漂移和加速度计偏置噪声方差；flag为正向逆向导航标志位取值，正向导航时flag＝1，逆向导航时flag＝-1；
[0186]
快速初始对准选取零速或者北斗速度作为观测量则回溯卡尔曼滤波的量测方程为：
[0187][0188]
其中，h＝[03×3flag
·
i3×
303
×9]；v为速度观测白噪声；flag为正向逆向导航标志位取值；
[0189]
此时回溯卡尔曼滤波器的状态方程为：
[0190][0191]
观测方程为：
[0192]zp
＝hx+v
[0193]
其中f、g状态转移矩阵和噪声矩阵采用惯导算法通用的数学模型。
[0194]
步骤5、在完成回溯滤波后，对估计的误差进行修正，并利用步骤1存储t＝181s～184s的数据进行正向导航追溯，完成整个初始对准过程。
[0195]
以上建立了连续系统误差模型，在程序中实现需进行离散化。离散化实质上是根据连续系统的系统矩阵计算出离散系统的转移阵，以及根据连续系统的系统过程方差强度阵计算出离散系统的噪声方差阵。
[0196]
连续系统的状态方程模型如下：
[0197][0198]
其中：x为系统的n维状态向量；f为n
×
n维系统矩阵；w为p维系统过程噪声；g为n
×
p维噪声输入矩阵；z为系统的m维观测向量；v为m维观测噪声；h为m
×
n维观测矩阵。
[0199]
计算φ
k,k-1
：
[0200]
利用定常系统的计算方法，状态转移阵φ
k,k-1
与系统矩阵f由如下关系：
[0201][0202]
其中，(tk,t
k+1
]为预测周期，记h＝t
k+1-tk。
[0203]
预测周期h一般较短，进行泰勒展开，得：
[0204][0205]
计算
[0206]
连续系统的系统噪声向量w(t)的协方差阵为q(t)，则输入噪声的方差阵为：
[0207]qq
＝g(t)q(t)g
t
(t)
[0208]
kalman滤波的输入噪声方差与连续系统的输入噪声方差qq有如下关系：
[0209][0210]
需要强调的是，本发明所述的实施例是说明性的，而不是限定性的，因此本发明包括并不限于具体实施方式中所述的实施例，凡是由本领域技术人员根据本发明的技术方案得出的其他实施方式，同样属于本发明保护的范围。

技术特征：
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系相对初始时刻导航惯性系i
n0
系的姿态矩阵，初始t0时刻时，为3x3的单位矩阵，λ
t
为惯导系统对准t时刻的所处的位置的经度；λ0为惯导系统对准t0时刻的所处的位置的经度；ω
ie
为地球自转角速率；t为惯导系统对准时长；l
t
为惯导系统对准t时刻的所处的位置的纬度；姿态四元数更新计算方法计算姿态四元数更新计算方法计算其中：[a
×
]符号表示的是矢量a的反对称矩阵；为惯导载体系b系相对初始时刻载体惯性系(i
b0
)的姿态矩阵；为陀螺敏感的载体角速度，对准开始时刻i
b0
坐标系与b坐标系重合，i3×3为3x3的单位阵；将载体系b系下的比力投影到导航坐标系n系下有：
其中：f
b
为动基座下加速度计输出；g
b
为重力在b坐标系上投影；为载体线运动加速度；f
ib0
为惯导加速度计输出比力在i
b0
上的投影；设上式两边同时取积分得到：其中：v
ib0
为比力矢量的积分在i
b0
系下的投影的矢量；t
k
为对准k时刻的时间；为初始时刻载体惯性系i
b0
系相对初始时刻导航惯性系i
n0
系的姿态矩阵；为惯导载体系b系相对初始时刻载体惯性系i
b0
系的姿态矩阵；为当地重力加速度矢量在初始时刻导航惯性系i
n0
下的投影；在载体做匀速运动或等幅摇摆运动条件下，得到：其中；g
n
＝[0 0
ꢀ‑
g]
t
，为当地重力加速度矢量在初始时刻导航惯性系i
n0
系下的投影；为的转置矩阵，g
n
为当地重力加速度在导航坐标系n系的投影；g为当地重力加速度大小；根据惯导解算速度基本方程得到：其中：为惯导系统导航坐标系n系的速度增量；v
n
为惯导系统导航坐标系n系的速度；为载体运动角速率在导航坐标系n系下的投影；地球自转角速率在导航坐标系n系下的投影；f
n
载体敏感的加速度在导航坐标系n系下的投影；上式两边积分同时乘以得到：其中，为步骤1所要求的姿态矩阵，为加速度在导航惯性系i
n0
系下的投影，有系下的投影，有为外参考对地速度，在静基座条件下，分别设：分别设：得到：
任意取对准过程中的两个时刻点得到得到4.根据权利要求1所述的一种基于低频逆向滤波的惯导快速对准方法，其特征在于：所述步骤2使用低频存储数据处理算法压缩存储数据。5.根据权利要求4所述的一种基于低频逆向滤波的惯导快速对准方法，其特征在于：所述低频存储数据处理算法的具体实现方法为：间隔1s分别存储加速度输出的在i
b0
坐标系投影的比力之和以及t时刻对应的四元数q：对应的四元数q：其中：为第k秒储存的比力和；为第k秒存储的四元数大小；δt为存储周期1s，并将要存储的数据转换成浮点型，其中占用3个浮点型空间，q占用4个浮点型空间。6.根据权利要求1所述的一种基于低频逆向滤波的惯导快速对准方法，其特征在于：所述步骤3的具体实现方法为：先进行低频正向导航算法，每个采样周期读取导航计算机存储的加速计和陀螺姿态更新的数据，通过地理坐标系的机械编排计算地理系下的速度和位置信息，待数据读到末尾，将正向导航输出的速度信息取反，同时将地球自转角速率取反：信息，待数据读到末尾，将正向导航输出的速度信息取反，同时将地球自转角速率取反：其中，为逆向导航解算的速度，为正向导航解算的速度，代表导航系下地球自转角速率；其中基于1hz数据进行当地地理系机械编排过程为：速度更新：其中，为k时刻无阻尼解算的速度；为采样周期内读取存储的加速度大小；t
s
为计算周期，ts＝1s；位置更新：其中：p
k
为k时刻的位置矢量，l,λ,h分别为载体所处的纬度、经度和高度；m
pv
为位置矢量矩阵，r
m
,r
n
分别为地球子午圈主曲率半径和卯酉圈主曲率半径；惯性系姿态更新：惯性系姿态更新为更新初始时刻i
n0
系相对地理坐标系n系之间的姿态矩阵对准初始时刻矩阵为为单位阵，初始时刻四元数迭代计算的四元数过程为：计算每个存储时间间隔的计算每个存储时间间隔的计算每个存储时间间隔的计算每个存储时间间隔的其中：表示导航系相对于惯性系的旋转矩阵，它包含两部分：地球自转引起的导航系旋转，以及系统在地球表面附近移动因地球表面弯曲引起的导航系旋转；v
n
、v
e
为惯导速度，通过上一节速度更新得到；计算得后采用四元数更新算法更新令求取四元数有：其中，将上式进行泰勒展开，得四阶近似：根据t-1时刻计算t时刻的计算t时刻的设将上式得到的四元数转换成姿态矩阵求得将上式得到的四元数转换成姿态矩阵求得7.根据权利要求1所述的一种基于低频存储的惯导快速对准方法，其特征在于：所述步骤4的回溯卡尔曼滤波器具体实现方法为：选取精对准中回溯卡尔曼滤波器的状态变量为：选择的状态变量包括姿态角误差量φ
e
、φ
n
、φ
u
，导航通道的东向、北向和垂向速度误差δv
e
、δv
n
、δv
u
，位置坐标点纬度、经度和高度误差δl、δλ、δh，陀螺常值漂移误差ε
x
、ε
y
、ε
z
，加
速度计测量常值偏差回溯卡尔曼滤波的状态方程为：回溯卡尔曼滤波的状态方程为：回溯卡尔曼滤波的状态方程为：回溯卡尔曼滤波的状态方程为：回溯卡尔曼滤波的状态方程为：回溯卡尔曼滤波的状态方程为：
其中，和为陀螺漂移和加速度计偏置噪声方差；flag为正向逆向导航标志位取值，正向导航时flag＝1，逆向导航时flag＝-1；快速初始对准选取零速或者北斗速度作为观测量则回溯卡尔曼滤波的量测方程为：其中，h＝[03×3flag
·
i3×
303
×9]；v为速度观测白噪声；flag为正向逆向导航标志位取值；此时回溯卡尔曼滤波器的状态方程为：观测方程为：z
p
＝hx+v其中f、g状态转移矩阵和噪声矩阵采用惯导算法通用的数学模型。

技术总结
本发明涉及一种基于低频逆向滤波的惯导快速对准方法，构建了基于低频逆向滤波来实现机载振动环境下的捷联惯导快速对准，通过设计低频数据压缩方法，在正向粗对准过称中将短时间内的高频数据压缩成1Hz数据，占用空间<3KB，然后粗对准完成后，提取存储的1Hz数据进行数据还原，同时基于存储的数据在短时间进行低频的(1Hz)正向、逆向无阻尼解算和正向和逆向滤波，借助复用数据来间接“延长”对准时间，解决了由于振动环境导致滤波器难以在短时间内收敛的问题，同时由于低频回溯过程需要的运算资源和存储空间很小，可以在不改变现有的硬件水平缩短对准时间和提高对准精度。平缩短对准时间和提高对准精度。平缩短对准时间和提高对准精度。


技术研发人员：侯斌 庞松 赵帅
受保护的技术使用者：中国船舶集团有限公司第七〇七研究所
技术研发日：2023.03.15
技术公布日：2023/7/31