基于MPU9250的无人机姿态信息采集及处理

何松, 陈兴武

(1.福建省数字化装备重点实验室，福建 福州 350118;2.福建工程学院 信息科学与工程学院，福建 福州 350118)

基于MPU9250的无人机姿态信息采集及处理

何松1,2, 陈兴武1,2

(1.福建省数字化装备重点实验室，福建 福州 350118;2.福建工程学院 信息科学与工程学院，福建 福州 350118)

研究MPU9250多轴姿态传感器在无人机上的姿态信息采集，通过对九轴传感器内的陀螺仪，重力加速度计，磁力计的数据接收后，结合卡尔曼滤波算法对其数据进行精准的解算；搭建了一个无人机姿态模拟的物理实验平台，进行无人机运动状态下的信息采集；最后，在MATLAB的数据仿真下测试其性能。实验结果表明该方法可使无人机信息采集的精准度提升一倍。

姿态控制； 信息处理； 卡尔曼滤波； 无人机

无人机近年来的迅速发展，并且广泛应用于商业，农业和军事等领域，而无人机的姿态信息对于实现精确运动控制至关重要。传统的机械陀螺仪和光纤陀螺仪等传感器价格昂贵，体积较大，不适合于无人机的发展[1]。因此，使用微机电系统(MEMS)技术制造的传感器进行姿态检测并广泛应用于无人机的控制系统中。通过多个传感器的姿态检测，并将其数据融合处理是当今的研究热点[2]。

使用功能强大体积小巧的九自由度惯性器件MPU9250进行姿态检测，用卡尔曼滤波对其进行预处理，采集得到实用的姿态数据，结合FPGA控制系统，最终实现无人机姿态信息的精确性。

1 系统总体设计

无人机姿态信息采集的系统是基于MAX10 FPGA为核心的控制平台，通过姿态传感器MPU9250对运动姿态进行检测，得到其航向角、俯仰角、翻滚角等姿态信息。采用卡尔曼滤波先进行数据整合，经处理后将数据传送给FPGA控制平台，对其实行姿态信息采集。系统结构如图1。无人机的姿态信息采集应尽可能的稳定、精度高、抗干扰能力强，对惯性导航器件要求应具备较高的灵敏度、较快的处理速度等特点，因此采用以MPU9250九自由度惯性传感器的主传感器，代替以前用的六轴惯性传感器MPU6000、三轴加速度传感器LSM303D、三轴陀螺仪L3GD20等，读取信号时每个传感器采集的数据都有一个相应的权值，通过联合卡尔曼滤波计算得到最终值。

图1 系统结构图Fig.1 System structure diagram of UAVs

1.1 MAX 10 FPGA 芯片特性分析

无人机姿态信息采集系统用到的MAX 10 FPGA不仅继承FPGA器件的特性，而且逻辑资源密度大幅提升，更为重要的是 MAX 10还加入了之前的一些拓展设备，如DSP模块、DDR3存储控制器等。其性能大幅提升，可以说它是一种非易失性的FPGA。MAX 10继承了普通FPGA器件的特性：

(1)采用FPGA设计ASIC电路(专用集成电路)，用户不需要投片生产，就能得到适用的芯片。

(2)采用了逻辑单元阵列的概念，可重复擦除写入，可以在不改变硬件电路的基础上实现多种不同电路功能。

(3)FPGA内部有丰富的触发器和I/O引脚。

(4)FPGA设计开发周期短、开发费用低，集成度高，可靠性好。

(5) FPGA功耗低，可以与CMOS、TTL电平兼容。

MAX 10的工作状态是由存放在片内RAM的程序来设置的。因此，使用之前需要对片内的RAM进行编程。可以根据不同的配置模式，采用不同的编程方式。配置完成后，FPGA进入工作状态。掉电后，FPGA恢复成白片，内部逻辑关系消失，因此，FPGA能够反复使用。FPGA的编程无需专用的FPGA编程器，只需通用的EPROM、PROM编程器即可。当需要修改FPGA功能时，只需换一片EPROM即可。这样，同一片FPGA，不同的编程数据，可以产生不同的电路功能。因此，FPGA的使用非常灵活。

1.2 MPU9250的特性

MPU9250为全球首例整合9轴运动姿态检测的数字传感器，其相比上一代产品，体积小巧，并且功耗低。

MPU9250内部有3轴陀螺仪、3轴重力加速度计、3轴磁力计，都是以16位ADC信号输出；有集成电路总线(IIC)接口和串行外设接口(SPI)2种模式接口，传输速率分别可达400 kHz/s(IIC)和20 MHz/s(SPI)。其陀螺仪的角速度测量范围最高可达± 2 000 °/s，具有良好的动态响应特性。重力加速度的最大测量范围为± 16g(g为重力加速度)，磁力计可达±4 800 μT，可用于对偏航角的辅助测量。

2 惯性器件硬件设计

