叢 麗,王音心,路 輝
(北京航空航天大學(xué)電子信息工程學(xué)院,北京 100191)
基于動(dòng)態(tài)判斷的組合導(dǎo)航性能改進(jìn)方法
叢 麗,王音心,路 輝
(北京航空航天大學(xué)電子信息工程學(xué)院,北京 100191)
針對(duì)微機(jī)電系統(tǒng)(Micro Electro Mechanical System,MEMS)-慣性導(dǎo)航系統(tǒng)(Inertial Navigation System,INS)/全球定位系統(tǒng)(Global Positioning System,GPS)組合導(dǎo)航在城市車載導(dǎo)航應(yīng)用中的定姿性能和GPS中斷時(shí)的定位性能精度較差的問題,提出了一種基于動(dòng)態(tài)判斷的組合導(dǎo)航性能改進(jìn)方法。該方法將車輛的動(dòng)態(tài)分為三種,并設(shè)置了車輛在各個(gè)動(dòng)態(tài)下的姿態(tài)約束和運(yùn)動(dòng)模型,然后利用慣性元件的輸出判斷出當(dāng)前動(dòng)態(tài),并選擇對(duì)應(yīng)動(dòng)態(tài)的姿態(tài)約束實(shí)時(shí)修正MEMS-INS定姿結(jié)果。當(dāng)GPS中斷時(shí),則選擇相應(yīng)的運(yùn)動(dòng)模型進(jìn)行定位推算,以便控制MEMS-INS定位誤差。通過實(shí)驗(yàn)證明,該方法有效改進(jìn)了MEMS-INS/GPS車載組合導(dǎo)航的性能。
慣性導(dǎo)航系統(tǒng);動(dòng)態(tài)判斷;姿態(tài)約束;運(yùn)動(dòng)模型
車載導(dǎo)航應(yīng)用要求導(dǎo)航系統(tǒng)能夠提供較高精度的位置、速度和姿態(tài)信息。在城市車載導(dǎo)航中,考慮到成本問題,一般選取微機(jī)電系統(tǒng)(Micro Electro Mechanical System,MEMS)-慣性導(dǎo)航系統(tǒng)(Inertial Navigation System,INS)與全球定位系統(tǒng)(Global Positioning System,GPS)進(jìn)行組合定位。
受制作工藝的影響, MEMS-INS精度較低,其誤差隨時(shí)間積累嚴(yán)重。普通GPS接收機(jī)在衛(wèi)星可見性較好的情況下能提供較為準(zhǔn)確的位置、速度信息,來修正MEMS-INS的定位誤差,但卻無法準(zhǔn)確修正MEMS-INS姿態(tài)誤差。因此,需要對(duì)MEMS-INS的姿態(tài)誤差進(jìn)行修正,來提高車載動(dòng)態(tài)姿態(tài)測(cè)量的精度及可靠性。在與GPS組合的條件下一般采用多天線GPS測(cè)姿[1]方法和單天線GPS測(cè)姿方法[2],但前者存在系統(tǒng)結(jié)構(gòu)和算法比較復(fù)雜、成本較高、安裝難度大等理論和技術(shù)問題,后者在速度較低時(shí)定姿效果會(huì)受到影響,都具有一定的局限性。
同時(shí),在城市環(huán)境中由于建筑物的遮擋,GPS信號(hào)易發(fā)生中斷,此時(shí)MEMS-INS的快速漂移誤差會(huì)使MEMS-INS/GPS組合導(dǎo)航系統(tǒng)的定位性能迅速惡化[3]。因此,GPS信號(hào)中斷時(shí)MEMS-INS定位誤差的控制,也是車載GPS/MEMS-INS組合導(dǎo)航需要解決的重要問題。在GPS 中斷時(shí)的定位方法,其基本的類型主要有兩類。一是通過對(duì)MEMS/INS系統(tǒng)中的慣性傳感器誤差建模來防止中斷時(shí)導(dǎo)航性能惡化,例如設(shè)計(jì)了基于自適應(yīng)模糊模型的INS/GPS組合,能通過融合INS和GPS位置和速度數(shù)據(jù)來估計(jì)INS誤差并提升INS定位精度[4]。二是通過車輛運(yùn)動(dòng)約束來減少定位誤差,比如帶非完整約束的陸地車輛運(yùn)動(dòng)模型[5]。由于低成本MEMS-INS的誤差較大,并且容易受環(huán)境影響,因此采用神經(jīng)網(wǎng)絡(luò)等復(fù)雜非線性模型進(jìn)行INS誤差建模的方法,會(huì)存在計(jì)算量較大、實(shí)時(shí)性較差、模型環(huán)境適應(yīng)能力差等問題,因此采用車輛運(yùn)動(dòng)約束來控制MEMS-INS誤差的方法更具優(yōu)勢(shì)。
為了將以上兩個(gè)定姿和定位問題的解決相結(jié)合,又考慮到城市環(huán)境道路比較平坦,車輛運(yùn)動(dòng)模式也相對(duì)簡(jiǎn)單,如果能夠?qū)囕v運(yùn)動(dòng)狀態(tài)進(jìn)行準(zhǔn)確判斷[6],對(duì)車輛運(yùn)動(dòng)狀態(tài)的約束將會(huì)更加準(zhǔn)確,從而進(jìn)一步改進(jìn)MEMS-INS定姿性能和GPS中斷時(shí)的MEMS-INS定位誤差控制效果。本文針對(duì)上述問題,設(shè)計(jì)了一種基于動(dòng)態(tài)判斷的MEMS-INS /GPS車載導(dǎo)航性能改進(jìn)方法。
本文采用GPS/INS緊組合模式,用Kalman濾波算法以及反饋校正方式實(shí)現(xiàn)信息融合[7-8]。首先是MEMS慣性測(cè)量單元(Inertial Measurement Unit,IMU)實(shí)時(shí)輸出載體的陀螺儀和加速度計(jì)信息,然后INS解算模塊根據(jù)力學(xué)編排方程獲得實(shí)時(shí)的INS定位和定姿結(jié)果。其中位置的初始值由GPS定位結(jié)果提供,姿態(tài)的初始值由調(diào)平法加磁羅盤輔助獲得[9]。同時(shí)將GPS的偽距和偽距率作為觀測(cè)量對(duì)INS導(dǎo)航誤差進(jìn)行濾波,最后獲得的誤差估計(jì)來修正INS導(dǎo)航結(jié)果以獲得組合導(dǎo)航輸出,然后將修正后的導(dǎo)航結(jié)果反饋回INS解算模塊,再進(jìn)行下一時(shí)刻的解算和濾波。
其中,MEMS-IMU輸出的參數(shù)是加速度計(jì)和陀螺儀輸出的三軸加速度與角速度,分別表示為fb=[fbx,fby,fbz]和ωb=[ωbx,ωby,ωbz],b為載體坐標(biāo)系,該坐標(biāo)系X軸指向載體右側(cè),Y軸沿載體縱軸方向向前,Z軸垂直于載體豎直向上。本文采用帶非完整運(yùn)動(dòng)約束的陸地車輛運(yùn)動(dòng)模型,該模型假設(shè)車輛在行駛過程中,運(yùn)動(dòng)方向的法向和天向沒有速度,忽略地球自轉(zhuǎn)并將重力加速度設(shè)為恒定。利用該模型可以推出加速度計(jì)和陀螺儀輸出參數(shù)與車輛動(dòng)態(tài)的關(guān)系[10]:
(1)
(2)
式中,vf為車輛行駛速度,(θ,γ,ψ)為車輛俯仰角、橫滾角、航向角,g為重力常數(shù)。
2.1 總體設(shè)計(jì)
在第一章介紹的MEMS-INS/GPS組合流程基礎(chǔ)上,本文設(shè)計(jì)了GPS/INS組合模式和GPS中斷兩種模式,如圖1所示。
當(dāng)存在GPS信號(hào)時(shí),在緊組合模式中加入了動(dòng)態(tài)判斷模塊,利用IMU輸出數(shù)據(jù)判斷車輛當(dāng)前運(yùn)動(dòng)狀態(tài),分別是勻速直線(靜止)、變速直線以及轉(zhuǎn)彎。然后結(jié)合INS姿態(tài)解算結(jié)果和IMU輸出數(shù)據(jù),選擇相應(yīng)動(dòng)態(tài)的姿態(tài)約束公式獲得最終姿態(tài)角,并用其校正INS姿態(tài)誤差。
當(dāng)GPS中斷時(shí),則采用另一套GPS中斷定位模式。該模式中定姿方法與GPS/INS組合模式相同,而取代緊組合的定位方法則是在動(dòng)態(tài)判斷后利用不同動(dòng)態(tài)狀況選擇相應(yīng)運(yùn)動(dòng)模型進(jìn)行定位推算,最后將最終獲得的位置、速度、姿態(tài)約束結(jié)果反饋給INS解算模塊。

