嚴(yán)恭敏，李思錦，郭正東

（1.西北工業(yè)大學(xué) 自動(dòng)化學(xué)院，西安 710072；2.海軍潛艇學(xué)院，青島 266199）

捷聯(lián)慣導(dǎo)算法的關(guān)鍵在于姿態(tài)更新算法，其主流方法是先使用陀螺角增量多子樣采樣計(jì)算等效旋轉(zhuǎn)矢量，補(bǔ)償轉(zhuǎn)動(dòng)不可交換誤差，再使用等效旋轉(zhuǎn)矢量計(jì)算姿態(tài)更新四元數(shù)。等效旋轉(zhuǎn)矢量多子樣算法的理論基礎(chǔ)是Bortz 方程[1]，傳統(tǒng)基于泰勒級(jí)數(shù)展開或基于圓錐運(yùn)動(dòng)環(huán)境下優(yōu)化的多子樣算法推導(dǎo)，都忽略了Bortz方程中三階以上項(xiàng)的影響，雖然理論上在一個(gè)姿態(tài)更新周期內(nèi)子樣數(shù)越多精度越高，但是，實(shí)際算法精度往往達(dá)不到宣稱的理想效果，特別是在大角度機(jī)動(dòng)或大錐角圓錐運(yùn)動(dòng)環(huán)境下，有時(shí)采用高子樣算法的精度反而不如低子樣算法的精度。

目前，提高姿態(tài)解算精度的研究方向主要有兩個(gè)。其一是數(shù)值迭代算法，姿態(tài)的數(shù)學(xué)表示方法有姿態(tài)陣、等效旋轉(zhuǎn)矢量、四元數(shù)和羅德里格參數(shù)等，只要能建立起這些表示方法與角速度輸入之間的運(yùn)動(dòng)學(xué)關(guān)系，均可以采用積分或微分?jǐn)?shù)值迭代計(jì)算的方法進(jìn)行求解，這類方法的優(yōu)點(diǎn)是精度高且實(shí)現(xiàn)簡(jiǎn)單，稍顯不足之處是計(jì)算量略大[2-6]。其二是等效旋轉(zhuǎn)矢量的高階誤差補(bǔ)償算法，它考慮了Bortz 方程中高階項(xiàng)的影響，將高階項(xiàng)表示為多子樣的多重叉積之和的形式，先推導(dǎo)求解出高階誤差補(bǔ)償系數(shù)再進(jìn)行補(bǔ)償，其優(yōu)點(diǎn)是在進(jìn)行姿態(tài)更新應(yīng)用時(shí)計(jì)算量相對(duì)較小[7]。

等效旋轉(zhuǎn)矢量高階誤差補(bǔ)償系數(shù)的求解是比較煩瑣的，但幸運(yùn)的是可以將Bortz 方程中的低階項(xiàng)分解為多個(gè)部分，各部分恰好能夠表示為不同重?cái)?shù)的叉積形式，且它們之間互不相關(guān)，只要分別進(jìn)行求解即可[7,8]。文獻(xiàn)[9]給出了單重叉積系數(shù)即傳統(tǒng)多子樣系數(shù)的隨機(jī)仿真求解算法，通用性強(qiáng)且計(jì)算機(jī)編程實(shí)現(xiàn)簡(jiǎn)單；文獻(xiàn)[10]借鑒同樣的思路，給出了雙重叉積和三重叉積系數(shù)的求解算法，但由于多重叉積之間存在相關(guān)性問題，去除相關(guān)過程的推導(dǎo)依然有些煩瑣。本文采用多重叉積矩陣秩的數(shù)值判斷方法直接消除相關(guān)性，更加簡(jiǎn)單，該方法可直接推廣至五子樣和六子樣等更高子樣算法。最后，在圓錐運(yùn)動(dòng)和大機(jī)動(dòng)情況下進(jìn)行了算法的對(duì)比仿真驗(yàn)證，并給出了一些實(shí)際使用建議。

1 高階誤差補(bǔ)償算法原理

等效旋轉(zhuǎn)矢量微分方程(Bortz 方程)為[1]

其中，ω為動(dòng)坐標(biāo)系相對(duì)于參考坐標(biāo)系的轉(zhuǎn)動(dòng)角速度輸入，為等效旋轉(zhuǎn)矢量φ的模值。將式(1)中的余切函數(shù)作泰勒級(jí)數(shù)展開，經(jīng)整理后可得

將等效旋轉(zhuǎn)矢量作如下“和分解”[7]

