基于四元数二阶互补滤波的四旋翼姿态解算

基于四元数二阶互补滤波的四旋翼姿态解算
伏家杰;周翟和;尹辉;陈燕
【摘要】姿态解算是四旋翼飞行器的关键技术,其精度直接影响飞行器控制的可靠性和稳定性.针对当前常用一阶互补滤波算法中阻带衰减速率慢和陀螺仪常值漂移产生的稳态误差问题,通过增加积分环节,设计了一种基于四元数的二阶互补滤波算法,能更好地利用加速度计和磁力计的稳态信息有效补偿陀螺仪常值漂移,从而减少姿态解算的累积误差.仿真结果表明,该算法具有更好的稳定性,提高了系统的姿态解算精度.通过飞行器真实飞行数据对算法进行了实验验证,结果显示姿态的俯仰角、横滚角精度<1°,偏航角精度<2°,能很好地满足飞行器控制系统对姿态解算的精度要求.
【期刊名称】《机械制造与自动化》
【年(卷),期】2019(048)002
【总页数】5页(P169-172,176)
【关键词】姿态估计;四元数;互补滤波;四旋翼飞行器;MEMSIMU
【作者】伏家杰;周翟和;尹辉;陈燕
【作者单位】南京航空航天大学自动化学院,江苏南京211106;南京航空航天大学自动化学院,江苏南京211106;南京航空航天大学自动化学院,江苏南京211106;南京航空航天大学自动化学院,江苏南京211106
【正文语种】中文
【中图分类】V211.8
0 引言
四旋翼飞行器姿态解算的精度和速度将直接影响飞行器控制算法的稳定性、可靠性和实现的难易程度。

所以姿态解算是四旋翼飞行器控制实现的前提[1-2]。

随着微机电技术和计算机技术的发展，由于陀螺仪和加速度计组成的低成本惯性测量单元(IMU)具有体积小、重量轻、功耗低、性价比高等特点，在四旋翼飞行器姿态的测量系统得到普遍应用[3-4]。

本文研究的四旋翼飞行器姿态测量传感器主要由低成本微机电系统(MEMS)陀螺仪、加速度计和磁力计组成。

本文采用嵌入式微控制器和低成本IMU组成姿态测量系统，采用四元数方法描述系统运动模型，提出了一种基于四元数的二阶互补滤波算法。

该方法可以提高滤波器阻带衰减速率，通过加速度计和磁力计直接补偿陀螺仪漂移误差解算得到载体姿态值。

实验和仿真表明，将本文提出的算法应用于四旋翼飞行器系统中可以提高四旋翼飞行器姿态估计的准确性，增强其控制的稳定性。

1 四旋翼姿态系统整体设计
四旋翼飞行器姿态测量系统框图如图1所示，整个系统主要由姿态测量单元、控制单元以及显示与调试单元构成。

姿态测量单元主要由磁力计和惯性传感器组成。

惯性传感器采用的是InvenSense公司的低成本MPU6050。

该传感器单片上整合了三轴陀螺仪和三轴加速计。

磁力计采用弱磁传感器芯片HMC5883L。

控制单元选用了ARM嵌入式微处理器STM32F103RCT6作为系统控制核心。

传感器采集的数据经过数学推算和四元数二阶互补滤波处理可得到准确的飞行器三轴姿态角。

地面站软件可以实时分析和显示飞行器通过无线射频方式发送的姿态角数据并控制飞行器的运行状态。

本文使用的四旋翼飞行器工作于低动态和非复杂环境内，因此系统所用传感器和控制器足以满足整个系统的需求。

图1 四旋翼飞行器姿态测量系统
2 四元数二阶互补滤波姿态建模
为保证飞行器能够正常运行，姿态测量系统需要通过实时地更新载体的姿态信息补偿其运动变化情况[5-6]。

飞行器的姿态估计系统中，首先需要定义导航坐标系Oxnynzn和载体坐标系Oxbybzb。

本文中所采用的导航坐标系即为当地地理坐标系，其Oxn、Oyn和Ozn坐标轴的方向分别取东向、北向和天向(ENU)。

