基于自適應(yīng)漸消Sage-Husa 擴展卡爾曼濾波的協(xié)同定位算法

周萌萌，張冰，趙強，潘夢婷，左思雨

江蘇科技大學(xué) 電子信息學(xué)院，江蘇 鎮(zhèn)江 212100

0 引 言

近年來，隨著人類對海洋開發(fā)的日漸深入，自主水下航行器（autonomous underwater vehicle，AUV）被廣泛應(yīng)用于海洋資源開發(fā)、水下探測等領(lǐng)域，多AUV 組網(wǎng)協(xié)同作業(yè)逐漸成為人們關(guān)注的焦點[1]。AUV 位置估計在協(xié)同導(dǎo)航中十分重要，但由于復(fù)雜水下環(huán)境帶來的不確定性，多AUV 協(xié)同系統(tǒng)的定位精度受限。采用基于狀態(tài)空間模型的濾波方法，能夠得到具有統(tǒng)計意義的最優(yōu)估計，位置信息精度較高。文獻[2]提出了一種基于迭代差值濾波（IDDF）的多AUV 協(xié)同導(dǎo)航算法，該算法將迭代濾波技術(shù)與插值濾波（DDF）算法相結(jié)合，但是無法解決量測異常時的魯棒性抑制問題。文獻[3]將凸優(yōu)化算法應(yīng)用于多AUV 協(xié)同導(dǎo)航系統(tǒng)中，然而僅僅適用于少量AUV，在AUV 數(shù)量增多的情況下，其有效性有待商榷。文獻[4]提出了一種基于雙移動信標(biāo)的定位方法，通過該算法論證了分離角度和分離間距對定位性能的影響，但所用方法復(fù)雜，在AUV 受到外界干擾時，定位性能無法確保最優(yōu)。文獻[5]提出了一種基于交互式模型的多AUV 協(xié)同導(dǎo)航定位算法，但未考慮該算法在復(fù)雜噪聲背景下的有效性。

AUV 協(xié)同導(dǎo)航是一個典型的非線性位置估計問題，眾多學(xué)者提出了針對AUV 協(xié)同導(dǎo)航的定位算法。例如擴展卡爾曼濾波算法[6]（extended Kalman filter, EKF）、無跡卡爾曼濾波算法[7]（unscented Kalman filter, UKF）。但是，這兩種算法都未考慮外部干擾帶來的問題，模型的不確定性導(dǎo)致卡爾曼濾波難以精確估計系統(tǒng)噪聲和量測噪聲。文獻[8]提出了自適應(yīng)擴展卡爾曼濾波（AEKF）算法，該算法可以對噪聲協(xié)方差矩陣進行自適應(yīng)調(diào)整，實時估計噪聲，但也存在諸多缺點，例如濾波易發(fā)散，影響定位精度；計算量過大，對操作系統(tǒng)硬件要求過高；過程噪聲矩陣的正定性[9-11]難以得到保證。

本文擬提出一種基于自適應(yīng)漸消Sage-Husa擴展卡爾曼濾波算法。首先，對Sage-Husa 量測噪聲協(xié)方差矩陣進行改進；其次，再通過漸消記憶指數(shù)加權(quán)得到新息的協(xié)方差估計值，以提高新近數(shù)據(jù)的利用率；然后，引入漸消因子來調(diào)整誤差協(xié)方差矩陣，以提高濾波器的精確性和自適應(yīng)性；最后，針對噪聲協(xié)方差不匹配以及噪聲協(xié)方差時變的情況，分別采用所提算法與EKF 算法、期望最大化擴展卡爾曼濾波算法[12]（EM-EKF）進行仿真對比。

1 多AUV 協(xié)同導(dǎo)航系統(tǒng)問題

多AUV 協(xié)同導(dǎo)航定位是基于水聲通信網(wǎng)絡(luò)的合作定位方法，通常是少量AUV 配備高精度系統(tǒng)，充當(dāng)領(lǐng)航AUV，跟隨AUV 配備低精度系統(tǒng)。在運行過程中，各AUV 利用所攜帶的導(dǎo)航傳感器，通過水聲通信技術(shù)獲取領(lǐng)航AUV 的位置信息以及二者之間的距離信息后，結(jié)合信息融合技術(shù)來提高自身定位精度，從而提高集群協(xié)同作業(yè)過程中整體定位精度[13-14]。

