自适应混合滤波算法在微型飞行器姿态估计中的应用*

傅忠云，刘文波，孙金秋，徐贵力(.南京航空航天大学金城学院，江苏南京56;.南京航空航天大学自动化学院，江苏南京006)

自适应混合滤波算法在微型飞行器姿态估计中的应用*

傅忠云1*，刘文波2，孙金秋1，徐贵力2
(1.南京航空航天大学金城学院，江苏南京211156;2.南京航空航天大学自动化学院，江苏南京210016)

针对低成本惯性测量单元(IMU)存在漂移和噪声干扰等问题，提出了一种具有自适应参数调节的混合滤波算法。采用四元数法进行系统模型的描述，用梯度下降法对加速度计测得的数据进行处理，再通过互补滤波器将其与陀螺仪测量值进行融合，形成混合滤波算法。同时，考虑到飞行姿态的复杂性，进行参数λ的自适应调节，因而改进后的混合滤波算法，能保证各种飞行姿态变化情况下实时姿态的最优估算。实际系统在线实时性能测试表明，提出的算法简单，估计精度高，易于在嵌入式系统中实现，具有较高推广应用价值。

姿态估计;四元数;梯度下降法;互补滤波;自适应混合滤波算法

随着微小型无人飞行器在地质勘测，灾害监测及军事侦察等领域的广泛应用，准确的飞行姿态测量显得越来越重要。然而，由于微型无人机具有自身有效载荷小等缺点，使得一些高精度姿态测量装置无法应用。由陀螺仪和加速度计组成的低成本惯性测量单元(IMU)，具有体积小，重量轻，功耗低，系统实现简单，性价比高等特点，在很多领域得到了广泛的应用。而低成本陀螺仪存在积分累计误差，加速度计在动态姿态测量时易受高频噪声影响，测量精度较低，如何利用好的滤波算法对两者进行融合滤波，进而获得最优的姿态估计，成为解决此类问题的关键［1-3］。

针对以上问题，国内外很多学者进行了大量的研究工作。文献［4］中提出了利用卡尔曼滤波对陀螺仪随机漂移误差的补偿，但没有考虑加速度计的高频干扰问题。文献［5-10］等分别利用扩展卡尔曼滤波，无迹卡尔曼滤波，粒子滤波等滤波算法，使姿态估计精度有了一定程度的提高。但是卡尔曼滤波算法计算量较大，难以应用于嵌入式微控制器中。文献［11］利用显性互补滤波算法进行了固定旋翼无人机的姿态估计，避免了传统互补滤波器对姿态估计进行重构的缺点。文献［12］提出了一种梯度下降法对加速度计和磁强计的输出数据进行处理，利用互补滤波算法，将其和陀螺仪的积分结果进行融合，实现了在线实时姿态估计，但是由于融合时参数为定值，因此在飞行姿态变化剧烈等情况发生时估计误差较大，算法适应性差。本文采用嵌入式微处理器和低成本的IMU，利用四元数进行系统和姿态描述，提出一种基于自适应参数调节的梯度下降法和互补滤波的混合滤波算法，实现了高精度微型飞行器全姿态估计。

1 算法思想

本文设计的混合滤波算法总体思想为:首先，利用陀螺仪测得的角速度和四元数微分方程式得出角速度微分四元数;然后运用梯度下降法对加速度计测得数据进行处理，得到最小的误差四元数的微分值;再将两者进行互补融合，尽可能的减小陀螺仪的漂移误差和加速度计的高频干扰引起的姿态估计误差;最后对互补滤波后的姿态微分四元数进行积分，估算出最优的姿态值。同时，利用最优重力估计结果，对梯度下降法中可调参数λ的取值进行了优化。该混合滤波算法运算过程不涉及三角和反三角函数运算，全部使用简单的四元数加减乘除运算，运算效率高，计算工作量很小。

2 飞行姿态的四元数描述

