叢 麗，王音心，路 輝

(北京航空航天大學(xué)電子信息工程學(xué)院，北京 100191)

基于動態(tài)判斷的組合導(dǎo)航性能改進(jìn)方法

叢 麗，王音心，路 輝

(北京航空航天大學(xué)電子信息工程學(xué)院，北京 100191)

針對微機(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中斷時的定位性能精度較差的問題，提出了一種基于動態(tài)判斷的組合導(dǎo)航性能改進(jìn)方法。該方法將車輛的動態(tài)分為三種，并設(shè)置了車輛在各個動態(tài)下的姿態(tài)約束和運動模型，然后利用慣性元件的輸出判斷出當(dāng)前動態(tài)，并選擇對應(yīng)動態(tài)的姿態(tài)約束實時修正MEMS-INS定姿結(jié)果。當(dāng)GPS中斷時，則選擇相應(yīng)的運動模型進(jìn)行定位推算，以便控制MEMS-INS定位誤差。通過實驗證明，該方法有效改進(jìn)了MEMS-INS/GPS車載組合導(dǎo)航的性能。

慣性導(dǎo)航系統(tǒng)；動態(tài)判斷；姿態(tài)約束；運動模型

0 引言

車載導(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精度較低，其誤差隨時間積累嚴(yán)重。普通GPS接收機(jī)在衛(wèi)星可見性較好的情況下能提供較為準(zhǔn)確的位置、速度信息，來修正MEMS-INS的定位誤差，但卻無法準(zhǔn)確修正MEMS-INS姿態(tài)誤差。因此，需要對MEMS-INS的姿態(tài)誤差進(jìn)行修正，來提高車載動態(tài)姿態(tài)測量的精度及可靠性。在與GPS組合的條件下一般采用多天線GPS測姿[1]方法和單天線GPS測姿方法[2]，但前者存在系統(tǒng)結(jié)構(gòu)和算法比較復(fù)雜、成本較高、安裝難度大等理論和技術(shù)問題，后者在速度較低時定姿效果會受到影響，都具有一定的局限性。

同時，在城市環(huán)境中由于建筑物的遮擋，GPS信號易發(fā)生中斷，此時MEMS-INS的快速漂移誤差會使MEMS-INS/GPS組合導(dǎo)航系統(tǒng)的定位性能迅速惡化[3]。因此，GPS信號中斷時MEMS-INS定位誤差的控制，也是車載GPS/MEMS-INS組合導(dǎo)航需要解決的重要問題。在GPS 中斷時的定位方法，其基本的類型主要有兩類。一是通過對MEMS/INS系統(tǒng)中的慣性傳感器誤差建模來防止中斷時導(dǎo)航性能惡化，例如設(shè)計了基于自適應(yīng)模糊模型的INS/GPS組合，能通過融合INS和GPS位置和速度數(shù)據(jù)來估計INS誤差并提升INS定位精度[4]。二是通過車輛運動約束來減少定位誤差，比如帶非完整約束的陸地車輛運動模型[5]。由于低成本MEMS-INS的誤差較大，并且容易受環(huán)境影響，因此采用神經(jīng)網(wǎng)絡(luò)等復(fù)雜非線性模型進(jìn)行INS誤差建模的方法，會存在計算量較大、實時性較差、模型環(huán)境適應(yīng)能力差等問題，因此采用車輛運動約束來控制MEMS-INS誤差的方法更具優(yōu)勢。

為了將以上兩個定姿和定位問題的解決相結(jié)合，又考慮到城市環(huán)境道路比較平坦，車輛運動模式也相對簡單，如果能夠?qū)囕v運動狀態(tài)進(jìn)行準(zhǔn)確判斷[6]，對車輛運動狀態(tài)的約束將會更加準(zhǔn)確，從而進(jìn)一步改進(jìn)MEMS-INS定姿性能和GPS中斷時的MEMS-INS定位誤差控制效果。本文針對上述問題，設(shè)計了一種基于動態(tài)判斷的MEMS-INS /GPS車載導(dǎo)航性能改進(jìn)方法。

1 MEMS-INS/GPS組合原理與慣性元件輸出

本文采用GPS/INS緊組合模式，用Kalman濾波算法以及反饋校正方式實現(xiàn)信息融合[7-8]。首先是MEMS慣性測量單元(Inertial Measurement Unit,IMU)實時輸出載體的陀螺儀和加速度計信息，然后INS解算模塊根據(jù)力學(xué)編排方程獲得實時的INS定位和定姿結(jié)果。其中位置的初始值由GPS定位結(jié)果提供，姿態(tài)的初始值由調(diào)平法加磁羅盤輔助獲得[9]。同時將GPS的偽距和偽距率作為觀測量對INS導(dǎo)航誤差進(jìn)行濾波，最后獲得的誤差估計來修正INS導(dǎo)航結(jié)果以獲得組合導(dǎo)航輸出，然后將修正后的導(dǎo)航結(jié)果反饋回INS解算模塊，再進(jìn)行下一時刻的解算和濾波。

其中，MEMS-IMU輸出的參數(shù)是加速度計和陀螺儀輸出的三軸加速度與角速度，分別表示為fb=[fbx,fby,fbz]和ωb=[ωbx,ωby,ωbz]，b為載體坐標(biāo)系，該坐標(biāo)系X軸指向載體右側(cè)，Y軸沿載體縱軸方向向前，Z軸垂直于載體豎直向上。本文采用帶非完整運動約束的陸地車輛運動模型，該模型假設(shè)車輛在行駛過程中，運動方向的法向和天向沒有速度，忽略地球自轉(zhuǎn)并將重力加速度設(shè)為恒定。利用該模型可以推出加速度計和陀螺儀輸出參數(shù)與車輛動態(tài)的關(guān)系[10]：

(1)

(2)

式中，vf為車輛行駛速度，(θ,γ,ψ)為車輛俯仰角、橫滾角、航向角，g為重力常數(shù)。

2 基于動態(tài)判斷的車載組合性能改進(jìn)方法

2.1 總體設(shè)計

在第一章介紹的MEMS-INS/GPS組合流程基礎(chǔ)上，本文設(shè)計了GPS/INS組合模式和GPS中斷兩種模式，如圖1所示。

當(dāng)存在GPS信號時，在緊組合模式中加入了動態(tài)判斷模塊，利用IMU輸出數(shù)據(jù)判斷車輛當(dāng)前運動狀態(tài)，分別是勻速直線(靜止)、變速直線以及轉(zhuǎn)彎。然后結(jié)合INS姿態(tài)解算結(jié)果和IMU輸出數(shù)據(jù)，選擇相應(yīng)動態(tài)的姿態(tài)約束公式獲得最終姿態(tài)角，并用其校正INS姿態(tài)誤差。

當(dāng)GPS中斷時，則采用另一套GPS中斷定位模式。該模式中定姿方法與GPS/INS組合模式相同，而取代緊組合的定位方法則是在動態(tài)判斷后利用不同動態(tài)狀況選擇相應(yīng)運動模型進(jìn)行定位推算，最后將最終獲得的位置、速度、姿態(tài)約束結(jié)果反饋給INS解算模塊。

圖1 總體結(jié)構(gòu)示意圖Fig.1 The overall structure scheme

2.2 動態(tài)判斷

本文將城市車輛基本動態(tài)類型劃分為以下三種：1)勻速直線運動(包括靜止)，該狀態(tài)下姿態(tài)角和速度基本不發(fā)生變化；2)變速直線運動，該狀態(tài)下姿態(tài)角基本不發(fā)生變化，會有速度變化；3)轉(zhuǎn)彎運動，該狀態(tài)下航向角有較大變化。通過式(1)、式(2)，推出以上3種狀態(tài)下的慣性元件近似輸出如下：

