基于MEMS/GPS的航向姿態(tài)測(cè)量系統(tǒng)設(shè)計(jì)*

劉　強(qiáng)，王昌剛，劉玉寶，劉曉川

（中國(guó)船舶重工集團(tuán)第716研究所，江蘇連云港222006）

基于MEMS/GPS的航向姿態(tài)測(cè)量系統(tǒng)設(shè)計(jì)*

劉強(qiáng)*，王昌剛，劉玉寶，劉曉川

（中國(guó)船舶重工集團(tuán)第716研究所，江蘇連云港222006）

姿態(tài)信息是飛行控制中最關(guān)鍵的參數(shù)之一。針對(duì)飛行控制系統(tǒng)首要解決的姿態(tài)測(cè)量問(wèn)題。本文利用多微機(jī)電系統(tǒng)（MEMS）傳感器與GPS組合，研制了一種微型航向姿態(tài)測(cè)量系統(tǒng)?？紤]在以往的姿態(tài)測(cè)量系統(tǒng)中，動(dòng)加速度的影響限制其應(yīng)用，通過(guò)設(shè)計(jì)利用GPS進(jìn)行輔助修正的姿態(tài)解算算法，有效規(guī)避了運(yùn)動(dòng)加速度對(duì)測(cè)量精度的影響，使系統(tǒng)可同時(shí)滿足靜態(tài)情況和動(dòng)態(tài)情況的使用。將試驗(yàn)結(jié)果與商用姿態(tài)測(cè)量產(chǎn)品MTI-G-700的姿態(tài)結(jié)果進(jìn)行了比較，各項(xiàng)指標(biāo)均達(dá)到系統(tǒng)設(shè)計(jì)的性能要求，驗(yàn)證了航向姿測(cè)量系統(tǒng)設(shè)計(jì)的有效性。

航向姿態(tài)測(cè)量系統(tǒng)；微機(jī)電系統(tǒng)；GPS；飛行控制；

姿態(tài)測(cè)量是無(wú)人機(jī)實(shí)現(xiàn)姿態(tài)控制的前提，是導(dǎo)航系統(tǒng)不可分割的一個(gè)重要組成部分［1-4］。伴隨著導(dǎo)航系統(tǒng)的發(fā)展，姿態(tài)航向測(cè)量技術(shù)也越加的成熟起來(lái)。采用陀螺進(jìn)行姿態(tài)測(cè)量是最常用的方法，但陀螺測(cè)得的姿態(tài)角是發(fā)散的，不能長(zhǎng)期使用。GPS是載體姿態(tài)測(cè)量的另一重要工具，通常有多天線測(cè)姿和基于“偽姿態(tài)”的單天線測(cè)姿兩種測(cè)量方法，但其也有諸多的限制條件。隨著組合導(dǎo)航技術(shù)的產(chǎn)生，組合姿態(tài)航向測(cè)量技術(shù)逐漸取代了單一陀螺測(cè)姿的方法，使得姿態(tài)測(cè)量的精度越來(lái)越高。荷蘭的Xsens公司的MTI-G航姿器是一款六自由度的微型航姿系統(tǒng)［5］，集成了MEMS、IMU、GPS、磁強(qiáng)計(jì)、靜態(tài)氣壓計(jì)、溫度傳感器，經(jīng)數(shù)據(jù)融合后能提供精確的地磁數(shù)據(jù)、姿態(tài)數(shù)據(jù)、位置數(shù)據(jù)和速度數(shù)據(jù)；法國(guó)SBG Systems公司的IG-500A/E/N微型姿態(tài)航向參考系統(tǒng)（AHRS）［6］擁有嵌入式的擴(kuò)展卡爾曼濾波器，在高動(dòng)態(tài)條件下能夠提供高精度的姿態(tài)和方位信息，該航姿系統(tǒng)還帶有GPS輔助定位功能。此類姿態(tài)測(cè)量系統(tǒng)雖然精度較高，但是因其價(jià)格昂貴，不適用于微型飛行器或小型無(wú)人機(jī)。

