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

（1. 東南大學(xué) 儀器科學(xué)與工程學(xué)院，南京 210096；2. 微慣性儀表與先進(jìn)導(dǎo)航技術(shù)教育部重點(diǎn)實(shí)驗(yàn)室，南京 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)解算算法，仿真驗(yàn)證該算法可以更精確真實(shí)地模擬姿態(tài)更新。文獻(xiàn)[5-6]考慮了Bortz 方程的三叉積項(xiàng)，在旋轉(zhuǎn)矢量的四階畢卡分量解的基礎(chǔ)上提出了改進(jìn)算法，但是其推導(dǎo)過程比較繁瑣。文獻(xiàn)[7]推導(dǎo)了一種新的系數(shù)優(yōu)化算法，在文獻(xiàn)[5]基礎(chǔ)上簡化了其高階誤差補(bǔ)償算法的推導(dǎo)過程。文獻(xiàn)[8]結(jié)合角速度擬合多項(xiàng)式和姿態(tài)四元數(shù)高階泰勒級數(shù)展開，提出了一種新的四子樣姿態(tài)算法，提高了姿態(tài)解算的精度。文獻(xiàn)[9]利用切比雪夫多項(xiàng)式擬合角速度矢量，通過迭代求得羅德里格參數(shù)微分方程的精確解。文獻(xiàn)[10]提出了基于多項(xiàng)式迭代的旋轉(zhuǎn)矢量微分方程數(shù)值算法（RVPI），通過角速度擬合多項(xiàng)式迭代求解Bortz 方程，避免Bortz方程求解中的近似誤差，取得了較好效果。

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

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

1 多項(xiàng)式方法及多項(xiàng)式迭代方法

1.1 多項(xiàng)式描述

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

式中，

1.2 多項(xiàng)式迭代

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

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

將式(2)中第三項(xiàng)的系數(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)的積分項(xiàng)中，進(jìn)一步計(jì)算得：

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

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

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

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

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

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

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

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

在經(jīng)典圓錐運(yùn)動環(huán)境中，捷聯(lián)姿態(tài)計(jì)算的漂移誤差主要體現(xiàn)在圓錐軸（x 軸），而周期項(xiàng)誤差主要體現(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ù)時(shí)，l= （-1）(i+j+3)/2； 當(dāng)i為奇數(shù)、j為偶數(shù)時(shí)，l= （-1）(i+j+1)/2。

2.2 多項(xiàng)式改進(jìn)

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

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

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

即

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

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

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

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

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

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

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

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

即

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

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

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

3 仿真與分析

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

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

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

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

由圖1 可知，三種算法在錐軸上的漂移誤差隨時(shí)間逐漸變大，在非錐軸上的誤差呈周期性變化。傳統(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 時(shí)δφx的值，得到其在錐軸上最大誤差為 8.71 ×10-7''，而其在非錐軸上的誤差值近似為0。由此驗(yàn)證了改進(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)算法具有相同的漂移趨勢，但在整個(gè)半錐角范圍內(nèi)，改進(jìn)算法精度均優(yōu)于RVPI 算法；但與傳統(tǒng)算法相比，改進(jìn)算法與RVPI 具有相同的特點(diǎn)，即當(dāng)半錐角較小時(shí)，傳統(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 的漂移誤差更??；但在半錐角較小時(shí)，傳統(tǒng)算法精度較高。綜合圖2～4 中RVPI 和改進(jìn)算法與傳統(tǒng)算法的交匯點(diǎn)可以發(fā)現(xiàn)，改進(jìn)算法隨著子樣數(shù)的提高，交匯點(diǎn)逐漸向小錐角角度方向移動。改進(jìn)算法在n=3、4、5 時(shí)的交匯點(diǎn)半錐角分別約為：10 °，0.2 °，0.03 °；而RVPI 的交匯點(diǎn)分別約為：90 °，2 °，0.6 °。這表明，相較于RVPI，改進(jìn)算法具有一定的優(yōu)勢。

3.2 多項(xiàng)式角運(yùn)動仿真

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

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

圖5 大角速率機(jī)動誤差漂移對比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 時(shí)，改進(jìn)算法整體漂移量小于RVPI，且誤差振蕩幅值較小。這表明，改進(jìn)算法引入的約束條件未降低原有RVPI 在多項(xiàng)式運(yùn)動條件下的精度。

圖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 方程存在的原理性近似誤差后，角速度多項(xiàng)式的擬合精度成為制約導(dǎo)航解算精度的關(guān)鍵。本文在文獻(xiàn)[10]算法基礎(chǔ)上，進(jìn)一步提出了角速度擬合多項(xiàng)式的改進(jìn)算法。

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

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