通過以上不同動態(tài)間的輸出差別，定義兩種動態(tài)數(shù)據(jù)如下：

其中，tk為當(dāng)前離散時間，n為平滑統(tǒng)計的離散時間點數(shù)目。

這里的判斷條件有兩步，首先判斷勻速和變速；如果是變速，再判斷是直線或是轉(zhuǎn)彎運動。采用門限判斷的方法。其判斷條件如下：

上式表示了三種運動狀態(tài)的判斷方式，這里的門限λ1、λ2從原理上應(yīng)該是由加速度計和陀螺儀生成公式推出的，但是由于低成本慣導(dǎo)的噪聲較大而且難以預(yù)測，因此這里是通過同一慣導(dǎo)的多次實驗數(shù)據(jù)獲得。

2.3 不同動態(tài)下的姿態(tài)約束和運動模型

當(dāng)車輛運動狀態(tài)確定后，需要選擇對應(yīng)狀態(tài)下的姿態(tài)約束來對INS定姿結(jié)果進(jìn)行修正。同時GPS中斷時，也需要根據(jù)動態(tài)判斷結(jié)果選擇對應(yīng)動態(tài)的運動模型進(jìn)行定位。以下將介紹三種運動狀態(tài)下的姿態(tài)約束和運動模型。

1)勻速直線或靜止?fàn)顟B(tài)

