基于组合滤波的低成本飞行器姿态解算算法

王守华, 邓桂辉，纪元法，孙希延

(1.桂林电子科技大学 信息与通信学院，广西 桂林 541004; 2.认知无线电与信息处理教育部重点实验室，广西 桂林 541004)

基于组合滤波的低成本飞行器姿态解算算法

王守华1,2, 邓桂辉1*，纪元法1,2，孙希延1,2

(1.桂林电子科技大学 信息与通信学院，广西 桂林 541004; 2.认知无线电与信息处理教育部重点实验室，广西 桂林 541004)

(*通信作者电子邮箱674149099@qq.com)

在低成本的飞行器姿态检测系统中，互补滤波器由于原理简单、计算量小，而被广泛应用。针对低成本飞行器在非匀速运动时因为加速度计不能区分重力加速度与运动加速度引起基于互补滤波的姿态解算误差较大的问题，提出了一种互补与自适应限幅组合滤波的姿态解算算法，并给出了自适应限幅滤波门阀的设计方法。通过融合陀螺仪输出的角速度与加速度计输出的加速度获取限幅滤波的限幅阀值; 然后将归一化的加速度计输出增量通过限幅滤波的结果代替原互补滤波的加速度计输入，提高非匀速运动下姿态解算精度。经实际系统实时性能测试表明，所提算法估计精度高、计算量小，易于在低成本飞行器控制系统中实现。

姿态解算；互补滤波器；限幅滤波；四元数法；自适应

0 引言

近年来，低成本飞行器在物流业、环境监测、地质勘测及军事侦察等领域的应用越来越广泛，作为控制低成本飞行器的关键技术的姿态解算算法成为当前热点研究之一[1]。陀螺仪、加速度计和磁场计等低成本惯性测量单元输出的测量数据是飞行器载体姿态解算的原始数据，但是受到器件自身测量误差、温度和各种噪声等诸多因素的影响，姿态解算往往误差较大[2-3]。通过信息融合算法将多种传感器的数据进行融合往往能取长补短，解算出更精确的姿态角[4]。

在多传感器姿态解算的信息融合方案上，国内外研究者们提出了多种算法和改进策略。贾瑞才[5]、吴涛等[6]提出利用扩展卡尔曼滤波(Extended Kalman Filter, EKF)实现对陀螺仪随机漂移误差的补偿，提高了姿态解算精度，但忽略了加速度计的高频干扰问题，及EKF算法固有的线性化、截断误差和发散问题。为克服EKF固有的缺陷，Pan等[7]实现基于无迹卡尔曼滤波(Unscented Kalman Filter, UKF)的姿态估算算法，取得了比EKF更高的稳定性、解算精度及收敛速度; 而Tang等[8]表示应用在航天飞行器姿态解算上的容积卡尔曼滤波(Cubature Kalman Filter, CKF)算法估算性能要比UKF好, 但在低成本飞行器中，基于卡尔曼滤波的姿态算法存在计算量大、硬件要求高、难实现等缺点。Euston等[9]、Wu等[10]在低成本无人机上利用互补滤波器(Complementary Filter, CF)实现载体姿态估算，充分体现了CF原理简单、计算量小、容易实现、收敛稳定等优点，但是未考虑因加速度计不能区分运动加速度与重力加速度而产生的CF算法解算误差。Wu等[11]、阎世梁等[12]在应用CF解算姿态时通过计算三轴加速度计的输出的合加速度的大小来判断载体的运动状态，然后根据载体运动状态调节CF的转换频率，提高了基于CF的姿态解算精度，但是存在PI参数离散和高动态下互补滤波作用大大降低等问题。孙金秋等[13]设计了PI参数能自适应的CF姿态解算算法，通过合加速度大小来连续调节PI参数，实现连续使姿态估算误差小于1.47°，但存在对姿态三角无区分地使用同一个PI参数调节的问题，依然有高动态下互补滤波作用大大降低的问题。

