車載捷聯慣導雙里程計組合導航方法研究

郭楠楠，申亮亮，邵會兵，苑艷華

(北京控制與電子技術研究所，北京 100038)

0 引言

導航是指將運動載體從甲地引導到乙地的技術。為了完成引導任務，首要問題是確定載體的即時位置；為了實現經濟和快速導航，還需要測定載體的速度和姿態等運動參數，進而引導運載體迅速精確地駛向既定目標。所以導航的基本任務是確定載體的姿態、速度和位置等導航參數。

在軍事領域中，現代地面戰爭要求載車能夠在廣闊的作戰地域內靈活機動，并為武器發射提供精確的地理坐標和水平北向基準，做到停車就打或者邊走邊打，提升車載武器平臺的機動性，提高生存能力和快速反應能力，保證精確打擊精度，提升作戰效能。隨著衛星導航系統的逐步完善，由于衛星導航定位精度高且長期穩定性好，慣性/衛星組合導航技術很容易滿足車載導航系統動態定位和定向的要求。但是，衛星導航需要接收外部無線電信號，容易受到干擾甚至欺騙，在敏感地區和時間內載車不能完全依賴車載捷聯慣導系統和衛星進行組合導航[2]，各國的軍用車載導航系統都對衛星導航采用了用而不靠的原則。因此，研制成本低、精度高、準備時間短和自主性強的車載組合導航系統具有重要的軍事和現實意義。

作為軍事用途的車輛，自主性和隱蔽性至關重要，在系統構成上應優先采用“不接收任何信息，也不向外輻射任何信息”的導航子系統。車載捷聯慣導和車載里程計都具有不依賴外部設備和不受外部信號干擾的特點，所以該組合是一種理想的車載導航方案。在目前公開的文獻中，車載捷聯慣導和車載里程計組合導航時，將里程計安裝在車底盤左側或右側位置，該方法雖然是自主式組合導航的有效手段，但由于安裝誤差的存在，里程計信息未能真實反映車體中心的實際運行狀態，且里程計量測信息誤差會引入未知的導航誤差，從而影響系統組合導航的定位精度。

本文針對軍事車輛對自主導航的需求背景和高精度精確打擊的要求，系統地提出了一種由車載捷聯慣導和車載雙里程計構成的高精度全自主車載組合導航方案。該方案分別在車底盤左側和右側位置各安裝一個里程計，將雙里程計信息作為量測量設計組合導航融合算法，并采用序貫處理的方式進行Kalman濾波組合導航以減少計算量；且在導航過程中對雙里程計輸出信息進行χ2檢測，有效剔除了車體作大轉彎運行或有外部干擾時引起的里程計信息異常，確保了里程計信息的有效性，且相對于單里程計具有容錯功能，從而保證了系統的定位和定向精度。

1 組合導航系統原理

在算法設計上，一方面慣性測量單元(Inertial Measurement Unit，IMU)信息分別與兩路里程計信息組合解算出載體的速度、位置和姿態信息；另一方面，兩路里程計量測信息考慮安裝誤差變化后，通過構造速度/位置量測模塊，形成速度/位置量測Kalman濾波導航系統，再將兩路量測信息形成故障檢測系統，通過χ2檢測方案對系統量測值的可信度，即雙里程計的輸出進行對比判斷，判別應使用哪個里程計進行導航計算。在2個里程計均有效的情況下，采用序貫處理的方式使用雙里程計的量測值進行Kalman濾波處理。每隔一段時間對慣性導航系統的速度誤差、姿態誤差、位置誤差、加速度計零偏、陀螺漂移等誤差和里程計誤差參數進行閉環修正，實時輸出修正后的位置、速度和姿態信息，形成高精度的組合導航系統。具體原理如圖1所示。

圖1 組合導航系統原理圖Fig.1 Schematic diagram of integrated navigation system

2 組合導航融合算法模型

2.1 里程計輸出模型

里程計通常采用脈沖數方式輸出采樣時間間隔內的里程增量。定義里程計坐標系為m系，Y軸沿車體縱軸指向正前方，x軸沿車體橫軸指向右側，z軸垂直于X軸和Y軸并構成右手直角坐標系。里程計采樣時間間隔內輸出的脈沖數Ni在m系的投影寫成矢量形式為

(1)

設慣導系統確定的載體坐標系為b系，則里程計輸出的位置增量在b系的投影為

(2)

(3)

(4)

式中，ωd為隨機噪聲干擾，忽略二階小量，里程計輸出的b系位置增量可以寫成

(5)

其中

2.2 INS /OD狀態方程和量測方程

狀態量為27維，包含SINS的15維和6維里程計1誤差、6維里程計2誤差(刻度因子誤差、俯仰安裝誤差、航向安裝誤差以及桿臂誤差dL)，雙里程計刻度系數誤差和安裝誤差均視為隨機常數，狀態方程如下

dLk1,dKd2,dαθ2,dαψ2,dLk2]T

(6)

(8)

(9)

(10)

(11)

(12)

(13)

式中

M2=

M4=(vn×)(M1+M′)

M6=

若采用速度量測方式，構造量測方程如下

(14)

(15)

考慮到平臺失準角φn，有

忽略二階小量

(16)

故由此可得速度量測為

(17)

Zv=Hv·X+VD

(18)

Hv=

(19)

式中

同理可得另一路里程計量測方程。

2.3 Kalman濾波

2.3.1 系統離散化

Kalman濾波方程適合離散型的系統方程和量測方程式，將上述系統方程和量測方程式離散化為如下形式

(20)

連續狀態矩陣F(t)的計算周期Ts可大于慣性導航解算周期Δt，本方案中取值為0.01s，即濾波器的時間更新周期為0.01s。對計算出的矩陣F(t)進行離散化，即可得離散狀態轉移矩陣Φk/k-1為

