基于MPU6050和互补滤波的四旋翼飞控系统设计*

张承岫，李铁鹰，王耀力

（太原理工大学信息工程学院，太原030024）

基于MPU6050和互补滤波的四旋翼飞控系统设计*

张承岫，李铁鹰*，王耀力

（太原理工大学信息工程学院，太原030024）

针对四轴飞行器飞行性能不稳定和惯性测量单元（IMU）易受干扰、存在漂移等问题，利用惯性传感器MPU6050采集实时数据，以经典互补滤波为基础，提出一种可以自适应补偿系数的互补滤波算法，该算法在低通滤波环节加入PI控制器，依据陀螺仪测得的角速度实时调节PI控制器补偿系数。飞行器姿态控制系统采用双闭环PID控制方法，姿态解算的欧拉角作为系统外环，陀螺仪角速度作为系统内环。最后，搭建以NI myRIO为核心控制器的四轴飞行器，通过LabVIEW实现算法和仿真，实验结果表明，自适应互补滤波算法可以准确解算姿态信息，双闭环PID控制超调量小、反应灵敏，控制系统基本满足飞行要求。

四旋翼飞行器；惯性传感器MPU6050；自适应互补滤波；双闭环PID；LabVIEW语言

EEACC:2575；1270F；7320Edoi:10.3969/j.issn.1004-1699.2016.07.011

四旋翼飞行器因机动性能强、体积小、可垂直升降、隐蔽性好等优点，广泛应用于国内外军事和工业领域，国内尤以大疆无人飞行器控制系统领先。近年来，随着飞行器功能的不断革新，用户体验性逐步增强，操作变得更加简单，已受到广大航模航拍爱好者和科研领域研究者的青睐，并被使用在影视广告中，展现出很大的商业价值。

四轴飞行器的设计主要由姿态解算和飞行控制两部分组成。姿态解算中的姿态信息主要通过惯性测量单元IMU（Inertial Measurement Unit）测量获得，包括三轴加速度计和三轴陀螺仪。由于微机电系统MEMS（Micro-Electro-Mechanical System）具有低功耗、易于集成等优点，已成为IMU的首选，然而惯性传感器易受噪声干扰且存在漂移误差，姿态解算精度降低，影响飞控效果，因此需要通过数据融合提高解算的准确性。针对飞行器的姿态估计问题，研究者们提出了一系列的算法，文献［1-2］采用卡尔曼滤波［1-2］进行数据融合，针对非线性系统，Bucy，Sunahara等人又提出了扩展卡尔曼滤波［3-4］，然而卡尔曼滤波需要对噪声建模，建模的不精确会直接影响姿态估计的准确性；文献［5-6］采用粒子滤波，该方法可以解决非线性的问题，但是由于计算量大，在低成本的飞控系统中应用较少［5-6］；互补滤波则是从频域的角度，对数据进行融合，消除信号干扰，从根本上避免了噪声建模误差对姿态估计的影响。飞行控制方法主要包括反步法［7-8］、滑模控制［9-10］、组合法和PID控制［11］等。

本文在经典互补滤波的基础上，在加速度计的低通滤波环节加入PI控制器，根据陀螺仪角速度实时调节PI控制器补偿系数，提出一种可以自适应补偿系数的互补滤波算法，对姿态数据进行融合，并通过双闭环PID控制对四旋翼飞行器实现飞行控制，搭建飞行器硬件和控制系统仿真平台，并将采集的数据通过Wifi发送到上位机上，通过LabVIEW［12］界面实时显示波形曲线，对四旋翼飞行器控制系统稳定性和动态性能进行测试。

1　系统建模与姿态估计

定义导航坐标系（N系）和四旋翼载体坐标系（B系），XYZ导航坐标系为“东-北-天”坐标，以X轴正方向为头，X'Y'Z'为载体坐标系，N系和B系角度转换关系如图1所示，飞行器的姿态分别用横滚角ϕ，俯仰角θ，偏航角ψ描述［13］。

图1　导航坐标系与载体坐标系的角度转换关系

IMU固定在飞行器上，惯性传感器测得的数据基于载体坐标系，飞行器在飞行过程中，B系相对于N系会形成相应的角度，在姿态解算时，需将B系的数据转换到N系下，对应的欧拉角形式下的方向余弦矩阵可用式（1）表示。

