1 引 言

慣性導(dǎo)航系統(tǒng)是水下自主潛器航行時可以依賴的自主式導(dǎo)航裝備，其隱蔽性好、不怕干擾，但原理上存在長時間工作的誤差積累，初始對準(zhǔn)精度誤差也將導(dǎo)致系統(tǒng)導(dǎo)航精度的下降[1]。多普勒速度計程儀使用方便，因其發(fā)射波束很窄，且能以很陡的角度發(fā)射到海底，隱蔽性和抗干擾性較好，測得的平均速度精度很高。綜合INS和DVL的優(yōu)缺點，采用優(yōu)勢互補(bǔ)的INS/DVL組合系統(tǒng)成為水下自主潛器自主導(dǎo)航的選擇。

通常，針對組合導(dǎo)航的系統(tǒng)模型本質(zhì)上具有的非線性特性，當(dāng)前多采用擴(kuò)展卡爾曼濾波(EKF)進(jìn)行處理。EKF是對非線性模型進(jìn)行近似線性化，其線性化誤差會降低模型的準(zhǔn)確性，隨時間延長，其估計精度難以保證，且其需要計算非線性模型的Jacobi矩陣，過程繁瑣。JULIER和UHLMAN等人基于多元函數(shù)代表點思想提出的UKF方法不是逼近非線性狀態(tài)函數(shù)，而是采用一種UT變換技術(shù)，即采用確定的樣本點(Sigma點)來完成狀態(tài)變量沿時間的傳播，可很好地克服EKF的上述缺點[2-5]。但當(dāng)系統(tǒng)維數(shù)變高時，非局部的采樣導(dǎo)致估計誤差變大，為此可以引入基于樣本點尺度伸縮的SUT變換方法的濾波器Scaled-UKF(SUKF)來解決這個問題[6-8]。

2 系統(tǒng)模型

2.1 慣性導(dǎo)航系統(tǒng)誤差模型

選取12維系統(tǒng)狀態(tài)矢量：XINS=[δφδλδveδvnφeφnφuenεeεnεu]T，分別為慣導(dǎo)系統(tǒng)輸出的緯度誤差、經(jīng)度誤差、東向和北向速度誤差、平臺東向、北向和方位失準(zhǔn)角、東向和北向加速度計零偏以及東向、北向和方位陀螺漂移；系統(tǒng)噪聲矢量為：WINS=[0 0wδvewδvnwφewφnwφu0 0 0 0 0]T，其中，wδve、wδvn、wφe、wφn、wφu均為零均值的高斯白噪聲，分別看作加速度計誤差和陀螺漂移中的白噪聲成分。

系統(tǒng)誤差模型選取半解析式當(dāng)?shù)厮街副逼脚_式慣導(dǎo)系統(tǒng)，東北天坐標(biāo)系。在上述條件下，慣導(dǎo)系統(tǒng)的非線性誤差狀態(tài)方程就簡化為：

(1)

其中，狀態(tài)函數(shù)f[XINS]定義為：

式中，φ為艦船所處緯度;g為當(dāng)?shù)刂亓铀俣?RM、RN分別為地球卯酉圈和子午圈半徑;ωie為地球自轉(zhuǎn)角速度。

2.2 DVL誤差模型

根據(jù)多普勒計程儀工作原理,它測量載體相對海底的速度和偏流角,測量誤差主要有速度偏移誤差δVd,偏流角誤差δΔ,刻度系數(shù)誤差Δkd。δVd和δΔ用一階馬爾可夫過程表示,Δkd為隨機(jī)常數(shù)。相應(yīng)誤差方程為[9]：

(2)

2.3 組合導(dǎo)航系統(tǒng)狀態(tài)方程量測方程

(3)

其中，狀態(tài)函數(shù)f1[X]為：

f1[X]=[f[XINS]T-βdδVd-βΔδΔ0]T

