基于MPU9250的無人機姿態(tài)信息采集及處理

何松, 陳興武

(1.福建省數(shù)字化裝備重點實驗室，福建 福州 350118;2.福建工程學院 信息科學與工程學院，福建 福州 350118)

基于MPU9250的無人機姿態(tài)信息采集及處理

何松1,2, 陳興武1,2

(1.福建省數(shù)字化裝備重點實驗室，福建 福州 350118;2.福建工程學院 信息科學與工程學院，福建 福州 350118)

研究MPU9250多軸姿態(tài)傳感器在無人機上的姿態(tài)信息采集，通過對九軸傳感器內(nèi)的陀螺儀，重力加速度計，磁力計的數(shù)據(jù)接收后，結(jié)合卡爾曼濾波算法對其數(shù)據(jù)進行精準的解算；搭建了一個無人機姿態(tài)模擬的物理實驗平臺，進行無人機運動狀態(tài)下的信息采集；最后，在MATLAB的數(shù)據(jù)仿真下測試其性能。實驗結(jié)果表明該方法可使無人機信息采集的精準度提升一倍。

姿態(tài)控制； 信息處理； 卡爾曼濾波； 無人機

無人機近年來的迅速發(fā)展，并且廣泛應(yīng)用于商業(yè)，農(nóng)業(yè)和軍事等領(lǐng)域，而無人機的姿態(tài)信息對于實現(xiàn)精確運動控制至關(guān)重要。傳統(tǒng)的機械陀螺儀和光纖陀螺儀等傳感器價格昂貴，體積較大，不適合于無人機的發(fā)展[1]。因此，使用微機電系統(tǒng)(MEMS)技術(shù)制造的傳感器進行姿態(tài)檢測并廣泛應(yīng)用于無人機的控制系統(tǒng)中。通過多個傳感器的姿態(tài)檢測，并將其數(shù)據(jù)融合處理是當今的研究熱點[2]。

使用功能強大體積小巧的九自由度慣性器件MPU9250進行姿態(tài)檢測，用卡爾曼濾波對其進行預處理，采集得到實用的姿態(tài)數(shù)據(jù)，結(jié)合FPGA控制系統(tǒng)，最終實現(xiàn)無人機姿態(tài)信息的精確性。

1 系統(tǒng)總體設(shè)計

無人機姿態(tài)信息采集的系統(tǒng)是基于MAX10 FPGA為核心的控制平臺，通過姿態(tài)傳感器MPU9250對運動姿態(tài)進行檢測，得到其航向角、俯仰角、翻滾角等姿態(tài)信息。采用卡爾曼濾波先進行數(shù)據(jù)整合，經(jīng)處理后將數(shù)據(jù)傳送給FPGA控制平臺，對其實行姿態(tài)信息采集。系統(tǒng)結(jié)構(gòu)如圖1。無人機的姿態(tài)信息采集應(yīng)盡可能的穩(wěn)定、精度高、抗干擾能力強，對慣性導航器件要求應(yīng)具備較高的靈敏度、較快的處理速度等特點，因此采用以MPU9250九自由度慣性傳感器的主傳感器，代替以前用的六軸慣性傳感器MPU6000、三軸加速度傳感器LSM303D、三軸陀螺儀L3GD20等，讀取信號時每個傳感器采集的數(shù)據(jù)都有一個相應(yīng)的權(quán)值，通過聯(lián)合卡爾曼濾波計算得到最終值。

圖1 系統(tǒng)結(jié)構(gòu)圖Fig.1 System structure diagram of UAVs

1.1 MAX 10 FPGA 芯片特性分析

無人機姿態(tài)信息采集系統(tǒng)用到的MAX 10 FPGA不僅繼承FPGA器件的特性，而且邏輯資源密度大幅提升，更為重要的是 MAX 10還加入了之前的一些拓展設(shè)備，如DSP模塊、DDR3存儲控制器等。其性能大幅提升，可以說它是一種非易失性的FPGA。MAX 10繼承了普通FPGA器件的特性：

(1)采用FPGA設(shè)計ASIC電路(專用集成電路)，用戶不需要投片生產(chǎn)，就能得到適用的芯片。