惯性测量单元由三轴陀螺仪和三轴加速度计组成，陀螺仪测量系统旋转角速度，用ω表示，加速度计测量三轴加速度，用a表示。飞行器在静止或匀速飞行状态下的姿态角估计值可表示如式（2）所示，其中分别为在一段时间内沿三轴的加速度平均值［14］。

不考虑陀螺仪的漂移，并假设积分周期足够小的情况下，陀螺仪的角速度可以通过积分运算直接得到角度，如式（3），其中ωm为陀螺仪测得的旋转角速度。

由式（2）和式（3）得到的分别是理想情况下的角度估计值，在实际过程中，由于测量噪声、陀螺仪漂移的存在，通过以上方法得到的姿态解算值会存在很大误差，需要将两传感器的姿态数据加以融合，获得准确的姿态角。

2　自适应互补滤波原理及算法

惯性测量单元中的加速度计主要测量机体的线加速度，在静止状态下，可以准确测得加速度值，具有良好的静态特性，但在飞行过程中，易受噪声和震动的影响，导致测量不准确，动态特性较差；陀螺仪主要测量旋转角速度，跟踪性能好，但是由于角度是通过陀螺仪角速度经积分得到，而陀螺仪本身易受温度的影响存在零点漂移，随时间的积累，角度值存在很大误差。如果以此角度值作为解算的姿态角，对飞行器进行PID控制，将无法恢复飞行器的水平姿态，造成飞行失控。

为消除惯性传感器的漂移和噪声干扰，获得准确的姿态信息，综合加速度计和陀螺仪各自的优缺点可以发现，应用互补滤波算法，从频域的角度分别加入低通和高通滤波器，将两传感器的姿态信息加以融合，可去除干扰，消除零位误差，提高解算精度［15］。

其中θ为根据式（2）计算得到的姿态角预估值。通过式（4）可以消除高频、低频干扰，该滤波能否达到预期效果的关键在于参数K的选取，但是在噪声较大时，由于低通的阻带衰减较慢，滤波效果并不好。本文在上述互补滤波原理的基础上，在低通环节增加PI控制器，在角速度的不同阶段，动态调整补偿系数KP、Ki构成自适应互补滤波器，提高姿态数据融合精度，解算出准确的姿态角，自适应互补滤波原理见下图（2）所示。

图2　自适应互补滤波算法原理框图

根据原理框图可知，加速度计测量的加速度，经式（2）得到姿态估计值，再经PI控制与陀螺仪的角速度经积分得到的角度融合，解算出飞行器当前姿态角，融合的角度同时作为负反馈，实时更新飞行器欧拉角，姿态更新算法流程见图3所示。

图3　姿态更新算法流程图

自适应补偿系数可以通过式（5）得到，其中ωmax为陀螺仪的最大量程，ωc为陀螺仪的截止角速度（ωc＜ωmax）。

3　双闭环PID控制

飞行控制算法是决定飞行器性能的关键因素［16］，飞行器在飞行时，如果突遇外力或电磁干扰，会产生激烈震荡，此时单环PID无法快速恢复系统稳定，本文在传统PID控制的基础上，引入角速度反馈，搭建双闭环PID控制系统，提高系统抗干扰能力，加强系统的鲁棒性。

双闭环PID控制原理框图见下图4，经互补滤波算法解算得到的姿态角与期望姿态角的差值记为姿态误差经角度PID控制得到角度控制量，将更新的欧拉角作为系统外环反馈，而陀螺仪测量的角速度变化则作为内环反馈，进行角速度PID控制［17］。

图4　双闭环PID控制原理框图

双闭环PID控制算法采用位置式PID：

为防止油门突然增大或减小，引起飞行器震荡，导致系统不稳定，需要对PID控制量先进行限幅处理，并根据十字形飞行器模式将角度控制量转换为油门输出值。

4　实验测试

本文搭建了以NI（National Instruments）公司生产的NI myRIO-1900为核心控制器，InvenSense公司生产的MPU6050芯片作为惯性测量单元的硬件平台，并配以LM3510SM（700 kV）电机驱动、TATTU聚合物锂电源和机械组成模块；采用NI公司开发的LabVIEW软件编写算法和仿真程序，用于四旋翼飞行器软硬件系统测试。

