基于無序量測粒子濾波的無人船導(dǎo)航

古毅杰， 張闖， 康凱航

(大連海事大學(xué)航海學(xué)院， 遼寧 大連 116026)

0 引 言

無人船是能夠在海洋、河流等環(huán)境中自主完成任務(wù)的平臺(tái)，是自動(dòng)駕駛技術(shù)在水面環(huán)境中應(yīng)用的最主要體現(xiàn)[1]。全球?qū)Ш叫l(wèi)星系統(tǒng)(global navigation satellite system，GNSS)作為現(xiàn)今船舶定位導(dǎo)航的關(guān)鍵手段之一，能夠?yàn)闊o人船提供連續(xù)實(shí)時(shí)的導(dǎo)航解，但導(dǎo)航解的精度容易因衛(wèi)星信號(hào)衰減等因素的影響而降低。船舶定位一般通過濾波技術(shù)遞歸計(jì)算后驗(yàn)概率密度，然后將其作為貝葉斯估計(jì)的近似值來解決。在實(shí)踐中，經(jīng)常會(huì)出現(xiàn)偏離真實(shí)狀態(tài)的錯(cuò)誤估計(jì)，如果無法恢復(fù)到正確的軌跡，則稱這樣的偏離為發(fā)散。眾所周知，發(fā)散或者不可靠的估計(jì)是由建模誤差引起的，包括非線性、偏差或者缺乏統(tǒng)計(jì)模型知識(shí)[2-4]。為解決非線性系統(tǒng)的估計(jì)問題，眾多學(xué)者提出一些改進(jìn)算法，如擴(kuò)展卡爾曼濾波(extended Kalman filter，EKF)[5]、無跡卡爾曼濾波[6]和容積卡爾曼濾波[7]。這些方法提高了卡爾曼濾波算法求解非線性估計(jì)問題的能力，但都局限于高斯噪聲的假設(shè)，因此對(duì)非高斯噪聲的估計(jì)仍需進(jìn)一步改進(jìn)。粒子濾波(particle filter，PF)在非線性和非高斯濾波應(yīng)用中被驗(yàn)證是有效的[8]，它是一種基于蒙特卡羅方法和遞推貝葉斯估計(jì)的統(tǒng)計(jì)濾波方法。它對(duì)過程噪聲和量測噪聲沒有任何限制。PF的本質(zhì)是用由有限個(gè)加權(quán)樣本或粒子組成的離散隨機(jī)樣本近似表示概率分布。理論上，當(dāng)樣本量足夠大時(shí)，PF可以逼近任何形式的狀態(tài)變量的后驗(yàn)概率密度函數(shù)，因此，適用于任何可用狀態(tài)空間模型表示的非線性非高斯隨機(jī)系統(tǒng)。

常用的PF算法還存在一些嚴(yán)重的問題。當(dāng)似然函數(shù)位于系統(tǒng)狀態(tài)轉(zhuǎn)移概率密度的尾部時(shí)，從重要性概率密度得到的樣本與從真實(shí)后驗(yàn)概率密度得到的樣本相差很大，從而導(dǎo)致粒子權(quán)重集中在少數(shù)粒子上，無法表示實(shí)際的后驗(yàn)概率分布。這稱為PF的退化問題。解決此退化問題的主要方法是選取重要度較高的后驗(yàn)概率密度函數(shù)以及采用重采樣方法。文獻(xiàn)[9]利用無跡粒子濾波器(unscented particle filter，UPF)得到PF的重要性采樣密度。常用的重采樣技術(shù)包括多項(xiàng)式重采樣、系統(tǒng)重采樣和剩余重采樣[10-11]。采用重采樣方法雖然可以減少退化現(xiàn)象的發(fā)生，但會(huì)導(dǎo)致粒子多樣性的損失。為保證粒子的多樣性，學(xué)者們提出輔助變量PF[12]、重采樣移動(dòng)算法[13]、正則化PF[14]和基于智能優(yōu)化的PF[15]。