四元数是英国数学家Hamilton W R在1843年首先提出的［13］。四元数是一个四维的复合数，可以用来表示刚体旋转或者三维空间的坐标系。随着刚体运动力学的发展，人们发现采用单位四元数来描述刚体的旋转运动十分方便，而且可以避免欧拉法的“奇点”现象，以及方向余弦法计算量大、实时性差等问题。近年来，随着惯性导航技术的发展，四元数在捷联式惯性导航系统中有了较为广泛的应用。

假设参考坐标系为E(xE，yE，zE)，机体坐标系为S(xS，yS，zS)，参考系到机体系的姿态旋转四元数为SEq=［q1，q2，q3，q4］。由四元数SEq表示的旋转可以用三维坐标系中的旋转矩阵SER(q)代替［2］:

则参考系和机体系之间的转换关系可以表示为:

在空间三维坐标系下，无人飞行器的姿态通常由θ、γ和ψ描述。其中，θ为绕xE轴的旋转角度，称为俯仰角;γ为绕yE轴的旋转角度，称为横滚角; ψ为绕zE轴的旋转角度，称为偏航角。根据四元数代数学和欧拉旋转矩阵，可得姿态角［2］:

俯仰角:

横滚角:

偏航角:

因此，当姿态四元数确定后，即可获得对应的旋转矩阵，进而得到四元数表示的参考坐标系下的姿态角。

3 混合滤波算法

3.1 利用陀螺仪获取姿态

在载体坐标系下，一个三轴陀螺仪可以测得绕x，y，z三个轴的角速度，分别用ωx、ωy和ωz表示，将这3个量和0一起定义为陀螺仪角速度四元数向量Sω=［0ωxωyωz］。对参考系到机体系的姿态旋转四元数SEq进行标准化得到SE^q，则旋转四元数的微分方程式为:

即:

其中，SE˙q为SEq的导数，⊗为向量叉乘运算。

实际计算时，将式(6)离散化后，可利用式(7)进行迭代计算:

其中，Δt为系统采样周期。利用陀螺仪测得三轴加速度及式(6)和式(7)，求取更新的旋转四元数，再利用式(3)～式(5)即可解算出实时姿态角。

3.2 梯度下降法

梯度下降法，就是利用函数对应的负梯度方向来更新每次迭代的新的搜索方向，使得每次迭代能使待优化的目标函数逐步减小，通常用来求解函数的最小值。因此目标函数的确定是梯度下降法的前提，下面简述本文研究问题的目标函数的确定过程，及梯度下降法在姿态估算中的应用原理。

根据四元数坐标变换，可以将地球重力场的参考方向通过四元数的叉乘，转换到载体坐标系下进行表示，因为向量的四元数表示法有4个元素，所以，将0作为第1个元素放入向量的已有的3个元素之前。则在参考系下，地球重力场中地球的重力加速度始终垂直向下平行于该坐标系的z轴，且为一常值，因此可以得到标准化的重力场的绝对参考方向E^g=［0 0 0 1］。加速度计测得的重力加速度在载体坐标系中的各轴向分量分别为ax、ay和az，则加速度计四元数向量Sa=［0 axayaz］，标准化为S^a=［0^ax^ay^az］。

则重力场的参考方向在载体坐标系下可表示为:

式中，SE^q*=［q1-q2-q3-q4］是SE^q共轭四元数。SE^q*和SE^q分别是SEq*和SEq标准化形式。将E^g=［0

0 0 1］代入式(8)，即可得载体坐标系下的重力加速度各轴向分量值标准化值S^g=［0^gx^gy^gz］。将其与加速度传感器测得的加速度向量相减，得:

当该函数的2范数值‖fg(ES^q，S^a)‖=0时，测量值与实际值相同，即为正确的姿态。但是由于实际系统测量，坐标对准及数值运算和迭代时都会产生误差，因此该值不一定为0。本文应用梯度下降法求取最小值，就可以得到载体的最优姿态。

通常用目标函数的雅克比矩阵求取其梯度，其雅克比矩阵为:

迭代公式为:

