基于改進(jìn)自適應(yīng)濾波的SINS/DVL組合導(dǎo)航算法研究

魏延輝，劉 靜，郝晟功

（哈爾濱工程大學(xué) 自動(dòng)化學(xué)院，哈爾濱150001）

采用捷聯(lián)慣性導(dǎo)航系統(tǒng)SINS（strapdown inertial navigation system）與多普勒計(jì)程儀DVL（doppler velocity log）組合能夠?qū)崿F(xiàn)完全自主導(dǎo)航、短時(shí)定位定向精度高、抗干擾能力強(qiáng)，是水下導(dǎo)航的最廣泛的方式[1]。但是，復(fù)雜的水下環(huán)境會(huì)導(dǎo)致DVL 測(cè)速失準(zhǔn)，甚至?xí)霈F(xiàn)信號(hào)短期中斷現(xiàn)象，影響SINS/DVL組合導(dǎo)航系統(tǒng)的精度。

針對(duì)DVL 出現(xiàn)的測(cè)速失準(zhǔn)現(xiàn)象，文中提出了一種改進(jìn)的自適應(yīng)濾波算法，引入χ2故障檢測(cè)，當(dāng)觀測(cè)量存在異常時(shí)，通過(guò)設(shè)置自適應(yīng)調(diào)節(jié)因子矩陣對(duì)先驗(yàn)狀態(tài)均方誤差矩陣進(jìn)行自適應(yīng)調(diào)節(jié)從而抑制誤差。最后，通過(guò)仿真試驗(yàn)驗(yàn)證了所提算法能有效抑制DVL 測(cè)速失準(zhǔn)時(shí)產(chǎn)生的誤差， 并通過(guò)Kalman濾波、Sage-Husa 自適應(yīng)濾波和改進(jìn)的自適應(yīng)濾波對(duì)比仿真試驗(yàn)，驗(yàn)證該方法的有效性。

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

SINS/DVL 組合導(dǎo)航系統(tǒng)的誤差模型分為SINS誤差模型和DVL 誤差模型2 部分[2]。由于SINS 的垂直通道不穩(wěn)定且DVL 的垂直速度測(cè)量誤差較大，故文中僅考慮水平通道問(wèn)題。

根據(jù)SINS 長(zhǎng)時(shí)間工作的誤差特性， 選擇狀態(tài)變 量為：經(jīng)度、緯度誤差（δL，δλ），速度誤差（δvx，δvy），平臺(tái)失準(zhǔn)角（α，β，γ），陀螺漂移（εx，εy，εz）。其誤差微分方程為

式中：R 為地球半徑；Ω 為地球自轉(zhuǎn)角速度；λ 為當(dāng)?shù)鼐暥?；Ax，Ay分別為加速度計(jì)測(cè)量值在當(dāng)?shù)氐乩碜鴺?biāo)軸上的分量；βg為陀螺漂移反相關(guān)時(shí)間常數(shù)；wg為白噪聲。

令DVL 速度偏移誤差和偏流角誤差用一階馬爾科夫過(guò)程來(lái)表示，刻度系數(shù)誤差為常值[3]，則對(duì)DVL 速度偏移誤差δvd， 刻度系數(shù)誤差δc 及偏流角誤差δΔ進(jìn)行建模，有

式中：wd和wΔ分別為二者的激勵(lì)白噪聲。因此建立的系統(tǒng)狀態(tài)方程為

其中

式中：（Fins（t））7×7為慣性導(dǎo)航系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣；（Fε（t））3×3為陀螺漂移反相關(guān)時(shí)間矩陣；（Fd（t））3×3為DVL 誤差反相關(guān)矩陣。

SINS/DVL 采用速度組合方式[4]。在導(dǎo)航坐標(biāo)系下， 將SINS 解算出的速度與DVL 測(cè)量得到的速度作差，將此差值作為Kalman 濾波器的量測(cè)值，因此得到系統(tǒng)的量測(cè)方程，即

其中

式中：Vsx，Vsy為慣導(dǎo)的測(cè)量速度在平臺(tái)坐標(biāo)系中的投影；Kd為考慮偏流角的航跡向[5]。

將式（4）和式（5）進(jìn)行離散化，得到SINS/DVL組合導(dǎo)航系統(tǒng)的Kalman 濾波器為

式中：Φk，k-1為系統(tǒng)從k-1 時(shí)刻到k 時(shí)刻的n×n 維狀態(tài)轉(zhuǎn)移矩陣；Hk為系統(tǒng)k 時(shí)刻的m×n 維觀測(cè)矩陣；Wk-1為模型噪聲，Vk為觀測(cè)噪聲， 二者為相互獨(dú)立的零均值高斯白噪聲。其方差分別為Qk和Rk，滿足以下統(tǒng)計(jì)特性：

