圓錐運動下幾種姿態(tài)算法的比較與誤差建模*

熊麗娟，朱洪濤，王志勇，曹娟華，周 波

(1 南昌大學(xué)機電工程學(xué)院， 南昌 330031； 2 南昌航空大學(xué)航空制造工程學(xué)院， 南昌 330063；3 南昌航空大學(xué)測試與光電工程學(xué)院， 南昌 330063)

0 引言

實時姿態(tài)解算是導(dǎo)航、機器人、遠程操作和虛擬現(xiàn)實等工程領(lǐng)域的重要技術(shù)之一[1-4]。計算機和傳感器的飛速發(fā)展對姿態(tài)精度和計算速度提出了更高的要求。此兩者不僅和傳感器性能有關(guān)，還和處理器所采用的姿態(tài)算法緊密相關(guān)。評判選擇姿態(tài)算法時，常須考慮其在最不利條件下的性能。圓錐運動因可能引起最大不可交換誤差[5-6]，被眾多文獻認定為最不利條件。但常用姿態(tài)算法在不同圓錐運動條件下的性能表現(xiàn)尚未得到系統(tǒng)研究。

姿態(tài)解算方法中，四元數(shù)法因其非奇異性、簡單性和計算時間短等優(yōu)點而成為目前最流行的方法[7-9]。 文中針對4種基于四元數(shù)的常用高精度姿態(tài)算法，研究它們在不同圓錐運動條件下的姿態(tài)解算性能。這4種算法分別為：四子樣旋轉(zhuǎn)矢量算法(RV4)，四階龍格庫塔算法(RK4)，四階泰勒算法(T4)和五階泰勒算法(T5)。為進行較全面的分析，文中通過變化錐進步長和錐半角進行了一系列圓錐運動仿真實驗。實驗以姿態(tài)角誤差，即偏航、俯仰和橫滾角3者的誤差作為結(jié)果輸出，籍此研究4種算法的姿態(tài)解算精度，并歸納建立4者的姿態(tài)角誤差模型。相較而言，前人文獻更傾向于依據(jù)誤差漂移參數(shù)[1]、旋轉(zhuǎn)矢量幅值誤差或姿態(tài)四元數(shù)誤差[5, 7, 10]來評價姿態(tài)算法的精度。這類誤差模型因推算過程有假設(shè)與簡化存在，并不準確，亦不利于理解和用作誤差補償。

所選4種算法雖非當前精度最高的姿態(tài)算法[11-13]，但因文中意在展示一種新的姿態(tài)算法研究方式，故并不需要遍歷各種高級算法。另，因篇幅有限，文中僅展示了實驗結(jié)果中有代表性的一部分。

1 姿態(tài)解算數(shù)學(xué)模型

姿態(tài)解算需先更新姿態(tài)四元數(shù)，再將其換算為姿態(tài)角，即橫滾角、俯仰角和偏航角。除旋轉(zhuǎn)矢量法外，姿態(tài)四元數(shù)的更新一般以式(1)為基礎(chǔ)：

(1)

(2)

(3)

θ=-arcsinc31

(4)

式(4)中atan2是C++中的函數(shù)，返回-π ～ π范圍的弧角。當θ= ±π/2時，由于橫滾軸和偏航軸已成平行軸[1]，故無法獲取φ和ψ的唯一解。因此，當θ接近±π/2時，可采取交替更新φ和ψ的方法[1]。

2 圓錐運動下的四元數(shù)更新算法

圖1 典型圓錐運動示意圖

典型圓錐運動是因系統(tǒng)兩個正交軸上同時被施以相位差90°的角振動而產(chǎn)生[1]，具體如圖1所示：O-XYZ為導(dǎo)航坐標系，O-xbybzb為載體坐標系；OL是從導(dǎo)航到載體坐標系的旋轉(zhuǎn)矢量，正在YOZ平面內(nèi)以恒角速度ω0旋轉(zhuǎn)，載體坐標系此時即作典型圓錐運動，其xb軸可看作在導(dǎo)航坐標系空間中一錐半角為α的圓錐面上滾動，OL幅值亦為α。將OL記為Φ：

(5)

式中：t為載體圓錐運動時間；文中仿真的載體零點狀態(tài)如圖1所示。Φ的等效姿態(tài)四元數(shù)為：

(6)

載體相對導(dǎo)航坐標系的角速度為：

(7)

根據(jù)式(6)，不同時刻的精確姿態(tài)角可以算得；而式(7)可作為角速度傳感器測得的轉(zhuǎn)速，實現(xiàn)圓錐運動仿真。

2.1 RV4算法