再將式(3)代入式(2)，可得

令式(4)左右兩邊的項(xiàng)一一對(duì)應(yīng)相等，可得如下五個(gè)子式

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

式中，wj(j=N- 1,N- 2… 0)均為三維列向量，W為3×N維的系數(shù)矩陣。假設(shè)在一個(gè)姿態(tài)更新時(shí)間段（0,T］內(nèi)進(jìn)行了N次等間隔陀螺角增量采樣，記為Δθj+1，顯然有

式中，h=T/N為陀螺采樣間隔。根據(jù)文獻(xiàn)[9]，有角速度多項(xiàng)式系數(shù)與角增量之間的關(guān)系

式中，Γ是與N和T有關(guān)的已知矩陣。由式(8)可知，角速度多項(xiàng)式各項(xiàng)系數(shù)wj均是所有角增量采樣Δθj+1的線性組合，因此式(6)中角速度ω總可以表示為角增量 Δθj+1的線性組合多項(xiàng)式形式，即

式中，γij為逆陣Γ-1的第i行j列元素，是已知量。式(5a)顯示，φ1是角速度ω的積分即角增量，因而φ1也可以表示為Δθj+1的線性組合多項(xiàng)式形式，根據(jù)式(9)可得

若角速度ω關(guān)于時(shí)間t的最低非零冪次為0、最高非零冪次為N- 1，則經(jīng)積分后，角增量φ1的最低非零冪次為1、最高非零冪次為N；考慮到ω和φ1的最低非零冪次項(xiàng)和最高非零冪次項(xiàng)是線性相關(guān)的，則在一般非定軸轉(zhuǎn)動(dòng)情形下叉積φ1×ω的最低非零冪次為2、最高非零冪次為2N- 2，所以式(5b)經(jīng)積分后φ2的最低非零冪次為3、最高非零冪次為2N- 1；以此類推，可分析得φ3、φ4和φ5等分解項(xiàng)的最低和最高非零冪次，總結(jié)見表1。其中，在式(5e)的右端最后一項(xiàng)中含等效旋轉(zhuǎn)矢量模方因子φ2，顯然φ2=φTφ的最低非零冪次為2、最高冪次為無窮，所以φ5的最高非零冪次是無窮的。

表1 等效旋轉(zhuǎn)矢量分解的最低和最高非零冪次Tab1.The lowest and highest non-zero power for the decomposition of rotation vector

根據(jù)三維向量的叉乘性質(zhì)，將式(10)叉乘式(9)，其結(jié)果必定可以表示成如下N(N- 1)/2項(xiàng)單重叉積的線性組合形式

式(11)中，ki′j是與γij和時(shí)間t有關(guān)的系數(shù)，具體表達(dá)式比較復(fù)雜。將式(11)對(duì)時(shí)間積分不會(huì)改變其整體表示形式，僅會(huì)改變系數(shù)ki′j，因此，由式(5b)可知φ2也總可以表示為角增量采樣單重叉積 Δθi×Δθj的線性組合形式，記為

依此類推，φ3可以表示為N2(N- 1)/2項(xiàng)雙重叉積 Δθi×(Δθj×Δθk)的線性組合；而φ4也可以表示為N3(N- 1)/2項(xiàng)三重叉積的線性組合（注意:在文獻(xiàn)[7,10]中將φ4分為φ4-1和φ4-2兩部分是沒必要的，因?yàn)榭傆笑う萰×［Δθi×(Δθk×Δθl)］成立，即φ4-2必定可以表示成φ4-1的線性組合）。然而，由于式(5e)中含因子φ2，理論上φ5不能完全表示為四重叉積形式，所以采用多子樣多重叉積法最多只能精確補(bǔ)償至三重叉積項(xiàng)。當(dāng)然，若作模方近似φ2=(φ1+φ2+ …)T(φ1+φ2+ …)≈φ1Tφ1，則φ5的最高冪次為5N- 1，就有可能采用非叉乘的方式對(duì)其進(jìn)行近似補(bǔ)償，這不在論文的討論范圍內(nèi)。

式(5)和表1表明，傳統(tǒng)的等效旋轉(zhuǎn)矢量誤差補(bǔ)償算法只補(bǔ)償至φ2項(xiàng)，即忽略了φ3及其之后所有項(xiàng)，誤差階為O(t5)；φ3和φ4具有相同的誤差階O(t5)，若僅補(bǔ)償φ3則誤差依然為O(t5)，意義不大；若同時(shí)補(bǔ)償φ3和φ4而忽略φ5，則誤差階為O(t7)。目前，所謂的高階誤差補(bǔ)償算法，即為對(duì)φ2、φ3和φ4同時(shí)進(jìn)行了誤差補(bǔ)償?shù)牡刃D(zhuǎn)矢量多子樣算法。

