史劍鳴,章 飛,曾慶軍,孫陶瑩

(江蘇科技大學 電子信息學院,鎮(zhèn)江 212003)

由于水下環(huán)境復雜,通過傳感器獲得觀測信息的方式單一,且無法獲得先驗環(huán)境的準確信息,因此許多陸上經(jīng)典導航方法運用于水下機器人導航定位問題時存在許多制約與不足[1]．定位與地圖創(chuàng)建(simultaneous localization and mapping, SLAM)方法是一種重要且常用的自主導航方法[2-4]．傳統(tǒng)SLAM方法在密集雜波和特征數(shù)目多的水下環(huán)境中存在數(shù)據(jù)關聯(lián)精度低、計算復雜的問題,且在SLAM實現(xiàn)過程中,由于機器人運動過程中的誤差、傳感器的觀測誤差、以及傳感器不確定性造成地圖特征漏檢等問題,都將影響數(shù)據(jù)關聯(lián)過程中的準確性,因此,傳統(tǒng)SLAM方法的準確性對地圖特征信息的數(shù)據(jù)關聯(lián)非常敏感[4-7]．

近年來,基于隨機有限集合(random finite sets, RFS)理論的概率假設密度SLAM(PHD-SLAM)方法獲得了廣泛的關注[8-11],這是一種在估計地圖特征時不依賴數(shù)據(jù)關聯(lián),并將傳統(tǒng)SLAM方法中所忽略的由于傳感器不確定性造成的地圖特征漏檢概率帶入地圖特征濾波的SLAM新方法,適用于數(shù)據(jù)關聯(lián)模糊、目標特征多且變化劇烈的環(huán)境中[1,4-6]．與傳統(tǒng)的SLAM方法相比，在地圖特征數(shù)目估計,地圖特征位置精度估計,機器人位姿的估計,以及濾波算法的運行速度上都有較大改進[4,12-14]．但由于PHD算法的特性,需要提供一定先驗信息才能遞歸進行濾波運算,而SLAM過程是在未知環(huán)境中從未知位置進行運動,無法提供先驗信息,文獻[15]提出一種新特征策略,將移動機器人上一時刻的觀測集合作為當前時刻的先驗信息集合,帶入PHD濾波當中，從而估計出新出現(xiàn)地圖特征的位置．然而,由于新生目標集合僅僅采用上一時刻的觀測集合,因此在地圖特征位置及數(shù)目的估計精度上存在不足．

為此,文中提出一種混合新目標信息的隨機有限集SLAM方法(mixed birth map information RFS-SLAM, MBMI-PHD-SLAM)．該方法將上一時刻之前PHD-SLAM已探測過的地圖信息中靠近機器人的地圖信息加入上一時刻的觀測集合中,作為PHD-SLAM預測過程中新生地圖特征集合．當機器人重新回到已探測過的環(huán)境中或已探測過的地圖特征重新出現(xiàn)在機器人視域內(nèi)時,通過增加先驗信息,改進PHD-SLAM方法對地圖特征數(shù)目估計及地圖特征位置精度估計．

1 PHD-SLAM問題描述

1.1 地圖信息的RFS模型

(1) 地圖特征的RFS模型

不同于傳統(tǒng)SLAM方法,PHD-SLAM方法的觀測信息以及地圖特征不再采用向量序列的形式表示,而是使用隨機有限集合形式來表示．地圖特征的數(shù)目從零到任意大小隨意變化,隨著在機器人不斷出現(xiàn)在機器人視域內(nèi)的地圖特征而單調(diào)增加,每個地圖特征之間是相互獨立的[15]．

(1)

(2) 地圖觀測的RFS模型

在k時刻,位姿為Xk的機器人獲得的特征觀測集合表示為Zk,其RFS模型為

(2)

Dk(m,Xk)建模為一個Bernoulli型RFS,Dk(m,Xk)=φ的概率為1-PD(m|Xk)[12],表示由于傳感器的不確定性造成的地圖特征漏檢概率；Dk(m,Xk)={z}的概率密度為PD(m|Xk)gk(z|m,Xk),表示能夠檢測到地圖特征的概率，其中PD(m|Xk)表示當機器人位姿為Xk時,能夠探測到位于m處的地圖特征概率，gk(z|m,Xk)表示機器人攜帶的傳感器模型．

1.2 PHD的地圖創(chuàng)建

PHD-SLAM的實現(xiàn)過程與FastSLAM相似,采用Rao-Blackwellised(RB)粒子濾波器實現(xiàn)機器人位姿后驗估計．地圖信息不再采用FastSLAM中的向量序列的方式進行預測和更新,而是使用PHD濾波描述地圖特征的狀態(tài),并根據(jù)每一時刻的觀測更新地圖特征．

