何虔恩，曾聰杰，林志賢，劉開進(jìn)

(1.福州大學(xué) 物理與信息工程學(xué)院，福州350116; 2.福建星海通信科技有限公司，福州350001)

0　引言

慣性導(dǎo)航解算過(guò)程需要利用姿態(tài)陣將平臺(tái)系(P系，即慣性器件安裝的坐標(biāo)系)中的比力矢量轉(zhuǎn)換到導(dǎo)航坐標(biāo)系下，再進(jìn)行積分運(yùn)算，依次得到速度和位置信息。因此，要得到高精度的導(dǎo)航參數(shù)，需要有高精度的姿態(tài)陣。在捷聯(lián)式慣導(dǎo)系統(tǒng)中，慣性器件與載體固連，平臺(tái)系姿態(tài)變化劇烈，因而對(duì)姿態(tài)陣解算的精度和實(shí)時(shí)性要求較高。在平臺(tái)式慣導(dǎo)系統(tǒng)中，由于大部分載體角運(yùn)動(dòng)被框架伺服系統(tǒng)隔離，平臺(tái)姿態(tài)變化緩慢(主要由伺服控制誤差、陀螺漂移和導(dǎo)航坐標(biāo)系角運(yùn)動(dòng)等引起)，降低了對(duì)姿態(tài)陣解算的實(shí)時(shí)性要求，但對(duì)解算精度的要求仍然較高。另外，慣導(dǎo)系統(tǒng)初始對(duì)準(zhǔn)通常需要建立系統(tǒng)的線性化誤差模型，而不同的姿態(tài)陣解算方法，對(duì)應(yīng)的線性化誤差模型及其精度是不同的。因此，為了實(shí)現(xiàn)長(zhǎng)航時(shí)高精度的慣性導(dǎo)航系統(tǒng)，設(shè)計(jì)高精度的姿態(tài)陣解算方法至關(guān)重要。

針對(duì)捷聯(lián)式慣導(dǎo)系統(tǒng)的姿態(tài)解算方法，在20世紀(jì)60年代末和70年代初，Savage[1]、Jordan[2]和Bortz[3]先后提出和發(fā)展了雙速捷聯(lián)算法，許多研究人員就該方法進(jìn)行了相關(guān)補(bǔ)償算法的研究。國(guó)內(nèi)學(xué)者針對(duì)姿態(tài)解算方法開展了深入的研究工作[4-15]?？v觀公開的文獻(xiàn)資料，姿態(tài)解算的方法主要有以下四種：1)歐拉角法、改進(jìn)歐拉角法；2)方向余弦法；3)四元數(shù)法；4)等效轉(zhuǎn)動(dòng)矢量法[5]。其中，等效轉(zhuǎn)動(dòng)矢量法是一種可以有效克服載體運(yùn)動(dòng)中不可交換誤差的算法。歐拉角法、方向余弦法、四元數(shù)法是三種常用方法(又分別稱為三參數(shù)法、九參數(shù)法和四參數(shù)法)。相關(guān)學(xué)者對(duì)這三種解算方法進(jìn)行了對(duì)比研究，具體來(lái)說(shuō)，歐拉角法只需要解3個(gè)微分方程，但不可避免地會(huì)出現(xiàn)奇點(diǎn)；方向余弦法避免了奇點(diǎn)問(wèn)題，卻需要解9個(gè)繁雜的方程；四元數(shù)法既避免了奇點(diǎn)問(wèn)題，同時(shí)計(jì)算量也不大，因此，它成為捷聯(lián)慣導(dǎo)系統(tǒng)中姿態(tài)解算最常用的方法。已有研究只考慮了2個(gè)坐標(biāo)系之間一步旋轉(zhuǎn)到位的情況，而從數(shù)學(xué)層面上講，一個(gè)坐標(biāo)系到另一個(gè)坐標(biāo)系可以一步到位旋轉(zhuǎn)，也可以通過(guò)引入中間坐標(biāo)系，將旋轉(zhuǎn)過(guò)程分成若干步完成，此時(shí)，對(duì)應(yīng)的姿態(tài)陣(即方向余弦陣)被分解成若干個(gè)子矩陣的乘積，各個(gè)子矩陣分別解算，已有研究并沒(méi)有解決哪種旋轉(zhuǎn)方法對(duì)應(yīng)的姿態(tài)陣解算精度更高的問(wèn)題。

