捷聯(lián)慣導(dǎo)系統(tǒng)兩種速度更新算法的比較研究

林玉榮，陳亮，付振憲

(哈爾濱工業(yè)大學(xué) 航天學(xué)院，黑龍江 哈爾濱150001)

0 引言

傳統(tǒng)捷聯(lián)慣性導(dǎo)航算法［1-2］，通過將變換到導(dǎo)航系下的比力進(jìn)行積分獲得速度增量，積分過程中進(jìn)行了截?cái)嗯c簡(jiǎn)化，導(dǎo)致算法精度降低。

對(duì)偶四元數(shù)能夠?qū)⑥D(zhuǎn)動(dòng)和平移統(tǒng)一表征［3-7］，文獻(xiàn)［8］利用其設(shè)計(jì)了對(duì)偶四元數(shù)捷聯(lián)慣性導(dǎo)航算法，準(zhǔn)確求解出了速度增量，并指出“對(duì)高動(dòng)態(tài)、大機(jī)動(dòng)狀態(tài)下的高精度導(dǎo)航來說，對(duì)偶四元數(shù)算法是更好的選擇”。文獻(xiàn)［9 -11］通過仿真測(cè)試進(jìn)一步驗(yàn)證了對(duì)偶四元數(shù)在精度上的優(yōu)勢(shì)，但并未分析造成兩種算法精度差別的原因。

針對(duì)傳統(tǒng)速度更新算法與對(duì)偶四元數(shù)速度更新算法，本文從理論上研究了二者的不同之處，并通過定義兩類誤差，對(duì)二者之間的精度差別作出了定性的表征，同時(shí)也給出了定量的分析，結(jié)論的正確性通過高動(dòng)態(tài)環(huán)境下的仿真測(cè)試得到了驗(yàn)證。

1 兩種速度更新算法

設(shè)速度更新周期ΔT =tm-tm-1，為了便于理論分析，設(shè)導(dǎo)航周期等于速度更新周期，并選慣性坐標(biāo)系I 為導(dǎo)航坐標(biāo)系?？紤]到引力加速度和哥式加速度大小和方向在速度更新周期內(nèi)變化緩慢，由此引起的不可交換性誤差很小，因此下面對(duì)兩種速度更新算法進(jìn)行理論分析時(shí)不考慮引力加速度和哥式加速度，只討論載體本身運(yùn)動(dòng)對(duì)速度計(jì)算的影響。

1.1 傳統(tǒng)速度更新算法

在忽略引力加速度和哥式加速度的情況下，載體相對(duì)地球的運(yùn)動(dòng)速度在慣性導(dǎo)航坐標(biāo)系上的投影為

(1)式積分項(xiàng)為速度增量，記為

根據(jù)旋轉(zhuǎn)矢量與姿態(tài)矩陣的關(guān)系，速度增量的準(zhǔn)確表達(dá)式可寫為

式中:Φ(t)為B(m -1)坐標(biāo)系至B(t)坐標(biāo)系的等效旋轉(zhuǎn)矢量;φ(t)= |Φ(t)|.(2)式準(zhǔn)確求解非常困難，考慮到ΔT 較小，有如下近似:

將(3)式與(4)式代入(2)式，并忽略二階小量［Φ(t)×］2，得到傳統(tǒng)導(dǎo)航算法的速度增量表達(dá)式

實(shí)際導(dǎo)航解算中，導(dǎo)航計(jì)算機(jī)利用陀螺的角增量輸出與加速度計(jì)的速度增量輸出求解(5)式，計(jì)算公式如下:

1.2 對(duì)偶四元數(shù)速度更新算法

剛體在空間中從I 到B 的運(yùn)動(dòng)，如圖1所示，可分解為:先繞過點(diǎn)c 的單位向量l 旋轉(zhuǎn)θ 角，后沿l的方向平移距離d(或者先沿l 的方向平移距離d，后旋轉(zhuǎn)θ 角).上述的旋轉(zhuǎn)和平移運(yùn)動(dòng)可以用對(duì)偶四元數(shù)統(tǒng)一描述為

圖1 旋轉(zhuǎn)和平移的幾何表示Fig.1 Geometric representationof rotation and translation

與傳統(tǒng)的速度更新算法不同，在對(duì)偶四元數(shù)導(dǎo)航算法中可直接獲得定義在慣性導(dǎo)航系中的地速(載體相對(duì)地球的速度)增量的精確解［2］。若將一個(gè)速度更新周期內(nèi)載體系(B 系)相對(duì)于慣性系(I系)運(yùn)動(dòng)的對(duì)偶四元數(shù)記為

