蒲 輝,辛長(zhǎng)范,王瑞華,陳 銘

(1.中北大學(xué),山西 太原 030051；2.晉西工業(yè)集團(tuán)有限責(zé)任公司，山西 太原 030051)

對(duì)于空間飛行器姿態(tài)的確定,現(xiàn)在已經(jīng)開發(fā)出了多種根據(jù)矢量觀測(cè)的方法。最早和最簡(jiǎn)單的方法是TRIAD算法,最初由Black創(chuàng)建[1],Bar-Itzhack和Harman提出了TRIAD算法的最優(yōu)解[2],后來被Shuster證實(shí)[3]。然而,TRIAD算法利用兩個(gè)非平行矢量確定載體的三軸姿態(tài),且存在對(duì)主矢量敏感,不能利用第3根矢量的測(cè)量信息等缺點(diǎn),因此,它的準(zhǔn)確性是有限的,不能利用額外的觀測(cè)矢量來減少總姿態(tài)估計(jì)誤差。黎湧提出一種改進(jìn)的TRIAD算法,克服了主矢量敏感的問題[4],陳浩為了提高飛行器姿態(tài)測(cè)量精度和簡(jiǎn)化姿態(tài)確定的復(fù)雜性,提出了一種基于加權(quán)矢量和的TRIAD飛行器姿態(tài)確定算法[5]。

Wahba提出了使用任意數(shù)量的觀測(cè)向量的一般姿態(tài)估計(jì)問題[6],解決Wahba問題的算法分為兩類,第一類是求解表示姿態(tài)矩陣的四元數(shù),Davenport為求解飛行器姿態(tài)提出了q方法[7],夏家和運(yùn)用q方法有效解決了捷聯(lián)慣導(dǎo)的動(dòng)態(tài)粗對(duì)準(zhǔn)問題[8],高薪驗(yàn)證了四元數(shù)估計(jì)對(duì)準(zhǔn)算法姿態(tài)角誤差的收斂速度優(yōu)于雙矢量定姿對(duì)準(zhǔn)算法[9],徐曉蘇針對(duì)直接構(gòu)建量測(cè)模型導(dǎo)致收斂速度慢的問題,提出一種基于最優(yōu)四元數(shù)估計(jì)法構(gòu)造K矩陣原理的改進(jìn)算法[10],譚彩銘基于幾何原理提出了在給定不完整雙矢量觀測(cè)信息時(shí)的最簡(jiǎn)姿態(tài)確定算法[11];第二類是直接求解姿態(tài)矩陣,Farrell和Stuelpnagel提出了奇異值分解SVD算法[12],何穎應(yīng)用奇異值分解導(dǎo)數(shù)的計(jì)算公式提出一種在變量投影框架下的多點(diǎn)透視問題求解算法[13],嚴(yán)恭敏針對(duì)姿態(tài)確定的Wahba問題,證明了姿態(tài)矩陣的奇異值分解SVD算法與三種四元數(shù)特征向量求解QUEST算法之間的等價(jià)性,給出了一種快速奇異值分解算法[14]。

VITAE(Vector Inertia Tensor Attitude Estimation，矢量慣性張量姿態(tài)估計(jì))是一種新的姿態(tài)計(jì)算方法,由Russell于2018年提出[15]。該方法類似于TRIAD方法,但是與TRIAD不同的是,VITAE可以使用任何數(shù)量的參考矢量和觀測(cè)矢量,并使用各自慣性主軸來定義相關(guān)的正交坐標(biāo)系。

本文介紹了基于VITAE算法的多信息融合姿態(tài)估計(jì)過程與誤差分析,使用Matlab對(duì)該算法編程,通過計(jì)算相同和不同觀測(cè)精度下不同的質(zhì)點(diǎn)權(quán)重分配對(duì)飛行器姿態(tài)估計(jì)誤差的影響,研究了如何給多傳感器信息融合的姿態(tài)估計(jì)算法進(jìn)行質(zhì)點(diǎn)權(quán)重分配。

1 多信息融合姿態(tài)估計(jì)算法及其誤差分析

1.1 姿態(tài)計(jì)算