本文以微型飛行器為應(yīng)用背景開(kāi)展了姿態(tài)測(cè)量研究，針對(duì)MEMS傳感器存在精度低，漂移大，且當(dāng)飛行器運(yùn)動(dòng)時(shí)，加速度計(jì)信號(hào)將不能正確反映機(jī)體的姿態(tài)信息等缺點(diǎn)，利用三軸MEMS陀螺、三軸MEMS加速度計(jì)、三軸MEMS磁強(qiáng)計(jì)和微型GPS等多個(gè)傳感器設(shè)計(jì)合理可行的軟硬件解決方案。研制了一種應(yīng)用于微小型飛行器或小型無(wú)人機(jī)的姿態(tài)測(cè)量系統(tǒng)［7-8］。

1　系統(tǒng)總體方案

針對(duì)微小型無(wú)人機(jī)體積小、重量輕、功耗低等特點(diǎn)，系統(tǒng)選用基于MEMS的微小型慣性測(cè)量元件和GPS作為傳感器，為姿態(tài)解算提供原始的數(shù)據(jù)。采用擴(kuò)展卡爾曼濾波器設(shè)計(jì)了一種基于MEMS陀螺/加速度計(jì)/磁強(qiáng)計(jì)/GPS組合的姿態(tài)測(cè)量方法，實(shí)現(xiàn)了微小型無(wú)人機(jī)的精確姿態(tài)測(cè)量功能。系統(tǒng)總體結(jié)構(gòu)框圖如1所示。

圖1　系統(tǒng)總體結(jié)構(gòu)圖

圖1中虛線框?yàn)閿U(kuò)展卡爾曼濾波算法，實(shí)線框?yàn)榛贕PS測(cè)量信息的輔助修正算法，利用GPS經(jīng)緯高和速度，解算出實(shí)際的線性加速度補(bǔ)償加速度計(jì)輸出，用補(bǔ)償后所得的沿機(jī)體三軸的重力加速度分量作為觀測(cè)量，采用基于四元數(shù)的擴(kuò)展卡爾曼濾波算法實(shí)現(xiàn)對(duì)無(wú)人機(jī)姿態(tài)四元數(shù)的濾波估計(jì)，并利用地磁偏角對(duì)解算出的航向角進(jìn)行修正。有效解決了機(jī)體運(yùn)動(dòng)加速度和地磁偏角對(duì)航向姿態(tài)測(cè)量精度的影響，提高了航向姿態(tài)測(cè)量系統(tǒng)的測(cè)量精度。

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

該系統(tǒng)硬件平臺(tái)采用“DSP+MIMU+GPS”的架構(gòu)形式，其結(jié)構(gòu)如圖2所示。圖2中，虛線框內(nèi)為DSP及其外圍電路組成的核心處理單元，用于系統(tǒng)的實(shí)時(shí)解算和通信。慣性測(cè)量元件和GPS用于提供系統(tǒng)姿態(tài)解算的原始信息［9］。

圖2　硬件結(jié)構(gòu)圖

①處理器選型

考慮姿態(tài)解算過(guò)程中需完成狀態(tài)預(yù)測(cè)、狀態(tài)估值、估計(jì)均方誤差等諸多運(yùn)算，涉及到大量的高階矩陣運(yùn)算過(guò)程；系統(tǒng)中包括矩陣元素在內(nèi)的絕大部分變量，均為64位單精度浮點(diǎn)類型，大大提高了其對(duì)運(yùn)算精度的要求；姿態(tài)解算頻率要求達(dá)到100 Hz，對(duì)運(yùn)算速度要求高?；谏鲜鲋T多要求，選用TI（德州儀器）公司的的32位TMS320F28335型DSP控制器作為該嵌入式平臺(tái)的處理器。

③慣導(dǎo)模塊選擇