圖1 總體結(jié)構(gòu)示意圖Fig.1 The overall structure scheme
2.2 動(dòng)態(tài)判斷
本文將城市車輛基本動(dòng)態(tài)類型劃分為以下三種:1)勻速直線運(yùn)動(dòng)(包括靜止),該狀態(tài)下姿態(tài)角和速度基本不發(fā)生變化;2)變速直線運(yùn)動(dòng),該狀態(tài)下姿態(tài)角基本不發(fā)生變化,會(huì)有速度變化;3)轉(zhuǎn)彎運(yùn)動(dòng),該狀態(tài)下航向角有較大變化。通過式(1)、式(2),推出以上3種狀態(tài)下的慣性元件近似輸出如下:


通過以上不同動(dòng)態(tài)間的輸出差別,定義兩種動(dòng)態(tài)數(shù)據(jù)如下:
其中,tk為當(dāng)前離散時(shí)間,n為平滑統(tǒng)計(jì)的離散時(shí)間點(diǎn)數(shù)目。
這里的判斷條件有兩步,首先判斷勻速和變速;如果是變速,再判斷是直線或是轉(zhuǎn)彎運(yùn)動(dòng)。采用門限判斷的方法。其判斷條件如下:
上式表示了三種運(yùn)動(dòng)狀態(tài)的判斷方式,這里的門限λ1、λ2從原理上應(yīng)該是由加速度計(jì)和陀螺儀生成公式推出的,但是由于低成本慣導(dǎo)的噪聲較大而且難以預(yù)測(cè),因此這里是通過同一慣導(dǎo)的多次實(shí)驗(yàn)數(shù)據(jù)獲得。
2.3 不同動(dòng)態(tài)下的姿態(tài)約束和運(yùn)動(dòng)模型
當(dāng)車輛運(yùn)動(dòng)狀態(tài)確定后,需要選擇對(duì)應(yīng)狀態(tài)下的姿態(tài)約束來對(duì)INS定姿結(jié)果進(jìn)行修正。同時(shí)GPS中斷時(shí),也需要根據(jù)動(dòng)態(tài)判斷結(jié)果選擇對(duì)應(yīng)動(dòng)態(tài)的運(yùn)動(dòng)模型進(jìn)行定位。以下將介紹三種運(yùn)動(dòng)狀態(tài)下的姿態(tài)約束和運(yùn)動(dòng)模型。
1)勻速直線或靜止?fàn)顟B(tài)
航向角難以直接約束,但是該狀態(tài)下航向角基本不會(huì)發(fā)生變化:
ψ(ts)=ψ(ts-1)
其中,ts表示當(dāng)前路段采樣時(shí)間。
該動(dòng)態(tài)的特點(diǎn)是速度不變,因此運(yùn)動(dòng)模型采用地理坐標(biāo)系下的速度模型:
其中,RM和RN分別為子午圈半徑與卯酉圈半徑,λ,φ,h為車輛經(jīng)度、緯度、高度,(ve,vn,vu)為東向、北向、天向速度。
2)變速直線狀態(tài)
該狀態(tài)下的姿態(tài)角可以通過測(cè)得的組合速度進(jìn)行計(jì)算[11]:
γ=arccos(l·p)/(|l·p|)
其中,提升加速度矢量l為載體法向加速度an減去重力法向加速度gn,即l=an-gn,水平參考矢量p為重力矢量g乘以載體速度矢量vg,即p=g×vg。載體加速度信息可以通過速度的Kalman濾波模型估計(jì)得出。
當(dāng)車輛進(jìn)行變速直線運(yùn)動(dòng),其速度隨著路況會(huì)不斷變化,因此運(yùn)動(dòng)模型采用地理坐標(biāo)系下的加速運(yùn)動(dòng)模型:
由于實(shí)際車輛運(yùn)動(dòng)中加速度并非恒定,需要通過實(shí)時(shí)加速度修正模型。從式(2)推出非完整約束模型所獲得的加速度:
用該加速度實(shí)時(shí)修正模型中的加速度。
3)轉(zhuǎn)彎運(yùn)動(dòng)狀態(tài)
該狀態(tài)下俯仰角和橫滾角與變速直線運(yùn)動(dòng)相同。對(duì)于航向角,可通過式(1)進(jìn)行近似計(jì)算。航向角變化率:
最終計(jì)算航向角為:

