一種新的捷聯(lián)慣性導(dǎo)航系統(tǒng)姿態(tài)四元數(shù)方程求解方法

周召發(fā)， 胡文， 張志利， 徐梓皓， 陳河

(火箭軍工程大學(xué) 兵器發(fā)射理論與技術(shù)國(guó)家重點(diǎn)學(xué)科實(shí)驗(yàn)室， 陜西 西安 710025)

0　引言

姿態(tài)算法是捷聯(lián)慣性導(dǎo)航系統(tǒng)的關(guān)鍵技術(shù)之一。在捷聯(lián)慣性導(dǎo)航系統(tǒng)中，載體姿態(tài)的計(jì)算精度將直接影響著系統(tǒng)導(dǎo)航精度，因此提高載體姿態(tài)矩陣的計(jì)算精度是捷聯(lián)慣性導(dǎo)航系統(tǒng)研究的重要內(nèi)容[1-3]。由于載體的運(yùn)動(dòng)過(guò)程具有不確定性和剛體有限轉(zhuǎn)動(dòng)的不可交換性，現(xiàn)有的捷聯(lián)慣性導(dǎo)航系統(tǒng)姿態(tài)算法會(huì)引入不可交換誤差(也稱圓錐誤差)[4]，而減小不可交換誤差是提高解算載體姿態(tài)精度的有效方法。

自1971年Bortz[5]提出等效旋轉(zhuǎn)矢量的概念并建立分析圓錐運(yùn)動(dòng)誤差的理論基礎(chǔ)以來(lái)，大量國(guó)外學(xué)者對(duì)此進(jìn)行了研究。例如：Jordan提出了二子樣算法[4]，Miller[6]提出了三子樣算法，Lee等[7]提出了四子樣算法，Jiang等[8]提出了前一周期內(nèi)陀螺角增量輸出的優(yōu)化算法。在此基礎(chǔ)上，國(guó)內(nèi)一些學(xué)者對(duì)角速率輸入的圓錐補(bǔ)償算法進(jìn)行了一系列研究，提出了有效的優(yōu)化算法[9-10]。

多子樣等效旋轉(zhuǎn)矢量算法利用多項(xiàng)式對(duì)載體的角速度進(jìn)行擬合，利用旋轉(zhuǎn)矢量微分方程構(gòu)造旋轉(zhuǎn)矢量的泰勒級(jí)數(shù)展開(kāi)式求解等效旋轉(zhuǎn)矢量，通過(guò)等效旋轉(zhuǎn)矢量與四元數(shù)之間的關(guān)系求解姿態(tài)四元數(shù)[3,11]，再運(yùn)用上述兩個(gè)解算技巧減小不可交換誤差。傳統(tǒng)四元數(shù)算法是利用畢卡法(等價(jià)于單子樣旋轉(zhuǎn)矢量法)解算四元數(shù)微分方程，計(jì)算載體姿態(tài)四元數(shù)。畢卡法解算四元數(shù)微分方程既沒(méi)有對(duì)載體的角速度做多項(xiàng)式擬合，又沒(méi)有對(duì)姿態(tài)四元數(shù)進(jìn)行泰勒級(jí)數(shù)展開(kāi)。這兩大缺陷導(dǎo)致了四元數(shù)法的計(jì)算精度較低。因此，本文擬改進(jìn)四元數(shù)微分方程的求解方法，彌補(bǔ)傳統(tǒng)四元數(shù)算法的缺陷。

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

三子樣旋轉(zhuǎn)矢量法利用單周期內(nèi)3次角增量采樣值計(jì)算等效旋轉(zhuǎn)矢量，利用等效旋轉(zhuǎn)矢量計(jì)算出姿態(tài)四元數(shù)。三子樣旋轉(zhuǎn)矢量法對(duì)剛體在空間轉(zhuǎn)動(dòng)時(shí)產(chǎn)生的不可交換誤差做出了補(bǔ)償，減小了計(jì)算誤差。下面給出三子樣旋轉(zhuǎn)矢量法的主要推導(dǎo)過(guò)程。

