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

劉曉輝， 黨亞民，王潛心

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

一、引 言

(1)

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

二、最優化定姿方法

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

定義函數g(A)如下

(2)

當g(A)取最大值時，L(A)最小，因而問題轉化為求得使g(A)最大的Aopt，對B進行奇異值分解，可以得到

B=USVT

(3)

令

d=det(U)det(V)

(4)

則

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

(5)

2. 計算矩陣所有特征值和特征向量的最優化方法

Davenport[3]把姿態矩陣A轉化為四元數[9]形式，四元數定義式為

(6)

姿態矩陣和四元數的關系如下

(7)

(8)

(9)

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

(10)

令

(11)

(12)

(13)

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

g(q)=qTKq

(14)

式中

(15)

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

Kqopt=λmaxqopt

(16)

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

3. 迭代求解特征值法

定義Gibbs向量Y如下

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

(17)

則四元數q可以用Gibbs向量Y表示為

(18)

則式(14)可以變形為

(19)

λ=σ+Z·Y

(20)

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

(21)

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

det|S-ξI|=0

(22)

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

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

(23)

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

根據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)求得最優四元數。為了防止迭代發散和加快收斂速度，選用牛頓下山法作為迭代方法，該方法同時具有牛頓法和下山法的優點，即在下山法保證函數穩定下降的前提下，用牛頓法加快收斂速度。

其迭代公式為

(27)

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

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

(28)

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

三、試驗分析

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

選取載體坐標系下的基線W1、W2、W3、W4、W5、W6如下

姿態矩陣為

如果沒有誤差，則V1、V2、V3、V4、V5、V6應為

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

解算結果的精度如何，需要根據罰函數進行判定，根據3種方法對100個歷元解算結果得到的罰函數平均值見表1，各個歷元的罰函數如圖1所示。

表1 3種方法罰函數平均值

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

圖1 3種方法的罰函數

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

圖2 航向角與真值誤差

圖3 俯仰角與真值誤差

圖4 翻滾角與真值誤差

表2 姿態解算結果的標準差和平均值與真值之差 (°)

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

四、結束語

本文分析了兩種常用的最優化定姿方法及其存在的問題，提出了一種基于四元數的最優姿態解算方法。該方法首先利用牛頓下山法求解大特征值，然后根據特征值和最優四元數的關系求解最優四元數，從而求出姿態參數。基于奇異值分解的定姿方法由于需要對矩陣進行奇異值分解，對計算機要求較高；第二種方法需要求解出所有的特征值和特征向量，然后找到最大特征值對應的特征向量，該方法由于涉及矩陣特征值和特征向量的求解，因此對計算機要求也較高。本文所提方法由于只涉及對矩陣和向量的基本運算，因此對計算機的要求較低，便于編程實現。通過對仿真數據解算結果的分析可以發現：本文所提方法和前兩種最優化方法的解算結果一致，完全可以達到要求的定姿精度。

參考文獻：

[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] 程國采. 四元數法及其應用[M].長沙：國防科技大學出版社，1991.

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