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

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

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

0 引言

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

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

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

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

1.1 里程計(jì)測(cè)速模型

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

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

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

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

1.2 桿臂速度模型

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

桿臂速度為

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

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

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

1.3 狀態(tài)方程

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

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

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

1.4 量測(cè)方程

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

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

(16)

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

2 強(qiáng)跟蹤濾波

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

(17)

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

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

時(shí)間更新方程

量測(cè)更新方程

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

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

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

(25)

(29)

3 試驗(yàn)驗(yàn)證

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

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

4 結(jié)束語

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