改进互补滤波在六旋翼飞行器中的应用*

许晶晶， 郭培源， 董小栋， 徐 盼

(北京工商大学 计算机与信息工程学院，北京 100048)

0 引 言

近年来，低成本的多旋翼无人飞行器因结构简单、操作灵活、具有垂直起降和悬停能力等众多优点，在民用与工业领域的应用越来越广泛。

飞行器的姿态估计是飞行器实现稳定飞行的基础，而姿态解算的精度和速度将直接影响到飞控算法的稳定性和可靠性。而高精度的惯性导航器件并不适用于消费级无人机领域，因此,市面上应用最广泛的是较为低廉的微机电系统 (micro-electro-mechanical system，MEMS)传感器。但由于MEMS传感器中的陀螺仪角度是由积分得到的，长时间会出现零点漂移与温度漂移，随着时间的积累，角度值存在很大误差；MEMS传感器中的加速度计对干扰非常敏感，在飞行过程中，易受噪声和振动的影响，动态特性较差。因此，需要在软件上改进，引入信息融合技术将多种传感器的数据进行融合处理，以提高姿态解算的精度。Hoffmann F等人[1～4]提出了卡尔曼滤波算法对数据进行融合；吴涛等人[5～10]提出了用扩展卡尔曼滤波 (extended Kalman filtering,EKF) 算法来解决系统的非线性问题，但其计算量大，对处理器要求高，计算时间长，不适用于微型飞行器；刘洲等人[11～13]提出了一种自适应扩展互补滤波方法(adaptive extended complementary filtering,AECF)减少了系统的动态误差，但其对于陀螺仪的解算精度很低。综上所述，针对单个惯性传感器不可信，复杂算法对硬件要求高等问题，提出了一种改进的一阶互补滤波姿态估计算法,利用加速度计补偿陀螺仪偏差，并加入比例—积分(proportional-integral,PI)控制环节，用于实现姿态误差的动态权重补偿。实验表明:算法计算量小，实时性好，精度高，能够很好的应用于六旋翼飞行器中。

1 飞行器系统建模与姿态估计

1.1 坐标系定义

根据文献[14]对六旋翼飞行器的姿态描述，需要定义导航坐标系(N系)和机体坐标系(b系)。如图1所示，N系采用(东、北、天)的原则选择坐标轴 (X,Y,Z) 的正方向；b系的坐标轴为x,y,z。飞行器的姿态角度在N系中分别用横滚角φ、俯仰角θ和偏航角ψ描述。

捷联惯性导航系统将惯性传感器直接固定在飞行器上，惯性传感器测得的数据基于b系。姿态解算结果的坐标需从b系转换到N系,如式(1)

(X,Y,Z)=(x,y,z)

(1)

图1 六旋翼飞行器坐标系

(2)

式中φ,ψ,θ分别为偏航角、横滚角、俯仰角。

1.2 姿态更新四元数算法

Q(q0,q1,q2,q3)=q0+q1i+q2j+q3k

(3)

(4)

(5)

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

(6)

比较式(5)和式(6)可得飞行器的姿态角为

(7)

四元数的微分方程为

(8)

式中ωx,ωy,ωz为机体坐标系下的角速度。在已知初始四元数的情况下，通过三轴陀螺仪测量3个轴的角速度即可实时更新四元数的值，进而更新姿态角获得姿态信息。

1.3 互补滤波算法

MEMS传感器中的陀螺仪能够测量机体旋转角速度，其动态性能较好，但由于其角度由积分得到，随着时间的积累，角度值存在很大误差。加速度计的静态特性非常好，但由于其对干扰非常敏感，在飞行过程中，易受噪声和振动的影响，动态特性较差。因此，在低频段一般可以信赖加速度计的数据，在高频段信赖陀螺仪的数据，互补滤波算法就是依据两者在频域上的动态响应特性的互补关系，在频域中加入低通和高通滤波器，对两者的姿态数据进行融合，利用加速度计的测量值来修正陀螺仪的误差，提高系统的解算精度。设加速度计的测量值为αx,αy,αz，陀螺仪积分后的姿态解算值为νx,νy,νz，有

(9)

陀螺仪与加速度计之间的误差为

(10)

但由于在噪声较大时，低通滤波器的阻带衰减较慢，固定的参数必然难以达到在所有情况下的最优估计值，滤波效果难以达到预期。因此，本文在互补滤波的原理基础上，在计算出陀螺仪与加速度计之间的误差之后，引入了PI控制器，构成自适应互补滤波器，动态补偿系数Kp，Ki，噪声干扰较小时，适当提高加速度计的权重，在噪声干扰较大时，适当降低加速度计的权重，主要依靠陀螺仪进行姿态测量，从而提高姿态数据的融合精度，即使在噪声大的情况下，也能较为准确地解算出姿态角。原理如图2所示。姿态更新算法软件设计流程如图3所示。

图2 加入PI控制器的互补滤波算法原理框图

误差积分调节

误差比例调节

(11)

由此，获得更新的四元数并代入式(7)中，即可得出较为准确的姿态角度。