式中，SEq▽(k)是梯度下降法求解出的姿态四元数，SEqest(k-1)为迭代上一次估计值，▽fg(·)为目标函数fg(·)的梯度值，μk称为梯度方向的搜索步长，其值可由μk=α‖SEq˙ω(k)‖Δt确定，α与加速度计的噪声有关，一般取α＞1。这样就可以采用梯度下降法根据式(11)进行迭代求解SEq▽(k)。实际应用中，每次采样只需迭代一次即可，姿态的收敛速度由μk决定。同样利用式(3)～式(5)可解算出实时姿态角。

3.3 混合滤波器融合算法

上述3.1和3.2节中分别阐述了利用两种单独传感器解算出姿态角方法。但是，利用单独陀螺仪积分进行姿态角估计，受积分漂移，低频噪声等影响将不可避免地导致其估计值随时间发散，误差较大。而由于受到高频干扰的影响，加速度计在应用梯度下降法时难以收敛到最优解。下面将利用互补滤波器将两者进行融合，充分发挥两种传感器的优点，使陀螺仪估算的SEqω(k)的发散率和梯度下降法的SEq▽(k)的收敛率相等，进行互补融合，实现最优估计。

基本互补滤波算法的融合公式为:

式中:SEq▽(k)是梯度下降法求解出的姿态四元数，SEqω(k)为陀螺仪求解出的姿态四元数，ξ和1-ξ分别为两种单独姿态估计的权值。ξ的取值要使SEq▽(k)的收敛加权率和积分漂移产生的SEqω(k)发散的加权率相等，即:

其中，λ为可调参数。

将式μk=α‖SE˙qω(k)‖Δt及式(14)代入式(13)可得:

式中需要调整的参数有两个α和λ，且两参数之间无明显直接联系，应用时难以调节。考虑到系统实际要求SEq▽(k)的收敛率大于或等于载体的实际变化率，则应取较大的梯度方向的搜索步长μk，相应的α应取值很大，式(11)和式(14)可简化为:

同时，由于μk很大，ξ也可以近似为0，将式(7)、式(16)和式(17)代入式(13)，则有:

式中:

简化后的融合公式只有一个可变参数λ，易于调节，从式(18)和式(19)也可以看出，该融合算法是将陀螺仪的角速度四元数和由梯度下降法得到的加速度解算出的角速度先进行了融合，然后进行积分得出姿态角。

通常梯度下降法的参数λ取为一定值(根据具体系统试验结果确定)，但是当系统姿态变化剧烈时，将会出现明显的过冲或震荡现象。本文将该参数的取值增加了自适应调节能力，进行动态的调整。例如:当系统姿态不变向前冲或作水平滑动时，由于运动加速度将被引入加速度计测量值中，此时，加速度计测量值与上次估计结果的误差将很大，因此必须将参数λ调小;而当系统姿态快速变化时，则可能超出陀螺仪量程，造成测量误差较大，而此时加速度计测量值较为准确，则λ取较大值，因此对λ做了如下优化:

式中，|·|为取绝对值运算，ea(k-1)为上一个采样周期加速度计误差值。误差取e=［e0exeyez］T=S^g⊗S^a。λ取值上限为2，可以保证陀螺仪测量值的融入。另外，当λ＜0.2，则取下限值0.2，以保证加速度计的测量值对系统输出的作用。而且λ计算公式中融入了低通滤波，可减小参数变化速率，改善系统性能。改进后的自适应混合滤波AHF(A-daptive Hybrid Filter)算法总体框图如图1所示。

图1 自适应混合滤波算法总体框图

4 实验验证及结果

4.1 测试平台

系统传感器选用美国体感技术公司InvenSense的MPU-6050。它是全球首例整合三轴加速度计、三轴陀螺仪的MEMS传感器，有效避免了陀螺仪与加速度计的轴间差问题。虽然该模块本身具有“以数字输出6轴或9轴的旋转矩阵、四元数以及欧拉角格式的融合演算数据”功能，但是，由于代码封闭，具体滤波算法未知，参数不可调节，使用不方便。经测试，与本文提出算法比较，模块自带算法具有运算速度慢，加速度引入误差大等缺点。因此，该模块自带滤波算法未能得到广泛应用。本次实验平台处理器选用意法半导体生产的使用ARM-CortexM3内核的STM32F103C8 32位微处理器，通过SPI协议将数据使用2.4 GHz无线数据传输模块发送到上位机进行观测分析。2 Mbit/s的空中速率结合电脑的USB 1.1全速接口，使得上位机能够以100 Hz的频率实时绘制并导出数据波形。系统测试平台为自制四旋翼微型飞行器。图2为上位机监控界面，可直观的观测飞行姿态，方便程序的调试，同时添加了数据导出功能，可实时将数据导入到Excel表格中，方便数据分析。