在姿態(tài)更新區(qū)間Δt內(nèi)，載體從(k-1)Δt到kΔt時刻的四子樣旋轉(zhuǎn)矢量μk計算如式(8)[7]。

(8)

式中：k=1,2,3,…；μk1、μk2、μk3、μk4分別對應(yīng)四分之一更新區(qū)間內(nèi)的角積分。對典型圓錐運動而言，μki(i=1，2，3，4)可通過對式(7)積分得到。

求出對應(yīng)旋轉(zhuǎn)矢量μk的四元數(shù)為：

(9)

式中：μk為μk的幅值。

(10)

2.2 RK4算法

典型圓錐運動下，姿態(tài)四元數(shù)傳播方程具體化為：

(11)

qk=qk-1+(K1+2K2+2K3+K4)Δt/6

(12)

式中：K1、K2、K3、K4按式(13)順序求解：

(13)

2.3 泰勒級數(shù)算法

使用泰勒級數(shù)法[14]求解方程(11)得：

(14)

式中：上標(j)代表求導(dǎo)階數(shù)，l為所使用泰勒級數(shù)法的階數(shù)。

3 實驗結(jié)果

3.1 基礎(chǔ)分析

文中變化錐進步長(即ω0Δt，簡記為β)和錐半角α進行了一系列圓錐運動仿真實驗。為方便起見，文中所有仿真中ω0均保持在30°/s，而Δt在0.1 ～ 4 s間變化，這樣錐進步長β在3°～ 120°間變化；錐半角α在5°～ 85°間變化。

當α=5°、β=3°時，4種算法的姿態(tài)角誤差如圖2所示。此時4者精度由高到低排序為：RV4、T5、RK4、T4。圖2還顯示，由于圓錐運動中姿態(tài)角的周期變化，所有姿態(tài)角誤差都以類諧波的形式隨時間波動。

圖2中120 s的仿真誤差發(fā)展趨勢不夠明朗，因此仿真時長被延至20 000 s，其實驗結(jié)果(未圖示)清楚表明各算法的姿態(tài)角誤差絕對峰值幾乎均隨時間線性增加，且橫滾角誤差發(fā)散速度基本都比偏航和俯仰角誤差發(fā)散速度快。這與文獻中所述“圓錐誤差(漂

圖2 120 s仿真中的姿態(tài)角誤差(α=5°、ω0=30°/s、Δt=0.1 s)

移)主要出現(xiàn)在錐進軸上”[7, 15-16]相吻合。雖然多數(shù)文獻根據(jù)其簡化誤差模型認為偏航和俯仰角誤差基本無漂移[7, 15-16]，但仿真實驗結(jié)果證明，這兩者的絕對峰值其實也在累加(漂移)，只是速度通常沒有橫滾角誤差發(fā)散得快。

3.2 誤差建模

為研究姿態(tài)誤差會如何隨步長或其它因素的變化而變化問題，文中將4種算法在120 s仿真時間內(nèi)的最大姿態(tài)誤差，即最大姿態(tài)角絕對誤差E繪于圖3、圖4之中。圖3描繪了E在不同錐進步長下如何隨錐半角α的變化而變化，圖4則描繪E在不同錐半角下如何隨錐進步長β的變化而變化。

圖3 120 s仿真中最大姿態(tài)誤差與錐半角的關(guān)系

圖3、圖4均反映出RV4、T5、RK4和T4這4者的精度排序并不總是不變的,比如，T4算法雖不如RV4和RK4精確，但并非總是不如T5；T5算法在錐進步長較小時精度較高，但隨著錐進步長增大，其精度相比其余3者急速下降——也就是說，T5的精度易受步長影響；而最易受錐半角變化影響的是RV4算法，某些時候它的精度還不如RK4。

圖4 120 s仿真中最大姿態(tài)誤差與錐進步長的關(guān)系

由圖3可知，當α不超過30°時，lgE與lgα間基本呈線性關(guān)系。由圖4可知，當β不超過30°時，lgE與lgβ間亦基本呈線性關(guān)系。為此，可用式(15)表達4種算法的E與α、β之間的關(guān)系。

E=Aαmβn

(15)

式中：上標m即圖3數(shù)據(jù)擬合線斜率，上標n即圖4數(shù)據(jù)擬合線斜率，系數(shù)A取決于運動時長t和錐進速度ω0。對不同的算法，這3個參數(shù)自然不同。

根據(jù)上一小節(jié)中的分析，A值基本與時間t成正比，也與ω0成正比，故將式(15)中的A代以Cω0t，得：

E=Cω0tαmβn

(16)

式中:C是取決于算法的常數(shù)。

為確定4種算法的m和n值，將圖3、圖4中橫坐標不超過30°的數(shù)據(jù)擬合直線，斜率m、n分別列于表1、表2中。

