捷聯(lián)系統(tǒng)圓錐優(yōu)化算法研究

摘 要:傳統(tǒng)的捷聯(lián)姿態(tài)算法采用角增量計算旋轉(zhuǎn)矢量。而對于采用光纖陀螺、微機械陀螺等新型慣性傳感器的捷聯(lián)系統(tǒng)，陀螺輸出信號為角速率。通過角速率積分得角增量會帶來計算誤差。因此，以角速率作為圓錐算法的輸入信號。參照傳統(tǒng)算法，用角速率表示旋轉(zhuǎn)矢量的微分方程，以角速率的叉乘項擬合圓錐誤差項，并且在典型圓錐環(huán)境中計算多項式的系數(shù)。推導出一組改進的圓錐補償算法，并給出新算法的誤差漂移表達式。采用典型圓錐環(huán)境測試新算法的性能，當算法的輸入為角速率時，新算法的精度比較傳統(tǒng)算法有顯著的提高。 關(guān)鍵詞:捷聯(lián)姿態(tài)算法;旋轉(zhuǎn)矢量;角速率;圓錐補償

中圖分類號:U666.1 文獻標志碼:A文章編號:16717953(2009)04002204

Study of Optimized Coning Algorithm for SINS

LIU Xingzhang FU Jiannan2

(1.Liaonan Shipyard，Dalian，116041，China;2.College of automation，Harbin Engineering University，Harbin 150001，China)

Abstract: Among the numerous conventional treatises on strarpdown attitude algorithms， most utilize incremental angle to compute rotation vector. But that isn't appropriate for the fiber-optic gyro and micro-electromechanical system (MEMS) outputting angle rate. Because the integration of angle rate for the purpose of getting incremental angle causes error. So we used angle rate as algorithm input， expressed differential equation with angle rate， approximated coning term with multiply of angle rates. The coefficients were optimized under classical coning circumstance. The new set of coning compensation algorithms and their error drifts were derived. The performances of improved algorithm were tested under classical coning circumstance. When angle rate is used as input， the accuracy of attitude computation can be improved effectively as compared with conventional algorithms.

Key words: strarpdown attitude algorithm;rotation vector;angle rate;coning compensation

剛體轉(zhuǎn)動的不可交換性導致圓錐誤差的產(chǎn)生，而圓錐誤差是影響捷聯(lián)姿態(tài)矩陣計算精度的重要因素^[1]。研究合適的旋轉(zhuǎn)矢量修正算法是克服圓錐誤差的有效途徑。目前的旋轉(zhuǎn)矢量算法都是基于陀螺輸出角增量信息^[2]，對于陀螺輸出為角速率信號的新型捷聯(lián)系統(tǒng)已不再適用。本文以角速率作為算法的輸入信號，設(shè)計出改進的圓錐補償算法，提高了捷聯(lián)系統(tǒng)姿態(tài)計算的精度。

1 傳統(tǒng)算法

1.1 傳統(tǒng)三子樣算法

傳統(tǒng)姿態(tài)算法以旋轉(zhuǎn)矢量描述載體的運動姿態(tài)，旋轉(zhuǎn)矢量的微分方程可表示為^[3]:

#8226;=ω+12×ω+1α21-αsinα2-2cosα×(×ω)

(1)

其中表示幅值為α=(T)1/2的旋轉(zhuǎn)矢量，ω表示角速度。當α足夠小時，的高次項可忽略不計。簡化式(1)，得到工程中常用的近似方程:

#8226;=ω+12α×ω，α=∫ωdτ(2)

式(2)中的第二項反映了在姿態(tài)解算周期中剛體轉(zhuǎn)動的不可交換性誤差對旋轉(zhuǎn)矢量計算的影響。

在每個姿態(tài)計算周期h內(nèi)三次采樣角增量信號，得到三子樣旋轉(zhuǎn)矢量表達式^[4]:

Δ^=θ+3380(θ1×θ3)+5780θ2×(θ3-θ1)(3)

1.2 算法的誤差分析

當載體的兩個相互垂直軸有相同頻率而不同相位的正弦角振動輸入時，在與它們相垂直的第三軸就會產(chǎn)生圓錐運動。用載體的角速率描述典型圓錐運動為^[5]

ω=αΨcosΨtbΨsinΨt0(4)

式中Ψ為錐運動角速度，a，b為沿載體兩個正交軸的角振蕩幅值。積分得角增量

α=∫t+htωdτ=a(sinΨ(t+h)-sinΨt)b(cosΨ(t+h)-cosΨt)0(5)

由式(2)、式(5)可得

Δz=12∫α×ωdτ=abΨ2h-1ΨsinΨh(6)

