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

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

0 引言

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

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

所選4種算法雖非當(dāng)前精度最高的姿態(tài)算法[11-13]，但因文中意在展示一種新的姿態(tài)算法研究方式，故并不需要遍歷各種高級(jí)算法。另，因篇幅有限，文中僅展示了實(shí)驗(yàn)結(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ù)，返回-π ～ π范圍的弧角。當(dāng)θ= ±π/2時(shí)，由于橫滾軸和偏航軸已成平行軸[1]，故無(wú)法獲取φ和ψ的唯一解。因此，當(dāng)θ接近±π/2時(shí)，可采取交替更新φ和ψ的方法[1]。

2 圓錐運(yùn)動(dòng)下的四元數(shù)更新算法

圖1 典型圓錐運(yùn)動(dòng)示意圖

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

(5)

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

(6)

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

(7)

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

2.1 RV4算法

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

(8)

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

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

(9)

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

(10)

2.2 RK4算法

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

(11)

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

(12)

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

(13)

2.3 泰勒級(jí)數(shù)算法

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

(14)

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

3 實(shí)驗(yàn)結(jié)果

3.1 基礎(chǔ)分析

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

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

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

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

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

3.2 誤差建模

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

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

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

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

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

E=Aαmβn

(15)

式中：上標(biāo)m即圖3數(shù)據(jù)擬合線斜率，上標(biāo)n即圖4數(shù)據(jù)擬合線斜率，系數(shù)A取決于運(yùn)動(dòng)時(shí)長(zhǎng)t和錐進(jìn)速度ω0。對(duì)不同的算法，這3個(gè)參數(shù)自然不同。

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

E=Cω0tαmβn

(16)

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

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

表1 不同錐進(jìn)步長(zhǎng)β下的m值

*使用部分?jǐn)?shù)據(jù)擬合。

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

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

須申明的是，當(dāng)運(yùn)動(dòng)時(shí)間足夠長(zhǎng)，橫滾角誤差通常將成為3個(gè)姿態(tài)角誤差中最大的，此時(shí)E即橫滾角絕對(duì)誤差極值。

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

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

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

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

(17)

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

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

4 結(jié)論

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

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

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