基于畢卡迭代的捷聯姿態更新精確數值解法

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

(西北工業大學自動化學院，西安 710072)

基于畢卡迭代的捷聯姿態更新精確數值解法

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

(西北工業大學自動化學院，西安 710072)

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

捷聯姿態更新算法；畢卡級數；多項式擬合；仿真試驗

0 引 言

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

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

(1)

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

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

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

1 多項式角運動描述

在實際捷聯慣導系統中，大多數陀螺采樣直接獲得的是角增量信息，而姿態畢卡更新算法需要用到角速度作為輸入。因此，在角運動為多項式形式假設條件下先給出由角增量信息構造角速度的方法。

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

(2)

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

(3)

式中：簡記tj=jh，當tj>0時表示當前姿態更新周期內的角增量采樣；而當tj≤0時表示利用了前面姿態更新周期的角增量信息。

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

Θ=2WΓ

(4)

式中：

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

(5)

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

2 姿態四元數微分方程的畢卡級數解法

用四元數表示的姿態微分方程為[13]

(6)

(7)

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

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

(8)

(9)

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

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

(10)

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

(11)

根據如下兩四元數P和Q之間的乘法運算規則

(12)

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

(13)

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

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

(14)

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

(15)

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

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

(16)

進而式(5)中的W和Γ可轉化為

(17)

(18)

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

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

(19)

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

(20)

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

(21)

3 仿真與分析

3.1 圓錐運動仿真

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

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

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

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

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

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

3.2 大角度機動仿真

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

(22)

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

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

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

4 結 論

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

[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] 王立冬，孟亞峰，高慶. 基于角增量和角速率的旋轉矢量算法的等效性[J]. 宇航學報, 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] 嚴恭敏,嚴衛生,徐德民. 經典圓錐誤差補償算法中剩余誤差估計的局限性研究[J]. 中國慣性技術學報, 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] 宋敏. 高動態下捷聯慣性導航算法誤差分析與優化方法研究[D]. 長沙：國防科學技術大學,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] 朱常興,馮焱穎,周兆英,等. 原子慣性技術在航天航空領域的應用[J]. 宇航學報,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] 秦永元. 慣性導航(第二版)[M]. 北京:科學出版社，2014: 253-255.

[14] 袁信,鄭鍔. 捷聯式慣性導航原理[M]. 南京：航空專業教材編審組,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

航空科學基金(20165853041)

V249.3

A

1000-1328(2017)12- 1307- 07

10.3873/j.issn.1000- 1328.2017.12.007

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

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

電話：(029)88431369

E-mail: yangongmin@163.com