張晨陽 蔡 鵬 高大遠(yuǎn)

（海軍潛艇學(xué)院 青島 266000）

1 引言

水下自主航行器（AUV）目前擁有廣泛的應(yīng)用前景，在許多領(lǐng)域都可以看到它的身影，如何實(shí)現(xiàn)水下航行器的精確導(dǎo)航一直被諸多學(xué)者研究。水下單一的SINS 有自主性強(qiáng)，隱蔽性強(qiáng)等優(yōu)點(diǎn)，缺點(diǎn)是系統(tǒng)誤差會隨著時間而增加，長航時情況下會引起導(dǎo)航精度下降。以SINS 為主體，引入其他導(dǎo)航技術(shù)，進(jìn)行組合導(dǎo)航已經(jīng)逐漸成為一個重要的研究方向。SINS/DVL組合導(dǎo)航系統(tǒng)使用DVL測量的速度參數(shù)對SINS 的參數(shù)誤差進(jìn)行抑制，可以實(shí)現(xiàn)較高的導(dǎo)航精度，所以在水下導(dǎo)航時經(jīng)常使用SINS/DVL組合導(dǎo)航方法［1］。

針對SINS/DVL 組合導(dǎo)航系統(tǒng)，一般使用擴(kuò)展卡爾曼濾波（EKF）、無跡卡爾曼濾波（UKF）［2］等濾波方法。但是，EKF只選取非線性函數(shù)泰勒展開式的第一項，忽略了其他高階項，這樣會增加誤差，精度有限。而UKF 通過選取少量的采樣點(diǎn)來近似描述非線性函數(shù)，提高了濾波的精度，但是容易出現(xiàn)協(xié)方差矩陣負(fù)定的問題，而為了避免濾波算法發(fā)散，本文提出采用SRUKF［3］避免該問題，提高了濾波的可靠性。

當(dāng)模型不夠精確或者系統(tǒng)噪聲時變時，會導(dǎo)致濾波效果較差，得到的導(dǎo)航參數(shù)精度較低。為了提高濾波算法的魯棒性，本文提出一種交互無跡卡爾曼濾波算法，提高了濾波算法的自適應(yīng)性。最后，通過仿真實(shí)驗驗證提出算法的有效性。

2 SINS/DVL組合導(dǎo)航數(shù)學(xué)模型

2.1 狀態(tài)方程

SINS/DVL 組合導(dǎo)航誤差模型［4］分為SINS 誤差模型和DVL誤差模型。因為SINS垂直通道不穩(wěn)定并且DVL 的垂直速度測量誤差較大，因為僅考慮水平通道問題。

DVL 的測速誤差［5］主要包括，速度偏移誤差δVd，刻度系數(shù)誤差δKc，偏流角誤差δΔ，因為偏流角誤差對測速影響較小，不予考慮。所以，DVL 的誤差方程如下：

式中：βd，βK分別表示速度偏移誤差和刻度系數(shù)誤差的相關(guān)時間；wd和wK分別是二者的零均值高斯白噪聲。

選擇SINS的狀態(tài)變量為經(jīng)緯度誤差（δL,δλ），速度誤差（δvx,δvy），陀螺三個軸向的漂移（εx,εy,εz），平臺失準(zhǔn)角（α,β,γ）。

所以建立狀態(tài)方程為

2.2 觀測方程

SINS/DVL組合導(dǎo)航采用速度組合［6］的方式，把SINS 得到的速度與DVL 測量的速度作差，并把此差值作為觀測量輸入到濾波器中。

SINS 得到的速度在導(dǎo)航坐標(biāo)系下的投影為，即：

其中，Vn為速度真實(shí)值，為速度誤差值。

根據(jù)DVL的速度方程可得：

其中，DVL 在載體坐標(biāo)系下的真實(shí)速度為，Vn為導(dǎo)航坐標(biāo)系下速度的真實(shí)值，δKc為刻度系數(shù)誤差，δVDVL為隨機(jī)測量誤差，為姿態(tài)變換矩陣。

所以得到了系統(tǒng)的量測方程，即：

3 SRUKF濾波算法

由于在使用UKF 濾波算法的過程中，有可能出現(xiàn)協(xié)方差矩陣負(fù)定的情況，這樣會導(dǎo)致濾波算法不穩(wěn)定，得到的狀態(tài)估計結(jié)果也變得發(fā)散。SRUKF在UKF［7］的框架下引入了協(xié)方差矩陣的平方根形式替代原來的協(xié)方差矩陣參與整個運(yùn)算遞推過程，改善了濾波的準(zhǔn)確度和可靠性。算法流程［8］此處不再贅述。

4 IMM-SRUKF算法

交互多模型算法（IMM）［9］針對不同的子系統(tǒng)設(shè)置不同的濾波器，每個子系統(tǒng)有特定的濾波器，不同濾波器運(yùn)算時對應(yīng)著不同子系統(tǒng)狀態(tài)空間方程。其中，每一個子系統(tǒng)代表的運(yùn)行軌跡是不同的。

4.1 混合系統(tǒng)模型

假設(shè)混合系統(tǒng)一共有z個子系統(tǒng)，J1，J2,…,Jz分別代表子系統(tǒng)1 到子系統(tǒng)z，在初始時刻k=0時，子系統(tǒng)Jn發(fā)生概率：

μ1(0)，μ2(0),…,μz(0)滿足：

子系統(tǒng)之間的轉(zhuǎn)換概率：

m=1,2,…,z時，pm,n滿足：

混合系統(tǒng)由許多個非線性子系統(tǒng)構(gòu)成，當(dāng)處于子系統(tǒng)n時，系統(tǒng)一般表達(dá)式可描述為