本文提出自适应限幅滤波与互补滤波组合滤波的策略，其中自适应限幅滤波能较准确区分三轴重力加速度分量和三轴运动加速度分量，同时还保留了互补滤波器的计算量小、收敛稳定及易于在低成本飞行器中实现的优点，使基于重力加速度输入的互补滤波算法也能在载体带有运动加速度时解算出更精确的姿态角。

1 四元数法姿态解算

载体坐标系与导航坐标系的转动关系可以用四元数标记为:

Q=q0+q1i+q2j+q3k

(1)

四元数确定出b系到n系的坐标变换矩阵为:

(2)

n系与b系基本旋转对应的坐标变换矩阵记为:

(3)

对比式(2)、(3)可得:

(4)

可见载体的姿态更新实质上是如何计算四元数，记b系中xyz三轴旋转的角速率向量[ωxωyωz]T为ω，四元数向量[q0q1q2q3]T为q，则更新四元数的微分方程为:

(5)

其中，式(5)中的ω由式(6)求取:

(6)

2 基于互补滤波器的四元数法姿态解算

短时间内，陀螺仪提供高精度的姿态结算数据，而加速度计和磁场计精度较差；长时间内，陀螺仪因漂移的影响，产生积分误差，姿态精度随时间增长而下降，但是加速度计和磁场计的测量误差不会随时间增加。因此，根据两者在频域上的特性的互补，可以采用互补滤波器融合这三种传感器的数据，提高姿态解算的精度和系统的动态特性[13]。

记姿态角为η=[φθψ]，由三轴加速度计输出的加速度向量a和磁场计输出的地磁场强度向量m可以解算出一个带有高频噪声的姿态角η1=η+ΔηH，同时陀螺仪输出ω通过积分可以输出一个含低频噪声的姿态角η2=η+ΔηL。将η1通过低通滤波器HL(s)=C(s)/(s+C(s))滤除加速度计和磁场计的测量误差，并将η2通过高通滤波器HH(s)=s/(s+C(s))滤除陀螺仪因漂移的影响产生的积分误差。那么，最后输出接近真实值的姿态角:

(7)

取C(s)=kP+kI/s，kP为低通滤波器与高通滤波器的转接频率，引入积分环节kI/s是为了补偿陀螺仪的漂移误差[15]。在四元数法姿态解算算法流程中应用互补滤波器时，算法流程为图1。

图1 基于互补滤波器的四元数法姿态解算过程Fig. 1 Quaternion attitude calculation process based on complementary filter

由基于互补滤波器的姿态解算过程可见，跟卡尔曼滤波算法相比，互补滤波法有计算量少、容易实现的优点。

但是，图中的要求加速度计输出的是重力加速度g在b系的三轴分量时，才能使更新四元数得到高精度解算。实际上，在飞行器动态飞行过程中，往往受到的是重力加速度和运动加速度的合力，而加速计无法区分这二者。这个原因当飞行器作非匀速飞行时，即运动加速度非零时，基于互补滤波器的姿态解算误差随运动加速度的误差增大而增大。

3 自适应限幅滤波与互补滤波组合

给限幅滤波器设置一个阀值，通过限制输入的幅值可消除无用或者有害幅值。于是可以设计一个自适应限幅滤波器，使其限幅阀值始终跟随到略大于重力加速度的大小的位置，在飞行器非匀速飞行时最大化地滤除运动加速度。设计的自适应限幅滤波器结构如图2。

图2 自适应限幅滤波器的设计方案Fig. 2 Design scheme of adaptive limited filter

(8)

记ρ为设计的限幅滤波器的自适应限幅阀值，ρ的生成规则为:

(9)

若η0表示姿态角任一时刻的角度值，由于低成本飞行器的输出频率高，旋转角速度较小，所以ω·Δt也很小，那么有:

(10)

(11)

4 实验测试及分析

4.1 实验平台

算法设计验证及分析平台为Matlab R2011b。用于实验测试的软件及装置如图3所示。

