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

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

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

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

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

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

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

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

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

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

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

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

X=[φδvδpεb▽?duì)力圈力爪膌δtuδtru]T

(1)

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

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

(2)

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

(3)

1.2 量測(cè)方程的建立

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

(4)

(5)

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

Μ1φ+M2δVn+M3α

(6)

(7)

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

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

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

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

(8)

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

(9)

1.3 零速檢測(cè)方法

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

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

(10)

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

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

(11)

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

2 動(dòng)態(tài)跑車(chē)試驗(yàn)與分析

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

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

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

圖2 信號(hào)中斷時(shí)刻及中斷時(shí)間間隔

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

圖3 車(chē)輛行駛軌跡

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

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

圖5 中斷信號(hào)場(chǎng)景無(wú)速度約束輔助下的組合導(dǎo)航誤差曲線(xiàn)

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

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

圖6給出了車(chē)輛行駛速度與檢測(cè)到的零速時(shí)刻與車(chē)輛行駛速度對(duì)照?qǐng)D，從圖中可以看出，零速檢測(cè)方法檢測(cè)到的零速時(shí)刻很好地反應(yīng)了車(chē)輛處于停止時(shí)的時(shí)刻。

圖6 零速檢測(cè)方法檢測(cè)到零速時(shí)刻與車(chē)輛行駛速度

3 結(jié)論

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