基于無序量測粒子濾波的無人船導航

古毅杰， 張闖， 康凱航

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

0 引 言

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

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

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

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

1 PF及模糊量測更新

1.1 PF

非線性隨機系統可以用離散過程模型和量測模型[18-19]表示：

(1)

式中：xt和zt分別為t時刻的系統狀態矢量和量測矢量；f(·)和h(·)分別為狀態轉移函數和觀測函數；bt和vt分別為t時刻的過程噪聲矢量和量測噪聲矢量。

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

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

(2)

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

(3)

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

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

(4)

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

(5)

(6)

1.2 模糊量測更新

在量測更新階段之后，如果估計量測值與真實狀態量測值在相同條件下存在模糊點，則粒子協方差會增加。這種現象通常發生在式(1)中非線性函數h(xt)的拐點附近[22]。因為拐點附近的函數值在另一側具有相同的對應值，所以量測更新可能導致分散的后驗分布。

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

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

根據上文分析可知，模糊量測更新會導致分散的后驗分布以及不可靠的位置估計。本算法的目的是，當發生模糊量測更新時，獲得更優的當前估計置信度，從而獲得更好的濾波性能。該算法的核心思想是，在先驗分布適合量測更新的條件下，保存模糊量測并后續使用，本質上等同于量測到達延遲的情況。圖5給出了延遲量測超時序列的量測關系。量測zt-1對應于t-1時刻的系統狀態xt-1。假設t-1時刻zt-1的量測更新是模糊的，在zt的量測更新之后該濾波器使用zt-1，如同zt-1正好在t時刻zt之后到達。

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

(7)

其中

(8)

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

(3)根據式(5)計算權重，并歸一化權重。

(4)計算累積分布函數的協方差。

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

3 實船實驗結果分析

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

zt=h(xt)+vt

(9)

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

xt+1=xt+ut+bt

(10)

式中：ut和bt分別表示相對運動信息和附加的過程噪聲。船舶的相對運動信息由慣性導航系統(inertial navigation system，INS)估計。

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

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

圖7 船舶航行路徑

表1 實驗用設備參數

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

(11)

(12)

實驗過程中對各種誤差統計進行了測試，表2給出了各種標準差下不同場景的統計表，其中：過程噪聲和量測噪聲分別從標準差為σb和σv的高斯分布中采樣；INS的偏差從標準差為σu的高斯分布中采樣，在不同場景下保持不變，0.4 m相當于船用級INS允許范圍內的位置漂移。

表2 各種標準差下的不同場景統計 m

場景1為過程噪聲相對較小的情況。圖8給出了場景1下基于時間的各濾波算法的RMSE曲線，圖9給出了場景1下基于時間的協方差曲線。表3給出了0～1 500 s內所有算法的平均RMSE。在過程噪聲較小的情況下，PF、MPF和OOSMPF的RMSE沒有明顯的區別，只是OOSMPF的結果略優于其他算法。然而，EKF具有更小的協方差，這是因為基于卡爾曼濾波的線性化量測模型未能逼近周圍包含很多拐點的地形。因此，EKF提供了退化的狀態估計。與EKF相比，3種PF的算法的協方差較大，這是因為先驗密度覆蓋的地形輪廓具有較少的量測更新。

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

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

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

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

如表3所示，EKF的平均RMSE并不大，但模型的非線性導致濾波器不太穩定。當PF算法的協方差更小時，其性能下降(主要是因為不能很好地表征先驗分布)，因此，在這種情況下使用PF會導致性能較差。OOSMPF成功地處理了模糊的量測量，并提供了更可靠、準確的狀態估計。就RMSE和協方差而言，MPF在所有情況下都表現出與PF相似的特點，這是因為由于局部模糊性，偶爾會發生模糊量測更新。

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

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

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

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

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

4 結 論

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