四旋翼飞机姿态检测方法设计与实现*

姜香菊， 曾幼涵， 刘二林

(1.兰州交通大学 自动化与电气工程学院，甘肃 兰州 730070;2.兰州交通大学 机电工程学院，甘肃 兰州 730070)



四旋翼飞机姿态检测方法设计与实现*

姜香菊1， 曾幼涵1， 刘二林2

(1.兰州交通大学 自动化与电气工程学院，甘肃 兰州 730070;2.兰州交通大学 机电工程学院，甘肃 兰州 730070)

为了解决四旋翼飞机的姿态检测问题，设计了基于MPU6050和HCM5883L的九轴传感器，并提出了基于正弦算法的四旋翼飞机姿态角计算方法，该方法不需要判断即可由检测值直接获取角度大小及正负，较方向余弦算法具有更好的实用性。针对陀螺仪和加速度计的优缺点，设计了互补数据融合算法。仿真及实践均证明了检测方案的有效性。

四旋翼飞机; 姿态检测; MPU6050; 互补算法

0 引 言

四旋翼飞机是一种由4个螺旋桨对称安装而构成的飞行器。该飞行器主要有两种结构，一种为“+”字结构，一种为“X”结构。从Breguet-Richet发明世界上第一架四旋翼飞机至今，四旋翼飞机已经发展了一个多世纪。四旋翼飞机机械结构相对简单，可以通过对4个螺旋桨的控制来控制飞机的飞行姿态，因此四旋翼飞机可用于搜救、航拍、侦查以及军事对抗等各种场合[1～3]。四旋翼飞行器的姿态检测是飞行器控制的核心工作之一，很多研究人员都对该问题进行过深入的探讨[4]。于雅丽等人利用MMA7260和ENC—03M等对四旋翼飞机进行姿态检测[5]。汪绍华等人阐述了利用卡尔曼滤波算法解决四旋翼飞机的姿态检测问题的具体方案[8]。

本文从实际应用的角度出发，通过惯性坐标系和机体坐标系详细说明了所需检测的飞机姿态参数，利用正弦算法对数据进行处理；同时使用MPU6050和HMC5883进行了硬件电路设计，并对数据处理算法进行了分析说明。最后通过仿真及实际应用证明了所采用方法的正确性。

1 四旋翼飞机姿态检测参数分析

四旋翼飞机由检测装置，主控装置，电机驱动装置以及螺旋桨构成。本文中以“+”型四旋翼飞机为例进行说明。在进行控制理论研究时，一般要建立惯性坐标系和四旋翼飞机机体坐标系。如图1所示，坐标系OXYZ为惯性坐标系。OXYZ坐标系中，O为地面空间的任意一点，X轴平运行于水平面，方向指向北方；Y轴平行于水平面，方向指向东方；Z轴垂直于OXY平面，方向与重力方向相反。坐标系oxyz为机体坐标系，z轴垂直于飞机的旋翼平面且通过四旋翼飞机的重心，x轴与y轴则分别平行于两个对称的螺旋桨的旋转中心。可以用ξ=(x,y,z)∈R3表示飞机质心的位置矢量。可以用欧拉角η=(ψ,θ,φ)∈R3表示四旋翼飞机的飞行姿态。

图1 四旋翼飞机结构简图及受力分析

2 四旋翼飞机姿态检测的硬件设计与检测原理

2.1 四旋翼飞机姿态检测硬件设计

在四旋翼飞机的几何中心处，安装传感器MPU6050以及传感器HMC5883L。MPU—6050是6轴运动处理传感器。它集成了3轴MEMS陀螺仪，3轴MEMS加速度计，以及一个可扩展的数字运动处理器(digital motion processor，DMP)。MPU—6050对陀螺仪和加速度计分别用了3个16位的ADC，将其测量的模拟量转换为可输出的数字量。在MPU—6050上，通过其I2C接口连接非惯性数字传感器HMC5883L组建9轴传感器。HMC5883L 是表面贴装的高集成模块，并带有数字接口的弱磁传感器芯片，包括放大器、自动消磁驱动器、偏差校准、能使罗盘精度控制在1°～2°的12位模数转换器等。具体电路图见图2[4～6]。