考慮到微小型無(wú)人機(jī)機(jī)艙空間、載荷有限及指標(biāo)要求，選用ADI公司生產(chǎn)的中等精度的MEMS 磁/慣性傳感器 ADIS16407作為慣性測(cè)量單元。ADIS16407慣性測(cè)量模塊具有集成度高、體積小的特點(diǎn)，內(nèi)部集成了三軸角速率陀螺、三軸線加速度計(jì)和三軸磁強(qiáng)計(jì)等部件，內(nèi)部集成FIR濾波算法，能夠?yàn)闊o(wú)人機(jī)姿態(tài)解算提供所需的原始數(shù)據(jù)。

②GPS模塊選擇

GPS模塊采用瑞士 u-blox公司的 UBLOX NEO-6M芯片，該模塊尺寸小、功耗低，具有豐富的通信接口，硬件電路設(shè)計(jì)簡(jiǎn)單，設(shè)計(jì)中通過(guò)UART接口與DSP處理器通信。

基于系統(tǒng)結(jié)構(gòu)圖和核心部件的選型，完成了組合航向姿態(tài)測(cè)量系統(tǒng)的研制，如圖3所示。

圖3　航向姿態(tài)測(cè)量系統(tǒng)實(shí)物圖

3　算法設(shè)計(jì)

3.1姿態(tài)算法推導(dǎo)

首先假設(shè)陀螺儀的偏差漂移只受到高斯白噪聲影響［10］，即

接著根據(jù)四元數(shù)和角速度之間的關(guān)系，有下式

其中

ω1，ω2、ω3是機(jī)體坐標(biāo)相對(duì)于磁場(chǎng)NED坐標(biāo)的角速度。陀螺儀測(cè)量到的角速度大小是機(jī)體坐標(biāo)系相對(duì)于慣性坐標(biāo)系，需要將角速度轉(zhuǎn)換成機(jī)體坐標(biāo)相對(duì)于磁場(chǎng)NED坐標(biāo)的角速度。慣性坐標(biāo)系相對(duì)于磁場(chǎng)NED坐標(biāo)的角速度可以從下式得到

其中 ωearth是地球角速度，包含了地球的公轉(zhuǎn)和自轉(zhuǎn)，ωtranslation是無(wú)人機(jī)運(yùn)動(dòng)而造成的角速率。

接著將正確的角速度帶入Ω

合并（2）（4）式，并且令系統(tǒng)狀態(tài)

對(duì)其進(jìn)行微分

接著使用加速度計(jì)和磁力計(jì)當(dāng)做系統(tǒng)的量測(cè)值，加速度計(jì)的測(cè)量值包含重力場(chǎng)和無(wú)人機(jī)的線性加速度，其中重力場(chǎng)的方向就是NED坐標(biāo)的Z軸方向，但是加速度計(jì)沒(méi)有辦法單獨(dú)測(cè)量重力場(chǎng)，因此需要將無(wú)人機(jī)的線性加速度扣除才是正確的量測(cè)值［11］，即

而磁力計(jì)測(cè)量的是地球的磁場(chǎng)方向，地球的磁場(chǎng)方向位于磁場(chǎng)NED坐標(biāo)的XZ平面上，不能直接當(dāng)做磁場(chǎng)NED坐標(biāo)任一軸的量測(cè)值，但可以利用重力場(chǎng)方向和地球磁場(chǎng)方向的外積，因?yàn)檫@兩個(gè)方向的外積正好是磁場(chǎng)NED坐標(biāo)的Y軸。假設(shè)地球磁場(chǎng)方向?yàn)镋、大小為M，重力場(chǎng)方向?yàn)間、大小為G，如此可以得到Gg=gxb1+gyb2+gzb3

因此

3.2機(jī)動(dòng)加速度計(jì)算

姿態(tài)測(cè)量系統(tǒng)一般利用重力場(chǎng)的三軸分量來(lái)計(jì)算俯仰角和滾轉(zhuǎn)角。假設(shè)飛機(jī)某一瞬間的俯仰角為θ，滾轉(zhuǎn)角為φ，且沿飛機(jī)X、Y、Z三軸的重力場(chǎng)分量為gx，gy，gz。則