三子樣旋轉(zhuǎn)矢量法從等效旋轉(zhuǎn)矢量與四元數(shù)之間的關(guān)系進(jìn)行推導(dǎo)。根據(jù)方程

Q(tk+1)=Q(tk)?q(h),

(1)

(2)

載體的角速率用拋物線擬合如下：

(3)

式中：a、b、c均為擬合載體角速度的參數(shù)；0≤τ≤h，h=tk+1-tk.

對(duì)等效旋轉(zhuǎn)矢量Φ做泰勒展開(kāi)：

(4)

采用文獻(xiàn)[1]中求解旋轉(zhuǎn)矢量的推導(dǎo)過(guò)程，得到等效旋轉(zhuǎn)矢量的求解結(jié)果為

(5)

2　三子樣四元數(shù)法

傳統(tǒng)四元數(shù)法中，一般采用畢卡法來(lái)解算四元數(shù)微分方程，畢卡法的實(shí)質(zhì)是單子樣旋轉(zhuǎn)矢量法。多子樣旋轉(zhuǎn)矢量法可以增加子樣數(shù)來(lái)提高載體姿態(tài)計(jì)算精度。該方法在計(jì)算姿態(tài)四元數(shù)之前，首先要計(jì)算當(dāng)前周期的等效旋轉(zhuǎn)矢量，然后借助等效旋轉(zhuǎn)矢量計(jì)算當(dāng)前時(shí)刻的姿態(tài)四元數(shù)。等效旋轉(zhuǎn)矢量法求解姿態(tài)四元數(shù)不是直接求解，而是通過(guò)當(dāng)前周期的等效旋轉(zhuǎn)矢量進(jìn)行求解。四元數(shù)法直接解算四元數(shù)微分方程，求解出當(dāng)前時(shí)刻載體的姿態(tài)四元數(shù)。比較四元數(shù)法和等效矢量旋轉(zhuǎn)矢量法求解載體姿態(tài)四元數(shù)的計(jì)算過(guò)程可以發(fā)現(xiàn)：四元數(shù)法計(jì)算更直接、更簡(jiǎn)潔，但是計(jì)算精度比等效旋轉(zhuǎn)矢量法低。若提高四元數(shù)法的計(jì)算精度，使之達(dá)到與多子樣旋轉(zhuǎn)矢量法相同的精度等級(jí)，則四元數(shù)算法的優(yōu)點(diǎn)就會(huì)凸顯出來(lái)。因此，三子樣四元數(shù)法是借鑒三子樣旋轉(zhuǎn)矢量法求解過(guò)程找到的一種新的計(jì)算四元數(shù)方法。

多子樣旋轉(zhuǎn)矢量法的計(jì)算過(guò)程有3個(gè)突出的優(yōu)點(diǎn)：1)在計(jì)算等效旋轉(zhuǎn)矢量時(shí)，對(duì)載體角速度用多項(xiàng)式進(jìn)行擬合，最大限度地減小了載體的計(jì)算誤差；2)應(yīng)用泰勒展開(kāi)式計(jì)算等效旋轉(zhuǎn)矢量，使得計(jì)算等效旋轉(zhuǎn)矢量的精度更高；3)利用等效旋轉(zhuǎn)矢量建立輸出角增量和旋轉(zhuǎn)矢量的關(guān)系。其中前兩點(diǎn)是多子樣旋轉(zhuǎn)矢量法的主要優(yōu)點(diǎn)。本文將多子樣旋轉(zhuǎn)矢量法中泰勒展開(kāi)式和多項(xiàng)式擬合角速度的解算技巧應(yīng)用到解算四元數(shù)微分方程中，建立起角增量輸出情況下四元數(shù)微分方程新的計(jì)算方法——三子樣四元數(shù)法。

2.1　四元數(shù)微分方程求導(dǎo)公式

