[发明专利]适用于车载晃动的捷联惯组快速对准方法

摘要

本发明公开了一种适用于车载晃动的捷联惯组快速对准方法，利用捷联惯组计算速度误差周期长，车体晃动速度误差周期短的特点，设计环境速度提取滤波器从导航速度计算值中提取出车载环境实际晃动速度值，进行速度匹配的卡尔曼滤波对准。由于环境速度的有效提取能够大幅降低初始对准滤波器的量测误差，使车载晃动对初始对准精度和速度的影响得到有效抑制；从而实现了车载武器捷联惯组在车载晃动环境下快速、精确的初始对准。

主权项

一种适用于车载晃动的捷联惯组快速对准方法，其特征在于，包括如下步骤：步骤1、初始化导航计算参数、速度提取滤波器的滤波参数以及卡尔曼滤波器的滤波参数；导航计算参数包括地理坐标系上的速度Vn、导航纬度L、导航经度λ，捷联惯组姿态矩阵姿态四元数Q；Vn＝[Vn[0] Vn[1] Vn[2]]T，Vn[0]为东向速度、Vn[1]为北向速度，Vn[2]为天向速度，卡尔曼滤波器的滤波参数包括卡尔曼滤波状态估计变量X；所述地理坐标系上的速度Vn的初始值为0，即Vn＝[Vn[0] Vn[1] Vn[2]]T＝[0 0 0]T，导航纬度L和导航经度λ初值设定为捷联惯组所在点地理位置，捷联惯组姿态矩阵的初值为粗对准所得到的姿态矩阵；卡尔曼滤波器的滤波参数初始化方法如下：卡尔曼滤波状态估计变量X的初值设为零向量，状态协方差阵Re表示地球半径，设定卡尔曼滤波器过程噪声协方差阵Qs，令Qs＝diag[0 0 (Δa)2 (Δa)2 (ε)2 (ε)2 (ε)2]，Δa表示捷联惯组加速度计的最大误差量，ε表示捷联惯组陀螺的最大误差量；设定卡尔曼滤波器量测噪声协方差阵Rs，令Rs＝diag[(0.01)2 (0.01)2]；步骤2、采集捷联惯组陀螺输出的角速率以及捷联惯组加速度计输出的比力fb；步骤3、根据步骤2的角速率更新计算并归一化姿态四元数Q，根据归一化后的姿态四元数Q更新计算捷联惯组姿态矩阵步骤4、根据步骤2的比力fb和步骤3更新计算后的捷联惯组姿态矩阵更新计算速度Vn；步骤5、利用步骤4更新计算后的速度Vn更新计算捷联惯组的导航经度λ和导航纬度L；步骤6、利用步骤4更新计算后的速度Vn采用速度提取滤波器提取环境晃动瞬时速度；所述环境晃动瞬时速度包括东向晃动瞬时速度Vn′[0]和北向晃动瞬时速度Vn′[1]；计算东向速度Vn[0]和东向晃动瞬时速度Vn′[0]的差值得到东向速度误差观测量ΔVn[0]，计算北向速度Vn[1]和北向晃动瞬时速度Vn′[1]的差值得到北向速度误差观测量ΔVn[1]；速度提取滤波器的计算公式如下：y(k)=x(k)(k≤n)y(k)=Σi=0nbix(k-i)-Σj=0najy(k-j)(k>n)---(19)]]>式中，x(k)为第k次的输入信号，y(k)为第k次的滤波输出值，n为速度提取滤波器的阶数，以东向速度Vn[0]和北向速度Vn[1]作为输入信号，滤波输出值为东向晃动瞬时速度Vn′[0]和北向晃动瞬时速度Vn′[1]；步骤7、建立卡尔曼滤波器状态方程和速度匹配量测方程；步骤8、利用步骤7中得到的卡尔曼滤波器状态方程和速度匹配量测方程，以步骤6中得到的东向速度误差观测量ΔVn[0]、北向速度误差观测量ΔVn[1]作为观测信息进行卡尔曼滤波，更新计算卡尔曼滤波状态估计变量X；步骤9、利用步骤8中得到的状态估计变量修正步骤3归一化后的姿态四元数Q获得修正后的姿态四元数Q′，根据修正后的姿态四元数Q′计算修正后的姿态矩阵根据修正后的姿态矩阵计算对准输出姿态角；步骤10、判断是否达到预定的对准稳定时间，如果没有，重复步骤2至步骤9的计算；如果达到预定的对准稳定时间，将步骤9所计算出的对准输出姿态角作为捷联惯组的对准结果。