嚴恭敏，翁 浚，楊小康，秦永元

(西北工業(yè)大學(xué)自動化學(xué)院，西安 710072)

基于畢卡迭代的捷聯(lián)姿態(tài)更新精確數(shù)值解法

嚴恭敏，翁 浚，楊小康，秦永元

(西北工業(yè)大學(xué)自動化學(xué)院，西安 710072)

針對捷聯(lián)慣導(dǎo)在大角度機動等場合下姿態(tài)精確求解問題，論文根據(jù)四元數(shù)微分方程的畢卡級數(shù)解法，提出一種新的求解姿態(tài)更新的數(shù)值算法。新算法利用角增量建立角速度多項式擬合，再根據(jù)多項式四元數(shù)的乘法特點將其變換為多項式的卷積運算，求得更新四元數(shù)的冪級數(shù)解。新算法在推導(dǎo)過程中未做任何近似和假設(shè)，不存在原理性誤差。在大幅值圓錐運動和大角度機動環(huán)境下，新算法與傳統(tǒng)算法進行了對比仿真試驗，校驗了新算法具有明顯的精度優(yōu)勢。

捷聯(lián)姿態(tài)更新算法；畢卡級數(shù)；多項式擬合；仿真試驗

0 引 言

捷聯(lián)慣導(dǎo)算法的關(guān)鍵在于姿態(tài)更新算法，也即剛體定軸轉(zhuǎn)動問題，多年來學(xué)者們對其進行了廣泛而深入的研究[1]。描述姿態(tài)變換的數(shù)學(xué)工具有歐拉角、方向余弦陣、羅德里格參數(shù)、四元數(shù)和等效旋轉(zhuǎn)矢量等,求解姿態(tài)數(shù)值更新的方法有一階歐拉法、四階龍格-庫塔法、畢卡級數(shù)法和等效旋轉(zhuǎn)矢量算法等。

目前，最為流行的姿態(tài)更新求解方法是：先使用陀螺角增量多子樣采樣計算等效旋轉(zhuǎn)矢量，補償轉(zhuǎn)動不可交換誤差，再使用等效旋轉(zhuǎn)矢量計算姿態(tài)更新四元數(shù)，四元數(shù)表示簡潔而且無奇異。等效旋轉(zhuǎn)矢量多子樣算法的理論基礎(chǔ)是如下給出的Bortz方程[2]：

(1)

式中：φ(t)，ω(t)和Δθ(t)分別表示等效旋轉(zhuǎn)矢量、角速度和角增量,上標(biāo)×表示反對稱陣。

傳統(tǒng)基于泰勒級數(shù)展開的多子樣算法推導(dǎo)，忽略了Bortz方程右端三階項的影響，并將二階項中的等效旋轉(zhuǎn)矢量近似為角增量[3-5]。傳統(tǒng)基于圓錐運動環(huán)境下的優(yōu)化多子樣算法，將錐角假設(shè)為小角度，理論上其在一個姿態(tài)更新周期內(nèi)子樣數(shù)越多精度越高[6-7]。但是，上述推導(dǎo)過程中所做的前提近似或假設(shè)，使得傳統(tǒng)的算法精度往往達不到宣稱的理想效果[8]，特別是在大角度機動或大錐角圓錐運動環(huán)境下，有時采用高子樣算法的精度反而不如低子樣的精度。文獻[9]和[10]分別提出了顯式頻率整形和擴展圓錐誤差補償算法，對傳統(tǒng)算法做了改進，在大機動情形下提高了誤差補償精度，然而它們也是僅考慮了圓錐誤差二階項的影響，大機動條件下依然無法避免原理性誤差。作為傳統(tǒng)方法的改進，文獻[11]考慮了Bortz方程中三階項的影響，提出了更高精度的誤差補償算法，但是其推導(dǎo)過程比較繁瑣，仍然難以避免在大機動條件下產(chǎn)生誤差。未來隨著高精度5 m/h量級冷原子陀螺的使用，人們對導(dǎo)航計算精度要求必將越來越高，且隨著高超聲速飛行器和旋轉(zhuǎn)彈等大機動領(lǐng)域的應(yīng)用拓展，有必要研究更高精度的導(dǎo)航算法[10,12]。