四元數(shù)微分方程為

(6)

(7)

式中：ωx、ωy、ωz分別表示載體x軸、y軸、z軸的角速度。

記

(8)

則四元數(shù)微分方程可以寫(xiě)為

(9)

設(shè)Q和M(×)n階可導(dǎo)，對(duì)(9)式求各階導(dǎo)數(shù)，得到：

(10)

(11)

(12)

(13)

(14)

(15)

觀察(9)式～(15)式可以發(fā)現(xiàn)，自四元數(shù)微分方程的3階導(dǎo)數(shù)開(kāi)始，第n階導(dǎo)數(shù)的結(jié)果按照對(duì)Q求導(dǎo)次數(shù)由多到少的順序排列，各多項(xiàng)式的系數(shù)項(xiàng)排列恰好是楊輝三角形中的第n-1行。據(jù)此可寫(xiě)出四元數(shù)微分方程第n階導(dǎo)數(shù)的公式為

(16)

式中：C表示組合。運(yùn)用(16)式即可以求出四元數(shù)微分方程的任意階導(dǎo)數(shù)。

2.2　三子樣四元數(shù)法的推導(dǎo)過(guò)程

四元數(shù)微分方程的各階導(dǎo)數(shù)求解完畢，借鑒三子樣旋轉(zhuǎn)矢量法的計(jì)算技巧來(lái)解算四元數(shù)微分方程，并將這種解算方法稱為三子樣四元數(shù)法。

設(shè)Q(tk)和Q(tk+1)分別為tk和tk+1時(shí)刻從n系至b系的旋轉(zhuǎn)四元數(shù)，當(dāng)用(3)式表示載體角速度時(shí)，對(duì)Q(tk+1)做泰勒展開(kāi)：

(17)

記角增量為

(18)

(19)

(20)

聯(lián)立(7)式、(8)式以及(19)式，得到：

(21)

(22)

(23)

(24)

通過(guò)(18)式可以得到下列方程：

(25)

通過(guò)(25)式反解出a,b,c，得到：

(26)

通過(guò)(21)式～(24)式，可將(16)式進(jìn)行化簡(jiǎn)，得到三子樣四元數(shù)微分方程各階導(dǎo)數(shù)的通式為

(27)

結(jié)合(27)式，可以寫(xiě)出四元數(shù)微分方程各階導(dǎo)數(shù)，在計(jì)算和編程時(shí)用迭代法求解姿態(tài)四元數(shù)?；?jiǎn)(12)式～(15)式，將其中包含M的高階導(dǎo)數(shù)項(xiàng)去掉，得到姿態(tài)四元數(shù)的計(jì)算公式。限于篇幅，這里不再給出化簡(jiǎn)后的形式。

3　1階～6階三子樣四元數(shù)法表達(dá)式

下面求解旋轉(zhuǎn)四元數(shù)Q(tk+1)的泰勒展開(kāi)式。結(jié)合(21)式～(24)式，將(9)式～(15)式代入(17)式，化簡(jiǎn)整理，根據(jù)保留泰勒展開(kāi)式項(xiàng)數(shù)的多少得到不同階數(shù)的三子樣四元數(shù)算法。1階～6階三子樣四元數(shù)法的具體形式如下：

Q(tk+1)={I+[ah×]}Q(tk),

(28)

(29)

(30)

(31)

(32)

Q(tk+1)=[10[2c×]2+20[2c×][b×][a×]+
10[2c×][a×][b×]+10[2c×][a×]3+
15[b×][2c×][a×]+15[b×]3+
15[b×]2[a×]2+5[b×][a×][2c×]+
10[b×][a×][b×][a×]+5[b×][a×]2[b×]+
5[b×][a×]4+6[a×][2c×][b×]+
6[a×][2c×][a×]2+4[a×][b×][2c×]+
8[a×][b×]2[a×]+4[a×][b×][a×][b×]+
4[a×][b×][a×]3+3[a×]2[2c×][a×]+
3[a×]2[b×]2+3[a×]2[b×][a×]2+
[a×]3[2c×]+2[a×]3[b×][a×]+
[a×]4[b×]+[a×]6]Q(tk).