载体坐标系(b系)Oxb沿载体横轴向右，Oyb沿载体纵轴向前，Ozb沿载体竖轴向上。

Oxb、Oyb和Ozb分别对应载体的俯仰轴、横滚轴和偏航轴。

由欧拉定理，θ、γ、ψ为导航坐标系Oxnynzn旋转到载体坐标系Oxbybzb的一组欧拉角，分别为俯仰角、横滚角、偏航角。

经旋转可得姿态变换矩阵：
(1)
四元数是一种间接求解姿态变换矩阵的方法。

通过对四元数微分方程的解算，求解得到四元数即时值用来代替方向余弦矩阵微分方程的解算[7-8]。

Qq为导航坐标系到载体坐标系的姿态四元数向量：
(2)
用四元数表示导航系到载体系的转换关系可以得到四元数姿态矩阵：
(3)
根据式(3)可知，四元数姿态矩阵可以全姿态工作，同时微分方程不含三角函数计算量得以降低，因此本文采用四元数方法解算系统姿态角[9]。

姿态四元数Qq与陀螺仪输出角速率具有如下微分关系：
(4)
其中为陀螺仪输出的载体系相对导航系的角速率矢量。

本文采用一阶龙格-库塔法求解四元数微分方程。

(5)
其中T为采样周期。

每次更新后都要将四元数Qq归一化，用Cij表示的元素(i=1,2,3;j=1,2,3)，则可
以得到四元数表示的导航坐标系下的姿态角：
(6)
算法中仅使用陀螺仪的数据更新计算四元数，并没有使用其他辅助传感器对陀螺仪数据进行修正。

四元数微分方程在解算过程中涉及到积分运算，由于陀螺仪具有常值漂移，长时间工作会产生较大的累积积分误差[10]。

3 姿态算法的实现
3.1 互补滤波器
互补滤波器作为一种频域滤波器，它可以对不同频率特性的噪声取长补短来降低或滤除噪声的干扰[11]。

以陀螺仪和加速度计这一双输入系统为例，陀螺仪具有漂移，会产生低频累积误差。

加速度计容易受到高频噪声的干扰。

由于陀螺仪和加速度计输出具有频率互补的特性，可采用互补滤波的方法进行数据融合。

三轴姿态互补滤波器原理如图2所示。

图2 互补滤波姿态解算原理
图2中互补滤波器由低通滤波器GL(s)=C(s)/[C(s)+s]和高通滤波器
GH(s)=s/[C(s)+s]两部分构成。

当GL(s)+GH(s)=1时可以实现信号的重构，因此互补滤波算法能同时滤除低频干扰与高频干扰，实现传感器数据融合和姿态的高精
度测量[12]。

互补滤波器对姿态角的估计值用Laplace形式表示为：
(7)
3.2 二阶互补滤波器设计
一阶互补滤波器中选取C(s)=kp，根据式(7)可得差分方程表达式为：
(8)
一阶互补滤波实质上是一个Ι型的动态反馈控制系统，虽然它可以在一定程度上抑制陀螺仪积分产生的发散现象，但是由于陀螺仪输出包含的常值漂移最终会被积分至某一状态观测值，因此一阶互补滤波器无法消除系统的稳态误差[13]。

针对以上问题本文设计了基于四元数的二阶互补滤波算法，在反馈环节C(s)中加
入了积分环节来提高系统型别，增大阻带衰减速率，从而消除常值漂移产生的稳态误差。

(9)
式中kp和ki分别为比例系数和积分系数。

图3为加入积分器的二阶互补滤波控制系统原理图，其控制系统用Laplace形式
表示为：
(10)
图3 二阶互补滤波解算原理
四元数二阶互补滤波算法将加速度计和磁力计在不同坐标系下测量的误差值经过
PI调节后直接对陀螺仪进行补偿和校正，再用补偿后的陀螺仪数据去更新四元数，利用校正后四元数解算出精确的姿态角[14]。

由式(3)可得导航坐标系下的重力矢量gn转换到载体坐标系下的四元数表示形式：
(11)
将加速度计实际测量值Ab与重力在载体系下的分量Vb作向量积可以得到加速度计的误差值。

