二阶互补滤波姿态求解器的设计

南余荣,宛冬晴,潘 帅,姚亚楠

(浙江工业大学 信息工程学院，浙江 杭州 310023)

二阶互补滤波姿态求解器的设计

南余荣,宛冬晴,潘 帅,姚亚楠

(浙江工业大学 信息工程学院，浙江 杭州 310023)

为了降低小型四旋翼姿态解算的成本和提高姿态输出精度，采用了低成本MPU-9250作为IMU，设计了一种二阶互补滤波姿态求解器.二阶互补滤波算法所产生的计算量不大并且对处理芯片的性能要求不高，从而达到降低成本的目的.通过实际测试,以成品AHRS100作为航姿参考系统，得出姿态角度动态误差在3°以内.实验结果表明：所设计的姿态求解器能较精确地解算出四旋翼的姿态角度，满足了小型四旋翼对输出姿态精度的要求.

四旋翼；二阶互补滤波；姿态角度

随着科技的进步和时代的发展，无人机凭借自身特有的优势，在民用军事和农业中得到了广泛的应用[1]，例如可以用无人机进行航拍摄影，电力巡检，新闻报告，喷洒农药，侦查敌情，还可以用无人机来送快递，在这些方面无人机有着人工所不具备的特有优势，无人机成为当下科技行业研究的热点.一直以来，在诸多研究问题中，飞行器的姿态解算问题都是研究的重点[2-3].在小型四旋翼姿态解算中一般采用低成本捷联惯性导航测量单元，主要由陀螺仪，磁力计和加速度计组成，陀螺仪具有很好的动态跟踪特性[4-5]，在飞行器飞行过程中测得的角速度会比较准确，实际上单独用陀螺仪也能解算出四旋翼的姿态，但是由陀螺仪测量得到的积分角度会产生误差的累积，如果不加以补偿，长时间可能引起严重的偏差，所以笔者利用加速度计和磁力计对陀螺仪解算的角度进行误差补偿[6-7].

计算四旋翼在三维空间的旋转有很多种等价的表示方法，常见的有方向余弦、四元数和欧拉角[8].欧拉角虽然简单易于表示和理解，但是由于其自身的姿态表示特点导致万向节死锁，不能全方位地解算出四旋翼的姿态方位，方向余弦法虽然能满足四旋翼全方位姿态解算的要求，但是需要的运算量太大[9-10]，在采用低成本处理芯片的平台中，导致姿态不能实时输出.故笔者采用四元数来表示姿态变化，四元数有一些其他角位移表示方法所没有的优点如平滑插值，能和矩阵形式快速转换等[11-12].融合加速度计，陀螺仪和磁力计测得数据的方法有一阶互补滤波，二阶互补滤波和扩展卡尔曼滤波等[13]，扩展卡尔曼算法虽然比较先进，解算出来的角度能很好地跟踪飞行器真实的角度，但其设计复杂，计算量大，在小型四旋翼中普遍使用的低成本惯性测量单元不满足其对IMU性能的要求[14].一阶互补滤波算法虽然设计简单，易于调试，只需调节一个参数，但解算精确度差.故笔者采用了二阶互补滤波算法，以STM32F103C8T6为CPU，IMU采用MPU-9250，这两个模块累计成本在30元左右，而卡尔曼滤波算法采用的CPU和IMU模块累计成本普遍在80～120元之间，加入PI环节补偿单独用陀螺仪测量所产生的误差，通过调节两个参数KP和Ki最终解算出了较精确的姿态[15-16]，满足了小型低成本四旋翼飞行器求解姿态过程中对成本和精度的综合要求.

1 系统建模

对四旋翼进行姿态解算前首先要建立导航坐标系n系和机体坐标系b系，建立的导航坐标系坐标原点位于四旋翼的质心，xn轴指向北，yn指向东，zn指向地即东北地坐标系，三条轴之间都是俩俩垂直的，同理建立四旋翼的机体坐标系，机体坐标系和四旋翼固联，随着四旋翼的旋转而旋转，坐标系如图1所示.

图1 机体坐标系和导航坐标系Fig.1 Body axes coordinate system and navigation coordination system

(1)

四元数是一种高阶复数，能很方便地刻画飞行器绕任意轴的旋转，且计算量小.四元数q的表达式为

(2)

(3)