(33)

聯(lián)立(21)式～(24)式和(26)式，得到[ah×]、[bh2×]和[ch3×]的具體形式：

(34)

將(34)式代入(33)式，就可以得到角增量形式的三子樣四元數(shù)法計(jì)算結(jié)果。

4　實(shí)驗(yàn)仿真及分析

為了比較三子樣四元數(shù)算法與三子樣旋轉(zhuǎn)矢量法在解算載體姿態(tài)角上的精度差異，下面進(jìn)行仿真實(shí)驗(yàn)。由于圓錐運(yùn)動(dòng)是捷聯(lián)慣性導(dǎo)航姿態(tài)更新系統(tǒng)最?lèi)毫拥耐獠抗ぷ鳝h(huán)境，設(shè)計(jì)如下仿真實(shí)驗(yàn)：

觀察圖1(b)和圖1(c)可以發(fā)現(xiàn)，載體姿態(tài)誤差中橫滾角誤差隨著時(shí)間的延長(zhǎng)而發(fā)散，原因是三子樣四元數(shù)法進(jìn)行泰勒展開(kāi)后保留的階數(shù)少；逐漸提高三子樣四元數(shù)算法保留的階數(shù)，直至保留到9階時(shí)，橫滾角誤差收斂，仿真結(jié)果如圖1(c)所示。

圖2所示為不同階數(shù)時(shí)三子樣四元數(shù)法的橫滾角誤差圖。從圖2可知，三子樣四元數(shù)法取到8階時(shí)橫滾角誤差開(kāi)始趨于穩(wěn)定，取到9階時(shí)橫滾角誤差已經(jīng)穩(wěn)定。從橫滾角的收斂階數(shù)可知，三子樣四元數(shù)算法保留階數(shù)至少為9階。

相比于三子樣旋轉(zhuǎn)矢量法，三子樣四元數(shù)算法的計(jì)算量較大，因此有必要對(duì)三子樣四元數(shù)法的實(shí)時(shí)性進(jìn)行說(shuō)明。本文在仿真實(shí)驗(yàn)編程中，運(yùn)用迭代法給出了三子樣四元數(shù)法解算姿態(tài)四元數(shù)，而三子樣旋轉(zhuǎn)矢量法采用了文獻(xiàn)[1]的計(jì)算公式，仿真實(shí)驗(yàn)共進(jìn)行6次，實(shí)驗(yàn)結(jié)果如表1所示。

從表1可以看出：6次仿真試驗(yàn)中，三子樣旋轉(zhuǎn)矢量法平均用時(shí)2.02×10-4s，三子樣四元數(shù)法平均用時(shí)3.37×10-4s. 因此可以得出結(jié)論：雖然三子樣四元數(shù)法平均用時(shí)比三子樣旋轉(zhuǎn)矢量法長(zhǎng)，但可以滿足實(shí)時(shí)性要求。本文的對(duì)比仿真實(shí)驗(yàn)在理論上證實(shí)了三子樣四元數(shù)法的可行性，得出了9階三子樣四元數(shù)法的計(jì)算精度優(yōu)于三子樣旋轉(zhuǎn)矢量法的結(jié)果。

表1三子樣旋轉(zhuǎn)矢量法和9階三子樣四元數(shù)法仿真實(shí)驗(yàn)計(jì)算用時(shí)平均值

Tab.1Average calculating times of three-subsample rotation vector algorithm and three-subsample quaternion algorithm s

5　結(jié)論

1) 本文提出的三子樣四元數(shù)法的解算精度優(yōu)于三子樣旋轉(zhuǎn)矢量法。相比于旋轉(zhuǎn)矢量法，該算法不僅較大程度地提高了四元數(shù)的計(jì)算精度，而且免去了計(jì)算等效旋轉(zhuǎn)矢量這個(gè)中間步驟，直接求解姿態(tài)四元數(shù)，計(jì)算過(guò)程更直接簡(jiǎn)潔。

