DMP在礦山無人機航攝系統(tǒng)姿態(tài)解算中的應(yīng)用研究

（隴東學(xué)院 電氣工程學(xué)院，甘肅 慶陽 745000）

隨著控制技術(shù)的高速發(fā)展，礦山無人機的應(yīng)用已逐漸接近生活。目前，民用級礦山無人機的飛行控制單元，多采用基于ARM結(jié)構(gòu)的高性能微控制器進(jìn)行設(shè)計，其開發(fā)難度大、設(shè)計成本高。對礦山無人機的姿態(tài)解算都通過卡爾曼濾波算法進(jìn)行數(shù)據(jù)融合，其算法復(fù)雜、且消耗處理器運算時間。

設(shè)計提出一種使用普通單片機，利用運動處理器MPU6050內(nèi)部的DMP（數(shù)字運動處理）單元實現(xiàn)對礦山無人機姿態(tài)角的解算，整個姿態(tài)解算過程由硬件完成，除了能夠減輕處理器的運算負(fù)荷，提高系統(tǒng)的整體響應(yīng)速度，同時，也能夠降低飛行控制板對處理器的要求。

1 礦山無人機系統(tǒng)硬件設(shè)計

圖1為飛行器的硬件結(jié)構(gòu)框圖，其硬件系統(tǒng)由飛行控制模塊、動力輸出模塊、無線通信模塊和供電模塊四大模塊構(gòu)成，各模塊只要功能如圖1。

（1）姿態(tài)監(jiān)測模塊：是飛行器的核心靈魂,用于數(shù)據(jù)處理和協(xié)調(diào)控制其它模塊間的相互動作。

（2）動力輸出模塊：是飛行器的執(zhí)行機構(gòu)，用于為飛行器提供可靠的動力保障。

（3）無線通信模塊：是飛行器的人機交互接口，用于接收操控平臺的飛行指令同時將飛行器的當(dāng)前姿態(tài)及控制參數(shù)傳遞給操控平臺。

（4）供電模塊：是飛行器的電力保障單元，除負(fù)責(zé)為動力輸出模塊提供電力支持外，還要為飛行控制模塊及無線通信模塊工作提供所需的電能。

圖1 礦山無人機硬件的系統(tǒng)結(jié)構(gòu)圖

2 姿態(tài)解算方法

2.1 卡爾曼濾波算法

卡爾曼濾波是解決以均方誤差最小為準(zhǔn)則的最佳線性濾波問題，它根據(jù)前一個估計值和最近一個觀察數(shù)據(jù)來估計信號的當(dāng)前值。它是用狀態(tài)方程和遞推方法進(jìn)行估計的，而它的解是以估計值（常常是狀態(tài)變量的估計值）的形式給出其信號模型是從狀態(tài)方程和量測方程得到的[1]。

由文獻(xiàn)[2]～[4]結(jié)論可得，將礦山無人機看作一個線性系統(tǒng)，則通過陀螺儀的輸出數(shù)據(jù)可以建立礦山無人機的姿態(tài)線性模型：

通過公式（1）、（2）可以實現(xiàn)對礦山無人機姿態(tài)角的預(yù)測，其算法流程如圖所示：

圖2 卡爾曼濾波算法流程圖

2.2 DMP姿態(tài)解算

MPU6050是一款六軸運動處理器，除能夠輸出三軸陀螺儀數(shù)據(jù)和三軸加速度數(shù)據(jù)外，內(nèi)部還具有專用的DMP（數(shù)字運動處理）單元，能夠?qū)⒘S數(shù)據(jù)經(jīng)濾波融合生成四元數(shù)，使用DMP單元能夠減輕系統(tǒng)處理器的運算負(fù)荷，提高系統(tǒng)的整體響應(yīng)速度[5,6]。

配置DMP單元完成礦山無人機姿態(tài)解算的步驟為：

（1）下載mpu6050運動處理器庫；

（2）配置微處理器I2C總線為塊讀寫，總線速率為400Kbit/s；

（3）通過I2C總線配置mpu6050的工作參數(shù)；

（4）通過I2C總線將mpu6050固件燒寫在mpu6050的內(nèi)部RAM中；

（5）配置DMP單元的采樣頻率；

（6）配置DMP單元工作參數(shù)；

（7）配置mpu6050的FIFO；

（8）完成六軸測量數(shù)據(jù)校準(zhǔn)；

（9）啟動數(shù)據(jù)融合濾波處理；

（10）等待轉(zhuǎn)換數(shù)據(jù)輸出；

（11）讀取輸出數(shù)據(jù)。

DMP輸出的姿態(tài)解算結(jié)果為四元數(shù)，為長整型數(shù)據(jù)，轉(zhuǎn)換為浮點型數(shù)據(jù)后，經(jīng)公式（3）轉(zhuǎn)換為歐拉角。

q1、q2、q3、q4：DMP輸出四元數(shù)除以232；roll、pitch、yaw：橫滾角、俯仰角、航向角。

