基于高階多項式迭代的捷聯(lián)姿態(tài)更新優(yōu)化方法

劉錫祥，趙苗苗，張玉鵬，郭小樂，王 磊

（1. 東南大學(xué) 儀器科學(xué)與工程學(xué)院，南京 210096；2. 微慣性儀表與先進(jìn)導(dǎo)航技術(shù)教育部重點實驗室，南京 210096；3. 浙江大學(xué)光電科學(xué)與工程學(xué)院，杭州 310027）

在捷聯(lián)慣導(dǎo)系統(tǒng)中，姿態(tài)更新算法的優(yōu)劣直接影響系統(tǒng)的導(dǎo)航精度，而在姿態(tài)解算中不可避免地存在不可交換性誤差（又稱圓錐誤差），多年來學(xué)者們對其進(jìn)行了廣泛而深入的研究[1-3]。

文獻(xiàn)[4]通過引入同步旋轉(zhuǎn)坐標(biāo)系提出了新姿態(tài)解算算法，仿真驗證該算法可以更精確真實地模擬姿態(tài)更新。文獻(xiàn)[5-6]考慮了Bortz 方程的三叉積項，在旋轉(zhuǎn)矢量的四階畢卡分量解的基礎(chǔ)上提出了改進(jìn)算法，但是其推導(dǎo)過程比較繁瑣。文獻(xiàn)[7]推導(dǎo)了一種新的系數(shù)優(yōu)化算法，在文獻(xiàn)[5]基礎(chǔ)上簡化了其高階誤差補償算法的推導(dǎo)過程。文獻(xiàn)[8]結(jié)合角速度擬合多項式和姿態(tài)四元數(shù)高階泰勒級數(shù)展開，提出了一種新的四子樣姿態(tài)算法，提高了姿態(tài)解算的精度。文獻(xiàn)[9]利用切比雪夫多項式擬合角速度矢量，通過迭代求得羅德里格參數(shù)微分方程的精確解。文獻(xiàn)[10]提出了基于多項式迭代的旋轉(zhuǎn)矢量微分方程數(shù)值算法（RVPI），通過角速度擬合多項式迭代求解Bortz 方程，避免Bortz方程求解中的近似誤差，取得了較好效果。

上述姿態(tài)更新算法基本上都是基于角速度多項式運動模型直接或間接補償不可交換誤差，提高算法精度，然而角速度擬合精度仍是制約這些算法精度的重要因素。目前，增加角速度的多項式擬合精度方法有增加子樣數(shù)、利用前周期的采樣角增量提高角速度多項式擬合階次、增加其它約束構(gòu)建高階角速度多項式等，文獻(xiàn)[11]是在子樣數(shù)確定的條件下，通過利用前一個或多個周期的采樣信息來構(gòu)建更高階子樣算法，有效提高了精度。而本文則是通過引入圓錐約束增大了擬合階次，從而提高了算法精度。

本文在文獻(xiàn)[10]基礎(chǔ)上，利用圓錐環(huán)境下角速度矢量的各階導(dǎo)數(shù)在圓錐軸上數(shù)值之間的解析關(guān)系，建立高一階或高二階近似模型，從而實現(xiàn)對傳統(tǒng)角速度擬合多項式的改進(jìn)。該算法在確定的采樣頻率下，無需增加陀螺儀輸出的角增量數(shù)量便可以獲得擬合角速度的高階多項式，為等效旋轉(zhuǎn)矢量微分方程提供更精確的角速度矢量輸入，有效地實現(xiàn)姿態(tài)更新。

1 多項式方法及多項式迭代方法

1.1 多項式描述

實際慣導(dǎo)系統(tǒng)中，陀螺輸出的信息為角增量，而姿態(tài)更新算法常常采用角速度作為輸入，因此需要給出由角增量轉(zhuǎn)化為角速度的方法。令陀螺角增量的采樣周期為h0，在時間段 [-ph0,nh0]內(nèi)進(jìn)行了N次采樣（p,n均為整數(shù)，p≥ 0,n＞0且p+n=N），角增量分別記為Δθj（j=-p+ 1, -p+ 2, … ,n），可以得到角速度ω為時間t的有限次（N- 1）擬合多項式，表示為

