角速率輸入下的雙子樣二次迭代圓錐補償算法

任雪佳，丁立波，馮 陽，路 坦

(1 南京理工大學機械工程學院，江蘇 南京 210094；2 安徽湘凡科技有限公司，安徽 合肥 231200)

0 引言

捷聯慣性導航具有結構簡單、功耗低、可靠性高等優點,對于單兵彈藥,考慮到低成本與高精度的要求,能夠滿足其導航需求。研究單兵彈藥純慣性導航的姿態解算,可以在較低成本下提高單兵作戰的打擊能力,具有較高的實際應用價值。

姿態更新解算作為捷聯慣導系統的核心,已進行了大量的研究,姿態解算方法主要有:四元數法、旋轉矢量法、歐拉角法、方向余弦法和羅德利格參數法。1971年Bortz[1]提出的旋轉矢量微分方程,為補償載體在高動態環境下的圓錐誤差提供了理論基礎。基于此,國內外學者對基于旋轉矢量的圓錐補償算法進行了大量的研究,可概括為以下幾個方面:增加子樣數或前次周期項信息的圓錐算法系數設計[2-5]、通過補償截斷誤差提高精度[6-7]、對周期項進行補償[8]和圓錐算法結構設計[9]。但這些圓錐補償算法都是以角增量作為輸入,而目前廣泛使用的光纖陀螺、微機械陀螺等輸出為角速度,傳統的圓錐補償算法無法直接使用。

對于角速率輸入,通常采用分段線性化,認為每一個時間段ΔT內的角速度為常量,由Δθ=ω·ΔT來計算角增量,不能有效地提高姿態角度精度[10]。黃昊、杜燁等[11-12]利用梯形公式、辛普森公式或其他數值積分方式得到角增量,代入到傳統圓錐補償算法會使補償精度下降。針對以上問題,曾慶化等[13]推導出一類具有純角速率輸入的圓錐算法;Ben等[14]將Miller的思想引入圓錐算法設計,提出一種以角速率作為算法輸入的改進錐面算法的廣義方法;劉佳樂[15]直接利用角速率推導了雙子樣算法和加入前次周期信息的改進算法,測量精度得到了提高;湯傳業等[16]在計算優化系數時采用最小二乘法,進一步提高了算法精度;盧世昕等[17]在前一周期的基礎上對周期項進行了二次優化;丁鵬等[18]對單軸旋轉捷聯慣導系統,分析了基于角速率的多子樣旋轉矢量算法;Ding等[19]針對圓錐誤差中的高階補償項,在計算等效旋轉矢量時補償了圓錐修正項和三交叉乘積項;邢麗等[20]建立了推力速度坐標系并引入等效螺旋矢量,并采用梯形數字積分方法計算等效螺旋矢量,對圓錐和劃船誤差同時進行了補償。

目前,圓錐算法結構設計的研究分為雙速結構、單速結構兩類[20]。在分析角速率輸入的雙子樣單速算法基礎上,文中引入雙速回路,研究了雙子樣的二次迭代算法,在推導優化系數時進行簡化,減少了計算量。在經典圓錐運動環境下,對算法的可行性和有效性進行了仿真驗證,仿真結果表明,該算法對提高捷聯慣導系統的圓錐誤差計算精度具有積極作用。

1 經典圓錐運動

基于圓錐誤差可以由旋轉矢量的微分方程表示,工程中經常通過分析旋轉矢量的微分方程來設計算法。等效旋轉矢量Φ表征載體系從tk-1至tk時刻的等效旋轉,其微分方程可近似表示為[1]:

(1)

式中:ω為tk-1至tk時間段內的機體角速率;Φ為載體系由tk-1至tk時刻角位置變化所對應的等效旋轉矢量。

假設t時刻載體僅存在錐運動,旋轉矢量為[21]:

(2)

式中:α為錐運動的半錐角;Ω為錐運動的頻率。

對應的錐運動四元數Q(t)為:

(3)

設從t至(t+h)時刻載體系的更新四元數為q(h),旋轉四元數關系為[21]:

Q(t+h)=Q(t)?q(h)

(4)

即有

(5)

式中:h為姿態更新周期,其值為tk-1-tk。

(6)

2 基于角速率的二次迭代優化算法

2.1 雙子樣二次迭代算法推導

相比一次迭代的圓錐補償算法,二次迭代算法是將姿態更新周期分為兩個迭代周期,每個迭代周期內有N個角增量擬合采樣點。對于角速率的雙子樣二次迭代算法,設姿態更新周期為h,迭代周期為T,采樣周期為t,則有以下關系:

h=2T=4t

(7)

第一個迭代周期T1為[tk-1,tk-1+T],第二個迭代周期T2為[tk-1+T,tk-1+2T]。

在迭代周期T1內,假設載體系的角速率用二次曲線表示為:

ω(tk-1+τ)=a1+2b1τ+3c1τ2

(8)

式中:a1,b1,c1為多項式系數;0≤τ≤T。

將T1內的等效旋轉矢量Φ(tk-1+T)作泰勒級數展開:

(9)

(10)

反解出的多項式系數可表示為:

(11)

將式(8)對τ求各階導數:

(12)

其中i≥3。聯立式(1)和式(12)可得:

(13)

其中i≥6。代入式(9)得:

(14)

將式(11)代入式(14)可得到T1內的旋轉矢量:

(15)

在迭代周期T2內,假設載體系的角速率用拋物線表示為:

ω(tk-1+T+τ)=a2+2b2τ+3c2τ2

(16)

式中:a2,b2,c2為多項式系數;0≤τ≤T。

對應的旋轉矢量用泰勒級數展開:

(17)

(18)

2.2 系數優化

上面推導出的角速率交叉項系數是以拋物線擬合角速度得出的,但實際角速度不一定如此,因此需對這些系數進行修正以保證擬合所得的算法漂移最小。

當系統繞x軸作圓錐運動時,設

(19)

式中:k1,k2,k3,k4為待定優化系數,使得在圓錐運動環境下的算法漂移最小。考慮到計算量和工程化,圓錐誤差項僅計算3個叉乘項足以保證算法的精度,過多反而增加計算負擔且精度提升不明顯,因此將式(19)簡化為:

(20)

僅考慮x軸直流分量,將式(6)代入式(20),得

(21)

根據精度準則[2],有:

(22)

(23)

由于Ωh遠小于1,令(ΩT)3、(ΩT)5和(ΩT)7項系數為零,解得:

(24)

綜上所述,基于角速率的雙子樣二次迭代算法可表示為:

(25)

算法漂移為:

(26)

3 仿真實驗與分析

把典型圓錐運動直接作為測試輸入環境,將傳統雙子樣算法(Method 1)[21]、傳統雙子樣二次迭代算法(Method 2)[22]、基于角速率的雙子樣優化算法(Method 3)[15]、基于角速率的三子樣優化算法(Method 4)[15]、與文中算法(Method 5)進行比較,驗證文中提出的姿態算法的性能。

設置3組仿真條件:1)半錐角α為1°,圓錐運動頻率f為1 Hz,姿態更新周期h為24 ms,仿真總時長為10 s。2)半錐角α為1°,圓錐運動頻率f為10 Hz,姿態更新周期h為2.4 ms,仿真總時長為10 s。姿態角解算誤差如圖1～圖2所示;仿真時間內Method 1～Method 5在兩種圓錐運動環境下的最大姿態角誤差分別如表1～表2所示。3)半錐角α為1°,陀螺采樣間隔為2 ms,改變圓錐運動頻率,對Method 2和 Method 5兩種算法在x軸上的俯仰角誤差的最大值進行仿真,結果如表3所示。

圖1 各算法在條件1下的姿態角誤差Fig.1 Attitude angle error of each algorithm under condition 1

圖2 各算法在條件2下的姿態角誤差Fig.2 Attitude angle error of each algorithm under condition 2

表1 條件1下各算法姿態角誤差最大值

表2 條件2下各算法姿態角誤差最大值

對比條件1和條件2可知,在不考慮陀螺采樣上限的情況下,隨著圓錐運動頻率的增大,姿態更新周期相應減小,算法的精度相差僅一個數量級,驗證了算法的可行性和有效性,說明文中算法能在高動態環境下保證算法性能。

對比Method 1和Method 3,Method 2和Method 5可看出,在相同仿真條件下,當陀螺輸出為角速率時,采用梯形公式或辛普森公式等提取角增量再代入傳統的旋轉矢量算法,不僅在精度上存在局限,而且求出的角增量之和存在誤差,基于角速率的圓錐補償算法能夠明顯提高計算精度。對比Method 4和Method 5可看出,相比基于角速率的雙子樣和三子樣的一次迭代算法,文中提出的改進算法精度更高,且叉乘運算量和三子樣優化算法相同,使姿態精度和穩定性得到了進一步的改善。

為了更加清晰和明確地說明文中算法與傳統算法相比較的優勢,表3統計了Method 2和Method 5兩種算法在相同圓錐條件下,姿態解算誤差的最大值對比結果。

表3 2 ms采樣間隔下x軸上俯仰角誤差最大值

從表3中可以看出,隨著圓錐運動頻率的增大,導航解算誤差的最大值均小于傳統算法1～2個數量級,即載體的運動幅值變大時,文中算法的姿態解算精度更優于傳統算法。

4 結論

為解決傳統姿態算法在角速率輸入時計算精度較低、不能有效補償圓錐誤差的問題,文中在雙子樣一次迭代的基礎上,提出了采用雙子樣二次迭代的旋轉矢量姿態算法,推導了能夠滿足工程運用的算法簡化表達式。仿真結果表明,提出的姿態算法與傳統算法、基于角速率的雙子樣和三子樣的一次迭代算法相比,具有更高的姿態解算精度,且叉乘運算量和三子樣優化算法相同。故提出的角速率輸入下二次迭代的雙子樣旋轉矢量姿態算法具有一定的應用價值。