适用于车载晃动的捷联惯组快速对准方法
【技术领域】
[0001] 本发明设及适用于车载晃动的捷联惯组快速对准方法。
【背景技术】
[0002] 捷联惯性组合被广泛地应用于车载武器制导系统中，精确而快速的初始对准技术 能够确保车载武器捷联惯性制导系统的快速反应能力和精确打击能力。
[0003] 车载的射前对准环境是一种动基座环境，发动机、风、人员走动等会引起车体晃 动，而初始对准的动态环境越恶劣，对准所需的时间就更长，且精度也会受到更严重的影 响。如何在车载环境下克服环境因素的影响，实现车载武器制导系统更快速更准确的初始 对准，对提高车载导弹武器的实战能力具有重要意义，是目前迫切需要解决的问题。
[0004] 目前针对车载武器捷联惯组初始对准方法的研究，主要还是集中在静基座对准的 极限速度和极限精度的提高方面。然而，在实用中更需要考虑的是车载待发准静基座状态 下的晃动环境影响。实际车载武器捷联惯组初始对准仍然主要采用静基座假设条件下的零 速匹配卡尔曼滤波的方式进行自瞄准，通过卡尔曼滤波器的参数设置来一定程度上抑制晃 动的影响。若要根本上解决晃动基座影响的问题，需要采用动基座滤波模型，配合晃动速度 匹配的滤波方式，提升模型的准确性，实现更精确的车载武器捷联惯组晃动基座的初始对 准。
【发明内容】
[0005] 本发明所要解决的技术问题是提供一种能够在车载晃动环境下也能够完成快速 且精确的捷联惯组快速对准方法。
[0006] 本发明包括如下技术方案：
[0007] -种适用于车载晃动的捷联惯组快速对准方法，包括如下步骤：
[0008] 步骤1、初始化导航计算参数、速度提取滤波器的滤波参数W及卡尔曼滤波器的滤 波参数；导航计算参数包括地理坐标系上的速度yn、导航绅度L、导航经度A，捷联惯组姿 态矩阵G、姿态四元数Q;vn=[yn[0]yn山yn口]]T，yn[0]为东向速度、yn山为北向速 度，yn[2]为天向速度，卡尔曼滤波器的滤波参数包括卡尔曼滤波状态估计变量X;
[000引步骤2、采集捷联惯组巧螺输出的角速率诚，W及捷联惯组加速度计输出的比力 户；
[0010] 步骤3、根据步骤2的角速率6^更新计算并归一化姿态四元数Q，根据归一化后的 姿态四元数Q更新计算捷联惯组姿态矩阵G;
[0011] 步骤4、根据步骤2的比力fb和步骤3更新计算后的捷联惯组姿态矩阵G'更新计 算速度yn;
[0012] 步骤5、利用步骤4更新计算后的速度yn更新计算捷联惯组的导航经度A和导航 绅度L;
[0013] 步骤6、利用步骤4更新计算后的速度yn采用速度提取滤波器提取环境晃动 瞬时速度；所述环境晃动瞬时速度包括东向晃动瞬时速度yn' [0]和北向晃动瞬时速度 yn' [1];计算东向速度yn[0]和东向晃动瞬时速度yn' [0]的差值得到东向速度误差观测量 AVn[0]，计算北向速度yn[l]和北向晃动瞬时速度yn' [1]的差值得到北向速度误差观测量 AV。山；
[0014] 步骤7、建立卡尔曼滤波器状态方程和速度匹配量测方程；
[0015] 步骤8、利用步骤7中得到的卡尔曼滤波器状态方程和速度匹配量测方程，W步骤 6中得到的东向速度误差观测量Ayn[0]、北向速度误差观测量Ayn[i]作为观测信息进行 卡尔曼滤波，更新计算卡尔曼滤波状态估计变量X;
[0016] 步骤9、利用步骤8中得到的状态估计变量修正步骤3归一化后的姿态四元数Q获 得修正后的姿态四元数Q'，根据修正后的姿态四元数Q'计算修正后的姿态矩阵G'，根 据修正后的姿态矩阵q''计算对准输出姿态角；
[0017] 步骤10、判断是否达到预定的对准稳定时间，如果没有，重复步骤2至步骤9的计 算；如果达到预定的对准稳定时间，将步骤9所计算出的对准输出姿态角作为捷联惯组的 对准结果。
[001引所述步骤1中地理坐标系上的速度yn的初始值为0,即yn二[yn[0]yn山yn巧]]T= [0 0 0]T，导航绅度L和导航经度A初值设定为捷联惯组所在点地理位置，捷联惯组姿 态矩阵G的初值为粗对准所得到的姿态矩阵。
[0019] 所述步骤1中卡尔曼滤波器的滤波参数初始化方法如下：卡尔曼滤波状态估计变 量X的初值设为零向量，状态协方差阵
【主权项】
1. 一种适用于车载晃动的捷联惯组快速对准方法，其特征在于，包括如下步骤： 步骤1、初始化导航计算参数、速度提取滤波器的滤波参数以及卡尔曼滤波器的滤波参 数；导航计算参数包括地理坐标系上的速度Vn、导航炜度U导航经度λ，捷联惯组姿态矩 阵?:、姿态四元数Q;V n= [Vn[0] Vn[l] Vn[2]]T，Vn[0]为东向速度、Vn[l]为北向速度，V n[2] 为天向速度，卡尔曼滤波器的滤波参数包括卡尔曼滤波状态估计变量X ; 步骤2、采集捷联惯组陀螺输出的角速率，以及捷联惯组加速度计输出的比力 步骤3、根据步骤2的角速率?,t更新计算并归一化姿态四元数Q，根据归一化后的姿态 四元数Q更新计算捷联惯组姿态矩阵; 步骤4、根据步骤2的比力fb和步骤3更新计算后的捷联惯组姿态矩阵G更新计算速 度Vn; 步骤5、利用步骤4更新计算后的速度￥"更新计算捷联惯组的导航经度λ和导航炜度 L ； 步骤6、利用步骤4更新计算后的速度Vn采用速度提取滤波器提取环境晃动瞬时速度； 所述环境晃动瞬时速度包括东向晃动瞬时速度Viv [0]和北向晃动瞬时速度Viv [1];计算 东向速度巧0]和东向晃动瞬时速度V& [0]的差值得到东向速度误差观测量Λνη[0]，计 算北向速度Vn[l]和北向晃动瞬时速度V& [1]的差值得到北向速度误差观测量AVn[l]; 步骤7、建立卡尔曼滤波器状态方程和速度匹配量测方程； 步骤8、利用步骤7中得到的卡尔曼滤波器状态方程和速度匹配量测方程，以步骤6中 得到的东向速度误差观测量AVnD]、北向速度误差观测量AVn[l]作为观测信息进行卡尔 曼滤波，更新计算卡尔曼滤波状态估计变量X ; 步骤9、利用步骤8中得到的状态估计变量修正步骤3归一化后的姿态四元数Q获得修 正后的姿态四元数V，根据修正后的姿态四元数V计算修正后的姿态矩阵Cf，根据修 正后的姿态矩阵Cf计算对准输出姿态角； 步骤10、判断是否达到预定的对准稳定时间，如果没有，重复步骤2至步骤9的计算； 如果达到预定的对准稳定时间，将步骤9所计算出的对准输出姿态角作为捷联惯组的对准 结果。
2. 如权利要求1所述的适用于车载晃动的捷联惯组快速对准方法，其特征在于，所述 步骤1中地理坐标系上的速度Vn的初始值为0,即V n= [Vn[0] Vn[l] Vn[2]]T= [0 0 0]τ， 导航炜度L和导航经度λ初值设定为捷联惯组所在点地理位置，捷联惯组姿态矩阵的 初值为粗对准所得到的姿态矩阵。
3. 如权利要求1所述的适用于车载晃动的捷联惯组快速对准方法，其特征在于，所述 步骤1中卡尔曼滤波器的滤波参数初始化方法如下：卡尔曼滤波状态估计变量X的初值设 为零向量，状态协方差阵
Re表示地球半径，Qs=diag[0 0 (Aa)2 (Aa)2 (ε)2 (ε)2 (e)2]，Aa表示捷联惯组加 速度计的最大误差量，ε表示捷联惯组陀螺的最大误差量；Rs=diag[(0.01)2 (0.01)2]。
4. 如权利要求1所述的适用于车载晃动的捷联惯组快速对准方法，其特征在于，所述 步骤6的速度提取滤波器的计算公式如下：
式中，x(k)为第k次的输入信号，y(k)为第k次的滤波输出值，η为速度提取滤波器的 阶数，以东向速度Vn[0]和北向速度Vn[l]作为输入信号，滤波输出值为东向晃动瞬时速度 Viv [〇]和北向晃动瞬时速度Viv [1]。
5. 如权利要求1所述的适用于车载晃动的捷联惯组快速对准方法，其特征在于，速度 提取滤波器阶次为2阶。
6. 如权利要求1所述的适用于车载晃动的捷联惯组快速对准方法，其特征在于，卡尔 曼滤波状态估计变量父=[61^6\6，[0]6，[1](^(^(^]'61^为炜度误差、6入 为经度误差、SV n[0]为东向速度误差、δνη[1]为北向速度误差，Φχ、Φρ Φζ为三轴失准 角； 卡尔曼滤波器状态方程如下：
&表示地球半径，ω 表示地球自转角速率，g表示当地重力加速度值。
7. 如权利要求6所述的适用于车载晃动的捷联惯组快速对准方法，其特征在于，速度 匹配量测方程为Z = HX+ V，其中，
8. 如权利要求1所述的适用于车载晃动的捷联惯组快速对准方法，其特征在于，所述 步骤9中修正后的姿态四元数Q'的计算公式为：
9.如权利要求1所述的适用于车载晃动的捷联惯组快速对准方法，其特征在于，所述 步骤4具体包括如下步骤： 步骤4a、根据公式尸=CIZi计算地理系上的比力投影值俨； 步骤4b、根据公式
计算速度量dVn，根据公式VVn= Vn+dVn*AT 计算速度量 VVn，dVn=[dVn[0] dVn[l] dVn[2]]T、VVn=[VVn[0] VVn[l] VVn[2]] T，gn= [〇 〇 g] T表示重力加速度在导航系系的投影，其中g表示当地重力加速度值； 步骤4c、令
得到更新计算后的速度Vn。
【专利摘要】本发明公开了一种适用于车载晃动的捷联惯组快速对准方法，利用捷联惯组计算速度误差周期长，车体晃动速度误差周期短的特点，设计环境速度提取滤波器从导航速度计算值中提取出车载环境实际晃动速度值，进行速度匹配的卡尔曼滤波对准。由于环境速度的有效提取能够大幅降低初始对准滤波器的量测误差，使车载晃动对初始对准精度和速度的影响得到有效抑制；从而实现了车载武器捷联惯组在车载晃动环境下快速、精确的初始对准。
【IPC分类】G01C25-00
【公开号】CN104748763
【申请号】CN201510122689
【发明人】张义, 张华明, 姚寅伟, 李彦, 佟卫平, 杨俊峰
【申请人】北京航天自动控制研究所, 中国运载火箭技术研究院
【公开日】2015年7月1日
【申请日】2015年3月19日