本文通過(guò)分解姿態(tài)陣以及分別采用Euler角和四元數(shù)描述姿態(tài)參數(shù)，建立解算姿態(tài)陣的四種方法——整體-Euler角法、整體-四元數(shù)法、分解-Euler角法和分解-四元數(shù)法，并分別對(duì)這四種方法進(jìn)行對(duì)比分析。在相同初始條件和滿足各自約束條件的情況下，分別對(duì)這四種方法中的姿態(tài)模型進(jìn)行擾動(dòng)，并研究相應(yīng)線性化誤差模型的精度及其影響因素，再利用數(shù)值仿真法對(duì)四種姿態(tài)解算方法的結(jié)果及對(duì)應(yīng)的線性化誤差模型的精度進(jìn)行比較分析。

1　姿態(tài)陣的四種解算方法

1.1　整體-Euler角法

(1)

(2)

1.2　分解-Euler角法

(3)

(4)

又知：

(5)

1.3　整體-四元數(shù)法

(6)

根據(jù)四元數(shù)與方向余弦陣之間的關(guān)系

(7)

可得

因此，將式(6)展開并整理可得

(8)

給定四元數(shù)初值和每個(gè)時(shí)刻的P系角速度，采用數(shù)值積分算法即可遞推算出各個(gè)時(shí)刻的四元數(shù)向量，進(jìn)而可得姿態(tài)陣為

(9)

1.4　分解-四元數(shù)法

(10)

同理，

(11)

2　擾動(dòng)線性化模型及其精度分析

2.1　整體/分解-四元數(shù)法與整體/分解-Euler角法的等價(jià)性

利用方向余弦陣與四元數(shù)之間的關(guān)系式(7)可得

將四元數(shù)微分方程式(8)代入上式，整理得

證畢。

綜上，整體/分解-四元數(shù)法與整體/分解-Euler角法等價(jià)。

2.2　整體-Euler角法與分解-Euler角法的比較

基于第2.1節(jié)的證明，不失一般性，下面集中精力討論整體-Euler角法與分解-Euler角法對(duì)應(yīng)的兩種擾動(dòng)線性化模型。

2.2.1整體-Euler角法的擾動(dòng)線性化模型

對(duì)方程式(1)兩邊進(jìn)行擾動(dòng)并略去二階項(xiàng)，整理后可得

(12)

為了便于可觀測(cè)性分析和初始對(duì)準(zhǔn)濾波器設(shè)計(jì)，希望擾動(dòng)線性化模型的齊次部分是常系數(shù)的，因此，方程式(12)需要進(jìn)一步簡(jiǎn)化為

(13)

2.2.2分解-Euler角法的擾動(dòng)線性化模型

對(duì)方程式(3)兩邊進(jìn)行擾動(dòng)并略去二階項(xiàng)，整理后可得

(14)

為了便于可觀測(cè)性分析和初始對(duì)準(zhǔn)濾波器設(shè)計(jì)，希望擾動(dòng)線性化模型的齊次部分是常系數(shù)的，因此，方程式(14)需要進(jìn)一步簡(jiǎn)化為

(15)

2.3　分析比較

綜上可見(jiàn)，分解-Euler角法對(duì)應(yīng)的常系數(shù)擾動(dòng)線性化模型(15)比整體-Euler角法對(duì)應(yīng)的模型(13)具有更高的精度。

表1　擾動(dòng)線性化模型包含的齊次變系數(shù)部分的誤差項(xiàng)

3　計(jì)算機(jī)仿真分析

(16)

(17)

(18)

這樣，前述四種方法可互相比較。

3.1　四種方法的解算結(jié)果比較

3.2　四種方法對(duì)應(yīng)的擾動(dòng)線性化模型精度比較

仿真步驟(緯度設(shè)為北緯40°)：

(1)生成整體-Euler角法對(duì)應(yīng)誤差的參考軌跡

(2)生成整體-Euler角法對(duì)應(yīng)擾動(dòng)線性化模型的軌跡

(3)求模型線性化誤差造成的軌跡偏差

4　結(jié)論

