崔 瀟，秦永元，嚴(yán)恭敏，周 琪

(1. 西北工業(yè)大學(xué)自動(dòng)化學(xué)院，西安 710129；2. 西安飛行自動(dòng)控制研究所，西安 710065；3. 飛行器控制一體化技術(shù)國(guó)防科技重點(diǎn)實(shí)驗(yàn)室，西安 710065)

0 引 言

隨著現(xiàn)代戰(zhàn)爭(zhēng)對(duì)武器裝備精確打擊能力和快速反應(yīng)能力的要求日益提高，許多艦載和機(jī)載武器裝備都安裝了捷聯(lián)慣性導(dǎo)航系統(tǒng)(Strapdown inertial navigation system, SINS),而由于慣導(dǎo)系統(tǒng)為戰(zhàn)術(shù)級(jí)，傳遞對(duì)準(zhǔn)成為其初始對(duì)準(zhǔn)的主要方案[1-3]。因此，研究戰(zhàn)術(shù)武器系統(tǒng)的快速高精度傳遞對(duì)準(zhǔn)具有較大的工程應(yīng)用價(jià)值。

自1989年Kain和Cloutier[1]介紹了“速度+姿態(tài)”匹配的快速傳遞對(duì)準(zhǔn)方法以來(lái)，傳遞對(duì)準(zhǔn)進(jìn)入快速發(fā)展和應(yīng)用階段，于1998年在F-16戰(zhàn)機(jī)上進(jìn)行了試飛驗(yàn)證,僅在搖翼機(jī)動(dòng)下，10 s內(nèi)達(dá)到了1mrad的對(duì)準(zhǔn)精度[2]。隨后傳遞對(duì)準(zhǔn)研究主要集中在匹配方法、濾波模型和影響對(duì)準(zhǔn)精度的干擾因素等方面[3-6]。

傳統(tǒng)的傳遞對(duì)準(zhǔn)大都是以系統(tǒng)的誤差模型為基礎(chǔ)的，傳統(tǒng)方案主要分為兩類(lèi)濾波估計(jì)問(wèn)題：即基于小失準(zhǔn)角假設(shè)的線(xiàn)性濾波估計(jì)問(wèn)題和基于大失準(zhǔn)角假設(shè)的非線(xiàn)性濾波估計(jì)問(wèn)題。

針對(duì)大失準(zhǔn)角對(duì)準(zhǔn)時(shí)，目前采用的方法主要有兩種，一是傳遞對(duì)準(zhǔn)姿態(tài)裝訂時(shí)，對(duì)子慣導(dǎo)進(jìn)行安裝角補(bǔ)償，以期滿(mǎn)足小角度要求，然后進(jìn)行傳統(tǒng)線(xiàn)性模型傳遞對(duì)準(zhǔn)；二是采用非線(xiàn)性濾波模型和濾波算法。很多學(xué)者建立了以姿態(tài)角、姿態(tài)四元數(shù)或旋轉(zhuǎn)矢量為姿態(tài)描述的非線(xiàn)性誤差模型[7-11]，可適用于大失準(zhǔn)角下的傳遞對(duì)準(zhǔn)，雖然這類(lèi)模型的量測(cè)方程為線(xiàn)性，但系統(tǒng)方程均具有強(qiáng)非線(xiàn)性，各種諸如CDPF[7]、UKF[8-10]、SRUKF[11]等非線(xiàn)性濾波被引入到對(duì)準(zhǔn)算法，存在計(jì)算量較大且在大失準(zhǔn)角下估計(jì)精度不高等問(wèn)題。

