基于MPU9250的示教航行姿態(tài)研究

彭琰舉，宋文學(xué)，王晉，王鵬

（西安航空學(xué)院機(jī)械工程學(xué)院機(jī)電技術(shù)研究所陜西省泵類(lèi)裝備工程研究中心陜西西安710077）

隨著科學(xué)技術(shù)的不斷發(fā)展，基于PC系統(tǒng)的虛擬儀器儀表被廣泛應(yīng)用于工業(yè)及軍事領(lǐng)域中。其中，可輸出高精度的位置、速度、姿態(tài)、角速度、加速度和時(shí)間等信息的航姿測(cè)量?jī)x最為常見(jiàn)。但這類(lèi)儀器多采用慣性導(dǎo)航技術(shù)和組合導(dǎo)航技術(shù)，往往內(nèi)置了價(jià)格較為昂貴的高精度光纖陀螺、石英加速度計(jì)及多模衛(wèi)星導(dǎo)航接收機(jī)等設(shè)備[1-3]。

在教學(xué)中，若采用此類(lèi)儀器作為示教儀器，將會(huì)增加教育成本。文中所研制的儀器系統(tǒng)采用高度集成的數(shù)字式加速度計(jì)、磁場(chǎng)計(jì)、陀螺儀傳感器設(shè)計(jì)的AHRS航姿參考系統(tǒng)，既具備常見(jiàn)航測(cè)量?jī)x的功能，又更大地降低了教學(xué)成本，與此同時(shí)該系統(tǒng)又能被擴(kuò)展至各類(lèi)無(wú)線(xiàn)設(shè)備中，具有較高的市場(chǎng)價(jià)值和應(yīng)用前景[4-5]。

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

系統(tǒng)總體框架設(shè)計(jì)方案如圖1所示，本系統(tǒng)分為兩部分，圖 1（a）為服務(wù)端，圖 1（b）為客戶(hù)端。服務(wù)端主要由USB調(diào)試模塊、DC/DC、LDO，電池模塊、無(wú)線(xiàn)模塊ESP8266、航行姿態(tài)檢測(cè)模塊MPU9250等部分組成?？蛻?hù)端主要由PC、CH340[6]、模塊ESP8266組成。

圖1 系統(tǒng)邏輯框圖

其中ESP8266模塊采用了Tensilica公司的Xtensa? 32-bit LX6處理器[7-9]，它在封裝為QFN32的芯片上集成了主頻達(dá)160 MHz的IP內(nèi)核，可以同時(shí)工作在A(yíng)P模式與STA模式下，具備云升級(jí)及多總線(xiàn)擴(kuò)展功能，在物聯(lián)網(wǎng)領(lǐng)域應(yīng)用廣泛。

該系統(tǒng)中的MPU9250是一個(gè)QFN封裝的復(fù)合芯片（MCM），在3x3x1mm上集成了3軸16位加速度、3軸16位陀螺儀、3軸16位磁力計(jì)以及數(shù)字運(yùn)動(dòng)處理器（DMP），并且可以承受較大的震動(dòng)沖擊，該器件具備快速和慢速運(yùn)動(dòng)狀態(tài)跟蹤功能，內(nèi)置的溫度傳感器和偏差計(jì)數(shù)器可以有效的進(jìn)行器件的溫度補(bǔ)償，在85℃時(shí)可以保證僅有1%的時(shí)鐘漂移。使用I2C方案使得系統(tǒng)連接大大簡(jiǎn)化，通訊速率可達(dá)400 kHz，能夠直接輸出9軸全部數(shù)據(jù)，通過(guò)軟件濾波和歸一化計(jì)算運(yùn)算可以得到姿態(tài)參數(shù)。

2 系統(tǒng)原理

教學(xué)示教航行姿態(tài)系統(tǒng)，通過(guò)安裝在航模上的九軸數(shù)字傳感器MPU9250進(jìn)行姿態(tài)測(cè)量，其內(nèi)部結(jié)構(gòu)如圖2所示。核心控制器ESP8266通過(guò)I2C總線(xiàn)與傳感器MPU9250通訊讀取10組AD值。分別為，三軸加速度、三軸磁強(qiáng)計(jì)度、三軸陀螺儀、一組溫度的AD值，經(jīng)過(guò)歸一化姿態(tài)融合后就可以得到Pitch、Roll、Yaw角，再通過(guò)建立無(wú)線(xiàn)WIFI路由器AP模式的TCP/UDP服務(wù)，為客戶(hù)端提供測(cè)試所得的姿態(tài)數(shù)據(jù)。

