捷聯姿態計算中方向余弦與四元數法分析比較

【信息科學與控制工程】

捷聯姿態計算中方向余弦與四元數法分析比較

尹劍，陳紅，楊萌，黃皓

(西安應用光學研究所，西安710065)

摘要：捷聯慣性姿態計算中，傳統觀點認為，方向余弦法由于需要維護9個參數，而四元數法只需要4個參數，從而四元數法得到了廣泛應有，成為經典算法。但是從捷聯慣性導航與制導的整個過程看，方向余弦矩陣是必不可少的，而四元數卻是中間變量。通過對方向余弦與四元數姿態算法進行深入分析比較，并對2種算法進行仿真表明：這2種算法精度相當，且方向余弦法具有容易使用、計算量小、編程方便等優點，因此完全可以替代四元數法。

關鍵詞：捷聯慣性導航；姿態計算；方向余弦矩陣；四元數

收稿日期：2015-05-29

作者簡介：尹劍(1972—),男,博士研究生,高級工程師,主要從事光電定位定向技術研究。

doi:10.11809/scbgxb2015.09.027

中圖分類號：TP15

文章編號：1006-0707(2015)09-0106-05

收稿日期：2015-05-12

基金項目：陜西省行政學院科研立項(YKT010)

本文引用格式：尹劍，陳紅，楊萌，等.捷聯姿態計算中方向余弦與四元數法分析比較[J].四川兵工學報，2015(9):106-110.

Citationformat:YINJian,CHENHong,YANGMeng,etal.AnalysisandComparisonofDirectionCosineMatrixandQuaternionMethodsforStrapdownInertialNavigationAttitudeAlgorithm[J].JournalofSichuanOrdnance,2015(9):106-110.

AnalysisandComparisonofDirectionCosineMatrixand

QuaternionMethodsforStrapdownInertial

NavigationAttitudeAlgorithm

YINJian,CHENHong,YANGMeng,HUANGHao

(Xi’anInstituteofAppliedOptics,Xi’an710065,China)

Abstract:Traditional opinion believed, the direction cosine matrix method for strapdown inertial navigation attitude calculation uses nine parameters and the quaternion method only uses four parameters, so the quaternion method has been successfully applied to the strapdown inertial navigation systems for tens year, and has become classical method. However, the direction cosine matrix is essential in the subsequent strapdown inertial navigation calculation and the quaternion, which is only impermanent variable. Analysis, comparison and simulation of the two methods show that the attitude algorithm based on the direction cosine matrix is comprehensive, simple, and calculating time is shorter, which can replace the quaternion method.

Keywords:strapdowninertialnavigation;attitudealgorithm;directioncosinematrix；quaternion

慣導系統基本工作原理是以牛頓力學定律為基礎，通過測量載體在慣性參考系的加速度，將它對時間進行積分，之后將其變換到導航坐標系，得到在導航坐標系中的速度、偏航角和位置信息等[1-4]。對捷聯慣導系統而言，首先要計算出載體坐標系至導航計算的地理坐標系坐標系之問的坐標轉換矩陣，即姿態矩陣或方向余弦陣。直接安裝在載體上的慣性元件測得加速度和角加速度是沿載體軸的分量，將這些分量經過方向余弦陣，可以轉換到要求的計算機坐標系內的分量。這個矩陣可以描述載體和地理坐標系之間的關系，有了這個轉換矩陣，導航計算機便可根據相應的力學方程解出要求的導航和姿態參數來。方向余弦矩陣的原理如圖1所示。

圖1　地理坐標系與載體坐標系

載體的姿態角實際上就是載體坐標系OXbYbZb和地理坐標系OXtYtZt之間的方位關系，如圖1所示。b系相對于t系的角位置可以用一組歐拉角航向角H、俯仰角φ、橫滾角θ、來表示2個坐標系之間的變換關系

即

方向余弦法是用矢量的方向余弦來表示姿態矩陣的方法，也就是繞定點轉動的2個坐標系之間的關系可以用方向余弦矩陣來表示。方向余弦矩陣是隨時間變化的，其變換規律用的數學微分方程來描述，即時值就可以通過求解該微分方程得到。

