慣導(dǎo)系統(tǒng)陀螺漂移的ESO估計(jì)算法

夏衛(wèi)星,楊曉東

(海軍潛艇學(xué)院航海觀通系,山東 青島 266199)

0 引 言

慣導(dǎo)系統(tǒng)能夠完全自主地提供艦艇經(jīng)緯度位置、航行速度、航行姿態(tài)等信息,其不僅能夠保障艦艇的航行安全,而且為艦(艇)載武器系統(tǒng)提供基準(zhǔn)信息源。然而,由于陀螺漂移等其他慣性器件誤差的存在,使得慣導(dǎo)系統(tǒng)呈隨時(shí)間不斷積累的誤差特性,導(dǎo)航精度無法滿足實(shí)際要求。因此,必須對(duì)其進(jìn)行陀螺漂移估計(jì)(又稱綜合校正),通常采用兩點(diǎn)校、三點(diǎn)校和點(diǎn)點(diǎn)校等[1]。

“兩點(diǎn)校”對(duì)外部信息的要求較高,需要位置及方位信息;“三點(diǎn)校”需要以一定的時(shí)間間隔連續(xù)觀測(cè)3次位置信息,從而估計(jì)出陀螺漂移進(jìn)行補(bǔ)償并對(duì)位置誤差進(jìn)行修正[2-4]。文獻(xiàn)[5]指出,慣導(dǎo)系統(tǒng)進(jìn)行陀螺漂移估計(jì)時(shí),需處于水平阻尼狀態(tài),且保持勻速直航向航行,否則引入水平姿態(tài)角誤差,估計(jì)精度無法保證,甚至?xí)l(fā)散。文獻(xiàn)[6]以“兩點(diǎn)校”為例,從理論上探討分析了現(xiàn)行陀螺漂移估計(jì)算法的不足。文獻(xiàn)[2]提出了針對(duì)捷聯(lián)慣導(dǎo)系統(tǒng)的陀螺漂移估計(jì)補(bǔ)償技術(shù),該算法以“三點(diǎn)校”為基礎(chǔ),利用較長(zhǎng)時(shí)間間隔獲得的外部位置信息,建立其與陀螺漂移和方位誤差的關(guān)系式,再利用最小二乘法算出陀螺漂移并進(jìn)行方位誤差補(bǔ)償。

由文獻(xiàn)分析可見,“兩點(diǎn)校”算法耗時(shí)較長(zhǎng),在系統(tǒng)第一次重調(diào)后,陀螺漂移需間隔4～6 h才能得到精確補(bǔ)償,然而在相當(dāng)長(zhǎng)的校正時(shí)間間隔內(nèi),慣導(dǎo)系統(tǒng)的導(dǎo)航精度無法保證,“三點(diǎn)校”則需要的時(shí)間更長(zhǎng)。尤其對(duì)水下潛艇而言,其隱蔽性必然受到影響。由此可見,面對(duì)現(xiàn)代復(fù)雜的海戰(zhàn)場(chǎng),傳統(tǒng)綜合校正的估計(jì)精度和適用性無法保證。

為確保陀螺漂移估計(jì)精度,滿足艦(潛)艇平臺(tái)的實(shí)際應(yīng)用需求,從慣導(dǎo)系統(tǒng)姿態(tài)控制方程著手,基于擴(kuò)張狀態(tài)觀測(cè)器(extended state observer,ESO)提出了一種全新的陀螺漂移估計(jì)方案,能夠在短時(shí)間內(nèi)快速、無超調(diào)、高精度地估計(jì)陀螺漂移。

1 ESO的設(shè)計(jì)原理

ESO能夠?qū)崿F(xiàn)跟蹤被控對(duì)象的輸出,對(duì)各階狀態(tài)變量和擾動(dòng)進(jìn)行實(shí)時(shí)估計(jì)[7]。ESO不依賴于具體的系統(tǒng)模型。因此,ESO是通用且實(shí)用的狀態(tài)觀測(cè)器。ESO是一個(gè)動(dòng)態(tài)過程,僅僅利用被控對(duì)象的輸入和輸出信息,不需具體的模型描述[7-13]。

ESO與普通的狀態(tài)觀測(cè)器不同,其可有效觀測(cè)擴(kuò)張的狀態(tài)變量,實(shí)時(shí)估計(jì)系統(tǒng)中未建模部分,將非線性的控制對(duì)象變?yōu)槠胀ǖ姆e分串聯(lián)型控制對(duì)象[14-17]。其基本結(jié)構(gòu)如圖1所示。

圖1 n+1階ESO結(jié)構(gòu)Fig.1 n+1 order structure of ESO

基于上述原理,考慮非線性系統(tǒng)