其中,ti為姿態(tài)約束轉(zhuǎn)彎段起始時(shí)刻,tj為當(dāng)前時(shí)刻。
這里的運(yùn)動(dòng)模型采用式(8)的速度模型,但是由于轉(zhuǎn)彎時(shí)速度會(huì)發(fā)生變化,因此需要時(shí)刻修正模型中的速度,該速度由式(2)推出:
現(xiàn)場(chǎng)測(cè)試是在城市環(huán)境中驗(yàn)證GPS/MEMS-INS組合導(dǎo)航的性能。圖2給出了MEMS-IMU/GPS/磁羅盤組合導(dǎo)航實(shí)驗(yàn)系統(tǒng)組成框圖。其中GPS包括天線、射頻前端、高速數(shù)據(jù)采集器,一個(gè)MEMS-IMU提供慣性傳感器輸出參數(shù),磁羅盤提供初始定位航向角。這三套裝置通過串口與計(jì)算機(jī)連接,利用配套的MEMS-INS/GPS組合導(dǎo)航軟件進(jìn)行操作并輸出定位結(jié)果。
這里使用的GPS接收機(jī)是實(shí)驗(yàn)室自主研發(fā)的軟件接收機(jī)SatRecv。它的輸出頻率是25Hz,定位精度是5m,定速精度是0.2m/s。MEMS-IMU是BeijingStarnetoTechnologyCo.Ltd.(Beijing,China)生產(chǎn)的XW-IMU5220。它的陀螺零偏穩(wěn)定性是0.02 (°)/s,加速度計(jì)零偏穩(wěn)定性是8mg,輸出頻率是100 Hz。磁羅盤的定向精度是1.5°,輸出頻率是10 Hz。參考系統(tǒng)為NovAtel的SPAN,其輸出頻率是100 Hz,單點(diǎn)定位精度約1.5 m,定速精度是0.02 m/s,測(cè)姿精度俯仰角為0.05°,橫滾角0.05°,航向角0.01°。