PF也存在發(fā)散現(xiàn)象。作為發(fā)散監(jiān)測的手段，Kullback-Liebler散度、非標(biāo)準(zhǔn)化權(quán)重和新息序列[16]被用來將觀測結(jié)果與粒子云進(jìn)行比較。如果檢測到發(fā)散，通常利用最后一次可靠估計(jì)重新啟動(dòng)濾波器。特別是在含有坡度曲線標(biāo)志變化拐點(diǎn)的非線性量測模型中，量測的模糊性被認(rèn)為是導(dǎo)致濾波器退化和發(fā)散的重要原因，而且模糊量測更新會(huì)導(dǎo)致后驗(yàn)密度的發(fā)散。文獻(xiàn)[17]研究了基于有序?yàn)V波框架的H∞濾波實(shí)現(xiàn)無序量測的更新，能夠解決單步延遲的無序量測問題，但是無法解決非線性條件下的無序量測問題。文獻(xiàn)[18]提出一種基于無序量測更新估計(jì)的信息濾波算法，通過對(duì)選擇閾值與固定閾值的比較，丟棄無用的無序量測，從而在不影響濾波精度的條件下減少計(jì)算量。文獻(xiàn)[19]針對(duì)非線性條件下的無序量測問題，討論了快速邊緣PF算法，采用無序處理思想分別估計(jì)線性和非線性目標(biāo)運(yùn)動(dòng)狀態(tài)矢量。PF中的多重模態(tài)分布使用混合粒子濾波(mixture particle filter，MPF)處理全局定位問題[20]，并且在初始位置不確定性較大時(shí)采用MPF。本文主要解決模糊量測更新的問題，模糊量測更新定義為導(dǎo)致粒子協(xié)方差增加的量測更新，進(jìn)而模糊量測更新會(huì)導(dǎo)致粒子分散并提供不可靠的估計(jì)解。

本文主要研究取決于粒子先驗(yàn)分布量測模糊度的量測量。為獲得精確的位置估計(jì)值，提出一種基于無序量測的PF(out-of-sequence measurement based PF, OOSMPF)算法，當(dāng)量測量模糊或不足時(shí)跳過該時(shí)間步長的量測更新，而后用無序量測解彌補(bǔ)跳過的量測更新。最終，將本文提出的算法在無人船航行條件下與EKF、PF、MPF等方法進(jìn)行比較，通過均方根誤差估計(jì)證明本文提出的方法具有更優(yōu)越的性能。

1 PF及模糊量測更新

1.1 PF

非線性隨機(jī)系統(tǒng)可以用離散過程模型和量測模型[18-19]表示：

(1)

式中：xt和zt分別為t時(shí)刻的系統(tǒng)狀態(tài)矢量和量測矢量；f(·)和h(·)分別為狀態(tài)轉(zhuǎn)移函數(shù)和觀測函數(shù)；bt和vt分別為t時(shí)刻的過程噪聲矢量和量測噪聲矢量。

基于貝葉斯理論觀點(diǎn)，狀態(tài)估計(jì)問題是基于后驗(yàn)知識(shí)遞歸計(jì)算出當(dāng)前時(shí)刻(t時(shí)刻)的狀態(tài)后驗(yàn)分布p(xt|zi,i=1, 2,…,t)的。預(yù)測過程是利用先驗(yàn)分布p(xt|xt-1)預(yù)測系統(tǒng)模型的狀態(tài)的，而更新過程中使用最新的量測量校正先驗(yàn)分布，從而得到后驗(yàn)分布。首先，假設(shè)系統(tǒng)的狀態(tài)轉(zhuǎn)移服從一階馬爾科夫模型，即當(dāng)前時(shí)刻的狀態(tài)xt僅與t-1時(shí)刻的狀態(tài)xt-1相關(guān)。根據(jù)貝葉斯公式，可以推導(dǎo)出式(2)為預(yù)測，式(3)為更新[19]。

p(xt|zi,i=1,2,…,t-1)=

(2)

p(xt|zi,i=1,2,…,t)=

(3)

式中：p(zt|xt)為t時(shí)刻給定x的數(shù)據(jù)后的似然函數(shù)。

對(duì)于一般的非線性或非高斯系統(tǒng)，很難從上式中獲得后驗(yàn)分布的解。因此，引入蒙特卡羅采樣來解決這一問題。PF將后驗(yàn)分布近似為一組隨機(jī)采樣的粒子及其相關(guān)權(quán)重的集合，并且將計(jì)算出的數(shù)學(xué)期望作為狀態(tài)估計(jì)。序列重要性重采樣(sequential importance resampling，SIR)通常用于PF算法中，以克服PF的退化問題。根據(jù)文獻(xiàn)[19]，后驗(yàn)分布可以通過狄拉克混合近似技術(shù)近似表示為