2.1 传感器模块设计

传感器均采用SPI与FPGA相连，其中MPU9250内的AK8963电子罗盘采用IIC读取。由于SPI通信传输的高效率，系统运算速度也会更快，其硬件电路如图2。

图2 MPU9250惯性传感器接线图Fig.2 Circuit diagram of MPU9250 inertial sensor

2.2 FPGA电路设计

FPGA的原理图和PCB见图3和图4，其采用Altera公司高性能的FPGA,具体型号为10M08SCU169，具有对外接口丰富，实时性好等优点。本设计只用了其中部分接口，因此如若有需要增添的器件也可及时加入，满足大众需求。对各种传感器模块的数据采集都适用，所以选择10M08SCU169作为整个系统的处理芯片。

图3 FPGA部分接线图Fig.3 FPGA partial circuit diagram

图4 FPGA电路PCB 图Fig.4 FPGA circuit PCB

3 数据接收及处理

3.1 数据采集

本设计用到多个传感器都是数字传感器，故而FPGA可直接读取传感器的数据。以九轴惯性传感器MPU9250等传感器与主控芯片FPGA之间采取SPI通信进行数据采集。

首先对几个传感器进行初始化，采取SPI通信协议，对各个传感器数据读写函数进行初始化。接着确定传输地址，进行数据传输，待数据传输完毕再进行数据处理。以MPU9250为例，其程序流程图如图5所示。

图5 MPU9250传感器数据采集流程图Fig.5 MPU9250 sensor data acquisition flowchart

3.2 数据处理

3.2.1 卡尔曼滤波

卡尔曼滤波是1960年由R.E.Kalman提出的著名算法，它有两种很好的步骤：预估和纠正。卡尔曼滤波在很多领域都得到了较好的应用，文中所涉及的多传感器也应用到了卡尔曼滤波。卡尔曼滤波以系统测量的数据作为其输入，以系统状态的估计值作为输出，达到优化系统噪声和观测噪声的方法。

导航坐标系相对于载体坐标系之间的变换关系如式(1)所示。

(1)

之前的重力加速度在无人机本体的X、Y、Z轴上的分量xg、yg、zg，根据式(1)所示的导航坐标系n相对于载体坐标系b的方向余弦变换关系，有

把公式(1)代入公式(2)可得

求解式(3)，可得出无人机本体的俯仰角θ和横滚角φ。

之后用姿态矩阵和四元数法得出其对应的关系，采用卡尔曼滤波器计算状态x的最优估计。

采用文献[3]中卡尔曼滤波器,公式如式(4)。

其中：xk为状态向量，yk为观测向量，A为从k-1时刻到k时刻的状态转移矩阵，Uk为系统输入控制向量，B为输入控制向量的增益矩阵，H为状态量到观测量的增益矩阵，ωk为输入噪声，νk为测量噪声。

假设其噪声都服从正态分布，输入噪声的协方差为Q, 测量噪声的协方差为R, 则其5个核心公式如下。

状态量的预估计：

误差协方差预估计：

卡尔曼增益：

现状态估计：

误差协方差：

卡尔曼滤波中的噪声会影响其响应速度和效果，因此各项系数的初始值要设稳定。在软件处理卡尔曼滤波时尽量保证各方系数平衡。

3.2.2 角度的计算

MPU9250传感器传输回来的数据分别代表其3个轴测量到的角速度、加速度、磁感应强度，需要将其解算成姿态角度信息θ、φ、ψ(分别是无人机本体的俯仰角、横滚角以及航向角)。其中俯仰角是X轴与地面的角度，横滚角是Y轴与地面的角度，航向角是其绕Z轴旋转的角度。

通过积分就可以得到对应的角度[7]，计算公式如下：

其中：θk为前一时刻的角度值，θk-1为当前陀螺仪的角速度，ωk为陀螺仪当前时刻角速度的偏移量，ωbias_k为当前时刻角速度的偏移量，dt为积分时间(即采样周期)。

重力加速度计的测量可利用三角函数关系解算角度姿态[8]：

其中：Ax、Ay、Az分别是X、Y、Z这3个轴的加速度分量；γ为Z轴与重力加速度的夹角,ρ为俯仰角,φ为翻滚角。

磁力计测量的是磁感应强度，其在水平位置且无外加磁场干扰时，航向角可以通过三角函数关系计算[8]：

其中，Lx和Ly分别是X轴和Y轴输出的磁感应强度数据。

当磁力计不在水平位置的时候，则可通过倾斜补偿的方法以降低其角度测量的误差。倾斜补偿公式为：

其中：Mx、My、Mz分别为磁力计3个轴的输出数据；θ和φ分别为加速度计检测的俯仰角和横滚角。

通过补偿后的磁感应强度和式(13)可以计算出补偿后的航向角。

4 实验与分析

