嚴(yán)恭敏，翁 浚，楊小康，秦永元

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

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

嚴(yán)恭敏，翁 浚，楊小康，秦永元

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

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

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

0 引 言

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

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

(1)

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

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

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

1 多項(xiàng)式角運(yùn)動(dòng)描述

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

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

(2)

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

(3)

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

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

Θ=2WΓ

(4)

式中：

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

(5)

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

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

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

(6)

(7)

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

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

(8)

(9)

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

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

(10)

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

(11)

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

(12)

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

(13)

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

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

(14)

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

(15)

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

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

(16)

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

(17)

(18)

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

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

(19)

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

(20)

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

(21)

3 仿真與分析

3.1 圓錐運(yùn)動(dòng)仿真

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

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

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

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

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

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

3.2 大角度機(jī)動(dòng)仿真

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

(22)

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

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

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

4 結(jié) 論

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

[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é)報(bào), 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] 嚴(yán)恭敏,嚴(yán)衛(wèi)生,徐德民. 經(jīng)典圓錐誤差補(bǔ)償算法中剩余誤差估計(jì)的局限性研究[J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 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òng)態(tài)下捷聯(lián)慣性導(dǎo)航算法誤差分析與優(yōu)化方法研究[D]. 長(zhǎng)沙：國(guó)防科學(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é)報(bào),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

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

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

電話：(029)88431369

E-mail: yangongmin@163.com