其中

式中：qk，qj分別為Wk和Wj的數(shù)學(xué)期望；rk為Vk的數(shù)學(xué)期望；Qk和Rk為系統(tǒng)噪聲和觀測(cè)噪聲方差矩陣。

2 改進(jìn)的Sage-Husa 自適應(yīng)濾波算法

經(jīng)典Kalman 濾波算法在系統(tǒng)的結(jié)構(gòu)參數(shù)和噪聲統(tǒng)計(jì)特性都準(zhǔn)確已知的情況下才能獲得狀態(tài)的最優(yōu)估計(jì)； 而利用Sage-Husa 自適應(yīng)濾波對(duì)SINS/DVL 組合導(dǎo)航系統(tǒng)進(jìn)行信息融合，能不斷地利用量測(cè)數(shù)據(jù)估計(jì)和校正噪聲統(tǒng)計(jì)特性與模型參數(shù)，從而保證濾波始終處于正常狀態(tài)。

在Sage-Husa 自適應(yīng)濾波算法中， 每次濾波都要估計(jì)系統(tǒng)噪聲特性和觀測(cè)噪聲特性，若系統(tǒng)階數(shù)較高則會(huì)導(dǎo)致計(jì)算量大幅增加。然而，在實(shí)際系統(tǒng)中，如果濾波不出現(xiàn)異常，則無(wú)需對(duì)噪聲特性進(jìn)行調(diào)整，因此引入濾波收斂判據(jù)：

式中：s 為調(diào)節(jié)因子，反映判據(jù)的嚴(yán)格程度。式（7）成立，則說(shuō)明實(shí)際新息的方差大于理論值的s 倍，即濾波出現(xiàn)異常，量測(cè)噪聲變化較大，需要對(duì)其進(jìn)行調(diào)整。新息方差理論值為

則濾波收斂條件為

由于水下環(huán)境復(fù)雜，SINS/DVL 組合導(dǎo)航系統(tǒng)的量測(cè)噪聲統(tǒng)計(jì)特性受實(shí)際作業(yè)環(huán)境影響可能發(fā)生改變， 初始的先驗(yàn)值可能無(wú)法反映實(shí)際的噪聲情況，針對(duì)此情況，在Sage-Husa 自適應(yīng)濾波的基礎(chǔ)上進(jìn)行了改進(jìn)，利用量測(cè)信息實(shí)時(shí)估計(jì)量測(cè)噪聲的統(tǒng)計(jì)特性，從而精確估計(jì)系統(tǒng)的狀態(tài)變量。

結(jié)合濾波異常判斷條件， 用Sage-Husa 自適應(yīng)濾波估計(jì)量測(cè)噪聲協(xié)方差陣，則改進(jìn)后的算法為

其中

改進(jìn)的算法在新息計(jì)算后，增加了對(duì)濾波異常的判斷：當(dāng)濾波正常時(shí)，無(wú)需對(duì)濾波器做調(diào)整；當(dāng)濾波異常時(shí)，即量測(cè)噪聲突變，則利用新息對(duì)Rk進(jìn)行調(diào)整。濾波初始時(shí)刻的dk為最大值，此時(shí)是估計(jì)的Rk對(duì)當(dāng)前信息的依賴程度較高， 之后dk逐漸減小，新息對(duì)Rk的調(diào)整幅度逐漸減小，這樣增加了對(duì)量測(cè)噪聲變化的跟蹤能力。改進(jìn)后的算法只在噪聲統(tǒng)計(jì)特性發(fā)生變化時(shí)對(duì)Rk進(jìn)行調(diào)整，減輕了原算法中對(duì)Rk頻繁調(diào)整引起的計(jì)算負(fù)擔(dān)。

3 改進(jìn)的自適應(yīng)Kalman 濾波

相對(duì)于經(jīng)典Kalman 濾波算法， 自適應(yīng)Kalman濾波重新估計(jì)觀測(cè)噪聲協(xié)方差Rk，在一定程度上增加了計(jì)算量。在DVL 測(cè)量速度發(fā)生突變而導(dǎo)致觀測(cè)量失準(zhǔn)時(shí)， 自適應(yīng)Kalman 濾波的抑制效果并不理想且會(huì)累積誤差。文中在自適應(yīng)Kalman 濾波算法的基礎(chǔ)上進(jìn)行了改進(jìn)：在觀測(cè)量正常情況下，使用經(jīng)典Kalman 濾波算法：在觀測(cè)量失準(zhǔn)情況下，通過(guò)對(duì)先驗(yàn)均方誤差矩陣Pk，k-1進(jìn)行實(shí)時(shí)地自適應(yīng)調(diào)節(jié)從而使得新息協(xié)方差與實(shí)際新息方陣相符合。