其軟件設(shè)計流程圖如圖3所示。

圖3 DMP單元配置流程圖

表1 測量數(shù)據(jù)對比

經(jīng)過DMP單元處理，處理器可直接從MPU6050的數(shù)據(jù)緩存中讀出四元數(shù)及校正后的陀螺儀數(shù)據(jù)、加速度數(shù)據(jù)。通過公式，可將獲取的四元數(shù)轉(zhuǎn)化為歐拉角，得到飛行器的當(dāng)前姿態(tài)。

3 實驗對比

實驗平臺采用Freescale公司的MC9S12系列單片機作為主控單元，其工作主頻經(jīng)PLL倍頻后可達(dá)48MHz，與MPU6050通過I2C總線進(jìn)行數(shù)據(jù)交換，I2C總線通信速率為400Kbit/s，完成礦山無人機的姿態(tài)解算，并將計算出的姿態(tài)角通過串口發(fā)送到計算機，上位機將接收到的姿態(tài)角數(shù)據(jù)以描點法繪圖，串口通信速率為115200bit/s。

3.1 采用卡爾曼濾波姿態(tài)解算測試

圖4為采用卡爾曼濾波算法完成礦山無人機的姿態(tài)解算，上位機所顯示的姿態(tài)角波形。

圖4 卡爾曼濾波輸出姿態(tài)角曲線圖

由圖4可以看出，紅色曲線代表輸出的航向角Yaw、藍(lán)色曲線代表飛行器的俯仰角Pith、灰色曲線代表飛行器的橫滾角Roll。采用卡爾曼濾波算法輸出的姿態(tài)角在飛行器靜止時，Roll、Pith輸出曲線有波動，當(dāng)飛行器俯仰角、滾動角發(fā)生改變時，其航向角波動較大，因此，對航向角的解算一般需通過電子羅盤的測量數(shù)據(jù)對卡爾曼濾波輸出數(shù)據(jù)進(jìn)行校正。

且在姿態(tài)發(fā)生變化時，Roll、Pith、Yaw輸出曲線斜率較小，表明其響應(yīng)速度慢，跟隨性差。為提高卡爾曼濾波的響應(yīng)速度和跟隨性，就必須使用具有獨立的浮點運算單元的高性能微處理器來完成姿態(tài)解算。

3.2 DMP姿態(tài)解算測試

圖5采用DMP完成姿態(tài)解算，上位機顯示的姿態(tài)角波形。并將計算出的姿態(tài)角。

圖5 DMP輸出姿態(tài)角曲線圖

圖5輸出曲線含義與上圖一致。圖5采用DMP輸出的姿態(tài)角在飛行器靜止時，輸出曲線近似直線，當(dāng)飛行器運動狀態(tài)改變時，輸出曲線變化平穩(wěn)、無抖動、卻斜率較大。表明DMP姿態(tài)解算響應(yīng)速度快，跟隨性好。

3.3 實驗數(shù)據(jù)對比

表1為采用卡爾曼濾波和DMP單元完成礦山無人機姿態(tài)解算，綜合指標(biāo)對比。

4 結(jié)語

相比目前主流的礦山無人機飛行控制器設(shè)計方案，使用DMP在以下兩方面做出了改進(jìn)。

利用MPU6050內(nèi)部強大的運算處理能力完成姿態(tài)角的融合、濾波，輸出姿態(tài)角穩(wěn)定。

利用DMP單元完成礦山無人機姿態(tài)的解算過程，有效降低了處理器的負(fù)荷，增強了礦山無人機的響應(yīng)速度，同時降低了對硬件資源的要求。

經(jīng)測試，采用上述方法改進(jìn)后，利用主頻為50MHz的普通單片機就能完成對礦山無人機姿態(tài)角的準(zhǔn)確測量，且系統(tǒng)姿態(tài)角更新頻率可達(dá)200Hz，響應(yīng)速率快，飛行穩(wěn)定性強。

[1]王京衛(wèi).測繪無人機低空數(shù)字航攝影像去霧霾研究[J].測繪學(xué)報,2016,45(2).

[2]李永生,賴華榮,盧春盛,等.無人機攝影測量技術(shù)在礦產(chǎn)開采執(zhí)法監(jiān)察中的應(yīng)用[J].測繪通報, 2017(s1):95-99.

[3]馮少江,徐澤宇,石明全,等.基于改進(jìn)擴展卡爾曼濾波的姿態(tài)解算算法研究[J].計算機科學(xué),2017,44(9):227-229.

[4]馬玉清,吳房勝.PID算法在無人機智能控制系統(tǒng)中的應(yīng)用[J].湖南城市學(xué)院學(xué)報(自然科學(xué)版),2016, 25(2):119-120.

[5]葉龍.基于MPU6050傳感器方位角傾角算法研究[J].吉林大學(xué),2015,5:23-24.

[6]傅忠云,朱海霞.基于慣性傳感器MPU6050的濾波算法研究[J].電壓與聲光,2015,5(8):21-25.