(4)

式中：δ為狄拉克函數(shù)；N為粒子數(shù)；wt,i為t時(shí)刻第i個(gè)粒子的重要性權(quán)值；xt,i為t時(shí)刻第i個(gè)粒子的狀態(tài)。引入重要性分布函數(shù)q(xt,i|xt-1,i,zt)，則重要性權(quán)值可以表示為

(5)

(6)

1.2 模糊量測更新

在量測更新階段之后，如果估計(jì)量測值與真實(shí)狀態(tài)量測值在相同條件下存在模糊點(diǎn)，則粒子協(xié)方差會(huì)增加。這種現(xiàn)象通常發(fā)生在式(1)中非線性函數(shù)h(xt)的拐點(diǎn)附近[22]。因?yàn)楣拯c(diǎn)附近的函數(shù)值在另一側(cè)具有相同的對(duì)應(yīng)值，所以量測更新可能導(dǎo)致分散的后驗(yàn)分布。

根據(jù)量測模型的形狀和誤差統(tǒng)計(jì)可知，如果量測模型在拐點(diǎn)附近更陡或具有更小的誤差方差，則圖4中的離散后驗(yàn)分布具有更明顯的多模態(tài)分布。PF中的多模態(tài)分布可以使用混合表示法來處理，其中多模態(tài)分布占用的比例較大。Cramer-Rao界理論[23]表明：因?yàn)榱繙y對(duì)Fisher信息矩陣是半正定的，所以協(xié)方差的下界不會(huì)因量測更新而增加；只有當(dāng)量測模型的觀測函數(shù)h(x)的梯度為零時(shí)，量測才為零，此時(shí)最優(yōu)濾波的后驗(yàn)協(xié)方差小于或等于先驗(yàn)協(xié)方差。在估計(jì)狀態(tài)時(shí)并沒有較好地結(jié)合來自量測的附加信息，為得到最優(yōu)濾波器，最好利用模糊度來求取真實(shí)狀態(tài)信息。本文將模糊量測更新定義為導(dǎo)致粒子協(xié)方差增加的量測更新。模糊量測更新會(huì)引起較嚴(yán)重的粒子分散，因此估計(jì)的可信度較低，并且可能會(huì)對(duì)遞歸濾波器的后續(xù)步驟造成連鎖效應(yīng)。因此，提出基于無序量測的模糊更新來解決此問題。

2 基于無序量測的模糊更新

根據(jù)上文分析可知，模糊量測更新會(huì)導(dǎo)致分散的后驗(yàn)分布以及不可靠的位置估計(jì)。本算法的目的是，當(dāng)發(fā)生模糊量測更新時(shí)，獲得更優(yōu)的當(dāng)前估計(jì)置信度，從而獲得更好的濾波性能。該算法的核心思想是，在先驗(yàn)分布適合量測更新的條件下，保存模糊量測并后續(xù)使用，本質(zhì)上等同于量測到達(dá)延遲的情況。圖5給出了延遲量測超時(shí)序列的量測關(guān)系。量測zt-1對(duì)應(yīng)于t-1時(shí)刻的系統(tǒng)狀態(tài)xt-1。假設(shè)t-1時(shí)刻zt-1的量測更新是模糊的，在zt的量測更新之后該濾波器使用zt-1，如同zt-1正好在t時(shí)刻zt之后到達(dá)。

以SIR-PF算法為基礎(chǔ)，提出無序量測算法：

(7)

其中

(8)

式中：Qa,b表示從a時(shí)刻到b時(shí)刻的協(xié)方差矩陣；Ft,a和Qt,a分別表示從a時(shí)刻到t時(shí)刻的狀態(tài)轉(zhuǎn)移矩陣和相應(yīng)的協(xié)方差矩陣；Fa,s和Qa,s分別表示從a時(shí)刻到s時(shí)刻的狀態(tài)轉(zhuǎn)移矩陣和相應(yīng)的協(xié)方差矩陣；u為一個(gè)白噪聲向量。新的重要性權(quán)重表示為ωr,i(i=1,2，…,N)(r≤t，其中r為正整數(shù))，此權(quán)重是通過當(dāng)前粒子狀態(tài)值和非歸一化權(quán)重計(jì)算得到的，其中包含當(dāng)前的量測zt。根據(jù)新的權(quán)重對(duì)產(chǎn)生的粒子進(jìn)行重采樣，以獲得包含無序量測信息的粒子狀態(tài)值xr,i(i=1,2，…,N)。