為了適應(yīng)任意失準(zhǔn)角條件下的快速傳遞對(duì)準(zhǔn)，文獻(xiàn)[12]研究了二次對(duì)準(zhǔn)方案，首先在大失準(zhǔn)角較大時(shí)采用基于四元數(shù)的非線(xiàn)性模型，在估計(jì)達(dá)到一定精度后，對(duì)子慣導(dǎo)進(jìn)行補(bǔ)償，再切換到基于歐拉角的線(xiàn)性模型；文獻(xiàn)[13]提出了在慣性系框架內(nèi)完成傳遞對(duì)準(zhǔn)的思路，建立基于羅德里格參數(shù)的大失準(zhǔn)角傳遞對(duì)準(zhǔn)方案，設(shè)計(jì)了二階EKF濾波算法，但由于羅德里格參數(shù)描述姿態(tài)的奇異性，需在對(duì)準(zhǔn)過(guò)程中對(duì)奇異點(diǎn)進(jìn)行判別和處理。

本文主要在文獻(xiàn)[13]的基礎(chǔ)上，研究了任意失準(zhǔn)角下無(wú)奇異點(diǎn)的快速傳遞對(duì)準(zhǔn)方案，建立了慣性系下基于姿態(tài)矩陣的統(tǒng)一線(xiàn)性對(duì)準(zhǔn)模型，采用了慣性系內(nèi)比力積分+角速度積分的匹配方案，推導(dǎo)了矩陣形式的約束卡爾曼濾波實(shí)現(xiàn)，所建立的濾波模型和采用的濾波算法，與失準(zhǔn)角大小無(wú)關(guān)，在形式上和實(shí)現(xiàn)上完全一致，同時(shí)也避免了羅德里格參數(shù)對(duì)準(zhǔn)模型的奇異點(diǎn)問(wèn)題。

1 慣性系統(tǒng)—對(duì)準(zhǔn)模型

1.1 主子慣導(dǎo)系統(tǒng)實(shí)際測(cè)量值基本關(guān)系

由文獻(xiàn)[13-14]可知，主子慣導(dǎo)系統(tǒng)敏感的比力和角速度在地心慣性系內(nèi)的投影關(guān)系分別為：

(1)

(2)

假設(shè)在傳遞對(duì)準(zhǔn)開(kāi)始時(shí)刻，將主子慣導(dǎo)坐標(biāo)系m和s均進(jìn)行慣性凝固，分別得到慣性坐標(biāo)系im和is。對(duì)準(zhǔn)過(guò)程中姿態(tài)變化可利用陀螺儀輸出進(jìn)行跟蹤，忽略主慣導(dǎo)誤差時(shí)，即有

(3)

(4)

(5)

(6)

(7)

對(duì)于子慣導(dǎo)同理可得

(8)

將式(8)展開(kāi)整理，略去高階小量，可得

(9)

由式(1)、式(7)、式(9)可得

(10)

對(duì)主子慣導(dǎo)系統(tǒng)測(cè)量的角速度信息進(jìn)行類(lèi)似處理，可得

(11)

式中：

(12)

式(10)和式(11)表明，可以采用主子慣導(dǎo)系統(tǒng)實(shí)際測(cè)量值在慣性系內(nèi)的投影作為慣性系傳遞對(duì)準(zhǔn)的匹配量，這種匹配方式與經(jīng)典的傳遞對(duì)準(zhǔn)中基于測(cè)量參數(shù)匹配方式，即加速度和角速度匹配對(duì)應(yīng)。

1.2 傳遞對(duì)準(zhǔn)模型

式(10)和式(11)等號(hào)兩端包含了相關(guān)誤差，若對(duì)其進(jìn)行積分操作，則能夠有效平滑相關(guān)噪聲、桿臂殘差以及彈性變形項(xiàng)，提高信噪比。

對(duì)式(10)等號(hào)左邊取積分，并記

(13)

對(duì)式(10)等號(hào)右邊括號(hào)內(nèi)取積分，并記

(14)

由式(10)、式(13)、式(14)可得

(15)

對(duì)式(11)進(jìn)行類(lèi)似操作，同理可得

(16)

其中,

(17)

(18)

式(15)為比力量測(cè)的直接積分，可作為慣性系傳遞對(duì)準(zhǔn)的匹配量，與經(jīng)典傳遞對(duì)準(zhǔn)算法中的速度匹配類(lèi)似；式(16)則為角速度量測(cè)的直接積分，也是慣性系傳遞對(duì)準(zhǔn)的匹配量之一，與經(jīng)典傳遞對(duì)準(zhǔn)算法中的姿態(tài)匹配類(lèi)似。