2) 仿真實(shí)驗(yàn)結(jié)果表明：9階三子樣四元數(shù)法的誤差比三子樣旋轉(zhuǎn)矢量法低一個(gè)數(shù)量級(jí)。

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

[1]秦永元.慣性導(dǎo)航[M].北京:科學(xué)出版社，2006: 287-316.

QIN Yong-yuan. Inertial navigation[M].Beijing: Science Press,2006: 287-316.(in Chinese)

[2]Song M, Wu W Q, Pan X F. Approach to recovering maneuver accuracy in classical coning algorithms[J]. Journal of Guidance Control & Dynamics, 2013, 36(6):1872-1880.

[3]Wang M S, Wu W Q,Wang J L,et al. High-order attitude compensation in coning and rotarion coexisting environment[J].IEEE Transactions on Aerospace and Electronic Systems, 2015,51(2):1178-1190.

[4]王立冬，劉軍，魯軍.捷聯(lián)慣導(dǎo)搖擺基座自對(duì)準(zhǔn)中圓錐誤差補(bǔ)償算法[J].兵工學(xué)報(bào)，2012, 33(7)：826-830.

WANG Li-dong, LIU Jun, LU Jun. Coning error compensation algorithm of SINS self-alignment on swaying base[J]. Acta Armamentarii, 2012, 33(7):826-830.(in Chinese)

[5]Bortz J E. A new mathematical formulation for strapdown inertial navigation[J]. IEEE Transactions on Aerospace and Electronic Systems, 1971, 7(1)：61-66．

[6]Miller R B．A new strapdown attitude algorithm[J]．Journal of Guidance，Control，and Dynamics，1983，6(4)：287-291．

[7]Lee J G，Yoon Y J，Mark J G．Extension of strapdown attitude algorithm for high-frequency base motion[J]．Journal of Guidance，Control，and Dynamics，1990，13(4)：738-743．

[8]Jiang Y F, Lin Y P. Improved strapdown coning algorithms[J]. IEEE Transactions on Aerospace & Electronic Systems, 1992, 28(2):484-490．

[9]曾慶化，劉建業(yè)，熊智，等.一種角速率激光陀螺慣導(dǎo)系統(tǒng)高精度姿態(tài)算法[J].上海交通大學(xué)學(xué)報(bào)，2006,40(12): 2159-2163.

ZENG Qing-hua, LIU Jian-ye, XIONG Zhi, et al. A high-accuracy attitude algorithm of ring laser gyro strap-down inertial navigation system with pure angle rate output[J]. Journal of Shanghai Jiao Tong University, 2006,40(12):2159-2163.(in Chinese)

[10]陳建鋒，陳熙源，祝雪芬. 硬件增強(qiáng)角速率圓錐優(yōu)化算法的姿態(tài)解算精度分析及改進(jìn)[J]. 東南大學(xué)學(xué)報(bào)，2012, 42(4): 632-636.

CHEN Jian-feng, CHEN Xi-yuan, ZHU Xue-fen. Accuracy analysis of attitude computation and improvement for coning algorithm optimized with hardware-enhanced angular rates[J]. Journal of Southeast University, 2012,42(4):632-636.(in Chinese)

[11]王洪輝， 楊紹卿，吳成富，等. 旋轉(zhuǎn)矢量在高動(dòng)態(tài)全姿態(tài)飛行器運(yùn)動(dòng)方程中的應(yīng)用[J].兵工學(xué)報(bào),2016,37(3):439-446.

WANG Hong-hui, YANG Shao-qing, WU Cheng-fu, et al. Application of rotation vector in equations of motion for all-attitude aircrafts[J].Acta Armamentarii, 2016,37(3):439-446.(in Chinese)