基于運動約束輔助的車載組合導航算法

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

(1.中國電子科技集團公司第五十四研究所， 石家莊 050081；2.衛星導航系統與裝備技術國家重點實驗室， 石家莊 050081；3.中國人民解放軍63961部隊， 北京 100012；4.陸軍航空兵學院 陸軍航空兵研究所， 北京 101121)

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

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

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

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

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

1 基于運動約束輔助的組合濾波器設計

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

圖1 基于運動約束輔助的組合導航系統原理圖

1.1 狀態方程的建立

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

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

(1)

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

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

(2)

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

(3)

1.2 量測方程的建立

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

(4)

(5)

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

Μ1φ+M2δVn+M3α

(6)

(7)

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

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

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

當載車處于停止狀態時，由運動約束條件提供的ZUPT觀測值即可表示為

(8)

根據觀測值Z0，可很容易得到相應的量測矩陣H0：

(9)

1.3 零速檢測方法

零速檢測方法，主要是對IMU原始測量數據進行檢測，同時將慣導系統輸出的速度信息作為參考，綜合判斷車輛的行駛狀態，具體步驟如下：

1) 在車輛初始對準階段，采集1～2 min的IMU原始靜態數據，根據下式計算陀螺閾值λg、加計閾值λa，得到的閾值也可根據車輛的實際動態環境做適量的調整。

(10)

式中，|ωi|表示第i(i=1,…,N)次測量得到的陀螺角增量模值，|vi|表示第i(i=1,…,N)次測量得到的加計速度增量模值，g0表示當地重力分量的速度增量。

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

(11)

λv為速度參考閾值，考慮車停止狀態下的場景，通常選取：λv≤0.5 m/s。

2 動態跑車試驗與分析

車載組合導航測試系統由高精度光纖慣組、單位自行研制的多頻多模組合導航接收機組成。高精度光纖慣組的陀螺隨機漂移為0.02 °/h，加計零偏穩定性為50 μg。

試驗地點選擇在天津市，試驗時間約為5 300 s，以高精度慣組與組合導航接收機的組合結果作為參考基準，通過事后人為增加中斷信號以及信號中斷時間，測試長時間無衛導信號場景下，速度約束輔助信息對組合導航精度的影響。

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

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

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

圖3 車輛行駛軌跡

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

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

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

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

表1 中斷信號場景不同組合導航類型下的導航精度統計

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

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

3 結論

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