唐 煒, 劉林飛, 張 鵬, 胡海秀

(江蘇科技大學 機械工程學院,江蘇 鎮(zhèn)江 212003)

在捷聯(lián)慣導系統(tǒng)中,載體的所有導航信息都從當前姿態(tài)陣中獲取.從某種意義上說,姿態(tài)陣的可靠度將直接影響著系統(tǒng)的導航精度,故如何選擇合適的算法對姿態(tài)矩陣進行實時更新就成為了整個導航過程中最核心的工作[1-2].

傳統(tǒng)四元數(shù)法由于計算量小、可靠性高、可在全姿態(tài)下工作等突出特點在工程實踐中得到了廣泛應用,但該算法本身的缺陷使得其對不可交換性誤差無法作出有效補償,而且誤差累積效應也比較明顯.等效旋轉矢量法則可以有效降低這一誤差的影響,文獻[3-4]推導了等效旋轉矢量雙子樣算法微分方程的增強型解,但并未進行優(yōu)化,實際上在采樣時間間隔內(nèi)角速度不可能完全理想化,因此算法本身尚待完善.對于捷聯(lián)慣導姿態(tài)算法來說,圓錐運動是最惡劣的工作環(huán)境條件,它可以引起姿態(tài)解算的嚴重漂移.故確保圓錐運動環(huán)境條件下的算法漂移最小,就一定能確保在其余環(huán)境條件下的算法漂移最小[5].此外,隨著算法的子樣數(shù)增多和采樣頻率的提高,姿態(tài)矩陣計算會更加準確,但也將造成導航計算機的運算工作量快速增加,從而對系統(tǒng)的實時性造成不利影響.故如何尋求一種能有效兼顧系統(tǒng)實時性與導航精度的姿態(tài)解算算法就顯得十分必要.

文中基于圓錐運動環(huán)境,分別應用四元數(shù)法和優(yōu)化旋轉矢量雙子樣算法對捷聯(lián)慣導系統(tǒng)進行姿態(tài)解算,并對它們的解算誤差加以分析和比較,以期為姿態(tài)算法的工程化應用提供相應參考.

1 旋轉矢量雙子樣算法

1.1 旋轉矢量與姿態(tài)四元數(shù)

旋轉矢量法的姿態(tài)陣實時更新實際上就是通過不斷采集各陀螺儀與加速度計的輸出,計算出變化的四元數(shù),并與姿態(tài)四元數(shù)運算以實現(xiàn)對當前姿態(tài)四元數(shù)的更新.

設tk時刻的機體坐標系為b(k),導航坐標系為n(k).記b(k)至b(k+1)的旋轉四元數(shù)為q(h),則姿態(tài)陣的更新方程為:

(1)

式中：Φ為b(k)至b(k+1)的等效旋轉矢量,φ=‖Φ‖,q(h)也稱為[tk,tk+1]時間段內(nèi)的姿態(tài)四元數(shù).

1.2 旋轉矢量與微分方程

在tk≤t≤tk+1采樣間隔內(nèi),根據(jù)式(1)可得如下方程:

Q(t)=Q(tk)?q(t-tk)

(2)

經(jīng)過化簡運算,即可得到等效旋轉矢量的微分方程,即Bortz方程[6]:

(3)

對式(3)中的三角函數(shù)作展開,略去高次項,可得到工程實踐常用方程:

(4)

從式(4)可以看出,旋轉矢量的導數(shù)等于相應的角速率再加上兩項,這兩項修正項實際上表明了等效旋轉矢量法對不可交換性誤差所做出的補償.

1.3 旋轉矢量雙子樣算法

因采樣區(qū)間不可能無限小,故在采樣間隔內(nèi)進行動態(tài)積分就必定會帶來誤差,且會隨時間增加而不斷累積.此外,引起誤差漂移的因素還有陀螺儀常值漂移、傳感器初始對準精度等.在此不涉及如何減小誤差漂移,而僅考慮姿態(tài)解算方法自身的有效性.在姿態(tài)更新周期內(nèi),擬合曲線階數(shù)越高,子樣數(shù)越多,則算法精度就越高,但也會造成運算量明顯增加而影響到解算的實時性.為了兼顧系統(tǒng)精度及其實時性,文中采用等效旋轉矢量雙子樣算法進行姿態(tài)解算,并在此基礎上推導誤差補償項,以獲得更加精確的解[7-9].