图2 传感器硬件电路

2.2 角度检测基本原理

2.2.1 仰俯角和翻滚角检测基本原理

检测过程中6轴加速度传感器MPU6050和3轴地磁传感器HMC5883L均靠近四旋翼飞机中心，平贴于控制板安装，且HMC5883L的XYZ3轴与MPU6050的3轴相平行，2只传感器的距离尽量近。MPU6050的3轴与建立的机体坐标系的3轴相平行。假定四旋翼飞机匀速运动时，可以用方向余弦来计算角度θ与φ，具体原理见图3(图中oxyz坐标系为机体坐标系，MPU6050的3个轴与oxyz的3个轴平行，且MPU6050的中心与机体坐标系的中心重合)。

图3 MPU6050检测角度基本原理

如果MPU6050三个轴上检测到的加速度为gx，gy，gz，则有

(1)

由方向余弦公式可得

(2)

由于需要检测的数据为四旋翼飞机ox轴与水平面之间的夹角θ；oy轴与水平面之间的夹角φ，且在四旋翼飞机实际运行过程中-90°<θ<90°，-90°<φ<90°，所以，对式(2)进行处理且去掉∠gox项，可得

(3)

微控制器(MCU)编程时，可以根据各轴检测值的正负直接由式(3)计算出角度，式(3)计算的角度正负符合右手定则。在θ与φ较小时，可直接取θ=sinθ，φ=sinφ。显然由式(3)直接计算俯仰角和翻滚角较式(2)方便很多[4]。

2.2.2 偏航角检测基本原理

3 四旋翼飞机姿态检测软件设计

对于MPU6050与HMC5883L而言，2只传感器中的地磁传感器和加速度传感器的低频响应较好，但是高频响应特性较差。陀螺仪的高频与低频特性均较好，但是在实际使用过程中存在温漂和零漂，所以，必须通过数据的处理来获取精确的四旋翼飞机的姿态。考虑到在MCU中编程实现的方便性，可以用互补数据融合算法对数据进行处理。

(4)

陀螺仪动态特性好，但是存在漂移，因此，公式(4)计算出的俯仰角会出现积分误差。根据加速度计检测出的测量值，由式(3)可以计算出俯仰角，但是由于加速度传感器自身动态特性较差，同时四旋翼飞机自身为非刚体，在飞行过程中不可避免地会产生抖动，所以，式(3)计算出的俯仰角噪声较大，但是该值从整体上可以反映俯仰角大小。如果采用平均滤波等方法，则检测滞后，无法用于四旋翼飞机控制。互补数据融合算法的目的是利用式(3)计算出的噪声较大的俯仰角消除由式(4)得到的俯仰角的积分误差，具体原理见式(5)[7～9]。

(5)

该算法的数字实现方法为

(6)

4 仿真分析

为了对该互补算法的正确性进行分析，对式(5)进行拉式变换可得

(7)

利用Matlab进行仿真分析算法有效性，其Simulink仿真结果见图4、图5。图4中Out1模拟实际陀螺仪的输出。由仿真结果可以看出，由于陀螺仪的温漂，在四旋翼飞机的机体旋转角速度为零时，积分获取的角度值没有停留在实际角度处，而是会不断增加，所以，直接由角速度积分获取角度值不可用。在图5中，将有温漂的陀螺仪的值和有噪音的加速度值进行互补融合处理，处理后的结果消除了积分误差，且消除了加速度计获取信号的噪音。

图4 陀螺仪检测值直接积分仿真

图5 数据融合算法仿真

通过仿真结果进行对比可知，数据处理前，角度信号噪声较大；直接进行积分的信号因为积分误差逐渐偏离正常值。数据融合后，所得数据可以反映实际信号，效果较好。按照硬件设计方案和数据处理方法，结合传统PD算法进行飞机姿态控制，实践表明，该设计方案合理有效。验证效果见图6。

图6 飞机悬浮控制效果

5 结 论