式中，

1.2 多項式迭代

等效旋轉(zhuǎn)矢量微分方程（Bortz 方程）為

式中，Φ為等效旋轉(zhuǎn)矢量，。

將式(2)中第三項的系數(shù)由泰勒級數(shù)展開后得

將式(3)代入到式(2)，并對其進(jìn)行積分，再改成迭代形式[10]，整理后得

式中，Φ(0)=0，Φ(i)(i=0,1,2, …) 表示第i次迭代的等效旋轉(zhuǎn)矢量。

式(4)即為求解等效旋轉(zhuǎn)矢量的主要公式，接下來分別對式中積分進(jìn)行求解。

1) 將式(1)代入到式(4)的積分項中，進(jìn)一步計算得：

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

2) 對式(4)第三個積分項中的系數(shù)進(jìn)行求解，其表達(dá)式為

2 高階改進(jìn)多項式

文獻(xiàn)[10]基于多項式迭代的算法有效避免了Bortz方程的原理性近似誤差，而多項式迭代算法的精度主要取決于角速度多項式的擬合精度。角速度擬合多項式的階次越高，反映運載體的角運動的準(zhǔn)確度也越高。在確定的姿態(tài)更新周期和采樣頻率下，相關(guān)學(xué)者[11]利用前周期和本周期的角增量采樣值進(jìn)行了角速度的多項式擬合，提高了算法的子樣階次；本文基于文獻(xiàn)[10]，在子樣數(shù)n不變的情況下，通過引入圓錐運動增加約束條件，提高了擬合階次，使得擬合角速度的多項式階次由n-1 增大到n或n+1 ，從而提高了多項式迭代算法精度。

2.1 圓錐運動下角速度各系數(shù)之間的關(guān)系

圓錐運動的更新四元數(shù)和角速度矢量分別表示為：

由于任意曲線都可以由多項式擬合得到，角速度矢量由擬合多項式[12]表示為

對上式求導(dǎo)，并令其在t=0 時取值，于是有

在經(jīng)典圓錐運動環(huán)境中，捷聯(lián)姿態(tài)計算的漂移誤差主要體現(xiàn)在圓錐軸（x 軸），而周期項誤差主要體現(xiàn)在y 軸和z 軸上。因此，角速度在x 軸方向上存在的偏差對該漂移誤差產(chǎn)生了主要影響，接下來主要對角速度導(dǎo)數(shù)ω(i)的x 軸方向上的值進(jìn)行討論分析。令ω(i)(0)和ω(j)(0)叉乘（i≠j），可以得到

比較上述各式，可以發(fā)現(xiàn)以下規(guī)律：

式中，l=-1 或 1。其中，當(dāng)i為偶數(shù)、j為奇數(shù)時，l= （-1）(i+j+3)/2； 當(dāng)i為奇數(shù)、j為偶數(shù)時，l= （-1）(i+j+1)/2。

2.2 多項式改進(jìn)

傳統(tǒng)算法中，角速度擬合多項式的階次為n-1 ，n為子樣數(shù)。本文提出一種改進(jìn)型算法，其角速度的擬合多項式階次為n或n+1 。下面以三子樣算法為例進(jìn)行說明。

傳統(tǒng)三子樣算法是在對［t k,tk+1］時間段內(nèi)的運載體角速度采用拋物線擬合推導(dǎo)得到的。h為姿態(tài)更新周期，h=tk+1-tk。角速度ω的表達(dá)式為

為了用角增量表示上式中的參數(shù)c0、c1、c2，記

即

對上述方程組進(jìn)行求解，得到角速度各系數(shù)的表達(dá)式為

改進(jìn)型三子樣算法是在對［t k,tk+1］時間段內(nèi)的運載體角速度采用三階多項式擬合推導(dǎo)得到的。角速度ω'表達(dá)式為