四旋翼起飞前的姿态是决定飞行器能否平稳起飞的关键，因此测量到准确的姿态信息至关重要，图5为飞行器在静止时刻经自适应互补滤波后的欧拉角曲线。

由图5可知，由于试验台并非绝对水平，初始时刻的欧拉角不处于0°，经过互补滤波后，飞行器在静止时刻得欧拉角分别为：横滚角为-0.2°，俯仰角为-3.6°，偏航角为0.2°。

图5　自适应互补滤波后欧拉角曲线

四旋翼飞行器自适应互补滤波前后的动态测试对比，如图6所示。四旋翼在飞行过程中，载体坐标系绕导航坐标系旋转会产生相应的角度，MPU6050实时采集加速度和角速度。由图可见，互补滤波前，初始时刻三轴欧拉角均为10°，经过一段时间的转动后，横滚角、俯仰角、偏航角均产生偏移误差，无法满足姿态解算的可靠性要求。而经过自适应互补滤波后的曲线则不会发生偏移，仍保持在10°。

图6　自适应互补滤波前后的动态测试对比

依据自适应互补滤波法进行测试，陀螺仪的最大量程ωmax=2000°/s，截止角速度ωc=2πf，其中 f为陀螺仪截止频率，约为100 Hz，KP0，KP1经PID参数工程整定获得，分别为5和20。由于系统未加磁力计，四旋翼在飞行过程中无法修正Z轴的姿态信息，偏航角存在偏差，在不考虑飞行器偏航的情况下，可以忽略。

合理选择比例、积分、微分系数是决定PID控制器能否快速跟踪四旋翼姿态变化，及时调整电机输出，恢复飞行器平衡的关键。在对搭建的飞行器进行测试之前，先在LabVIEW上设计飞行器模型PID控制器仿真平台，模拟四旋翼在飞行过程中的惯性效应、重力和随机干扰的影响和电机的推力。程序运行后，调节setpoint给定四旋翼单轴姿态角度，调整KP，Ki，Kd参数，观察阶跃输入的响应曲线、响应时间和超调量的变化，慢慢调整PID增益，直到得到合适的响应曲线。经过测试，当setpoint给定20，即横滚角为20度时，最终选取四旋翼飞行器的PID增益KP为0.85，Ki为26，Kd为0.003，此时PID输出为0.19，响应时间为0.25 s，超调量为0.0333。飞行器模型PID控制器仿真界面如图7所示。

图7　前面板显示四旋翼飞行器模型单轴仿真界面

根据仿真确定的PID增益和自适应互补滤波算法测试的结果，对搭建的四旋翼飞行器进行测试。如图8所示，将飞行器装有1号、3号电机的X轴固定，即可以改变横滚角，给定横滚角为20°，期望角为0°，逐渐增加油门，观察传输到上位机显示的波形曲线以及四旋翼飞行器恢复平衡的情况。

图8　姿态控制调试

经过多次软硬件测试，飞行器与上位机通讯良好，无信号中断或失控现象，飞行器能够快速恢复平衡并稳定下来，结果表明系统的设计合理，基本满足飞行需求。

5　结束语

本文针对姿态精确解算和飞行控制不稳定问题，介绍了四旋翼飞行器飞控系统的设计方法，采用自适应互补滤波算法和双闭环PID控制实现姿态信息的实时解算和飞行控制，搭建以NI myRIO为核心控制器的四旋翼飞行器硬件平台，并在Lab-VIEW软件上实现单轴飞行器模型仿真、油门控制以及曲线的实时显示。本设计能够准确解算姿态信息，控制系统快速进入稳定状态，基本满足飞行要求。下一步将在系统中加入磁力仪和高度传感器，实现对偏航角的修正和定点悬停。

［1］ 王帅，魏国.卡尔曼滤波在四旋翼飞行器姿态测量中的应用［J］.兵工自动化，2011，30（1）：73-75.

［2］ 吴友强，纪浩，邹力涵.基于卡尔曼滤波的四旋翼飞行器悬停控制研究［J］.自动化与仪表，2013，28（11）：1-5.

［3］ 郭晓鸿，杨忠，陈喆，等.EKF和互补滤波器在飞行姿态确定中的应用［J］.传感器与微系统，2011，30（11）：149-152.

