基于MPU9150的四轴飞行器姿态控制系统设计

朱 阳，王伟成，王民慧

（贵州大学 电气工程学院，贵阳 550025）

0 引言

我国地域广大，自然条件复杂，是世界上自然灾害最为严重的国家之一。地震等自然灾害不仅给救灾带来不利，也进一步增加了施救的难度，加大了施救者受伤的危险程度。虽然自然灾害的发生无法阻止，但是减少灾害带来的二次伤害是可以实现的[1]，例如可以开发微型无人驾驶飞行器，对上述不可达的危险地域进行探测和搜寻即可减少伤害的发生，尤其是在地势险要或者人员无法到达的地区，能够准确地把侦查到的信息传送回来。而四轴飞行器能以灵活性、多功能性以及高效的处理能力来胜任复杂环境中的信息获取的角色。同时，与传统的搜救直升机相比，四轴飞行器以其简单的机械结构、更小的螺旋桨、优良的飞行安全性能和先进控制算法的应用等优势具有更广泛的前景。

本文基于STM32F103C8T6微处理器，采用MPU9150惯性测量模块，设计了四轴飞行器姿态控制系统。STM32系列处理器是ST公司推出的一款基于高性能、低成本、低功耗、外设丰富的嵌入式处理器，应用专门设计的低功耗高速内核丰富的片上资源可满足各类传感器通讯需求，与传统的飞行控制器相比可大大降低系统的开发成本、节约资源。MPU9150系统集成度高，包含了三轴加速度计、三轴陀螺仪和三轴磁力计，电路简单可靠，测量范围广，准确度高，动态响应快，体积小、功耗低，能够快速的融合陀螺仪和加速度计数据得出姿态四元数，适合用于四轴飞行器的姿态控制系统。

1 四轴飞行器结构与原理

四轴飞行器的基本结构如图1所示，飞行器在空中运动中，为了使整个机体转矩平衡，采用正反桨设计，即对角线的两组桨相同，相邻的两个桨相反，分为顺时针旋转（1、3）和逆时针旋转（2、4），这样正常飞行时两个桨正转两个桨反转，转矩抵消，避免飞行器打转[2]，当然，旋转时需加大两个正浆或两个反浆来改变总的转矩，从而改变偏航角，控制对角线上的一组桨的转速不同，使机体倾斜一个角度产生水平分力推动飞行器平移，飞行速度可以由俯仰角的大小与电机的转速来控制，最终实现飞行器的偏航运动、上下飞行与前后飞行。

图1 四轴飞行器的基本结构图

2 姿态控制系统功能

四轴飞行器有6个自由度，而只有4个控制输入，因此，四轴飞行器的控制问题属于欠驱动控制问题，具有不稳定和强耦合等特点。除了受自身机械结构和旋翼空气动力学影响外，还很容易受到外界干扰。其中姿态控制是四轴飞行器研究的重点，姿态传感器数据的采集并对数据进行融合处理都是姿态控制的关键，要求飞行器具有快速响应能力，及时进行自适应调整，以确保飞行器姿态稳定。

本文主要研究对象是四轴飞行器的姿态，根据姿态控制子系统的数学模型[3]，有两个基本坐标系：“地理”坐标系和“载体”坐标系。地理坐标系即当地水平坐标系，也称NED坐标系，而“载体”坐标系指的是四轴自己的坐标系。姿态的数据来源有五个：重力、地磁、陀螺仪、加速度计和电子罗盘。其中前两个来自“地理”坐标系，后三个来自“载体”坐标系。在“地理”坐标系中，重力的值始终是（0,0,1g），地磁的值始终是（0,1,x），这值就是由放置在四轴上的传感器测量出来的。姿态控制系统需要检测的状态有：飞行器在机体坐标系下3个轴向的角速度、角度和相对地面的高度。机体坐标系如图2所示，该坐标系固定在机体上，原点在飞行器重心，轴OX与前后螺旋桨连线平衡，前方指向X轴正方向；轴OY与左右螺旋桨连线平衡，右方指向Y轴正方向；轴OZ与轴OY、OX所在平面垂直，并与轴OY、轴OX组成右手坐标系。