(3)根據(jù)式(5)計(jì)算權(quán)重，并歸一化權(quán)重。

(4)計(jì)算累積分布函數(shù)的協(xié)方差。

在初始化濾波后，以離散時(shí)間形式迭代地執(zhí)行SIR。如果后驗(yàn)粒子協(xié)方差矩陣的行列式|st,2|大于相應(yīng)的先驗(yàn)粒子協(xié)方差矩陣的行列式|st,1|，則認(rèn)為更新是一個(gè)模糊量測更新，將先驗(yàn)粒子作為時(shí)間步長的解，即只執(zhí)行預(yù)測步驟。如果更新被認(rèn)為是模糊的，則跳過量測更新，跳過的量測值存儲(chǔ)在集合A中；否則，認(rèn)為此更新為一個(gè)正確的更新，并且檢測集合A是否為空。如果集合A非空，則執(zhí)行無序量測，并按先進(jìn)先出的順序利用每個(gè)量測值。在無序量測模糊量測更新之后，為了生成xr,i需要權(quán)重被重采樣。如果新獲得粒子的協(xié)方差矩陣的行列式小于當(dāng)前的后驗(yàn)粒子的協(xié)方差矩陣的行列式|st,2|，算法以xr,i作為解。而后，將上一次成功更新的粒子狀態(tài)值xt,i(i=1,2，…,N)存儲(chǔ)到xs,i(i=1,2，…,N)中，用作無序量測的輸入。與標(biāo)準(zhǔn)PF算法相比，該算法需要額外的計(jì)算來處理模糊量測更新，并且通過比較先驗(yàn)密度和后驗(yàn)密度的大小來確定模糊量測更新，后驗(yàn)協(xié)方差可以在量測更新之后求取。

3 實(shí)船實(shí)驗(yàn)結(jié)果分析

在GPS失鎖期間，利用無人船上的測深儀給出海底深度測量值，并將其與電子海圖顯示與信息系統(tǒng)數(shù)據(jù)進(jìn)行比較，以估計(jì)出無人船的位置。如圖6所示，船舶測深儀是通過測量超聲波信號(hào)自水底反射至接收的時(shí)間間隔來確定水深的一種儀器設(shè)備，測深儀提供的深度量測模型為

zt=h(xt)+vt

(9)

式中：xt表示無人船位置，是一個(gè)由經(jīng)緯度表示的二維向量；h(xt)表示在xt處估計(jì)的深度量測信息；vt表示附加的量測噪聲。無人船運(yùn)動(dòng)模型由馬爾科夫過程建模：

xt+1=xt+ut+bt

(10)

式中：ut和bt分別表示相對(duì)運(yùn)動(dòng)信息和附加的過程噪聲。船舶的相對(duì)運(yùn)動(dòng)信息由慣性導(dǎo)航系統(tǒng)(inertial navigation system，INS)估計(jì)。

圖6 無人船測深原理示意圖

實(shí)驗(yàn)中，無人船以5 kn的速度沿著圖7所示的路徑航行，船舶在凌水港附近航行約1 500 s。船舶配備的測深儀、INS等設(shè)備的參數(shù)見表1。測深儀設(shè)定為每隔1 s給出1次深度信息。

圖7 船舶航行路徑

表1 實(shí)驗(yàn)用設(shè)備參數(shù)

在相同條件下分別對(duì)EKF[6]、PF[8]、MPF[19]和OOSMPF進(jìn)行比較分析。3種基于PF的算法都是基于1 000個(gè)粒子的SIR框架實(shí)現(xiàn)的。對(duì)于每一種方法，都進(jìn)行了100次蒙特卡羅模擬，得到了均方根誤差(root mean square error，RMSE)和估計(jì)誤差協(xié)方差的平均值。每個(gè)時(shí)間步長的RMSE由下式計(jì)算：

(11)

(12)