(1) 地圖預測

Vk|k-1(m|Xk)=Vk-1|k-1(m|Xk-1)+b(m|Xk)

(4)

式中：Vk-1|k-1(m|Xk-1)為k-1時刻根據(jù)機器人位姿Xk-1得到的后驗地圖特征信息RFS的PHD；Vk|k-1(m|Xk)為k時刻預測可能出現(xiàn)的地圖特征RFS的PHD；b(m|Xk)為隨著機器人運動可能進入機器人視域內(nèi)新生地圖特征信息RFS的PHD．

(2) 地圖的更新

Vk|k(m|Xk)=Vk|k-1(m|Xk)(1-PD(m|Xk))+

(5)

式中：Vk|k(m|Xk)為k時刻后驗地圖特征信息RFS的PHD；Λ(m|Xk)=PD(m|Xk)gk(z|m,Xk),PD(m|Xk)表示位姿為Xk的機器人能夠探測到位于m處地圖特征的概率；ck(z)為k時刻由雜波引起的雜波RFS的PHD．

上述RB-PHD-SLAM方法中,b(m|Xk)提供地圖特征信息濾波器先驗信息,用以確定新的地圖特征在機器人視域內(nèi)可能出現(xiàn)的位置.而SLAM問題是在未知的環(huán)境中從未知地點出發(fā),缺乏先驗信息,傳統(tǒng)的PHD-SLAM方法在k時刻使用k-1觀測集合作為k時刻的b(m|Xk)，因此對機器人視域內(nèi)新出現(xiàn)的地圖特征估計存在滯后.尤其當機器人重新回到已探測過的環(huán)境中或已探測過的地圖特征重新出現(xiàn)在機器人視域內(nèi)時，未能利用已經(jīng)探測過的地圖信息來改善對當前時刻地圖特征的估計精度．針對這一問題,文中提出一種混合新生地圖信息策略作為b(m|Xk),將上一時刻之前探測到的地圖信息中靠近機器人位置的地圖特征加入到觀測信息中,作為新生地圖信息集合,通過增加先驗信息用以改善當前時刻地圖特征的估計精度．

2 新生地圖特征的RFS建模

在估計地圖特征目標數(shù)目以及地圖特征精度中,預測階段起重要作用,文中使用的混合新生目標信息的方法,增加預測階段中的新生地圖特征RFS的PHD,當機器人從初始時刻出發(fā)時由于缺乏先驗信息,使用k-1觀測集合作為k時刻可能出現(xiàn)新地圖特征信息b(m|Xk)．當機器人經(jīng)過PHD-SLAM更新出已探測區(qū)域地圖信息后,將已探測地圖信息中靠近機器人位姿的地圖特征作為先驗信息,加入上一時刻觀測集合中一起作為當前時刻的b(m|Xk)帶入當前時刻的PHD預測階段．

b(m|Xk)=Zk-1+Mf

(6)

(7)

式中：Mf為已探測地圖中靠近機器人位姿的地圖特征的RFS的PHD；ν為機器人的速度；R為機器人所攜帶傳感器掃描半徑；m為經(jīng)過PHD-SLAM方法得出的已探測地圖特征信息．根據(jù)機器人速度ν和傳感器掃描半徑R,確定出一個閾值T,當已探索過的地圖特征集合內(nèi)有地圖特征位置與機器人位置之間距離小于這一閾值時,將該地圖特征作為可能出現(xiàn)在當前時刻機器人視域內(nèi)的新地圖特征,γ(m|Xk-1)表示已探測地圖特征中小于閾值的地圖特征形成的RFS的PHD．上述方法實現(xiàn)的偽代碼如下:

1 ifM=φ

2b(m|Xk)=Zk-1

3 else ifM={m1,m2…mn}

4 fori=1 ton

5 T=R+ν

6 if (mi-Xk-1)

7Mf=Mf+mi

8 end for

9b(m|Xk)=Zk-1+Mf

10 endif

混合新生地圖信息策略中的b(m|Xk)不僅包括k-1時刻的觀測集合還加入了已探測區(qū)域內(nèi)地圖信息中靠近機器人位姿的地圖特征集合,當機器人視域內(nèi)已探測過的地圖特征重新出現(xiàn)時,加入b(m|Xk)中作為先驗信息的這部分地圖特征,經(jīng)過PHD-SLAM地圖濾波后,重新出現(xiàn)的地圖特征的權重會增加,從而提高了對地圖特征點位置和數(shù)目的估計精度．

