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

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

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

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

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

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

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

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

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

圖1 礦山無人機硬件的系統(tǒng)結構圖

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

2.1 卡爾曼濾波算法

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

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

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

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

2.2 DMP姿態(tài)解算

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

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

（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輸出的姿態(tài)解算結果為四元數，為長整型數據，轉換為浮點型數據后，經公式（3）轉換為歐拉角。

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

其軟件設計流程圖如圖3所示。

圖3 DMP單元配置流程圖

表1 測量數據對比

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

3 實驗對比

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

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

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

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

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

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

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

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

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

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

3.3 實驗數據對比

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

4 結語

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

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

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

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

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

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

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

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

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

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