圖2 GPS/INS組合導(dǎo)航實(shí)驗(yàn)系統(tǒng)Fig.2 The test GPS/INS integrated navigation system
本次現(xiàn)場(chǎng)測(cè)試路段為北京市奧林匹克森林公園附近,如圖3所示,實(shí)驗(yàn)時(shí)間約500 s,其中含有2個(gè)轉(zhuǎn)彎,行車過程中因?yàn)槁窙r原因會(huì)出現(xiàn)一些靜止情況。
在上述測(cè)試環(huán)境下,我們首先需要判斷整個(gè)過程的車輛運(yùn)動(dòng)狀態(tài)。圖4所示為本次測(cè)試中動(dòng)態(tài)判斷結(jié)果。我們每0.5 s判斷一次運(yùn)動(dòng)狀態(tài),這里的運(yùn)動(dòng)狀態(tài)分別是勻速直線(或靜止)狀態(tài)、變速直線狀態(tài)以及轉(zhuǎn)彎狀態(tài)。基于多次實(shí)驗(yàn)數(shù)據(jù),獲得的勻速與變速判斷門限值λ1為0.03g,直線與轉(zhuǎn)彎判斷門限值λ2為0.07 (°)/s。用實(shí)線代表動(dòng)態(tài)判斷結(jié)果,并利用參考系統(tǒng)的輸出速率和轉(zhuǎn)彎角速度作為動(dòng)態(tài)判斷依據(jù)。
從圖4可知,在真實(shí)行車過程中,基本不存在完全的勻速行駛狀態(tài),因此這里判斷的勻速直線運(yùn)動(dòng)都是靜止?fàn)顟B(tài)。通過速度、轉(zhuǎn)彎角速度與載體運(yùn)動(dòng)類型可以看出,基本上能正確判斷出靜止、直線和轉(zhuǎn)彎狀態(tài)。這里共有2個(gè)彎和2個(gè)長(zhǎng)靜止段,在350 s左右有個(gè)較為短暫的靜止部分。
為了證明姿態(tài)約束方法對(duì)定姿性能的提升,將INS定姿結(jié)果和采用姿態(tài)約束后的結(jié)果進(jìn)行對(duì)比,對(duì)比結(jié)果如圖5-圖7所示。
由圖5-圖7可看出,INS單獨(dú)定姿會(huì)產(chǎn)生較大的姿態(tài)積累誤差,導(dǎo)致定姿效果變差。在加入姿態(tài)約束后,姿態(tài)精度有了明顯提升,具體的精度統(tǒng)計(jì)結(jié)果見表1。

