聞澤陽 許江寧 于夢琦

(海軍工程大學(xué)導(dǎo)航工程系 武漢 430033)

基于MEMS IMU的室內(nèi)導(dǎo)航系統(tǒng)設(shè)計*

聞澤陽 許江寧 于夢琦

(海軍工程大學(xué)導(dǎo)航工程系 武漢 430033)

為了實現(xiàn)低成本的室內(nèi)導(dǎo)航，設(shè)計了基于慣性測量單元(IMU)的室內(nèi)組合導(dǎo)航定位系統(tǒng)， 慣性測量單元(IMU)安裝在室內(nèi)行人的肩部，在行人處于靜止狀態(tài)時以IMU輸出速度誤差與角速度誤差為觀測信息，并利用這些觀測信息通過卡爾曼濾波器估計IMU解算誤差。當行人處于行進狀態(tài)時，通過氣壓計測量室內(nèi)行人所處的海拔高度，并通過磁力計測量航向角，氣壓計所測得的數(shù)據(jù)對IMU輸出高度進行修正，磁力計所測量的數(shù)據(jù)對IMU的航向角進行修正，在論文所構(gòu)建的框架下來抑制IMU誤差積累。

室內(nèi)導(dǎo)航; 慣性導(dǎo)航系統(tǒng); 卡爾曼濾波器; 氣壓計; 磁力計

(Department of Navigation, Naval University of Engineering, Wuhan 430033)

Class Number U666.1

1 引言

導(dǎo)航具有很長時間的歷史，聰明的古羅馬人利用北極星和太陽的位置作為參照橫渡地中海，中國古代同樣也為人類導(dǎo)航做出了重大貢獻——指南針的發(fā)明。從現(xiàn)在科學(xué)定義的角度分析，導(dǎo)航是正確引導(dǎo)運載體沿著預(yù)定的航線在規(guī)定的時間從起始點到達目的地的技術(shù)或者方法，用來完成上述引導(dǎo)任務(wù)的設(shè)備(包括載體上和地面上的設(shè)備)系統(tǒng)成為導(dǎo)航系統(tǒng)[1]。按照導(dǎo)航是否需要外接參考信息，可以將導(dǎo)航分為自主導(dǎo)航和非自主式導(dǎo)航兩種[2]。自主式導(dǎo)航一般是指慣性導(dǎo)航(Inertial Navigation System,INS),非自主式導(dǎo)航主要包括天文導(dǎo)航、無線導(dǎo)航和微型導(dǎo)航等。本文將主要就衛(wèi)星導(dǎo)航展開討論。

近幾年，行人導(dǎo)航系統(tǒng)(PNS)特別是室內(nèi)行人導(dǎo)航系統(tǒng)獲得了巨大的關(guān)注。受限于室內(nèi)環(huán)境GPS信號質(zhì)量較差的條件，慣導(dǎo)系統(tǒng)累積誤差無法得到有效補償。此外，室內(nèi)運動距離短、機動性強，因此室內(nèi)導(dǎo)航定位系統(tǒng)需滿足高精度定位、快速反應(yīng)等要求，而低成本的慣性導(dǎo)航系統(tǒng)受限于器件精度及技術(shù)工藝水平等諸多因素，必須依靠有效的外部信息輔助慣導(dǎo)系統(tǒng)來校正和抑制其累積誤差。

陀螺儀和加速度計是慣性導(dǎo)航系統(tǒng)最關(guān)鍵的兩個部件。載體在慣性空間的旋轉(zhuǎn)角速度由陀螺儀測量，載體的線加速度由加速度計測量，獲得數(shù)據(jù)后通過轉(zhuǎn)換和積分，載體的姿態(tài)、速度和位置等導(dǎo)航信息便應(yīng)運而生。早期的陀螺儀應(yīng)用十分有限，主要因其干擾力矩較大，精度很低。直到20世紀50年代，美國麻省理工學(xué)院的draper實驗室成功研制出液浮陀螺儀，才使得陀螺儀真正意義上達到了慣性導(dǎo)航的要求[3]。

非自主式導(dǎo)航系統(tǒng)必須要有其他地方的設(shè)備進行協(xié)同工作，它無法單獨依靠自身的導(dǎo)航設(shè)備進行導(dǎo)航。非自主式導(dǎo)航系統(tǒng)應(yīng)用廣泛，無線電導(dǎo)航系統(tǒng)早期應(yīng)用很多，如今已被微型定位系統(tǒng)所取代。衛(wèi)星定位系統(tǒng)具有諸多優(yōu)點，例如全天候、實時性等。它在全球范圍內(nèi)為用戶提供高度精度的三圍位置、速度及時間信息，因而廣泛用于軍民領(lǐng)域。目前，美國的全球定位系統(tǒng)(GPS)已經(jīng)投入運營，它的應(yīng)用也最為廣泛，中國的“北斗”定位系統(tǒng)也正在加緊建設(shè)和完善，并且也已經(jīng)發(fā)揮了很好的導(dǎo)航作用。但衛(wèi)星定位系統(tǒng)在信號較弱甚至沒有的地區(qū)將會非常不便利，而且易受外界環(huán)境的干擾，數(shù)據(jù)更新速率也相對較慢。