設姿態(tài)更新周期為h,采用直線擬合角速度.即令角速度的模型為

(5)

并記角增量為

(6)

則可得到角速度、角增量各階導數(shù)的以下關系:

(7)

由于姿態(tài)更新周期一般為毫秒級,Φ可視為小量,故將第2項Φ用角增量Δθ代替,得

(8)

將等效旋轉矢量按照Taylor級數(shù)展開,可得

(9)

計算式(8)的各階導數(shù),并將結果代入式(9)繼續(xù)化簡,得

(10)

令Δθ1,Δθ2分別為[tk,tk+0.5h],[tk+0.5h,tk+1]時間段內(nèi)的角增量,對式(6)分段積分

(11)

將式(11)代入式(10),化簡整理后可得

(12)

式(12)是式(8)的精確解.與傳統(tǒng)等效旋轉矢量雙子樣算法相比,此處的計算結果多出一項Σ(h).在此,將Σ(h)稱為等效旋轉矢量雙子樣算法的誤差補償項.

2 圓錐運動環(huán)境下算法優(yōu)化

2.1 圓錐運動模型

假設b系為載體的瞬時坐標系O-xyz,R系為載體平衡時的坐標系O-XYZ,b系的運動角速度為ω,半錐角α為小角,即b系在R系附近微幅擺動.運動軌跡如圖1,這種運動稱為圓錐運動.

圖1 圓錐運動示意圖Fig.1 Conning motion

將R系至b系的變換看作是無中間過程的一次性等效旋轉,則旋轉瞬軸可表示為

(13)

2.2 圓錐運動與子樣

w(t)=

(14)

故b系相對于R系的角速度為

(15)

設Δθ1,Δθ2為姿態(tài)更新周期h二等分的角增量輸出

(16)

根據(jù)式(15)可得

(17)

記

(18)

2.3 圓錐運動下的系數(shù)優(yōu)化

根據(jù)式(12)可假設

Φ(h)=Δθ1+Δθ2+k1Δθ1×Δθ2+k2Δθ1×(Δθ1×Δθ2)+k3Δθ2×(Δθ1×Δθ2)

(19)

式中:k1，k2，k3均為待定優(yōu)化系數(shù),優(yōu)化的標準是圓錐運動環(huán)境下算法漂移最小.由于實際在姿態(tài)更新周期內(nèi)角速度變化規(guī)律不一定按直線變化,故k1,k2,k3不一定與式(12)對應項的系數(shù)相同.

將式(14)展開,并將式(18)代入化簡可得

(20)

(21)

(22)

(23)

將三角函數(shù)按照Taylor級數(shù)展開并取前3項，代入化簡整理后可得

(24)

(25)

將式(25)代入式(19),并與式(12)進行對比,在兼顧直線擬合和圓錐運動優(yōu)化所得的兩個方程的基礎上來求解未知數(shù).在此定義函數(shù)Ψ為

(26)

整理并化簡可得

(27)

根據(jù)最小方差原則確定k2和k3.令k2=k3=k,引出函數(shù)f:

(28)

此時求解k2和k3就轉化為求解函數(shù)f的最小值問題.當f取最小值時

(29)

故旋轉矢量雙子樣算法在圓錐運動環(huán)境下的優(yōu)化算法為:

(30)

3 解算實例分析

3.1 實驗條件

本實驗使用了2TS-450雙軸速率位置轉臺,它可同時設置內(nèi)框(繞Z軸)和外框(繞Y軸)的運動規(guī)律,且以報文的形式對外實時輸出其運動狀態(tài).在此將沿三軸相互正交配置的陀螺儀和加速度計安裝于轉臺中心位置,使轉臺內(nèi)、外框分別按以下兩種正弦規(guī)律運動:①規(guī)律1:內(nèi)、外框振幅為10°,擺動周期為1 s;②規(guī)律2:內(nèi)、外框振幅為8°,擺動周期為1.25 s.兩種情況下均待轉臺運行穩(wěn)定后,開始多通道采集轉臺和傳感器的數(shù)據(jù)，采樣頻率為400 Hz，時間為25 s.