3 MBMI-PHD-SLAM方法及實現(xiàn)

文中提出的MBMI-PHD-SLAM方法遞歸的過程中,機器人位姿估計分布使用Rao-Blackwellised粒子濾波器,每一個粒子的位姿代表機器人可能出現(xiàn)的位姿,粒子的權重代表可能性的大小,在機器人位姿已知情況下,采用PHD濾波器描述地圖特征的概率分布,并利用觀測集合進行更新．在k-1時刻,粒子集合的PHD-SLAM的強度可以表示為:

每個粒子所對應的地圖后驗分布采用高斯混合項的方式實現(xiàn),即PHD的分布由一系列的加權高斯項表示,每一個高斯項的峰值表示路標可能存在的位置,可能性的大小由權重體現(xiàn)．

3.1 每個粒子的GM-PHD地圖估計

(1) 預測階段

根據(jù)每個粒子采樣位置,可以得到k時刻新出現(xiàn)特征的高斯混合項:

(8)

(9)

根據(jù)式(4)可知,地圖預測的高斯混合PHD為:

(10)

k時刻地圖特征預測的高斯混合項包括k時刻新出現(xiàn)的地圖特征高斯混合項,以及k-1時刻PHD濾波后得到的高斯混合項,即k-1時刻PHD濾波得到的老地圖特征依然存在的高斯混合項．

為比較不同MCMC抽樣算法的計算效率，選擇陜北地區(qū)劉家河、神木等5個水文測站的年最大洪峰流量系列資料進行應用，資料概況如表1所示。

(2) 更新階段

每個粒子的觀測似然函數(shù)也采用高斯混合的形式,因此更新后地圖的后驗PHD同樣具有高斯混合的形式

(11)

(12)

(13)

(14)

式中：μk|k,Pk|k,Sk可以通過基于EKF和UKF的PHD濾波器更新獲得,由于EKF-PHD的結構簡單,運算速度快,因此文中采用EKF-PHD的方式實現(xiàn)后驗地圖的估計更新．

(3) PHD的剪切、合并與地圖特征的提取

PHD剪切、合并完成后,對高斯分量權重大于地圖特征提取門限Tf的高斯分量進行地圖特征提取,其高斯項的均值表示地圖特征的位置,滿足門限Tf高斯分量的個數(shù)表示地圖特征的數(shù)目．

3.2 機器人位姿估計

機器人位姿估計采用RB粒子濾波器實現(xiàn),實現(xiàn)過程包括兩部分:粒子重要性采樣以及粒子重采樣．

(15)

(16)

(2) 粒子重采樣

4 仿真結果及分析

4.1 仿真環(huán)境參數(shù)

文中的仿真環(huán)境為Matlab7,地圖大小為50 m×60 m,地圖特征數(shù)為30個,聲吶傳感器掃描角度為360°,掃描半徑為10 m．機器人運動速度為3 m/s．運動噪聲為均值為0,方差為0.5 m的高斯噪聲,粒子個數(shù)為20個,根據(jù)經(jīng)驗值并參考文獻[8]選取Tp=10-5．為驗證改進的MBMI-PHD-SLAM方法的性能,將文中方法和文獻[6]中RB-PHD-SLAM方法分別在雜波密度較低和較高的情況下進行仿真比較,對機器人定位誤差,地圖特征數(shù)目估計及地圖特征位置估計誤差進行對比分析．

4.2 仿真結果及分析

(1) 低雜波密度環(huán)境下仿真結果及分析

圖1～4為輕度噪聲觀測環(huán)境下MBMI-PHD-SLAM和RB-PHD-SLAM運行50次地圖仿真結果,輕度噪聲環(huán)境雜波強度為3,雜波服從Poisson分布,觀測距離噪聲為均值為0,方差為0.3 m的高斯噪聲．傳感器檢測地圖特征目標概率PD=0.95．

圖1 低雜波密度下MBMI-PHD-SLAM結果Fig.1 Results of MBMI-PHD-SLAM with low clutter density

圖2是兩種方法對每一步機器人位姿誤差對比,圖3是地圖特征數(shù)目誤差對比,圖4是地圖特征位置估計誤差對比,文中采用最優(yōu)子模型分配距離(optimal subpattern assignment, OSPA) 進行評價集合之間差異程度的誤差距離．

圖2 低雜波密度下機器人位姿誤差對比Fig.2 Comparison of vehicle position with low clutter density

圖3 低雜波密度下地圖特征數(shù)目估計誤差對比Fig.3 Comparison of map feature number estimation error with low clutter density