图2 机体坐标系

飞行器运动过程可以分解成机体绕三个轴运动来描述：当飞行器升降运动时，飞行器即做俯仰运动，飞行器在X-Z平面进行绕Y轴运动，产生的角度即俯仰角(pitch)；当飞行器发生转向运动时，飞行器将产生偏航，飞行器在X-Y平面进行绕Z轴运动，产生的角度即航向角(yaw)；当让飞行器侧身移动时，飞行器将要做横滚运动，飞行器在Y-Z平面进行绕X轴转动，产生的角度即航滚角(roll)。

整个姿态控制系统担负着传感器信息采集、数据融合及姿态解算等各种任务，其主要工作过程是主控制器能快速获得各传感器的数据，实时检测无人机的状态，包括姿态、位置、速度等信息，并对数据进行处理；在计算出自身姿态之后，飞行器需要控制电机来执行相应的姿态调整，采用PID控制器来获得电机控制量，以PWM的方式输出驱动电机，以实现对其姿态的控制。

3 硬件电路设计

3.1 MPU9150性能分析

随着微机械加工技术和微电子技术的不断发展与相互融合，各种基于MEMS(微机电系统)技术的元器件应运而生，它们具有功耗低、可靠性、工作效率高及成本低等特点，易于实现智能化、数字化及批量化。MPU9150芯片就是一种MEMS传感器芯片，其具有MEMS的抗冲击能力强、系统集成度高、体积小、成本低、功耗低，性能优良的特点。MPU9150是全球第一款九轴惯性传感芯片，包含了三轴加速度计、三轴陀螺仪和三轴磁力计，其中加速度和陀螺仪的精度为16bit，磁力计的精度为13bit，保证了测量的精确度，其加速度计的量程有±2g，±4g，±8g，±16g可选，陀螺仪的量程为±250，±500，±1000，和±2000O/sec可选，磁力计的满量程为±1200uT，并且内置DMP用于姿态融合[4]。使MPU9150在无人机飞行控制系统中使用非常方便。MPU9150引脚功能描述如表1所示，典型接线图如图3所示。

表1 MPU9150引脚描述

图3 MPU9150典型接线图

3.2 硬件电路结构

为了实现飞行器姿态控制，需要得到飞行器的俯仰角和滚转角作为姿态反馈，形成闭环控制，为提高其飞行稳定性，需加入角速率反馈以增加阻尼，飞行器的飞行姿态通过惯性测量单元来获取，系统采用整合了3轴陀螺仪、3轴加速计、3轴磁力计的9轴运动处理组件MPU-9150，消除了组合陀螺仪与加速计时存在的轴差问题；并引入磁力传感器互补数据，可采用四元数来描述姿态，可以避免欧拉角的奇异问题；另外陀螺仪具备增强偏置和温度稳定的功能，减少了用户校正操作，且具备改进的低频噪声性能；加速计则具备可编程中断和自由降落中断的功能。MPU-9150姿态检测电路图如图4所示。

图4 MPU-9150姿态检测电路

4 姿态解算与数据融合分析

4.1 姿态解算

姿态解算部分是飞行器控制系统的核心，主要负责读取姿态传感器数据，从中解算出飞行器姿态，并将其作为姿态反馈，形成闭环控制，将期待的姿态与当前姿态的数据差值提供给PID控制器，从而计算得到电机的控制量。

姿态解算主要用到陀螺仪、加速度计和磁力计三个传感器的数据。由于陀螺仪输出的角速度瞬时数据积分后存在较大累积误差，且积分角度偏差随着时间增大而增大；而加速度计对绕重力加速度轴的转动无法感知，因此长时间的自旋运动将无法估计，这就需要引入磁力计来弥补这一不足。因此，用加速度计对陀螺仪进行不断的校正，引入磁力计互补数据，融合三者的数据才能更准确的反应出当前飞行器的姿态[5]。

