基于運(yùn)動約束輔助的車載組合導(dǎo)航算法

王立兵，周 俊，劉鵬飛，智奇楠，賈瑞才

(1.中國電子科技集團(tuán)公司第五十四研究所， 石家莊 050081；2.衛(wèi)星導(dǎo)航系統(tǒng)與裝備技術(shù)國家重點(diǎn)實(shí)驗(yàn)室， 石家莊 050081；3.中國人民解放軍63961部隊(duì)， 北京 100012；4.陸軍航空兵學(xué)院 陸軍航空兵研究所， 北京 101121)

現(xiàn)代意義的地面戰(zhàn)爭，要求移動車載系統(tǒng)能夠在電磁環(huán)境極為復(fù)雜、惡劣的地域環(huán)境下靈活機(jī)動，并為武器發(fā)射平臺提供精確的地理坐標(biāo)和姿態(tài)基準(zhǔn)，要求能夠做到邊走邊打、停車就打。這就對車載導(dǎo)航系統(tǒng)提出了更高的要求[1]。傳統(tǒng)意義的車載導(dǎo)航系統(tǒng)通常由慣性導(dǎo)航系統(tǒng)(INS)和衛(wèi)星定位系統(tǒng)(GNSS)組成。但是在城市高樓區(qū)、林蔭道、隧道內(nèi)等復(fù)雜環(huán)境下GNSS信號會嚴(yán)重丟失甚至中斷，慣性導(dǎo)航系統(tǒng)定位精度、車載組合濾波器的估計(jì)精度都會嚴(yán)重下降。文獻(xiàn)[2-3]給出了車輛運(yùn)動學(xué)約束輔助下的組合導(dǎo)航算法，當(dāng)車輛停止時濾波器同樣會出現(xiàn)發(fā)散的現(xiàn)象；文獻(xiàn)[4-5]推導(dǎo)了航位推算算法和航位推算誤差，在增加了里程計(jì)的前提下提高了慣性導(dǎo)航精度；文獻(xiàn)[6]針對車輛的動態(tài)檢測結(jié)果提出了混合濾波算法，來降低慣性導(dǎo)航誤差，但是文獻(xiàn)中的非線性濾波工程實(shí)際應(yīng)用較為困難，計(jì)算復(fù)雜度較高；文獻(xiàn)[7-9]針對典型城區(qū)環(huán)境，論述了車載運(yùn)動約束導(dǎo)航算法的具體實(shí)現(xiàn)過程，但是沒有將零度修正和動態(tài)零速修正相互結(jié)合起來；

傳統(tǒng)的零速修正技術(shù)(Zero-velocity Update，ZUPT)要求車輛每隔一定時間停車一次，這樣就降低了車輛的靈活機(jī)動性能，同時車輛的停車狀態(tài)不好判斷；動態(tài)零速修正技術(shù)(DZUPT:Dynamic Zero-velocity Update)則要求車輛處于行駛狀態(tài)下，當(dāng)車輛停止時DZUPT很有可能會導(dǎo)致組合濾波器發(fā)散、估計(jì)精度下降。

現(xiàn)有的提高慣性導(dǎo)航精度的方法有慣導(dǎo)/里程計(jì)組合的航位推算算法，但是該算法引入了額外的觀測信息，里程計(jì)的車輛安裝和在線校正均不利于機(jī)動性較強(qiáng)的載車場景使用；另外就是利用地面車輛現(xiàn)有的側(cè)向和天向約束信息，但是現(xiàn)有文獻(xiàn)中均沒有將速度約束的零速修正和動態(tài)零速修正信息相結(jié)合起來，對零速點(diǎn)的檢測也沒有工程上較為簡便可行的方法；

本文采用零速修正與動態(tài)零速修正相結(jié)合的運(yùn)動約束輔助方式，在GNSS信號長時間丟失的場景下，可為車輛在行駛、停止兩種運(yùn)行狀態(tài)下，提供可靠、有效的約束輔助信息，保證組合導(dǎo)航系統(tǒng)的精度。

文章給出了一種簡單的適用于工程應(yīng)用的零速檢測方法，在考慮了桿臂效應(yīng)、安裝偏差角的前提下給出了組合濾波器模型。最后的跑車試驗(yàn)驗(yàn)證了方法的有效性。

1 基于運(yùn)動約束輔助的組合濾波器設(shè)計(jì)