傳統(tǒng)的多子樣算法推導(dǎo)的目標(biāo)是獲得一組確定的不可交換誤差補償系數(shù)，即角增量各子樣之間叉乘的系數(shù)，子樣數(shù)越多，推導(dǎo)過程就越復(fù)雜，為了公式推導(dǎo)可以順利進行，需做必要的近似簡化或假設(shè)，使得推導(dǎo)結(jié)果是近似或隱含適用條件的。本文摒棄了傳統(tǒng)的求取確定性補償系數(shù)的思路，在角速度函數(shù)為關(guān)于時間的多項式條件下(任何連續(xù)函數(shù)都能用多項式以任意給定的精度逼近)，根據(jù)四元數(shù)微分方程的畢卡級數(shù)解直接推導(dǎo)姿態(tài)更新的數(shù)值算法，其結(jié)果是以時間多項式為元素的變換四元數(shù)，在推導(dǎo)過程中無需做任何近似，精度僅僅取決于計算機的數(shù)值計算精度。新算法易于軟件編程實現(xiàn)，通過對比仿真試驗校驗了新算法的高精度性能。

1 多項式角運動描述

在實際捷聯(lián)慣導(dǎo)系統(tǒng)中，大多數(shù)陀螺采樣直接獲得的是角增量信息，而姿態(tài)畢卡更新算法需要用到角速度作為輸入。因此，在角運動為多項式形式假設(shè)條件下先給出由角增量信息構(gòu)造角速度的方法。

假設(shè)角速度ω(t)是關(guān)于時間t的N-1次多項式，即

(2)

假設(shè)陀螺采樣間隔為h，在時間段(-ph,nh]內(nèi)進行了N次角增量采樣(p≥0,n>0且p+n=N)，分別記為Δθj(j=-p+1,-p+2,…,n)，對式(2)積分可得

(3)

式中：簡記tj=jh，當(dāng)tj>0時表示當(dāng)前姿態(tài)更新周期內(nèi)的角增量采樣；而當(dāng)tj≤0時表示利用了前面姿態(tài)更新周期的角增量信息。

根據(jù)式(3)，將相繼N次角增量合并在一起寫成矩陣形式，如下所示

Θ=2WΓ

(4)

式中：

由式(4)容易求得以角增量表示的多項式系數(shù)矩陣

(5)

由此可見，根據(jù)相繼的N次角增量采樣，通過式(5)和式(2)總可以構(gòu)造一個(N-1)次的多項式角速度擬合。

2 姿態(tài)四元數(shù)微分方程的畢卡級數(shù)解法

用四元數(shù)表示的姿態(tài)微分方程為[13]

(6)

(7)

若四元數(shù)初值Q(0)和角速度函數(shù)W(t)均已知，類似于矩陣微分方程的畢卡級數(shù)解[14]，不難求得四元數(shù)微分方程(7)的畢卡級數(shù)解為

Q(t)=Q(0)°q(t,0)

(8)

(9)

式中：q(t,0)為從時間0到t的姿態(tài)變化四元數(shù)，完全由角速度決定。一般情況下無法再對式(9)作進一步處理;但是，如果假設(shè)角速度W(t)為關(guān)于時間t的多項式函數(shù)，則可得到q(t,0)的多項式積分解，具體分析如下所述。

首先，計算式(9)右端的單重積分項，將三維矢量表示為零標(biāo)量四元數(shù)，得

(10)

其次，將式(10)代入式(9)右端的雙重積分項，可得

(11)

根據(jù)如下兩四元數(shù)P和Q之間的乘法運算規(guī)則

(12)

并且注意到兩個以多項式為元素的四元數(shù)之乘積仍然是多項式四元數(shù)，式(11)轉(zhuǎn)化為

(13)

