发明名称 |
适用于车载晃动的捷联惯组快速对准方法 |
摘要 |
本发明公开了一种适用于车载晃动的捷联惯组快速对准方法,利用捷联惯组计算速度误差周期长,车体晃动速度误差周期短的特点,设计环境速度提取滤波器从导航速度计算值中提取出车载环境实际晃动速度值,进行速度匹配的卡尔曼滤波对准。由于环境速度的有效提取能够大幅降低初始对准滤波器的量测误差,使车载晃动对初始对准精度和速度的影响得到有效抑制;从而实现了车载武器捷联惯组在车载晃动环境下快速、精确的初始对准。 |
申请公布号 |
CN104748763A |
申请公布日期 |
2015.07.01 |
申请号 |
CN201510122689.X |
申请日期 |
2015.03.19 |
申请人 |
北京航天自动控制研究所;中国运载火箭技术研究院 |
发明人 |
张义;张华明;姚寅伟;李彦;佟卫平;杨俊峰 |
分类号 |
G01C25/00(2006.01)I |
主分类号 |
G01C25/00(2006.01)I |
代理机构 |
中国航天科技专利中心 11009 |
代理人 |
褚鹏蛟 |
主权项 |
一种适用于车载晃动的捷联惯组快速对准方法,其特征在于,包括如下步骤:步骤1、初始化导航计算参数、速度提取滤波器的滤波参数以及卡尔曼滤波器的滤波参数;导航计算参数包括地理坐标系上的速度V<sup>n</sup>、导航纬度L、导航经度λ,捷联惯组姿态矩阵<img file="FDA0000684991470000015.GIF" wi="88" he="62" />姿态四元数Q;V<sup>n</sup>=[V<sup>n</sup>[0] V<sup>n</sup>[1] V<sup>n</sup>[2]]<sup>T</sup>,V<sup>n</sup>[0]为东向速度、V<sup>n</sup>[1]为北向速度,V<sup>n</sup>[2]为天向速度,卡尔曼滤波器的滤波参数包括卡尔曼滤波状态估计变量X;步骤2、采集捷联惯组陀螺输出的角速率<img file="FDA0000684991470000011.GIF" wi="99" he="81" />以及捷联惯组加速度计输出的比力f<sup>b</sup>;步骤3、根据步骤2的角速率<img file="FDA0000684991470000012.GIF" wi="71" he="78" />更新计算并归一化姿态四元数Q,根据归一化后的姿态四元数Q更新计算捷联惯组姿态矩阵<img file="FDA0000684991470000013.GIF" wi="93" he="76" />步骤4、根据步骤2的比力f<sup>b</sup>和步骤3更新计算后的捷联惯组姿态矩阵<img file="FDA0000684991470000014.GIF" wi="72" he="71" />更新计算速度V<sup>n</sup>;步骤5、利用步骤4更新计算后的速度V<sup>n</sup>更新计算捷联惯组的导航经度λ和导航纬度L;步骤6、利用步骤4更新计算后的速度V<sup>n</sup>采用速度提取滤波器提取环境晃动瞬时速度;所述环境晃动瞬时速度包括东向晃动瞬时速度V<sup>n′</sup>[0]和北向晃动瞬时速度V<sup>n′</sup>[1];计算东向速度V<sup>n</sup>[0]和东向晃动瞬时速度V<sup>n′</sup>[0]的差值得到东向速度误差观测量ΔV<sup>n</sup>[0],计算北向速度V<sup>n</sup>[1]和北向晃动瞬时速度V<sup>n′</sup>[1]的差值得到北向速度误差观测量ΔV<sup>n</sup>[1];步骤7、建立卡尔曼滤波器状态方程和速度匹配量测方程;步骤8、利用步骤7中得到的卡尔曼滤波器状态方程和速度匹配量测方程,以步骤6中得到的东向速度误差观测量ΔV<sup>n</sup>[0]、北向速度误差观测量ΔV<sup>n</sup>[1]作为观测信息进行卡尔曼滤波,更新计算卡尔曼滤波状态估计变量X;步骤9、利用步骤8中得到的状态估计变量修正步骤3归一化后的姿态四元数Q获得修正后的姿态四元数Q′,根据修正后的姿态四元数Q′计算修正后的姿态矩阵<img file="FDA0000684991470000021.GIF" wi="108" he="81" />根据修正后的姿态矩阵<img file="FDA0000684991470000022.GIF" wi="75" he="81" />计算对准输出姿态角;步骤10、判断是否达到预定的对准稳定时间,如果没有,重复步骤2至步骤9的计算;如果达到预定的对准稳定时间,将步骤9所计算出的对准输出姿态角作为捷联惯组的对准结果。 |
地址 |
100854 北京市海淀区北京142信箱402分箱 |