1.1 從AUV 的系統(tǒng)模型

從航行模式來看，協(xié)同導(dǎo)航系統(tǒng)結(jié)構(gòu)可分為主從式和并行式結(jié)構(gòu)，在此給出的是多領(lǐng)航者主從式AUV 協(xié)同定位模型。由于AUV 的深度位置信息可以由深度計精確測量，俯仰角在平穩(wěn)運動時的變化幅度較小，因此可將AUV 水下三維定位問題轉(zhuǎn)化為二維空間的定位問題。

式中：Vk為前向合成速度； ωk為偏航角速度；φk為航向角；t為采樣周期。由式（1）可知，協(xié)同導(dǎo)航系統(tǒng)為非線性結(jié)構(gòu)。其中，Vk和 ωk可由AUV 的本體傳感器測量，為系統(tǒng)的控制輸入：

式中：uk為 控制輸入模型；Vmk和 ωmk分 別 為AUV的速度以及偏航角速度的量測值；wvk和wωk均為相互獨立的零均值高斯白噪聲。再令系統(tǒng)噪聲協(xié)方差矩陣為Qk=diag[]， 其中和為高斯白噪聲wvk和wωk的方差矩陣。

從而，可將跟隨AUV 的二維運動學(xué)方程由式（1）簡寫為

式中：f(·)為 非線性模型的轉(zhuǎn)移函數(shù)；wk為過程噪聲。

1.2 協(xié)同定位量測模型

協(xié)同定位的量測模型是對二維平面上的領(lǐng)航AUV 和跟隨AUV 之間的位置關(guān)系建模。假設(shè)兩領(lǐng)航AUV 與跟隨AUV 的水平距離為R1k和R2k，則可分別表示為

將R1k和R2k作為量測值，得到量測方程：

式中，vk為量測噪聲。假設(shè)vk為零均值的相互獨立高斯白噪聲，則噪聲協(xié)方差矩陣表示為

2 Sage-Husa 自適應(yīng)擴展卡爾曼濾波算法

由式（3）和式（5）建立多AUV 協(xié)同定位運動模型和量測模型，用Sage-Husa 自適應(yīng)擴展卡爾曼濾波（Sage-Husa AEKF）算法濾波，具體原理如下。

非線性化系統(tǒng)方程為

式中，h(·)為非線性系統(tǒng)的量測函數(shù)。

殘差為

卡爾曼測量更新為

自適應(yīng)估計器為

由式（15）可知，前半項 (1-dk)利用遺忘因子b與前一時刻的相乘，初始值設(shè)為正定，因而相乘結(jié)果也為正定。后半項用來更新所要計算的，當(dāng)濾波過程中出現(xiàn)收斂趨勢時，誤差協(xié)方差Pk會 減小，且Hk為 一定值，則HkPk/k-1會逐漸減小，并趨于0，對量測噪聲協(xié)方差估計的影響可忽略不計。因此，將式（15）的量測噪聲協(xié)方差矩陣改為

3 漸消因子的選取

由式（10）可知，新息的理論協(xié)方差Ck為

式中，若Pk-1增 加，則Ck也會增加。同樣，在未知信息的影響下，Ck也會增加。在此情況下，Rk的增加亦會導(dǎo)致Ck的增加。因此，由開窗法[15]得到Ck的估計值為

由式（9）～式（13）和式（16）～式（18）可求得漸消因子 λk。

其中，tr(.)為 求跡運算。為 保證 λk≥1， 當(dāng)≤Nk時， λk=1。當(dāng)新息協(xié)方差的理論值偏離估計值，通過調(diào)整 λk來補償不斷增加的估計誤差。

由式（20）可知， λk的 求取需要依靠準(zhǔn)確的，但由式（19）計算得到的僅是對滑動窗口內(nèi)的數(shù)據(jù)進行算術(shù)平均，沒有利用新近數(shù)據(jù)。因此，接下來將通過一種漸消記憶指數(shù)加權(quán)濾波估計器，加大對新近量測數(shù)據(jù)的利用權(quán)重，突出新近數(shù)據(jù)的作用。以下按照負(fù)指數(shù)函數(shù)規(guī)律對加權(quán)系數(shù)βi賦值，則有