四元數理論是數學中的古老分支[5]，是由哈密而頓于1943年首先提出的，四元數具有很強的物理意義，它可以表征剛體姿態變換時順時歐拉軸和所需的轉角。但是該理論建立之后長期沒有得到實際應用，70年代以來，隨著數字計算機和計算技術、空間技術、特別是捷聯式慣性導航技術的發展，四元數算法在捷聯式慣性系統、剛體姿態控制、定位和穩定控制中得到了廣泛應用[6,7]。尤其是在捷聯慣性系統姿態計算領域，已經成為經典的算法。

對于這2種算法，通常觀點認為，方向余弦法姿態矩陣具有9個元素，所以解算矩陣微分方程時，實際是解算9個聯合微分方程，一般來說計算工作量比較大，實時計算困難，工程上不太實用[8-10]。而四元數法與方向余弦法比較，計算量小，存儲容量少，僅需要進行簡單的四元數規范化處理便可以保證姿態矩陣的正交性，因而成為一種普遍采用的方法。

然而，通常所統計的計算量，僅僅是根據姿態更新算法本身得到的，沒有綜合考慮完整的姿態計算過程以及整個捷聯計算任務(包括測量的比力矢量求解)；而且姿態算法的最終選擇不能夠僅僅依據參數的多少來進行確定。本文將對這2種姿態計算方法進行分析和比較。

1方向余弦姿態算法

方向余弦矩陣滿足如下的微分方程

e[Δ θ×]= Icosθ0+sinθ0[Δθ0×]+ (1-cosθ0)Δθ0Δθ0T

2四元數姿態算法

四元數是一個帶實系數的一階四元數組，是一個四維空間中的矢量，四元數Q可以寫成如下的形式：Q=q0+q1i+q2j+q3k，式中q0、q1、q2、q3、為4個實數，i,j,k為四元數的3個虛數單位的基，作為一個正交的基。

其中：

為了最終求解載體姿態，需要將更新后的四元數表示為方向余弦姿態矩陣然后求出載體姿態歐拉角，算法如下

3算法仿真及分析比較

3.1算法精度比較

為了對2種算法的精度進行比較，采用了對捷聯慣性姿態運算來說最惡劣的工作環境條件：圓錐運動進行仿真。仿真圓錐運動的半錐角為1°，圓錐運動頻率為2Hz，計算周期數為600點，計算頻率均為100Hz。

仿真比較一：采用方向余弦角增量采樣算法和四元數角增量采樣畢卡逼近算法進行比較。方向余弦算法的航向角H、俯仰角φ、橫滾角θ誤差如圖2所示，圖2中橫坐標為計算周期數，縱坐標為誤差值，誤差值單位為度。

四元數畢卡逼近算法的航向角H、俯仰角φ、橫滾角θ誤差如圖3所示。

圖2　方向余弦角增量法誤差/(°)

仿真比較二：采用方向余弦角速度采用龍格庫塔算法和四元數角速度采樣的四階龍格庫塔算法進行比較。

方向余弦四階龍格庫塔算法的航向角H、俯仰角φ、橫滾角θ誤差如圖4所示。

四元數四階龍格庫塔算法的航向角H、俯仰角φ、橫滾角θ誤差如圖5所示。

由上述仿真結果可見，方向余弦算法和四元數算法無論使用角增量采樣算法和使用角速度采樣的龍格庫塔算法，在計算精度方面差別非常小，具體統計結果如表1所示。

圖4方向余弦四階龍格庫塔法誤差/(°)

圖5　四元數四階龍格庫塔法誤差/(°)

可以看出，無論是方向余弦方法還是四元數方法，在采用角增量采樣時，采用龍格庫塔法精度明顯高于角增量法，分析龍格庫塔算法可知，由于龍格庫塔法在進行計算時在一個計算周期內利用了2次采樣數據，因此龍格庫塔算法可以被看作是一種雙子樣算法。其計算精度也與雙子樣算法類似。