組合導(dǎo)航能彌補以上兩種導(dǎo)航方式各自的不足，因而發(fā)揮各個導(dǎo)航系統(tǒng)的優(yōu)勢，更能適應(yīng)現(xiàn)代戰(zhàn)爭對導(dǎo)航的要求。因此，世界各國廣泛采用多設(shè)備組合、冗余設(shè)計、多功能的組合導(dǎo)航系統(tǒng)。其容錯能力和余度導(dǎo)航能力都得到了巨大的提高，從而增加了導(dǎo)航系統(tǒng)的可靠性。此外，組合導(dǎo)航由于其互補的能力因而對各個導(dǎo)航系統(tǒng)的精度要求相對較低，縮減了整個系統(tǒng)的成本。微型計算機與數(shù)字濾波技術(shù)的發(fā)展，即為組合導(dǎo)航的研究奠定了物質(zhì)基礎(chǔ)，又提供了有力的處理工具。目前有多重組合導(dǎo)航的方式，如全球定位系統(tǒng)/慣性導(dǎo)航系統(tǒng)(GPS/INS)，全球定位系統(tǒng)/多普勒導(dǎo)航系統(tǒng)(GPS/DNS),以慣性導(dǎo)航為主、GPS和地形輔助參考導(dǎo)航(TRN)為輔的組合導(dǎo)航系統(tǒng)(INS/GPS/TRN)，組合衛(wèi)星定位系統(tǒng)(GPS/GLONASS),衛(wèi)星導(dǎo)航/天文導(dǎo)航(GPS/CNS)等[4]。

據(jù)調(diào)查統(tǒng)計[5]，現(xiàn)在人類80%的時間都處于室內(nèi)，室內(nèi)地理信息的研究相應(yīng)的變得越來越重要。但是現(xiàn)如今大多數(shù)地圖數(shù)據(jù)僅僅是適用于室外地理信息，GPS定位技術(shù)的準確性也僅僅局限在室外，室內(nèi)的情況則無法勝任。本文正是在此背景下，研究基于低精度(價格較為低廉)MEMS慣性器件如何在室內(nèi)長時間進行較高精度導(dǎo)航定位的功能。嘗試使用INS導(dǎo)航的方式提高姿態(tài)、速度和位置的解算精度，并且加入氣壓計，讀取溫度和氣壓并經(jīng)過校正程序得到真實的氣壓高度，通過磁力計輸出行走航向，從而更好地完成室內(nèi)導(dǎo)航的功能[6～7]。

本文提出了以氣壓計、磁力計輔助IMU的室內(nèi)組合導(dǎo)航定位系統(tǒng)，IMU安裝在行人肩部，當行人處于靜止狀態(tài)時以慣導(dǎo)輸出速度為誤差及角速度誤差為觀測量，當行人處于運動狀態(tài)時以慣導(dǎo)氣壓計、磁力計輸出高度、航向為基準，對慣導(dǎo)輸出進行直接修正。

2 儀器介紹

2.1 主要器件

1) MPU-6050

MPU-6050集成了3軸MEMS陀螺儀，3軸MEMS加速度計以及一個課擴展的數(shù)字運動處理器DMP，可用I2C接口連接一個第三方的數(shù)字傳感器，比如磁力計。擴展之后，就可以通過其I2C或SPI幾口輸出一個9軸的信號。MPU-6050也可以通過其I2C接口連接非慣性的數(shù)字傳感器，比如壓力傳感器。MPU-6050的角速度全格感測范圍為±250、±500、±1000與±2000°/sec (dps)，可準確追蹤快速與慢速動作，并且，用戶可程式控制的加速器全格感測范圍為±2g、±4g、±8g與±16g。產(chǎn)品傳輸可透過最高至400kHz的IC或最高達20MHz的SPI(MPU-6050沒有SPI)。

2) 磁力計HMC-5983

Honeywell三軸電子羅盤HMC5983是一個溫度補償?shù)娜S集成電子羅盤，它是一種表面貼裝的高集成模塊，用于測量磁場，被廣泛應(yīng)用在磁場檢測，手機、平板電腦、上網(wǎng)本、電子消費、自動導(dǎo)航和個人導(dǎo)航設(shè)備等領(lǐng)域。