2 高階誤差補(bǔ)償系數(shù)求解

2.1 求解步驟

根據(jù)式(9)和式(10)的表達(dá)方式直接求解式(12)中的單重叉積系數(shù)kij，甚至進(jìn)一步求解雙重或三重叉積系數(shù)，十分的煩瑣。因此，這里通過隨機(jī)數(shù)值仿真的方法求解這些系數(shù)，主要以求解雙重叉積系數(shù)kijk為例，單重叉積系數(shù)kij和三重叉積系數(shù)kijkl的求解過程類似。

求解等效旋轉(zhuǎn)矢量誤差補(bǔ)償雙重叉積系數(shù)kijk的具體步驟如下:

（1）給定子樣數(shù)N，不妨將等效旋轉(zhuǎn)矢量更新周期作歸一化處理，即令T=1；

（2）隨機(jī)生成一組角速度多項(xiàng)式系數(shù)［wN-1wN-2…w0］，根據(jù)式(5a)～式(5c)和卷積算法[4]依次計(jì)算φ1、φ2和φ3的多項(xiàng)式系數(shù)，再計(jì)算多項(xiàng)式在T=1時(shí)刻取值φ3(1)，此即三階不可交換誤差的期望值。

（3）根據(jù)步驟（2）中的角速度多項(xiàng)式系數(shù)，按式(7)生成一組角增量數(shù)據(jù)［Δθ1Δθ2… ΔθN］，它們滿足量測(cè)方程kijk為n=N2(N-1)/2個(gè)候選的待定系數(shù)；

（4）重復(fù)步驟（2）和（3）共記m次，一般需滿足3m≥n，將所有量測(cè)方程合并在一起寫成方程組形式

（5）根據(jù)向量的雙重叉積性質(zhì)（雅可比恒等式）V1×(V2×V3)+V2×(V3×V1)+V3×(V1×V2)=0可知，由角增量構(gòu)造的矩陣不是列滿秩的，可試著刪去某一列并判斷刪除前后的秩是否一致，若相等則將該列刪去并刪去與該列對(duì)應(yīng)的系數(shù)kijk，消除相關(guān)性，用此方法直到將角增量矩陣化為列滿秩的，當(dāng)然，也可通過調(diào)用Matlab 軟件的“rref”函數(shù)獲得角增量矩陣的極大無關(guān)列向量組直接實(shí)現(xiàn)列滿秩。

（6）在角增量矩陣列滿秩情況下利用最小二乘法求解方程組，得到n′個(gè)互不相關(guān)的雙重叉積補(bǔ)償系數(shù)kijk，可用Matlab 的“format rat”命令將浮點(diǎn)小數(shù)解轉(zhuǎn)化為分?jǐn)?shù)解輸出。

2.2 二子樣誤差補(bǔ)償系數(shù)

經(jīng)過仿真計(jì)算，獲得二子樣算法的所有誤差補(bǔ)償系數(shù)如表2所列。

表2 二子樣算法誤差補(bǔ)償系數(shù)Tab.2 Error compensation coefficients of 2 sub-sample algorithm

二子樣算法的誤差補(bǔ)償系數(shù)數(shù)目不多，可根據(jù)表2直接寫出完整的等效旋轉(zhuǎn)矢量高階誤差補(bǔ)償算法公式，為

式中，求和符號(hào)∑的右下標(biāo)ij、ijk或ijkl表示遍歷表2中的所有取值。后文三子樣和四子樣算法的誤差補(bǔ)償系數(shù)使用方法同式(13)，不再詳細(xì)給出公式。

2.3 三子樣誤差補(bǔ)償系數(shù)

三子樣算法的誤差補(bǔ)償系數(shù)數(shù)目較多，參見表3，其中包含單重叉積系數(shù)3 個(gè)、雙重叉積系數(shù)8 個(gè)、三重叉積系數(shù)18 個(gè)。

2.4 四子樣誤差補(bǔ)償系數(shù)

四子樣算法的誤差補(bǔ)償系數(shù)如表4和表5所列，共包含單重叉積系數(shù)6 個(gè)、雙重叉積系數(shù)20 個(gè)、三重叉積系數(shù)60 個(gè)。