图3 滤波算法程序流程

2 实验结果与分析

为了验证算法的可行性与效果，搭建了基于匿名科创制作的开源六旋翼飞行器平台进行实验，如图4所示。其主控芯片采用STM32F103，主频72MHz，具有丰富的外设接口。姿态芯片采用MPU6050，包含3轴陀螺仪和3轴加速度计。通信方式为WiFi模式，可以实时回传数据，无需保存到内存卡，因此，在其提供的地面站上非常方便进行滤波算法调试。本文以X轴的实时输出为例，提供实时三轴惯性原始数据和参考姿态角。

图4 匿名科创实验平台

采用双闭环控制PID算法控制6个电机的转速，软件上控制系统的周期为3ms，脉宽调制(pulse width modulation,PWM)频率为10kHz，同时进行限幅处理，避免油门突然增大或减小引起飞行器震荡，导致系统不稳定。陀螺仪输出信号的低通滤波器的截止频率ωc=100Hz，陀螺仪的输出量程为±1000°/s，更新周期为0.0025s，在此不考虑偏航角对飞行器偏航情况的影响。PID参数为0.85,32,0.05，一阶滤波、卡尔曼滤波、二阶互补滤波算法的实时滤波效果波形如图5所示。由图5可以看出二阶互补滤波与卡尔曼滤波效果相近。但可以看出，二阶互补滤波与卡尔曼滤波效果相较于一阶滤波有一定的滞后，由于其计算量相对较大，实时跟踪性较一阶滤波效果差。表1为时间消耗与角度估计的标准差比较。

图5 X轴3种滤波对比

由图6实时数据波形的结果可以得出：传统一阶互补滤波的姿态解算误差角为12.5°，改进一阶互补滤波算法的姿态解算误差角为3.5°。改进一阶互补滤波算法的实时滤波动态曲线过渡平滑，实时跟踪效果较传统互补滤波更为理想。在震动较大的环境下，其实时解算精度可控制在0°～5°之间。由此，验证了算法的可行性，且改进后的一阶滤波效果更好，有效地减弱了干扰带来的影响，提高了姿态解算的精度。

表1 时间消耗与角度估计误差的标准差比较

图6 一阶滤波与加入PI控制的改进一阶互补滤波

3 结 论

分析了基于一阶互补滤波算法的姿态解算原理与实现方法，指出了运动中的加速度对姿态的影响，并提出了震动较大时的改进互补滤波算法，将加速度计解算出的角度加入一个PI控制。实验结果表明：算法提高了姿态解算的精度，且实时性较好，适用于六旋翼姿态解算。

[1] 刘 恋,向凤红,毛剑琳.修正的UKF滤波时差定位算法[J].传感器与微系统,2017,36(4):138-142.

[2] Hoffmann F,Goddemeier N,Bertram T.Attitude estimation and control of a quadrocopter[C]∥IEEE/RSJ International Confe-rence on Intelligent Robots and Systems,IEEE,2010:1072-1077.

[3] Mahmood A,Wallace J W,Jensen M A.Radio frequency UAV attitude estimation using direction of arrival and polarization[C]∥European Conference on Antennas and Propagation,IEEE,2017:1857-1859.

[4] Chang J,Cieslak J,Dávila J,et al.A two-step approach for an enhanced quadrotor attitude estimation via IMU data[J].IEEE Transactions on Control Systems Technology, 2017,99:1-9.

[5] Tarhan M,Altugˇ E.EKF-based attitude estimation and stabilization of a quadrotor UAV using vanishing points in catadioptric images[J].Journal of Intelligent & Robotic Systems,2011,62(3-4):587-607.

[6] 闫保芳,毛庆洲.一种基于卡尔曼滤波的超宽带室内定位算法[J].传感器与微系统,2017,36(10)：137-140,143.

[7] 吴 涛,白 茹,朱礼尧,等.基于卡尔曼滤波的航姿参考系统设计[J].传感技术学报,2016,29(4):531-535.

[8] Wang S,Yang Y.Quadrotor aircraft attitude estimation and control based on Kalman filter[C]∥Control Conference,IEEE,2012:5634-5639.

[9] 贾瑞才.基于四元数EKF的低成本MEMS姿态估计算法[J].传感技术学报,2014(1):90-95.

[10] Mahony R,Hamel T,Pflimlin J M.Nonlinear complementary filters on the special orthogonal group[J].IEEE Transactions on Automatic Control,2008,53(5):1203-1218.

[11] 傅忠云,刘文波,孙金秋,等.自适应混合滤波算法在微型飞行器姿态估计中的应用[J].传感技术学报,2014(5):698-703.

[12] 刘 洲,单修洋,谭 芳.自适应显式互补滤波在六旋翼飞行器中的应用[J].传感器与微系统,2017,36(5):157-160.

[13] 赵 阳,娄小平,刘 锋,等.自适应MEMS加速度计滤波算法[J].传感器与微系统,2016,35(11):120-122.

[14] 秦永元.惯性导航[M].北京：科学出版社,2013:5-7.