SINS/DVL 組合導(dǎo)航系統(tǒng)的系統(tǒng)模型采用閉環(huán)反饋校正，系統(tǒng)模型誤差較小，可以忽略不計(jì)。因此，可以利用新息的統(tǒng)計(jì)特性來(lái)判斷觀測(cè)值是否異常。當(dāng)DVL 正常工作時(shí)，測(cè)量新息是零均值白噪聲序列；當(dāng)DVL 突然失準(zhǔn)時(shí)，測(cè)量新息不再是零均值的白噪聲。引入χ2故障檢測(cè)，將上述條件轉(zhuǎn)變?yōu)榧僭O(shè)檢驗(yàn)問(wèn)題，即

式中：H0為原假設(shè)， 表示DVL 正常工作情況；H1為備擇假設(shè)，表示DVL 失準(zhǔn)情況；Pvk為測(cè)量信息的協(xié)方差矩陣。

令假設(shè)檢驗(yàn)統(tǒng)計(jì)量為

其中Tk服從自由度為t 的χ2分布，即

式中λk為非中心化參數(shù)。取顯著性水平為α，則DVL 正常工作與否的邊界條件為

即觀測(cè)量是否正常的判別準(zhǔn)則為

當(dāng)Tk＞TD時(shí)，觀測(cè)量存在異常，為了抑制誤差，需在此時(shí)采取措施進(jìn)行調(diào)節(jié)，使得測(cè)量新息與實(shí)際相符，即滿足diag（vkvkT）=diag（Pvk）。

由于新息中攜帶觀測(cè)量信息，直接對(duì)其進(jìn)行調(diào)節(jié)會(huì)導(dǎo)致濾波失真，在此通過(guò)對(duì)先驗(yàn)狀態(tài)均方誤差矩陣Pk，k-1進(jìn)行調(diào)節(jié)使測(cè)量新息協(xié)方差與新息方陣相符。采用的方法是，通過(guò)增加系數(shù)矩陣λm實(shí)現(xiàn)對(duì)Pk，k-1的調(diào)節(jié)控制，具體步驟如下：

首先，設(shè)定一個(gè)調(diào)節(jié)因子λm，則最優(yōu)的先驗(yàn)均方誤差矩陣為

令diag（vkvkT）=diag（Pvk）

若λm＜1，即調(diào)節(jié)因子元素≯1 時(shí)，則調(diào)節(jié)因子失效，此時(shí)對(duì)先驗(yàn)狀態(tài)均方誤差矩陣Pk，k-1的調(diào)節(jié)是無(wú)效的，Pk，k-1保持不變。

綜上所述，調(diào)節(jié)因子λm的最終公式為

文中對(duì)自適應(yīng)濾波算法的改進(jìn)僅僅是調(diào)節(jié)了偏離實(shí)際的先驗(yàn)狀態(tài)均方誤差矩陣Pk，k-1，在本質(zhì)上它與經(jīng)典Kalman 濾波算法是一致的，都采用間接法中的閉環(huán)反饋機(jī)制。并且，改進(jìn)的自適應(yīng)濾波算法根據(jù)當(dāng)前時(shí)刻的新息調(diào)節(jié)Pk，k-1，相對(duì)于傳統(tǒng)Kalman濾波只增加了少量計(jì)算量，明顯提高了計(jì)算精度。

4 仿真試驗(yàn)與結(jié)果分析

4.1 仿真試驗(yàn)

為驗(yàn)證該算法的有效性， 在MatLab 環(huán)境下進(jìn)行SINS/DVL 組合導(dǎo)航系統(tǒng)仿真試驗(yàn)， 仿真試驗(yàn)參數(shù)設(shè)定見(jiàn)表1。

表1 仿真參數(shù)的設(shè)定Tab.1 Simulation parameter setting

4.2 仿真結(jié)果分析

為了更真實(shí)地模擬DVL 測(cè)速突然失準(zhǔn)的狀況，在5000～6000 s，12000～13000 s，23000～24000 s，30000～31000 s，將DVL 的噪聲增大為原來(lái)的10 倍，然后恢復(fù)正常。在此仿真條件下，采用經(jīng)典卡爾曼濾波、改進(jìn)的Sage-Husa 自適應(yīng)濾波和改進(jìn)的自適應(yīng)卡爾曼濾波算法分別對(duì)SINS/DVL 組合導(dǎo)航系統(tǒng)進(jìn)行仿真。速度誤差、位置誤差的仿真結(jié)果如圖1～圖4所示。