在客戶(hù)端使用Unity3Dengine創(chuàng)建UI，如圖3所示。

圖2 MPU9250邏輯結(jié)構(gòu)圖

示教時(shí)，針對(duì)性的展示姿態(tài)角，它們是：俯仰角pitch、滾轉(zhuǎn)角yaw、偏航角roll。在地面上選一點(diǎn)Og，使Xg軸在水平面內(nèi)并指向某一方向；Zg軸垂直于地面并指向地心，重力方向；Yg軸在水平面內(nèi)垂直于Xg軸，其指向按左手定則確定。

圖3 示教虛擬展示

3 硬件設(shè)計(jì)

3.1 USB通訊電路設(shè)計(jì)

CH340是一個(gè)USB總線(xiàn)的轉(zhuǎn)接芯片，通過(guò)USB總線(xiàn)提供異步串口、打印口、并口以及常用的2線(xiàn)和4線(xiàn)等同步串行接口。該芯片提供了兼容USB相關(guān)規(guī)范和Windows操作系統(tǒng)的USB轉(zhuǎn)串行通訊，在本系統(tǒng)中大大簡(jiǎn)化了硬件通訊設(shè)計(jì)。

如圖4所示，USB與串行通訊的轉(zhuǎn)換完全在芯片CH340內(nèi)部完成。在本系統(tǒng)能夠中實(shí)現(xiàn)下載與通訊調(diào)試兩種角色。其中一鍵程序電路，DTR為數(shù)據(jù)終端準(zhǔn)備好信號(hào)，RTS為數(shù)據(jù)終端請(qǐng)求信號(hào)。

圖4 USB通訊電路

3.2 無(wú)線(xiàn)控制器電路設(shè)計(jì)

在本系統(tǒng)設(shè)計(jì)中如圖5所示。

無(wú)線(xiàn)通訊部分，只需在外部加入的由L1、L2、C5組成的π型選頻網(wǎng)絡(luò)即可，在PCB制版時(shí)需要加入50歐姆的阻抗匹配即可。

由于ESP8266進(jìn)入BOOT下載模式必須滿(mǎn)足以下條件：

1）CHIP_EN引腳置高

2）GPIO0引腳置低（置高則進(jìn)入從FLASH運(yùn)行模式）

而實(shí)現(xiàn)一鍵下載的關(guān)鍵就在于，GPIO0引腳和RESET引腳電平狀態(tài)，當(dāng)DTR為1，RTS為0時(shí)，RESET復(fù)位引腳拉低，反之，GPIO0引腳拉低，邏輯關(guān)系如表1所示：

在使用PC端自動(dòng)下載固件時(shí)，在軟件端需要啟動(dòng)硬件流控制并適當(dāng)在不同模式間加入50 ms延時(shí)才能正常使用，否則可能出現(xiàn)異常。

3.3 電源電路設(shè)計(jì)

本系統(tǒng)設(shè)計(jì)的電源如圖6所示，輸入電壓工作范圍為6～24 V，輸出電壓為3.30 V，采用DC/DC（BUCK）與LDO協(xié)同工作，在DC/DC中加入了旁路濾波選頻器（C3、R6），在輸出級(jí)加入了兩級(jí)自恢復(fù)保險(xiǎn)（F1、F2）。

其中DC/DC選用芯源半導(dǎo)體生產(chǎn)的MP1584EN，與之配合的電感選用FDV系列鐵粉芯電感，電容選用TDK系列，輸出級(jí)采用電壓3倍的耐壓值，LDO選用AMS1117。

該設(shè)計(jì)一方面具有較高的轉(zhuǎn)換效率，還可適應(yīng)更寬的輸入電壓范圍，另一方面有效降低了系統(tǒng)電源紋波，提高整體電路輸出電壓工作穩(wěn)定性和熱穩(wěn)定性，同時(shí)加入的自恢復(fù)保險(xiǎn)避免了短路風(fēng)險(xiǎn)對(duì)核心控制器的損害。

4 軟件設(shè)計(jì)

態(tài)解算一般有兩種算法，一種是IMU，另一種是被各種無(wú)人機(jī)廣泛使用的AHRS。

圖5 ESP8266控制器子系統(tǒng)

表1 ESP8266啟動(dòng)邏輯

圖6 供電源電路

4.1 姿態(tài)解算