則地速增量的精確解為

式中q*IB(ΔT)為qIB(ΔT)的共軛。

根據(jù)(7)式和(8)式有

且

應(yīng)用二重向量差乘變換公式

(9)式可化為

對(duì)(12)式中的三角函數(shù)取4 階近似，忽略高階小量，可得到對(duì)偶四元數(shù)速度更新算法中的速度增量計(jì)算公式如下:

考慮到慣性器件的輸出是增量形式，(10)式計(jì)算如下:

2 兩種速度更新算法的精度比較

從第1 節(jié)的分析可以看出:傳統(tǒng)速度更新算法計(jì)算速度增量時(shí)，在積分運(yùn)算前忽略了二階小量［Φ(t)×］2并對(duì)三角函數(shù)作了一階近似;對(duì)偶四元數(shù)速度更新算法計(jì)算速度增量時(shí)，利用基于慣性器件輸出定義的螺旋矢量給出了速度增量準(zhǔn)確解，在導(dǎo)航解算中對(duì)速度增量準(zhǔn)確解中的三角函數(shù)作了四階近似。因此，兩種算法得到的速度增量精度是不同的。

將(14)式代入(13)式，得到

(15)式即為在對(duì)偶四元數(shù)導(dǎo)航算法中，利用慣性器件輸出計(jì)算速度增量的結(jié)果。比較(15)式和(6)式，可看出，(15)式右端比(6)式多了4 項(xiàng)叉乘項(xiàng)，下面分析這4 項(xiàng)的影響。叉乘項(xiàng)中出現(xiàn)的Δvsm、Φc分別為劃船誤差、圓錐誤差，當(dāng)載體的角運(yùn)動(dòng)方向與線運(yùn)動(dòng)方向在空間不斷變化時(shí)，即載體處于比較高動(dòng)態(tài)環(huán)境下，Δ vsm、Φc是不為0 的整流誤差項(xiàng)，這些整流誤差與慣性器件輸出的增量是不共線的，因此會(huì)造成(15)式右端的4 項(xiàng)叉乘項(xiàng)不為0，由此對(duì)導(dǎo)航精度產(chǎn)生影響，影響程度取決于載體所處動(dòng)態(tài)環(huán)境的惡劣程度。

由上述分析可看出，對(duì)偶四元數(shù)速度更新算法的精度明顯高于傳統(tǒng)速度更新算法，精度優(yōu)勢(shì)體現(xiàn)在(15)式中右端后4 項(xiàng)。下面將定義兩種誤差，通過這兩種誤差對(duì)兩種速度更新算法在精度上的不同作定量的分析。

2.1 兩種誤差的定義

傳統(tǒng)速度更新算法中的速度增量表達(dá)式(5)式，是在忽略(2)式中二階小量［Φ(t)×］2的前提下得到的，將由此引起的誤差定義為截?cái)嗾`差

按照傳統(tǒng)速度更新算法的實(shí)現(xiàn)思想，即使保留(2)式中的二階小量［Φ(t)×］2，所得到的速度增量與對(duì)偶四元數(shù)算法中的速度增量相比也還是存在誤差。前者計(jì)算速度增量時(shí)在積分運(yùn)算之前對(duì)三角函數(shù)作了一階近似，而后者是在求得速度增量準(zhǔn)確解后對(duì)三角函數(shù)作了四階近似。對(duì)于這種在積分運(yùn)算前對(duì)表達(dá)式作近似和近似階次低引起的誤差，在此將其定義為近似積分誤差

截?cái)嗾`差和近似積分誤差的存在，導(dǎo)致傳統(tǒng)速度更新算法比對(duì)偶四元數(shù)速度更新算法精度低。

2.2 兩種誤差的計(jì)算

以二子樣速度更新算法為例，推導(dǎo)上述兩種誤差的定量計(jì)算公式，分析誤差大小與載體運(yùn)動(dòng)的關(guān)系，并比較兩種速度更新算法在精度量級(jí)上的差別。

在速度更新周期內(nèi)，對(duì)載體的角速度和比力采用線性函數(shù)擬合

對(duì)于tm-1≤t≤tm，有

在［tm-1，tm］內(nèi)等間隔采樣兩次，利用陀螺輸出的角增量Δθm(i)，i =1，2，與加速度計(jì)輸出的速度增量Δ vm(i)，i =1，2，可求得(18)式中的各項(xiàng)系數(shù):

顯然在ΔT 固定的情況下，系數(shù)a 和b、A 和B分別取決于陀螺、加速度計(jì)的輸出增量值。下面就利用這些系數(shù)定量地分析傳統(tǒng)更新算法中存在的兩類誤差。