［4］ 贾瑞才.基于四元数EKF的低成本MEMS姿态估计算法［J］.传感技术学报，2014，27（1）：90-95.

［5］ 夏楠，邱天爽，李景春，等.一种卡尔曼滤波与粒子滤波相结合的非线性滤波算法［J］.电子学报，2013，41（1）：148-152.

［6］ 曲仕茹，马志强.改进的粒子滤波在四旋翼姿态估计中的应用［J］.飞行力学，2013，31（5）：458-461，466.

［7］ 赵元伟，卢京朝.四旋翼飞行器的建模及基于反步法的控制［J］.科学技术与工程，2013，13（34）：10425-10430.

［8］ 陈奕梅，陈增辉，修春波.四旋翼无人飞行器反步法的控制［J］.天津工业大学学报，2015，34（5）：32-36.

［9］ 李波波，贾秋玲.基于滑模控制的四旋翼飞行器控制器设计［J］.电子设计工程，2013，21（16）：76-78.

［10］薛鹏，任鹏飞，曹学儒.四旋翼飞行器滑模控制的稳定性分析［J］.河南工程学院学报（自然科学版），2015，27（1）：33-35.

［11］Yibo L，Qiling X，Jianda H.Modeling and PID Control of Tethered Unmanned Quadrotor Helicopter［C］//Mechatronic Sciences，Electric Engineering and Computer（MEC），Proceedings 2013 International Conference on.IEEE，2013：58-262.

［12］王玉刚，王恒新，陈晔，等.基于LabVIEW的某型航箭发射检测仪设计［J］.现代电子技术，2015，38（20）：81-83.

［13］万晓凤，康利平，余运俊，等.互补滤波算法在四旋翼飞行器姿态解算中的应用［J］.测控技术，2015，34（2）：8-11.

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

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

［16］叶树球，詹林.基于PID的四旋翼飞行器姿态控制系统［J］.计算机与现代化，2015，20（5）：117-120.

［17］Cavalcante Sa R，De Araujo A L C，Varela A T，et al.Construction and PID Control for Stability of an Unmanned Aerial Vehicle of the Type Quadrotor［C］//Robotics Symposium and Competition（LARS/LARC），2013 Latin American.IEEE，2013：95-99.

张承岫（1989-），女，黑龙江齐齐哈尔人，硕士研究生，主要研究方向为四旋翼飞行器控制算法优化及智能控制，316567231@qq.com；

李铁鹰（1956-），男，山西太原人，教授，博士，硕士生导师，院党委书记，长期从事智能控制与自动化研究，Lty@tyut. edu.cn；

王耀力（1965-），男，山西太原人，副教授，博士，硕士生导师，长期从事信息系统设计、人机视觉分析与处理以及嵌入式系统电路设计理论的研究，wangyaoli@tyut. edu.cn。

Design of Quad-Rotor Aircraft Flight Control System Based on MPU6050 and Adaptive Complementary Filter*

ZHANG Chengxiu，LI Tieying*，WANG Yaoli
（College of Information Engineering，Taiyuan University of Technology，Taiyuan 030024，China）

Against the flight performance instability of quad-rotor aircraft and Inertial Measurement Unit（IMU）'s defects，such as lack of anti-interference and drifting etc，this paper presents a complementary filtering algorithm that adaptive to compensation coefficient，based on classic complementary filter.Collecting real-time data via inertial sensor MPU6050，this algorithm adds PI controller into the low-pass filter link，and adjusts the compensation coefficient of PI controller in real-time mode，based on the angular velocity of the gyroscope.Aircraft attitude control system adopts double closed loop PID control method，which takes the Euler angles of the attitude algorithm as its outer loop and the angular velocity of the gyroscope as its inner loop.In the final test，a quad-rotor aircraft is built with NI myRIO as a core controller，and implement the algorithm and simulation through the LabVIEW.Experimental result demonstrates that adaptive complementary filter algorithm can calculate attitude information accurately，with the advantages of little control overshoot and the sensitive response of the double closed loop PID control，the control system basically meets the requirements of flight.

quad-rotor aircraft；inertial sensor MPU6050；adaptive complementary filter；double closed loop PID；LabVIEW language

TP273

A

1004-1699（2016）07-1011-05

项目来源：山西省自然科学基金项目（2013011015-1）

2016-01-26修改日期：2016-03-07