圖3 實(shí)驗(yàn)路線 (來自谷歌地球)Fig.3 Test trajectory (from Google Earth)

圖4 動(dòng)態(tài)判斷結(jié)果Fig.4 Results of vehicle dynamics identification

圖5 俯仰角測(cè)量結(jié)果對(duì)比Fig.5 Estimation result comparison of the pitch

圖6 橫滾角測(cè)量結(jié)果對(duì)比Fig.6 Estimation result comparison of the roll

圖7 航向角測(cè)量結(jié)果對(duì)比Fig.7 Estimation result comparison of the yaw

定姿方法姿態(tài)角誤差均方根/(°)俯仰角橫滾角航向角INS定姿6.354.567.61姿態(tài)約束1.510.771.37
表1展示了整個(gè)測(cè)試過程中采用姿態(tài)約束前后的定姿精度。可知,加入姿態(tài)約束能提升姿態(tài)角精度。最后獲得的姿態(tài)角相對(duì)于INS單獨(dú)定姿,俯仰角提升了76%,橫滾角提升了83%,航向角提升了82%。
為了驗(yàn)證GPS中斷時(shí)的定位性能,這里仿真4個(gè)GPS中斷的情況,如圖8所示。第一段包括變速直線和轉(zhuǎn)彎部分,中斷40 s,第二段包括變速直線部分,中斷45 s,第三段包括變速直線和轉(zhuǎn)彎部分,中斷50 s,第四段包括變速直線和靜止部分,中斷100 s。
圖9給出了存在GPS中斷時(shí)的定位路線結(jié)果。并對(duì)使用模型推算和INS單獨(dú)定位的導(dǎo)航結(jié)果進(jìn)行對(duì)比。

圖 8 仿真GPS中斷路線圖Fig.8 Trajectory with simulated GPS outages

圖9 仿真GPS中斷部分定位結(jié)果Fig.9 Estimation results of position during simulated GPS outages
由圖9可知,當(dāng)GPS中斷后,由INS單獨(dú)定位,定位結(jié)果漂移將會(huì)非常嚴(yán)重,而采用模型推算后能保證定位結(jié)果與參考路線基本一致。具體的定位精度對(duì)比由表2所示。
表 2 GPS中斷時(shí)定位精度對(duì)比
Tab.2 Comparison of position estimation accuracy comparison during GPS outages

