朱倚嫻，周 玲

（1.南通大學(xué) 機(jī)械工程學(xué)院，江蘇 南通 226019；2.運(yùn)城學(xué)院 物理與電子工程系，山西 運(yùn)城 044000）

自主水下航行器（autonomous underwater vehicle，AUV）[1-4]具有作業(yè)深度大、隱蔽性強(qiáng)、可長時間在水下作業(yè)的優(yōu)點，從而在海洋環(huán)境探測、海洋資源開發(fā)等方面發(fā)揮著巨大作用。準(zhǔn)確可靠的導(dǎo)航系統(tǒng)是AUV 成功執(zhí)行任務(wù)并安全返航的重要保證。為提高定位精度，多傳感器組合導(dǎo)航是普遍采用的導(dǎo)航策略[5-8]。將多個導(dǎo)航傳感器的同種或非同種量測信息通過信息融合算法進(jìn)行有機(jī)融合可以獲得精度更高的導(dǎo)航結(jié)果?？柭鼮V波作為一種實時遞推、存儲量小的最優(yōu)估計算法，被廣泛應(yīng)用于組合導(dǎo)航信息融合。但標(biāo)準(zhǔn)卡爾曼濾波算法要求系統(tǒng)過程噪聲和量測噪聲均為方差已知的不相關(guān)高斯白噪聲序列[9-10]。對于AUV 導(dǎo)航系統(tǒng)，噪聲統(tǒng)計特性難以準(zhǔn)確獲得，且水下工作環(huán)境中的諸多不確定因素，如水流干擾、鹽度變化等均可能導(dǎo)致噪聲特性發(fā)生動態(tài)變化。為應(yīng)對這種情況，自適應(yīng)濾波應(yīng)運(yùn)而生。自適應(yīng)濾波是一種基于未知參數(shù)估計的濾波參數(shù)動態(tài)調(diào)整算法，其在處理不確定性信息過程時，具有更強(qiáng)的適應(yīng)性和更優(yōu)的濾波性能[11]。

現(xiàn)有的自適應(yīng)濾波主要有兩類：一類是強(qiáng)跟蹤濾波算法；一類是基于新息的自適應(yīng)估計算法。強(qiáng)跟蹤濾波通過實時調(diào)整狀態(tài)估計誤差的協(xié)方差陣和相應(yīng)的增益陣來保持對實際系統(tǒng)狀態(tài)的跟蹤。Gao 等[12]提出一種基于馬氏距離的強(qiáng)跟蹤無跡卡爾曼濾波，將新息向量的馬氏距離作為衡量系統(tǒng)模型偏離高斯系統(tǒng)的誤差程度，據(jù)此實現(xiàn)組合導(dǎo)航局部濾波器的自適應(yīng)調(diào)整，從而提高局部狀態(tài)估計的魯棒性。馬云峰[13]利用多個無須先驗知識的次優(yōu)漸消因子對不同的數(shù)據(jù)通道進(jìn)行漸消，改進(jìn)了強(qiáng)跟蹤卡爾曼濾波算法的跟蹤能力，并通過基于MSINS/GPS的組合導(dǎo)航仿真驗證了改進(jìn)算法的有效性。強(qiáng)跟蹤卡爾曼濾波器雖然結(jié)構(gòu)簡單，但其以犧牲濾波精度為代價來保證濾波收斂性?；谛孪⒌淖赃m應(yīng)估計算法根據(jù)量測信息的動態(tài)變化在線調(diào)整噪聲方差陣。周啟帆等[14]提出基于冗余量測的自適應(yīng)卡爾曼濾波算法，對系統(tǒng)冗余量測值的一階、二階差分序列進(jìn)行有效統(tǒng)計分析，據(jù)此估計量測噪聲統(tǒng)計特性并在線調(diào)整濾波參數(shù)。此外，一些學(xué)者考慮將兩種濾波算法相結(jié)合[15-17]，優(yōu)勢互補(bǔ)，形成新的自適應(yīng)濾波算法，但這類方法存在算法復(fù)雜、計算量較大的缺點。