式中：運算符“*”表示兩個多項式系數(shù)行向量之間的卷積運算。

同理，將式(13)代入式(9)右端的三重積分項，得

(14)

至此，獲得式(9)的冪級數(shù)解為

(15)

式(15)是關(guān)于時間t的無窮階冪級數(shù)，但在實際應(yīng)用中，總是選取式(15)的前低階有限項作為姿態(tài)更新的數(shù)值解；容易看出，當(dāng)保留前m階時，數(shù)值截斷誤差為O(tm+1)。為了降低計算量，當(dāng)選前m階時，在所有保留的積分項中均可刪去次數(shù)高于m的多項式系數(shù)，不影響截斷誤差的階次。一般在姿態(tài)更新周期nh小于0.1s并且角速度多項式系數(shù)在數(shù)值上不超過10量級的情況下，可選擇保留畢卡級數(shù)的前10～15階作為精確的數(shù)值解。

特別地，若將姿態(tài)更新間隔nh作歸一化處理，即令nh=1，則由式(15)可求得

(16)

進而式(5)中的W和?？赊D(zhuǎn)化為

(17)

(18)

例如，當(dāng)N=6，m=10時，乘法次數(shù)為3600；而當(dāng)N=4，m=5時，乘法次數(shù)為600，通常也能取得比較滿意的數(shù)值精度。

與本文算法的計算量相比，若采用如下傳統(tǒng)的擴展形式不可交換誤差補償算法[6,10]

(19)

式中：δφ(T)和kij分別為圓錐誤差積分和不可交換誤差補償系數(shù)。式(19)中含有N(N-1)/2次矢量叉乘運算和系數(shù)乘法運算，因此傳統(tǒng)算法的乘法次數(shù)為

(20)

根據(jù)式(18)和式(20)，可獲得本文算法與傳統(tǒng)算法的計算量比率為

(21)

3 仿真與分析

3.1 圓錐運動仿真

圓錐運動仿真參數(shù)設(shè)置為：圓錐頻率f=1 Hz，半錐角變化范圍α=0.05″～90°，角增量采樣間隔h=10 ms 。經(jīng)過仿真，在圓錐軸上的姿態(tài)漂移誤差ε如圖1所示，實線為傳統(tǒng)圓錐誤差補償算法的姿態(tài)漂移誤差，點劃線為本文所提新算法的誤差，各圖例依次對應(yīng)子樣數(shù)N=2～6(這里未采用前一姿態(tài)更新周期的角增量信息，即參數(shù)p=0)。從圖1可以得出以下幾點結(jié)論：

1)當(dāng)子樣為2時，兩種算法誤差曲線幾乎重合，即兩者精度相當(dāng)。

2)新算法隨子樣數(shù)增加精度不斷提高;2，3子樣(或4，5子樣)之間的精度相對來說比較接近。

3)對于傳統(tǒng)算法，在半錐角很小時子樣數(shù)越高精度才會越高，比如當(dāng)半錐角小于1″時6子樣的精度才高于5子樣；而當(dāng)半錐角較大時，傳統(tǒng)算法的高子樣算法精度反而不如低子樣算法，特別在半錐角α= 90°時6子樣算法的漂移誤差高達20°/h。

4)在大半錐角情況下，新算法精度明顯優(yōu)于傳統(tǒng)算法；而當(dāng)半錐角較小時，新算法也具有足夠高的精度，對于實際應(yīng)用而言誤差可忽略不計，總體上看，新算法受半錐角影響小，具有更強的實用性。

此外，仿真還顯示，傳統(tǒng)算法在非圓錐軸上的誤差波動較大，而新算法誤差均很小。不妨以半錐角α= 90°環(huán)境下的6子樣算法為例，圖2給出了兩種算法在一個圓錐角運動周期內(nèi)的非圓錐軸上的角度更新誤差(δφx和δφy)比較。從圖2可以看出，傳統(tǒng)算法的誤差最大達到了320″，而新算法的誤差始終很小，僅為10-6″量級。這說明，如果出現(xiàn)非整周期的圓錐運動，傳統(tǒng)算法可能會引起較大的姿態(tài)解算誤差。