航姿参考系统和数据采集选用微型航姿参考系统MAHRS 3DM-E10A，该系统通过RS-232串行接口方便连接，提供100 Hz的实时三轴惯性原始数据和参考姿态角。3DM-E10A提供的横滚角、俯仰角静态误差±0.1°，动态误差±1.0°；航向角静态误差±0.5°，动态误差±2.0°。

如图3(a)所示，双轴电动转台使用TT-3DM-2E-10 Version 1.2, 具备串行输出实时角度位置数据、串行输入控制指令及旋转角位置动态跟踪测量与控制等优点，主轴与俯仰轴转角范围连续无限、角度位置综合测量精度为±0.08°、控制到位分辨率为±0.01°及速率范围为0.1°/s～300°/s，满足实验测试算法需求。

图3(b)为TT-3DM-2E-10配套的转台控制Labview软件。图3(c)为作者开发的用于通过串口记录理论姿态角及3DM-E10A系统输出的9个原始数据的Labview软件，记录频率与3DM-E10A输出频率同样。

图3 用于实验测试的软件及装置Fig. 3 Software and equipment for test

4.2 静态及低动态测试

静态及低动态测试数据采集方法为：通过图3(b)所示控制转台的Labview开发的软件通过串口控制如图3(a)所示双轴转台转动，如图3(c)所示的Labview开发的软件通过串口记录理论姿态角及惯性元件输出9个原始数据。

利用自适应限幅滤波与互补滤波器组合滤波算法解算的静态及低动态下载体的姿态角解算及与航姿参考系统的误差如图4。

图4 静态及低动态测试Fig. 4 Static and low dynamic test

由图4可见，在静态状态下，基于组合滤波算法解算的姿态角误差在±0.2°内；在低动态状态下，误差在±0.7°内。基于典型互补滤波算法的解算结果，在静态下的误差与基于组合滤波算法解算的姿态角误差基本一致，但是低动态下，特别是角速率比较大时，解算最大误差可达±2°。

静态及低动态测试表明，在载体静态下，该算法保留了典型互补滤波算法的优点；在低动态下，在载体角速率较大时，自适应限幅滤波也发挥作用，有效减弱了运动加速度带来的影响，提高了姿态解算的精度。

4.3 快速滑动测试

为了验证算法消除运动加速度对互补滤波法姿态解算影响的效果，将3DM-E10A置于一个接近水平面的平面上滑动，并通过串口采集原始数据。其中该实验用的平面为固定钢化玻璃且几乎没有弹性，经反复测量横滚角约-0.3°，俯仰角约-0.5°，由于测试不同算法时使用的是同一个滑动过程采集的数据，所以不会对对比实验产生影响，可视为理论上的0°。

图5为滑动时，三轴加速度计输出的重力加速度和运动加速度的合加速度。图6为基于组合滤波、典型互补滤波及经典卡尔曼滤波这三个姿态解算算法的比较。

图5 平面快速滑动过程载体的三轴加速度Fig. 5 Three axis accelerations of carrier during fast moving on level surface

图6 姿态解算算法比较Fig. 6 Comparison of attitude calculation algorithms

结合图5和图6可见: 理论上均为0°的横滚角和俯仰角由于较大的运动加速度的对互补滤波算法解算的影响产生了约6°的误差，对经典卡尔曼滤波的解算产生了7°的影响，而基于组合滤波基本不受影响。基于互补滤波解算的航向角在停止滑动时也未能达到收敛的稳定值，还需要约200 ms后才能稳定，经典卡尔曼滤波收敛速度较快；基于组合滤波收敛最快。

快速滑动测试表明，在运动加速度突变或较大时，基于自适应限幅滤波与互补滤波器组合的姿态解算算法极大地减少了运动加速度对姿态解算影响，性能优于基于典型的互补滤波算法及经典卡尔曼滤波的姿态解算。

5 结语