取慣導(dǎo)在東向和北向上的解算速度vINSe、vINSn與多普勒計程儀在東向和北向上的速度vDVLe、vDVLn之差作為觀測量，得到系統(tǒng)量測方程：

=HX+η

(4)

式中，量測噪聲η=[ηeηn]T為零均值高斯白噪聲;量測矩陣H為[10]：

式中，Kv表示考慮了偏流角的航跡向。

3 尺度變換方式的UKF(SUKF)濾波器設(shè)計

SUT是對標(biāo)準(zhǔn)UT變換的一種修正形式。這種方法能夠?qū)θ我夥植夹问降腟igma點進(jìn)行尺度變換，并且能保持隨機(jī)變量的均值和方差不變。下面給出SUT變換選點策略和均值方差計算方法。

(5)

(6)

由式(1)確定的點和式(2)確定的加權(quán)，則SUT按照如下方式計算統(tǒng)計特性：

(7)

(8)

由于SUKF與UKF在測量更新階段是相同的，區(qū)別僅在預(yù)測階段。關(guān)于詳細(xì)的UKF遞推計算公式，可參見文獻(xiàn)[2]和[3]。

4 仿真結(jié)果及結(jié)論

圖1 慣導(dǎo)東向速度誤差曲線

圖2 慣導(dǎo)北向速度誤差曲線

圖3 慣導(dǎo)緯度誤差曲線

圖4 慣導(dǎo)經(jīng)度誤差曲線

圖5 東向水平誤差角曲線

圖6 北向水平誤差角曲線

圖7 方位誤差角曲線

圖1～圖7分別為慣導(dǎo)速度,經(jīng)緯度與3個平臺誤差角變化曲線。其中點劃線為SUKF濾波結(jié)果，實線為未濾波情況。

由仿真可見，經(jīng)SUKF濾波后，慣導(dǎo)速度誤差有所減小，經(jīng)度誤差由快速發(fā)散變?yōu)檠杆贉p小，3個平臺誤差角的常值漂移消除。結(jié)果表明，采用SUKF濾波方法實現(xiàn)INS/DVL組合導(dǎo)航是可行的。

參考文獻(xiàn):

[1] 陳永冰，鐘斌.慣性導(dǎo)航原理[M].北京：國防工業(yè)出版社，2007.

[2] JULIER S,UHLMAN J K.A general method for approximating nonlinear transformations of probability distributions[R].Robotics Research Group,Department of Engineering Science,University of Oxford,1994.

[3] JULIER S,UHLMAN J K,etc.A new method for the nonlinear transformation of means and covariances in filters and estimators[J].IEEE Trans A C,2000,45(3):477-482.

[4] 李濤.非線性濾波方法在導(dǎo)航系統(tǒng)中的應(yīng)用研究[D].長沙：國防科技大學(xué)，2003.

[5] Zhang Chuan-bin,Tian Wei-feng,Jin Zhi-hua.Application of unscented kalman filter in state estimation for land vehicle navigation system[J].Journal of System Simulation,2005,17(6):1456-1458.

[6] JULIER S.The scaled unscented transformation[C].Proceedings of the American Control Conference.AK:Anchorage,2002.

[7] 張紅梅，鄧正隆.一種新的慣性導(dǎo)航初始對準(zhǔn)濾波方法[J].中國慣性技術(shù)學(xué)報，2005，13(1)：1-4.

[8] Zhang Hong-mei,Deng Zheng-long.A UKF-based attitude determination method for gyroless satellite[J].Journal of Systems Engineering and Electronics,2004,15(2):105-109.

[9] 熊崴，莊良杰，翁海娜，劉玉峰.INS/ESGM/Doppler組合導(dǎo)航系統(tǒng)中的Kalman濾波方法[J].中國慣性技術(shù)學(xué)報，2000，8(4)：10-19.

[10] 楊躍輪.智能導(dǎo)航系統(tǒng)研究[D].哈爾濱：哈爾濱工程大學(xué),2002.