低成本傳感器數(shù)據(jù)融合技術(shù)輔助iPhone導(dǎo)航研究*

洪海斌，郭 杭，郭俊鴿，方 爽，滕長勝

（1.南昌大學(xué) 信息工程學(xué)院，江西 南昌 330031;2.中測(cè)新圖（北京）遙感技術(shù)有限責(zé)任公司，北京 100039）

0 引言

隨著定位和導(dǎo)航系統(tǒng)在個(gè)人移動(dòng)終端（如手機(jī)、平板電腦等）中的不斷普及，個(gè)人的定位和導(dǎo)航技術(shù)最近幾年被廣泛關(guān)注，成為時(shí)下研究的熱點(diǎn)。2010年6月，蘋果公司推出了新一代手機(jī)iPhone 4G［1］，與前幾代產(chǎn)品不同的是，iPhone 4中加入了微機(jī)電三軸陀螺儀，這一舉措使得基于移動(dòng)終端的LBS服務(wù)進(jìn)入了一個(gè)嶄新的時(shí)代。

加速度計(jì)和陀螺儀是慣性導(dǎo)航的基本測(cè)量單元之一，二者均安裝于運(yùn)動(dòng)載體的內(nèi)部。加速度計(jì)可以用來測(cè)量載體的運(yùn)動(dòng)加速度，利用已知的初始位置和速度，對(duì)加速度進(jìn)行積分，就可知道載體的速度和位置等信息;陀螺儀用來測(cè)量運(yùn)動(dòng)載體的角運(yùn)動(dòng)，感知每個(gè)軸向的角度變化信息，最終能夠輸出載體的姿態(tài)信息。

單純使用陀螺儀的方案可用于需要高分辨率和快速反應(yīng)的旋轉(zhuǎn)檢測(cè);單純使用加速度計(jì)的方案可用于有固定的重力參考坐標(biāo)系、存在線性或傾斜運(yùn)動(dòng)，但旋轉(zhuǎn)運(yùn)動(dòng)被限制在一定范圍內(nèi)的應(yīng)用。而同時(shí)處理直線運(yùn)動(dòng)和旋轉(zhuǎn)運(yùn)動(dòng)時(shí)，就需要使用加速度和陀螺儀的方案，為使設(shè)計(jì)和制作的陀螺儀具有更高的加速度和較低的機(jī)械噪聲，或?yàn)樾Ｕ铀俣扔?jì)的旋轉(zhuǎn)誤差，一些廠商會(huì)使用磁力計(jì)來完成傳統(tǒng)上用陀螺儀實(shí)現(xiàn)的傳感功能，以完成相應(yīng)定位。這表明混合的陀螺儀、加速度計(jì)或磁感應(yīng)計(jì)結(jié)合的方案［2，3］正成為MEMS陀螺儀技術(shù)應(yīng)用的趨勢(shì)。若只使用傳統(tǒng)的加速度計(jì)，用戶得到的要么是反應(yīng)敏感的但噪聲較大的輸出，要么是反應(yīng)慢但較純凈的輸出，而如將加速度計(jì)與陀螺儀相結(jié)合，就能得到既純凈又反應(yīng)敏捷的輸出。

從MEMS陀螺儀的應(yīng)用方向來看，陀螺儀能夠測(cè)量沿一個(gè)軸或幾個(gè)軸運(yùn)動(dòng)的角速度，可與MEMS 加速度計(jì)形成優(yōu)勢(shì)互補(bǔ)。本文提出了一種基于iPhone手機(jī)平臺(tái)下的多傳感器（包括加速度計(jì)、陀螺儀、磁力計(jì)以及GPS）信息融合技術(shù)，為有效解決手機(jī)用戶的精確定姿定位需求提供了一套可行的實(shí)施方案。

1 低成本慣性導(dǎo)航初始對(duì)準(zhǔn)

初始對(duì)準(zhǔn)一方面能夠使慣性導(dǎo)航系統(tǒng)所描述的坐標(biāo)系與導(dǎo)航坐標(biāo)系相重合，另一方面使導(dǎo)航計(jì)算機(jī)正式工作時(shí)有精確的初始條件，如初始速度、初始位置和初始姿態(tài)等。初始對(duì)準(zhǔn)結(jié)果的好壞直接影響著整個(gè)系統(tǒng)導(dǎo)航信息的精度。初始對(duì)準(zhǔn)通常分成靜態(tài)和動(dòng)態(tài)2種，從原理上講，2種模式下的初始對(duì)準(zhǔn)方法均包括粗對(duì)準(zhǔn)和精對(duì)準(zhǔn)2個(gè)過程，但2種模式下的對(duì)準(zhǔn)方法卻有所差異。下面主要討論MEMS慣性導(dǎo)航系統(tǒng)的靜態(tài)初始對(duì)準(zhǔn)方法。