多信息融合姿態(tài)估計(jì)算法的輸出是載體坐標(biāo)系到慣性坐標(biāo)系的變換矩陣。首先,在每個(gè)參考矢量和相關(guān)觀測(cè)矢量的頂端放置一個(gè)質(zhì)點(diǎn),并計(jì)算出此質(zhì)點(diǎn)系的慣性張量。這樣,觀測(cè)矢量中包含的信息就轉(zhuǎn)移到了慣性張量中。在觀測(cè)矢量無誤差的情況下,載體坐標(biāo)系慣性張量的特征值與慣性坐標(biāo)系慣性張量的特征值相同。對(duì)每個(gè)慣性張量對(duì)角化可得到特征值、特征向量和主軸坐標(biāo)系與各坐標(biāo)系之間的變換矩陣。通過找到各自主軸坐標(biāo)系的變換,就可以計(jì)算出載體坐標(biāo)系與慣性坐標(biāo)系之間的方向余弦矩陣,從而得到姿態(tài)估計(jì)值。

分配給每個(gè)矢量的質(zhì)點(diǎn)的權(quán)重可以根據(jù)其相關(guān)的測(cè)量誤差進(jìn)行調(diào)整,以獲得更精確的姿態(tài)估計(jì)值。由于慣性張量是對(duì)稱的,有兩個(gè)或三個(gè)相同的特征值的情況下,特征向量是退化的,不能定義一個(gè)唯一的正交坐標(biāo)系。在這些情況下,可以調(diào)整質(zhì)點(diǎn)的權(quán)重來消除退化性,恢復(fù)姿態(tài)估計(jì)的準(zhǔn)確性。如果在載體坐標(biāo)系中得到的觀測(cè)矢量慣性的特征值與慣性坐標(biāo)系中得到的參考矢量慣性的相關(guān)特征值非常接近,則姿態(tài)估計(jì)可以非常準(zhǔn)確。

(1)

I=m[tr(C)(Id)-C]

(2)

其中，Id是3*3的單位矩陣。對(duì)于一組參考矢量,慣性坐標(biāo)系上的總慣性張量II是所有參考矢量及對(duì)應(yīng)質(zhì)點(diǎn)的慣性張量之和。

(3)

由式(3)確定的慣性張量是一個(gè)實(shí)對(duì)稱的3*3矩陣,可以很容易地進(jìn)行對(duì)角化,得到其特征值和特征向量。特征方程為

IXi=λiXi

(4)

其中Xi和λi分別為第i個(gè)特征向量和特征值。將方程(4)轉(zhuǎn)換到主軸坐標(biāo)系,得

(5)

其中，Ix、Iy、Iz為沿各自主軸的慣性矩,λ1、λ2、λ3為各自的特征值。令UI為從主軸坐標(biāo)系到慣性坐標(biāo)系的變換,令Ub為從主軸坐標(biāo)系到載體坐標(biāo)系的變換,即

Xb=UbXbXI=UIXd

(6)

由于Ub將主軸坐標(biāo)系中的特征向量轉(zhuǎn)換為載體坐標(biāo)系,所以Ub的每一列是載體坐標(biāo)系中的特征向量。同樣,UI的每一列也是慣性坐標(biāo)系中的特征向量。因此,將載體坐標(biāo)系中的慣性張量和慣性坐標(biāo)系中的慣性張量對(duì)角化,就可以得到各自的特征向量和變換矩陣Ub和UI。

消除式(6)中的Xd,得到慣性坐標(biāo)系和載體坐標(biāo)系之間的變換,即

(7)

因此,載體姿態(tài)的估計(jì)值A(chǔ)為

(8)

如果參考矢量和觀測(cè)矢量沒有誤差,則由式(8)給出的載體姿態(tài)估計(jì)可以得到精確的解。在實(shí)際工作中,參考矢量與觀測(cè)矢量的誤差相比,其誤差可以忽略不計(jì)。因此,UI可以準(zhǔn)確計(jì)算,姿態(tài)估計(jì)誤差僅由觀測(cè)矢量的誤差引起。總之,一旦利用向量和權(quán)重計(jì)算出各自的慣性張量,就通過對(duì)各自慣性張量的對(duì)角線化找到特征向量。然后用特征向量構(gòu)造各自的變換矩陣,用來計(jì)算從慣性坐標(biāo)系到載體坐標(biāo)系的變換。