各種情況下方向余弦算法與四元數算法精度相當，誤差趨勢完全相同，說明方向余弦的方法與四元數的方法在表示剛體空間轉動方面是等價的。精度上的細微差別主要是由于三角函數運算時的取舍誤差造成的。

3.2運算量比較

對于運算量比較，采用運算統計的方法，分別對采用角增量和角速率的姿態運算周期內的姿態更新、四元數轉換姿態陣等過程進行比較,比較結果如表2、表3、表4所示。

表2　角增量姿態更新算法運算量比較

表3　角速率姿態更新算法運算量比較

表4　四元數轉換成方向余弦姿態矩陣

通過上述運算量統計比較可以看到，在使用角增量采樣的情況下，方向余弦法以及改進方向余弦法減少了耗時比較大的乘除法運算，從而使得計算機的運算量得以減少，因此可以提高系統運算速度。在使用角速率采樣時，由于方向余弦法采用的是三維矩陣，而四元數法要使用四維矩陣進行運算，從而使得四元數算法的運算效率也低于方向余弦法。且四元數法在需要輸出姿態值及后續運算需要方向余弦矩陣時，還需要進行從四元數到方向余弦矩陣的轉換，這又增加了不少運算量。因此，總體上看方向余弦法的運算效率要高于四元數法。

4結論

通過以上仿真及分析比較可以看出，在導航、制導等慣性運算中方向余弦矩陣是必需的，對于方向余弦矩陣的運算，可用采用直接運算的方向余弦法，也可以采用廣泛使用的四元數算法， 四元數法首先需要對四元數進行初始化，定義從導航坐標系到飛行器體坐標系的四元數，然后根據采樣的值，對四元數方程進行更新，還需要根據實時確定的四元數求出四元數相應的方向余弦矩陣，進而求解出載體姿態。從上述四元數運算過程來看，四元數實質上是姿態運算中的一個中介運算載體，是一個中間變量。 而方向余弦算法則直接以方向余弦矩陣作為核心變量，根據采樣值直接對該矩陣進行更新運行。因此，方向余弦算法在慣性運算表達上更直接，也更容易理解。

仿真分析表明，方向余弦算法的運算精度與四元數法相當，且誤差趨勢一致，表明兩者對剛體旋轉的表示是一致的；在實際應用方面，方向余弦算法具有使用方便、計算量小，且編程容易的特點。雖然傳統觀點認為，方向余弦算法需要維護9個參數，而四元數只有4個參數，但是考慮到后續運算對方向余弦矩陣的需求，四元數還是需要轉換為姿態矩陣的。隨著計算機技術的發展，兩者的區別就變得越來越模糊了。

參考文獻：

[1]秦永元.慣性導航[M].西安：西北工業大學出版社，2006.

[2]胡小平.自主導航理論與應用[M].長沙:國防科技大學出版社,2002.

[3]鄧正隆.慣性技術[M].哈爾濱:哈爾濱工業大學出版社,2006:140-180.

[4]楊培根，龔智炳.光電慣性技術[M].北京：兵器工業出版社,1999:195-199.

[5]張榮輝,賈宏光,陳濤,等.基于四元數法的捷聯式慣性導航系統的姿態解算[J].光學精密工程,2008,16(10):1964-1970.

[6]金毅民,陶忠,孫治家.軸系運動的數學仿真與軸系誤差[J].應用光學,2003,24(B08):37-49.

[7]劉俊峰.三維轉動的四元數表述[J].大學物理,2004,23(4):39-43.

[8]張琬琳，尹劍，郭栓運.Q-MEMS陀螺零偏補償技術研究[J]．應用光學，2010，7(4)：549-552．

[9]楊艷娟.捷聯慣性導航系統關鍵技術研究[D].哈爾濱:哈爾濱工程大學,2001.

[10]彭富倫，王靜，吳頤雷，等.車載光電偵察系統目標定位及誤差分析[J]．應用光學，2014，35(4)：557-562．

(責任編輯楊繼森)