HMC5983內(nèi)置高分辨的HMC118X系列的磁阻傳感器加上ASIC，還有放大器，自動消磁帶、偏執(zhí)帶和一個12bitADC，這款使得5983航向角的精度達到1-2°。

3) 氣壓計BMP180

BMP180是一款高精度、小體積、超低能耗的壓力傳感器，可以應(yīng)用在移動設(shè)備中，它性能卓越，絕對精度最低可達到0.02hPa，符合測量大氣壓的精度要求，并且耗能極低，只有3uA。在低功耗的情況下，測量高度誤差為0.5m，在高功耗情況下測量誤差僅為0.17m，樓層通常情況下高約3m左右，故在樓層變換的時候該精度足以判斷樓層高度。

BMP180采用強大的8-pin陶瓷無引線芯片承載(LCC)超薄封裝，可以通過I2C總線直接與各種微處理器相連。

2.2 系統(tǒng)構(gòu)成

本次測試仿真使用的IMU單元為mini INS/GPS電路板(組合了MPU-6050、HMC5983、BMP180和ublox MAX-7Q)。

電路板的驅(qū)動和解算程序能夠完成以下功能：

1) 讀取MPU-6050輸出的三軸角速度和三軸加速度ADC值；

2) 讀取HMC5893磁力計各個軸所感應(yīng)到的磁力強度ADC值；

3) 讀取氣壓計BMP180的溫度和氣壓并經(jīng)過校正程序得到真實的氣壓高度；

4) 運行AHRS解算程序，得到當前模塊的俯仰，橫滾，航向，氣壓高度和溫度；

5) 程序?qū)⒏鱾€傳感器值、解算后的姿態(tài)信息發(fā)送到主串口TXO輸出給上位機。連接上位機將顯示波形和各個傳感器的值。

通過加速度計和陀螺儀的數(shù)據(jù)能夠探測出行人步行的狀態(tài)，當測試人員處于靜止階段的時候，卡爾曼濾波器便會開始工作。因為當測試人員處于靜止階段的時候速度和角速度的值都應(yīng)當為0，腳上IMU所測出的速度和姿態(tài)信息將被送入卡爾曼濾波器，作為觀測速度和角速度誤差的數(shù)據(jù)。

與此同時肩部IMU單元裝置有BMP180,用來測定此時此刻測試人員所處的高度信息(選在肩部的是因為人在行走過程中足部波動過大不便于測量)。將磁力計裝置在肩部，測量航向角。當行人處于運動狀態(tài)時，直接利用磁力計、氣壓計輸出對慣導(dǎo)進行修正。

行人靜止時，通過卡爾曼濾波器估計慣導(dǎo)解算誤差[5]。

(1)

系統(tǒng)量測方程為

(2)

該卡爾曼濾波器使用了如下公式組(3)：

Pk=(I-KkHk)Pk/k-1

(3)

氣壓計BMP180也將引入系統(tǒng)之中，由于現(xiàn)代GPS接收器在海拔高度信息方面十分不可靠，有效的高度信息獲取就顯得十分重要了，利用氣壓和溫度數(shù)據(jù)并經(jīng)過解算便可以獲取精度很高的高度信息。

探測器周圍的壓力隨著高度增加而減小，因此，壓力他側(cè)氣可以被用來計算其所在位置的高度，利用如下公式便可(其中p0是海平面的氣壓)：

通常情況下[8～9]，氣壓是隨著天氣變化而產(chǎn)生變化的(即使溫度的變化)，為了能夠準確地測出海拔高度，嚴格上來說是更為準確地測出所在樓層，故采用了使用溫度和氣壓數(shù)據(jù)的解算程序以達到目的。

除此之外，裝置在肩部的磁力計HMC-5983也將投入使用以抑制航向角誤差。

磁力計通過測量地磁場水平方向分量確定磁北極方向并推算出其他方向。磁南北極與地理南北極兩軸線的夾角λ成為磁偏角，在磁極點處磁場和當?shù)氐乃矫娲怪?，在赤道附近磁場和當?shù)氐乃矫嫫叫?，所以在北半球和南北球磁場方向都傾斜指向地面。地磁場是一個矢量，在一個固定的地點，地磁場矢量可以被分解為兩個于當?shù)厮矫嫫叫械姆窒砗鸵粋€與當?shù)厮矫娲怪钡姆至?。當磁力計水平放置的時候，傳感器坐標系的X、Y軸在水平面內(nèi)，X軸為前進方向，那么磁力計的X、Y、Z三個坐標軸就和這三個地磁場分量對應(yīng)起來，如圖1所示。