為滿足式（21）對加權(quán)系數(shù)序列的要求，令

則，此時

將式（24）代入式（19），替代原來的權(quán)系數(shù)1/(k-1) ， 得到的為

另，由式（23）又可知

將式（27）代入式（20），則可求出 λk。此算法簡單且易實現(xiàn)，也即由式（8）～式（16）、式（17）～式（20）以及式（27）構(gòu)成了本文所提自適應(yīng)漸消Sage-Husa擴展卡爾曼濾波(Saga-Husa AFEKF)算法。

4 仿真驗證

為驗證本文算法的有效性，依據(jù)文獻[16]設(shè)定相關(guān)條件數(shù)據(jù)（數(shù)據(jù)來源完整可靠），本節(jié)將分別在噪聲協(xié)方差不匹配時及噪聲協(xié)方差時變時，將本文算法與EKF，EM-EKF 算法進行仿真對比。

4.1 仿真條件

領(lǐng)航AUV 和跟隨AUV 的實際運行軌跡如圖1 所示，采樣周期t=1 s，仿真總時長為1 500 s。狀態(tài)值為=[0 0 60π/180]，量測噪聲協(xié)方差矩陣值為=diag([1]2)，系統(tǒng)噪聲協(xié)方差矩陣值為Qk=diag([0.1 0.1]2)， 誤 差 協(xié) 方 差 值 為Pk=diag([1 1 0.1]2)，=zeros(1,1)，遺 忘 因 子b=0.995。領(lǐng)航AUV1 起點（-500 m，0 m），領(lǐng)航AUV2 起點（500 m，0 m），均以初始航向角60°沿直線航行；跟隨AUV 起點（0，0），以初始航向角60°進行S 形運動。為驗證本文所提方法的有效性，采用定位誤差（LE）、均方根誤差（RMSE）和平均定位誤差（ALE）作為多AUV 協(xié)同定位的性能指標(biāo)：

圖1 領(lǐng)航AUV 和跟隨AUV 真實軌跡Fig. 1 True trajectory of leader AUV and follower AUV

4.2 噪聲協(xié)方差不匹配情況下仿真分析

在AUV 協(xié)同定位中，量測噪聲易受外界干擾，不穩(wěn)定性較大。過程噪聲主要由元器件的誤差確定，而元器件又時常受溫度、濕度等外部因素的影響發(fā)生改變。設(shè)噪聲協(xié)方差在800～1 000 s發(fā)生改變，即=diag([10]2)，Qk=diag([1 1]2)，以檢驗3 種算法在噪聲協(xié)方差不匹配時的自適應(yīng)能力。仿真結(jié)果如圖2 和圖3 所示（圖例中的DR代表航位推算）。表1 為3 種算法協(xié)同定位性能的比較結(jié)果。

表1 噪聲協(xié)方差不匹配時3 種算法的協(xié)同定位性能比較Table 1 Cooperative localization performance comparison of three algorithms with unmatched noise covariance

圖2 噪聲協(xié)方差不匹配時跟隨AUV 定位誤差比較Fig. 2 Comparison of localization errors of follower AUV with unmatched noise covariance

圖3 噪聲協(xié)方差不匹配時跟隨AUV 航行軌跡比較Fig. 3 Comparison of navigation trajectory of follower AUV with unmatched noise covariance

如圖2 和圖3 所示，DR 航推位置逐漸偏離真實軌跡，定位精度逐漸發(fā)散，原因是DR 航推信息僅由DVL 和羅經(jīng)提供相應(yīng)的速度和位置信息，定位誤差會逐漸累積。當(dāng)噪聲協(xié)方差矩陣嚴(yán)重不匹配時，3 種算法均趨于收斂。其中EKF 濾波波動較大，定位精度較低。EM-EKF 算法濾波結(jié)果優(yōu)于EKF 算法，是因為前者可以在線調(diào)整量測噪聲協(xié)方差矩陣。本文算法的定位精度優(yōu)于EMEKF 和EKF 算法，因為本文算法能夠自適應(yīng)調(diào)整噪聲協(xié)方差矩陣的統(tǒng)計特性，由更準(zhǔn)確的新息協(xié)方差估計值引入漸消因子修正預(yù)測誤差協(xié)方差矩陣，從而調(diào)節(jié)濾波增益，獲取更準(zhǔn)確的狀態(tài)向量值。本文算法具有更好的自適應(yīng)性和濾波精度。