3.2 解算對比分析

在此分別應用傳統(tǒng)四元數(shù)法和優(yōu)化旋轉矢量雙子樣法兩種算法進行捷聯(lián)慣導姿態(tài)解算,并將兩種情況下解算結果與轉臺標準信號進行對比分析.

1)規(guī)律1下的傳感器輸出航向角速率和橫滾角速率原始信號見圖2.系統(tǒng)姿態(tài)解算出的航向角誤差和橫滾角誤差分別如圖3,4.慣導解算誤差累積效應不可避免,故姿態(tài)角會存在誤差漂移現(xiàn)象.

a)航向角速率

b) 橫滾角速率圖2 規(guī)律1下的陀螺儀輸出角速率原始信號Fig.2 Original curves of ωφ-t & ωγ-t under swing pattern 1

a)四元數(shù)法

b) 優(yōu)化旋轉矢量雙子樣算法圖3 規(guī)律1下解算后的航向角誤差Fig.3 Curves of φ error under swing pattern 1

a)四元數(shù)法

b) 優(yōu)化旋轉矢量雙子樣算法圖4 規(guī)律1下解算后的橫滾角誤差Fig.4 Curves of γ error under swing pattern 1

2)規(guī)律2下的傳感器輸出航向角速率和橫滾角速率原始信號見圖5.系統(tǒng)姿態(tài)解算出的航向角誤差和橫滾角誤差分別如圖6,7.

a) 航向角速率

b) 橫滾角速率圖5 規(guī)律2下的陀螺儀輸出角速率原始信號Fig.5 Original curves of ωφ-t & ωγ-t under swing pattern 2

a) 四元數(shù)法

b) 優(yōu)化旋轉矢量雙子樣算法圖6 規(guī)律2下解算后的航向角誤差Fig.6 Curves of φ error under swing pattern 2

a) 四元數(shù)法

b)優(yōu)化旋轉矢量雙子樣算法圖7 規(guī)律2下解算后的橫滾角誤差Fig.7 Curves of γ error under swing pattern 2表1 姿態(tài)解算誤差的方差對比Table 1 Variances of attitude errors about two algorithms

算法擺動規(guī)律1航向角誤差方差橫滾角誤差方差擺動規(guī)律2 航向角誤差方差橫滾角誤差方差 四元數(shù)法1.311E-0021.377E-0025.126E-0035.257E-003 優(yōu)化旋轉矢量雙子樣算法4.187E-0033.925E-0031.753E-0031.484E-003

表1為轉臺在上述兩種擺動規(guī)律條件下,兩種算法解算誤差在剔除一次趨勢項后的方差對比.

通過對比分析,可得到如下結論:① 從理論上分析,由于采樣時間不能趨近無窮小以及截斷性誤差等影響因素的存在,兩種算法都會有不同程度的誤差存在,但是相同情況下,優(yōu)化旋轉矢量雙子樣算法的誤差值相對較小；② 由于增加了優(yōu)化后的誤差補償項,加之旋轉矢量法自身特點,故優(yōu)化旋轉矢量雙子樣算法的解算結果會比四元數(shù)法更加精確；③ 由于實驗是采用雙軸輸入,故解算結果會存在一定的相互影響,但即便如此,旋轉矢量雙子樣算法對誤差累積的抑制效果依然比四元數(shù)法要好；④ 與四元數(shù)法相比,旋轉矢量雙子樣算法的計算時間會有所增加,但并沒有影響系統(tǒng)解算的實時性.

4 結論