(2)采用了邏輯單元陣列的概念，可重復擦除寫入，可以在不改變硬件電路的基礎(chǔ)上實現(xiàn)多種不同電路功能。

(3)FPGA內(nèi)部有豐富的觸發(fā)器和I/O引腳。

(4)FPGA設(shè)計開發(fā)周期短、開發(fā)費用低，集成度高，可靠性好。

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

MAX 10的工作狀態(tài)是由存放在片內(nèi)RAM的程序來設(shè)置的。因此，使用之前需要對片內(nèi)的RAM進行編程。可以根據(jù)不同的配置模式，采用不同的編程方式。配置完成后，F(xiàn)PGA進入工作狀態(tài)。掉電后，F(xiàn)PGA恢復成白片，內(nèi)部邏輯關(guān)系消失，因此，F(xiàn)PGA能夠反復使用。FPGA的編程無需專用的FPGA編程器，只需通用的EPROM、PROM編程器即可。當需要修改FPGA功能時，只需換一片EPROM即可。這樣，同一片F(xiàn)PGA，不同的編程數(shù)據(jù)，可以產(chǎn)生不同的電路功能。因此，F(xiàn)PGA的使用非常靈活。

1.2 MPU9250的特性

MPU9250為全球首例整合9軸運動姿態(tài)檢測的數(shù)字傳感器，其相比上一代產(chǎn)品，體積小巧，并且功耗低。

MPU9250內(nèi)部有3軸陀螺儀、3軸重力加速度計、3軸磁力計，都是以16位ADC信號輸出；有集成電路總線(IIC)接口和串行外設(shè)接口(SPI)2種模式接口，傳輸速率分別可達400 kHz/s(IIC)和20 MHz/s(SPI)。其陀螺儀的角速度測量范圍最高可達± 2 000 °/s，具有良好的動態(tài)響應(yīng)特性。重力加速度的最大測量范圍為± 16g(g為重力加速度)，磁力計可達±4 800 μT，可用于對偏航角的輔助測量。

2 慣性器件硬件設(shè)計

2.1 傳感器模塊設(shè)計

傳感器均采用SPI與FPGA相連，其中MPU9250內(nèi)的AK8963電子羅盤采用IIC讀取。由于SPI通信傳輸?shù)母咝?，系統(tǒng)運算速度也會更快，其硬件電路如圖2。

圖2 MPU9250慣性傳感器接線圖Fig.2 Circuit diagram of MPU9250 inertial sensor

2.2 FPGA電路設(shè)計

FPGA的原理圖和PCB見圖3和圖4，其采用Altera公司高性能的FPGA,具體型號為10M08SCU169，具有對外接口豐富，實時性好等優(yōu)點。本設(shè)計只用了其中部分接口，因此如若有需要增添的器件也可及時加入，滿足大眾需求。對各種傳感器模塊的數(shù)據(jù)采集都適用，所以選擇10M08SCU169作為整個系統(tǒng)的處理芯片。

圖3 FPGA部分接線圖Fig.3 FPGA partial circuit diagram

圖4 FPGA電路PCB 圖Fig.4 FPGA circuit PCB

3 數(shù)據(jù)接收及處理

3.1 數(shù)據(jù)采集

本設(shè)計用到多個傳感器都是數(shù)字傳感器，故而FPGA可直接讀取傳感器的數(shù)據(jù)。以九軸慣性傳感器MPU9250等傳感器與主控芯片F(xiàn)PGA之間采取SPI通信進行數(shù)據(jù)采集。

首先對幾個傳感器進行初始化，采取SPI通信協(xié)議，對各個傳感器數(shù)據(jù)讀寫函數(shù)進行初始化。接著確定傳輸?shù)刂?，進行數(shù)據(jù)傳輸，待數(shù)據(jù)傳輸完畢再進行數(shù)據(jù)處理。以MPU9250為例，其程序流程圖如圖5所示。

圖5 MPU9250傳感器數(shù)據(jù)采集流程圖Fig.5 MPU9250 sensor data acquisition flowchart

3.2 數(shù)據(jù)處理

3.2.1 卡爾曼濾波

