基于多矢量定姿的動(dòng)基座最優(yōu)化對準(zhǔn)算法*

劉文倩, 程向紅, 曹 鵬, 丁 鵬

(1.東南大學(xué) 微慣性儀表與先進(jìn)導(dǎo)航技術(shù)教育部重點(diǎn)實(shí)驗(yàn)室·南京·210096；2.東南大學(xué) 儀器科學(xué)與工程學(xué)院·南京·210096；3.上海航天控制技術(shù)研究所·上海·201109)

0 引 言

初始對準(zhǔn)技術(shù)是捷聯(lián)慣導(dǎo)系統(tǒng)的核心問題和關(guān)鍵技術(shù)。初始對準(zhǔn)本質(zhì)上是確定初始姿態(tài)矩陣的過程[1-6]，包括粗對準(zhǔn)階段和精對準(zhǔn)階段[7-8]。粗對準(zhǔn)是在短時(shí)間內(nèi)提供一個(gè)大致已知的初始姿態(tài)矩陣，為精對準(zhǔn)做好準(zhǔn)備。代表性的粗對準(zhǔn)方法主要包括靜基座或準(zhǔn)靜基座下的解析法，以及動(dòng)態(tài)環(huán)境下的基于優(yōu)化的對準(zhǔn)方法。解析法以地球自轉(zhuǎn)角速度和重力加速度作為信息源[9]。雖然解析法原理簡單，計(jì)算量小，但其僅利用了兩個(gè)不同時(shí)刻的矢量觀測信息，信息利用率不高，對準(zhǔn)精度較低[8]。因此，可構(gòu)建觀測矢量組[10-14]，將對準(zhǔn)問題轉(zhuǎn)化為多矢量定姿問題——即Wahba問題的求解。Wu和Pan等[10,15]采用最優(yōu)對準(zhǔn)(Optimization-Based Align-ment, OBA)方法提高了信息的利用率，從而提高了對準(zhǔn)精度。但是，OBA方法的主要缺點(diǎn)是無法抵抗由陀螺偏置和隨機(jī)噪聲帶來的影響。為了解決這些問題，Huang等[16]提出了一種新的快速運(yùn)動(dòng)粗對準(zhǔn)方法(DAEBIA-FT)用于低成本的SINS/GPS系統(tǒng)。Chang等[17]將姿態(tài)對準(zhǔn)問題轉(zhuǎn)化成了姿態(tài)估計(jì)問題。但是，這些方法均需建立非線性狀態(tài)空間模型，由此增加了計(jì)算難度。

1 坐標(biāo)系定義

常用坐標(biāo)系的定義如下：

(1)地球坐標(biāo)系(e系)—oexeyeze：地球坐標(biāo)系的原點(diǎn)位于地心，oeze軸沿地球自轉(zhuǎn)方向，oexe軸在赤道平面內(nèi)，由地心指向載體所處子午線。e系與地球固聯(lián)，隨地球自轉(zhuǎn)而轉(zhuǎn)。

(2)導(dǎo)航坐標(biāo)系(n系)—onxnynzn：導(dǎo)航坐標(biāo)系的原點(diǎn)位于載體重心，onxn軸指向水平東向，onyn軸指向水平北向，onzn軸垂直于當(dāng)?shù)匦D(zhuǎn)橢球面并指向天。

(3)載體坐標(biāo)系(b系)—obxbybzb：載體坐標(biāo)系的原點(diǎn)位于載體重心，obxb軸指向載體橫軸向右，obyb軸指向載體縱軸向前，obzb軸沿載體立軸向上。

(4)初始時(shí)刻載體慣性坐標(biāo)系(b(0)系)—ob(0)xb(0)yb(0)zb(0)：初始時(shí)刻將b系凝固后形成的坐標(biāo)系。

(5)初始時(shí)刻導(dǎo)航慣性坐標(biāo)系(n(0)系)—on(0)xn(0)yn(0)zn(0)：初始時(shí)刻將n系凝固后形成的坐標(biāo)系。

2 初始對準(zhǔn)算法

傳統(tǒng)雙矢量定姿要求兩個(gè)量測矢量具有極高的準(zhǔn)確度，量測不準(zhǔn)確會(huì)產(chǎn)生較大的對準(zhǔn)誤差。為克服該缺陷并提高對準(zhǔn)精度，本文采用多矢量提供量測信息并進(jìn)行對準(zhǔn)，通過多個(gè)量測信息求解最優(yōu)正交矩陣，將對準(zhǔn)問題轉(zhuǎn)化成了Wahba問題。

2.1 矢量信息的計(jì)算