式(6)表示真實的圓錐誤差項，這里圓錐誤差的整流項僅存在于z軸。式(3)為傳統(tǒng)三子樣法的計算得到的圓錐誤差項。兩式相差即是算法的誤差漂移。

ε=Δx-Δ^x(7)

當陀螺輸出為角增量時，傳統(tǒng)的三子樣算法的誤差漂移為

ε=1204120α2(Ψh)7(8)

當陀螺輸出為角速率時，參照Simpson積分法，傳統(tǒng)的三子樣算法可采用下述的角增量提取公式:

θ1=[5ω(t)+8ω(t+h/3)-ω(t+2h/3)]#8226;h/36θ2=[-ω(t)+8ω(t+h/3)+5ω(t+2h/3)]#8226;h/36θ3=[-ω(t+h/3)+8ω(t+2h/3)+5ω(t+h)]#8226;h/36θ=[ω(t)+3ω(t+h/3)+3ω(t+2h/3)+ω(t+h)]#8226;h/6(9)

將式(9)代入式(3)中，再由式(7)可得角速率輸入情況下，傳統(tǒng)三子樣算法的誤差漂移為

ε=12592α2(Ψh)5(10)

通過比較式(8)與式(10)可知，由角速率積分提取角增量的過程中，Simpson積分法引入積分算法誤差，導致總的算法誤差明顯增大。

2 改進算法

從上節(jié)可知，當使用角速率陀螺時，傳統(tǒng)旋轉(zhuǎn)矢量算法的適用性出現(xiàn)問題。其性能與不包含圓錐補償?shù)乃碾ALgkd法相比并無優(yōu)越性^[6]。研究直接使用角速率信號表示旋轉(zhuǎn)矢量，提出一類改進的圓錐補償算法。

2.1 算法的優(yōu)化設(shè)計

在典型的圓錐環(huán)境中對算法進行設(shè)計。圓錐運動的角速率描述如式(4)所示。則

ωi×ωj=[0，0，abΨ 2sin(Ψ(tj-ti))]T(11)

陀螺采樣周期為H，即h=NH。計算周期為整數(shù)N倍的采樣周期，則式(11)有

ωi×ωj=[0，0，abΨ 2sin(ΨH(j-i))]T(12)

由上式可見，與θi×θj的性質(zhì)相類似，ωi×ωj的計算值僅與相對時間有關(guān)，而與絕對時間無關(guān)。

設(shè)λ=ΨH，式(12)化為

ωi×ωj=abλ2H2sin(λ(j-i))(13)

由于相同時間間隔的角速率叉乘項的計算值是相等的，用下式計算得Δ^。

Δ^=∑N-1i=0kN-iω(i)×ω(N)(14)

式中ω(i)，i=0…N-1分別為姿態(tài)計算周期內(nèi)的各個采樣值。將式(13)代入式(14)中，可得

Δ^=abH2[(A11k1+A12k2+…+A1，nkn)λ3-(A21k1+A22k2+…+A2，nkn)λ5+…+(-1)i+1(Ai，1k1+Ai，2k2+…+A2，nkn)λ2i+1+…](15)

式中系數(shù)Ai，j定義為

Ai，j=j2i-1(2i-1)!(16)

為了確定系數(shù)ki的值，將式(6)中真實的圓錐誤差項展開成Taylor級數(shù)的形式，有

Δ=ab2Ψ(Ψh)33!-(Ψh)55!+…=abH2[d1λ3-d2λ5+…+(-1)idiλ2i+1+…](17)

其中

di=N2i+1H22(2i+1)!，h=NH(18)

將式(15)寫為矩陣的形式有

Δ^=abH2[λ3，λ5，λ7…]A∞×N#8226;kN×1(19)

其中

A∞×N=A11…A1，N………Ai，1…Ai，N………(20)

kN-1×1=[k11，…，kN，1]T(21)

式(17)的矩陣形式為

Δ=abH2[λ3，λ5，λ7…]D∞×1(22)

其中

D∞×1=[d1，…，d2]T(23)

算法的優(yōu)化過程既是在姿態(tài)更新周期內(nèi)，使圓錐誤差項的真實值與計算值盡可能相等。設(shè)子樣數(shù)為N，那么待定系數(shù)ki的個數(shù)就為N。由式(19)和式(22)，待定系數(shù)ki可以由下式計算

[Ai，j][ki]=[Di]i=1，…N j=1，…N (24)

優(yōu)化系數(shù)ki是由矩陣Ai，j與Di，的前為N行計算得到的。由式(7)可知，改進圓錐補償算法的誤差漂移為

ε=abH2λ2N+3∑Nj=1AN+1，jkj-dN+1(25)

