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

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

0 引言

導(dǎo)航是指將運動載體從甲地引導(dǎo)到乙地的技術(shù)。為了完成引導(dǎo)任務(wù)，首要問題是確定載體的即時位置；為了實現(xiàn)經(jīng)濟(jì)和快速導(dǎo)航，還需要測定載體的速度和姿態(tài)等運動參數(shù)，進(jìn)而引導(dǎo)運載體迅速精確地駛向既定目標(biāo)。所以導(dǎo)航的基本任務(wù)是確定載體的姿態(tài)、速度和位置等導(dǎo)航參數(shù)。

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

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

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

1 組合導(dǎo)航系統(tǒng)原理

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

圖1 組合導(dǎo)航系統(tǒng)原理圖Fig.1 Schematic diagram of integrated navigation system

2 組合導(dǎo)航融合算法模型

2.1 里程計輸出模型

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

(1)

設(shè)慣導(dǎo)系統(tǒng)確定的載體坐標(biāo)系為b系，則里程計輸出的位置增量在b系的投影為

(2)

(3)

(4)

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

(5)

其中

2.2 INS /OD狀態(tài)方程和量測方程

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

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

(6)

(8)

(9)

(10)

(11)

(12)

(13)

式中

M2=

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

M6=

若采用速度量測方式，構(gòu)造量測方程如下

(14)

(15)

考慮到平臺失準(zhǔn)角φn，有

忽略二階小量

(16)

故由此可得速度量測為

(17)

Zv=Hv·X+VD

(18)

Hv=

(19)

式中

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

2.3 Kalman濾波

2.3.1 系統(tǒng)離散化

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

(20)

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

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

(21)

其中，F(xiàn)k=F(tk)。

系統(tǒng)噪聲的離散化如下

(22)

2.3.2 故障檢測

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

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

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

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

(23)

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

(24)

可以得到故障檢測函數(shù)為

(25)

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

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

2.3.3序貫處理

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

(26)

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

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

3 試驗驗證

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

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

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

圖4 定位誤差對比結(jié)果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安裝角度和標(biāo)度因數(shù)估計誤差曲線

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

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

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

圖10 組合導(dǎo)航姿態(tài)估計曲線Fig.10 Integrated navigation attitude estimation curve

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

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

在本次實驗中，在4000～4600s內(nèi)給里程計2數(shù)據(jù)加入故障(故障值大?。?0)，再次驗證算法的有效性，故障卡方檢測值和融合結(jié)果如圖12所示。

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

從圖13可以看出，雖然在4000～4600s內(nèi)里程計2異常，卡方檢測能夠及時檢測出故障，且在發(fā)現(xiàn)故障后，不進(jìn)行里程計2的量測更新，只利用里程計1的信息進(jìn)行量測更新，使定位結(jié)果能達(dá)到最優(yōu)；數(shù)據(jù)正常后，卡方檢測能同時識別出，故障能立即恢復(fù)。

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

4 結(jié)論

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