基于優(yōu)化旋轉(zhuǎn)矢量雙子樣算法的捷聯(lián)慣導(dǎo)姿態(tài)解算

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

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

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

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

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

1 旋轉(zhuǎn)矢量雙子樣算法

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

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

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

(1)

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

1.2 旋轉(zhuǎn)矢量與微分方程

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

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

(2)

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

(3)

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

(4)

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

1.3 旋轉(zhuǎn)矢量雙子樣算法

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

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

(5)

并記角增量為

(6)

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

(7)

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

(8)

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

(9)

計(jì)算式(8)的各階導(dǎo)數(shù),并將結(jié)果代入式(9)繼續(xù)化簡,得

(10)

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

(11)

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

(12)

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

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

2.1 圓錐運(yùn)動(dòng)模型

假設(shè)b系為載體的瞬時(shí)坐標(biāo)系O-xyz,R系為載體平衡時(shí)的坐標(biāo)系O-XYZ,b系的運(yùn)動(dòng)角速度為ω,半錐角α為小角,即b系在R系附近微幅擺動(dòng).運(yùn)動(dòng)軌跡如圖1,這種運(yùn)動(dòng)稱為圓錐運(yùn)動(dòng).

圖1 圓錐運(yùn)動(dòng)示意圖Fig.1 Conning motion

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

(13)

2.2 圓錐運(yùn)動(dòng)與子樣

w(t)=

(14)

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

(15)

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

(16)

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

(17)

記

(18)

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

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

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

(19)

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

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

(20)

(21)

(22)

(23)

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

(24)

(25)

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

(26)

整理并化簡可得

(27)

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

(28)

此時(shí)求解k2和k3就轉(zhuǎn)化為求解函數(shù)f的最小值問題.當(dāng)f取最小值時(shí)

(29)

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

(30)

3 解算實(shí)例分析

3.1 實(shí)驗(yàn)條件

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

3.2 解算對比分析

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

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

a)航向角速率

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

a)四元數(shù)法

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

a)四元數(shù)法

b) 優(yōu)化旋轉(zhuǎn)矢量雙子樣算法圖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)化旋轉(zhuǎn)矢量雙子樣算法圖6 規(guī)律2下解算后的航向角誤差Fig.6 Curves of φ error under swing pattern 2

a) 四元數(shù)法

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

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

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

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

4 結(jié)論

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

參考文獻(xiàn)(References)

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

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

[ 3 ] 屈熠,趙忠. 增強(qiáng)型等效旋轉(zhuǎn)矢量算法在圓錐運(yùn)動(dòng)下的仿真[J].計(jì)算機(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 ] 李海濤,曹詠弘,祖靜. 等效旋轉(zhuǎn)矢量法在旋轉(zhuǎn)彈姿態(tài)解算中的應(yīng)用[J].測試技術(shù)學(xué)報(bào),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 ] 馬捷,孟超,王瑋,等. 三子樣旋轉(zhuǎn)矢量優(yōu)化姿態(tài)算法在捷聯(lián)慣導(dǎo)中的研究[J].火炮發(fā)射與控制學(xué)報(bào),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組合導(dǎo)航系統(tǒng)設(shè)計(jì)及實(shí)現(xiàn)方案[J]. 江蘇科技大學(xué)學(xué)報(bào):自然科學(xué)版,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)慣導(dǎo)四子樣旋轉(zhuǎ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)慣導(dǎo)姿態(tài)更新的四子樣旋轉(zhuǎn)矢量優(yōu)化算法研究[J].中國慣性技術(shù)學(xué)報(bào),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è),曾慶化,等. 等效旋轉(zhuǎn)矢量系數(shù)優(yōu)化的參數(shù)解析法[J]. 中國慣性技術(shù)學(xué)報(bào),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] 秦永元.慣性導(dǎo)航[M].北京：科學(xué)出版社，2006:320-325.