基于卡尔曼滤波器的航姿系统测姿算法研究*

苑艳华，李四海，南 江

(西北工业大学自动化学院，西安 710129)

捷联航姿系统是一种自主式低成本的航向姿态测量系统，此外还可以提供载体的角速率和加速度信息，有着广泛的应用前景。但由于低精度航姿系统陀螺精度较低，漂移较大，载体长时间机动时不能保证要求的姿态精度。采用低精度的捷联航姿系统与GPS卫星定位系统实现组合，是解决长时间飞行过程中姿态测量问题的良好途径［1］，但是从军事应用的角度考虑，GPS信号并不可靠，因此必须解决辅助导航系统不可用或大部分不可用时，如何利用捷联航姿系统本身的信息来提高精度，以满足长时间飞行对航姿系统的姿态要求。目前这方面的研究较少［2－5］，由于航姿系统通常没有当地经纬度等初始位置信息的输入且没有位置信息和速度信息的解算，通常采用的导航算法已不再适用［6－7］。

本文设计了一种基于卡尔曼滤波技术的姿态算法，用重力加速度在机体系的分量gb和陀螺漂移作为待估计的状态量，在滤波周期内，利用基于角运动信息的状态方程进行卡尔曼滤波的时间更新过程，同时运用系统自身信息对载体飞行状态进行判别，在非机动时利用基于比力信息的量测方程进行卡尔曼滤波的量测更新过程，去修正由低精度的陀螺漂移造成的姿态误差，仿真结果表明:此算法可长时间输出较高精度的姿态信息。

1 捷联航姿系统工作原理

捷联航姿系统由低精度光纤陀螺或MEMS陀螺、加速度计、磁传感器和信息处理电路组成［8］。陀螺敏感载体运动角速度，对于航姿系统中陀螺的输出，计算时无法区分载体的角速率、地球表观角速率和地球的自转角速率，在每个采样周期内采集陀螺的输出，经信号预处理后，根据四元素微分方程［9］来更新姿态角。但由于陀螺漂移较大，姿态精度较低，同时利用载体在非机动时的加速度计输出计算出俯仰角θ、横滚角γ，然后再利用式(1)计算磁航向角［10］。由于加速度计测量精度较高，其测角精度也很高，因此在某些条件下，可以利用加速度计输出计算姿态角，来更新因陀螺漂移造成的姿态误差。

其中，Hx，Hy，Hz分别为三轴磁阻传感器的输出，β为磁航向角，定义地球表面任一点磁北与地理子午面的夹角为磁偏角α，载体相对于真北的航向角为ψ=β±α。

以上是工程上航姿系统常用的计算姿态的算法，但由于航姿系统的陀螺精度较低，漂移较大，通常的航姿算法中并未考虑陀螺漂移的影响［11］，因此长时间机动工作时不能保证要求的姿态精度，针对此问题，本文提出了基于卡尔曼滤波器的姿态融合算法。

2 基于卡尔曼滤波的姿态融合算法

2.1 基于角运动信息的系统方程

根据对姿态角估计的问题可以间接转化为对重力加速度在机体系的分量gb估计的思想，本文推出了基于角运动信息的系统方程。

对等式两边同时微分得:

将式(4)代入式(3)得:

同时为了降低陀螺漂移的影响，将陀螺等效漂移列入系统状态，本文将陀螺等效漂移考虑为一阶马尔科夫过程。

其中，wε(t)为白噪声，Tg为相关时间。

综合式(5)和式(6)，可得出基于角运动信息的系统方程为:

2.2 基于比力信息的量测方程

加速度计测量的是载体相对于惯性空间的比力信息，由比力方程可知，与载体固连的加速度计测量的真实比力信息fb可以在b系中描为:

2.3 系统离散化及姿态融合算法

式(7)建立的系统方程是非线性的，可以用扩展卡尔曼滤波，也可以用上一步的估值来构造当前步的系数矩阵将其近似为线性系统。

对式(7)进行离散化得:

一步转移矩阵和等效离散噪声方差阵计算如下:

当载体平稳飞行时，为 0，则

忽略加速度计等效零偏，则在载体非机动情况下时，基于比力信息的量测方程为:

对系统方程进行离散化后，根据离散卡尔曼滤波基本方程即可完成姿态融合算法。由于式(9)表示的是载体在非机动情况下的量测方程，因此本文的姿态融合算法中，首先利用载体自身信息对运动状态进行判别，当载体处于机动状态时只进行系统方程的时间更新过程，当载体处于非机动状态时，才进行尔曼滤波的量测更新过程，即利用加速度计输出信息来修正由于陀螺仪漂移而产生的估计误差，同时用估计的陀螺漂移实时地修正陀螺仪的零位漂移。设状态量gb的估计值为，则

计算出俯仰角和横滚角后，再根据三轴磁阻传感器的输出利用式(1)来计算磁航向角。

3 载体飞行状态的判别

3.1 飞行状态的判别准则

根据加速度计的输出方程式(8)可知当载体处于非机动时

由上式可知，当飞机平稳飞行时fb的模值约等于g，因此以利用加速度计输出信息设计飞行状态的模值判别准则如下:

当|‖fb‖－g|≤Thlad时，认为载体处于非机动状态，否则认为载体处于机动状态，其中Thlad的取值取决于加速度的等效零位偏置和测量噪声的大小。

3.2 模值判别准则误判分析

以上所述的模值判别的理论依据是当忽略等效加计偏置和测量噪声时，飞机平稳飞行时，加计输出的模值等于重力加速度值。在忽略加速度计等效偏置和测量噪声条件下，式(8)描述的加速度计输出模型在n系中可以简化为:

