基于強跟蹤濾波的捷聯(lián)慣導(dǎo)/里程計組合導(dǎo)航

陳雨，王健博，張凌東，曹全，陳世業(yè)，劉宇航

(1.北京航天發(fā)射技術(shù)研究所，北京 100076；2.火箭軍駐211廠軍事代表室，北京 100076)

0 引言

現(xiàn)代戰(zhàn)爭要求車載發(fā)射平臺具有自主定位定向能力，由于捷聯(lián)慣導(dǎo)系統(tǒng)具有自主性、隱蔽性、抗干擾性以及可以在全球范圍內(nèi)連續(xù)提供運載體全部運動參數(shù)的特點，車載發(fā)射平臺普遍配備捷聯(lián)慣導(dǎo)系統(tǒng)。但捷聯(lián)慣導(dǎo)系統(tǒng)導(dǎo)航誤差隨時間發(fā)散，通?？梢圆捎眯l(wèi)星導(dǎo)航或里程計與捷聯(lián)慣導(dǎo)系統(tǒng)進行組合以限制慣導(dǎo)系統(tǒng)導(dǎo)航誤差或限制其發(fā)散速度。衛(wèi)星導(dǎo)航信號容易受遮擋，且容易受電子信號干擾，限制了其在軍事上的使用。相對于衛(wèi)星導(dǎo)航，里程計與捷聯(lián)慣導(dǎo)組合導(dǎo)航仍具有自主性和抗干擾能力強的特點，因此，捷聯(lián)慣導(dǎo)/里程計組合導(dǎo)航在車載發(fā)射平臺中得到了廣泛的應(yīng)用。

捷聯(lián)慣導(dǎo)/里程計組合導(dǎo)航通常采用速度匹配方法[1-3]、位置匹配方法[4-6]或里程增量匹配方法[7-11]。速度匹配需要對里程計信號進行微分，當(dāng)里程計的脈沖當(dāng)量大時，造成很大的量化誤差，導(dǎo)致觀測量不準(zhǔn)確，影響濾波精度。位置匹配時，需要將航位推算的位置誤差擴充到組合導(dǎo)航狀態(tài)變量中，增加了狀態(tài)變量維數(shù)，造成計算量增加。為了克服速度匹配時量化誤差對觀測量的影響，文獻(xiàn)[7-10]提出采用導(dǎo)航系下的里程增量匹配方法，文獻(xiàn)[11]提出采用載體系下的里程增量匹配方法。通常捷聯(lián)慣導(dǎo)IMU(inertial measurement unit)敏感中心與里程計速度測量點之間存在一定長度的桿臂，當(dāng)載車有角運動時，捷聯(lián)慣導(dǎo)IMU敏感中心與里程計速度測量點的速度有差異。組合導(dǎo)航時，由桿臂引起的速度差異與組合導(dǎo)航系統(tǒng)的誤差傳播無關(guān)，如不消除就會影響組合導(dǎo)航精度。文獻(xiàn)[12]提出將桿臂擴充到捷聯(lián)慣導(dǎo)/里程計組合導(dǎo)航狀態(tài)中的桿臂在線補償算法以消除桿臂對組合導(dǎo)航的影響。

捷聯(lián)慣導(dǎo)/里程計組合導(dǎo)航通常將里程計刻度系數(shù)誤差建模為隨機常值，由于受輪胎胎壓、溫度、復(fù)雜路面特性等的影響，刻度系數(shù)實時變化。本文提出將強跟蹤濾波引入組合導(dǎo)航，以跟蹤里程計刻度系數(shù)的變化，同時，采用平均速度匹配方法，降低載車振動及里程計量化誤差對組合導(dǎo)航的影響。將桿臂誤差列入狀態(tài)變量，進行在線估計。建立了狀態(tài)變量包括桿臂誤差在內(nèi)的平均速度匹配組合導(dǎo)航模型，并通過實際跑車驗證了本文方法的有效性。

1 組合導(dǎo)航模型

1.1 里程計測速模型

安裝于載車底盤的里程計以脈沖形式輸出非轉(zhuǎn)向輪軸中心點在地面投影的前向速度。采樣間隔內(nèi)的脈沖頻率大小代表了載車前進的速度大小，定義載車坐標(biāo)系(m系)為右前上坐標(biāo)系，考慮載車正常行駛時的運動學(xué)約束，里程計的輸出可以寫成矢量形式為