卡爾曼濾波是1960年由R.E.Kalman提出的著名算法，它有兩種很好的步驟：預估和糾正?？柭鼮V波在很多領(lǐng)域都得到了較好的應(yīng)用，文中所涉及的多傳感器也應(yīng)用到了卡爾曼濾波。卡爾曼濾波以系統(tǒng)測量的數(shù)據(jù)作為其輸入，以系統(tǒng)狀態(tài)的估計值作為輸出，達到優(yōu)化系統(tǒng)噪聲和觀測噪聲的方法。

導航坐標系相對于載體坐標系之間的變換關(guān)系如式(1)所示。

(1)

之前的重力加速度在無人機本體的X、Y、Z軸上的分量xg、yg、zg，根據(jù)式(1)所示的導航坐標系n相對于載體坐標系b的方向余弦變換關(guān)系，有

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

求解式(3)，可得出無人機本體的俯仰角θ和橫滾角φ。

之后用姿態(tài)矩陣和四元數(shù)法得出其對應(yīng)的關(guān)系，采用卡爾曼濾波器計算狀態(tài)x的最優(yōu)估計。

采用文獻[3]中卡爾曼濾波器,公式如式(4)。

其中：xk為狀態(tài)向量，yk為觀測向量，A為從k-1時刻到k時刻的狀態(tài)轉(zhuǎn)移矩陣，Uk為系統(tǒng)輸入控制向量，B為輸入控制向量的增益矩陣，H為狀態(tài)量到觀測量的增益矩陣，ωk為輸入噪聲，νk為測量噪聲。

假設(shè)其噪聲都服從正態(tài)分布，輸入噪聲的協(xié)方差為Q, 測量噪聲的協(xié)方差為R, 則其5個核心公式如下。

狀態(tài)量的預估計：

誤差協(xié)方差預估計：

卡爾曼增益：

現(xiàn)狀態(tài)估計：

誤差協(xié)方差：

卡爾曼濾波中的噪聲會影響其響應(yīng)速度和效果，因此各項系數(shù)的初始值要設(shè)穩(wěn)定。在軟件處理卡爾曼濾波時盡量保證各方系數(shù)平衡。

3.2.2 角度的計算

MPU9250傳感器傳輸回來的數(shù)據(jù)分別代表其3個軸測量到的角速度、加速度、磁感應(yīng)強度，需要將其解算成姿態(tài)角度信息θ、φ、ψ(分別是無人機本體的俯仰角、橫滾角以及航向角)。其中俯仰角是X軸與地面的角度，橫滾角是Y軸與地面的角度，航向角是其繞Z軸旋轉(zhuǎn)的角度。

通過積分就可以得到對應(yīng)的角度[7]，計算公式如下：

其中：θk為前一時刻的角度值，θk-1為當前陀螺儀的角速度，ωk為陀螺儀當前時刻角速度的偏移量，ωbias_k為當前時刻角速度的偏移量，dt為積分時間(即采樣周期)。

重力加速度計的測量可利用三角函數(shù)關(guān)系解算角度姿態(tài)[8]：

其中：Ax、Ay、Az分別是X、Y、Z這3個軸的加速度分量；γ為Z軸與重力加速度的夾角,ρ為俯仰角,φ為翻滾角。

磁力計測量的是磁感應(yīng)強度，其在水平位置且無外加磁場干擾時，航向角可以通過三角函數(shù)關(guān)系計算[8]：

其中，Lx和Ly分別是X軸和Y軸輸出的磁感應(yīng)強度數(shù)據(jù)。

當磁力計不在水平位置的時候，則可通過傾斜補償?shù)姆椒ㄒ越档推浣嵌葴y量的誤差。傾斜補償公式為：

其中：Mx、My、Mz分別為磁力計3個軸的輸出數(shù)據(jù)；θ和φ分別為加速度計檢測的俯仰角和橫滾角。

通過補償后的磁感應(yīng)強度和式(13)可以計算出補償后的航向角。

4 實驗與分析

