DMP在矿山无人机航摄系统姿态解算中的应用研究

（陇东学院 电气工程学院，甘肃 庆阳 745000）

随着控制技术的高速发展，矿山无人机的应用已逐渐接近生活。目前，民用级矿山无人机的飞行控制单元，多采用基于ARM结构的高性能微控制器进行设计，其开发难度大、设计成本高。对矿山无人机的姿态解算都通过卡尔曼滤波算法进行数据融合，其算法复杂、且消耗处理器运算时间。

设计提出一种使用普通单片机，利用运动处理器MPU6050内部的DMP（数字运动处理）单元实现对矿山无人机姿态角的解算，整个姿态解算过程由硬件完成，除了能够减轻处理器的运算负荷，提高系统的整体响应速度，同时，也能够降低飞行控制板对处理器的要求。

1 矿山无人机系统硬件设计

图1为飞行器的硬件结构框图，其硬件系统由飞行控制模块、动力输出模块、无线通信模块和供电模块四大模块构成，各模块只要功能如图1。

（1）姿态监测模块：是飞行器的核心灵魂,用于数据处理和协调控制其它模块间的相互动作。

（2）动力输出模块：是飞行器的执行机构，用于为飞行器提供可靠的动力保障。

（3）无线通信模块：是飞行器的人机交互接口，用于接收操控平台的飞行指令同时将飞行器的当前姿态及控制参数传递给操控平台。

（4）供电模块：是飞行器的电力保障单元，除负责为动力输出模块提供电力支持外，还要为飞行控制模块及无线通信模块工作提供所需的电能。

图1 矿山无人机硬件的系统结构图

2 姿态解算方法

2.1 卡尔曼滤波算法

卡尔曼滤波是解决以均方误差最小为准则的最佳线性滤波问题，它根据前一个估计值和最近一个观察数据来估计信号的当前值。它是用状态方程和递推方法进行估计的，而它的解是以估计值（常常是状态变量的估计值）的形式给出其信号模型是从状态方程和量测方程得到的[1]。

由文献[2]～[4]结论可得，将矿山无人机看作一个线性系统，则通过陀螺仪的输出数据可以建立矿山无人机的姿态线性模型：

通过公式（1）、（2）可以实现对矿山无人机姿态角的预测，其算法流程如图所示：

图2 卡尔曼滤波算法流程图

2.2 DMP姿态解算

MPU6050是一款六轴运动处理器，除能够输出三轴陀螺仪数据和三轴加速度数据外，内部还具有专用的DMP（数字运动处理）单元，能够将六轴数据经滤波融合生成四元数，使用DMP单元能够减轻系统处理器的运算负荷，提高系统的整体响应速度[5,6]。

配置DMP单元完成矿山无人机姿态解算的步骤为：

（1）下载mpu6050运动处理器库；

（2）配置微处理器I2C总线为块读写，总线速率为400Kbit/s；

（3）通过I2C总线配置mpu6050的工作参数；

（4）通过I2C总线将mpu6050固件烧写在mpu6050的内部RAM中；

（5）配置DMP单元的采样频率；

（6）配置DMP单元工作参数；

（7）配置mpu6050的FIFO；

（8）完成六轴测量数据校准；

（9）启动数据融合滤波处理；

（10）等待转换数据输出；

（11）读取输出数据。

DMP输出的姿态解算结果为四元数，为长整型数据，转换为浮点型数据后，经公式（3）转换为欧拉角。

q1、q2、q3、q4：DMP输出四元数除以232；roll、pitch、yaw：横滚角、俯仰角、航向角。

其软件设计流程图如图3所示。

图3 DMP单元配置流程图

表1 测量数据对比

经过DMP单元处理，处理器可直接从MPU6050的数据缓存中读出四元数及校正后的陀螺仪数据、加速度数据。通过公式，可将获取的四元数转化为欧拉角，得到飞行器的当前姿态。

3 实验对比

实验平台采用Freescale公司的MC9S12系列单片机作为主控单元，其工作主频经PLL倍频后可达48MHz，与MPU6050通过I2C总线进行数据交换，I2C总线通信速率为400Kbit/s，完成矿山无人机的姿态解算，并将计算出的姿态角通过串口发送到计算机，上位机将接收到的姿态角数据以描点法绘图，串口通信速率为115200bit/s。

3.1 采用卡尔曼滤波姿态解算测试

图4为采用卡尔曼滤波算法完成矿山无人机的姿态解算，上位机所显示的姿态角波形。

图4 卡尔曼滤波输出姿态角曲线图

由图4可以看出，红色曲线代表输出的航向角Yaw、蓝色曲线代表飞行器的俯仰角Pith、灰色曲线代表飞行器的横滚角Roll。采用卡尔曼滤波算法输出的姿态角在飞行器静止时，Roll、Pith输出曲线有波动，当飞行器俯仰角、滚动角发生改变时，其航向角波动较大，因此，对航向角的解算一般需通过电子罗盘的测量数据对卡尔曼滤波输出数据进行校正。

且在姿态发生变化时，Roll、Pith、Yaw输出曲线斜率较小，表明其响应速度慢，跟随性差。为提高卡尔曼滤波的响应速度和跟随性，就必须使用具有独立的浮点运算单元的高性能微处理器来完成姿态解算。

3.2 DMP姿态解算测试

图5采用DMP完成姿态解算，上位机显示的姿态角波形。并将计算出的姿态角。

图5 DMP输出姿态角曲线图

图5输出曲线含义与上图一致。图5采用DMP输出的姿态角在飞行器静止时，输出曲线近似直线，当飞行器运动状态改变时，输出曲线变化平稳、无抖动、却斜率较大。表明DMP姿态解算响应速度快，跟随性好。

3.3 实验数据对比

表1为采用卡尔曼滤波和DMP单元完成矿山无人机姿态解算，综合指标对比。

4 结语

相比目前主流的矿山无人机飞行控制器设计方案，使用DMP在以下两方面做出了改进。

利用MPU6050内部强大的运算处理能力完成姿态角的融合、滤波，输出姿态角稳定。

利用DMP单元完成矿山无人机姿态的解算过程，有效降低了处理器的负荷，增强了矿山无人机的响应速度，同时降低了对硬件资源的要求。

经测试，采用上述方法改进后，利用主频为50MHz的普通单片机就能完成对矿山无人机姿态角的准确测量，且系统姿态角更新频率可达200Hz，响应速率快，飞行稳定性强。

[1]王京卫.测绘无人机低空数字航摄影像去雾霾研究[J].测绘学报,2016,45(2).

[2]李永生,赖华荣,卢春盛,等.无人机摄影测量技术在矿产开采执法监察中的应用[J].测绘通报, 2017(s1):95-99.

[3]冯少江,徐泽宇,石明全,等.基于改进扩展卡尔曼滤波的姿态解算算法研究[J].计算机科学,2017,44(9):227-229.

[4]马玉清,吴房胜.PID算法在无人机智能控制系统中的应用[J].湖南城市学院学报(自然科学版),2016, 25(2):119-120.

[5]叶龙.基于MPU6050传感器方位角倾角算法研究[J].吉林大学,2015,5:23-24.

[6]傅忠云,朱海霞.基于惯性传感器MPU6050的滤波算法研究[J].电压与声光,2015,5(8):21-25.