文中分別利用四元數(shù)法和優(yōu)化旋轉矢量雙子樣算法對捷聯(lián)慣導系統(tǒng)進行姿態(tài)解算.通過對比可知,在相同工作環(huán)境下,優(yōu)化旋轉矢量雙子樣算法的計算量雖有所增加,但在滿足系統(tǒng)實時性要求的前提下可較明顯地提高姿態(tài)解算精度.此外,優(yōu)化旋轉矢量雙子樣算法本身還能有效降低不可交換性誤差以及誤差累積效應對系統(tǒng)精度的不良影響,可有效彌補四元數(shù)法在這一方面存在的不足.實例表明,優(yōu)化旋轉矢量雙子樣算法在低子樣數(shù)的情況下對如何有效提高系統(tǒng)精度具有重要的參考價值.

參考文獻(References)

[ 1 ] 張樹俠.捷聯(lián)式慣性導航系統(tǒng)原理[M].哈爾濱：哈爾濱工程大學出版社，1992:11-20.

[ 2 ] Bortz J E.A new mathematical formulation for strapdown inertial navigation[J].IEEETransactionsonAerospaceandElectronicSystems,1971,7(1)：61-66.

[ 3 ] 屈熠,趙忠. 增強型等效旋轉矢量算法在圓錐運動下的仿真[J].計算機仿真,2010,11(3):25-27.

Qu Yi,Zhao Zhong. Simulation of improved rotation vector algorithm in coning motion[J].ComputerSimulation,2010,11(3):25-27.(in Chinese)

[ 4 ] 李海濤,曹詠弘,祖靜. 等效旋轉矢量法在旋轉彈姿態(tài)解算中的應用[J].測試技術學報,2011,25(4):287-291.

Li Haitao,Cao Yonghong,Zu Jing. Application of rotation vector algorithm to attitude updating for spinning projectiles[J].JournalofTestandMeasurementTechnology,2011,25(4):287-291.(in Chinese)

[ 5 ] 馬捷,孟超,王瑋,等. 三子樣旋轉矢量優(yōu)化姿態(tài)算法在捷聯(lián)慣導中的研究[J].火炮發(fā)射與控制學報,2009 (3): 25-28.

Ma Jie,Meng Chao,Wang Wei,et al. Research on optimal attitude algorithm of 3-subsample rotation vector in strapdown inertial navigation system[J].JournalofGunLaunch&Control,2009(3): 25-28.(in Chinese)

[ 6 ] 張軍,王直,劉維亭. SINS/Two-antenna GPS組合導航系統(tǒng)設計及實現(xiàn)方案[J]. 江蘇科技大學學報:自然科學版,2004,18 (5): 43-46.

Zhang Jun,Wang Zhi,Liu Weiting. Design and realization of SINS/Two-antenna GPS integrated navigation system[J].JournalofJiangsuUniversityofScienceandTechnology:NaturalScienceEdition,2004,18 (5):43-46.(in Chinese)

[ 7 ] 張澤,段廣仁. 捷聯(lián)慣導四子樣旋轉矢量姿態(tài)更新算法[J].控制工程,2010,17(3):272-274.

Zhang Ze, Duan Guangren. On four-sample rotation vector algorithm for SINS attitude updating[J].ControlEngineeringofChina,2010, 17(3):272-274.(in Chinese)

[ 8 ] 秦永元,張士邈.捷聯(lián)慣導姿態(tài)更新的四子樣旋轉矢量優(yōu)化算法研究[J].中國慣性技術學報,2001, 9(4):1-3.

Qin Yongyuan, Zhang Shimiao. Research on optimal four-sample rotation vector algorithm for SINS attitude updating[J].JournalofChineseInertialTechnology, 2001,9(4):1-3.(in Chinese)

[ 9 ] 祝燕華,劉建業(yè),曾慶化,等. 等效旋轉矢量系數(shù)優(yōu)化的參數(shù)解析法[J]. 中國慣性技術學報,2008,16(1): 46-62.

Zhu Yanhua,Liu Jianye,Zeng Qinghua,et al.Parameter analysis method for optimizing rotation vector[J].JournalofChineseInertialTechnology,2008,16(1).46-62(in Chinese)

[10] 秦永元.慣性導航[M].北京：科學出版社，2006:320-325.