(1)

其非線性狀態(tài)觀測(cè)器為

(2)

式中,e1為輸出誤差;z1為狀態(tài)變量;β01、β02為適當(dāng)?shù)膮?shù);b為控制變量u的系數(shù)。

(3)

則該擴(kuò)張系統(tǒng)的狀態(tài)觀測(cè)器[18-20]為

(4)

由上述文獻(xiàn)分析可見,ESO是一動(dòng)態(tài)過程,它只需利用系統(tǒng)的輸入—輸出信息,無需假定f(x1(t),x2(t),t,ω(t))是否連續(xù),是否已知,只要f(x1(t),x2(t),t,ω(t))有界,且參數(shù)b已知,選擇適當(dāng)參數(shù)β01,β02,β03,即可很好地實(shí)時(shí)估計(jì)式(3)的狀態(tài)變量x1(t),x2(t),以及被擴(kuò)張的狀態(tài)的實(shí)時(shí)作用量x3(t)=f(x1(t),x2(t))。

2 水平姿態(tài)角的ESO估計(jì)

慣導(dǎo)系統(tǒng)作為一非線性系統(tǒng)[1,21-24],若對(duì)其水平姿態(tài)角實(shí)現(xiàn)快速、無超調(diào)的精確跟蹤與估計(jì),必須選擇合適的非線性狀態(tài)跟蹤估計(jì)器。ESO作為典型的非線性估計(jì)器,其不要求事先知道水平姿態(tài)角的均值與方差,即可實(shí)現(xiàn)精確估計(jì)與跟蹤。

對(duì)垂向比力分量不作簡(jiǎn)化處理,考慮慣導(dǎo)系統(tǒng)速度誤差[21,25]:

(5)

式中,Ω為地球自轉(zhuǎn)角速度;vx、vy為東、北向速度;δvx、δvy為東、北向速度誤差;φ、λ為緯度和經(jīng)度;δφ、δλ為位置誤差;α、β為慣導(dǎo)水平姿態(tài)角;ΔAx、ΔAy為東向、北向加速度計(jì)零偏;R為地球半徑。

令速度誤差為系統(tǒng)量測(cè)量,則與式(5)對(duì)應(yīng)的系統(tǒng)量測(cè)方程為

(6)

(7)

對(duì)式(7)構(gòu)造ESO

(8)

式中,h、a、δ、B1、B2為ESO參數(shù);fal函數(shù)及ESO各參數(shù)定義詳見文獻(xiàn)[7]。

(9)

(10)

式中,B3、B4為ESO參數(shù),其余各參數(shù)定義同上。

(11)

可知,只需獲取精確的外界信號(hào)即可實(shí)現(xiàn)水平姿態(tài)角的準(zhǔn)確估計(jì)。

3 陀螺漂移的ESO估計(jì)

由于眾多影響因素的存在,導(dǎo)致慣導(dǎo)系統(tǒng)無法準(zhǔn)確無誤的模擬地理坐標(biāo)系,必然存在姿態(tài)偏差,其中,陀螺漂移為主要因素。

考慮慣導(dǎo)系統(tǒng)姿態(tài)控制與陀螺漂移的內(nèi)在聯(lián)系,得

(12)

式中,γ為方位姿態(tài)角,其余參數(shù)定義同上。

考慮

(13)

(14)

(15)

由此,基于ESO,即可運(yùn)用式(12)對(duì)陀螺漂移進(jìn)行實(shí)時(shí)估計(jì)，即

(16)

式(12)估計(jì)的穩(wěn)態(tài)誤差為

(17)

4 算法估計(jì)流程

圖2 陀螺漂移ESO估計(jì)算法流程Fig.2 ESO estimation algorithm flow of gyro drift

5 算法仿真與實(shí)驗(yàn)

仿真條件設(shè)定:

(1) 艦艇航行參數(shù)設(shè)定

航行速度v=10 kn;

航向H=60°;

初始位置φ=36°N、λ=122.2°E。

(2) 慣導(dǎo)系統(tǒng)參數(shù)設(shè)定

姿態(tài)初始誤差α=3′,β=3′,γ=5′;

加速度計(jì)零位偏置ΔAx=ΔAy=10-5g;

陀螺常值漂移:εx=0.002 (°)/h,εy=0.002 (°)/h,εz=0.002 (°)/h。

(3) 外界量測(cè)信息精度設(shè)定

速度誤差為σvrx=σvry=0.02 m/s;

航向誤差為10″。

(4) ESO參數(shù)選擇

(5) 數(shù)據(jù)采樣間隔t=0.1 s。