若已知gx，gy，gz就可以經(jīng)由下式求得俯仰角及滾轉(zhuǎn)角：

但在飛行過(guò)程中無(wú)法直接測(cè)量gx，gy，gz。如果飛行瞬間的三軸速度分別為U、V、W，三軸速度分別為p、q、r，而加速度計(jì)量測(cè)得到的加速度分別為ax、ay、az，則加速度與這些參數(shù)之間的關(guān)系為：

通過(guò)機(jī)械三軸加速度計(jì)測(cè)量可以得知ax、ay、az，由機(jī)械三軸陀螺儀可以測(cè)得p、q、r，而飛行瞬間的三軸加速度U˙、V˙、W˙則可以通過(guò)GPS接收器所測(cè)的速度的一次差分而取得，詳細(xì)計(jì)算過(guò)程如下

其中，下標(biāo)n為時(shí)間標(biāo)志；V˙N、V˙E、V˙D分別表示飛行器向北、向東、向下的速度；V˙N、V˙E、V˙D分別表示飛行器向北、向東、向下的加速度；Δt為系統(tǒng)的采樣時(shí)間。最后將算出的VN、VE、VD、V˙N、V˙E、V˙D透過(guò)旋轉(zhuǎn)矩陣將器轉(zhuǎn)成飛行器機(jī)體坐標(biāo)系上的U˙、V˙、W˙。

旋轉(zhuǎn)矩陣表示如下：

因此

取得以上的信息后，便可以通過(guò)之前的公式算出三軸重力場(chǎng)分量gx，gy，gz。此方法的優(yōu)點(diǎn)是沒(méi)有積分運(yùn)算，所以量測(cè)誤差不會(huì)累積甚至發(fā)散，長(zhǎng)時(shí)間而言具有較高的準(zhǔn)確性。但其缺點(diǎn)是需要量測(cè)或計(jì)算三軸加速度分量，其結(jié)果通常都含有較高的噪聲。

3.3磁偏角計(jì)算

GPS可以提供用大地坐標(biāo)系表示的信息，通過(guò)文獻(xiàn)［12］可知GPS參考大地坐標(biāo)系與地磁坐標(biāo)系之間會(huì)隨著地理經(jīng)緯度的變化而變化，因此只要能夠找到GPS坐標(biāo)系與地磁坐標(biāo)系之間的轉(zhuǎn)換關(guān)系，即可利用GPS經(jīng)緯度實(shí)時(shí)計(jì)算地磁偏角［13］。

其中α，β分別表示經(jīng)度和緯度。

其中αN，βN分別表示磁北極的經(jīng)度和緯度。GCM表示地磁坐標(biāo)系到大地坐標(biāo)系的轉(zhuǎn)換矩陣

其中

推導(dǎo)出所有坐標(biāo)關(guān)系后，可以得到

地磁坐標(biāo)系和地理坐標(biāo)之間的轉(zhuǎn)換旋轉(zhuǎn)角（磁偏角）為

4　試驗(yàn)驗(yàn)證

為了驗(yàn)證算法的可行性，本文在翼展2 m的飛行器上分別進(jìn)行了地面試驗(yàn)，空中飛行不帶GPS修正和帶GPS修正動(dòng)態(tài)試驗(yàn)，主要驗(yàn)證內(nèi)容如下：①姿態(tài)解算算法的性能；②高機(jī)動(dòng)情況下，帶GPS修正算法的機(jī)動(dòng)性補(bǔ)償效果。并將三種種試驗(yàn)條件下得到的俯仰角、滾轉(zhuǎn)角、航向角輸出結(jié)果與國(guó)外最新的航姿測(cè)量產(chǎn)品MTI-G-700的輸出結(jié)果進(jìn)行了比較，地面試驗(yàn)結(jié)果如圖4所示?？罩酗w行不帶GPS修正動(dòng)態(tài)試驗(yàn)結(jié)果如圖5所示，空中飛行帶GPS修正動(dòng)態(tài)試驗(yàn)結(jié)果如圖6所示。

