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

(火箭軍工程大學 兵器發(fā)射理論與技術(shù)國家重點學科實驗室， 陜西 西安 710025)

0　引言

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

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

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

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

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

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

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

(1)

(2)

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

(3)

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

對等效旋轉(zhuǎn)矢量Φ做泰勒展開：

(4)

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

(5)

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

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

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

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

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

(6)

(7)

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

記

(8)

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

(9)

設Q和M(×)n階可導，對(9)式求各階導數(shù)，得到：

(10)

(11)

(12)

(13)

(14)

(15)

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

(16)

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

2.2　三子樣四元數(shù)法的推導過程

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

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

(17)

記角增量為

(18)

(19)

(20)

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

(21)

(22)

(23)

(24)

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

(25)

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

(26)

通過(21)式～(24)式，可將(16)式進行化簡，得到三子樣四元數(shù)微分方程各階導數(shù)的通式為

(27)

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

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

下面求解旋轉(zhuǎn)四元數(shù)Q(tk+1)的泰勒展開式。結(jié)合(21)式～(24)式，將(9)式～(15)式代入(17)式，化簡整理，根據(jù)保留泰勒展開式項數(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ù)法計算結(jié)果。

4　實驗仿真及分析

為了比較三子樣四元數(shù)算法與三子樣旋轉(zhuǎn)矢量法在解算載體姿態(tài)角上的精度差異，下面進行仿真實驗。由于圓錐運動是捷聯(lián)慣性導航姿態(tài)更新系統(tǒng)最惡劣的外部工作環(huán)境，設計如下仿真實驗：

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

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

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

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

表1三子樣旋轉(zhuǎn)矢量法和9階三子樣四元數(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ù)的計算精度，而且免去了計算等效旋轉(zhuǎn)矢量這個中間步驟，直接求解姿態(tài)四元數(shù)，計算過程更直接簡潔。

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

參考文獻(References)

[1]秦永元.慣性導航[M].北京:科學出版社，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)慣導搖擺基座自對準中圓錐誤差補償算法[J].兵工學報，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è)，熊智，等.一種角速率激光陀螺慣導系統(tǒng)高精度姿態(tài)算法[J].上海交通大學學報，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]陳建鋒，陳熙源，祝雪芬. 硬件增強角速率圓錐優(yōu)化算法的姿態(tài)解算精度分析及改進[J]. 東南大學學報，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)矢量在高動態(tài)全姿態(tài)飛行器運動方程中的應用[J].兵工學報,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)