由于无人机在飞行过程中可能受到外界干扰，影响其正常轨迹，而在该系统的分析处理下，会对其姿态和角速度进行纠正，使其回归正常轨道，用MATLAB对此系统进行仿真，对其角速度，姿态及控制力矩进行分析。再加入MPU9250的无人机控制系统后，该系统对角度的调节更加稳定，对系统的调节响应更加快速。

本实验是建立在自制的模拟平台上(见图6)实现的，数据滤波则是在MATLAB中进行的，由系统检测接收到的数据记录下来，在利用PC机上的MATLAB进行数据处理,实现对姿态角的卡尔曼滤波。

图6 飞行模拟转台Fig.6 Flight simulation turntable

在MATLAB中对其进行仿真，分别模拟无人机在静止和运动状态下的情况，不考虑风的干扰，对其姿态信息进行采集。

图7是在静止位置测的俯仰角，不考虑最后取件时的误差。可以看出系统测量与实际测量误差最大达±0.04°，而经滤波处理后它们之间的最大误差为±0.025°。可见精度得到了一定的提升。

图7 在静止位置的俯仰角Fig.7 Pitching angle at rest position

图8是转台旋转过程中测的俯仰角，其在旋转过程中可能会有部分抖动，但对数据精准分析无多大影响，可以看到系统测量最大误差在±3(°)，经滤波处理后，其最大误差在±1.2(°)，可见精度得到了较大的提高。

图8 在运动状态的俯仰角Fig.8 Pitching angle of motion

图9是在静止位置的偏航角，其系统测量与实际测量的最大偏差在±0.03°，经滤波处理后在±0.02°。

图10是在动态下的偏航角，由图10可看出系统测量偏差在±3.5°，滤波偏差在±1.5°。精度得到了显著提高。

经测试该系统能对无人机的俯仰、翻滚和偏航的数据读取，实现了对采集到的数据进行一定的处理，表明本系统采集信息的精准性，可以满足对无人机姿态数据采集的需求。

图9 在静止位置的偏航角Fig.9 Yaw angle at rest position

图10 在运动状态的偏航角Fig.10 Yaw angle of motion

5 结论

无人机的姿态采集是其整个控制系统的关键技术之一。本文研究了基于MPU9250的无人机姿态信息采集，并结合了卡尔曼滤波对其数据进行处理，在自制的姿态模拟平台上对其进行测试，验证了该方法可使其精准度最高提升一倍左右，并且该系统还具有多接口，实时性好，具有良好的应用前景。

[1]秦勇，臧希喆，王晓宇，等．基于MEMS惯性传感器的机器人姿态监测系统的研究[J]．传感技术学报，2007，20(2):298-301．

[2]周兆英，叶雄英，李勇，等.微型系统和微型制造技术[J].微米纳米科学与技术，1996,2(1):1-11.

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

[4]沙占友，王彦朋，张永昌.单片偏航角速度陀螺仪的原理与应用[J],传感器世界,2004(9):31-34.

[5]Marins J L, Yun X P，Bachmann E R，et al. An extended Kalman filter for quaternion-based orientation estimation using MARG sensors[C]∥Proc of the 2001 IEEE /RSJ International Conference on Intelligent Robots and Systems,Maui, Ha-waii, USA, Oct.29-Nov.3,2001. Monterey, CA: Naval Postgraduale School，2001:2003-2011.

[6]毕盛，闵华清，李淳，等．姿态传感器采集测试系统的设计与实现[J]．计算机测量与控制，2011，19(7):1562-1564．

[7]李伟，何鹏举，高社生．多传感器加权信息融合算法研究[J]．西北工业大学学报，2010，28(5):674-678．

[8]Kimberly T．Tilt sensing using linear accelerometers[J]．Free Scale Semiconductor Application Note，2007(6):1-7．

(特约编辑：黄家瑜)

MPU9250-based UAV attitude information acquisition and processing

He Song1,2，Chen Xingwu1,2

(1.Fujian Provincial Key Laboratory of Digital Equipment, Fuzhou 350118, China;2. College of Information Science and Engineering, Fujian University of Technology, Fuzhou 350118, China)

The attitude information acquisition of MPU9250-based UAV was researched via nine axis gyroscope sensor, gravity accelerometer and magnetometer. Kalman filtering algorithm was employed to obtain the accurate data. A physical experimental platform was constructed for UAV attitude simulated information acquisition of UAV motion. The performance data of the UAV altitude information acquisition was tested in MATLAB simulation. The experimental results show that the method can improve the precision of the UAV information acquisition by about two times.

attitude control; information processing; Kalman filter; unmanned vehicle(UAV)

2016-10-08

国家科技支撑计划项目(2015BAF24B01);福建省科技计划项目(2014H6006)

何松(1994- )，男，湖北鄂州人，硕士研究生，研究方向：无人机飞控，电气控制。

10.3969/j.issn.1672-4348.2016.06.015

V249.1

A

1672-4348(2016)06-0587-06