值得注意的是，在推導(dǎo)式(15)和式(16)的過(guò)程中，并未對(duì)主子慣導(dǎo)系統(tǒng)之間的相對(duì)姿態(tài)提出任何假設(shè)。無(wú)論主子慣導(dǎo)系統(tǒng)之間相對(duì)姿態(tài)為小姿態(tài)角還是大姿態(tài)角，慣性系內(nèi)得到的匹配方程具有一致的形式，因此，可以在慣性系內(nèi)，設(shè)計(jì)出無(wú)需區(qū)分相對(duì)姿態(tài)大小的統(tǒng)一的傳遞對(duì)準(zhǔn)模型，由于模型基于姿態(tài)矩陣表示，避免了文獻(xiàn)[13]中奇異點(diǎn)問(wèn)題，下面將以比力積分加角速度積分匹配為基礎(chǔ)，來(lái)設(shè)計(jì)基于姿態(tài)矩陣任意失準(zhǔn)角下的無(wú)奇異快速傳遞對(duì)準(zhǔn)算法。

(19)

考慮陀螺漂移和加速度計(jì)零偏為隨機(jī)常值，即

(20)

(21)

(22)

式中：Eij為8×8矩陣，且第ij個(gè)元素為1，其余為0。

由式(15)、式(16)可構(gòu)成量測(cè)方程，即

(23)

其中，Vk+1為量測(cè)噪聲矩陣，且

(24)

2 濾波器設(shè)計(jì)

式(21)、式(23)描述的濾波模型具有下面三個(gè)特征：1)系統(tǒng)方程和量測(cè)方程均為線(xiàn)性；2)狀態(tài)為矩陣形式；3)部分狀態(tài)，即姿態(tài)矩陣受到正交約束。針對(duì)上述三個(gè)特征，設(shè)計(jì)如下矩陣濾波算法來(lái)實(shí)現(xiàn)對(duì)參數(shù)的估計(jì)。

2.1 濾波時(shí)間更新

要利用標(biāo)準(zhǔn)卡爾曼濾波基本方程，首先要將式(21)、式(23)矩陣方程向量化，為此定義如下向量化算子：

x=vec(X)

(25)

式中：X∈Rm×n,x∈Rmn×1，即向量化算子vec(·)表示將矩陣X按列首尾相連得到向量x，也稱(chēng)按列堆棧[15]。

若定義?為Kronecker積，則[15]

vec(AXB)=(BT?A)vec(X)

(26)

假設(shè)在tk時(shí)刻狀態(tài)Xk和估計(jì)均方誤差陣Pk已知，對(duì)式(21)進(jìn)行向量化操作，并利用式(26)特性，可得

vec(Wk)

(27)

即等價(jià)于標(biāo)準(zhǔn)卡爾曼濾波離散狀態(tài)方程

xk+1=Ψkxk+wk

(28)

則狀態(tài)一步預(yù)測(cè)

(29)

利用算子vec的線(xiàn)性特性和式(26)，則式(29)可等價(jià)于

(30)

一步預(yù)測(cè)均方誤差

(31)

式中：Qk=cov{Wk}。

式(30)和式(31)即為矩陣濾波的時(shí)間更新。

2.2 濾波量測(cè)更新

對(duì)式(23)描述的量測(cè)方程，首先忽略姿態(tài)矩陣正交約束的影響，推導(dǎo)量測(cè)更新算法。

記量測(cè)一步預(yù)測(cè)

(32)

殘差

(33)

對(duì)式(33)進(jìn)行向量化操作，即

(34)

則由標(biāo)準(zhǔn)卡爾曼濾波可得

Sk+1=Γk+1Pk+1/k(Γk+1)T+Rk+1

(35)

Kk+1=Pk+1/k(Γk+1)T(Sk+1)-1