姿態(tài)陣解算精度對(duì)長(zhǎng)航時(shí)高精度慣導(dǎo)系統(tǒng)的使用性能有著重要的影響。通過(guò)分解姿態(tài)陣以及分別采用Euler角和四元數(shù)描述姿態(tài)參數(shù)，建立了解算姿態(tài)陣的整體-Euler角法、分解-Euler角法、整體-四元數(shù)法和分解-四元數(shù)法；證明了整體/分解-Euler角法與整體/分解-四元數(shù)法的等價(jià)性；對(duì)相應(yīng)的擾動(dòng)線性化模型的精度進(jìn)行了對(duì)比分析，數(shù)值仿真結(jié)果驗(yàn)證了整體/分解-Euler角法與整體/分解-四元數(shù)法的等價(jià)性，同時(shí)表明：分解法對(duì)應(yīng)的線性化誤差模型的精度優(yōu)于整體法，這對(duì)高精度慣導(dǎo)的初始對(duì)準(zhǔn)和導(dǎo)航算法設(shè)計(jì)具有重要的指導(dǎo)意義。

[1]Savage P G. Strapdown inertial navigation integration algorithm design part 1:Attitude algorithms[J]. Journal of Guidance Control & Dynamics, 1998, 22(2):19-28.

[2]Jordan J W. An accurate strapdown direction cosine algorithm[R]. Report TND-5384, NASA, Washington, 2001.

[3]Bortz J E. A new mathematical formulation for strapdown inertial navigation[J]. IEEE Transactions on Aerospace & Electronic Systems,1971(1):61-66.

[4]趙晗, 汪立新, 趙曦晶,等. 基于間隔子樣旋轉(zhuǎn)矢量的SINS圓錐誤差補(bǔ)償[J]. 壓電與聲光, 2015, 37(1):158-161.

[5]雷鳴, 蔡體菁, 李勇建. 捷聯(lián)慣導(dǎo)系統(tǒng)算法比較研究[J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 2002, 10(1):20-24.

[6]Luo J, Gang Y, Tang L J. Attitude estimation of quad-rotor aircraft based on hybrid filter[J]. Measurement & Control Technology, 2017,36(7):5-8,12.

[7]Chang L, Li J, Chen S. Initial alignment by attitude estimation for strapdown inertial navigation systems[J]. IEEE Transactions on Instrumentation & Measurement, 2015, 64(3):784-794.

[8]Wu Z, Yao M, Ma H, et al. Improving accuracy of the vehicle attitude estimation for low-cost INS/GPS integration aided by the GPS-measured course angle[J]. IEEE Transactions on Intelligent Transportation Systems, 2013, 14(2):553-564.

[9]Valenti R G, Dryanovski I, Xiao J. Keeping a good attitude: A quaternion-based orientation filter for IMUs and MARGs[J]. Sensors, 2015, 15(8):19302-19330.

[10]Tanveer F, Kadri M B, Aziz N, et al. Fuzzy based tuning of a sensor fusion based low cost attitude estimator[C]//6thInternational Conference on Innovative Computing Technology(INTECH). IEEE, 2016:672-676.

[11]Jing X, Cui J, He H, et al. Attitude estimation for UAV using extended Kalman filter[C]//29thChinese Control and Decision Conference(CCDC). IEEE, 2017:3307-3312.

[12]Li X, Chen M, Zhang L. Quaternion-based robust extended Kalman filter for attitude estimation of micro quadrotors using low-cost MEMS[C]//35thChinese Control Conference(CCC). IEEE, 2016:10712-10717.

[13]Tong X, Li Z, Han G, et al. Adaptive EKFbased on HMM recognizer for attitude estimation using MEMS MARG sensors[J]. IEEE Sensors Journal, 2017(99):1.

[14]Zhang X, Yang Z, Zhang T, et al. An improved Kalman filter for attitude determination of multi-rotor UAVs based on low-cost MEMS sensors[C]//IEEE Chinese Guidance, Navigation and Control Conference(CGNCC). IEEE, 2016:407-412.

[15]Ren X, Liu S. Combiningextended Kalman filter with complementary filter for UAV attitude estimation based on MEMS MARG sensors[C]// International Forum on Management, Education and Information Technology Application. 2016:746-752.

[16]高鐘毓. 慣性導(dǎo)航系統(tǒng)技術(shù)[M]. 北京: 清華大學(xué)出版社, 2012.