本文分析了基于互补滤波器的姿态解算原理和实现方法，指出了运动加速度对姿态解算的影响，并提出了根据陀螺仪输出和运动状态的判断来设计一个具有自适应限幅门阀的限幅滤波器的方法，将三轴加速度计的输出通过这个限幅滤波器后再用互补滤波算法解算出载体的姿态角。测试表明该算法在匀速或者较高动态状态下，都能达到较高的解算精度，提高了姿态解算的精度。

References)

[1] 傅忠云, 刘文波, 孙金秋,等. 自适应混合滤波算法在微型飞行器姿态估计中的应用[J]. 传感技术学报, 2014,27(5):698-703.(FU Z Y, LIU W B, SUN J Q, et al. Application of adaptive hybrid filter algorithm in the estimation of the micro air vehicle attitude[J]. Chinese Journal of Sensors and Actuators, 2014,27(5):698-703.)

[2] 金光明, 张国良, 陈林鹏,等. MEMS陀螺仪静态漂移模型与滤波方法研究[J]. 传感器与微系统, 2007, 26(11):48-50.(JIN G M, ZHANG G L, CHEN L P, et al. Research on filter method and model of MEMS gyro static drift[J]. Transducer and Microsystem Technologies, 2007, 26(11):48-50.)

[3] DI L, FROMM T, CHEN Y Q. A data fusion system for attitude estimation of low-cost miniature UAVs[J]. Journal of Intelligent & Robotic Systems, 2012, 65(1):621-635.

[4] 彭孝东, 张铁民, 李继宇,等. 基于传感器校正与融合的农用小型无人机姿态估计算法[J].自动化学报, 2015, 41(4):854-860.(PENG X D, ZHANG T M, LI J Y, et al. Attitude estimation algorithm of agricultural small-UAV based on sensors fusion and calibration[J]. Acta Automatica Sinica, 2015, 41(4):854-860.)

[5] 贾瑞才. 基于四元数EKF的低成本MEMS姿态估计算法[J]. 传感技术学报, 2014, 27(1):90-95.(JIA R C. Attitude estimation algorithm for low cost MEMS based on quaternion EKF[J]. Chinese Journal of Sensors and Actuators, 2014,27(1):90-95.)

[6] 吴涛, 白茹, 朱礼尧,等. 基于卡尔曼滤波的航姿参考系统设计[J]. 传感技术学报, 2016, 29(4):531-535.(WU T, BAI R, ZHU L Y, et al. Design of AHRS based on Kalman filter[J]. Chinese Journal of Sensors and Actuators, 2016, 29(4):531-535.)

[7] PAN Y, SONG P, LI K, et al. Attitude estimation of miniature unmanned helicopter using unscented Kalman filter[C]// Proceedings of the 2011 International Conference on Transportation, Mechanical, and Electrical Engineering. Piscataway, NJ: IEEE, 2011:1548-1551.

[8] TANG X, WEI J, CHEN K. Square-root adaptive cubature Kalman filter with application to spacecraft attitude estimation[C]// Proceedings of the 2012 15th International Conference on International Conference on Information Fusion. Piscataway, NJ: IEEE, 2012:1406-1412.

[9] EUSTON M, COOTE P, MAHONY R, et al. A complementary filter for attitude estimation of a fixed-wing UAV[C]// Proceedings of the 2008 IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway, NJ: IEEE, 2008:340-345.

[10] WU J, ZHOU Z, CHEN J, et al. Fast complementary filter for attitude estimation using low-cost MARG sensors[J]. IEEE Sensors Journal, 2016, 16(18): 6997-7007.

[11] WU Z Y, ZENG Y J, SHAO H X, et al. Betterment of attitude estimation based on complementary filter applied in drumstick[C]// Proceedings of the 2012 World Congress on Information and Communication Technologies. Piscataway, NJ: IEEE, 2012:1162-1165.

[12] 阎世梁, 王银玲, 张华. 基于改进互补滤波器的低成本微小飞行器姿态估计方法[J]. 计算机应用, 2013, 33(7):2078-2082.(YAN S L, WANG Y L, ZHANG H. Improved complementary filter for attitude estimation of micro air vehicles using low-cost inertial measurement units[J]. Journal of Computer Applications, 2013, 33(7):2078-2082.)