姿态检测是四旋翼飞机控制过程中的关键问题。四旋翼飞机作为欠驱动系统，运行在三维空间。其基本姿态检测量包括俯仰角、翻滚角和偏航角。本文利用HMC5883L和MPU6050构成的9轴传感器完成了原始数据获取工作。由于通过方向余弦算出的俯仰角或者翻滚角等存在噪音，无法直接用于控制系统。文中设计了互补数据融合算法。仿真及实际运行均证明了该设计方案的合理性。相较于卡尔曼滤波，该方法容易理解，更容易在MCU中实现[10～12]。创新点在于设计了四旋翼飞机姿态检测方案，方案的硬件设计紧凑合理；采用了正弦算法计算俯仰角及翻滚角，方法可直接计算出对应的角度，且角度正负直接算出，符合右手定则，方法较方向余弦方便很多；设计互补数据融和算法，方法简单有效，且方便在MCU中实现。

[1] 刘晓杰.基于视觉的微小型旋翼飞机位姿估计研究与实现[D].长春：吉林大学,2009.

[2] Saad Sardar,Muhammad Bilal Kadri. Autonomous control of a quad-copter via fuzzy gain scheduled PD control[C]∥Proc IEEE 12th international Conference on Frontiers of Information Techno-logy,Computer Society,2014:73-78.

[3] 吴成富,刘小齐,马松辉,等.四旋翼无人机建模及其四元数空置率设计[J].飞行力学,2013,31(2):122-125.

[4] 李运堂,贾宇宁,王鹏峰,等.基于多传感器的四旋翼飞行器硬件系统设计[J].传感器与微系统,2015,34(2):119-142.

[5] 于雅莉,孙 枫,王元昔.基于多传感器的四旋翼飞行器硬件电路设计[J].传感器与微系统,2011,30(8):113-115,123.

[6] 刘春阳,徐军领,程洪涛,等.MPU9250传感器的姿态检测与数据融合[J].河南科技大学学报:自然科学版,2015,36(4):14-17.

[7] 姜香菊,刘二林.两轮自平衡机器人角度检测数据融合算法[J].计算机工程与应用,2013,49(8):203-205.

[8] 汪绍华,杨 莹.基于卡曼滤波的四旋翼飞行器姿态估算和控制算法研究[J].控制理论与应用,2013,30(9):1109-1115.

[9] 万晓凤,康利平，余运俊,等.互补滤波算法在四旋翼飞行器姿态解算中的应用[J].测控技术,2015,34(2):8-211.

[10] 杨凌霄,李晓阳.基于卡尔曼滤波的两轮自平衡车姿态检测方法[J].计算机仿真,2014,31(6):406-409.

[11] 刘二林,姜香菊.基于卡尔曼滤波的两轮自平衡车姿态检测[J].自动化与仪器仪表,2015(2):52-54.

[12] 叶锃锋，冯恩信.基于四元数和卡尔曼滤波的两轮车姿态稳定方法[J].传感技术学报,2012,25(4):524-528.

姜香菊(1979-)，副教授，硕士生导师，主要从事计算机控制和检测技术的研究工作。

Design and realization of quad-rotor airplane attitude detection method*

JIANG Xiang-ju1, ZENG You-han1， LIU Er-lin2

(1.School of Automatic & Electrical Engineering,Lanzhou Jiaotong University,Lanzhou 730070,China;2.School of Mechatronic Engineering,Lanzhou Jiaotong University,Lanzhou 730070,China)

In order to solve the problem of quad-rotor airplane attitude detection,a nine-axis sensor are designed based on MPU6050 and HCM5883L.An aircraft body attitude angle calculation method which is based on sine algorithm are proposed.The calculation method based on sine is better than based on cosine.It can gain angle and positive or negative.According to advantages and disadvantages of gyroscope and accelerometer,a complementary data fusion algorithm is designed.The validity of the scheme is proved by the simulation and practice.

quad-rotor airplane; attitude detection; MPU6050; complementary algorithm

10.13873/J.1000—9787(2017)07—0079—04

2016—07—04

甘肃省高等学校科研项目(2014A—041)；校青年基金资助项目(2014034)

TP274

A

1000—9787(2017)07—0079—04