基于量測(cè)失準(zhǔn)角的傳遞對(duì)準(zhǔn)姿態(tài)匹配方法

基于量測(cè)失準(zhǔn)角的傳遞對(duì)準(zhǔn)姿態(tài)匹配方法

捷聯(lián)一致系統(tǒng)是現(xiàn)代飛機(jī)軍事訓(xùn)練設(shè)備之一。它具有獨(dú)立、隱蔽、不受外部干擾等優(yōu)點(diǎn)。在機(jī)載導(dǎo)彈發(fā)射之前,導(dǎo)彈捷聯(lián)慣導(dǎo)系統(tǒng)(子慣導(dǎo))連續(xù)利用機(jī)載慣導(dǎo)系統(tǒng)(主慣導(dǎo))的導(dǎo)航參數(shù),通過(guò)濾波器對(duì)姿態(tài)失準(zhǔn)角進(jìn)行在線估計(jì),使其估計(jì)精度達(dá)到要求,進(jìn)行傳遞對(duì)準(zhǔn)。用于傳遞對(duì)準(zhǔn)的匹配參數(shù)包括線運(yùn)動(dòng)參數(shù)和角運(yùn)動(dòng)參數(shù)?！熬€運(yùn)動(dòng)+角運(yùn)動(dòng)”匹配是目前常用的匹配方案,該方案的可觀測(cè)性最好。加入角運(yùn)動(dòng)參數(shù)作為匹配量,是現(xiàn)代快速傳遞對(duì)準(zhǔn)的典型特征,其中以Kain等提出的“速度+姿態(tài)”匹配方案使用最為廣泛,該方案只需載機(jī)實(shí)施搖翼機(jī)動(dòng),大大縮短了對(duì)準(zhǔn)時(shí)間。在各類文獻(xiàn)中,實(shí)現(xiàn)傳遞對(duì)準(zhǔn)姿態(tài)匹配的算法各不相同。文獻(xiàn)的姿態(tài)量測(cè)方程比較復(fù)雜,存在大量的數(shù)學(xué)運(yùn)算。文獻(xiàn)采用“姿態(tài)矩陣”量測(cè)進(jìn)行了簡(jiǎn)化,但是“姿態(tài)矩陣”量測(cè)方法在某些特殊的姿態(tài)角時(shí),某些失準(zhǔn)角誤差難以估計(jì)。在國(guó)外文獻(xiàn)中,較多地使用了幾個(gè)姿態(tài)矩陣相乘的結(jié)果作為量測(cè)量,形式簡(jiǎn)單,計(jì)算量小,但是都沒(méi)有給出推導(dǎo)過(guò)程。本文首先對(duì)4種姿態(tài)匹配算法進(jìn)行介紹和逐步的推導(dǎo),說(shuō)明姿態(tài)匹配算法的特點(diǎn)和優(yōu)化過(guò)程,并且從優(yōu)化過(guò)程中得出一種最優(yōu)的姿態(tài)匹配算法;指出4種姿態(tài)匹配算法的相互關(guān)系;然后,為4種姿態(tài)匹配算法建立了傳遞對(duì)準(zhǔn)狀態(tài)方程;最后,在相同條件下對(duì)4種姿態(tài)匹配算法進(jìn)行仿真,分析了仿真結(jié)果,驗(yàn)證本文的觀點(diǎn)。1第三,利用傳統(tǒng)姿態(tài)匹配方法進(jìn)行檢測(cè)當(dāng)傳遞對(duì)準(zhǔn)中采用姿態(tài)信息作為匹配量時(shí),姿態(tài)匹配量測(cè)方程就成為了傳遞對(duì)準(zhǔn)方程中的重要組成部分。為了方便論述,把傳遞對(duì)準(zhǔn)姿態(tài)匹配算法分為4種:把使用主、子慣導(dǎo)姿態(tài)角之差的結(jié)果作為量測(cè)量的方法稱為“姿態(tài)角匹配法”(方法1);把使用主、子慣導(dǎo)姿態(tài)矩陣元素值之差的結(jié)果作為量測(cè)量的方法稱為“姿態(tài)矩陣匹配法”(方法2);“姿態(tài)角匹配法”和“姿態(tài)矩陣匹配法”都是傳統(tǒng)姿態(tài)匹配方法,文獻(xiàn)中使用了傳統(tǒng)姿態(tài)匹配方法。將使用主、子慣導(dǎo)的姿態(tài)矩陣相乘的結(jié)果作為量測(cè)量的方法稱為現(xiàn)代姿態(tài)匹配方法,包括“量測(cè)失準(zhǔn)角匹配法”(方法3)和在此基礎(chǔ)上的改進(jìn)方法——“最優(yōu)姿態(tài)匹配法”(方法4),有許多文獻(xiàn)使用了現(xiàn)代姿態(tài)匹配方法。下面依次介紹和推導(dǎo)這4種方法,并且說(shuō)明方法4是最優(yōu)的姿態(tài)匹配算法。首先給出文中使用坐標(biāo)系的定義:n為導(dǎo)航坐標(biāo)系;n′為子慣導(dǎo)數(shù)學(xué)平臺(tái)坐標(biāo)系;m為主慣導(dǎo)機(jī)體坐標(biāo)系;s為子慣導(dǎo)彈體坐標(biāo)系。由于主慣導(dǎo)系統(tǒng)比子慣導(dǎo)系統(tǒng)精度高數(shù)個(gè)量級(jí),可認(rèn)為n和m坐標(biāo)系是無(wú)誤差坐標(biāo)系。1.1cns姿態(tài)量測(cè)方程傳統(tǒng)的姿態(tài)量測(cè)方程使用主、子慣導(dǎo)姿態(tài)角直接相減的結(jié)果作為量測(cè)量。定義主慣導(dǎo)的俯仰角為θ,橫滾角為γ,航向角為ψ;子慣導(dǎo)的俯仰角為θs,橫滾角為γs,航向角為ψs;主、子慣導(dǎo)姿態(tài)角的關(guān)系為θs=θ+δθ,γs=γ+δγ,ψs=ψ+δψ(1)式中:δθ,δγ,δψ為主、子慣導(dǎo)姿態(tài)角之差。定義主慣導(dǎo)的姿態(tài)矩陣為Cnm,子慣導(dǎo)的姿態(tài)矩陣為?Cns,?Cns為子慣導(dǎo)真實(shí)姿態(tài)矩陣Cns的計(jì)算值,Cnm和?Cns分別為Cnm=[C11C12C13C21C22C23C31C32C33]?Cns=[C′11C′12C′13C′21C′22C′23C′31C′32C′33]}(2)?Cns與子慣導(dǎo)數(shù)學(xué)平臺(tái)失準(zhǔn)角?n存在如下關(guān)系:?Cns=?CnnCns=[Ι-(?n×)]Cns(3)式中:?n=[?E?Ν?U]Τ為小量;Cns=CnmCms,并且Cms具有如下關(guān)系:Cms=Ι+(λ×)(4)式中:λ為主、子慣導(dǎo)之間的姿態(tài)失準(zhǔn)角,為小量。把式(4)代入式(3),得?Cns=Cnm-(?n×)Cnm+Cnm(λ×)(5)由式(1)和式(5),可得δθ=fθ(-C22?E+C12?Ν+C33λx-C31λz)(6)δγ=fγ[(C21C33-C31C23)?E-C31C32λx+λy]+fγ[(C13C31-C11C33)?Ν-C32C33λz](7)δψ=fψ[C12C32?E+?U+(C13C22-C12C23)λx]+fψ[(C12C21-C11C22)λz-C22C32?Ν](8)式中:fθ=1/1-C322;fγ=1/(C312+C332);fψ=1/(C122+C222)。由式(6)～式(8),可構(gòu)成方法1的姿態(tài)量測(cè)方程。該方法是通過(guò)子慣導(dǎo)姿態(tài)角的三角函數(shù)關(guān)系以及泰勒展開(kāi)式,直接計(jì)算出主、子慣導(dǎo)之間的姿態(tài)角之差,并根據(jù)姿態(tài)角之差與失準(zhǔn)角之間的關(guān)系,構(gòu)造出姿態(tài)量測(cè)矩陣。這種方法的推導(dǎo)十分直觀,然而在姿態(tài)量測(cè)方程的計(jì)算時(shí),排除形式相同的重復(fù)計(jì)算后,子慣導(dǎo)計(jì)算機(jī)至少還需要1次開(kāi)方,3次除法和27次乘法運(yùn)算。因此,在工程應(yīng)用時(shí),該方法將給子慣導(dǎo)計(jì)算機(jī)增加很大的計(jì)算負(fù)擔(dān)。1.2主慣導(dǎo)中種姿態(tài)矩陣元素的選取問(wèn)題為了減少方法1的計(jì)算量,多個(gè)文獻(xiàn)提出了“姿態(tài)矩陣匹配法”,此處稱為方法2。該方法不需計(jì)算主、子慣導(dǎo)之間的姿態(tài)角之差,而是計(jì)算主、子慣導(dǎo)之間的姿態(tài)矩陣元素值之差,根據(jù)姿態(tài)矩陣元素值之差與失準(zhǔn)角之間的關(guān)系,構(gòu)造出姿態(tài)矩陣量測(cè)方程。由式(5),可得主、子慣導(dǎo)姿態(tài)矩陣之差為Ζa=C^sn-Cmn=-(?n×)Cmn+Cmn(λ×)(9)式中:Za為3×3維矩陣。如果選取Za全部9個(gè)矩陣元素作為量測(cè)向量,將使量測(cè)向量的維數(shù)增加很大,一般可選取其中3個(gè)元素作為量測(cè)向量,如可選取的量測(cè)量為za=[C′12-C12C′31-C31C′32-C32]Τ(10)其量測(cè)向量為[z1z2z3]=[C22?U-C32?Ν-C11λz+C13λxC11?Ν-C21?E+C32λz-C33λyC12?Ν-C22?E-C31λz+C33λx](11)式中:C22=cosθcosψ?？梢?jiàn),當(dāng)載機(jī)主慣導(dǎo)沿東向或西向飛行時(shí),即ψ=90°或270°時(shí),則C22=0,此時(shí)子慣導(dǎo)航向誤差?U將無(wú)法估計(jì),主要原因是子慣導(dǎo)濾波器沒(méi)有選取全部9個(gè)姿態(tài)矩陣元素之差作為量測(cè)量,在特定條件下濾波器的可觀測(cè)性差。在式(9)中,選取其他3個(gè)元素也會(huì)存在相同的問(wèn)題。因此,方法2至少需要選取2組量測(cè)方程,并且2組量測(cè)方程需要切換使用。1.3量測(cè)失準(zhǔn)角匹配法。根據(jù)超越法由方法1和方法2可見(jiàn),傳統(tǒng)姿態(tài)匹配方法存在一定的缺點(diǎn)。在國(guó)外文獻(xiàn)中,使用主、子慣導(dǎo)姿態(tài)矩陣的乘積結(jié)果作為量測(cè)量的方法得到了廣泛的使用,此處將這種方法稱為現(xiàn)代姿態(tài)匹配方法,其姿態(tài)量測(cè)方程稱為現(xiàn)代姿態(tài)量測(cè)方程。現(xiàn)代姿態(tài)匹配方法的起源是在Kain等提出的量測(cè)失準(zhǔn)角的基礎(chǔ)上發(fā)展和推導(dǎo)的。實(shí)際上,使用數(shù)學(xué)平臺(tái)失準(zhǔn)角?n、實(shí)際姿態(tài)失準(zhǔn)角λ和量測(cè)失準(zhǔn)角?m這3個(gè)失準(zhǔn)角之間的關(guān)系也可推導(dǎo)出一種姿態(tài)量測(cè)方程。定義量測(cè)失準(zhǔn)角為?m,并且?m為小量,則有:Ζa=C^nsCmn=Ι-(?m×)(12)根據(jù)姿態(tài)矩陣的相關(guān)性質(zhì),式(12)為Ζdcm=CnsC^nnCmn=CmsCmnC^nnCmn(13)由式(3)和式(4),式(13)可變?yōu)棣cm=[Ι-(λ×)]Cnm[Ι+(?n×)]Cmn(14)將式(14)展開(kāi),省略二階小量,可得Ζdcm=Ι+((Cnm?n)×)-(λ×)(15)由式(12)和式(15),根據(jù)反對(duì)稱陣性質(zhì),得?m=λ-Cnm?n(16)式(16)是一種姿態(tài)匹配算法,稱為“量測(cè)失準(zhǔn)角匹配法”,即方法3,其中?m由式(12)求得。與方法1和方法2兩種傳統(tǒng)姿態(tài)匹配方法相比,方法3只需利用主慣導(dǎo)和子慣導(dǎo)的姿態(tài)矩陣信息,因此,該方法簡(jiǎn)化了姿態(tài)量測(cè)方程形式,計(jì)算量也減少許多。1.4最優(yōu)姿態(tài)匹配法傳統(tǒng)姿態(tài)匹配方法是建立在子慣導(dǎo)的安裝角是小量的基礎(chǔ)上的。然而現(xiàn)代載機(jī)為了增加武器的外掛數(shù)量,經(jīng)常采用多點(diǎn)式懸掛方式,如美軍的B-52轟炸機(jī)可采用3點(diǎn)式懸掛方法,即每個(gè)掛架可外掛3枚武器,此時(shí)的安裝角已經(jīng)不是小量,因此,姿態(tài)失準(zhǔn)角λ的小量假設(shè)也不再成立。然而,傳統(tǒng)的姿態(tài)量測(cè)方程中并沒(méi)有考慮子慣導(dǎo)安裝角的補(bǔ)償方法,因此具有一定的局限性。同樣,方法3也是在認(rèn)為λ是小量的條件下推導(dǎo)的,需要對(duì)其改進(jìn)。因此,上述的3種方法不能應(yīng)用在安裝角是大角度的情況下,應(yīng)用范圍有限。利用方法3,進(jìn)一步加以改進(jìn),可得到適用范圍更為廣泛的“最優(yōu)姿態(tài)匹配法”,可以應(yīng)用到子慣導(dǎo)安裝角是大角度的環(huán)境中,稱為方法4。方法4的姿態(tài)量測(cè)矩陣如式(17)所示,主要的改進(jìn)是在式(12)的主、子慣導(dǎo)姿態(tài)矩陣相乘結(jié)果(C^snCnm)的基礎(chǔ)上,右乘C^ms,來(lái)補(bǔ)償已知的子慣導(dǎo)安裝角。Ζa=C^nsCmnC^sm=Ι-(?m×)(17)式中:改進(jìn)后的?m為小量;C^ms為子慣導(dǎo)安裝角姿態(tài)矩陣的補(bǔ)償值,然而由于安裝誤差、機(jī)翼變形等因素的存在,該補(bǔ)償值也是理論值,即還存在失準(zhǔn)角?α,?α為不能直接補(bǔ)償?shù)陌惭b誤差角,并且?α是小量,如下所示:C^sm=Csm[Ι-(?α×)](18)將式(3)和式(18)代入式(17),得Ζa=Cns[Ι+(?n×)]CmnCsm[Ι-(?α×)](19)式(19)簡(jiǎn)化可得Ζa=Cns[Ι+(?n×)]Csn[Ι-(?α×)](20)由姿態(tài)矩陣的相關(guān)性質(zhì)及式(20),可得Ζa=[Ι+((Cns?n)×)][Ι-(?α×)](21)式(21)展開(kāi)后,忽略二階小量,可得Ζa=Ι+((Cns?n)×)-(?α×)(22)由式(17)和式(22),可得?m=?α-Cns?n(23)式(23)與式(16)形式相似,其中?m由式(17)求得,但是?m和?α的定義已經(jīng)不同。至此,可得到式(23)所示的姿態(tài)匹配算法,即方法4。4種方法的推導(dǎo)中,都省略了二階小量。方法4與方法1和方法2比較,由于該方法使用主、子慣導(dǎo)姿態(tài)矩陣及失準(zhǔn)角矩陣信息,形式簡(jiǎn)單,易于理解,也使計(jì)算量大大減小;并且,方法1和方法2只能使用在子慣導(dǎo)安裝角是小角度條件下,而方法4并沒(méi)有這樣的限制。與方法3比較,方法4同樣具有可在子慣導(dǎo)安裝角是大角度環(huán)境中使用的優(yōu)點(diǎn)。因此,方法4是4種方法中最優(yōu)的姿態(tài)匹配算法。2推導(dǎo)姿態(tài)匹配算法m通過(guò)以上推導(dǎo),說(shuō)明了4種姿態(tài)匹配算法的優(yōu)缺點(diǎn)。實(shí)際上,4種姿態(tài)匹配算法在理論上也是具有相互關(guān)系的,具體說(shuō)明如下:式(5)右乘Cnm,簡(jiǎn)化后得CnmC^sn=Ι-Cnm(?n×)Cmn+(λ×)(24)由式(12)的定義,即Ζa=C^nsCmn=Ι-(?m×),得Ι+(?m×)=CnmC^sn(25)將式(25)代入式(24),簡(jiǎn)化后可得?m×=(λ×)-Cnm(?n×)Cmn(26)由姿態(tài)矩陣相關(guān)性質(zhì)及式(26),得(?m×)=(λ×)-((Cnm?n)×)(27)式(27)去掉反對(duì)稱陣形式,得?m=λ-Cnm?n(28)至此可得,式(28)就是式(16),即,由式(5)同樣可以推導(dǎo)出式(16)。因此可以證明,方法3可在方法1的基礎(chǔ)上推導(dǎo)得出,也可說(shuō)明方法1和方法3兩種方法在理論上是統(tǒng)一的。兩種方法的主要差別是由推導(dǎo)過(guò)程不同而造成的:方法1在式(5)的基礎(chǔ)上,利用三角函數(shù)關(guān)系以及泰勒展開(kāi)式,得到了一種比較復(fù)雜的姿態(tài)匹配算法;而方法3通過(guò)定義量測(cè)失準(zhǔn)角為?m,巧妙地避開(kāi)了復(fù)雜的計(jì)算,也因此推導(dǎo)出了一種簡(jiǎn)單的姿態(tài)匹配算法。由于方法2是方法1的改進(jìn)算法;由此可以證明方法1、方法2和方法3是統(tǒng)一的。由于方法4是方法3的改進(jìn)算法,因此,可以證明方法4是4種姿態(tài)匹配算法中的最優(yōu)算法。3階狀態(tài)向量傳遞對(duì)準(zhǔn)模型為x˙=Fx+Gwz=Ηx+v}(29)由于在推導(dǎo)過(guò)程中,方法1、方法2和方法3中定義的實(shí)際失準(zhǔn)角為λ,并且λ是小量,可直接使用Kalman濾波器估計(jì)。而方法4補(bǔ)償了已知子慣導(dǎo)安裝角,需要估計(jì)的失準(zhǔn)角是?α。因此,在仿真中,建立兩個(gè)傳遞對(duì)準(zhǔn)方程,分別用于方法1～方法4的傳遞對(duì)準(zhǔn)仿真,但是使用統(tǒng)一的仿真輸入條件,全部選取21階狀態(tài)向量。方法1～方法3的21階狀態(tài)向量為x=[(?n)Τ(δvn)ΤλΤ(gB)Τ(gS)Τ(aB)Τ(aS)Τ]Τ(30)傳遞對(duì)準(zhǔn)狀態(tài)方程為??n=δωinn-ωinn×?n+εnδv˙n=fn×?n-(2δωien+δωenn)×vn-(2ωien+ωenn)×δvn+?nλ˙=0,gB˙=0,gS˙=0aB˙=0,aS˙=0}(31)式中:gB為子慣導(dǎo)陀螺漂移;gS為陀螺刻度系數(shù);aB為加速度計(jì)偏移;aS為加速度計(jì)刻度系數(shù);λ為姿態(tài)失準(zhǔn)角。上述的狀態(tài)向量全部建模為隨機(jī)常值。εn為陀螺誤差在導(dǎo)航系中的投影;Δn為加速度計(jì)誤差在導(dǎo)航系中的投影。方法4的21階狀態(tài)向量為x=[(?n)Τ(δvn)Τ(?α)Τ(gB)Τ(gS)Τ(aB)Τ(aS)Τ]Τ(32)與式(30)的區(qū)別在于λ改變?yōu)?α,其傳遞對(duì)準(zhǔn)狀態(tài)方程與式(31)的區(qū)別在于λ改為?α,并且將?α建模為隨機(jī)常值,如下所示:??α=0(33)4子慣導(dǎo)安裝角仿真結(jié)果將傳遞對(duì)準(zhǔn)分為獨(dú)立的功能模塊,進(jìn)行模塊化設(shè)計(jì)和仿真。采用Monte-Carlo方法,在相同的條件下,依次對(duì)方法1～方法4進(jìn)行5s的仿真。方法1～方法3采用相同的狀態(tài)方程,但姿態(tài)匹配方程不同。仿真中采用搖翼機(jī)動(dòng),“速度+姿態(tài)”匹配方案。在仿真數(shù)據(jù)中,子慣導(dǎo)IMU誤差與文獻(xiàn)取值相同:陀螺誤差為3.3(°)/h,刻度系數(shù)為200×10-6(1σ);加速度計(jì)誤差為5×10-4g,刻度系數(shù)為500×10-6(1σ)。子慣導(dǎo)安裝角定義為俯仰角10°,橫滾角60°,偏航角8°。安裝誤差角依次設(shè)置為:俯仰失準(zhǔn)角40′,滾動(dòng)失準(zhǔn)角30′,偏航失準(zhǔn)角20′。仿真結(jié)果如圖1～圖4所示。其中圖1、圖2和圖3分別是采用方法1、方法2和方法3的俯仰(x軸)、橫滾(y軸)和航向(z軸)3個(gè)軸向的失準(zhǔn)角估計(jì)值和估計(jì)均方誤差?？梢?jiàn),方法1～方法3在子慣導(dǎo)安裝角不是小角度時(shí),已經(jīng)不能收斂到估計(jì)值。方法1～方法3是在子慣導(dǎo)安裝角為小角度的情況下推導(dǎo)的,因此適用的范圍也是在子慣導(dǎo)安裝角為小角度的情況下,當(dāng)子慣導(dǎo)安裝角不是小角度時(shí),方法1～方法3的姿態(tài)