從圖中對比中可知,當機器人在開始階段兩種方法并無太大差距,而當25步后地圖信息構建完成,機器人重新回到已探測過的環(huán)境中,已探測地圖中地圖特征可能重新出現(xiàn)在機器人視域內(nèi)時,MBMI-PHD-SLAM方法的地圖特征數(shù)目誤差及OSPA誤差開始減小且變化平緩波動?。?/p>

圖4 低雜波密度下OSPA誤差對比Fig.4 Comparison of map OSPA error with low clutter density

將表1和表2兩種方法進行對比可知在低密度雜波下,機器人在25步之前,處在未探測過的環(huán)境中時,地圖特征數(shù)目誤差及地圖特征位置誤差基本相同．但當25步之后MBMI-PHD-SLAM方法對于地圖特征數(shù)目估計精度提高了72.48%,地圖特征位置估計精度提高了49.53%,機器人位姿估計精度提高了37.01%．通過以上數(shù)據(jù)的對比分析可以看出,當機器人重新回到已探測過的環(huán)境時,與RB-PHD-SLAM方法相比,文中所提方法在對地圖特征數(shù)目和位置以及機器人位置的估計精度方面均有大幅度的提高．

表1 未探測過的環(huán)境(25步之前)Table 1 Unexplored environment(25 steps before) m

表2 重新回到探測過的環(huán)境(25步之后)Table 2 Back to the detected environment(25 steps before) m

(2) 高密度雜波仿真結果及分析

圖5～8為高密度雜波下MBMI-PHD-SLAM方法運行50次仿真結果,重度噪聲環(huán)境雜波強度為10,雜波服從Poisson分布,觀測距離噪聲為均值為0,方差為0.3 m的高斯噪聲．傳感器檢測地圖特征目標概率PD=0.95．

圖5 高雜波密度下MBMI-PHD-SLAMFig.5 Results of MBMI-PHD-SLAM with high clutter density

圖6是兩種算法對機器人位姿估計誤差對比,圖7,圖8為每一步地圖特征數(shù)目誤差及地圖特征位置誤差對比．

圖6 高雜波密度下機器人位姿估計誤差對比Fig.6 Comparison of vehicle position with high clutter density

圖7 高雜波密度下地圖特征數(shù)目估計誤差對比Fig.7 Comparison of map feature number estimation error with high clutter density

圖8 高雜波密度下OSPA誤差對比Fig.8 Comparison of map OSPA error with high clutter density

25步后地圖信息構建完成,機器人重新回到已探測過的環(huán)境中,已探測地圖中地圖特征可能重新出現(xiàn)在機器人視域內(nèi)時MBMI-PHD-SLAM方法相較于傳統(tǒng)RB-PHD-SLAM在地圖特征數(shù)目估計以及地圖特征位置估計上仍有顯著提高．

與輕噪聲環(huán)境結果相比,重噪聲環(huán)境下兩種方法的性能有所下降,但25步后RB-PHD-SLAM地圖特征數(shù)目變化程度以及OSPA誤差變化程度仍大于MBMI—PHD-SLAM．

由表3，4可知,當25步之后MBMI-PHD-SLAM對于地圖特征數(shù)目估計精度提高了75.78%,地圖特征位置估計精度提高了33.95%,機器人位姿估計精度提高了25.07%．通過表中數(shù)據(jù)對比分析可知,當機器人重新回到已探測過的環(huán)境中時,文中所提方法相比RB-PHD-SLAM方法,在對地圖特征數(shù)目和位置以及機器人位置的估計精度方面有大幅度的提高．

表3 未探測過的環(huán)境(25步之前)Table 3 Unexplored environment(25 steps before) m

表4 重新回到探測過的環(huán)境(25步之后)Table 4 Back to the detected environment (25 steps before) m

5 結論

(1)針對在密集雜波和地圖特征數(shù)目多且變化劇烈的水下環(huán)境中,水下機器人對地圖特征點位置和數(shù)目以及機器人自身位置估計精度較低的問題,提出了MBMI-PHD-SLAM方法,該方法使用隨機有限集合的方式更加準確的描述地圖特征信息,地圖特征觀測信息,以及傳感器不確定性造成的漏檢概率等真實環(huán)境因素,并可在使用少量粒子數(shù)目的情況下依舊能夠得到較高的地圖估計精度及機器人位姿估計精度．

(2) 使用混合新生地圖特征策略改進了傳統(tǒng)RB-PHD-SLAM,通過增加先驗信息,明顯提高了對地圖特征位置和數(shù)目估計精度及機器人位姿的估計精度．

(3) 通過兩種不同雜波密度環(huán)境下的仿真實驗,驗證了該方法在提高位姿估計精度以及地圖估計精度方面的有效性．