本系統(tǒng)姿態(tài)檢測(cè)部分由加速度計(jì)，磁場(chǎng)計(jì)，陀螺儀構(gòu)成，能夠?yàn)轱w行器提供準(zhǔn)確可靠的航向（yaw），橫滾（roll）和側(cè)翻（pitch）等姿態(tài)航行信息。傳感器所測(cè)得的數(shù)據(jù)分別為3X，3Y，3Z三軸的加速度和三軸角速度原坐標(biāo)系，因此常采用四元數(shù)和歐拉角表示旋轉(zhuǎn)狀態(tài)[10]，其公式如式（1）（2）所示。

式（1）中w、x、y、z為四元數(shù)，式（2）中φ、θ、ψ分別為全局坐標(biāo)系下的角度。

在系統(tǒng)中采用上述方法對(duì)姿態(tài)進(jìn)行解算。

4.2 濾波器設(shè)計(jì)

盡管采用了數(shù)字式的九軸傳感器，但由于加速度及磁場(chǎng)對(duì)外界震動(dòng)影響較大，需要結(jié)合陀螺儀進(jìn)行歸一化解算，進(jìn)而得到AHRS狀態(tài)，本文采用卡爾曼濾波器進(jìn)行濾波，采用最小均方誤差為估計(jì)準(zhǔn)則，這種方法采用信號(hào)與噪聲的狀態(tài)空間模型，利用循環(huán)迭代求出當(dāng)前測(cè)量估計(jì)值。這類(lèi)方法非常適合微處理實(shí)時(shí)計(jì)算[11-15]。其濾波模型如式（4）所示：

對(duì)所測(cè)得的數(shù)值取不同的權(quán)值，把它們結(jié)合到一起，進(jìn)行修正，如式（5）、（6）所示：

FK是作用在xk上的狀態(tài)變換矩陣；

Bk是作用在控制向量uk上的輸入模型；

Hk是測(cè)量矩陣，wk為噪聲干擾；

Qk為協(xié)方差矩陣，Pk|k為當(dāng)前狀態(tài)協(xié)方差；

就那樣分手了。三年的校園愛(ài)情，一年的共同生活，蒼茫地結(jié)束。而夏天也就過(guò)去了，所有的陽(yáng)光都好像在夏季里消耗殆盡。

|k-1為上次更新的狀態(tài)（角度）；

其中（5）式為預(yù)測(cè)估計(jì)階段，包含預(yù)測(cè)狀態(tài)和協(xié)方差矩陣；式（6）是數(shù)據(jù)更新階段包含，卡爾曼系數(shù)，更新?tīng)顟B(tài)的推算和協(xié)方差的推算；

其中角度解算濾波步驟如下[16]：

1）計(jì)算預(yù)估計(jì)協(xié)方差矩陣；

2）計(jì)算卡爾曼增益矩陣；

3）更新測(cè)量估計(jì)；

5）循環(huán)重復(fù)以上步驟。

4.3 解算程序

以下為嵌入式卡爾曼濾波算法實(shí)現(xiàn)：

4.4 實(shí)驗(yàn)結(jié)果

測(cè)試時(shí)沿著Z軸旋轉(zhuǎn)并記錄數(shù)據(jù)，如圖7所示。

如圖7～8所示，為示教器實(shí)際運(yùn)動(dòng)軌跡，測(cè)量所得的軌跡與濾波后的軌跡。經(jīng)過(guò)濾波后的姿態(tài)解算，基本與原始運(yùn)動(dòng)軌跡一致，角度解算達(dá)到設(shè)計(jì)要求。

5 結(jié) 論

在航姿示教器的設(shè)計(jì)過(guò)程中，充分考慮了噪聲對(duì)系統(tǒng)的干擾，在硬件使用了更為精密的電源設(shè)計(jì)，在軟件上采用了卡爾曼權(quán)值濾波。同時(shí)系統(tǒng)采用了C/S架構(gòu)無(wú)線(xiàn)WIFI模式，便于與計(jì)算機(jī)、手機(jī)等設(shè)備連接。通過(guò)實(shí)驗(yàn)室及教學(xué)驗(yàn)證，達(dá)到了設(shè)計(jì)要求，可以滿(mǎn)足示教功能的姿態(tài)解算，具有較好的示教效果，具有一定的參考價(jià)值。

圖7 偏航角roll測(cè)試

圖8 偏航角roll姿態(tài)解算濾波