式(12)描述的加速度轨迹是一个球心在(0，0，－g)，半径为g的球。即当载体机动加速度在地理坐标系的投影an的取值位于这个球面上或附近时，就会得出载体平稳飞行的结论。因此只用模值判别会存在原理性的误判。

对于上述问题可以加入卡尔曼滤波新息来判别，卡尔曼滤波新息指:

4 仿真结果及分析

仿真参数设置:马尔科夫陀螺漂移均值为20°/h;相关时间为3 600 s;陀螺初始零偏误差为10°/h;陀螺的系统噪声均方差为10°/h;加速度计的零位偏置为0.001gn;加速度的测量噪声均方差为0.001gn;卡尔曼滤波的时间更新周期为10 ms;卡尔曼滤波的量测更新周期为100 ms，仿真总时间为3 600 s。

本文模拟了歼击机的对地攻击过程来设计飞行轨迹［12］，包括各种典型的机动动作，同时为了验证本文的算法，延长了飞机连续机动的时间。设计轨迹的姿态和加速度计参数如图1所示，用上述提出的姿态融合算法计算的俯仰角和横滚角误差如图2所示，估计的等效陀螺漂移如图3所示，本文判别准则对飞行状态的判别情况如图4所示。

图1 飞行轨迹的姿态角和加速度参数

图2 本文算法的姿态角误差

图3 本文算法的陀螺漂移估计值

图4 判别准则对飞行状态的判别情况

在整个飞行过程中，载体从601 s到645 s的44 s时间以2°/s的角速率进行了右盘旋运动，从901 s到945 s的44 s时间以2°/s的角速率进行了左盘旋运动;载体从1 861 s到1 981 s的120 s的时间内以1.5°/s的角速率左盘旋半周，从2 101 s到2 341 s的240 s的时间内以1.5°/s的角速率右盘旋一周。从图2可以看出，姿态误差角出现了几个峰值，姿态误差峰值的出现是陀螺等效漂移在盘旋时间段内积分的结果，后两次盘旋的时间比较长，姿态角误差峰值也较大，但载体从2 101 s到2 341 s的240 s连续机动的时间内姿态角误差最大值为0.7°，而用通常的导航算法姿态误差将达到2°(由于篇幅限制，通常导航算法仿真误差图未加入)，因此连续机动的姿态精度提高了65%，且载体从2 500 s到2 560 s的时间内一直做减速运动，姿态误差角也很小，这是由于反馈了陀螺漂移估计值的结果。在3 600 s的整个飞行过程中的姿态角误差都在1°以内，因此本文设计的基于卡尔曼滤波的姿态融合算法比较稳定，且提高了载体长时间机动的姿态精度，同时从图4和载体的机动情况对比可以看出，本文设计的判别准则准确地对飞行状态进行了识别。

5 结论

本文提出了一种基于卡尔曼滤波技术的姿态融合算法，用重力加速度在机体系的分量gb和陀螺漂移作为待估计的状态量，在滤波周期内，利用基于角运动信息的系统方程进行卡尔曼滤波的时间更新过程，在量测更新周期内，先利用判别准则对载体飞行状态进行判别，若载体处于非机动状态，则利用基于比力信息的量测方程进行卡尔曼滤波的量测更新过程，去修正由低精度的陀螺漂移造成的姿态误差。仿真结果表明:本文所设计的姿态融合算法比较稳定，计算简单，且明显提高了长期机动的动态精度，为下一步的航姿系统研发奠定了理论基础。

［1］徐玉，李平，韩波.一种面向机动的低成本姿态测量系统［J］.传感技术学报，2007，20(10):2272－2275.

［2］Zhu Rong，Sun Dong，Zhou Zhaoying.A Linear Fusion Algorithm for Attitude Determination Using Low Cost MEMS-Based Sensors［J］.Measurement，2007，40(3):3222－328.

［3］Gao Zhongyu，Niu Xiaoji，Guo Meifeng.Quaternion-Based Kalman Filter for Micro-Machined Strapdown Attitude Heading Reference system［J］.ChineseJournal of Aeronautics，2002，15(3):171－175.

［4］Gebre Egziabher D，HayWard R C，Powell J D.Design of Multi-Sensor Attitude Determination Systems［J］.IEEE Transactions on Aerospace and Electronic System，2004，40(2):627－649.

［5］Zhou Hongren，Kumar KSP.A Current Statistical Model and Adaptive Algorithm for Estimating Maneuvering Targets［J］.AIAA，Journal of Guidance，Control and Dynamics，1984，7(5):596－602.

［6］汪芳，朱少华，雷宏杰.基于卡尔曼滤波器的数字式捷联航姿系统算法设计［J］.中国惯性技术学报，2008，16(2):208－211.

［7］郭鹏飞，任章.机动加速度辅助的航姿系统扩展卡尔曼滤波［J］.系统工程与电子技术，2009，31(3):625－629.

［8］魏萍.基于ARM的嵌入式航向姿态参考系统的研究［D］.浙江大学硕士学位论文，2006:23－45.

［9］秦永元.惯性导航［M］.北京:科学出版社，2006:305－325.

［10］李秉玺，赵忠，孙照鑫.磁阻传感器的捷联式磁航向仪及误差补偿［J］.传感技术学报，2003，6(2):191－194.

［11］傅建国，王孝通，马野，等.自主姿态测量最优算法研究［J］.航空学报，2005，26(5):594－597.

［12］牛尔卓，任建新，谭剑.捷联惯导系统测试中仿真轨迹的设计与分析［J］.计算机仿真，2009(08):17－20.