估計(jì)方案:慣導(dǎo)系統(tǒng)工作于水平阻尼狀態(tài),t=10h00m～10h04m,慣導(dǎo)系統(tǒng)開始陀螺漂移ESO估計(jì),接收位置、速度以及航向信息。采用ESO估計(jì)慣導(dǎo)系統(tǒng)水平姿態(tài)角,同時(shí)基于估計(jì)出的水平姿態(tài)角及航向量測(cè)信息估計(jì)水平姿態(tài)角的微分信號(hào),由式(16)快速求解慣導(dǎo)系統(tǒng)陀螺漂移,待估計(jì)結(jié)果穩(wěn)定后(t<1 min),t=10h05m補(bǔ)償系統(tǒng)陀螺漂移并對(duì)系統(tǒng)位置、航向重調(diào),估計(jì)補(bǔ)償結(jié)束,t=10h05m～36h00m觀察各狀態(tài)誤差發(fā)散情況。

由圖3可知,ESO可在較短時(shí)間內(nèi),以最快的速度估計(jì)出平臺(tái)水平失調(diào)角,具有較好的響應(yīng)特性,估計(jì)結(jié)果穩(wěn)態(tài)精度高,短時(shí)間內(nèi)即可達(dá)到穩(wěn)態(tài)。

圖3 水平姿態(tài)角的ESO估計(jì)Fig.3 ESO estimation of horizontal attitude angle

圖4～圖6為慣導(dǎo)系統(tǒng)陀螺漂移的ESO估計(jì)曲線,圖中粗實(shí)線為設(shè)定值,細(xì)實(shí)線為ESO估計(jì)值。基于水平姿態(tài)角的估計(jì)值,短時(shí)間內(nèi)(t<1min)可實(shí)現(xiàn)陀螺漂移的準(zhǔn)確估計(jì)。相對(duì)傳統(tǒng)綜合校正,在保證陀螺漂移估計(jì)精度的前提下,時(shí)間大幅縮短。對(duì)軍用艦艇,尤其對(duì)潛艇而言,能夠有效降低暴露概率,提高潛艇隱蔽性。

圖4 東向陀螺漂移的ESO估計(jì)Fig.4 ESO estimation of east gyro drift

圖5 北向陀螺漂移的ESO估計(jì)Fig.5 ESO estimation of north gyro drift

圖6 方位陀螺漂移的ESO估計(jì)Fig.6 ESO estimation of position gyro drift

基于設(shè)定的仿真條件,采用式(17)對(duì)陀螺漂移ESO估計(jì)的穩(wěn)態(tài)誤差進(jìn)行量化分析,得

由定量分析結(jié)果可知,ESO估計(jì)算法解得的陀螺漂移估計(jì)穩(wěn)態(tài)誤差量級(jí)為10-4(°)/h,該穩(wěn)態(tài)誤差對(duì)陀螺漂移的估計(jì)影響較小。由公式(17)分析可見,該穩(wěn)態(tài)誤差量級(jí)取決于加速度計(jì)零位偏置ΔAx、ΔAy的精度限制。

圖7為實(shí)施陀螺漂移ESO估計(jì)方案后,慣導(dǎo)系統(tǒng)各狀態(tài)誤差的發(fā)散,圖7中,實(shí)線為ESO估計(jì)補(bǔ)償結(jié)果,虛線為未補(bǔ)償。由圖7可見,由于對(duì)陀螺漂移進(jìn)行了高精度的估計(jì)和補(bǔ)償,系統(tǒng)導(dǎo)航誤差明顯降低,表明設(shè)計(jì)的陀螺漂移ESO估計(jì)方案有效。

圖7 估計(jì)補(bǔ)償后的慣導(dǎo)系統(tǒng)誤差Fig.7 Inertial navigation system system error after estimated compensation

6 結(jié) 論

傳統(tǒng)的慣導(dǎo)系統(tǒng)陀螺漂移估計(jì)方法(兩點(diǎn)校)耗時(shí)長(zhǎng)(約4～6 h),三點(diǎn)校則需更長(zhǎng)時(shí)間,且校正精度受航行狀態(tài)影響,對(duì)于軍用艦艇,尤其是潛艇而言,其適用性不強(qiáng)。基于此,論文提出了陀螺漂移的ESO估計(jì)方法,從慣導(dǎo)系統(tǒng)控制方程著手,針對(duì)其內(nèi)在聯(lián)系,基于ESO實(shí)現(xiàn)了陀螺漂移快速估計(jì)。該算法不受艦艇航行狀態(tài)約束,所需時(shí)間短、估計(jì)精度高,對(duì)于軍用艦艇慣導(dǎo)系統(tǒng)具有較強(qiáng)的實(shí)際應(yīng)用價(jià)值。