陳雨,王健博,張凌東,曹全,陳世業(yè),劉宇航
(1.北京航天發(fā)射技術(shù)研究所,北京 100076;2.火箭軍駐211廠軍事代表室,北京 100076)
現(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)證了本文方法的有效性。
安裝于載車底盤的里程計(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)可以寫成
捷聯(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為桿臂距離誤差。
組合導(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為地球卯酉圈半徑。
由于捷聯(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à)。
將捷聯(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)
為了驗(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ì)出桿臂誤差。
里程計(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)用提供了一種有效的方法。