結(jié)果顯示，表3和表4中的單重叉積誤差補(bǔ)償系數(shù)kij與文獻(xiàn)[9]一致，它們都是在多項(xiàng)式角運(yùn)動(dòng)條件下獲得的。文獻(xiàn)[11]綜合考慮了多項(xiàng)式角運(yùn)動(dòng)和圓錐運(yùn)動(dòng)的影響，推導(dǎo)并給出了所謂的擴(kuò)展圓錐算法（見表3和表4第2 列括號(hào)內(nèi)的數(shù)據(jù)），使其更加適應(yīng)于圓錐運(yùn)動(dòng)環(huán)境。

表3 三子樣算法誤差補(bǔ)償系數(shù)Tab.3 Error compensation coefficients of 3 sub-sample algorithm

表4 四子樣算法誤差補(bǔ)償系數(shù)（單重和雙重叉積）Tab.4 Error compensation coefficients of 4 sub-sample algorithm (for single ＆ double cross product)

表5 四子樣算法誤差補(bǔ)償系數(shù)（三重叉積）Tab.5 Error compensation coefficients of 4 sub-sample algorithm (for triple cross product)

表4中的雙重叉積和表5中的三重叉積誤差補(bǔ)償系數(shù)與文獻(xiàn)[7,10]的結(jié)果不完全一致，但系數(shù)的數(shù)目是對(duì)應(yīng)相等的。究其原因主要在于本文采用2.1 節(jié)步驟（5）的方法消除角增量叉積之間的相關(guān)性，兩種方法消去的系數(shù)不一樣。本文方法無需細(xì)致的公式推導(dǎo)整理，直接利用計(jì)算機(jī)從數(shù)值上進(jìn)行化簡(jiǎn)更加便捷。需要指出的是，在求解雙重叉積和三重叉積等高階誤差補(bǔ)償系數(shù)時(shí)，所有文獻(xiàn)均未考慮圓錐運(yùn)動(dòng)的影響，因而，即使采用了單重叉積擴(kuò)展圓錐算法，高階誤差補(bǔ)償算法依然不能適應(yīng)劇烈的高頻大錐角圓錐運(yùn)動(dòng)環(huán)境。當(dāng)然，要同時(shí)考慮多項(xiàng)式角運(yùn)動(dòng)和圓錐運(yùn)動(dòng)的影響，進(jìn)一步推導(dǎo)高階擴(kuò)展算法是十分困難的。

利用本文方法還可以方便地求解五子樣和六子樣等更高子樣數(shù)的算法，但誤差補(bǔ)償系數(shù)繁多且實(shí)際應(yīng)用意義也不大，不再詳細(xì)列出。

3 仿真驗(yàn)證

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

圓錐運(yùn)動(dòng)仿真參數(shù)設(shè)置為:圓錐頻率f=2 Hz，半錐角變化范圍α=0.01 °～90 °，角增量采樣間隔h=10 ms。經(jīng)過仿真，在圓錐軸上的姿態(tài)漂移誤差ε如圖1所示，圖中實(shí)線為四元數(shù)迭代算法（Qpicard[2]）的姿態(tài)漂移誤差，短虛線為擴(kuò)展算法（Uncompressed[11]）的誤差，點(diǎn)劃線為“擴(kuò)展+高階誤差補(bǔ)償”算法（Uncomp+Highorder[7,10]）的誤差，長(zhǎng)虛線為本文給出的“非擴(kuò)展+高階誤差補(bǔ)償”算法（Comp+Highorder）的誤差，四種算法均采用了四子樣進(jìn)行解算。從圖1中可以看出:（1）當(dāng)錐角較小時(shí)Uncompressed 算法是比較有效的，在錐角中等大小情況下須與Highorder 算法結(jié)合才具備更高的精度（提高約兩個(gè)數(shù)量級(jí)），但當(dāng)錐角很大時(shí)誤差較Qpicard 大；（2）Qpicard 算法在全錐角范圍內(nèi)精度適中，當(dāng)錐角很大時(shí)依然保持較高精度；（3）Comp+Highorder 算法在錐角不大的情況下精度與Qpicard 算法一致，而當(dāng)錐角很大時(shí)與Uncomp+Highorder 算法幾乎一樣，精度均優(yōu)于Uncompressed 算法。

圖1 圓錐漂移誤差對(duì)比Fig.1 Comparisons for coning drift errors

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