圖4　地面動(dòng)態(tài)試驗(yàn)試驗(yàn)結(jié)果

圖5　空中飛行不帶GPS修正動(dòng)態(tài)試驗(yàn)結(jié)果

圖6　空中飛行帶GPS修正動(dòng)態(tài)試驗(yàn)結(jié)果

由圖4可知在地面小機(jī)動(dòng)情況下本文所設(shè)計(jì)的航姿測(cè)量系統(tǒng)與MTI姿態(tài)測(cè)量產(chǎn)品輸出的俯仰角、滾轉(zhuǎn)角和航向角結(jié)果非常吻合。圖5可知在動(dòng)態(tài)無(wú)GPS修正的情況下，姿態(tài)測(cè)量結(jié)果較差，測(cè)量曲線波動(dòng)劇烈，這與基于MEMS微慣性器件的姿態(tài)測(cè)量設(shè)備容易受機(jī)動(dòng)加速度，而傳統(tǒng)的姿態(tài)測(cè)量方法無(wú)法消除運(yùn)動(dòng)加速度對(duì)姿態(tài)測(cè)量精度的影響，導(dǎo)致試驗(yàn)數(shù)據(jù)波動(dòng)劇烈，測(cè)量精度較差。由圖6可知在GPS修正的情況下，通過(guò)GPS的測(cè)量信息融合到姿態(tài)測(cè)量算法之中，有效消除了運(yùn)動(dòng)加速度對(duì)姿態(tài)測(cè)量精度的影響，從而實(shí)現(xiàn)姿態(tài)輸出與真值測(cè)量設(shè)備趨勢(shì)一致，且跟蹤誤差滿足動(dòng)態(tài)精度要求，俯仰角和滾轉(zhuǎn)角精度小于2°（RMS），航向角精度小于3°（RMS），達(dá)到了系統(tǒng)的設(shè)計(jì)指標(biāo)。動(dòng)態(tài)試驗(yàn)表明，本文所設(shè)計(jì)的姿態(tài)測(cè)量方法在無(wú)人機(jī)高機(jī)動(dòng)飛行狀態(tài)下仍能夠有效抑制陀螺漂移導(dǎo)致的姿態(tài)發(fā)散，有效消除了MEMS微慣性器件姿態(tài)測(cè)量受機(jī)動(dòng)加速度的影響，且能夠?qū)崟r(shí)高精度的跟蹤無(wú)人機(jī)的真實(shí)姿態(tài)。

5　結(jié)論

本文提出一種基于三軸MEMS陀螺，三軸MEMS加速度計(jì)，MEMS磁力計(jì)和微型GPS研制微型姿態(tài)測(cè)量系統(tǒng)的方法，通過(guò)地面和空中飛行動(dòng)態(tài)試驗(yàn)，將本文結(jié)果與最新的商用姿態(tài)測(cè)量產(chǎn)品MTI-G-700輸出的姿態(tài)結(jié)果進(jìn)行了對(duì)比，驗(yàn)證了本文方法的可行性和優(yōu)點(diǎn)。高機(jī)動(dòng)飛行試驗(yàn)表明，本文算法遠(yuǎn)好于未作機(jī)動(dòng)性補(bǔ)償?shù)腗EMS微慣性器件的姿態(tài)測(cè)量算法，且系統(tǒng)設(shè)計(jì)簡(jiǎn)單，尺寸較?。?0×50×30），造價(jià)低，便于在小型無(wú)人機(jī)上應(yīng)用，總體性能已接近國(guó)外同類產(chǎn)品水平，目前該型產(chǎn)品已經(jīng)在某小型無(wú)人機(jī)的飛行控制及導(dǎo)航系統(tǒng)上獲得成功應(yīng)用。

［1］王昌剛，劉強(qiáng)，韓瑜.姿態(tài)航向測(cè)量與誤差補(bǔ)償方法研究［J］.傳感器與微系統(tǒng)，2014，33（11）：33-40.

［2］葉小嶺，杜浩，王偉.應(yīng)用于多旋翼MAVs的姿態(tài)測(cè)量系統(tǒng)［J］.傳感技術(shù)學(xué)報(bào)，2012，25（3）：344-348.