2 二阶互补滤波算法设计和姿态解算过程

陀螺仪动态响应好，积分后可测倾角，但在解算姿态的过程中，会产生累积误差和零漂等，在低频段信号不好.磁力计和加速度在低频段表现优秀，但是瞬时特性很差，当四旋翼姿态剧烈变化时，不能满足四旋翼实时解算的要求.因此可以结合它们分别在高频段和低频段的优势，利用互补滤波原理对陀螺仪测得的姿态角度进行高通滤波，对磁力计和加速度计测得的姿态角度进行低通滤波，得到在高频段和低频段都较好的信号，滤波原理如图2所示.

图2 二阶互补滤波原理Fig.2 The principle of second order complementary filter algorithm

由图(2)可以进一步得到二阶互补滤波公式为

(4)

(5)

(6)

总的误差向量e为

(7)

式中：e1为加速度计测得的单位向量与从四元数那里获得的重力向量之间的误差向量；e2为磁力计测得的单位向量与从四元数那里得到的单位向量之间的误差向量.令δ=(Kp+Ki/s)·e，陀螺仪测得的角速度为wg，将δ与wg进行叠加得到修正后的角速度w，最后来更新四元数，得

(8)

式中w=0+wxi+wyj+wzk，由式(8)可以得到四元数的迭代方程式为

(9)

由式(9)可知：只需传入角速度在三个轴的分量和周期T即可求得下一周期的四元数值，将求得的四元数进行单位化，得到最终表示四旋翼姿态的欧拉角为

(10)

详细的姿态解算过程，如图3所示.

图3 姿态解算过程Fig.3 The process of attitude calculation

3 实验测试和结果分析

3.1 实验平台

我们选用STM32F103C8T6为CPU，IMU采用MPU-9250，MPU-9250是一个QFN封装的复合芯片，它由两部分组成，一组是3轴加速度计和3轴陀螺仪，另一组则是AKM公司的AK896 3轴磁力计.采用AHRS100成品航姿系统作为参考系统进行对比，AHRS100在各种恶劣条件下均能精确解算出四旋翼的姿态.

3.2 测试结果和分析

将一阶互补滤波算法和二阶互补滤波算法的廉价IMU单元与AHRS100固定在四旋翼上并在实际场景下飞行，通过计算机串口同时读取3个测量单元的数据进行实时姿态解算，采样时间为60s，以AHRS100成品航姿系统测得的姿态数据为参考标准进行MATLAB绘图.图4为一阶互补滤波姿态求解器、二阶互补滤波姿态求解器和参考航姿系统所测得的俯仰角输出对比情况，进一步利用MATLAB内置的计算工具，求出俯仰角误差，如图5所示，可以很明显看到大部分时间里一阶解算角度误差都是大于二阶解算角度误差，二阶解算角度误差第4s和第6s左右达到最大，接近2°.而一阶解算角度最大误差在第25s达到了3.2°.

图4 俯仰角对比情况Fig.4 Contrast of pitch angle

图5 俯仰角误差Fig.5 Error of pitch angle

图6为测得的横滚角输出对比情况，进一步绘制出横滚角误差(图7)，可以很明显看到大部分时间里一阶解算角度误差都是大于二阶解算角度误差.在几个特殊的时间点由于飞行器姿态变化剧烈导致姿态求解角度和真实角度偏差较大，在第18s左右二阶解算角度误差达到最大，达到2.3°.而一阶解算角度最大误差在第39s左右达到了4.3°.

图6 横滚角对比情况Fig.6 Contrast of roll angle

图7 横滚角误差Fig.7 Error of roll angle

图8为测得的偏航角输出对比情况，进一步绘制出偏航角误差，如图9所示，可以很明显看到大部分时间里一阶解算角度误差都是大于二阶解算角度误差.二阶解算角度误差大多在1.5°以内.而一阶解算角度误差在很多时刻都超过了1.5°，解算精度明显低于二阶解算精度.

图8 偏航角对比情况图Fig.8 Contrast of yaw angle

图9 偏航角误差Fig.9 Error of yaw angle

可以得到四旋翼在飞行过程中，无论是俯仰角，横滚角还是偏航角，一阶解算的精确度都是明显低于二阶解算的精确度.大多数时间内二阶解算俯仰角误差，横滚角误差和偏航角误差分别控制在2°，1.5°，1.5°以内，姿态角最大动态误差小于2.5°，满足小型低成本四旋翼飞行器对输出姿态精度的要求.

