一種迭代求解特征值的定姿方法

劉曉輝， 黨亞民，王潛心

(中國測繪科學(xué)研究院，北京 100830)

一、引 言

(1)

F.L.Markley提出，一種利用奇異值分解(UVD)求解最優(yōu)姿態(tài)矩陣Aopt的方法[6-7]；Davenport把二次罰函數(shù)轉(zhuǎn)換成四元數(shù)形式[3]，這是對Wahba問題的極大簡化，因?yàn)榍蠼庾顑?yōu)四元數(shù)比求解有9個(gè)元素的最優(yōu)姿態(tài)矩陣所受約束更少；Keat詳細(xì)說明了如何通過計(jì)算特征值和特征向量來求解最優(yōu)四元數(shù)[8]。但是對矩陣進(jìn)行奇異值分解，或直接求解矩陣特征值和特征向量對計(jì)算機(jī)要求較高，因此本文提出一種最優(yōu)化的直接計(jì)算的方法。

二、最優(yōu)化定姿方法

1. 基于奇異值分解(UVD)的最優(yōu)化定姿方法[6-7]

定義函數(shù)g(A)如下

(2)

當(dāng)g(A)取最大值時(shí)，L(A)最小，因而問題轉(zhuǎn)化為求得使g(A)最大的Aopt，對B進(jìn)行奇異值分解，可以得到

B=USVT

(3)

令

d=det(U)det(V)

(4)

則

Aopt=U[diag(1,1,d)]VT

(5)

2. 計(jì)算矩陣所有特征值和特征向量的最優(yōu)化方法

Davenport[3]把姿態(tài)矩陣A轉(zhuǎn)化為四元數(shù)[9]形式，四元數(shù)定義式為

(6)

姿態(tài)矩陣和四元數(shù)的關(guān)系如下

(7)

(8)

(9)

將式(7)代入式(2)得

(10)

令

(11)

(12)

(13)

則函數(shù)g(q)可以表示為關(guān)于四元數(shù)q的二次型函數(shù)

g(q)=qTKq

(14)

式中

(15)

考慮到式(4)的約束條件，利用拉格朗日乘子法可以證明[8]使g(q)達(dá)到最大的四元數(shù)恰好是矩陣K的最大特征值所對應(yīng)的特征向量，即

Kqopt=λmaxqopt

(16)

如果直接計(jì)算，就需要求出矩陣K的所有特征值和特征向量，然后找到最大特征值所對應(yīng)的特征向量，即為所求最優(yōu)四元數(shù)qopt，把其代入式(9)即可求出姿態(tài)矩陣。

3. 迭代求解特征值法

定義Gibbs向量Y如下

Y=Q/q=ntan(θ/2)

(17)

則四元數(shù)q可以用Gibbs向量Y表示為

(18)

則式(14)可以變形為

(19)

λ=σ+Z·Y

(20)

將式(17)代入式(18)得

(21)

若ξ是任意方陣S的特征值，則它們滿足如下關(guān)系

det|S-ξI|=0

(22)

式(22)可以表示為如下形式

-ξ3+2σξ2+κξ+Δ=0

(23)

式中，σ=0.5tr(S)=tr(B)；κ=tr(adjS)；Δ=detH。

根據(jù)Cayley-Hamilton原理，S滿足如下等式

S3=2σS2-κS+ΔI

(24)

對式(24)變形可以得到如下等式

(25)

式中，α=λ2-σ2+κ；β=λ-σ；γ=(λ+σ)α-Δ。

把式(25)代入式(21)并整理，得

λ4-(a+b)λ2-cλ+(ab+cσ-d)=0

(26)

式中，a=σ2-tr(adjS)；b=σ2+ZTZ；c=detS+ZTSZ；d=ZTS2Z。

對上式利用迭代方法即可求出λ，然后將其代入式(18)、式(19)求得最優(yōu)四元數(shù)。為了防止迭代發(fā)散和加快收斂速度，選用牛頓下山法作為迭代方法，該方法同時(shí)具有牛頓法和下山法的優(yōu)點(diǎn)，即在下山法保證函數(shù)穩(wěn)定下降的前提下，用牛頓法加快收斂速度。

其迭代公式為

(27)

式中，ω為下山因子，ω的取值從1開始逐次減半直至滿足|f(xk+1)|<|f(xk)|。

關(guān)于λ迭代初值的選擇，把式(16)代入式(14)并考慮式(2)得

(28)

從上式可以看出，λmax≈1，因此用1作為初值比較合理。

三、試驗(yàn)分析

本節(jié)將分別用奇異值分解方法(方法1)、求解所有特征值和特征向量方法(方法2)和直接求解最大特征值方法(方法3)對仿真基線數(shù)據(jù)進(jìn)行處理，然后對各種方法的計(jì)算精度和代價(jià)函數(shù)進(jìn)行分析。

選取載體坐標(biāo)系下的基線W1、W2、W3、W4、W5、W6如下

姿態(tài)矩陣為

如果沒有誤差，則V1、V2、V3、V4、V5、V6應(yīng)為