1.2 誤差分析

(9)

(10)

I=I0+Q

(11)

其中

(12)

在式(11)中,I0為觀測(cè)矢量無誤差時(shí)的慣性張量,Q為觀測(cè)矢量有誤差時(shí)I的誤差。

將一階擾動(dòng)理論應(yīng)用于式(4)中的特征值問題,研究Q對(duì)載體坐標(biāo)系慣性張量相關(guān)的特征值、特征向量和變換矩陣的影響。若觀測(cè)向量的誤差很小,則無誤差特征值問題的解如下

I0X0i=λ0iX0i

(13)

在式(13)中,與慣性張量相關(guān)的特征值只有三個(gè),而相關(guān)的特征向量構(gòu)成了一個(gè)完整的正交向量集。

當(dāng)存在誤差時(shí),則特征方程為

IXi=λiXi

(14)

其中I由式(11)給出,λi由式(15)給出,Xi由式(16)給出:

λi=λ0i+λ1i

(15)

(16)

在式(15)和(16)中只保留了一階誤差項(xiàng),λ1i為特征值的一階誤差。由于式(13)中的未擾動(dòng)特征向量形成了一個(gè)完整的正交集合,所以Xi的一階誤差是式(16)中的求和項(xiàng),其中ami為系數(shù)。因此,公式(16)為一階近似解和由此產(chǎn)生的姿態(tài)的最小二乘最優(yōu)估計(jì)值。將公式(11)、(15)和(16)代入(14),并去掉由(13)給出的零階項(xiàng),可以發(fā)現(xiàn):

(17)

將式(17)乘以X0i,并利用特征向量的正交性,得到特征值的一階誤差為

(18)