目前常用的姿态解算方法主要有梯度下降法，互补滤波法，卡尔曼融合DMP输出和磁场数据。考虑到MPU9150传感器有一个内置的DMP，能够快速融合陀螺仪和加速度计的输出，而且输出的姿态数据很稳定，动态性能很好，虽然DMP中算法没有融合磁场数据，导致航向角在使用一段时间后会不可逆转的偏离正确位置，但可以采用卡尔曼滤波的方法，对两个航向角进行滤波，同时将磁场数据融合进去，以纠正航向角的偏差，从而既保证了其动态性能，又保证长时间运行航向角不会出现偏差。因此这里采用卡尔曼融合DMP输出和磁场数据的飞行姿态解算方法。算法流程图如图5所示。

图5 卡尔曼融合DMP和磁场数据流程图

4.2 数据融合分析

4.2.1 静态效果分析

实验方法为先将MPU9150模块静止放置20s，然后再对其连续采样12s，采样频率为50Hz。数据处理方法是，对每个轴输出的欧拉角数据求平均值。

然后再求方差：

通过静止时方差的大小来分析其静态性能。卡尔曼融合DMP和磁场数据算法的核心在于确定卡尔曼滤波中的系统自协方差Q和测量值的自协方差R两个系数。不同Q值和R值的静态航向角方差结果如表2所示。

表2 卡尔曼融合DMP输出和磁场数据在静态时航向角方差。

图6 卡尔曼融合DMP输出和磁场数据在静态时航向角方差

从上图中可以明显看出航向角的静态方差随着R的增大而变小，随着Q的变小而变小，且Q越大方差随R变化效果越明显。最后列出Q=0.00001，R=0.9静态效果如图7所示。

图7 卡尔曼融合DMP输出和磁场数据静态效果

从静态效果图可以看出，卡尔曼融合DMP和磁场数据的方法中航向角的误差最小可达到0.00113。表明在保证纠正效果--即姿态角不漂移的前提下卡尔曼融合DMP输出和磁场数据的算法静态效果理想。

4.2.2 动态效果分析

在确保静态性能良好的条件下，采用一个将航向角快速旋转180°的方法来验证卡尔曼融合DMP输出和磁场数据算法的动态性能。结果如图8所示。

由图8可以看出卡尔曼融合DMP和磁场数据的算法能很好的反应真实的旋转过程。

综合静态和动态性能分析，卡尔曼融合DMP和磁场数据的算法不仅在静态性能上表现很好，而且在动态性能方面也表现良好。可以得到稳定的姿态数据，实现良好的姿态控制，从而使飞行器达到稳定飞行的效果。

5 结束语

图8 卡尔曼融合DMP和磁场数据(Q=0.00001，R=0.1)

四轴飞行器是一种特殊结构的飞行器，其在民用及军事方面都有着广阔的应用前景[6]。本文根据四轴飞行器姿态控制系统的数学模型，对姿态控制系统的功能要求进行了分析，以STM32为主控制器，采用MUP-9150芯片进行姿态控制，并采用卡尔曼融合DMP和磁场数据的算法进行姿态解算，实现了飞行器良好的姿态控制。整个姿态控制系统集成度高，精度高，在测量精度、动态性能以及硬件电路方面均能满足要求，为飞行器姿态控制系统的设计提供了一种高效稳定的实现方法[6]。实验结果表明，系统满足四轴飞行器飞行姿态控制的要求。

[1] 张鹏.一种新型四轴搜救飞行器设计[J].科技广场.2010(9):145-146.

[2] 郭晓鸿.一种基于STM32的四旋翼飞行器[J].应用科技.2011.38(7).36-37.

[3] Pau Segui-Gasco. A Novel Actuation Concept for a Multi Rotor UAV[A].2013 International Conference on Unmanned Aircraft Systems (ICUAS)[C].2013, Atlanta, GA.373-376.

[4] InvenSense Inc.MPU9150 Product Speci fi cation Revision 4.0[S].InvenSense Inc,2012.

[5] 秦永元.惯性导航[M].科学出版社,2006:287-327.

[6] 黄成功.基于MPX4115的小型无人机气压高度测量系统设计[J].宇航计测技术.2009(8).31-32.