方向測(cè)試段位置誤差均方根/mINS單獨(dú)定位模型推算東向157.979.572153.976.843182.246.104672.579.87北向172.854.20247.219.353631.488.334302.529.57天向118.537.21238.906.333223.875.90487.199.21
由表2和圖9可以看出,相對(duì)于INS單獨(dú)定位,模型推算能大幅度減小定位誤差,并保證在40 s內(nèi)定位誤差不超過10 m。尤其是存在靜止段時(shí),能在更長(zhǎng)的時(shí)間段內(nèi)保持較好的定位精度。
本文提出了基于動(dòng)態(tài)判斷的陸地車輛導(dǎo)航性能提升方法。該方法首先利用加速度計(jì)和陀螺儀輸出的數(shù)據(jù)進(jìn)行陸地車輛動(dòng)態(tài)判斷,然后選擇對(duì)應(yīng)的姿態(tài)約束來修正MEMS-INS姿態(tài)角。在GPS中斷時(shí),根據(jù)實(shí)時(shí)的動(dòng)態(tài)判斷結(jié)果選擇相應(yīng)的運(yùn)動(dòng)模型進(jìn)行導(dǎo)航狀態(tài)推算,從而有效控制MEMS-INS定位誤差。最后通過跑車實(shí)驗(yàn)證明,該方法有效改進(jìn)了MEMS-INS/GPS車載組合導(dǎo)航的性能。下一步可加入爬坡、下坡、爬坡轉(zhuǎn)彎的運(yùn)動(dòng)狀態(tài)的判斷和運(yùn)動(dòng)模型的設(shè)計(jì),進(jìn)一步滿足車輛運(yùn)動(dòng)環(huán)境的需求。
[1]Hwang D H, Sang H O, Sang J L, et al. Design of a low-cost attitude determination GPS/INS integrated navigation system[J]. GPS Solut, 2005, 9(4):294-311.
[2]王輝.GPS單天線測(cè)姿系統(tǒng)研究[D].南京:南京航空航天大學(xué),2005.
[3]Atia M M,Noureldin A,Korenberg M.Gaussian process regression approach for bridging GPS outages in integrated navigation systems[J]. Electronics Letters, 2011, 47(1):52-53.
[4]Noureldin A,El-Shafie A, El-Sheimy N. Adaptive neuro-fuzzy module for inertial navigation system/global positioning system integration utilising position and velocity updates with real-time cross-validation[J]. Iet Radar Sonar Navigation, 2007, 1(5):388-396.
[5]Brandt A, Gardner J F. Constrained navigation algorithms for strapdown inertial navigation systems with reduced set of sensors[C]// Proceedings of the American Control Conference, 1998(3):1848-1852.
[6]Wang J H, Gao Y. Land vehicle dynamics-aided inertial navigation[J]. Aerospace & Electronic Systems IEEE Transactions on, 2010, 46(4): 1638-1653.
[7]Li Y, Wang J, Rizos C,et al.Low-Cost Tightly Coupled GPS/INS Integration Based on a Nonlinear Kalman Filtering Design[J]. Institute of Navigation National Technical Meeting 2006:958-966.
[8]Petovello M. Real-time Integration of a Tactical-Grade IMU and GPS for High-Accuracy Positioning and Navigation[D]. Calgary :University of Calgary , 2003.
[9]翟海廷. 低成本慣導(dǎo)與單天線GPS高精度緊組合定位定姿技術(shù)[D].北京:北京航空航天大學(xué),2012
[10]Dissanayake G, Sukkarieh S, Nebot E, et al. The aiding of a low-cost strapdown inertial measurement unit using vehicle model constraints for land vehicle applications[J]. IEEE Transactions on Robotics & Automation, 2001, 17(5):731-747.
[11]翟海廷, 叢麗, 秦紅磊,等. 基于單天線的MEMS-INS/GPS組合定姿方法[J]. 計(jì)算機(jī)工程與設(shè)計(jì), 2012, 33(10):3999-4003..
Modified Method of Integrated Navigation Performance Based on Dynamics Identification
CONG Li,WANG Yinxin,LU Hui
(School of Electronic and Information Engineering,Beihang University,Beijing 100191, China)
For the problems of low-precision attitude determination performance and positioning performance during global positioning system (GPS) outages of micro electro mechanical system-inertial navigation system (MEMS-INS)/GPS integration in urban vehicle application,a method based on land vehicle dynamics was proposed. This method divided the vehicle dynamics into three types,and set up vehicle attitude constraints and motion model in each dynamics. Then, the vehicle dynamics by the INS sensors output was identified,and corresponding attitude constraints to modify the result of INS attitude determination was selected. During GPS outages, corresponding model was selected to control INS positioning error.Through the land vehicle experiments,it was indicated that the proposed methods could effectively modify MEMS-INS/GPS navigation performance.
inertial navigation system;dynamic identification; attitude constraint
2016-12-05
國(guó)家自然科學(xué)基金項(xiàng)目資助(61101077)
叢麗(1981— ),女,北京人,博士,講師,研究方向:衛(wèi)星導(dǎo)航、組合導(dǎo)航和戰(zhàn)術(shù)導(dǎo)航。E-mail:congli_hlj@163.com。
TN967.2
A
1008-1194(2017)02-0001-06