這個(gè)結(jié)果可用來計(jì)算姿態(tài)估計(jì)的置信度,因?yàn)樘卣髦档恼`差很小,則Q也很小。式(17)中的系數(shù)ami,可以通過將式(18)乘以X0m來求得,其結(jié)果為

(19)

將式(19)代入式(16)中,可得到特征向量的一階近似值。如果式(19)中的特征值相互接近,則可能違背一階近似的假設(shè),因?yàn)閍mi與特征值的差值成反比。因此Q必須遠(yuǎn)小于特征值的差值,才能使特征向量的一階誤差較小?？傊?觀測(cè)向量的誤差會(huì)影響慣性張量,從而影響特征向量。由于特征向量形成了一個(gè)正交集合,所以式(16)中的一階擾動(dòng)誤差相當(dāng)于一個(gè)微小轉(zhuǎn)動(dòng),稱為姿態(tài)誤差向量，即

(20)

因此,姿態(tài)誤差矢量的轉(zhuǎn)動(dòng)角可用系數(shù)aij來定義,如式(21)所示。

(21)

定義姿態(tài)協(xié)方差矩陣Pθθ為姿態(tài)誤差矢量的協(xié)方差矩陣,如式(22)所示,其中E表示期望值。

Pθθ=E{ΔθΔθT}=

(22)

由于協(xié)方差矩陣中的每個(gè)項(xiàng)都涉及特征值之差,如式(19)所示,如果特征值在數(shù)值上非常接近,估計(jì)精度就會(huì)下降。對(duì)于固定的觀測(cè)向量,可以調(diào)整各質(zhì)點(diǎn)的權(quán)重來增加特征值的差異,從而提高姿態(tài)估計(jì)精度。當(dāng)然,調(diào)整質(zhì)量后的姿態(tài)估計(jì)將不再是最優(yōu)的,但是,數(shù)值結(jié)果表明,姿態(tài)估計(jì)精度的損失非常小。下面將通過數(shù)值仿真研究各質(zhì)點(diǎn)的權(quán)重對(duì)姿態(tài)估計(jì)精度的影響。

2 質(zhì)量分配對(duì)估計(jì)誤差影響的仿真研究

設(shè)慣性系下的兩個(gè)參考矢量分別為：

r1=(1,0,0)T；r2=(0,1,0)T

(23)

對(duì)應(yīng)的觀測(cè)矢量的均值為0,方差分別為：

σ1=1e-2rad；σ2=1e-2rad

(24)

(25)

其中,ηi為零均值高斯噪聲,A為由式(26)給出的慣性坐標(biāo)系到載體坐標(biāo)系的變換矩陣。

(26)

圖1 質(zhì)點(diǎn)權(quán)重m1對(duì)特征值的影響

進(jìn)一步研究質(zhì)點(diǎn)權(quán)重對(duì)姿態(tài)估計(jì)誤差的影響,改變m1及m2,計(jì)算姿態(tài)估計(jì)值及姿態(tài)估計(jì)誤差,同時(shí)進(jìn)行10 000次蒙特卡洛模擬仿真計(jì)算,通過計(jì)算出姿態(tài)協(xié)方差矩陣Pθθ,計(jì)算流程圖如圖2所示,得到姿態(tài)估計(jì)誤差的變化如圖3所示。

圖2 計(jì)算流程圖

在圖3中,給兩個(gè)觀測(cè)向量分配權(quán)重以獲得最優(yōu)解,當(dāng)m1接近0時(shí),估計(jì)誤差約為1.13×10-2rad;m1=m2時(shí),估計(jì)誤差約為7.89×10-3rad;m1接近1時(shí),估計(jì)誤差約為8.89×10-3rad。由此可知,在測(cè)量精度相同時(shí),兩個(gè)觀測(cè)矢量質(zhì)點(diǎn)分配相同的權(quán)重可以得到最準(zhǔn)確的測(cè)量誤差,得到最優(yōu)解。

若σ1<σ2,令σ1=1×10-4rad,σ2=1×10-2rad,再進(jìn)行仿真計(jì)算,得到在σ1<σ2時(shí),質(zhì)點(diǎn)權(quán)重m1對(duì)估計(jì)誤差的影響,如圖4所示,當(dāng)m1接近0時(shí),估計(jì)誤差約為2.37×10-3rad;m1=m2時(shí),估計(jì)誤差約為3.7×10-3rad;m1接近1時(shí),估計(jì)誤差約為8.1×10-5rad。

圖4 σ1=1×10-4 rad,σ2=1×10-2 rad時(shí)，m1對(duì)估計(jì)誤差的影響

令σ1=1×10-4rad,σ2=1×10-3rad,再進(jìn)行仿真計(jì)算,得到質(zhì)點(diǎn)權(quán)重m1對(duì)估計(jì)誤差的影響,如圖5所示,估計(jì)誤差較圖4中更小,當(dāng)m1接近0時(shí),估計(jì)誤差約為1.7×10-3rad;m1=m2時(shí),估計(jì)誤差約為7.66×10-4rad;m1接近1時(shí),估計(jì)誤差約為3.79×10-5rad。

圖5 σ1=1×10-4 rad,σ2=1×10-3 rad時(shí)，m1對(duì)估計(jì)誤差的影響

由此可知,在測(cè)量精度不同時(shí),給測(cè)量精度高的觀測(cè)矢量分配更高的權(quán)重能夠顯著提高估計(jì)誤差精度,得到更優(yōu)的姿態(tài)估計(jì)。

3 結(jié)束語

本文通過對(duì)多信息融合姿態(tài)估計(jì)算法中的質(zhì)點(diǎn)權(quán)重分配進(jìn)行研究,得出如下結(jié)論:在多信息融合姿態(tài)估計(jì)算法中,兩個(gè)觀測(cè)矢量的精度相近時(shí),質(zhì)點(diǎn)應(yīng)分配相同的權(quán)重,而兩個(gè)觀測(cè)矢量的精度不同時(shí),精度高的觀測(cè)矢量應(yīng)分配更高的權(quán)重,這樣才能得到最優(yōu)的姿態(tài)估計(jì)。此結(jié)果對(duì)姿態(tài)估計(jì)的精度分析具有一定的價(jià)值。