由圖1和圖2可見(jiàn)，增加10 倍噪聲后，速度和位置誤差都存在明顯跳變， 且曲線呈現(xiàn)發(fā)散趨勢(shì)，即DVL 速度信息失準(zhǔn)時(shí)，會(huì)導(dǎo)致組合導(dǎo)航的精度下降，甚至?xí)斐山M合導(dǎo)航失效。

圖1 正常情況下的速度誤差和位置誤差Fig.1 Velocity error and position error in normal conditions

圖2 十倍噪聲情況下的速度誤差和位置誤差Fig.2 Velocity error and position error in case of ten times noise

通過(guò)對(duì)比不同算法仿真結(jié)果， 可以發(fā)現(xiàn)，DVL噪聲突然增大為原來(lái)的10 倍，量測(cè)噪聲突變，采用3 種不同的濾波算法得到的速度和位置誤差均發(fā)生突變，組合導(dǎo)航的精度受到影響。

為了直觀地比較濾波性能，表2列出了3 種濾波算法各個(gè)導(dǎo)航參數(shù)的誤差絕對(duì)值的最終穩(wěn)定值、絕對(duì)值最大值。由表可知，經(jīng)典Kalman 濾波誤差較大，經(jīng)緯度誤差分別為89.334 m 及95.415 m；改進(jìn)的Sage-Husa 自適應(yīng)濾波精度次之， 經(jīng)緯度誤差分別為72.045 m 及73.125 m；改進(jìn)的自適應(yīng)濾波算法精度最高，經(jīng)緯度誤差分別為33.482 m 及43.385 m。

圖3 十倍噪聲情況下改進(jìn)Sage-Husa 濾波算法的速度誤差和位置誤差Fig.3 Velocity error and position error of improved Sage-Husa filtering algorithm in case of ten times noise

圖4 十倍噪聲情況下改進(jìn)自適應(yīng)濾波算法的速度誤差和位置誤差Fig.4 Velocity error and position error of improved adaptive filtering algorithm in case of ten times noise

表2 三種濾波算法的效果對(duì)比Tab.2 Comparison of three filtering algorithms

由圖3，圖4及表2可以看出，在仿真試驗(yàn)中由于DVL 噪聲突變，導(dǎo)致Kalman 濾波的精度降低，組合導(dǎo)航精度迅速降低，且有發(fā)散趨勢(shì)。相對(duì)于經(jīng)典Kalman 濾波算法，改進(jìn)的Sage-Husa 算法精度有所提高，但累積了異常時(shí)候產(chǎn)生的誤差。因此，采用改進(jìn)的Sage-Husa 算法，能夠稍微抑制DVL 測(cè)速失準(zhǔn)產(chǎn)生的誤差，但是效果有限。

而在此所提出的改進(jìn)的自適應(yīng)濾波算法，通過(guò)χ2故障檢測(cè)可以判斷出DVL 噪聲突變情況，并根據(jù)當(dāng)前時(shí)刻的新息調(diào)節(jié)先驗(yàn)狀態(tài)均方誤差矩陣Pk，k-1，能夠有效抑制誤差并且不積累DVL 噪聲異常時(shí)產(chǎn)生的誤差，既保證了精度，又能在一定程度上減少運(yùn)算量，抑制誤差效果比較明顯，且誤差波動(dòng)范圍更小，系統(tǒng)表現(xiàn)的更加穩(wěn)定。

5 結(jié)語(yǔ)

文中所提出的改進(jìn)的自適應(yīng)濾波算法，解決了SINS/DVL 組合導(dǎo)航系統(tǒng)由于受到復(fù)雜海況影響出現(xiàn)的測(cè)速突然失準(zhǔn)問(wèn)題。該算法通過(guò)引入χ2故障檢測(cè)判斷觀測(cè)量異常的情況，并設(shè)置自適應(yīng)調(diào)節(jié)因子矩陣對(duì)先驗(yàn)狀態(tài)均方誤差矩陣進(jìn)行自適應(yīng)調(diào)節(jié)從而抑制誤差。由仿真結(jié)果可見(jiàn)，改進(jìn)的自適應(yīng)濾波算法精度是最高的，改進(jìn)的Sage-Husa 自適應(yīng)濾波算法次之，經(jīng)典Kalman 濾波算法精度較低。改進(jìn)的自適應(yīng)濾波算法能夠有效提高系統(tǒng)組合導(dǎo)航定位精度， 具有一定的抗干擾能力和較好的工程適用性，從而算法的實(shí)際應(yīng)用提供很好的解決方案和參考。