(36)

(37)

式中：Γk+1=(Gk+1)T?Hk+1。

為了將式(37)恢復(fù)到矩陣形式，引入如下定理：對(duì)于給定矩陣X∈Ri3×i4,Z∈Ri1×i2，且A∈Ri1i2×i3i4，定義z∈Ri1i2,x∈Ri3i4分別為矩陣Z,X向量化的向量，即z=vec(Z),x=vec(X)。將矩陣寫(xiě)成如下分塊矩陣的形式

(38)

其中，Ajl∈Ri1×i3，則向量化方程:

z=Ax

(39)

等價(jià)于如下矩陣方程

(40)

式中：Elj∈Ri4×i2，且第lj個(gè)元素為1，其余元素為0。定理的詳細(xì)證明過(guò)程參見(jiàn)文獻(xiàn)[16]。

將定理應(yīng)用于式(37)右端第二項(xiàng)，其余項(xiàng)則利用向量化算子的逆運(yùn)算，可得

(41)

(42)

式(41)則為矩陣狀態(tài)在時(shí)刻tk+1的更新方程。

由標(biāo)準(zhǔn)卡爾曼濾波理論，矩陣估計(jì)均方誤差為

Pk+1/k+1= (I24-Kk+1Γk+1)Pk+1/k(I24-Kk+1Γk+1)T+

(43)

式(33)、式(35)、式(36)、式(41)和式(43)即為不考慮約束條件下所設(shè)計(jì)的矩陣濾波量測(cè)更新。

2.3 姿態(tài)矩陣正交約束下濾波算法

針對(duì)濾波算法中的姿態(tài)矩陣正交約束問(wèn)題，借鑒偽量測(cè)處理方法，推導(dǎo)本文所設(shè)計(jì)的濾波算法在姿態(tài)矩陣正交約束條件下的實(shí)現(xiàn)。

考慮下式約束

(44)

(45)

(46)

偽量測(cè)方程(46)為姿態(tài)矩陣Ck的線(xiàn)性方程，很自然可運(yùn)用到矩陣卡爾曼濾波的量測(cè)更新過(guò)程中，即在進(jìn)行正常的矩陣卡爾曼量測(cè)更新之后，接著進(jìn)行如下正交化量測(cè)更新過(guò)程

Sk+1=Pk+1/k+μortI9

(47)

Kk+1=Pk+1/k(Sk+1)-1

(48)

(49)

Pk+1/k+1= (I9-Kk+1)Pk+1/k(I9-Kk+1)T+

(50)

3 仿真與分析

為進(jìn)一步檢驗(yàn)本文所提的基于矩陣卡爾曼濾波傳遞對(duì)準(zhǔn)方法的性能，進(jìn)行了傳遞對(duì)準(zhǔn)的數(shù)字仿真。

3.1 仿真條件

1)以艦船在水中會(huì)受到來(lái)自風(fēng)浪的干擾而發(fā)生繞俯仰軸、橫滾軸和方位軸的搖擺運(yùn)動(dòng)為例。這種運(yùn)動(dòng)可看作由一系列幅值和頻率相近的正弦波來(lái)描述[9-10]：

(51)

式中：θ,γ,φ分別為繞俯仰軸,橫滾軸和方位軸的搖擺角度；θm,γm,φm分別為搖擺角度幅值；ωθ,ωγ,ωφ為搖擺的角頻率；θ0,γ0,φ0為搖擺的初始相位。

表1 搖擺參數(shù)設(shè)置Table 1 Simulation parameters of swaying

2)子慣導(dǎo)參數(shù)設(shè)置如表2所示。

表2 子慣導(dǎo)參數(shù)設(shè)置Table 2 Sensors specifications of SINS

3)濾波器參數(shù)設(shè)置：矩陣狀態(tài)變量初值為X(0)=03×8，系統(tǒng)噪聲取為陀螺、加速度計(jì)噪聲。由于系統(tǒng)方程中未考慮撓曲變形及載體振動(dòng)等干擾，這里適當(dāng)增大量測(cè)噪聲，比力量測(cè)噪聲取為0.001 m/s2，角速度積分量測(cè)取為5×10-6(°)/s。