接下來求解ω'各系數(shù)，在求解過程中，將看成未知量，求解步驟和傳統(tǒng)三子樣算法一樣。因此得到

對比式(17)和式(19)，可以得到

由2.1 節(jié)式(13)總結(jié)出來的規(guī)律可以得到

只考慮多項式系數(shù)之間的叉積在圓錐軸上的關(guān)系，可近似得到

將式(20)代入式(21)，得

式(22)中的高階項影響較小，可忽略不計。于是有

即

式中，“+”代表矩陣的廣義逆。

當(dāng)子樣數(shù)n= 4時，由2.1 節(jié)式(13)總結(jié)出來的規(guī)律可以得到因此四階擬合多項式不能通過此規(guī)律獲得，轉(zhuǎn)為利用五階擬合多項式，多項式系數(shù)可由分別與的比例關(guān)系獲得；當(dāng)子樣數(shù)n= 5時，多項式系數(shù)可通過與的比例關(guān)系獲得。表1 列出了3 子樣、4 子樣、5 子樣算法的角速度高階擬合多項式的表達(dá)式。由此類推，即可獲得任意n子樣的n或n+1 階擬合多項式，當(dāng)子樣n為奇數(shù)，階數(shù)為n；當(dāng)子樣n為偶數(shù)，階數(shù)為n+1 。

表1 3～5 子樣高階擬合多項式Tab.1 Higher-order fitting polynomials with 3 ～ 5 subsamples

3 仿真與分析

以圓錐運動和多項式角運動兩種環(huán)境作為測試輸入，利用MATLAB 對算法的漂移誤差進(jìn)行仿真。仿真對比了三種不同的算法，分別為改進(jìn)算法（New）、傳統(tǒng)優(yōu)化圓錐算法（Classical）與文獻(xiàn)[10]提出的高階旋轉(zhuǎn)矢量微分方程精確數(shù)值算法（RVPI）。

3.1 圓錐運動仿真

圓錐運動仿真參數(shù)設(shè)置為：圓錐頻率f=1 Hz，子樣數(shù)n=4 ，采樣時間為0.01 s，仿真時長為3 s，半錐角α=10°。仿真結(jié)果如圖1 所示。

圖1 圓錐誤差漂移對比Fig.1 Comparison of coning error drift

由圖1 可知，三種算法在錐軸上的漂移誤差隨時間逐漸變大，在非錐軸上的誤差呈周期性變化。傳統(tǒng)算法的漂移誤差遠(yuǎn)大于其它兩種算法，因此主要對RVPI 算法和改進(jìn)算法進(jìn)行對比分析。RVPI 算法在錐軸上波動較大，最大達(dá)到了 7.62 ×10-5''，其在非錐軸上漂移誤差較小，最大達(dá)到了 1.55 ×10-6''；而改進(jìn)算法的三軸誤差始終都很小，將錐軸上誤差圖放大后仍然觀察不出其量級，因此取t=3 s 時δφx的值，得到其在錐軸上最大誤差為 8.71 ×10-7''，而其在非錐軸上的誤差值近似為0。由此驗證了改進(jìn)算法的精度，且在相同條件下，改進(jìn)算法要優(yōu)于傳統(tǒng)算法和RPVI 算法。

令其它參數(shù)不變，分別取半錐角參數(shù)為0～90°進(jìn)行仿真，對比三種算法前3s 隨半錐角變化情況的漂移誤差，結(jié)果如圖2 所示。

圖2 三種算法漂移對比（n = 4）Fig.2 Comparison of three algorithms drift (n = 4)

由圖可以看出，RVPI 與改進(jìn)算法具有相同的漂移趨勢，但在整個半錐角范圍內(nèi)，改進(jìn)算法精度均優(yōu)于RVPI 算法；但與傳統(tǒng)算法相比，改進(jìn)算法與RVPI 具有相同的特點，即當(dāng)半錐角較小時，傳統(tǒng)算法精度較高；隨著半錐角的增大，改進(jìn)算法與RVPI 的優(yōu)勢逐漸顯現(xiàn)。