图2 上位机监控界面

4.2 试验结果及分析

静态试验:

为了测试本算法系统性能，首先进行了静态试验，因为系统没有磁强计，因此只进行了俯仰角和横滚角的静态数据测试及比对。同时，因为静止时加速度计测得的角度非常准确，所以，将本文提出参数自适应调节的混合滤波(AHF)算法估算出的姿态角和加速度计(ACC)测量值进行比较。如图3所示为静止时俯仰角和横滚角加速度计测量值及估算值比较图。从图3可以看出，本算法静态误差非常小，系统进行了加速度计和陀螺仪零偏校准，经测试静止放置5分钟测得的静态误差绝对值的平均值为0.05°，有效地避免了陀螺仪的漂移问题。

正常飞行试验:

正常飞行姿态仍然进行俯仰角和横滚角的测量及估算，将本文算法估算角度值和高精度光纤陀螺仪及加速度测量值进行比较。图4所示为俯仰角和横滚角曲线图，由图可见该算法估计值与光纤陀螺仪FOG(Fibre-Optic Gyroscope)测量值曲线吻合度很高，曲线平滑，能有效滤除加速度计的毛刺和突变现象。将本文算法估算值与光纤陀螺仪测算结果的差值的绝对值作为估算误差，俯仰角估算的误差平均值为0.523°，横滚角估算的误差平均值为0.498°。以上测试结果表明该系统在静止和正常飞行状态时的姿态角估计精度完全满足实际要求。

为测试本算法的性能，进行了水平滑动和姿态快速变化两种极端飞行姿态试验，以俯仰角为例进行测试，并将本文提出的自适应参数调节混合滤波算法和参数不变混合滤波算法以及几种常用的滤波算法进行了比较。

如图5所示为飞行器做水平滑动运动时各算法对运动加速度的滤除效果图。水平滑动时俯仰角的理论真值为0°，则对几种算法估算值的绝对值取平均值，即可得运动加速度滤除误差，结果如表1所示。可见改进后的混合滤波算法在运动加速度特性滤除时表现出了良好的性能。

表1 运动加速度滤除误差

图3 静止时姿态角

图4 正常飞行时姿态角

图5 水平滑动时加速度滤除效果图

当控制器姿态角度变化过快时，加速度计则会发生突变，同时也可能超出陀螺仪量程，造成陀螺仪测量失效，积分误差大。如图6所示，通过几种算法比较可见，本文算法系统快速性好，无超调，无稳态误差;卡尔曼滤波(Kalman)算法［14］对未精确建模系统，收敛速度慢，同时还存在超调;显性互补滤波(ECF)和参数不变混合滤波(HF)算法恢复速度都较慢。

图6 姿态快速变化时各算法估算俯仰角曲线

5 结论

本文将梯度下降法和互补滤波器结合起来，并对参数λ的选择进行了优化和改进，形成自适应参数可调的混合滤波算法，通过实时系统在线测试，该算法能对飞行器各种姿态进行高精度姿态角度估算。同时，通过和其他常用滤波算法进行比较，该算法具有姿态估算精度高，快速性好，无超调及静差等优点，完全能满足实际工程需求。

［1］高钟毓.惯性导航系统技术［M］.北京:清华大学出版社，2012.

［2］曹娟娟，房建成，盛蔚，等.低成本多传感器组合导航系统在小型无人机自主飞行中的研究与应用［J］.航空学报，2009，30 (10):1923-1929.

［3］吴永亮，王田苗，梁建宏.微小型无人机三轴磁强计现场误差校正方法［J］.航空学报，2011，32(2):330-336.