這里選用文獻(xiàn)[2,11]中的以多項(xiàng)式表示的2 s 大角度機(jī)動(dòng)環(huán)境，重寫多項(xiàng)式系數(shù)矩陣如下

對(duì)Qpicard、Uncompressed、Uncomp+Highorder和Comp+Highorder 四種算法進(jìn)行了仿真，采樣間隔10 ms 且均為四子樣算法，結(jié)果如圖2所示。從圖2中可以看出:（1）僅用Uncompressed 算法精度不高；（2）Qpicard 算法精度最高，說明特別適用于大角度機(jī)動(dòng)情形；（3）Uncomp+Highorder 和Comp+Highorder兩種算法的精度相當(dāng)，可見在采用高階補(bǔ)償算法之后，與是否再運(yùn)用擴(kuò)展算法關(guān)系不大；（4）Highorder 算法與傳統(tǒng)Uncompressed 算法相比精度提高了三個(gè)數(shù)量級(jí)。

圖2 大機(jī)動(dòng)失準(zhǔn)角誤差對(duì)比Fig.2 Misalignment angle comparisons under high-maneuvering

3.3 子樣數(shù)與采樣間隔關(guān)系仿真

圓錐運(yùn)動(dòng)條件同3.1 節(jié)，比較了四子樣“擴(kuò)展+高階誤差補(bǔ)償”算法（Uncomp+Highorder4）與二子樣高階誤差補(bǔ)償算法（Highorder2）的差別，其中Uncomp+Highorder4 的采樣間隔為10 ms、Highorder2的采樣間隔為10 ms 并不斷減半直至0.625 ms，仿真結(jié)果見圖3。由圖3可見，Highorder2 算法隨著采樣間隔的不斷減小精度不斷提高，且精度提高程度不受錐角大小的影響，采樣間隔減半精度大約能夠提高一個(gè)數(shù)量級(jí)，因此，選用高頻低子樣數(shù)的誤差補(bǔ)償算法往往比采用低頻高子樣數(shù)的算法更加有效。

圖3 不同子樣數(shù)與采用間隔下的算法漂移Fig.3 Error drifts for different sub-sample number vs sampling interval

4 結(jié) 論

在捷聯(lián)慣導(dǎo)系統(tǒng)姿態(tài)更新中，采用高階誤差補(bǔ)償算法是提高等效旋轉(zhuǎn)矢量解算精度一種措施，但是高階誤差系數(shù)的求解比較煩瑣，文獻(xiàn)[7]依靠解析推導(dǎo)的方法進(jìn)行求解就出現(xiàn)了一處錯(cuò)誤（參見文獻(xiàn)[7]的表1中的系數(shù)a34-1，其值-238/5885 應(yīng)為-101/1874），它很難依靠公式推導(dǎo)發(fā)現(xiàn)和復(fù)核。文獻(xiàn)[10]應(yīng)用更簡(jiǎn)單的隨機(jī)仿真方法修正了該處錯(cuò)誤，最終獲得了全面正確的等效旋轉(zhuǎn)矢量高階誤差補(bǔ)償算法。本文同樣也采用隨機(jī)仿真的方法求解高階誤差補(bǔ)償系數(shù)，但在消除誤差補(bǔ)償系數(shù)間的相關(guān)性時(shí)使用矩陣秩的數(shù)值判斷方法，結(jié)果更加簡(jiǎn)潔，從根本上省去了公式推導(dǎo)的麻煩，具有更好的通用性和可推廣性。

仿真表明，針對(duì)圓錐運(yùn)動(dòng)環(huán)境，“擴(kuò)展+高階誤差補(bǔ)償”算法在錐角不是特別大的情況下精度較高；而針對(duì)多項(xiàng)式大角度機(jī)動(dòng)環(huán)境，迭代算法的精度更高。其實(shí)，不論哪種角運(yùn)動(dòng)環(huán)境，提高采樣頻率都可以大幅度地降低姿態(tài)更新誤差，采用高頻低子樣數(shù)的簡(jiǎn)單算法遠(yuǎn)比采用低頻高子樣數(shù)的高階誤差補(bǔ)償復(fù)雜算法更加有效。在實(shí)際應(yīng)用中，由于受陀螺儀噪聲誤差等因素的制約，物理傳感器造成的誤差可能遠(yuǎn)比數(shù)學(xué)算法引起的誤差大得多，因而選用過高精度的姿態(tài)更新算法沒有太多實(shí)際意義，研究高精度迭代算法和高階誤差補(bǔ)償算法更多的是理論參考價(jià)值。