1.1 粗對(duì)準(zhǔn)［4］

傳統(tǒng)的慣性導(dǎo)航系統(tǒng)靜態(tài)粗對(duì)準(zhǔn)通常采用解析粗對(duì)準(zhǔn)方法來完成，通過使用重力、地球旋轉(zhuǎn)矢量以及加速度計(jì)和陀螺儀的觀測(cè)值來求取姿態(tài)旋轉(zhuǎn)矩陣，然后求出3個(gè)姿態(tài)角（橫滾角γ、俯仰角θ、航向角ψ），方法如下

式中g(shù)為當(dāng)?shù)刂亓铀俣龋琇為當(dāng)?shù)鼐暥?，ωie為地球自轉(zhuǎn)角速度，fb為加速度計(jì)輸出量為陀螺儀輸出量。

由于低成本陀螺的大漂移，低信噪比，因此，低成本慣導(dǎo)在靜基座環(huán)境下的粗對(duì)準(zhǔn)不能使用解析粗對(duì)準(zhǔn)方法解決。通過加速度計(jì)輸出量可以求出橫滾角γ和俯仰角θ的值，方法如下所示

對(duì)于z通道，由于重力誤差遠(yuǎn)小于低成本IMU偏差，因此，航向角ψ的值可以通過磁力計(jì)獲得。

1.2 精對(duì)準(zhǔn)

精對(duì)準(zhǔn)過程是建立在粗對(duì)準(zhǔn)提供的概略初始姿態(tài)角的基礎(chǔ)上進(jìn)行的，精對(duì)準(zhǔn)的目的是通過卡爾曼濾波方法精確估計(jì)出導(dǎo)航坐標(biāo)系與計(jì)算坐標(biāo)系之間的偏差，待姿態(tài)失準(zhǔn)角到達(dá)穩(wěn)定時(shí)求出精確的初始姿態(tài)角。文獻(xiàn)［2］中提到采用比力的東向和北向測(cè)量值作為觀測(cè)量的精對(duì)準(zhǔn)方法，精對(duì)準(zhǔn)方法原理圖如圖1所示。

圖1 比力為觀測(cè)量的精對(duì)準(zhǔn)Fig 1 Fine alignment of specific measurement

導(dǎo)航系下的比力誤差方程為

導(dǎo)航系下的陀螺測(cè)量值誤差方程為

2 低成本GPS/INS組合導(dǎo)航系統(tǒng)建模方法

對(duì)于車載導(dǎo)航來說，運(yùn)載體在地面上移動(dòng)時(shí)豎直方向和左右方向的速度幾乎接近于 0［5，6］，因此，添加載體在豎直方向和左右方向上的速度作為觀測(cè)量，從增加系統(tǒng)可觀測(cè)性出發(fā)，這樣更有利于對(duì)導(dǎo)航參數(shù)的最優(yōu)估計(jì)。GPS/INS組合導(dǎo)航系統(tǒng)原理圖如圖2所示。

圖2 GPS/INS組合導(dǎo)航系統(tǒng)原理圖Fig 2 Principle diagram of GPS/INS integrated navigation system

載體坐標(biāo)系下速度表達(dá)式為

理論上，載體在豎直方向和左右方向上的速度約為0，假設(shè)y軸指向運(yùn)載體前方，則有

實(shí)際求出的載體在豎直方向和左右方向的速度（，）即為誤差值，添加和作為觀測(cè)量。

2.1 狀態(tài)方程

其中，φE，φN，φU為數(shù)學(xué)平臺(tái)失準(zhǔn)角;δvE，δvN，δvU分別為載體的東向、北向和天向速度誤差;δL，δλ，δh分別為緯度誤差、經(jīng)度誤差和高度誤差;ξx，ξy，ξz，Δx，Δy，Δz分別為陀螺隨機(jī)常值漂移和加速度計(jì)隨機(jī)常值零偏。系統(tǒng)的狀態(tài)轉(zhuǎn)移矩陣F（t）為

其中，F(xiàn)N，系統(tǒng)噪聲W和系數(shù)矩陣G的具體形式，見文獻(xiàn)［7］。

2.2 量測(cè)方程

式中 下標(biāo)I為慣性導(dǎo)航下的解算結(jié)果，下標(biāo)G為GPS輸出結(jié)果，V（t）為觀測(cè)噪聲。