3.2 大角度機動仿真

實際上，第3.1節(jié)給出的短時低頻大幅值圓錐運動也可以視為一種大角度機動類型。下面采用文獻[9-10]中以多項式表示的2 s 大角度機動環(huán)境，重寫多項式系數(shù)如下所示：

(22)

仿真時除采用傳統(tǒng)圓錐誤差補償算法和本文新算法外，還增加了文獻[10]提出的擴展圓錐補償算法，但由于擴展算法的推導(dǎo)非常繁瑣，尚未查找到6子樣系數(shù)的公開文獻，因而擴展算法最多只仿真到5子樣。三種算法的角增量采樣間隔均為h=10 ms，仿真結(jié)果的姿態(tài)漂移誤差(δφx、δφy和δφz)如圖3所示。由圖3可得以下兩點結(jié)論：

1)擴展算法3～5子樣的精度明顯優(yōu)于傳統(tǒng)圓錐算法，但是兩者的算法精度都隨著子樣數(shù)的增加反而下降，傳統(tǒng)算法N=6子樣時x軸姿態(tài)漂移誤差將近20″，見圖(a 5)，擴展算法N=5子樣時z軸漂移也超過了0.5″，見圖(b 4)。

2)新算法的精度均高于傳統(tǒng)算法和擴展算法，且新算法精度隨子樣數(shù)增加不斷提高。事實上，式(22)為角速度的4次多項式描述，新算法的高于4子樣的解式(15)可以認為是姿態(tài)更新的精確數(shù)值解或冪級數(shù)解析解，其精度僅受限于計算機的數(shù)值計算精度，正如新算法中圖(c 4)和圖(c 5)所示，它們的誤差都非常小，幾乎可忽略不計。

4 結(jié) 論

傳統(tǒng)圓錐誤差補償算法在純圓錐運動環(huán)境下且半錐角比較小時是非常有效的，然而對于大錐角情況或者應(yīng)用于大角度機動環(huán)境，都會產(chǎn)生較大的姿態(tài)漂移誤差。論文根據(jù)四元數(shù)微分方程的畢卡級數(shù)解，提出了一種新的直接求解姿態(tài)四元數(shù)更新的數(shù)值算法，在算法推導(dǎo)過程中不做任何近似處理，算法精度高，環(huán)境適應(yīng)性好。通過與傳統(tǒng)的基于等效旋轉(zhuǎn)矢量的多子樣算法對比仿真，校驗了新算法在圓錐運動和大角度機動等場合均具有明顯的精度優(yōu)勢，因而具有更好的應(yīng)用價值。雖然新算法的計算量比傳統(tǒng)算法稍大些，但是相對于當(dāng)代高性能導(dǎo)航計算機的處理能力而言，還是容易滿足實時計算要求的。在事后分析處理方面，新算法還可以作為研究其它姿態(tài)更新算法的精度比較參考基準(zhǔn)。由于缺乏高精度的慣導(dǎo)系統(tǒng)、高動態(tài)環(huán)境模擬設(shè)備以及實時高精度的姿態(tài)參考基準(zhǔn)，目前論文研究主要集中在理論分析和仿真對比層面上，希望所提出的理論和算法今后能在實際系統(tǒng)中得到進一步驗證。

[1] Cheng H, Gupta K C. A historical note on finite rotations[J].Journal of Applied Mechanics, 1989, 56: 139-145.

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

[3] Miller R. A new strapdown attitude algorithm[J]. Journal of Guidance, Control, and Dynamics, 1983, 6(4): 287-291.

[4] Lee J G, Yoon Y J, Mark J G, et al. Extension of strapdown attitude algorithm for high-frequency base motion[J]. Journal of Guidance, Control, and Dynamics, 1990, 13(4): 738-743.