圖1 地磁場在水平面的矢量分解圖

圖1中X和Y軸在水平面內(nèi)，Y軸向右且垂直于X軸，Z軸鉛垂向下，HX，HY，HZ分別為地磁場強度在地平面坐標系下X、Y、Z軸的磁場強度分量。angle為當?shù)卮抛游缇€順時針旋轉(zhuǎn)至傳感器前進方向的夾角[10]。

由于磁力計固定在肩膀上，當行人運動時會發(fā)生姿態(tài)變化，其俯仰角θ和翻滾角γ可以由慣導(dǎo)測出，根據(jù)坐標變換式(4)，磁力計的三軸測量值Hx，Hy，Hz在水平面的投影值HX，HY為

HX=Hxsinθsinφ+Hycosφ-Hzcosθsinφ
Hy=Hxcosθ+Hzsinθ

(4)

根據(jù)磁力計在水平方向的投影值HX，HY，就可以計算肩上磁力計前進方向與當?shù)卮抛游缇€的夾角angle：

(5)

由于反正切公式計算出來的角度范圍與傳感器的輸出要求不符。所以要進行角度的處理，選擇易于編程的正切函數(shù)的變種函數(shù)atan2(y,x)。當參數(shù)x和y不同時為0時，atan2(y,x)是以坐標軸原點(0,0)為起點，指向(x,y)的射線在坐標平面上與x軸正方向之間的角的角度，該角度大小以弧度表示。

atan2(y,x)函數(shù)定義如下：

該函數(shù)的值域為(-π,π]。當y<0時，對atan2(y,x)的負數(shù)結(jié)果加上2π，將此函數(shù)的值域結(jié)果變化到(0,2π]范圍內(nèi)，所以磁力計的角度輸出為

如果考慮上當?shù)卮牌铅?，則航向角要為磁力計的角度加上地磁偏角，即：

α=angle+λ

(6)

每個地區(qū)的磁偏角λ是一個固定值，可以通過查表確定，并在解算程序中修正α的輸出在0～360°之間。

3 系統(tǒng)設(shè)計

利用慣導(dǎo)輸出判斷室內(nèi)行人運動狀態(tài)[11～12]，當行人靜止時，以慣導(dǎo)輸出的速度增量及角速度作為誤差觀測值，進行卡爾曼濾波，估計并補償慣導(dǎo)姿態(tài)、速度、位置誤差。行人處于運動狀態(tài)時，以氣壓計輸出的高度信息和磁力計輸出的航向角信息為基準，對慣導(dǎo)進行直接裝訂。避免慣導(dǎo)解算中由這兩個量引入的誤差。系統(tǒng)原理框圖如圖2所示。

圖2 系統(tǒng)原理框4

3.1 測試及討論

為了驗證本文設(shè)計的室內(nèi)導(dǎo)航系統(tǒng)的可行性，本節(jié)設(shè)計了兩種測試試驗，分別是平坦的地下停車場，來測試該系統(tǒng)在此室內(nèi)環(huán)境的導(dǎo)航軌跡與實際軌跡的誤差以檢驗其性能優(yōu)良。另外一個則是在有電梯的樓層之中，測試人員分別在各個樓層停留一段時間，測試氣壓計對于樓層的判斷是否足夠準確(氣壓計BMP180的測高精度可達0.17m，該精度足夠判斷行人所處的樓層)。

3.2 室內(nèi)

測試人員處于地面平坦的室內(nèi)環(huán)境，保持靜止，取校正前后慣導(dǎo)輸出的緯度為例，對比純慣性導(dǎo)航系統(tǒng)與本文設(shè)計的室內(nèi)導(dǎo)航定位系統(tǒng)的定位精度，其對比曲線如圖3所示。

圖3 位置輸出對比

圖3中，曲線為純慣導(dǎo)系統(tǒng)的緯度輸出曲線，實線為本文設(shè)計室內(nèi)導(dǎo)航定位系統(tǒng)的緯度輸出曲線?？梢钥闯觯疚脑O(shè)計的室內(nèi)導(dǎo)航定位系統(tǒng)定位精度明顯提高，慣導(dǎo)位置輸出曲線得到了有效的抑制，輸出標準差由0.3°降低到0.09°。

圖4 高度輸出

3.3 樓層測試環(huán)境

本部分將測試位于肩部IMU的氣壓計BMP180測量樓層信息的導(dǎo)航性能。測試人員將從二樓至一樓往返兩次，記錄本文設(shè)計的室內(nèi)導(dǎo)航定位系統(tǒng)輸出高度，如圖4所示。