將(18)式代入(6)式，得到二子樣傳統(tǒng)導(dǎo)航算法中速度增量的計(jì)算值

將(18)式代入(16)式，得到(20)式存在的截?cái)嗾`差為

將(18)式代入(17)式，得到(20)式存在的近似積分誤差為

若記

(22)式中的各項(xiàng)系數(shù)為:

比較(20)式與(21)式、(22)式的結(jié)果可以看出:傳統(tǒng)速度更新算法求得的速度增量ΔvBT近似到ΔT4，存在著比較明顯的截?cái)嗾`差與近似積分誤差，若載體處在高動(dòng)態(tài)或大機(jī)動(dòng)運(yùn)動(dòng)狀態(tài)下，這兩類誤差會(huì)越發(fā)明顯。

由(17)式可知

即(21)式與(22)式的和即是(15)式中右端后4項(xiàng)。可見，對(duì)偶四元數(shù)速度更新算法的計(jì)算精度等價(jià)于傳統(tǒng)速度更新算法補(bǔ)償了截?cái)嗾`差和近似積分誤差后的計(jì)算精度，速度增量ΔvBDQ精確到ΔT9.在高動(dòng)態(tài)或大機(jī)動(dòng)運(yùn)動(dòng)環(huán)境下，對(duì)偶四元數(shù)算法在精度上的優(yōu)勢(shì)更顯著，而且這種精度優(yōu)勢(shì)還隨著算法子樣數(shù)的增加而提高，如表1所示。表1中對(duì)各子樣算法下，兩種速度更新算法中速度增量所精確到的ΔT階次進(jìn)行了比較。

3 仿真分析

3.1 仿真環(huán)境

選東北天地理坐標(biāo)系為導(dǎo)航坐標(biāo)系，載體初始位置:經(jīng)度110°，緯度40.5°，高度0 m.設(shè)置如下兩種同時(shí)包含圓錐運(yùn)動(dòng)與劃船運(yùn)動(dòng)的復(fù)雜動(dòng)態(tài)測(cè)試條件。

表1 兩類速度更新算法的精度量級(jí)比較Tab.1 Comparison of accuracy magnitude orders between two velocity update algorithms

條件1 載體相對(duì)地球作幅值較大的三軸線振動(dòng)與角振動(dòng)，投影在導(dǎo)航系中的三軸線位移為S =投影在導(dǎo)航系中的載體姿態(tài)角分別為俯仰角航向角滾轉(zhuǎn)角γ =

3.2 理想情況下的測(cè)試

不考慮慣性器件的測(cè)量誤差，考察傳統(tǒng)二子樣導(dǎo)航算法與對(duì)偶四元數(shù)二子樣導(dǎo)航算法的性能。

在條件1 下，取ΔT =0.5 s，仿真比較結(jié)果如圖2～圖7所示，圖中繪出的是真實(shí)值減去計(jì)算值的誤差曲線。圖2～圖4中能夠清楚地看出兩種算法導(dǎo)航參數(shù)誤差的變化規(guī)律;隨著導(dǎo)航時(shí)間的增長(zhǎng)，如圖5～圖7所示，傳統(tǒng)算法T 的導(dǎo)航誤差發(fā)散的快且幅值遠(yuǎn)大于對(duì)偶四元數(shù)算法(DQ)，后者的精度優(yōu)勢(shì)非常明顯。

在高動(dòng)態(tài)條件2 下，取ΔT =0.002 s，兩種二子樣導(dǎo)航算法的仿真比較結(jié)果如圖8～圖10 所示，在此高動(dòng)態(tài)環(huán)境下，沿Z 軸方向會(huì)有明顯的劃船整流誤差，對(duì)偶四元數(shù)算法對(duì)此誤差的抑制能力明顯優(yōu)于傳統(tǒng)算法。

圖2 條件1 理想情況下的姿態(tài)角誤差(時(shí)間200 s)Fig.2 Attitude angle errors under ideal condition 1 (200 s)

圖3 條件1 理想情況下的地速誤差(時(shí)間200 s)Fig.3 Ground velocity errors under ideal condition 1 (200 s)

圖4 條件1 理想情況下的位置誤差(時(shí)間200 s)Fig.4 Position errors under ideal condition 1 (200 s)

圖5 條件1 理想情況下的姿態(tài)角誤差(時(shí)間10 000 s)Fig.5 Attitude angle errors under ideal condition 1 (10 000 s)

圖6 條件1 理想情況下的地速誤差(時(shí)間10 000 s)Fig.6 Ground velocity errors under ideal condition 1 (10 000 s)

表2 理想情況下σvN 的理論值和實(shí)驗(yàn)值Tab.2 Both theoretical and experimental values of σvN in ideal case

圖7 條件1 理想情況下的位置誤差(時(shí)間10 000 s)Fig.7 Position errors under ideal condition 1 (10 000 s)

圖8 條件2 理想情況下的姿態(tài)角誤差Fig.8 Attitude angle errors under ideal condition 2

為了全面考察兩種導(dǎo)航算法的性能，應(yīng)對(duì)它們的運(yùn)算效率進(jìn)行考察。比較(6)式與(15)式可以看出，由于叉乘項(xiàng)的增多，導(dǎo)致對(duì)偶四元數(shù)導(dǎo)航算法的運(yùn)算負(fù)擔(dān)加大。以完成一個(gè)導(dǎo)航周期的運(yùn)行時(shí)間為比較標(biāo)準(zhǔn)，分別對(duì)兩種導(dǎo)航算法進(jìn)行多次測(cè)試，平均運(yùn)行時(shí)間的統(tǒng)計(jì)結(jié)果為δtDQ=3.88 ×10-4s(對(duì)偶四元數(shù)導(dǎo)航算法)，δtT=3.7 ×10-4s(傳統(tǒng)導(dǎo)航算法).可見，與傳統(tǒng)捷聯(lián)導(dǎo)航算法相比，對(duì)偶四元數(shù)導(dǎo)航算法在犧牲較少運(yùn)算效率的前提下，換來了精度上的顯著優(yōu)勢(shì)。

圖9 條件2 理想情況下的地速誤差Fig.9 Ground velocity errors under ideal condition 2

圖10 條件2 理想情況下的位置誤差Fig.10 Position errors under ideal condition 2

3.3 實(shí)際情況下的測(cè)試

考慮慣性器件的測(cè)量誤差，并設(shè)定:

陀螺隨機(jī)誤差標(biāo)準(zhǔn)差σg=10-3°/h;加速度計(jì)隨機(jī)誤差標(biāo)準(zhǔn)差σa=10-5g，g 為重力加速度。

在條件1 下，取ΔT =0.2 s，兩種導(dǎo)航算法的仿真比較結(jié)果如圖11～圖13 所示，與圖2～圖4中理想情況下的結(jié)果相比，可以看出:實(shí)際情況下的曲線出現(xiàn)了隨機(jī)漂移，這種漂移在Y 軸地速誤差與緯度誤差曲線上比較明顯。這是慣性器件測(cè)量誤差隨時(shí)間積累造成的影響，不過從總體看對(duì)偶四元數(shù)導(dǎo)航算法在精度上的優(yōu)勢(shì)仍然很明顯。

圖11 條件1 實(shí)際情況下的姿態(tài)角誤差(時(shí)間200 s)Fig.11 Attitude angle errors under real condition 1 (200 s)

圖12 條件1 實(shí)際情況下的地速誤差(時(shí)間200 s)Fig.12 Ground velocity errors under real condition 1 (200 s)

圖13 條件1 實(shí)際情況下的位置誤差(時(shí)間200 s)Fig.13 Position errors under real condition 1 (200 s)

對(duì)于高動(dòng)態(tài)條件2，取ΔT =0.01 s，兩種導(dǎo)航算法的仿真比較結(jié)果如圖14～圖15 所示?？梢钥闯?由于慣性器件測(cè)量誤差的積累影響，造成的導(dǎo)航參數(shù)隨機(jī)漂移現(xiàn)象比條件1 嚴(yán)重，兩種導(dǎo)航算法的性能從X 軸與Y 軸的導(dǎo)航參數(shù)結(jié)果難以區(qū)分;不過隨著導(dǎo)航時(shí)間的增長(zhǎng)，從Z 軸上抑制整流誤差的能力上可以看出對(duì)偶四元數(shù)的精度優(yōu)勢(shì)會(huì)逐漸顯著。逐級(jí)降低慣性器件測(cè)量精度，保證其他高動(dòng)態(tài)測(cè)試條件不變，當(dāng)測(cè)試時(shí)間t =400 s 時(shí)，兩種導(dǎo)航算法的Z 軸地速誤差比較結(jié)果如表3所示，可得出結(jié)論:在慣性器件的測(cè)量輸出能夠比較準(zhǔn)確地反映高動(dòng)態(tài)運(yùn)動(dòng)信息時(shí)，對(duì)偶四元數(shù)導(dǎo)航算法的性能始終優(yōu)于傳統(tǒng)導(dǎo)航算法;反之，當(dāng)慣性器件精度下降不能保證上述條件時(shí)，兩種導(dǎo)航算法精度降低到一致。

4 結(jié)論

針對(duì)捷聯(lián)慣導(dǎo)系統(tǒng)，對(duì)傳統(tǒng)的速度更新算法和對(duì)偶四元數(shù)速度更新算法進(jìn)行了對(duì)比與研究，通過定義兩類誤差(截?cái)嗾`差和近似積分誤差)，定性地分析了對(duì)偶四元數(shù)速度更新算法較傳統(tǒng)速度更新算法在精度上的優(yōu)勢(shì)，并對(duì)兩種算法的精度差別進(jìn)行了定量的計(jì)算。結(jié)論的正確性通過理想情況下與實(shí)際情況下的仿真測(cè)試得到了驗(yàn)證。