[5] 王立冬，孟亞峰，高慶. 基于角增量和角速率的旋轉(zhuǎn)矢量算法的等效性[J]. 宇航學(xué)報, 2014, 35(3): 340-344.[Wang Li-dong, Meng Ya-feng, Gao Qing. Equivalence analysis of rotation vector algorithm based on angle increment and angular velocity[J]. Journal of Astronautics, 2014, 35(3): 340-344.]

[6] Ignagni M B. Efficient class of optimized coning compensation algorithms[J].Journal of Guidance, Control, and Dynamics, 1996, 19(2): 424-429.

[7] Park C G, Kim K J, Lee J G, et al. Formalized approach to obtaining optimal coefficients for coning algorithms[J]. Journal of Guidance, Control, and Dynamics, 1999, 22(1): 165-168.

[8] 嚴恭敏,嚴衛(wèi)生,徐德民. 經(jīng)典圓錐誤差補償算法中剩余誤差估計的局限性研究[J]. 中國慣性技術(shù)學(xué)報, 2008, 16(4): 379-385.[Yan Gong-min, Yan Wei-sheng, Xu De-min. Limitations of error estimation for classic coning compensation algorithm[J]. Journal of Chinese Inertial Technology, 2008, 16(4): 379-385.]

[9] Savage P G. Coning algorithm design by explicit frequency shaping[J].Journal of Guidance, Control, and Dynamics, 2010,33(4):1123-1132.

[10] 宋敏. 高動態(tài)下捷聯(lián)慣性導(dǎo)航算法誤差分析與優(yōu)化方法研究[D]. 長沙：國防科學(xué)技術(shù)大學(xué),2012.[Song Min. Research on error analysis and optimization methods for strapdown inertial navigation algorithm under highly dynamic environment [D]. Changsha: National University of Defense Technology, 2012.]

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

[12] 朱常興,馮焱穎,周兆英,等. 原子慣性技術(shù)在航天航空領(lǐng)域的應(yīng)用[J]. 宇航學(xué)報,2009,30(1):18-24. [Zhu Chang-xing, Feng Yan-ying, Zhou Zhao-ying, et al. Applications of atom inertial technology in aerospace engineering [J]. Journal of Astronautics, 2009,30(1):18-24.]

[13] 秦永元. 慣性導(dǎo)航(第二版)[M]. 北京:科學(xué)出版社，2014: 253-255.

[14] 袁信,鄭鍔. 捷聯(lián)式慣性導(dǎo)航原理[M]. 南京：航空專業(yè)教材編審組,1985: 54-55.

AnAccurateNumericalSolutionforStrapdownAttitudeAlgorithmBasedonPicardIteration

YAN Gong-min, WENG Jun, YANG Xiao-kang, QIN Yong-yuan

(School of Automation, Northwestern Polytechnical University, Xi’an 710072, China)

For strapdown attitude updating algorithm, in order to reach a high accuracy under a high attitude maneuver, a new numerical attitude algorithm is proposed based on the Picard series solution for the attitude quaternion differential equation. In this new algorithm, the angular velocity polynomial fit is obtained for the gyro angular increment, and the product of the polynomial quaternion is converted into the convolution operation of the angular velocity polynomial coefficients, then the Picard series solution is well settled. There exists no approximation or hypothesis in this deduction, which means the new algorithm is analytically accurate. Finally, under the large amplitude cone motion and high maneuver environment, some comparison tests both using the traditional algorithm and the new presented algorithm are carried out, and the results show the significant accuracy improvement in the new algorithm.

Strapdown attitude algorithm; Picard series; Polynomial fit; Simulation test

2017- 02- 23;

2017- 09- 21

航空科學(xué)基金(20165853041)

V249.3

A

1000-1328(2017)12- 1307- 07

10.3873/j.issn.1000- 1328.2017.12.007

嚴恭敏(1977-)，男，博士，副教授，主要從事慣性導(dǎo)航與信息融合理論方面的研究。

通信地址：陜西省西安市西北工業(yè)大學(xué)自動化學(xué)院183號信箱(710072)

電話：(029)88431369

E-mail: yangongmin@163.com