式中：Ni為里程計在采樣周期內(nèi)輸出的脈沖數(shù)；k為里程計刻度系數(shù)；Ts為采樣周期。

考慮到捷聯(lián)慣導(dǎo)載體坐標(biāo)系(b系)與m系不重合，則b系和導(dǎo)航坐標(biāo)系(n系)下的里程計的輸出分別為

忽略二階小量，式(4)可以寫成

1.2 桿臂速度模型

捷聯(lián)慣導(dǎo)IMU敏感中心與里程計速度測量點之間存在一定長度的桿臂，當(dāng)載車有角運動時，捷聯(lián)慣導(dǎo)IMU敏感中心與里程計速度測量點的速度有差異。捷聯(lián)慣導(dǎo)IMU敏感中心與里程計速度測量點的速度存在關(guān)系：

桿臂速度為

實際組合導(dǎo)航時，根據(jù)捷聯(lián)慣導(dǎo)姿態(tài)矩陣與標(biāo)稱桿臂計算桿臂速度，由于慣導(dǎo)姿態(tài)矩陣和標(biāo)稱桿臂存在誤差，實際計算的桿臂速度為

忽略二階小量，式(8)可以寫成

式中：δlb為桿臂距離誤差。

1.3 狀態(tài)方程

組合導(dǎo)航使用前可以對捷聯(lián)慣導(dǎo)的安裝誤差、里程計的刻度系數(shù)和桿臂進行標(biāo)定，由于刻度系數(shù)與輪胎的直徑相關(guān)，受輪胎氣壓等的影響，刻度系數(shù)存在誤差，安裝誤差和桿臂也可能標(biāo)定不準(zhǔn)確，因此，將里程計刻度系數(shù)誤差、捷聯(lián)慣導(dǎo)安裝誤差、桿臂誤差都列為狀態(tài)。捷聯(lián)慣導(dǎo)/里程計組合導(dǎo)航系統(tǒng)狀態(tài)定義為

將里程計刻度系數(shù)誤差、捷聯(lián)慣導(dǎo)俯仰安裝誤差、捷聯(lián)慣導(dǎo)方位安裝誤差和桿臂距離誤差都建模為隨機常值。狀態(tài)方程包括捷聯(lián)慣導(dǎo)誤差方程、慣性器件誤差模型、里程計刻度系數(shù)誤差模型、捷聯(lián)慣導(dǎo)安裝誤差模型、桿臂誤差模型，可以寫成

式中：RM為地球子午圈曲率半徑；h為高度；L為緯度；vN為北向速度；vE為東向速度；RN為地球卯酉圈半徑。

1.4 量測方程

由于捷聯(lián)慣導(dǎo)與里程計測量點非理想剛性連接，載車機動時，慣導(dǎo)安裝處存在振動，該振動引起捷聯(lián)慣導(dǎo)速度與里程計測量速度存在誤差，并且里程計采樣周期很短，造成里程計輸出的瞬時速度存在很大的量化誤差。為了消除振動及量化誤差對組合導(dǎo)航的影響，采用濾波周期內(nèi)的平均速度作為觀測量，即對速度觀測量在一段時間進行積分，然后除以濾波周期，得到平均觀測量，即

考慮到實際卡爾曼濾波周期比較短，在濾波周期內(nèi)捷聯(lián)慣導(dǎo)速度誤差、失準(zhǔn)角、里程計刻度系數(shù)誤差、捷聯(lián)慣導(dǎo)安裝誤差、桿臂誤差近似為常值，將式(15)與式(9)代入式(14)，可得觀測方程為

(16)

實際應(yīng)用時，卡爾曼濾波周期的選擇要綜合考慮里程計量化誤差、慣導(dǎo)誤差在濾波周期內(nèi)是否近似為常值誤差及振動環(huán)境。若濾波周期選擇為1 s，則本文提出的采用平均速度作為觀測量的方法與文獻(xiàn)[7，9]提出的采用單位時間內(nèi)位移增量之差作為觀測量的方法等價。

2 強跟蹤濾波

將捷聯(lián)慣導(dǎo)/里程計組合導(dǎo)航的狀態(tài)方程、觀測方程寫成離散化形式有

(17)

式中：wk，vk均是離散化的零均值白噪聲，其協(xié)方差陣分別為Qk，Rk。