［4］叶锃峰，冯恩信.基于四元数和卡尔曼滤波的两轮车姿态稳定方法［J］.传感技术学报，2012，25(4):524-528.

［5］Wu Y L，Wang T M，Liang J H，etal.Attitude Estimation for Small Helicopter Using Extended Kalman Filter［C］∥Proceedings of the 2008 IEEE Conference on Robotics，Automation and Mechatronics. Piscataway:IEEE Press，2008:577-581.

［6］朱文杰，王广龙，高凤岐，等.基于MIMU和磁强计的在线实时定姿方法［J］.传感技术学报，2013，26(4):536-540.

［7］Tarhan M，Altug E.EKF Based Attitude Estimation and Stabilization of a Quadrotor UAV Using Vanishing Points in Catadioptric Images［J］.Journal of Intelligent and Robotic Systems，2011，62(3-4): 587-607.

［8］Marina H G，Espinosa F，Santos C.Adaptive UAV Attitude Estimation Employing Unscented Kalman Filter，FOAM and Low-Cost MEMS Sensors［J］.Sensors，2012，12(7):9566-9585.

［9］朱丰超，姚敏立，贾维敏.基于低成本陀螺和倾角仪的姿态估计［J］.宇航学报，2011，32(8):1728-1733.

［10］Won S P，Melek W W，Golnaraghi F.A Kalman/Particle Filter Based Position and Orientation Estimation Method Using a Position Sensor/Inertial Measurement Unit Hybrid System［J］.IEEE Transactions on Industrial Electronics，2010，57(5):1787-1798.

［11］Euston M，Coote P，Mahony R，et al.A Complementary Filter for Attitude Estimation ofa Fixed-Wing UAV［C］∥Proceedings ofthe 2008 IEEE/RSJ International Conference on Robots and System. Piscataway:IEEE Press，2008:340-345.

［12］Sebastian O H，Madgwick，Andrew J L，et al.Estimation of IMU and MARG Orientation Using a Gradient Descent Algorithm［C］∥IEEE International Conference on Rehabilitation Robotics，2011.

［13］程国采.四元数法及其应用［M］.长沙:国防科技大学出版社，1991.

［14］刘兴川，张盛，李丽哲，等.基于四元数的MARG传感器姿态测量算法［J］.清华大学学报(自然科学版)，2012，52(5):627-631.

傅忠云(1980-)，女，硕士，讲师，主要从事测控技术及嵌入式系统的教学及应用研究，358328647@qq.com;

刘文波(1968-)，女，教授，博导，主要从事信号处理、计算机测控技术研究，wenboliu@nuaa.edu.cn。

Application of Adaptive Hybrid Filter Algorithm in the Estimation of the Micro Air Vehicle Attitude*

FU Zhongyun1*，LIU Wenbo2，SUN Jinqiu1，2，XU Guili2

(1.Nanhang Jincheng College，Nanjing 211156，China; 2.College of Automation Engineering，University of Aeronautics and Astronautics，Nanjing 210016，China)

Concerning the drift and noise interference of low cost inertial measurement unit，a hybrid filtering algorithm with adaptive adjustment of parameters was proposed.With the quaternion for describing the attitudes，the accelerometer data is processed using gradient descent algorithm.And then the results are fused with Gyro measurements through the complementary filter，which is called the mixed filter algorithm.At the same time，considering the complexity of flight attitude，the parameters can be adaptively adjusted.So the improved hybrid filter algorithm can guarantee the optimal attitude estimation in real time for various flight attitudes.The online test results of the realtime system show that the proposed algorithm simple to realize and has high estimation accuracy.It is especially suitable for implementation on embedded hardware which has high application value.

attitude estimation;quaternion;gradient descent algorithm;complementary filter;adaptive hybrid filter algorithm

V249.3

A

1004－1699(2014)05－0698－06

10.3969/j.issn.1004－1699.2014.05.024

项目来源:国家自然科学基金项目(60974105);航空科学基金项目(20100152003)

2014-02-18

2014-04-29