[13] 孙金秋, 游有鹏, 傅忠云. 基于自适应显式互补滤波的姿态解算方法[J]. 测控技术, 2015, 34(4):24-27.(SUN J Q, YOU Y P, FU Z Y. Attitude estimation based on adaptive explicit complementary filter[J]. Measurement & Control Technology, 2015, 34(4):24-27.)

[14] 秦永元.惯性导航[M].2版.北京:科学出版社, 2014:244-260.(QIN Y Y. Inertial Navigation[M]. 2nd ed. Beijing: Science Press, 2014:244-260.)

[15] 梁延德, 程敏, 何福本,等. 基于互补滤波器的四旋翼飞行器姿态解算[J]. 传感器与微系统, 2011, 30(11):56-58.(LIANG Y D, CHENG M, HE F B, et al. Attitude estimation of a quad-rotor aircraft based on complementary filter[J]. Transducer and Microsystem Technologies, 2011, 30(11):56-58.)

This work is supported by the National Natural Science Foundation of China (61362005), the Key Laboratory of Cognitive Radio and Information Processing, Ministry of Education (CRKL150113), the Innovation Project of Graduate Education in Guilin University of Electronic Technology (2016YJCX90).

WANG Shouhua, born in 1975, M. S., associate professor. His research interests include information processing, navigation and positioning.

DENG Guihui, born in 1989, M. S. candidate. His reserch interests include integrated navigation, indoor naigation.

JI Yuanfa, born in 1975, Ph. D, professor. His research interests include satelite communication, satellite navigation and digital signal processing.

SUN Xiyan, born in 1973, Ph. D, professor. His research interests include satellite navigation, electronic countermeasure.

Attitude calculation algorithm of low-cost aircraft based on combined filter

WANG Shouhua1,2, DENG Guihui1*, JI Yuanfa1,2, SUN Xiyan1,2

(1.CollegeofInformationandCommunication,GuilinUniversityofElectronicTechnology,GuilinGuangxi541004,China;2.KeyLaboratoryofCognitiveRadioandInformationProcessingofMinistryofEducation,GuilinGuangxi541004,China)

In the low-cost aircraft attitude detection system, the complementary filter was widely used because of simple principle and low computational complexity. Aiming at the problem that the accelerometer can not distinguish between the acceleration of gravity and motion may cause attitude calculation error by complementary filtering, an attitude calculation algorithm of complementary and adaptive limited filter was proposed for low cost aircraft, and the design method of adaptive limited filter gate valve was given. The angular velocity output by a gyroscope and the acceleration output by an accelerometer were fused to obtain the gate valve of the limited filter, and then the normalized output increment of the accelerometer by limiting filter replaced the accelerometer input of the original complementary filter. The accuracy of attitude determination under uniform motion was improved. The actual system test shows that the proposed algorithm has high accuracy and low cost, and is easy to be realized in low cost aircraft control system.

attitude calculation; complementary filter; limiting filtering; quaternion method; adaptive

2016-10-28;

2016-12-13。 基金项目:国家自然科学基金资助项目(61362005);“认知无线电与信息处理”教育部重点实验室2015年主任基金资助项目(CRKL150113); 桂林电子科技大学研究生教育创新计划项目(2016YJCX90)。

王守华(1975—),男,山东滨州人,副教授,硕士,主要研究方向:信息处理、导航定位; 邓桂辉(1989—),男,广东茂名人,硕士研究生,主要研究方向:组合导航、室内导航; 纪元法(1975—)，男，山东莘县人，教授，博士，主要研究方向:卫星通信、卫星导航、数字信号处理; 孙希延(1973—)，女，山东潍坊人，研究员，博士，主要研究方向:卫星导航、电子对抗。

1001-9081(2017)05-1507-05

10.11772/j.issn.1001-9081.2017.05.1507

TP391.9

A