針對AUV 導(dǎo)航系統(tǒng)噪聲統(tǒng)計特性不穩(wěn)定、難以準(zhǔn)確獲得的情況，本文提出一種基于模糊邏輯的自適應(yīng)濾波器。濾波新息能夠反映量測噪聲統(tǒng)計特性的變化，因此引入模糊邏輯設(shè)計調(diào)整規(guī)則，建立新息和調(diào)節(jié)變量間的對應(yīng)關(guān)系，從而實現(xiàn)對濾波量測噪聲方差陣的在線調(diào)整，使其逼近真實量測噪聲模型。通過仿真，驗證了本文所提濾波算法相較于標(biāo)準(zhǔn)Kalman 濾波算法、Sage-Husa 自適應(yīng)濾波算法，具有更高的估計精度。

1 AUV 組合導(dǎo)航系統(tǒng)

1.1 導(dǎo)航系統(tǒng)描述

捷聯(lián)慣性導(dǎo)航系統(tǒng)（strapdown inertial navigation system，SINS）可提供載體的姿態(tài)、速度、位置信息，導(dǎo)航信息全面且具有較高的器件可靠性，但其導(dǎo)航誤差會隨時間推移而逐漸增加[18-19]。因此引入磁羅經(jīng)（magnetic compass，MCP）、多普勒測速儀（doppler velocity log，DVL）、地形輔助導(dǎo)航（terrain aided navigation，TAN）等輔助導(dǎo)航傳感器構(gòu)成組合導(dǎo)航系統(tǒng)，以提高AUV 水下導(dǎo)航的可靠性和準(zhǔn)確性。為確保系統(tǒng)容錯性能，采用聯(lián)邦濾波的結(jié)構(gòu)實現(xiàn)組合導(dǎo)航。輔助導(dǎo)航傳感器MCP、DVL、TAN 分別提供航向角、速度和位置信息。SINS 作為參考系統(tǒng)，其量測信息與輔助導(dǎo)航傳感器量測信息作差，分別輸入3 個局部濾波器，經(jīng)局部濾波輸出的3 個局部狀態(tài)估計值將在主濾波中進(jìn)行融合，最終輸出全局狀態(tài)估計。

1.2 導(dǎo)航系統(tǒng)模型

定義東北天坐標(biāo)系為導(dǎo)航坐標(biāo)系（n 系），右前上坐標(biāo)系為載體坐標(biāo)系（b 系）。AUV 組合導(dǎo)航系統(tǒng)的狀態(tài)向量定義為

其中：φu、φe、φn為n 系天向、東向和北向失準(zhǔn)角；δVU、δVE、δVN為天向、東向和北向速度誤差；δL、δλ、δh 為緯度、經(jīng)度和高度誤差；εx、εy、εz為b 系x、y、z軸向的陀螺漂移；為b 系x、y、z 軸向的加速度計偏置。

局部濾波器的狀態(tài)方程為

其中：Xk-1、Xk分別為tk-1、tk時刻導(dǎo)航系統(tǒng)的誤差狀態(tài)向量；Φk，k-1為tk-1到tk時刻的狀態(tài)轉(zhuǎn)移矩陣；Wk為方差為Qk的零均值高斯白噪聲。

3 個局部濾波器的觀測方程分別為

其中：下標(biāo)SINS、MCP、DVL、TAN 分別表示相應(yīng)的輸出；H、VE、VN、L 和λ 分別表示航向、東向速度、北向速度、緯度和經(jīng)度；和分別是方差為和的量測噪聲向量；和分別為3 個局部濾波器的量測矩陣，具體為

主濾波器將局部狀態(tài)估計值進(jìn)行融合，融合算法為

2 模糊自適應(yīng)濾波

AUV導(dǎo)航系統(tǒng)的量測噪聲會受工作環(huán)境的影響而發(fā)生變化，從而難以獲得其準(zhǔn)確的統(tǒng)計特性。而標(biāo)準(zhǔn)Kalman 濾波需要準(zhǔn)確已知噪聲統(tǒng)計特性，且濾波精度與噪聲統(tǒng)計特性準(zhǔn)確度相關(guān)，因此，對于可靠性要求較高的AUV 組合導(dǎo)航系統(tǒng)，探索能夠在濾波過程中根據(jù)實際情況在線調(diào)整量測噪聲統(tǒng)計特性的自適應(yīng)濾波很有必要。

對于標(biāo)準(zhǔn)Kalman 濾波，量測信息更新后得到的濾波新息將用于計算濾波估計值。tk時刻的濾波新息為