(1)

(2)

(3)

(4)

進(jìn)行分解可得

(5)

(6)

將式(6)左邊定義為β，表示為

(7)

(8)

將β中的因式離散化求解如下

(9)

其中，T為姿態(tài)更新周期，k為更新次數(shù)，I表示單位矩陣。對α的離散化求解如下

(10)

(11)

2.2 Wahba 問題及SVD算法

(12)

其中，wi為權(quán)重系數(shù)，本文取為1。該式反映同一矢量在兩個(gè)坐標(biāo)系中測量值的不一致誤差。

對式(12)中的誤差平方進(jìn)行如下的等價(jià)變形

(13)

將式(13)代入式(12)，得

(14)

(15)

其中，αi、βi(i=1,2,…,m)表示b(0)系下觀測矢量和n(0)系下觀測矢量的第i個(gè)測量值。將式(15)中的矩陣A記為

(16)

假設(shè)矩陣A可逆且對其進(jìn)行奇異值分解得到A=UDVT。其中，U和V都是酉矩陣，D表示由矩陣A的奇異值構(gòu)成的對角矩陣，表示為D=diag(σ1,σ2,σ3)，則式(15)可變?yōu)?/p>

=tr(C*D)

=max

(17)

≤σ1+σ2+σ3

(18)

(19)

2.3 算法優(yōu)化

(20)

(21)

其中，φ×表示φ對應(yīng)的反對稱矩陣。將式(21)代入式(20)并變形，可得

(22)

根據(jù)姿態(tài)轉(zhuǎn)換矩陣的微分方程，可得

(23)

(24)

將式(22)代入式(23)，忽略二階小量，可得

(25)

令式(24)與式(25)相等，如下

(26)

經(jīng)過簡化整理，得到

(27)

文中，將陀螺零偏定義為常值，因此得到

(28)

(29)

(30)

對式(30)進(jìn)行整理移項(xiàng)，可得到量測模型

(31)

(32)

Zk=Hkφ

(33)

(34)

(35)

式中，Hk為k時(shí)刻的量測矩陣，Kk表示濾波增益，Pk表示狀態(tài)估計(jì)的均方誤差陣，ek表示新息。式中，dk=(1-b)/(1-bk+1)，0

3 仿真實(shí)驗(yàn)及分析

3.1 數(shù)學(xué)仿真實(shí)驗(yàn)

P(0)=diag{(0.01)2,(0.01)2,(0.01)2,

(0.01)2,(0.01)2,(0.01)2}

Q=diag{(0.01)2,(0.01)2,(0.01)2}

R=diag{(0.1)2,(0.1)2,(0.1)2}

仿真中，載體首先進(jìn)行60s的勻速運(yùn)動(dòng)，在60s～70s進(jìn)行加速運(yùn)動(dòng)，加速度為0.1m/s2，在80s時(shí)進(jìn)行拐彎運(yùn)動(dòng)，拐彎角度為40°，拐彎角速度為1(°)/s，并且伴有輕微的搖晃運(yùn)動(dòng)。航向角、俯仰角和橫滾角搖晃幅度分別為1°、2°、3°，搖晃角頻率分別為π/5(rad/s)、3π/10(rad/s)、2π/5(rad/s)。載體的運(yùn)動(dòng)軌跡如圖1所示，行駛過程的總時(shí)長為420s，最大速度約為7m/s。

圖1 仿真運(yùn)動(dòng)軌跡

圖2 姿態(tài)角誤差曲線

表1 350s～420s對準(zhǔn)結(jié)果的均方根值(RMSs)

3.2 半物理仿真實(shí)驗(yàn)

利用三軸轉(zhuǎn)臺(tái)進(jìn)行3組微幅晃動(dòng)條件下的對準(zhǔn)實(shí)驗(yàn)。航向角H、俯仰角P、橫滾角R的搖擺幅值和搖擺頻率設(shè)置如表2所示。陀螺儀和加速度計(jì)的主要技術(shù)指標(biāo)為：陀螺儀的常值漂移為0.01(°)/h，隨機(jī)游走為0.005(°)/h1/2，加速度計(jì)的常值偏置為0.1mg，隨機(jī)游走為0.1mg/Hz1/2。圖3表示第二組數(shù)據(jù)在分別采用傳統(tǒng)OBA方法和改進(jìn)OBA對準(zhǔn)方法時(shí)的對準(zhǔn)曲線。

表2 搖擺條件設(shè)置

圖3 姿態(tài)角誤差曲線

表3 3組實(shí)驗(yàn)的對準(zhǔn)結(jié)果

4 結(jié) 論