一種圓錐運動條件下的等效旋轉矢量算法研究

王文舉，劉生攀

(貴州航天控制技術有限公司，貴陽 550009 )

0 引言

在捷聯慣導系統的姿態、速度和位置更新算法中，姿態算法對整個系統精度的影響最大，它是算法研究和設計的核心。姿態算法有歐拉角法、方向余弦法、四元數法、旋轉矢量法等[1-2]。歐拉角算法包含三角運算，實時計算有一定的困難，且在一定條件下方程出現退化，不適用全姿態的解算。方向余弦法避免了歐拉角法中的方程退化問題，但計算量大，工程中并不實用。四元數法計算量小，實現簡單，工程中較實用，其本質是旋轉矢量法中的單子樣算法，沒有對有限轉動過程中的不可交換誤差進行補償，比較適合低動態載體的姿態解算，對于高動態載體，算法漂移會比較嚴重。旋轉矢量法采用多子樣算法對不可交換誤差進行補償，算法實現簡單，并且可以對系數進行優化，適用于角運動頻繁或角振動的載體姿態解算。

為了解決角運動過程中的不可交換誤差，1971年Bortz提出了旋轉矢量法，它有2個計算步驟，先求解旋轉矢量，再進行四元數更新。目前的旋轉矢量多以角增量為參數進行求解，根據姿態更新周期內對陀螺輸出角增量等間隔采樣數的不同，等效旋轉矢量的求解可分為單子樣法、雙子樣法、三子樣法和四子樣法等[3-9]。

對于給定的捷聯姿態算法，需要采用一定的測試輸入來評價其性能。對于捷聯慣導姿態更新，經典圓錐運動是最惡劣的工作環境條件之一，它會誘發數學平臺的嚴重漂移，所以對旋轉矢量算法作優化處理。通常采用經典圓錐運動作為環境條件，如果能確保圓錐運動環境條件下算法精度高，就能確保在其余環境條件下算法精度高[10-11]。

本文首先介紹了經典圓錐運動，常用的等效旋轉矢量法，然后詳細推導了一種新的等效旋轉矢量法，并且以經典圓錐運動作為姿態算法的測試輸入。仿真結果表明，姿態、速度、位置精度得到了明顯提高。

1 經典的圓錐運動

經典圓錐運動下載體運動可以用旋轉矢量表示為[12]：

(1)

其中，φ是圓錐運動的錐半角，Ω是圓錐運動的角頻率。任意t時刻，載體坐標系繞Oybzb平面上的轉軸Φ相對于慣性坐標系轉動φ角度，轉軸時刻在變化而轉角恒定不變，載體坐標系的xb軸在空間畫出一個圓錐面(半錐角為φ，xb軸為錐軸)，如圖1所示，這正是該角運動稱為圓錐運動的原因。

經典圓錐運動時，載體相對慣性空間的角速率在載體坐標系的分量為：

(2)

經典圓錐運動對應的載體更新四元數為：

(3)

2 等效旋轉矢量算法

等效旋轉矢量法也是建立在剛體矢量旋轉思想的基礎上，與四元數法的不同在于：在姿態更新周期內，四元數法直接計算姿態四元數，而旋轉矢量法先計算姿態變換四元數，再計算姿態四元數。旋轉矢量的微分方程如下：

(4)

式中，旋轉矢量的導數等于ω再加上2個修正項，而修正項反映了不可交換誤差產生的影響。目前，旋轉矢量算法多以角增量為參數，基于泰勒級數展開進行多子樣求解，如單子樣法、雙子樣法、三子樣法、四子樣法等。雙子樣算法公式如下：

(5)

三子樣算法公式如下:

(6)

高階泰勒級數展開原則上要求函數Φ足夠光滑，而實際陀螺輸出總會或多或少包含電氣噪聲，噪聲并不反映載體的真實角運動，同時對角速度函數的光滑性也造成不良影響。因此，多子樣算法的精度有限，并非子樣數越多算法的實用精度就越高，并且高子樣的計算量較大。在選擇子樣數時，應權衡利弊，兼顧精度要求、計算速度等方面。

3 一種新的等效旋轉矢量算法

旋轉矢量算法多以角增量為參數進行求解，激光陀螺一般輸出為角增量，但是光纖陀螺輸出為角速率[13]。如果把角速率轉化為角增量，很難保證旋轉矢量精度不受影響，這一差異使得使用角增量代替旋轉矢量進行姿態更新時會產生誤差，并且誤差隨時間會不斷累積。本節在旋轉矢量算法的基礎上研究了一種新的等效旋轉矢量算法，該算法使用角增量和角速率，以角速率為主，等效旋轉矢量精度更高，與二子樣算法相比較，圓錐誤差補償精度得到進一步提高。

類似于傳統二子樣算法的推導思路，陀螺在一個計算周期(T，T+h)內，需要3次陀螺采樣值，假設h內輸出可表示為：

ω(τ)=a+2bτ+3cτ20≤τ≤h

(7)

記角增量為：

(8)

可計算Δθ(0)及其各階導數，如下：

(9)

由于姿態更新周期h一般為毫秒級，旋轉矢量Φ也可視為小量[14]。

Φ(τ)≈Δθ(τ)

(10)

忽略高階小量，式(4)可以寫成：

(11)

可計算Φ(0)的各階導數，如下：

(12)

將Φ(h)用泰勒級數展開[15]，得：

(13)

(14)

將式(14)代入式(13), 其中：

可知：

(15)

并考慮陀螺的角增量輸出，則旋轉矢量可用式(16)估計：

Φ=Δθ+Xh2ω1×ω3+Yh2ω2×(ω3-ω1)

(16)

式中：

在圓錐運動條件下，捷聯慣導姿態更新過程會誘發數學平臺的嚴重漂移。因此，以圓錐運動作為測試條件，對旋轉矢量算法中的系數作優化，根據圓錐運動可得到理論四元數q(h)，利用旋轉矢量

(17)

4 仿真分析

捷聯慣導加表零偏穩定性在100μg，陀螺零漂穩定性在0.01(°)/h，慣性器件采樣周期為5ms，姿態更新周期10ms。以經典圓錐運動作為姿態輸入，錐半角為1°，角頻率為8Hz，導航時間為1h。對比雙子樣法與新的等效旋轉矢量法的解算效果，如圖2～圖7所示，在姿態精度、速度精度、位置精度等方面，新的算法都優于傳統的二子樣算法，并且計算量相當。

圖2 航向角誤差對比圖Fig.2 Contrast diagram of heading error

圖3 俯仰角誤差對比圖Fig.3 Contrast diagram of pitch error

圖4 橫滾角誤差對比圖Fig.4 Contrast diagram of roll error

圖5 東向速度誤差對比圖Fig.5 Contrast diagram of east direction velocity error

圖6 北向速度誤差對比圖Fig.6 Contrast diagram of north direction velocity error

圖7 位置誤差對比圖Fig.7 Contrast diagram of position error

5 結論

本文根據光纖陀螺輸出為角速率的特點，在旋轉矢量姿態更新算法的基礎上研究了一種新的圓錐誤差補償表達式。等效旋轉矢量使用角增量和角速率，以角速率為主，其精度高于二子樣算法，運算量相當，通過仿真驗證了改進算法具有較高的精度，且計算量相對較小，易于工程實現，為光纖陀螺捷聯系統姿態算法解算提供了一個更為有效的途徑。