由式（8）可見，新息即為實際量測值Zk和濾波估計值間的差值。Kalman 濾波通過時間更新得到基于前一刻的tk時刻狀態(tài)估計值，具體為

根據(jù)新息定義式（8）可知，其反映了量測信息統(tǒng)計特性的變化，因此考慮引入變量RORk來衡量新息rk的實際方差和理論方差間的偏離程度，具體為

其中：tr（·）表示計算矩陣的跡；Sk和Ck分別為濾波新息的理論方差和實際方差。

利用滑動平均窗計算新息集合的方差，并將此方差其近似作為新息的實際方差，具體為

其中M 為滑動平均窗的窗口大小，一般取10～30[20]。

新息rk的理論方差為

其中Pk，k-1為Kalman 濾波的一步預(yù)測誤差方差矩陣。

由式（10）可知，變量RORk值反映了Rk值與Rk-1值的偏離程度，因此利用變量RORk實現(xiàn)量測噪聲方差陣的在線自適應(yīng)調(diào)整，使其逼近實際值。引入調(diào)節(jié)系數(shù)βk實現(xiàn)量測噪聲方差陣Rk的自適應(yīng)調(diào)節(jié)，則需建立變量RORk和調(diào)節(jié)系數(shù)βk間的邏輯關(guān)系。本文選用模糊邏輯來實現(xiàn)調(diào)節(jié)規(guī)則。模糊邏輯包含模糊量化處理、模糊推理規(guī)則、解模糊判決3個步驟。

定義各模糊變量所屬的模糊語言集合即為模糊量化處理?，F(xiàn)定義變量RORk所屬的模糊集合為{小于，等于，大于}，記為{Less，Equal，More}；βk所屬的模糊集合為{減小，保持，增大}，記為{Reduce，Maintain，Enlarge}。定義變量RORk和調(diào)節(jié)系數(shù)βk的隸屬度函數(shù)如圖1 所示。

圖1 隸屬度函數(shù)Fig.1 Membership function