從圖4可以看出，在氣壓計的校正下，慣導(dǎo)系統(tǒng)輸出高度能夠較為準確地反映測試人員所處的樓層信息，相比純慣導(dǎo)狀態(tài)下的高度發(fā)散，該系統(tǒng)可以輸出可靠的高度信息。

4 結(jié)語

本文提出了以氣壓計、磁力計輔助IMU的室內(nèi)組合導(dǎo)航定位系統(tǒng)，IMU安裝在行人肩部，當行人處于靜止狀態(tài)時以慣導(dǎo)輸出速度為誤差及角速度誤差為觀測量，當行人處于運動狀態(tài)時以慣導(dǎo)氣壓計、磁力計輸出高度、航向為基準，對慣導(dǎo)輸出進行直接修正。室內(nèi)測試實驗與樓層測試實驗可以證明，相比純慣導(dǎo)系統(tǒng)，該系統(tǒng)的定位精度定位精度較高，高度輸出更為可靠準確。

[1] 姜朋,趙琳,孟超等.基于MEMS-IMU的捷聯(lián)式慣性導(dǎo)航系統(tǒng)技術(shù)與實現(xiàn)研究[D].哈爾濱:哈爾濱工程大學(xué),2012:40-41.

[2] 張國良,曾靜,鄧芳林.用四元數(shù)法提取加速度計三軸轉(zhuǎn)臺測試中的動態(tài)誤差量[J].中國慣性技術(shù)學(xué)報，2000(3):1-3.

[3] 秦永元.慣性導(dǎo)航[M].北京:科學(xué)出版社，2006:1-5,40-41,78-79,358-361.

[4] 曾慶化，劉建業(yè)，彭文明等.我國衛(wèi)星系統(tǒng)相關(guān)技術(shù)發(fā)展分析[J].航天控制，2006,24(4):91-96.

[5] 徐元,陳熙源,李慶華等.一種基于雙IMU框架的室內(nèi)個人導(dǎo)航方法[J].中國慣性技術(shù)學(xué)報，2015(6):2-4.

[6] Godha S,Lachapelle G, Cannon M E. Integrated GPS/INS system for pedestrian navigation in a signal degraded environment[C]//ION GNSS,2006:1-6.

[7] 徐田來,游文虎,崔平遠.基于模糊自適應(yīng)卡爾曼濾波的INS/GPS組合導(dǎo)航系統(tǒng)算法研究[J].宇航學(xué)報，2005(5):2-4.

[8] 曾偉一，林訓(xùn)超，曾友周等.組合導(dǎo)航技術(shù)的發(fā)展趨勢[J].成都航空職業(yè)技術(shù)學(xué)院學(xué)報,2011(2):1-2.

[9] 張炎華,王立端,戰(zhàn)興群等.慣性導(dǎo)航技術(shù)的新發(fā)展及發(fā)展趨勢[J].中國造船,2008(1):1-2.

[10] 林敏敏,房建成,高國江.GPS/SINS組合導(dǎo)航系統(tǒng)混合校正卡爾曼濾波方法[J].中國慣性技術(shù)學(xué)報,2003(3):2-4.

[11] 韓輔君,徐靜,宋世忠.基于低成本多傳感器的自適應(yīng)組合濾波[J].光學(xué)精密工程,2011(12):1-3.

[12] 于永軍,劉建業(yè),熊志等.高動態(tài)載體精度捷聯(lián)管道算法[J].中國慣性技術(shù)學(xué)報,2011(2):1-3.

Design of Indoor Navigation System Based on Double MEMS IMU

WEN Zeyang XU Jiangning YU Mengqi

An indoor integrated navigation and positioning system based on inertial measurement unit (IMU) is designed, for the purpose of realizing low-cost indoor navigation. The IMU is installed on the pedestrian’s shoulder, Kalman filter (KF) estimate IMU calculation error when the pedestrian is in a static state, using the speed error and angular velocity error of IMU as the measurements. Altitude and yaw are measured by barometer and magnetometer, providing correction information, and restraining the error divergence of IMU.

indoor navigation, inertial navigation system, Kalman filter, barometer, magnetometer

2016年6月6日,

2016年7月20日

聞澤陽,男,研究方向：艦船綜合導(dǎo)航系統(tǒng)和高晶度慣性導(dǎo)航系統(tǒng)。許江寧,男,教授，博士生導(dǎo)師，研究方向：慣性技術(shù)及應(yīng)用、衛(wèi)星導(dǎo)航技術(shù)教學(xué)。

U666.1

10.3969/j.issn.1672-9730.2016.12.018