王文舉，劉生攀

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

0 引言

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

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

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

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

1 經(jīng)典的圓錐運(yùn)動(dòng)

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

(1)

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

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

(2)

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

(3)

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

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

(4)

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

(5)

三子樣算法公式如下:

(6)

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

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

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

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

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

(7)

記角增量為：

(8)

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

(9)

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

Φ(τ)≈Δθ(τ)

(10)

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

(11)

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

(12)

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

(13)

(14)

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

可知：

(15)

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

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

(16)

式中：

在圓錐運(yùn)動(dòng)條件下，捷聯(lián)慣導(dǎo)姿態(tài)更新過程會(huì)誘發(fā)數(shù)學(xué)平臺(tái)的嚴(yán)重漂移。因此，以圓錐運(yùn)動(dòng)作為測試條件，對(duì)旋轉(zhuǎn)矢量算法中的系數(shù)作優(yōu)化，根據(jù)圓錐運(yùn)動(dòng)可得到理論四元數(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)典圓錐運(yùn)動(dòng)作為姿態(tài)輸入，錐半角為1°，角頻率為8Hz，導(dǎo)航時(shí)間為1h。對(duì)比雙子樣法與新的等效旋轉(zhuǎn)矢量法的解算效果，如圖2～圖7所示，在姿態(tài)精度、速度精度、位置精度等方面，新的算法都優(yōu)于傳統(tǒng)的二子樣算法，并且計(jì)算量相當(dāng)。

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

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

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

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

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

圖7 位置誤差對(duì)比圖Fig.7 Contrast diagram of position error

5 結(jié)論

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