由于傳感器觀測存在誤差，V1、V2、V3、V4、V5、V6不可能準(zhǔn)確地獲得上面的數(shù)值。假設(shè)基線誤差服從零均值高斯分布，根據(jù)基線精度和姿態(tài)角精度的關(guān)系[10]，并參考實(shí)際測量中姿態(tài)角的測量精度，在V1、V2、V3、V4、V5、V6中都加入均值為零、標(biāo)準(zhǔn)差為0.000 1 m的隨機(jī)白噪聲。對每種情況進(jìn)行100次仿真，并利用上述3種方法對仿真數(shù)據(jù)進(jìn)行解算。

解算結(jié)果的精度如何，需要根據(jù)罰函數(shù)進(jìn)行判定，根據(jù)3種方法對100個(gè)歷元解算結(jié)果得到的罰函數(shù)平均值見表1，各個(gè)歷元的罰函數(shù)如圖1所示。

表1 3種方法罰函數(shù)平均值

從表1可以看出，3種方法的罰函數(shù)平均值都很小，且3種方法的罰函數(shù)平均值完全相同，說明姿態(tài)解算的精度很高。

圖1 3種方法的罰函數(shù)

從圖1可以看出，3種方法的罰函數(shù)完全重合，這說明它們的定姿結(jié)果很可能完全相同。為了進(jìn)一步分析，下面將每個(gè)歷元的姿態(tài)角都計(jì)算出來，并將其與真值進(jìn)行比較。3種方法計(jì)算得到的航向角、俯仰角、翻滾角與真值的誤差如圖2—圖4所示，姿態(tài)解算結(jié)果的平均值與真值的差值及標(biāo)準(zhǔn)差見表2。

圖2 航向角與真值誤差

圖3 俯仰角與真值誤差

圖4 翻滾角與真值誤差

表2 姿態(tài)解算結(jié)果的標(biāo)準(zhǔn)差和平均值與真值之差 (°)

通過圖2—圖4和表2可以看出，3種方法的定姿結(jié)果完全相同，航向角、俯仰角和翻滾角的定姿精度都能達(dá)到0.2°左右，最大偏差不超過0.5°。

四、結(jié)束語

本文分析了兩種常用的最優(yōu)化定姿方法及其存在的問題，提出了一種基于四元數(shù)的最優(yōu)姿態(tài)解算方法。該方法首先利用牛頓下山法求解大特征值，然后根據(jù)特征值和最優(yōu)四元數(shù)的關(guān)系求解最優(yōu)四元數(shù)，從而求出姿態(tài)參數(shù)?；谄娈愔捣纸獾亩ㄗ朔椒ㄓ捎谛枰獙仃囘M(jìn)行奇異值分解，對計(jì)算機(jī)要求較高；第二種方法需要求解出所有的特征值和特征向量，然后找到最大特征值對應(yīng)的特征向量，該方法由于涉及矩陣特征值和特征向量的求解，因此對計(jì)算機(jī)要求也較高。本文所提方法由于只涉及對矩陣和向量的基本運(yùn)算，因此對計(jì)算機(jī)的要求較低，便于編程實(shí)現(xiàn)。通過對仿真數(shù)據(jù)解算結(jié)果的分析可以發(fā)現(xiàn)：本文所提方法和前兩種最優(yōu)化方法的解算結(jié)果一致，完全可以達(dá)到要求的定姿精度。

參考文獻(xiàn)：

[1] BAR-ITZHACK I Y, HARMAN R R. Optimized TRIAD Algorithm for Attitude Determination[J].Journal of Guidance Control and Dynamics, 1997, 20(1):208-211.

[2] FARRELL J L, STUELPNAGEL J C, WESWNER R H, et al. A Least Squares Estimate of Spacecraft Attitude[J].SIAM Review,1966,8(3):384-386.

[3] DAVENPORT P B. A Vector Approach to the Algebra of Rotations with Applications[R].Washington D.C.：NASA，1965.

[4] BLACK H D.A Passive System for Determining the Attitude of a Satellite[J]. AIAA Journal,1964,2(7)：1350-1351.

[5] Wahba G.A Least Squares Estimate of Satellite Attitude, Problem 65.1[J].SIAM Review,1965,7(3)：409.

[6] MARKLEY F L. Attitude Determination Using Vector Observation and the Singular Value Decomposition [J]. The Journal of the Astronautical Sciences. 1988, 36(3): 245-258.

[7] MARKLEY F L. Attitude Determination Using Vector Observation: A Fast Optimal Matrix Algorithm [J].The Journal of the Astronautical Sciences(S0021-9142). 1993, 41(2): 261-281.

[8] KEAT J E. Analysis of Least-square Attitude Determination Routine DOAOP[R].[S.l.]:Computer Science Corp., Report，1977.

[9] 程國采. 四元數(shù)法及其應(yīng)用[M].長沙：國防科技大學(xué)出版社，1991.

[10] LU G. Development of a GPS Multi-antenna System for Attitude Determination[D].Calgary, Canada：The University of Calgary，1995.