嚴(yán)恭敏，陳若彤，郭 鹍

（1.西北工業(yè)大學(xué) 自動化學(xué)院，西安 710072；2.中船航海科技有限責(zé)任公司，北京 100070）

捷聯(lián)慣導(dǎo)系統(tǒng)初始對準(zhǔn)的重點在于初始姿態(tài)陣的求解，它通?？煞譃闉V波估計和確定性算法兩種方法[1]。濾波方法通過建立系統(tǒng)誤差模型，模型中往往包含了慣性傳感器誤差，在系統(tǒng)建模準(zhǔn)確的情況下一般具有較好的對準(zhǔn)精度。然而，濾波方法通常只有在小失準(zhǔn)角誤差線性建模條件下才會比較有效，雖然大失準(zhǔn)模型及其非線性濾波方法研究較多，但實際工程使用的卻比較少。采用大失準(zhǔn)角模型通常也只是為了獲得粗略的失準(zhǔn)角，再轉(zhuǎn)而使用小失準(zhǔn)角線性模型，才能得到高精度的初始對準(zhǔn)結(jié)果[2]。

確定性姿態(tài)算法與模型是否線性無關(guān)，在航天器姿態(tài)確定中應(yīng)用較多，它同樣也適用于捷聯(lián)慣導(dǎo)的粗對準(zhǔn)過程，有時在慣導(dǎo)機動干擾不大的場合也能獲得較高的初始對準(zhǔn)精度，甚至達(dá)到精對準(zhǔn)的效果[3-4]。姿態(tài)確定算法早期研究主要源于解決衛(wèi)星姿態(tài)的確定問題。1964年Harold Black提出了利用兩組觀測矢量求取姿態(tài)轉(zhuǎn)換矩陣的代數(shù)方法TRIAD（Tri-Axial Attitude Determination，常稱為雙矢量定姿法）；1965年Grace Wahba提出了應(yīng)用觀測矢量進(jìn)行星體姿態(tài)確定的最小二乘優(yōu)化問題（常稱為 Wahba問題）[5]；1968年Davenport提出了q-method方法，采用四元數(shù)姿態(tài)描述方式解決 Wahba問題，或稱 QUEST估計方法（QUaternion ESTimator）[6]。由于早期計算機的運算能力有限，研究者們提出了一系列改進(jìn)的四元數(shù)優(yōu)化快速算法，比如FOAM（Fast Optimal Attitude Matrix）、ESOQ（EStimator of the Optimal Quaternion）、ESOQ2（Second ESOQ）等算法，主要用于降低計算量和提高數(shù)值魯棒性[7-10]。研究者普遍認(rèn)為，與姿態(tài)的矩陣優(yōu)化相比，四元數(shù)優(yōu)化具有更少的約束，因而研究工作主要圍繞四元數(shù)展開。實際上，Wahba問題的四元數(shù)優(yōu)化與矩陣優(yōu)化是完全等價的，前者相對于后者并不具有絲毫的姿態(tài)求解精度優(yōu)勢，反而是四元數(shù)及其快速算法更加煩瑣，快速算法的計算量降低程度對現(xiàn)代計算機而言幾乎可忽略不計。

本文簡要介紹了姿態(tài)矩陣的奇異值分解優(yōu)化與四元數(shù)特征向量求解優(yōu)化之間的等價性，在四元數(shù)優(yōu)化過程中提出了一種新的構(gòu)造四階矩陣的方法；為了增強姿態(tài)陣優(yōu)化算法的實用性，給出了一種快速奇異值分解優(yōu)化算法，其浮點乘法計算量與四元數(shù)優(yōu)化快速算法 ESOQ2相近；最后，對幾種算法之間的等價性進(jìn)行了仿真驗證。

1 定姿算法理論分析

1.1 問題描述

假設(shè)三維空間中有m(m＞2)個不共面的矢量Vi（i= 1,2,,m），在直角坐標(biāo)系b系和r系中同時對這些矢量進(jìn)行測量，結(jié)果分別記為和，由于存在測量誤差，它們只能近似滿足如下變換關(guān)系：

為了定量描述“最優(yōu)”性能，這里構(gòu)造指標(biāo)函數(shù)為

1.2 三階矩陣的SVD方法

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

將式(3)代入式(2)，得：

進(jìn)一步對式(5)作變換，可得：

其中，記

假設(shè)矩陣A可逆且其奇異值分解為A=UDVT，則式(6)可變?yōu)?/p>

由于奇異值σ1≥σ2≥σ3＞ 0（如果有奇異值近似等于零則需特別討論，文中暫不涉及），所以在式(9)中最后等號當(dāng)且僅當(dāng)時成立，這正好對應(yīng)于C*=I。根據(jù)可求得最優(yōu)姿態(tài)矩陣：

1.3 四階矩陣的特征向量方法

① QUEST方法1

其中，記

顯然，矩陣M為四階實對稱陣，其特征值均為實數(shù)且特征向量均為實向量。

對式(15)求導(dǎo)并令其等于零，可得：

這說明λ是矩陣M的特征值，是相應(yīng)的特征向量。將式(16)代入式(13)，可得：

從前述分析可知，必然有極大值等式λmax=σ1+σ2+σ3成立，這說明SVD姿態(tài)陣方法與特征向量四元數(shù)方法得到的結(jié)果是等價的。

② QUEST方法2

若將三維向量視為零標(biāo)量四元數(shù)，則式(5)可化為

比較式(13)和式(18)，發(fā)現(xiàn)它們在形式上完全一致，若記和i=A直接采用元素展開法可以驗證式(19)成立：