4)主子慣導(dǎo)安裝誤差角設(shè)置：小失準(zhǔn)角[1° 1° 1°]T和大失準(zhǔn)角[60° 60° 150°]T。

3.2 仿真結(jié)果

根據(jù)第3.1節(jié)設(shè)置的仿真條件，分別在大、小失準(zhǔn)角條件下進(jìn)行了50次蒙特卡洛仿真，兩種情況下的仿真結(jié)果如圖1～圖4所示。圖中實(shí)線(xiàn)表示本文提出的MKF方法，虛線(xiàn)表示傳統(tǒng)方法。其中，小角度條件下，傳統(tǒng)方法采用的是線(xiàn)性模型的速度+姿態(tài)匹配方案；而大失準(zhǔn)角條件下，同時(shí)對(duì)比了文獻(xiàn)[8-10]中基于非線(xiàn)性模型的傳統(tǒng)UKF方案和文獻(xiàn)[13]提出的二階EKF(EKF2)方案。

從圖1～圖4可以看出，無(wú)論失準(zhǔn)角大小，矩陣濾波對(duì)準(zhǔn)方案具有同樣的估計(jì)效果，估計(jì)速度明顯優(yōu)于傳統(tǒng)方法，而且在估計(jì)收斂之后的穩(wěn)態(tài)性能也優(yōu)于傳統(tǒng)方法，尤其是在大失準(zhǔn)角時(shí)，優(yōu)勢(shì)較傳統(tǒng)UKF更為明顯，而且具有快速性特點(diǎn)，傳統(tǒng)UKF方法在大失準(zhǔn)角時(shí)，精度有所下降，由于本文采用文獻(xiàn)[13]相同的量測(cè)匹配方案，因此對(duì)準(zhǔn)精度同EKF2相當(dāng)。

兩種失準(zhǔn)角情形下的估計(jì)均值和估計(jì)誤差均方根統(tǒng)計(jì)結(jié)果(50次)如表3所示。結(jié)果表明，無(wú)論失準(zhǔn)角大小，基于矩陣濾波對(duì)準(zhǔn)方案三個(gè)失準(zhǔn)角估計(jì)精度都能達(dá)到0.03°(誤差均方根)以下，均優(yōu)于傳統(tǒng)線(xiàn)性模型和非線(xiàn)性模型方法。該方案兼顧了傳遞對(duì)準(zhǔn)的快速性和準(zhǔn)確性。

表3 失準(zhǔn)角估計(jì)結(jié)果Table 3 Monte-Carlo means and RMS of the misalignment

4 結(jié) 論

針對(duì)傳統(tǒng)傳遞對(duì)準(zhǔn)需區(qū)分失準(zhǔn)角大小而確定對(duì)準(zhǔn)模型和濾波算法的問(wèn)題，提出以姿態(tài)矩陣為狀態(tài)的統(tǒng)一無(wú)奇異傳遞對(duì)準(zhǔn)模型，并推導(dǎo)了基于比力積分+角速度積分為量測(cè)的矩陣卡爾曼濾波傳遞對(duì)準(zhǔn)濾波算法，該算法適用于任意失準(zhǔn)角，且與傳統(tǒng)方法相比，無(wú)需初值、無(wú)需主慣導(dǎo)初始參數(shù)裝訂，即傳遞對(duì)準(zhǔn)不再區(qū)分粗、精過(guò)程。仿真結(jié)果表明，無(wú)論失準(zhǔn)角大小，在搖擺運(yùn)動(dòng)下算法均能夠在10 s內(nèi)快速完成對(duì)準(zhǔn),相比于傳統(tǒng)對(duì)準(zhǔn)方法，精度和快速性均有所提高，為戰(zhàn)術(shù)武器任意失準(zhǔn)角條件下的快速傳遞對(duì)準(zhǔn)提供了參考。