x(k)?Rnx,u(k)?Rnu,y(k)?Rny,分別表示系統(tǒng)的狀態(tài)，輸入，輸出；An?Rnx×nx,Bn?Rnx×nx,Cn?Rnx×nx表示適宜維度矩陣；gn:Rnx→Rnx為非線性狀態(tài)函數(shù)；hn:Rnx→Rny為非線性觀測函數(shù)；w(k),v(k)表示過程噪聲和觀測噪聲，是相互獨(dú)立的高斯白噪聲向量，協(xié)方差矩陣分別為Q(k),R(k)。

4.2 IMM算法步驟

IMM的基本原理［10］是，在不同時刻每個濾波器對每個子系統(tǒng)進(jìn)行狀態(tài)估計，把不同子系統(tǒng)狀態(tài)估計結(jié)果融合得到最終的狀態(tài)估計，并在某一時刻把前一時刻的最終狀態(tài)估計結(jié)果作為初始值，帶入系統(tǒng)狀態(tài)方程中，然后針對每個子系統(tǒng)進(jìn)行濾波。算法最后更新某個子系統(tǒng)的發(fā)生概率，計算每個濾波器得到的狀態(tài)估計結(jié)果的權(quán)值，進(jìn)行求和得到最終的系統(tǒng)處于這一時刻的狀態(tài)估計結(jié)果。

具體算法步驟［11～12］如下：

1）混合概率

μm(k-1)表示k-1時刻子系統(tǒng)m發(fā)生的概率，pm,n為轉(zhuǎn)移概率矩陣

2）輸入交互

(k-1|k-1) 為子系統(tǒng)m的狀態(tài)估計，Pm(k-1|k-1) 為子系統(tǒng)m的協(xié)方差矩陣，(k-1|k-1) 是子系統(tǒng)n的狀態(tài)估計，P0n,x(k-1|k-1) 是子系統(tǒng)n的協(xié)方差矩陣。

3）調(diào)用濾波算法

把(k-1|k-1)，P0n,x(k-1|k-1) 作為初始狀態(tài)，帶入到SRUKF濾波算法中。

4）子系統(tǒng)概率更新

似然函數(shù)：

子系統(tǒng)概率：

Sn(k|k-1)，(k|k-1)，yn(k)分別是子系統(tǒng)n在k時刻的觀測協(xié)方差矩陣，觀測估計值和觀測值。

5）對不同子系統(tǒng)輸出結(jié)果加權(quán)求和

(k|k)，(k|k)分別是子系統(tǒng)n在k時刻的最優(yōu)狀態(tài)估計和協(xié)方差矩陣，(k|k)和P(k|k)分別是系統(tǒng)在k時刻最優(yōu)狀態(tài)估計和協(xié)方差矩陣，(k|k)是k時刻系統(tǒng)的觀測估計值。

5 仿真實(shí)驗

將提出的IMM-SRUKF 算法應(yīng)用到SINS/DVL組合導(dǎo)航系統(tǒng)中，進(jìn)行仿真驗證。設(shè)定SINS 系統(tǒng)的

性能參數(shù)如下：陀螺儀的常值漂移和隨機(jī)漂移均設(shè)為0.05°/h，加速度計的常值漂移和隨機(jī)漂移均設(shè)為50μg，初始失準(zhǔn)角：縱搖角：1.5°，橫搖角1.5°，航向角1.5° ；DVL 系統(tǒng)：速度偏移誤差δVd=0.10m/s，刻度系數(shù)誤差δKc=0.001，速度偏移誤差和刻度系數(shù)誤差的相關(guān)時間均為100s。

正常狀況下，SINS/DVL 的觀測噪聲沒有發(fā)生變化。采用SURKF 和IMM-SRUKF 算法得到速度誤差和位置誤差曲線如圖1 所示，從圖中可以看出采用SINS/DVL 組合導(dǎo)航提高了導(dǎo)航精度，抑制了速度誤差的增長，但是無法消除位置誤差的增長。當(dāng)系統(tǒng)觀測噪聲沒有變化時，采用兩種算法得到誤差估計結(jié)果接近。

圖1 正常情況下的速度誤差和位置誤差

觀測噪聲服從高斯白噪聲分布，觀測噪聲協(xié)方差矩陣隨著時間推移會發(fā)生變化，在4h～5h和14h～15h 時將觀測噪聲增大為原來的4 倍，之后恢復(fù)正常。采用兩種濾波算法誤差估計結(jié)果如圖2 所示，當(dāng)觀測噪聲發(fā)生突變時，采用兩種算法得到的速度誤差和位置誤差均發(fā)生了突變，導(dǎo)航的精度降低。但是單獨(dú)利用SRUKF 算法時不能有效地抑制噪聲變化帶來的誤差，并且累積了所產(chǎn)生的誤差。而采用本文所提出的IMM-SRUKF 濾波算法時，可以抑制噪聲帶來的誤差，并且產(chǎn)生的誤差并不會積累，可以保證導(dǎo)航的精度。

6 結(jié)語

本文研究了水下捷聯(lián)慣導(dǎo)系統(tǒng)（SINS）與多普勒計程儀（DVL）的組合導(dǎo)航問題，根據(jù)兩種系統(tǒng)的原理和誤差模型，建立了SINS/DVL 組合導(dǎo)航模型。在非線性系統(tǒng)濾波過程中，為了避免出現(xiàn)協(xié)方差矩陣負(fù)定的問題，提出利用SRUKF 算法，提高了濾波算法的穩(wěn)定性。為了解決觀測噪聲變化帶來的濾波精度下降問題，提出了IMM-SRUKF 濾波算法，提高了算法的魯棒性。仿真結(jié)果證明，提出的濾波算法精度較高，有一定的自適應(yīng)能力。