從而容易證明有M=N，當(dāng)然獲得M的運算量會略小些，但是計算N的過程更直觀些。因此，四階實對稱矩陣N的最大特征值λmax所對應(yīng)的單位特征向量即為所求的四元數(shù)

③ QUEST方法3

直接將式(2)中的誤差平方改寫成四元數(shù)的形式，為

將式(20)代入式(2)，可得指標(biāo)函數(shù)：

2 快速奇異值分解算法（FSVD）

對于式(7)，當(dāng)A可逆時B=ATA必定是對稱正定陣，求解矩陣B=(Bij)的特征多項式為

其中，

由于實對稱陣的特征值均為實數(shù)，根據(jù)三次方程的求根公式可直接求得f(λ) = 0的三個根，分別為

其中，

實對稱矩陣存在完備的正交特征向量系，即存在兩兩正交的特征向量，利用這一性質(zhì)有助于簡化B的特征向量求解過程。假設(shè)特征值經(jīng)過排序后有λ1≥λ2≥λ3＞0，以下按照特征值重復(fù)情況求解特征向量。

① 當(dāng)有三重特征值時，即λ1=λ2=λ3，則可直接構(gòu)造三個特征向量：

記C=λ2I-B，則有rank(C)=1，這說明C的三個行向量之間是線性相關(guān)的。在矩陣C中尋找絕對值最大的元素（理論上可以是任意不為0的元素），假設(shè)為Cij，則有：

其中，Ci表示矩陣C的第i行向量，而為矩陣C的第i行j列元素。

如果j=1或j=2，可直接構(gòu)造如下特征向量：

而如果j=3，則直接構(gòu)造如下特征向量：

當(dāng)λ1=λ2＞λ3時，使用如下方式構(gòu)造特征向量v1和v3：

而當(dāng)λ1＞λ2=λ3時，使用如下方式構(gòu)造特征向量v3和v1：

③ 當(dāng)有三個互異特征值時，與λ1對應(yīng)特征向量v1滿足如下方程：

同樣，若記C=λ1I-B，則有rank(C)=2，這說明C的三個行向量應(yīng)在同一平面上（規(guī)定零向量可在任意平面上），且至少有兩個是不相互平行的，計算如下三組行向量的叉乘：

理論上vt1、vt2、vt3三者相互平行，只要不是零向量均可作為特征向量，但在數(shù)值上可選取模值最大者，作為λ1對應(yīng)的特征向量v1。

對于特征向量v2求法類似于v1，而針對特征向量v3，可選擇v3=v1×v2。

最后，將特征向量v1、v2、v3做規(guī)范化處理，令

則有矩陣B的特征值和特征向量分解：

其中，Λ=diag(λ1λ2λ3)，且顯然V為單位正交陣。

由奇異值分解式UΛ1/2VT=A兩邊同時右乘VΛ-1/2VT，直接得最優(yōu)單位正交陣：

FSVD算法的浮點乘法計算量分析：三次方程求根式(23)～(25)中約30次；計算特征向量過程中，以情況③最為復(fù)雜，約需50次；計算最優(yōu)單位正交陣式(37)約需60次。因此，總計乘法約140次。當(dāng)然，在式(24)(25)(35)(37)中還需要2次三角函數(shù)、1次反三角函數(shù)和7次開方運算。

3 仿真驗證

本節(jié)從仿真的角度對前述理論分析進(jìn)行驗證，設(shè)計如下仿真步驟：

② 生成m=100個隨機測量向量誤差向量和加權(quán)因子，其中，

④ 分別采用 SVD、FSVD、QUEST1、QUEST2和QUEST3五種算法求解最優(yōu)姿態(tài)矩陣或四元數(shù)，除FSVD采用第 2節(jié)所提方法外，其它算法直接調(diào)用了Matlab的svd()和eig()函數(shù)；

⑤ 以SVD算法作為參考基準(zhǔn)，計算其它四種算法相對于SVD算法的失準(zhǔn)角誤差的模值；

從圖1可以看出，SVD和三種QUEST算法之間的誤差均很小，僅為10-15rad量級，該誤差主要由計算機的數(shù)值計算精度引起，這在實際應(yīng)用中完全可忽略不計。由于計算矩陣時進(jìn)行了平方運算，使得 FSVD的誤差約大一個數(shù)量級，在矩陣條件數(shù)很大的情況下計算機數(shù)值求解誤差可能會比較大。至此，通過仿真驗證，可以認(rèn)為SVD、FSVD、QUEST1、QUEST2和QUEST3五種多矢量定姿算法是相互等價的。

圖1 各種算法之間的誤差Fig.1 Error comparisons for the five algorithms

4 結(jié) 論

針對姿態(tài)確定的Wahba問題，證明了姿態(tài)矩陣的奇異值分解算法與三種四元數(shù)特征向量求解算法之間的等價性，給出了一種快速奇異值分解算法，對幾種算法進(jìn)行了仿真驗證。理論分析和仿真實驗表明，對于當(dāng)前計算機的運算能力而言，各種算法之間的數(shù)值計算精度和計算量并無顯著差別，均可滿足實際應(yīng)用要求。建議今后大家在撰寫論文時，特別是在基于慣性系的初始對準(zhǔn)相關(guān)論文中，直接采用姿態(tài)陣進(jìn)行表示并用奇異值分解方法進(jìn)行求解即可，這樣更加簡潔和直觀，而沒必要采用更復(fù)雜和不易理解的四元數(shù)描述，看似提升了論文的理論水平，其實兩者的本質(zhì)和結(jié)果是完全等效的[11-12]。