為進(jìn)一步探究改進(jìn)算法的解算精度和穩(wěn)定性，給出算法在3 子樣、5 子樣條件下隨半錐角變化的漂移誤差。

圖3 三種算法漂移對比（n = 3）Fig.3 Comparison of three algorithms drift (n = 3)

圖4 三種算法漂移對比（n = 5）Fig.4 Comparison of three algorithms drift (n = 5)

對圖2、圖3、圖4 進(jìn)行對比分析，改進(jìn)算法始終都比RVPI 的漂移誤差更??；但在半錐角較小時，傳統(tǒng)算法精度較高。綜合圖2～4 中RVPI 和改進(jìn)算法與傳統(tǒng)算法的交匯點可以發(fā)現(xiàn)，改進(jìn)算法隨著子樣數(shù)的提高，交匯點逐漸向小錐角角度方向移動。改進(jìn)算法在n=3、4、5 時的交匯點半錐角分別約為：10 °，0.2 °，0.03 °；而RVPI 的交匯點分別約為：90 °，2 °，0.6 °。這表明，相較于RVPI，改進(jìn)算法具有一定的優(yōu)勢。

3.2 多項式角運動仿真

參考文獻(xiàn)[13]中的大角速率機動仿真環(huán)境設(shè)置，給出以多項式表示的運載體角速度為

設(shè)定陀螺儀采樣間隔為0.01s，仿真時長為1s，子樣數(shù)n=4 。各算法的角度誤差漂移如圖5 所示。

圖5 大角速率機動誤差漂移對比Fig.5 Comparison of large angular rate error drift

由圖5 可以看出，傳統(tǒng)算法的精度遠(yuǎn)低于另外兩種算法，而RVPI 和改進(jìn)算法的對比不太明顯，故只考慮RVPI 和改進(jìn)算法，基于3～5 子樣分別對其進(jìn)行了仿真分析，如圖6～8 所示。

分析圖6～8 可以發(fā)現(xiàn)，在子樣n=3 和n=4 兩種情況下，RVPI 和改進(jìn)算法兩者誤差均處于同一量級，三軸誤差大小各有不同，精度近似相當(dāng)；而在n=5 時，改進(jìn)算法整體漂移量小于RVPI，且誤差振蕩幅值較小。這表明，改進(jìn)算法引入的約束條件未降低原有RVPI 在多項式運動條件下的精度。

圖6 RVPI、改進(jìn)算法誤差對比（n = 3）Fig.6 Comparison of error drift between RVPI and New algorithm (n = 3)

圖7 RVPI、改進(jìn)算法誤差對比（n = 4）Fig.7 Comparison of error drift between RVPI and New algorithm (n = 4)

圖8 RVPI、改進(jìn)算法誤差對比（n = 5）Fig.8 Comparison of error drift between RVPI and New algorithm (n = 5)

4 結(jié) 論

經(jīng)過對文獻(xiàn)[10]進(jìn)行的分析，本文認(rèn)為其在解決Bortz 方程存在的原理性近似誤差后，角速度多項式的擬合精度成為制約導(dǎo)航解算精度的關(guān)鍵。本文在文獻(xiàn)[10]算法基礎(chǔ)上，進(jìn)一步提出了角速度擬合多項式的改進(jìn)算法。

引入圓錐運動增加約束，使得n個陀螺樣本擬合的多項式階數(shù)由n-1 增加到n或n+1 ；在理論分析基礎(chǔ)上，分別模擬仿真了多項式角運動和不同半錐角條件下的圓錐運動，通過求解等效旋轉(zhuǎn)矢量微分方程驗證并對比了改進(jìn)算法、傳統(tǒng)優(yōu)化圓錐算法和高階旋轉(zhuǎn)矢量微分方程數(shù)值算法（RVPI）。

試驗表明，由于角速度擬合精度的提高，改進(jìn)算法在高動態(tài)環(huán)境下的性能優(yōu)于RVPI 算法；在多項式角運動下與RVPI 算法精度相當(dāng)；且隨著子樣數(shù)增大，改進(jìn)算法更具精度優(yōu)勢。