航向角難以直接約束，但是該狀態(tài)下航向角基本不會發(fā)生變化：

ψ(ts)=ψ(ts-1)

其中，ts表示當(dāng)前路段采樣時間。

該動態(tài)的特點是速度不變，因此運動模型采用地理坐標(biāo)系下的速度模型：

其中，RM和RN分別為子午圈半徑與卯酉圈半徑，λ,φ,h為車輛經(jīng)度、緯度、高度，(ve,vn,vu)為東向、北向、天向速度。

2)變速直線狀態(tài)

該狀態(tài)下的姿態(tài)角可以通過測得的組合速度進(jìn)行計算[11]：

γ=arccos(l·p)/(|l·p|)

其中，提升加速度矢量l為載體法向加速度an減去重力法向加速度gn，即l=an-gn，水平參考矢量p為重力矢量g乘以載體速度矢量vg，即p=g×vg。載體加速度信息可以通過速度的Kalman濾波模型估計得出。

當(dāng)車輛進(jìn)行變速直線運動，其速度隨著路況會不斷變化，因此運動模型采用地理坐標(biāo)系下的加速運動模型：

由于實際車輛運動中加速度并非恒定，需要通過實時加速度修正模型。從式(2)推出非完整約束模型所獲得的加速度：

用該加速度實時修正模型中的加速度。

3)轉(zhuǎn)彎運動狀態(tài)

該狀態(tài)下俯仰角和橫滾角與變速直線運動相同。對于航向角，可通過式(1)進(jìn)行近似計算。航向角變化率：

最終計算航向角為：

其中，ti為姿態(tài)約束轉(zhuǎn)彎段起始時刻，tj為當(dāng)前時刻。

這里的運動模型采用式(8)的速度模型，但是由于轉(zhuǎn)彎時速度會發(fā)生變化，因此需要時刻修正模型中的速度，該速度由式(2)推出:

3 實驗測試與驗證

現(xiàn)場測試是在城市環(huán)境中驗證GPS/MEMS-INS組合導(dǎo)航的性能。圖2給出了MEMS-IMU/GPS/磁羅盤組合導(dǎo)航實驗系統(tǒng)組成框圖。其中GPS包括天線、射頻前端、高速數(shù)據(jù)采集器，一個MEMS-IMU提供慣性傳感器輸出參數(shù)，磁羅盤提供初始定位航向角。這三套裝置通過串口與計算機(jī)連接，利用配套的MEMS-INS/GPS組合導(dǎo)航軟件進(jìn)行操作并輸出定位結(jié)果。

這里使用的GPS接收機(jī)是實驗室自主研發(fā)的軟件接收機(jī)SatRecv。它的輸出頻率是25Hz，定位精度是5m，定速精度是0.2m/s。MEMS-IMU是BeijingStarnetoTechnologyCo.Ltd.(Beijing,China)生產(chǎn)的XW-IMU5220。它的陀螺零偏穩(wěn)定性是0.02 (°)/s，加速度計零偏穩(wěn)定性是8mg,輸出頻率是100 Hz。磁羅盤的定向精度是1.5°，輸出頻率是10 Hz。參考系統(tǒng)為NovAtel的SPAN，其輸出頻率是100 Hz,單點定位精度約1.5 m，定速精度是0.02 m/s，測姿精度俯仰角為0.05°，橫滾角0.05°，航向角0.01°。

圖2 GPS/INS組合導(dǎo)航實驗系統(tǒng)Fig.2 The test GPS/INS integrated navigation system

本次現(xiàn)場測試路段為北京市奧林匹克森林公園附近，如圖3所示，實驗時間約500 s，其中含有2個轉(zhuǎn)彎，行車過程中因為路況原因會出現(xiàn)一些靜止情況。