在实际应用中磁力计所在的载体系需要与导航系水平面重合，此时磁力计在导航系下测量的磁场输出为和EU的精确值可以利用加速度计修正后的四元数姿态矩阵解算。

(12)
其中Mb为磁力计在载体系下的测量值。

通过式(12)变换可以得到在导航系下磁力计水平信息的输出，则再利用四元数姿态矩阵将导航系下的参考向量Mn投影到载体系下：
(13)
磁力计实际测量值Mb与参考向量在载体系下的分量Wb作向量积运算可以得到
磁力计的误差值。

加速度计误差和磁力计误差结果相加可以补偿陀螺仪：
e=Ab×Vb+Mb×Wb
(14)
根据向量积误差用PI修正陀螺仪零偏：
(15)
其中：为累积误差，为修正后的陀螺仪输出。

四元数二阶互补滤波算法原理如图4所示。

4 仿真与实验分析
本文采用两种实验方法验证四元数二阶互补滤波算法的有效性：一种是由MATLAB产生数据进行静态仿真实验，另一种是使用飞行器在实际飞行过程中真实传感器数据进行实验分析。

图4 二阶互补滤波器算法流程
4.1 姿态算法仿真验证
实验相关参数如下：采样周期TS=10ms，总采样时间60s。

三轴陀螺仪噪声协方差Qg=0.01，三轴加速度计噪声协方差Va=0.001，三轴磁力计噪声协方差
Um=0.000 1。

仿真实验设计了数据发生器模拟四旋翼飞行器的姿态角角速率动态变化情况。

采用四元数二阶互补滤波仿真结果如图5所示。

图5 二阶互补滤波误差仿真
图5为俯仰角和偏航角误差仿真，横滚角和俯仰角类似。

从图5可以看出，使用加速度计或磁力计进行姿态解算其波形还有一定量的噪声无法满足四旋翼飞行的需求。

由于陀螺仪具有常值漂移，仅单独使用陀螺仪进行姿态解算，随着时间的增加将出现积分角度漂移。

仿真图中陀螺仪仅进行了60s姿态解算，漂移误差已将近1.5°。

二阶互补滤波器能有效地融合陀螺仪、加速度计和磁力计的输出数据，陀螺数据中漂移误差也得到了有效修正。

一阶互补滤波和二阶互补滤波仿真效果对比如图6所示。

图6 两种算法滤波对比
图6为俯仰角和偏航角对比，横滚角和俯仰角类似。

在图6仿真中的10s和50s 处，一阶互补滤波对俯仰角和横滚角的估计误差已达1°以上。

根据图6和表1中的误差数据分析可知，与一阶互补滤波算法相比二阶互补滤波算法对姿态角的估计误差值更小且误差曲线更为稳定，特别在四旋翼姿态持续动态变化过程中，二阶互补滤波估计性能明显高于一阶互补滤波。

表1 姿态角误差标准差(°) 误差标准差原算法本文算法俯仰角0.3850.065横滚角0.3600.091偏航角0.3540.085
4.2 实验验证
如图7所示为四旋翼的实验数据采集实物平台，通过地面站控制四旋翼飞行器飞行，载体上微控制器每10 ms内中断采集一次陀螺仪、加速度计和磁力计解算的角度值和滤波数据融合结果。

飞控板搭载了NRF无线射频收发芯片，实验数据以无线射频方式传输到计算机地面站中。

图7 实验数据采集平台
为了便于分析实验数据，分别对飞行器在俯仰、横滚和偏航的飞行状态下进行实验数据采集。

对采集得到的数据仅对前60s数据进行截断分析。

图8中左侧为两种滤波算法60s的实验波形，右侧为相应实验波形的55～60s局部放大图。

由实验结果可以看出，无论使用哪种滤波算法，对姿态测量系统的导航参数均有明显的改善。

由波形的局部放大图可知，两种滤波算法对俯仰角和横滚角的估计精度基本相同，而对于偏航角的估计，本文算法仍然可以保持较高的估计精度，原算法已经产生较大的估计误差，误差为5°左右。