在捷聯(lián)慣導(dǎo)系統(tǒng)中，速度更新算法的精度高，必然獲得高精度的位置導(dǎo)航信息，進(jìn)而通過修正導(dǎo)航坐標(biāo)系提高載體姿態(tài)的確定精度。所以，對(duì)偶四元數(shù)速度更新算法在精度上的優(yōu)勢(shì)，提高了對(duì)偶四元數(shù)捷聯(lián)慣導(dǎo)算法的整體性能優(yōu)勢(shì)，這一點(diǎn)已通過兩種情況下的仿真測(cè)試得到了充分驗(yàn)證。

References)

［1］ Savage P G.Strapdown inertial navigation integration algorithm design part 2:velocity and position algorithms［J］.Journal of Guidance，Control and Dynamics，1998，21(2):208 -221.

［2］ Savage P G.Strapdown inertial navigation integration algorithm design part 1:attitude algorithms［J］.Journal of Guidance，Control and Dynamics，1998，21(1):19 -28.

［3］ Connolly T H，Pfeiffer F.Cooperating manipulator control using dual quaternion coordinates［C］∥Proceedings of the 33rd Conference on Decision and Control.Lake Buena Vista:IEEE，1994:2417 -2418.

［4］ Daniilidis K，Bayro-Corrochano E.The dual quaternion approach to hand-eye calibration［C］∥Proceedings of ICPR ’96.Vienna:IEEE Computer Society Press，1996:318 -322.