算法的優(yōu)化過程可總結(jié)為:首先，由式(20)、式(21)計算系數(shù)Ai，j與Di。然后通過式(24)得到優(yōu)化后的系數(shù)ki。最后將系數(shù)ki代入到式(14)中，推導出改進的圓錐補償算法。

2.2 算例

如果陀螺在t時刻輸出角速率ω(0)，并在每個計算周期內(nèi)對陀螺輸出在t+h/3、t+2h/3、t+h時刻采樣兩次ω(1)、ω(2)、ω(3)。得到改進的三子樣算法

Δ^=3H8[ω(0)+3ω(1)+3ω(2)+ω(3)]+H2872240ω(0)+2756ω(1)+26192240ω(2)×ω(3)(16)

由式(25)，改進的三子樣算法的誤差漂移為

ε=ab8907055(Ψh)9(17)

當N=2時，得到改進的二子樣算法

Δ^=2H6[ω(0)+4ω(1)+ω(2)]+H2145ω(0)+2845ω(1)×ω(2)(18)

由式(25)，改進的二子樣算法的誤差漂移為

ε=ab80640(Ψh)7(19)

通常情況下Ψh<1。比較式(17)與式(10)可知，改進算法的計算誤差遠小于傳統(tǒng)算法。精度提高三個數(shù)量級以上。

3 仿真分析

在式(4)所描述的典型圓錐環(huán)境中，測試傳統(tǒng)三子樣算法(3I)、改進二子樣算法(2C)、改進三子樣算法(3C)的性能。

陀螺的器件誤差設(shè)置為0。選取圓錐運動頻率10Hz，即Ψ=20πrad#8226;s-1，角振蕩幅值a=b=1deg。陀螺的輸出信號為角速率，采樣頻率為100Hz。各算法的姿態(tài)誤差取對數(shù)形式如圖1所示。由于圓錐誤差的整流分量主要體現(xiàn)在航向誤差角上^[7]，這里僅給出了航向誤差角的曲線。以改進的三子樣算法為例分析3I的誤差漂移。誤差漂移與陀螺采樣頻率的關(guān)系如圖2所示。陀螺采樣頻率變化范圍:100Hz~300Hz。誤差漂移與圓錐頻率的關(guān)系如圖3所示。圓錐頻率變化范圍:1Hz~20Hz。

分析圖1可知，在相同采樣頻率的條件下，改進的圓錐補償算法比較傳統(tǒng)算法精度上有顯著的提高(3個數(shù)量級左右)。同時，算法的精度與計算周期內(nèi)的子樣數(shù)成正比。由圖2可知，在同一圓錐運動頻率下，算法的誤差漂移與陀螺的采樣頻率成反比。由圖3可知，在相同陀螺采樣頻率下，算法的誤差漂移與圓錐運動頻率成反比。即載體所處的運動環(huán)境越惡劣，計算就誤差越大。

4 結(jié)論

以角速率作為輸入信號，優(yōu)化設(shè)計了旋轉(zhuǎn)矢量多項式中的系數(shù)，推導得出改進的圓錐補償算法。通過分析與仿真，可得以下結(jié)論:角速率輸入時，改進算法避免了傳統(tǒng)算法的積分誤差，其性能優(yōu)于傳統(tǒng)算法;無論是改進算法還是傳統(tǒng)算法，增加計算周期內(nèi)的子樣數(shù)，提高陀螺采樣頻率都是提高算法精度的有效途徑。

參考文獻

[1] B. ITZHACK.Navigation computation in terrestrial strapdown inertial navigation system[J].IEEE Transactions on Aerospace and Electronic，1979，13(6):679-689.

[2] SAVAGE.P.G.Strapdown Inertial Navigation Algorithm Design，Part 2:Attitude Algorithms[J].AIAA Journal of Guidance，Control and Dynamics，1998，21(1):19-28.

[3] LITMANOVICH.Y.A.Two new classes of strapdown navigation algorithms[J]. AIAA Journal of Guidance，Control and Dynamics，2000，23(1):34-44.

[4] WU.Y.Y.Strapdown inertial navigation system algorithm based on dual quaternion[J].IEEE Transactions on aerospace and electronic systems，2005，41(1):110-132.

[5] SALYCHEV.O.S.Inertial Systems in Navigation and Geophysics[M].Moscow:Bauman MSTU Press，1998.

[6] ROSCOE.K.M.Equivalency between strapdown navigation coning and sculling integral/algorithms[J].AIAA Journal of Guidance，Control and Dynamics，2000，24(2):201-206.

[7] SALYCHEV.O.S.Applied Inertial Navigation:Problems and Solutions [M].Moscow:Bauman MSTU Press，2004.