4 结 论

针对小型四旋翼姿态解算这一问题进行了详细的分析，通过对比一阶互补滤波算法，卡尔曼滤波算法，二阶互补滤波算法最后设计了一种基于四元数的二阶互补滤波姿态求解器，并对解算的原理和流程做了详细介绍.以四旋翼飞行器为实验平台，通过实际飞行测试，对比二阶互补滤波姿态求解器和一阶互补滤波姿态求解器解算出的角度误差，得出了二阶解算精度明显高于一阶解算精度的结论，验证了笔者所设计的二阶互补滤波算法在提升输出姿态精度上的有效性.

[1] 郭晓鸿，杨忠，陈喆，等.EKF和互补滤波在飞行姿态确定中的应用[J].传感器与微系统，2011，30(11):56-58.

[2] 王立波.捷联惯导系统设计与数据处理技术研究[M].北京：科学出版社，2006.

[3] 史智宁，韩波，徐玉，等.基于重力场自适应互补滤波的无人直升机水平姿态估计方法[J].传感技术学报，2009，22(7):993-996.

[4] 方路平，高坤，潘清，等.基于陀螺仪的穿刺手术导航系统设计[J].浙江工业大学学报，2016，44(2)：129-133.

[5] 王宇，董辉，徐建明，等.一种新的智能小车控制策略[J].浙江工业大学学报，2012，40(2)：217-221.

[6] 胥芳，倪紫京，占红武.自平衡小车的模型分析与数据处理[J].浙江工业大学学报，2016，44(3)：288-291.

[7] 张荣辉，贾宏光，陈涛，等.基于四元数法的捷联惯性导航系统的姿态解算[J].光学精密工程.2008，16(10):1963-1970.

[8] 张承岫，李铁鹰，王耀力.基于MPU6050和互补滤波的四旋翼飞控系统设计[J].传感技术学报，2016，29(7)：1011-1015.

[9] 蔡磊，牛小骥，张提升，等.MEMS惯导在四旋翼飞行器中导航性能的实验分析[J].传感技术学报，2016，29(5)：711-715.

[10] 彭孝东，张铁民，李继宇，等.基于传感器校正与融合的农用小型无人机姿态估计算法[J].自动化学报，2015，41(4)：854-860.

[11] 王延东，贾宏光.组合导航系统滤波器截断误差抑制方法[J].传感技术学报，2014，27(5)：616-621.

[12] 郑君里，杨为理，应启珩.信号与系统[M].北京：高等教育出版社，2000.

[13] 文常保，高丽红，方吉善，等.基于改进型限幅平均滤波法的高精度称重系统研究[J].传感技术学报，2014，27(5)：649-653.

[14] 张洪涛.四旋翼微型飞行器位姿及控制策略的研究[D].哈尔滨：哈尔滨工业大学，2014.

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

[16] 彭孝东，张铁民，李继宇，等.基于传感器校正与融合的农用小型无人机姿态估计算法[J].自动化学报，2015，41(4)：854-860.

(责任编辑：陈石平)

Design of second order complementary filter attitude solver

NAN Yurong, WAN Dongqing, PAN Shuai, YAO Yanan

(College of Information Engineering, Zhejiang University of Technology, Hangzhou 310023, China)

A second order complementary filter solver is designed in order to reduce the cost and improve the output precision of small quad-rotors in attitude calculation process. The Low-cost MPU-9250 is adopted as IMU in this paper. The second-order complementary filtering algorithm produces little computation and the performance requirement of the processing chip isn’t high. It will achieve the purpose of reducing cost. Through the practical test, using AHRS100 as attitude reference system, and the dynamic error of the attitude angle is less than 3°. The experimental result show that the solver designed in this paper can calculate the attitude angle fairly accurately and meet the requirement of output precision of small quad-rotors.

quad-rotors; second order complementary filter; attitude angle

2016-11-30

浙江省自然科学基金资助项目(Z12E070003)

南余荣(1966—)，男，浙江乐清人，教授，博士，研究方向为电力传动及其自动化，E-mail：nyr@zjut.edu.cn.

TP273

A

1006-4303(2017)04-0416-05