［3］Jeffrey D Barton.Fundamentals of Small Unmanned Aircraft Flight ［J］.Johns Hopkins Apl Technical Digest，2012，31（2）：132-149.

［4］吳峰，王向軍，湯其劍.基于數(shù)字調(diào)節(jié)方法的MEMS陀螺零位補(bǔ)償技術(shù)研究［J］.傳感技術(shù)學(xué)報(bào)，2012，25（12）：25-28.

［5］MTI-G User Manual and Technical Documentation［Z］.Xsens Technologies，2007.

［6］IG-500NGPS Aided AHRSUserManual［Z］.SBG Systems，2009.

［7］Chul Woo Kang，Chan Gook Park.Improvement of Attitude Estimation Using Hidden Markov Model Classification［J］.American Institute of Aeronautics and Astronautics，2010，25（11）：15-18.

［8］Karlgaard and H.Schaub.Nonsingular Attitude Filtering Using Modified Rodrigues Parameters［J］.Journal of the Astronautical Sciences，2010，57（4）：777-791.

［9］Or D Dantsker，Renato Mancuso，Michael S Selig.High Frequency Sensor Data Acquisition System for Flight Control and Aerodynamic Data Collection［J］.American Institute of Aeronautics and Astronautics，2014，14（8）：42-47.

［10］Zhi Yu Peng.Attitude Determination Based on Rodrigues Parameters［D］.The Thesis of Tamkang Universty，2012.

［11］Liu qiang，Gao Fu yin，Wang Chang Gang，et al.Improvement of Attitude Estimation Using Modified Rodrigues Parameters and In-telligent Controller［J］.Applied Mechanics and Materials，2014，32（1）：635-639.

［12］Rotenberg D，Luinge H，Baten C，et al.Veltink.Compensation of Magnetic Disturbances Improves Inertial and Magnetic Sensing of Human Body Segment Orientation［J］.IEEE Trans Neural Syst Rehabil，2005，13：395-401.

［13］Daniel Roetenberg，Per J Slycke，Peter H Veltink.Ambulatory Position and Orientation Tracking Fusing Magnetic and Inertial Sensing［J］.IEEE Transacti-Ons On Biomedical Engineering，2007，54 （5）：161-167.

劉強(qiáng)（1985-），男，漢族，中船重工第七一六所工程師，碩士研究生學(xué)歷，主要研究方向?yàn)闊o(wú)人機(jī)導(dǎo)航與飛行控制，工業(yè)機(jī)器人運(yùn)動(dòng)控制技術(shù)，基于MEMS傳感器的誤差建模與補(bǔ)償技術(shù)，SAR無(wú)人機(jī)偵察路徑規(guī)劃等，nuaalq@qq.com。

Design of Attitude and Heading Reference System Base on MEMS/GPS*

LIU Qiang*，WANG Changgang，LIU Yubao，LIU Xiaochuan
（716 Institute of China Shipbuilding Industry Company，Lianyungang Jiangsu 222006，China）

Attitude information is one of the most important parameters in flight control.Focus on the most important problem of attitude measurement in flight control system.This paper，a kind of micro attitude attitude and heading reference（AHRS）is developed based on the combination of multiple MEMS sensors and GPS.In the past，the attitude measurement system，kinetic acceleration effects limit its application，through design of auxiliary correction algorithm based on GPS velocity，effectively avoid the influence of kinetic acceleration on the measurement accuracy. The system can meet the use of static and dynamic situation.The test results are compared with commercial attitude measurement product MTI-G-700 show that，the design of attitude and heading reference system is validity.

AHRS；MEMS；GPS；Flight Control

V249

A

1004-1699（2016）08-1290-07

EEACC：723010.3969/j.issn.1004-1699.2016.08.028

項(xiàng)目來(lái)源：江蘇省自然科學(xué)基金青年基金項(xiàng)目（BK2012236）

2016-01-21修改日期：2016-04-11