基于運(yùn)動約束的組合導(dǎo)航系統(tǒng)框圖如圖1所示。當(dāng)GNSS信號的觀測質(zhì)量較好時，組合濾波器可利用GNSS提供的偽距、偽距率信息進(jìn)行量測更新；當(dāng)GNSS信號較差甚至無信號時，此時可充分利用慣性導(dǎo)航系統(tǒng)提供的導(dǎo)航信息，首先通過零速檢測模塊，判斷出車輛行駛的狀態(tài)(車輛行駛中或者處于停止?fàn)顟B(tài))，然后構(gòu)造相應(yīng)的運(yùn)動約束信息，為組合濾波器提供有效的觀測。

圖1 基于運(yùn)動約束輔助的組合導(dǎo)航系統(tǒng)原理圖

1.1 狀態(tài)方程的建立

系統(tǒng)狀態(tài)方程中選取22維的狀態(tài)量，分別為三維姿態(tài)角誤差φ、三維速度誤差δv、三維位置誤差δp、三維陀螺零偏εb、三維加計(jì)偏置▽、俯仰安裝偏差角αθ、航向安裝偏差角αψ、INS中心到GNSS天線中心的三維桿臂誤差δl、GNSS接收機(jī)鐘差δtu以及GNSS接收機(jī)鐘漂δtru：

X=[φδvδpεb▽αθαψδlδtuδtru]T

(1)

狀態(tài)方程中的姿態(tài)誤差方程、速度誤差方程以及位置誤差方程可由INS誤差方程得到，在此不再贅述。

陀螺零偏和加計(jì)偏置建模為一階馬爾科夫過程和白噪聲的組合。

(2)

使用車輛運(yùn)動約束輔助信息時，還應(yīng)考慮到慣導(dǎo)系統(tǒng)在車輛安裝時與衛(wèi)導(dǎo)天線之間的桿臂效應(yīng)。實(shí)際上，在系統(tǒng)運(yùn)行過程中，桿臂會隨著系統(tǒng)的震動或者形變而變化。通常情況下，桿臂誤差一般是由于系統(tǒng)機(jī)械部件的物理形變引起的，這些物理形變基本上都是低頻的，因此實(shí)際應(yīng)用中，將桿臂誤差建模為時間常數(shù)為無窮大的隨機(jī)常數(shù)過程，滿足以下微分方程：

(3)

1.2 量測方程的建立

1) DZUPT條件下量測方程的建立。車體坐標(biāo)系速度Vm可表示為

(4)

(5)

對式(4)進(jìn)行微分，可得：

Μ1φ+M2δVn+M3α

(6)

(7)

式中，M1(1,×)表示矩陣M1的第一行，M3(1,3)表示矩陣M3的第1行3列的元素；

針對車輛行進(jìn)過程中出現(xiàn)的側(cè)滑、跳躍現(xiàn)象對零速修正帶來的影響這一問題，本算法的濾波器中有偽距/偽距率提供的GNSS原始測量信息和運(yùn)動約束構(gòu)造的輔助信息，當(dāng)GNSS信號較好時，濾波器中GNSS觀測信息會對速度約束信息進(jìn)行很好的修正作用，側(cè)滑、跳躍現(xiàn)象不會對對載體的精度產(chǎn)生較大影響；

2) ZUPT條件下量測方程的建立

當(dāng)載車處于停止?fàn)顟B(tài)時，由運(yùn)動約束條件提供的ZUPT觀測值即可表示為

(8)

根據(jù)觀測值Z0，可很容易得到相應(yīng)的量測矩陣H0：

(9)

1.3 零速檢測方法

零速檢測方法，主要是對IMU原始測量數(shù)據(jù)進(jìn)行檢測，同時將慣導(dǎo)系統(tǒng)輸出的速度信息作為參考，綜合判斷車輛的行駛狀態(tài)，具體步驟如下：

1) 在車輛初始對準(zhǔn)階段，采集1～2 min的IMU原始靜態(tài)數(shù)據(jù)，根據(jù)下式計(jì)算陀螺閾值λg、加計(jì)閾值λa，得到的閾值也可根據(jù)車輛的實(shí)際動態(tài)環(huán)境做適量的調(diào)整。

(10)

式中，|ωi|表示第i(i=1,…,N)次測量得到的陀螺角增量模值，|vi|表示第i(i=1,…,N)次測量得到的加計(jì)速度增量模值，g0表示當(dāng)?shù)刂亓Ψ至康乃俣仍隽俊?/p>