在上述測試環(huán)境下，我們首先需要判斷整個過程的車輛運動狀態(tài)。圖4所示為本次測試中動態(tài)判斷結(jié)果。我們每0.5 s判斷一次運動狀態(tài)，這里的運動狀態(tài)分別是勻速直線(或靜止)狀態(tài)、變速直線狀態(tài)以及轉(zhuǎn)彎狀態(tài)。基于多次實驗數(shù)據(jù)，獲得的勻速與變速判斷門限值λ1為0.03g，直線與轉(zhuǎn)彎判斷門限值λ2為0.07 (°)/s。用實線代表動態(tài)判斷結(jié)果，并利用參考系統(tǒng)的輸出速率和轉(zhuǎn)彎角速度作為動態(tài)判斷依據(jù)。

從圖4可知，在真實行車過程中，基本不存在完全的勻速行駛狀態(tài)，因此這里判斷的勻速直線運動都是靜止?fàn)顟B(tài)。通過速度、轉(zhuǎn)彎角速度與載體運動類型可以看出，基本上能正確判斷出靜止、直線和轉(zhuǎn)彎狀態(tài)。這里共有2個彎和2個長靜止段，在350 s左右有個較為短暫的靜止部分。

為了證明姿態(tài)約束方法對定姿性能的提升，將INS定姿結(jié)果和采用姿態(tài)約束后的結(jié)果進(jìn)行對比，對比結(jié)果如圖5-圖7所示。

由圖5-圖7可看出，INS單獨定姿會產(chǎn)生較大的姿態(tài)積累誤差，導(dǎo)致定姿效果變差。在加入姿態(tài)約束后，姿態(tài)精度有了明顯提升，具體的精度統(tǒng)計結(jié)果見表1。

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

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

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

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

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

定姿方法姿態(tài)角誤差均方根/(°)俯仰角橫滾角航向角INS定姿6.354.567.61姿態(tài)約束1.510.771.37

表1展示了整個測試過程中采用姿態(tài)約束前后的定姿精度?？芍?，加入姿態(tài)約束能提升姿態(tài)角精度。最后獲得的姿態(tài)角相對于INS單獨定姿，俯仰角提升了76%，橫滾角提升了83%，航向角提升了82%。

為了驗證GPS中斷時的定位性能，這里仿真4個GPS中斷的情況，如圖8所示。第一段包括變速直線和轉(zhuǎn)彎部分，中斷40 s，第二段包括變速直線部分，中斷45 s，第三段包括變速直線和轉(zhuǎn)彎部分，中斷50 s，第四段包括變速直線和靜止部分，中斷100 s。

圖9給出了存在GPS中斷時的定位路線結(jié)果。并對使用模型推算和INS單獨定位的導(dǎo)航結(jié)果進(jìn)行對比。

圖 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單獨定位，定位結(jié)果漂移將會非常嚴(yán)重，而采用模型推算后能保證定位結(jié)果與參考路線基本一致。具體的定位精度對比由表2所示。

表 2 GPS中斷時定位精度對比

Tab.2 Comparison of position estimation accuracy comparison during GPS outages

方向測試段位置誤差均方根/mINS單獨定位模型推算東向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可以看出，相對于INS單獨定位，模型推算能大幅度減小定位誤差，并保證在40 s內(nèi)定位誤差不超過10 m。尤其是存在靜止段時，能在更長的時間段內(nèi)保持較好的定位精度。

4 結(jié)論

本文提出了基于動態(tài)判斷的陸地車輛導(dǎo)航性能提升方法。該方法首先利用加速度計和陀螺儀輸出的數(shù)據(jù)進(jìn)行陸地車輛動態(tài)判斷，然后選擇對應(yīng)的姿態(tài)約束來修正MEMS-INS姿態(tài)角。在GPS中斷時，根據(jù)實時的動態(tài)判斷結(jié)果選擇相應(yīng)的運動模型進(jìn)行導(dǎo)航狀態(tài)推算，從而有效控制MEMS-INS定位誤差。最后通過跑車實驗證明，該方法有效改進(jìn)了MEMS-INS/GPS車載組合導(dǎo)航的性能。下一步可加入爬坡、下坡、爬坡轉(zhuǎn)彎的運動狀態(tài)的判斷和運動模型的設(shè)計，進(jìn)一步滿足車輛運動環(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單天線測姿系統(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ī)工程與設(shè)計, 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

國家自然科學(xué)基金項目資助(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