经分析造成这一现象的原因主要有：磁力计对偏航角解算需要加速度计提供水平信息，加速度计高频干扰与磁力计噪声叠加削弱了磁力计对陀螺仪修正作用；一阶互补滤波器仅对陀螺仪和磁力计分配不同权重的滤波系数，通常陀螺仪滤波系数远大于磁力计滤波系数，在没有积分环节的情况下，陀螺仪常值漂移产生的稳态误差很难消除。

图8 两种滤波算法实验波形
5 结语
本文提出一种由MEMS陀螺仪、加速度计和磁力计构成的四旋翼飞行器姿态测量系统，设计了基于四元数的互补滤波器的姿态测量系统数据融合方法。

根据加速度
计和磁力计提供的辅助信息，采用互补滤波算法可以对陀螺漂移误差起到有效的抑制作用。

本文提出的算法增加了积分环节可以有效地解决系统的稳态误差。

该姿态测量系统采用四元数进行姿态解算，具有运算量小、结构简单、不存在万向锁等优点。

仿真和最终实验结果均表明，本文算法能够提高系统的姿态估计精度，满足飞行器高精度的控制需求。

参考文献:
【相关文献】
[1] 沈晓卫, 姚敏立, 赵鹏. 基于互补滤波的动中通天线姿态估计[J]. 中国惯性技术学报, 2011,
19(2):194-197.
[2] 何虔恩, 高钟毓, 吴秋平,等. 基于互补滤波的惯导系统水平阻尼网络设计[J]. 中国惯性技术学报, 2012, 20(2):157-161.
[3] 虞波, 周翟和, 赵庆涛,等. 一种带遗忘因子的自适应卡尔曼滤波器及其在移动机器人中的应用[J]. 机械制造与自动化, 2016,45(4):149-152.
[4] 张勇刚, 张云浩, 李宁. 基于互补滤波器的MEMS/GPS/地磁组合导航系统[J]. 系统工程与电子技术, 2014, 36(11):2272-2279.
[5] 梁光胜, 杜梦楠, 周子豪,等. 基于互补滤波的两轮自平衡车姿态控制[J]. 测控技术, 2015,
34(5):72-74.
[6] 吕印新, 肖前贵, 胡寿松. 基于四元数互补滤波的无人机姿态解算[J]. 燕山大学学报,
2014(2):175-180.
[7] 向少林, 钱正洪, 白茹,等. 基于互补滤波算法的四轴飞行控制系统设计[J]. 机电工程, 2016,
33(2):207-211.
[8] 沈超, 周翟和, 胡佳佳. 基于无线射频技术的移动机器人姿态解算调试系统设计[J]. 机械制造与自动化, 2016,45(6):156-158.
[9] 张栋, 焦嵩鸣, 刘延泉. 互补滤波和卡尔曼滤波的融合姿态解算方法[J]. 传感器与微系统, 2017, 36(3):62-65.
[10] Shen X, Yao M, Jia W, et al. Adaptive complementary filter using fuzzy logic and simultaneous perturbation stochastic approximation algorithm[J]. Measurement, 2012, 45(5):1257-1265.
[11] Widodo R B, Edayoshi H, Wada C. Complementary filter for orientation estimation:
Adaptive gain based on dynamic acceleration and its change[C]// International Symposium on Soft Computing and Intelligent Systems. IEEE, 2015:906-909.
[12] Euston M, Coote P, Mahony R, et al. A complementary filter for attitude estimation of
a fixed-wing UAV[C]// Ieee/rsj International Conference on Intelligent Robots and Systems. IEEE, 2008:340-345.
[13] Y.Tang. Velocity estimation based on a complementary filter for smart car speed control[A]. Advanced Science and Industry Research Center.Proceedings of 2014 International Conference on Artificial Intelligence and Industrial Application
(AIIA2014)[C].Advanced Science and Industry Research Center:Science and Engineering Research Center,2014.
[14] Sabatini A M. Quaternion-based extended Kalman filter for determining orientation by inertial and magnetic sensing[J]. IEEE Transactions on Biomedical Engineering, 2006, 53(7):1346-1356.。