由于無人機在飛行過程中可能受到外界干擾，影響其正常軌跡，而在該系統(tǒng)的分析處理下，會對其姿態(tài)和角速度進行糾正，使其回歸正常軌道，用MATLAB對此系統(tǒng)進行仿真，對其角速度，姿態(tài)及控制力矩進行分析。再加入MPU9250的無人機控制系統(tǒng)后，該系統(tǒng)對角度的調(diào)節(jié)更加穩(wěn)定，對系統(tǒng)的調(diào)節(jié)響應(yīng)更加快速。

本實驗是建立在自制的模擬平臺上(見圖6)實現(xiàn)的，數(shù)據(jù)濾波則是在MATLAB中進行的，由系統(tǒng)檢測接收到的數(shù)據(jù)記錄下來，在利用PC機上的MATLAB進行數(shù)據(jù)處理,實現(xiàn)對姿態(tài)角的卡爾曼濾波。

圖6 飛行模擬轉(zhuǎn)臺Fig.6 Flight simulation turntable

在MATLAB中對其進行仿真，分別模擬無人機在靜止和運動狀態(tài)下的情況，不考慮風的干擾，對其姿態(tài)信息進行采集。

圖7是在靜止位置測的俯仰角，不考慮最后取件時的誤差。可以看出系統(tǒng)測量與實際測量誤差最大達±0.04°，而經(jīng)濾波處理后它們之間的最大誤差為±0.025°。可見精度得到了一定的提升。

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

圖8是轉(zhuǎn)臺旋轉(zhuǎn)過程中測的俯仰角，其在旋轉(zhuǎn)過程中可能會有部分抖動，但對數(shù)據(jù)精準分析無多大影響，可以看到系統(tǒng)測量最大誤差在±3(°)，經(jīng)濾波處理后，其最大誤差在±1.2(°)，可見精度得到了較大的提高。

圖8 在運動狀態(tài)的俯仰角Fig.8 Pitching angle of motion

圖9是在靜止位置的偏航角，其系統(tǒng)測量與實際測量的最大偏差在±0.03°，經(jīng)濾波處理后在±0.02°。

圖10是在動態(tài)下的偏航角，由圖10可看出系統(tǒng)測量偏差在±3.5°，濾波偏差在±1.5°。精度得到了顯著提高。

經(jīng)測試該系統(tǒng)能對無人機的俯仰、翻滾和偏航的數(shù)據(jù)讀取，實現(xiàn)了對采集到的數(shù)據(jù)進行一定的處理，表明本系統(tǒng)采集信息的精準性，可以滿足對無人機姿態(tài)數(shù)據(jù)采集的需求。

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

圖10 在運動狀態(tài)的偏航角Fig.10 Yaw angle of motion

5 結(jié)論

無人機的姿態(tài)采集是其整個控制系統(tǒng)的關(guān)鍵技術(shù)之一。本文研究了基于MPU9250的無人機姿態(tài)信息采集，并結(jié)合了卡爾曼濾波對其數(shù)據(jù)進行處理，在自制的姿態(tài)模擬平臺上對其進行測試，驗證了該方法可使其精準度最高提升一倍左右，并且該系統(tǒng)還具有多接口，實時性好，具有良好的應(yīng)用前景。

[1]秦勇，臧希喆，王曉宇，等．基于MEMS慣性傳感器的機器人姿態(tài)監(jiān)測系統(tǒng)的研究[J]．傳感技術(shù)學報，2007，20(2):298-301．

[2]周兆英，葉雄英，李勇，等.微型系統(tǒng)和微型制造技術(shù)[J].微米納米科學與技術(shù)，1996,2(1):1-11.

[3]劉春陽，徐軍領(lǐng)，程洪濤，等. MPU9250傳感器的姿態(tài)檢測與數(shù)據(jù)融合[J].河南科技大學學報(自然科學版)，2015，36(4)：14-17.

[4]沙占友，王彥朋，張永昌.單片偏航角速度陀螺儀的原理與應(yīng)用[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]畢盛，閔華清，李淳，等．姿態(tài)傳感器采集測試系統(tǒng)的設(shè)計與實現(xiàn)[J]．計算機測量與控制，2011，19(7):1562-1564．

[7]李偉，何鵬舉，高社生．多傳感器加權(quán)信息融合算法研究[J]．西北工業(yè)大學學報，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