Φk/k-1=eFTs≈I+FkTs

(21)

其中，Fk=F(tk)。

系統噪聲的離散化如下

(22)

2.3.2 故障檢測

組合導航系統的故障檢測是組合導航系統的可靠性保障，可以分為單機級故障檢測和系統級故障檢測。單機級的故障檢測是在傳感器出現明顯異常時發出故障信息，從而不去參與系統運算，已經在各傳感器中實現了故障實時監測；系統級的故障檢測方法，主要是避免因故障子系統對濾波器的影響，從而導致濾波發散的問題。在濾波中配備實時的故障檢測及隔離算法，一旦檢測到故障就必須對故障進行隔離。最后通過系統信息重構，快速實現剩余子系統重組，使整體系統不致因故障而失效，保證組合導航系統在任務期間的戰備完好性和可靠性。

在容錯組合導航系統中，必須實時地確定各量測信息的有效性，以便決定是否進行組合。這樣做的原因是防止任意導航子系統的故障污染到其他狀態，提高組合系統輸出的導航信息的可靠性。

針對組合導航系統的特點，采用一種殘差χ2檢驗法來確定系統量測信息的有效性。該方法并不確定造成故障的具體原因，而僅僅是實時地確定一組量測的有效性，因此十分適用于系統級的故障檢測及隔離。

Kalman濾波器在量測更新時的殘差為

(23)

當無故障發生時，Kalman濾波器的殘差rk是零均值高斯白噪聲，而其方差為

(24)

可以得到故障檢測函數為

(25)

其中，λk是服從自由度為m的χ2分布，即λk～χ2(m)，其中m為量測維數，則故障判定準則為：若λk>TD則判斷為有故障；若λk

預先設定的判斷門限TD可由誤警率查χ2分布表得到。當檢測發現某一時刻任意一個里程計信息故障，則認為該里程計數據異常，對當次量測作丟棄處理，只進行狀態更新而不進行量測更新和濾波修正。

2.3.3序貫處理

序貫處理將量測更新中對量測量的集中處理分散為對各組的順序處理，有效降低了計算量，且能很好地進行單個傳感器的故障識別，將雙里程計的量測方程寫成如下形式

(26)

序貫處理量測更新過程如圖2所示。

圖2 序貫處理流程示意圖Fig.2 Flow diagram of sequential processing

3 試驗驗證

采用激光陀螺捷聯慣導系統進行車載試驗驗證，慣導系統中陀螺精度為0.01(°)/h、加速度計精度為5×10-5g，系統自帶定位精度為2m(CEP)的GPS接收機，可作為位置考核基準。2個里程計在組合導航前經過預先標定，得到里程儀1的標度因數和安裝偏差角為KD1=0.036087、αθ1=0.5687919、αψ1=0.88269，里程儀2的標度因數和安裝偏差角為KD2=0.036337、αθ2=0.592625、αψ2=0.923592。

車載試驗持續時間約6600s，跑車路徑如圖3所示，圖4所示為融合算法和單里程計組合導航算法的定位誤差對比結果圖，圖5～圖11所示分別為融合算法的位置估計誤差曲線、陀螺漂移估計誤差曲線、加速度計零偏估計誤差曲線、里程計安裝角度和標度因數估計誤差曲線、里程計桿臂估計誤差曲線、組合導航姿態估計曲線和里程計數據故障檢測值。圖4和圖5的誤差為與GPS比較的結果，其中有些時刻因GPS數據無效而導致誤差較大。由圖4可以看出，采用融合算法后定位誤差整體減小到20m以內，最大處減小6m。

圖3 車載試驗行車路徑曲線Fig.3 Vehicle test route curve

圖4 定位誤差對比結果Fig.4 Positioning error comparison results

圖5 位置估計誤差曲線Fig.5 Position estimation error curve

圖6 陀螺漂移估計誤差曲線Fig.6 Error curve of gyro drift estimation

圖7 加速度計零偏估計誤差曲線Fig.7 Error curve of accelerometer bias estimation

(a) 里程計1安裝角度和標度因數估計誤差曲線

(b) 里程計2安裝角度和標度因數估計誤差曲線

(a) 里程計1桿臂估計誤差曲線

(b) 里程計2桿臂估計誤差曲線

圖10 組合導航姿態估計曲線Fig.10 Integrated navigation attitude estimation curve

(a) 里程計1數據故障檢測值

(b) 里程計2數據故障檢測值圖11 里程計1/2數據故障檢測值Fig.11 Data fault detection value of odometer 1/2

在本次實驗中，在4000～4600s內給里程計2數據加入故障(故障值大小：50)，再次驗證算法的有效性，故障卡方檢測值和融合結果如圖12所示。

圖12 里程計1數據故障檢測值Fig.12 Data fault detection value of odometer 1

從圖13可以看出，雖然在4000～4600s內里程計2異常，卡方檢測能夠及時檢測出故障，且在發現故障后，不進行里程計2的量測更新，只利用里程計1的信息進行量測更新，使定位結果能達到最優；數據正常后，卡方檢測能同時識別出，故障能立即恢復。

圖13 里程計2加入異常數據故障檢測值Fig.13 Chi-square test results of odometer 2 with abnormal data fault detection value

4 結論

本文將單位時間內慣導解算位置增量和雙里程計航位推算位置增量之差作為量測值，設計了27維的捷聯慣導/雙里程儀組合導航濾波算法。采用序貫處理的Kalman濾波融合算法有效減少了計算量，且雙里程計具有容錯功能，可提高系統的穩定性和可靠性，工程可實現性強。車載試驗結果表明，該組合算法正確有效，定位結果能得到最優融合，可以達到理想的定位和定向精度。