表1 不同錐進步長β下的m值

*使用部分數(shù)據(jù)擬合。

表2 不同錐半角α下的n值

表1顯示：當β≤ 60°，RV4算法的m總在3左右；當β≥ 90°時，其m在4左右。深入剖析發(fā)現(xiàn)，β≤ 60°時，120 s仿真時間內(nèi)RV4算法的最大姿態(tài)誤差E為俯仰角誤差；而β≥ 90°時，其E為橫滾角誤差。所以，m值從3跳變到4是因為E從俯仰角誤差切換到了橫滾角誤差，或者說3是RV4俯仰角誤差的m值，而4是RV4橫滾角誤差的m值。

須申明的是，當運動時間足夠長，橫滾角誤差通常將成為3個姿態(tài)角誤差中最大的，此時E即橫滾角絕對誤差極值。

表1還顯示：①無論β如何變化，RK4和T4算法的m值均在2左右。因為在這兩種算法的120 s仿真實驗中，大多數(shù)情況E都是橫滾角誤差，所以其m值未出現(xiàn)跳變。②當β= 3°，T5算法的m在1左右；當β≥ 12°，其m在2左右。經(jīng)剖析發(fā)現(xiàn)，1實為T5偏航角誤差的m值，而2為其橫滾角誤差的m值。

同理由表2推知：①當α不超過75°，RV4橫滾角誤差的n總在4左右；②無論α如何變化，RK4橫滾角誤差的n總在4左右，T4橫滾角誤差的n亦在4左右，而T5橫滾角誤差的n總在6左右。

值得一提的是，表2中RK4算法的n值在α= 55°時突然降至3.75。這種突變在圖3中也有出現(xiàn)——RK4的E值在α= 55°附近突然下降。圖4則反映，當α從5°升至55°，RK4的精度從遠不如RV4漸升至與其相當。經(jīng)深入分析，這一現(xiàn)象源自RK4橫滾角誤差發(fā)散方向會隨α增加而變化——當α升至一定值，其誤差發(fā)散由朝正方向發(fā)展變?yōu)槌摲较虬l(fā)展。該轉(zhuǎn)變點α值會隨β值變化，β增大，轉(zhuǎn)變點α值也漸漸增大；當β增至120°時，轉(zhuǎn)變點消失(因為α不會超過90°)。橫滾角誤差發(fā)散方向的改變令RK4橫滾角誤差在轉(zhuǎn)變點處大大減小并散失其誤差支配地位。上述現(xiàn)象在4種算法中為RK4算法所獨有，一定程度上提高了RK4算法對高機動運動環(huán)境的適應(yīng)能力。這也是為什么一些學(xué)者會發(fā)現(xiàn)RV4的抗干擾能力不如RK4[17-18]。

綜上，當α≤ 30° 且β≤ 30°時，圓錐運動t時長內(nèi)以上4種算法的最大橫滾角誤差，記為|δφ|max，可建模如下：

(17)

式中：|δφ|max單位(°)，錐進速度ω0單位(°)/s，圓錐運動時長t單位s，錐半角α和錐進步長β單位(°)；利用實驗結(jié)果算得系數(shù)C1、C2、C3、C4依次為：9.11e-19，2.63e-14，1.20e-13，5.31e-18。該式給出的是錐進軸姿態(tài)角誤差模型，而非以往文獻常給出的“圓錐誤差模型[7]”(即文中旋轉(zhuǎn)矢量μk的誤差幅值模型)。

在同樣使用環(huán)境下用4種算法分別進行1 200次姿態(tài)更新發(fā)現(xiàn)：RV4算法用時0.168 8 s， RK4算法用時0.270 7 s，T4算法用時448.049 6 s，T5算法用時1 365.3 s。顯然，RV4是4者中計算速度最快的算法，而T5是四者中速度最慢的算法。

4 結(jié)論

文中研究了4種高精度姿態(tài)算法，通過分析其在圓錐運動下的姿態(tài)角計算誤差，發(fā)現(xiàn)其精度排序?qū)㈦S條件的改變而變化。這和以往一些文獻給出的精度優(yōu)劣論斷不一樣[19-20]。

就計算速度而言，RV4和RK4算法比T4和T5算法快得多。綜合考慮計算精度和速度，多數(shù)情況下RV4算法會是4者中姿態(tài)解算性能最佳者。

文中還建立了4種算法的錐進軸姿態(tài)角誤差模型，亦即文中實驗條件下的橫滾角誤差模型。在工程應(yīng)用中，若能即時對待測物的角速度進行頻域分析，可針對其包含的圓錐運動分量使用此誤差模型進行誤差預(yù)測乃至補償[21-22]。