根據(jù)系統(tǒng)的狀態(tài)方程和量測(cè)方程，結(jié)合導(dǎo)航參數(shù)的誤差方程，再利用擴(kuò)展卡爾曼濾波技術(shù)［8］對(duì)狀態(tài)量進(jìn)行最優(yōu)估計(jì)，然后按照?qǐng)D2混合校正法［9］所示對(duì)輸出量進(jìn)行開環(huán)校正，對(duì)慣性器件進(jìn)行閉環(huán)校正，最終得到較為精確的位置、速度和姿態(tài)信息。

3 車載實(shí)驗(yàn)

本次車載實(shí)驗(yàn)搭載的IMU為iPhone 4手機(jī)中內(nèi)嵌的傳感器（包括加速度計(jì)、陀螺儀、磁力計(jì)等），GPS采用iPhone4中自帶的GPS接收機(jī)，實(shí)驗(yàn)起始時(shí)刻有幾分鐘的靜態(tài)觀測(cè)數(shù)據(jù)。原始實(shí)驗(yàn)觀測(cè)數(shù)據(jù)通過Core Motion軟件導(dǎo)出，陀螺常值漂移取為100°/h，加速度計(jì)常值零偏取為60 mgn;IMU載體坐標(biāo)如圖3，實(shí)驗(yàn)結(jié)果如圖4～圖8和表1所示。

圖3 IMU載體坐標(biāo)系示意圖Fig 3 Schematic diagram of IMU body coordinate system

圖4 運(yùn)行軌跡Fig 4 Moving trajectory

圖5 GPS/INS解算后位置Fig 5 Calculated position of GPS/INS

圖6 GPS/INS解算后速度Fig 6 Calculated velocity of GPS/INS

表1 GPS/INS組合后的位置與GPS位置誤差統(tǒng)計(jì)表Tab 1 Error statistics between GPS position and GPS/INS integrated position

4 結(jié)論

對(duì)于一些低成本IMU，由于敏感元器件自身的缺陷而不能使用常規(guī)的方法完成IMU/GPS聯(lián)合解算，所以，往往通過尋找一些外圍設(shè)備（磁力計(jì)、GPS速度等）的輔助來提高整個(gè)系統(tǒng)的性能和解算精度。本車載實(shí)驗(yàn)結(jié)果表明:依靠加速度計(jì)自身計(jì)算出的俯仰角和橫滾角加上磁力計(jì)計(jì)算出來的航向角作為初始姿態(tài)值來完成后面的捷聯(lián)解算具有一定的合理性。通過添加載體在豎直方向和左右方向的速度作為觀測(cè)量后的解算結(jié)果中，GPS/INS組合后的位置與GPS位置北向位置偏差約為2.7 m、東向位置偏差為2.3 m左右、天向位置偏差約為2.8 m。

圖7 GPS/INS解算后姿態(tài)Fig 7 Calculated attitude of GPS/INS

圖8 GPS/INS解算后位置與GPS接收機(jī)位置偏差Fig 8 Offset between GPS receiver position and calculated position by GPS/INS

［1］三軸陀螺儀與加速度計(jì)如何輔助 Iphone定位［N/OL］.［2011—05—20］.http://www.weizhiquan.com/archives/1072.

［2］管 斌，李 濤，吳美平，等.一種應(yīng)用磁強(qiáng)計(jì)提高導(dǎo)航系統(tǒng)航向精度的方法［J］.傳感器與微系統(tǒng)，2010，29（8）:37-40.

［3］許國珍.微慣性/衛(wèi)星/磁強(qiáng)計(jì)組合導(dǎo)航技術(shù)研究［D］.長沙:國防科技大學(xué)，2007:20-34.

［4］Eu-Hwan Shin.Accuracy improvement of low cost INS/GPS for land applications［EB/OL］.University of Calgary.［2001—12—10］.http://www.g-eomatics.ucalgary.ary.ca/links/GradTheses.html.

［5］Eun-Hwan Shin.Estimation techniques for low-cost inertial navigation［EB/OL］.University of Calgary.［2005—05—13］.http://www.geomatics.ucalgary.ca/links/GradTheses.html.

［6］Campion G，Andera B D.Controllability and sate feedback stabilizability of non-holonomic mechanical systems［EB/OL］.［1990—12—20］.http://www.springerlink.com/content/ku4r07186455n842/fulltext.pdf，107-114.

［7］全 偉，劉百奇，宮曉琳，等.慣性/天文/衛(wèi)星組合導(dǎo)航技術(shù)［M］.北京:國防工業(yè)出版社，2011:120-132.

［8］秦永元，汪叔華.卡爾曼濾波與組合導(dǎo)航原理［M］.西安:西北工程大學(xué)出版社，2010:168-175.

［9］張雨楠.INS/GPS組合導(dǎo)航系統(tǒng)濾波器設(shè)計(jì)［D］.哈爾濱:哈爾濱工程大學(xué)，2007:20-34.