實(shí)驗(yàn)過程中對(duì)各種誤差統(tǒng)計(jì)進(jìn)行了測試，表2給出了各種標(biāo)準(zhǔn)差下不同場景的統(tǒng)計(jì)表，其中：過程噪聲和量測噪聲分別從標(biāo)準(zhǔn)差為σb和σv的高斯分布中采樣；INS的偏差從標(biāo)準(zhǔn)差為σu的高斯分布中采樣，在不同場景下保持不變，0.4 m相當(dāng)于船用級(jí)INS允許范圍內(nèi)的位置漂移。

表2 各種標(biāo)準(zhǔn)差下的不同場景統(tǒng)計(jì) m

場景1為過程噪聲相對(duì)較小的情況。圖8給出了場景1下基于時(shí)間的各濾波算法的RMSE曲線，圖9給出了場景1下基于時(shí)間的協(xié)方差曲線。表3給出了0～1 500 s內(nèi)所有算法的平均RMSE。在過程噪聲較小的情況下，PF、MPF和OOSMPF的RMSE沒有明顯的區(qū)別，只是OOSMPF的結(jié)果略優(yōu)于其他算法。然而，EKF具有更小的協(xié)方差，這是因?yàn)榛诳柭鼮V波的線性化量測模型未能逼近周圍包含很多拐點(diǎn)的地形。因此，EKF提供了退化的狀態(tài)估計(jì)。與EKF相比，3種PF的算法的協(xié)方差較大，這是因?yàn)橄闰?yàn)密度覆蓋的地形輪廓具有較少的量測更新。

圖8 場景1下不同算法的RMSE曲線

圖9 場景1下不同算法的協(xié)方差曲線

場景2為過程噪聲相對(duì)較大的情況。圖10和11分別給出了基于時(shí)間的RMSE和協(xié)方差曲線。在所有的濾波算法中，OOSMPF的濾波性能最好。

表3 不同濾波算法RMSE的平均值 m

如表3所示，EKF的平均RMSE并不大，但模型的非線性導(dǎo)致濾波器不太穩(wěn)定。當(dāng)PF算法的協(xié)方差更小時(shí)，其性能下降(主要是因?yàn)椴荒芎芎玫乇碚飨闰?yàn)分布)，因此，在這種情況下使用PF會(huì)導(dǎo)致性能較差。OOSMPF成功地處理了模糊的量測量，并提供了更可靠、準(zhǔn)確的狀態(tài)估計(jì)。就RMSE和協(xié)方差而言，MPF在所有情況下都表現(xiàn)出與PF相似的特點(diǎn)，這是因?yàn)橛捎诰植磕：裕紶枙?huì)發(fā)生模糊量測更新。

圖10 場景2下不同算法的RMSE曲線

圖11 場景2下不同算法的協(xié)方差曲線

場景3為過程模型和量測模型存在偏差并且過程噪聲較大的情況。圖12和13分別給出了基于時(shí)間的RMSE和協(xié)方差曲線。EKF的結(jié)果往往取決于其先前的軌跡，早期軌跡中大的誤差對(duì)后續(xù)時(shí)間步長移動(dòng)窗口中的預(yù)測階段有不利影響。運(yùn)行100次可以觀察到1次軌跡發(fā)散，在這種情況下，窗口大小為1的EKF的軌跡發(fā)散得更加頻繁，而PF呈現(xiàn)出較強(qiáng)的魯棒性，OOSMPF的濾波性能最好，而且OOSMPF提供了比PF和MPF更可靠的估計(jì)值。

圖12 場景3下不同算法的RMSE曲線

圖13 場景3下不同算法的協(xié)方差曲線

4 結(jié) 論

本文設(shè)計(jì)了一種基于無序量測的粒子濾波算法處理模糊的量測更新，進(jìn)而解決濾波器退化的問題。由于導(dǎo)致離散后驗(yàn)分布的模糊量測更新受到先驗(yàn)分布的影響，本文將模糊量測更新定義為導(dǎo)致粒子協(xié)方差增加的量測更新，并通過重塑先驗(yàn)分布來控制粒子的協(xié)方差。在SIR-PF的框架下，當(dāng)先驗(yàn)分布足以用于量測更新時(shí)，針對(duì)無序量測問題利用模糊量測給出一個(gè)優(yōu)化解。在無人船框架下對(duì)濾波結(jié)果進(jìn)行了實(shí)船驗(yàn)證。與EKF、PF以及MPF等其他算法相比，本文設(shè)計(jì)的算法取得了更好的位置估計(jì)均方根誤差性能。