2) 將采集得到的IMU原始數(shù)據(jù)與λg、λa進(jìn)行比較，在此引入速度參考，得到可以進(jìn)行零速修正的零速判斷準(zhǔn)則：

(11)

λv為速度參考閾值，考慮車停止?fàn)顟B(tài)下的場景，通常選?。害藇≤0.5 m/s。

2 動態(tài)跑車試驗(yàn)與分析

車載組合導(dǎo)航測試系統(tǒng)由高精度光纖慣組、單位自行研制的多頻多模組合導(dǎo)航接收機(jī)組成。高精度光纖慣組的陀螺隨機(jī)漂移為0.02 °/h，加計(jì)零偏穩(wěn)定性為50 μg。

試驗(yàn)地點(diǎn)選擇在天津市，試驗(yàn)時間約為5 300 s，以高精度慣組與組合導(dǎo)航接收機(jī)的組合結(jié)果作為參考基準(zhǔn)，通過事后人為增加中斷信號以及信號中斷時間，測試長時間無衛(wèi)導(dǎo)信號場景下，速度約束輔助信息對組合導(dǎo)航精度的影響。

針對測試軌跡共設(shè)置了八處人為中斷，中斷時刻以及中斷時間間隔如圖2所示，信號中斷時間間隔范圍為300～500 s，中斷過程中包括了車輛行進(jìn)中的所有狀態(tài)，包括停車等待紅燈、直線行駛、轉(zhuǎn)彎行駛以及繞圈行駛等。

圖2 信號中斷時刻及中斷時間間隔

試驗(yàn)軌跡如圖3所示，可以看出：在有速度約束輔助信息的情況下，中斷時刻的軌跡圖與基準(zhǔn)軌跡重合度很高，零速檢測到的車輛停止時的軌跡也在基準(zhǔn)軌跡以內(nèi)；在無速度約束輔助信息的情況下，中斷時刻的軌跡圖已嚴(yán)重偏離基準(zhǔn)軌跡，特別是中斷二時的軌跡圖。

圖3 車輛行駛軌跡

圖4給出了中斷信號場景速度約束輔助下的組合導(dǎo)航定位測速誤差曲線，可以看到單個方向絕對位置誤差不超過10 m；單個方向上的絕對速度誤差不超過0.25 m/s；圖5給出了中斷信號場景在無速度約束輔助下的組合導(dǎo)航定位測速精度，從圖中可以看出導(dǎo)航精度要差很多，單向絕對位置誤差在200 m以內(nèi)，單向絕對速度誤差在1 m/s以內(nèi)。

圖4 中斷信號場景速度約束輔助下的組合導(dǎo)航誤差曲線

圖5 中斷信號場景無速度約束輔助下的組合導(dǎo)航誤差曲線

表1給出了中斷信號場景不同組合導(dǎo)航類型下的導(dǎo)航精度統(tǒng)計(jì)表，中斷信號場景下，運(yùn)動約束輔助下的組合導(dǎo)航水平定位精度可保證在2.7 m以內(nèi)，垂直定位精度在1.8 m以內(nèi)，水平測速精度在0.05 m/s以內(nèi)，垂直測速精度在0.02 m/s以內(nèi)；在無速度約束輔助信息的情況下，組合導(dǎo)航精度較差，其中水平定位精度要差很多。

表1 中斷信號場景不同組合導(dǎo)航類型下的導(dǎo)航精度統(tǒng)計(jì)

圖6給出了車輛行駛速度與檢測到的零速時刻與車輛行駛速度對照圖，從圖中可以看出，零速檢測方法檢測到的零速時刻很好地反應(yīng)了車輛處于停止時的時刻。

圖6 零速檢測方法檢測到零速時刻與車輛行駛速度

3 結(jié)論

采用ZUPT與DZUPT相結(jié)合的運(yùn)動約束輔助方式，在考慮慣導(dǎo)系統(tǒng)桿臂誤差及安裝誤差角的前提下，給出了相應(yīng)的組合濾波模型，并進(jìn)行了動態(tài)跑車試驗(yàn)。試驗(yàn)結(jié)果表明，在GNSS信號較差甚至無GNSS信號的路況場景下，零速檢測方法均能有效判斷出車輛的行駛狀態(tài)，給出相應(yīng)的運(yùn)動約束輔助信息，保證車載組合導(dǎo)航系統(tǒng)的精度。