由于里程計刻度系數(shù)受輪胎胎壓、溫度、載荷、輪胎打滑等的影響實時變化，因此，本文提出采用具有突變狀態(tài)跟蹤能力的濾波器進行組合導(dǎo)航。而強跟蹤濾波正是為解決模型不確定性與狀態(tài)突變在卡爾曼濾波算法基礎(chǔ)上改進的一種濾波算法[13-15]。卡爾曼濾波方程為：

時間更新方程

量測更新方程

強跟蹤濾波利用卡爾曼濾波殘差序列相互正交的性質(zhì)，在線實時調(diào)整漸消因子。在標(biāo)準(zhǔn)卡爾曼濾波方程用式(23)替換卡爾曼濾波方程中預(yù)測方差矩陣更新方程式(19)，則得到強跟蹤濾波基本方程。

式中：Dk為漸消因子矩陣。

漸消因子矩陣的計算方法如下[9-11]：

(25)

(29)

3 試驗驗證

為了驗證本文提出的捷聯(lián)慣導(dǎo)/里程計組合導(dǎo)航方法的有效性，進行了實際跑車驗證。捷聯(lián)慣導(dǎo)使用的激光陀螺零偏穩(wěn)定性約為0.002 (°)/h，石英撓性加速度計零偏穩(wěn)定性約為20 μg。捷聯(lián)慣導(dǎo)完成多位置初始對準(zhǔn)后，進行組合導(dǎo)航跑車試驗，試驗過程中采集高精度衛(wèi)星導(dǎo)航定位數(shù)據(jù)作為捷聯(lián)慣導(dǎo)/里程計組合導(dǎo)航位置基準(zhǔn)，衛(wèi)星導(dǎo)航定位精度為3 m(CEP)。跑車試驗里程為48 km，跑車試驗持續(xù)時間3 848 s。強跟蹤濾波器中遺忘因子ρ取為0.99，弱化因子lk取為1，除刻度系數(shù)誤差的ηi取為1.1外，其他狀態(tài)都取為1。跑車試驗軌跡如圖1所示。

跑車試驗結(jié)果如圖2～5所示，其中，圖2為定位誤差曲線，圖3為里程計刻度系數(shù)誤差和捷聯(lián)慣導(dǎo)安裝角估計曲線，圖4為加速度計零偏估計曲線，圖5為桿臂估計曲線。從圖2可見，定位誤差總的趨勢是隨里程的增加而增大，這是因為里程計只能提供速度信息，不能提供直接位置觀測，里程計的作用是降低慣性定位誤差的發(fā)散速度，整個試驗過程定位誤差在3 800 s達(dá)到最大38 m，行駛里程為35 km，定位精度約為1‰D，隨后誤差有所減小，總行駛里程數(shù)為48 km，因此定位終點的定位精度優(yōu)于1‰D。從圖3可見，里程計刻度系數(shù)在整個試驗過程中變化約5‰，相對于定位精度1‰D，可知該方法能有效跟蹤里程計刻度系數(shù)的變化，捷聯(lián)慣導(dǎo)的俯仰安裝角和方位安裝角估計值相對于標(biāo)定值在實時變化。由于可以對里程計刻度系數(shù)和捷聯(lián)慣導(dǎo)的安裝誤差進行實時估計，因此可以有效延長組合導(dǎo)航系統(tǒng)的標(biāo)定周期。從圖4可以看出，組合導(dǎo)航能實時對加速度計零偏進行估計，其中加速度計零偏始初估計值為初始對準(zhǔn)對加速度計零偏的估計值。從圖5可以看出相對于桿臂的標(biāo)定值，組合導(dǎo)航能估計出桿臂誤差。

4 結(jié)束語

里程計刻度系數(shù)受環(huán)境的影響實時變化，本文提出在捷聯(lián)慣導(dǎo)/里程計組合導(dǎo)航時利用強跟蹤濾波對里程計刻度系數(shù)進行強跟蹤估計。推導(dǎo)了狀態(tài)變量包括桿臂誤差在內(nèi)的平均速度匹配組合導(dǎo)航模型。平均速度匹配可以降低里程計量化誤差對組合導(dǎo)航的影響，特別是對于里程計分辨率較低的情況。實際跑車試驗結(jié)果表明，該組合導(dǎo)航方法定位精度約為1‰D，且能估計組合導(dǎo)航中主要的誤差源，為捷聯(lián)慣導(dǎo)/里程計組合導(dǎo)航工程應(yīng)用提供了一種有效的方法。