由表1 可知，在量測噪聲協(xié)方差矩陣嚴(yán)重不匹配時，與EM-EKF 算法相比，本文算法的均方根誤差減少17.82%，平均誤差減少17.87%；與EKF算法相比，本文算法的均方根誤差減少24.48%，平均誤差減少22.54%。由此可知，在此情況下，本文算法具有更好的定位精度。

4.3 噪聲協(xié)方差時變情況下仿真分析

在AUV 協(xié)同定位中，噪聲受到外部環(huán)境的干擾后，其統(tǒng)計特性通常隨時間發(fā)生變化。量測噪聲和過程噪聲協(xié)方差不準(zhǔn)確時會降低濾波的魯棒性，導(dǎo)致定位精度降低。本文設(shè)定噪聲協(xié)方差的值在1 000～1 200 s 發(fā)生跳變，其統(tǒng)計特性變?yōu)?.1 節(jié)所述值的5 倍，即 5和 5Qk，以檢驗算法在噪聲協(xié)方差矩陣時變情況下的性能。圖4 和圖5 為仿真結(jié)果，表2 為3 種算法協(xié)同定位性能的比較結(jié)果。

圖4 噪聲協(xié)方差時變情況下跟隨AUV 定位誤差比較圖Fig. 4 Comparison on localization errors of follower AUV with the time-varying noise covariance

圖5 噪聲協(xié)方差時變情況下跟隨AUV 航行軌跡比較圖Fig. 5 Comparison on navigation trajectory of follower AUV with the time-varying noise covariance

表2 噪聲協(xié)方差時變情況下3 種算法的協(xié)同定位性能比較Table 2 Cooperative localization performance comparison of three algorithms with time-varying noise covariance

如圖4 和圖5 所示，DR 航推位置逐漸偏離真實軌跡，定位誤差隨著時間的變化不斷累積。當(dāng)噪聲協(xié)方差矩陣處于時變情況時， EKF 算法的定位誤差在1 000 s 左右出現(xiàn)劇烈波動，定位誤差逐漸增大，1 200 s 之后定位誤差開始降低，波動幅度較??； EM-EKF 算法的定位精度優(yōu)于EKF 算法，是因為該算法可以自適應(yīng)估計量測噪聲協(xié)方差矩陣。與另2 種算法相比，本文算法結(jié)果波動較為平緩，定位精度更高；原因是噪聲協(xié)方差處于時變狀態(tài)下時，本文算法通過自適應(yīng)估計器對量測噪聲協(xié)方差矩陣進行自適應(yīng)調(diào)整，實時調(diào)整增益矩陣，補償噪聲協(xié)方差矩陣新息的影響，提高了濾波算法的抗干擾性和定位精度。

由表2 可知，當(dāng)噪聲協(xié)方差處于時變情況下時，與EM-EKF 算法相比，本文算法的均方根誤差減少42.11%，平均誤差減少34.87%；與EKF算法相比，本文算法的均方根誤差減少51.23%，平均誤差減少46.90%。在此情況下，本文算法具有更好的定位精度。

5 結(jié) 語

由于水下環(huán)境復(fù)雜多變，易導(dǎo)致AUV 的噪聲統(tǒng)計特性發(fā)生改變，在此情況下，EKF 及UKF 等常規(guī)卡爾曼濾波算法難以解決此類問題，無法準(zhǔn)確估算AUV 在水下的位置狀態(tài)，不能有效提升AUV 定位精度?；诖?，本文提出了一種自適應(yīng)漸消Sage-Husa 擴展卡爾曼濾波方法，對量測噪聲協(xié)方差矩陣進行改進并引入漸消因子。仿真結(jié)果表明，當(dāng)噪聲協(xié)方差不匹配或者噪聲協(xié)方差處于時變情況下，本文所提算法可有效提高AUV定位精度，具有一定的自適應(yīng)性及抗干擾性。此外，本文只考慮了白噪聲對算法的影響，后續(xù)可進一步研究其他非高斯噪聲，如有色噪聲等因素對協(xié)同定位導(dǎo)航算法的影響。