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

王文舉，劉生攀

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

0 引言

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

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

對于給定的捷聯(lián)姿態(tài)算法，需要采用一定的測試輸入來評價其性能。對于捷聯(lián)慣導(dǎo)姿態(tài)更新，經(jīng)典圓錐運動是最惡劣的工作環(huán)境條件之一，它會誘發(fā)數(shù)學(xué)平臺的嚴(yán)重漂移，所以對旋轉(zhuǎn)矢量算法作優(yōu)化處理。通常采用經(jīng)典圓錐運動作為環(huán)境條件，如果能確保圓錐運動環(huán)境條件下算法精度高，就能確保在其余環(huán)境條件下算法精度高[10-11]。

本文首先介紹了經(jīng)典圓錐運動，常用的等效旋轉(zhuǎn)矢量法，然后詳細(xì)推導(dǎo)了一種新的等效旋轉(zhuǎn)矢量法，并且以經(jīng)典圓錐運動作為姿態(tài)算法的測試輸入。仿真結(jié)果表明，姿態(tài)、速度、位置精度得到了明顯提高。

1 經(jīng)典的圓錐運動

經(jīng)典圓錐運動下載體運動可以用旋轉(zhuǎn)矢量表示為[12]：

(1)

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

經(jīng)典圓錐運動時，載體相對慣性空間的角速率在載體坐標(biāo)系的分量為：

(2)

經(jīng)典圓錐運動對應(yīng)的載體更新四元數(shù)為：

(3)

2 等效旋轉(zhuǎn)矢量算法

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

(4)

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

(5)

三子樣算法公式如下:

(6)

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

3 一種新的等效旋轉(zhuǎn)矢量算法

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

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

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

(7)

記角增量為：

(8)

可計算Δθ(0)及其各階導(dǎo)數(shù)，如下：

(9)

由于姿態(tài)更新周期h一般為毫秒級，旋轉(zhuǎn)矢量Φ也可視為小量[14]。

Φ(τ)≈Δθ(τ)

(10)

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

(11)

可計算Φ(0)的各階導(dǎo)數(shù)，如下：

(12)

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

(13)

(14)

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

可知：

(15)

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

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

(16)

式中：

在圓錐運動條件下，捷聯(lián)慣導(dǎo)姿態(tài)更新過程會誘發(fā)數(shù)學(xué)平臺的嚴(yán)重漂移。因此，以圓錐運動作為測試條件，對旋轉(zhuǎn)矢量算法中的系數(shù)作優(yōu)化，根據(jù)圓錐運動可得到理論四元數(shù)q(h)，利用旋轉(zhuǎn)矢量

(17)

4 仿真分析

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

圖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 結(jié)論

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