何松, 陳興武

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

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

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

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

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

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

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

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

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

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

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

1.1 MAX 10 FPGA 芯片特性分析

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

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

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

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

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

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

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

1.2 MPU9250的特性

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

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

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

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

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

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

2.2 FPGA電路設(shè)計(jì)

FPGA的原理圖和PCB見圖3和圖4，其采用Altera公司高性能的FPGA,具體型號(hào)為10M08SCU169，具有對(duì)外接口豐富，實(shí)時(shí)性好等優(yōu)點(diǎn)。本設(shè)計(jì)只用了其中部分接口，因此如若有需要增添的器件也可及時(shí)加入，滿足大眾需求。對(duì)各種傳感器模塊的數(shù)據(jù)采集都適用，所以選擇10M08SCU169作為整個(gè)系統(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è)計(jì)用到多個(gè)傳感器都是數(shù)字傳感器，故而FPGA可直接讀取傳感器的數(shù)據(jù)。以九軸慣性傳感器MPU9250等傳感器與主控芯片F(xiàn)PGA之間采取SPI通信進(jìn)行數(shù)據(jù)采集。

首先對(duì)幾個(gè)傳感器進(jìn)行初始化，采取SPI通信協(xié)議，對(duì)各個(gè)傳感器數(shù)據(jù)讀寫函數(shù)進(jìn)行初始化。接著確定傳輸?shù)刂?，進(jìn)行數(shù)據(jù)傳輸，待數(shù)據(jù)傳輸完畢再進(jìn)行數(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提出的著名算法，它有兩種很好的步驟：預(yù)估和糾正。卡爾曼濾波在很多領(lǐng)域都得到了較好的應(yīng)用，文中所涉及的多傳感器也應(yīng)用到了卡爾曼濾波?？柭鼮V波以系統(tǒng)測(cè)量的數(shù)據(jù)作為其輸入，以系統(tǒng)狀態(tài)的估計(jì)值作為輸出，達(dá)到優(yōu)化系統(tǒng)噪聲和觀測(cè)噪聲的方法。

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

(1)

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

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

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

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

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

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

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

狀態(tài)量的預(yù)估計(jì)：

誤差協(xié)方差預(yù)估計(jì)：

卡爾曼增益：

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

誤差協(xié)方差：

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

3.2.2 角度的計(jì)算

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

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

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

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

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

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

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

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

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

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

4 實(shí)驗(yàn)與分析

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

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

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

在MATLAB中對(duì)其進(jìn)行仿真，分別模擬無人機(jī)在靜止和運(yùn)動(dòng)狀態(tài)下的情況，不考慮風(fēng)的干擾，對(duì)其姿態(tài)信息進(jìn)行采集。

圖7是在靜止位置測(cè)的俯仰角，不考慮最后取件時(shí)的誤差?？梢钥闯鱿到y(tǒng)測(cè)量與實(shí)際測(cè)量誤差最大達(dá)±0.04°，而經(jīng)濾波處理后它們之間的最大誤差為±0.025°?？梢娋鹊玫搅艘欢ǖ奶嵘?。

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

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

圖8 在運(yùn)動(dòng)狀態(tài)的俯仰角Fig.8 Pitching angle of motion

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

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

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

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

圖10 在運(yùn)動(dòng)狀態(tài)的偏航角Fig.10 Yaw angle of motion

5 結(jié)論

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

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

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

[3]劉春陽，徐軍領(lǐng)，程洪濤，等. MPU9250傳感器的姿態(tài)檢測(cè)與數(shù)據(jù)融合[J].河南科技大學(xué)學(xué)報(bào)(自然科學(xué)版)，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)傳感器采集測(cè)試系統(tǒng)的設(shè)計(jì)與實(shí)現(xiàn)[J]．計(jì)算機(jī)測(cè)量與控制，2011，19(7):1562-1564．

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

國(guó)家科技支撐計(jì)劃項(xiàng)目(2015BAF24B01);福建省科技計(jì)劃項(xiàng)目(2014H6006)

何松(1994- )，男，湖北鄂州人，碩士研究生，研究方向：無人機(jī)飛控，電氣控制。

10.3969/j.issn.1672-4348.2016.06.015

V249.1

A

1672-4348(2016)06-0587-06