根據(jù)式（10）變量RORk的定義，若RORk＜1，則表示新息rk的實際方差小于理論方差，這種情況下，Rk值需要調(diào)節(jié)減小，反之亦反?；谏鲜稣{(diào)節(jié)規(guī)則，采用“if ...then ...”格式的模糊語句建立模糊規(guī)則，具體描述如下[21]：

因此，量測噪聲方差陣Rk可通過

實現(xiàn)在線更新，其中a 為正常數(shù)，是βk的放大系數(shù)。如果a ＞1，βk對Rk的調(diào)節(jié)將會增大，從而使得Rk更快地接近實際值；如果a ＜1，βk對Rk的調(diào)節(jié)將會減小，從而使得Rk更準(zhǔn)確地接近實際值。據(jù)此，定義a 為

測量噪聲方差陣Rk根據(jù)式（13）進(jìn)行在線調(diào)節(jié)實現(xiàn)對實際量測噪聲模型的跟蹤，從而有利于提高濾波的準(zhǔn)確性。

綜上，AUV 組合導(dǎo)航系統(tǒng)模糊自適應(yīng)濾波算法的流程如圖2 所示，其中i=1，2，3 表示第i 個局部濾波器。由SINS/MCP/DVL/TAN 組成的AUV 導(dǎo)航系統(tǒng)采用聯(lián)邦濾波的結(jié)構(gòu)，3 個局部濾波器均采用本文所設(shè)計的模糊自適應(yīng)濾波算法。首先進(jìn)行局部濾波的時間更新，再利用濾波新息經(jīng)過模糊推理得出量測噪聲方差陣的調(diào)節(jié)值，然后進(jìn)行局部濾波量測更新，最終3 個局部濾波器輸出的局部狀態(tài)估計值在主濾波器進(jìn)行融合，得出全局狀態(tài)估計值

圖2 模糊自適應(yīng)濾波算法流程示意圖Fig.2 Flow chart of the fuzzy adaptive filter algorithm

3 仿真實驗與分析

為驗證本文所提自適應(yīng)濾波算法的有效性，將模糊自適應(yīng)濾波和Sage-Husa 自適應(yīng)濾波、標(biāo)準(zhǔn)Kalman 濾波分別用于SINS/MCP/DVL/TAN 構(gòu)成的AUV 組合導(dǎo)航系統(tǒng)，采用Matlab 仿真進(jìn)行對比。仿真中各導(dǎo)航傳感器誤差參數(shù)設(shè)置如表1 所示。AUV初始位置為東經(jīng)118°，北緯32°，深度-380 m。SINS解算更新周期為10 ms，其他輔助導(dǎo)航系統(tǒng)的量測更新周期為1 s。仿真時間為25 min，AUV 進(jìn)行水平面的“Z”字形航行，航行期間進(jìn)行一系列不同機(jī)動，在200～220 s 區(qū)間內(nèi)加速運(yùn)動，0～200 s、220～420 s、620～820 s、1 020～1 280 s、1 300～1 500 s 區(qū)間內(nèi)勻速運(yùn)動，420～620 s 區(qū)間內(nèi)左拐彎機(jī)動，820～1 020 s區(qū)間內(nèi)右拐彎機(jī)動，1 280～1 300 s 區(qū)間內(nèi)減速機(jī)動。

表1 導(dǎo)航傳感器誤差參數(shù)Tab.1 Error parameters of the navigation sensors

仿真中，設(shè)初始量測噪聲為R0，拐彎機(jī)動狀態(tài)下量測噪聲會增大，因此設(shè)置兩個拐彎機(jī)動時間段420～620 s 和820～1 020 s 內(nèi)的量測噪聲為R=12R0。采用不同的濾波方法所得的速度和位置誤差結(jié)果如圖3 和圖4 所示。表2 列舉了相應(yīng)的詳細(xì)數(shù)據(jù)統(tǒng)計結(jié)果。

由圖3 和圖4 可見，在非拐彎機(jī)動過程中，采用模糊自適應(yīng)濾波、標(biāo)準(zhǔn)Kalman 濾波和Sage-Husa自適應(yīng)濾波3 種濾波方法具有相當(dāng)?shù)乃俣群臀恢霉烙嬀?。在拐彎過程中，量測噪聲發(fā)生突變，標(biāo)準(zhǔn)Kalman 濾波因為沒有對濾波模型做出相應(yīng)的修正而導(dǎo)致估計精度迅速下降；另外兩種自適應(yīng)濾波方法根據(jù)量測噪聲的實際變化對量測噪聲方差陣進(jìn)行了相應(yīng)的調(diào)整，從而使得濾波模型更接近于實際值，因此估計精度明顯優(yōu)于標(biāo)準(zhǔn)Kalman 濾波。具體地通過表2 可得，模糊自適應(yīng)濾波與Sage-Husa 自適應(yīng)濾波相比，東向、北向速度誤差分別減小約27.0%、42.0%；緯度、經(jīng)度誤差分別減小約49.4%、58.0%。即模糊自適應(yīng)濾波在應(yīng)對量測噪聲突變狀況時具有更高的速度位置估計精度。這得益于模糊自適應(yīng)濾波根據(jù)新息方差進(jìn)行的實時智能調(diào)整。綜上可得，本文所提模糊自適應(yīng)濾波算法在應(yīng)對量測噪聲變化的情形下，能有效提高濾波估計精度，從而有利于提高AUV 組合導(dǎo)航系統(tǒng)的定位精度。

圖3 采用不同濾波方法所得速度誤差曲線圖Fig.3 Velocity errors with different filtering methods

圖4 采用不同濾波方法所得位置誤差曲線圖Fig.4 Position errors with different filtering methods

表2 幾種濾波算法的導(dǎo)航結(jié)果統(tǒng)計值Tab.2 Statistics of the navigation results with different filtering methods

4 結(jié)論

本文針對AUV 導(dǎo)航系統(tǒng)易受復(fù)雜環(huán)境影響的情況，提出基于模糊邏輯的自適應(yīng)濾波器，通過建立濾波新息和調(diào)節(jié)變量間的模糊調(diào)整規(guī)則，使量測噪聲方差陣可根據(jù)實際濾波新息進(jìn)行在線調(diào)整。通過基于SINS/MCP/DVL/TAN 的組合導(dǎo)航系統(tǒng)仿真，驗證了所提算法的有效性，從而有利于提高組合導(dǎo)航系統(tǒng)的穩(wěn)定性和準(zhǔn)確性。但該算法與經(jīng)典Kalman濾波、Sage-Husa 自適應(yīng)濾波算法相比，由于需要計算滑動窗內(nèi)的新息方差，計算量和計算復(fù)雜度均有所增加，未來可進(jìn)一步研究算法優(yōu)化。