［5］ Dai J S.An historical review of the theoretical development of rigid body displacements from Rodrigues parameters to the finite twist［J］.Mechanism and Machine Theory，2006，41(1):41 -52.

［6］ Erhan Ata，Yusuf Yayli.Dual quaternions and dual projective spaces［J］.Chaos，Solitons ＆ Fractals，2009，40(3):1255 -1263.

［7］ Leonardo Traversoni.Interpolating movements using CAGD tools［C］∥Proceeding of CGIV 2007，Bangkok:IEEE Computer Society Press，2007:229 -238.

［8］ 武元新.對(duì)偶四元數(shù)導(dǎo)航算法與非線性高斯濾波［D］.長(zhǎng)沙:國(guó)防科學(xué)技術(shù)大學(xué)，2005.WU Yuan-xin.Research on dual-quaternion navigation algorithm and nonlinear gaussian filtering［D］.Changsha:National University of Defense Technology，2005.(in Chinese)

［9］ 孫楓，奔粵陽.捷聯(lián)慣導(dǎo)算法中螺旋理論的應(yīng)用［J］.系統(tǒng)工程與電子技術(shù)，2007，29(9):1533 -1535.SUN Feng，BEN Yue-yang.Application of spiral theory in strapdown inertial navigation algorithm［J］.Systems Engineering and Electronics，2007，29(9):1533 -1535.(in Chinese)

［10］ 楊偉光.捷聯(lián)慣性導(dǎo)航系統(tǒng)動(dòng)態(tài)誤差標(biāo)定與補(bǔ)償算法研究［D］.長(zhǎng)沙:國(guó)防科學(xué)技術(shù)大學(xué)，2009.YANG Wei-guang.Study on calibration and compensation of dynamic errors in SINS［D］.Changsha:National University of Defense Technology，2009.(in Chinese)

［11］ 吳高龍，劉華偉.對(duì)偶四元數(shù)導(dǎo)航的簡(jiǎn)化算法［J］.應(yīng)用科學(xué)學(xué)報(bào)，2010，28(6):646 -654.WU Gao-long，LIU Hua-wei.Simplified dual-quaternion navigation algorithm［J］.Journal of Applied Sciences，2010，28(6):646 -654.(in Chinese)