周萌萌，張冰，趙強(qiáng)，潘夢(mèng)婷，左思雨

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

0 引 言

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

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

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

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

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

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

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

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

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

從而，可將跟隨AUV 的二維運(yùn)動(dòng)學(xué)方程由式（1）簡(jiǎn)寫(xiě)為

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

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

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

將R1k和R2k作為量測(cè)值，得到量測(cè)方程：

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

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

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

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

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

殘差為

卡爾曼測(cè)量更新為

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

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

3 漸消因子的選取

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

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

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

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

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

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

則，此時(shí)

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

另，由式（23）又可知

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

4 仿真驗(yàn)證

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

4.1 仿真條件

領(lǐng)航AUV 和跟隨AUV 的實(shí)際運(yùn)行軌跡如圖1 所示，采樣周期t=1 s，仿真總時(shí)長(zhǎng)為1 500 s。狀態(tài)值為=[0 0 60π/180]，量測(cè)噪聲協(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 起點(diǎn)（-500 m，0 m），領(lǐng)航AUV2 起點(diǎn)（500 m，0 m），均以初始航向角60°沿直線航行；跟隨AUV 起點(diǎn)（0，0），以初始航向角60°進(jìn)行S 形運(yùn)動(dòng)。為驗(yàn)證本文所提方法的有效性，采用定位誤差（LE）、均方根誤差（RMSE）和平均定位誤差（ALE）作為多AUV 協(xié)同定位的性能指標(biāo)：

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

5 結(jié) 語(yǔ)

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