量子粒子群優(yōu)化下的RBPF-SLAM算法研究

伍永健，陳躍東，陳孟元

地圖構(gòu)建作為機(jī)器人自主導(dǎo)航的基礎(chǔ)，是指移動機(jī)器人在未知環(huán)境下依據(jù)自身攜帶的傳感器信息建立地圖模型[1]。常用的地圖模型有柵格地圖[2]、幾何地圖[3]以及拓?fù)涞貓D[4]等。然而，地圖構(gòu)建問題與定位問題緊密相關(guān)，定位的結(jié)果用于地圖構(gòu)建，而已經(jīng)構(gòu)建好的地圖又能實現(xiàn)精確定位，因此同時定位與地圖構(gòu)建(SLAM[5])被提出并受到廣泛研究和應(yīng)用。目前，SLAM技術(shù)大都基于概率理論，比如卡爾曼濾波[6]及擴(kuò)展卡爾曼濾波[7]、最大似然估計[8]、粒子濾波[9]和Markov定位[10]等。文獻(xiàn)[11]將UT和UKF運用到高斯項更新及采樣粒子權(quán)重計算過程中，提出一種無跡高斯混合概率假設(shè)密度SLAM算法；文獻(xiàn)[12]在雁群粒子群算法的基礎(chǔ)上采用分?jǐn)?shù)階微積分和混沌思想訓(xùn)練模糊自適應(yīng)擴(kuò)展卡爾曼濾波，從而實現(xiàn)同時定位與地圖創(chuàng)建；文獻(xiàn)[13]在基本SLAM算法的迭代過程中引入元胞自動機(jī)(CA)，建立“SLAMCA生長–重定位”的閉環(huán)作用機(jī)制。

Rao-Blackwellized粒子濾波算法是目前解決SLAM問題的有效方法，它將SLAM問題分解成機(jī)器人的位姿估計和地圖估計，采用粒子濾波器和擴(kuò)展卡爾曼濾波器估計概率，但仍存在算法運行時間長，粒子退化嚴(yán)重等不足；此后很多改進(jìn)的RBPF算法被提出，如文獻(xiàn)[14]提出一種基于高斯分布的RBPF-SLAM算法，通過高斯分布分散高權(quán)重粒子獲得新粒子，雖然算法能在粒子減少的條件下保持可靠估計，但忽略對低權(quán)重粒子的考慮，抑制樣本匱乏現(xiàn)象還存在不足；文獻(xiàn)[15]提出一種粒子群優(yōu)化遺傳重采樣的改進(jìn)RBPF-SLAM算法，采用粒子群優(yōu)化策略調(diào)整粒子集，并對權(quán)重較小的粒子進(jìn)行變異操作，但粒子群的引入容易陷入局部最優(yōu)，加上重采樣中只針對權(quán)重較小粒子操作，對緩解粒子退化無法產(chǎn)生滿意的效果。文獻(xiàn)[16]采用改進(jìn)的提議分布并結(jié)合基于等級的自適應(yīng)局部重采樣(APRR)算法，設(shè)計了一種基于退火參數(shù)優(yōu)化混合提議分布的RBPF算法，對高權(quán)重和低權(quán)重粒子只進(jìn)行復(fù)制操作，對增加粒子多樣性緩解粒子退化效果不佳。

考慮這些不足，本文從解決RBPF算法運行時間長、提議分布精度不高以及重采樣過程的粒子退化出發(fā)，將量子粒子群算法[17]引入到Rao-Blackwellized粒子濾波算法，提出一種QPSORBPF-SLAM算法，一方面在基本提議分布中加入觀測信息，使改進(jìn)的提議分布更加接近真實狀態(tài)，另一方面在重采樣中根據(jù)QPSO算法更新粒子位姿，對高低權(quán)值粒子進(jìn)行自適應(yīng)交叉變異操作。QPSO-RBPF-SLAM保持了粒子的多樣性，有效緩解了粒子退化，同時算法能在減少運算時間和粒子數(shù)的條件下獲得可靠的估計，整體性能得到較大提高，能夠精確估計出機(jī)器人的位姿并獲得高精度的地圖。

1 RBPF-SLAM

移動機(jī)器人SLAM實質(zhì)上是一個Markov鏈的過程：在一個未知環(huán)境中機(jī)器人從起始位置出發(fā)，在運動過程中，使用里程計記錄自身運動的信息()和外部傳感器獲取的環(huán)境信息() ，估計機(jī)器人的軌跡()與構(gòu)建增量式環(huán)境地圖，同時使用創(chuàng)建好的地圖及傳感器的信息實現(xiàn)自定位。根據(jù)貝葉斯濾波遞歸原理，從概率學(xué)的角度得出SLAM的遞歸公式為

基于Rao-Blackwellized粒子濾波SLAM算法的思想：計算機(jī)器人軌跡和地圖m的后驗概率，將其分解為式(2)所示的軌跡估計和地圖估計兩個后驗概率乘積。

首先對機(jī)器人的軌跡進(jìn)行估計，利用Rao-Blackwellized粒子濾波器實現(xiàn)，其中每個粒子代表機(jī)器人一條可能的行走軌跡。

然后再結(jié)合觀測模型對地圖進(jìn)行更新。將地圖表示為服從高斯分布的特征路標(biāo)的集合，因此對地圖的估計可通過特征路標(biāo)估計得到，這里采用擴(kuò)展卡爾曼濾波來實現(xiàn)。

因此，在粒子代表的軌跡上利用傳感器實時觀察獲得的路標(biāo)信息構(gòu)成最后的地圖。

利用Rao-Blackwellized 濾波器在傳感器觀測信息與里程計信息基礎(chǔ)下構(gòu)建增量式地圖的步驟可以分為4步：

1) 初始化：當(dāng)t=0時，選取N個粒子，每個粒子的權(quán)重為。

3) 粒子權(quán)重：為了彌補(bǔ)采樣時提議分布跟目標(biāo)分布的差距，需要計算每一個獨立粒子的權(quán)重，由重要性采樣公式得出式(3)：

2 QPSO-RBPF-SLAM

在重要性采樣中，需要依據(jù)提議分布對下一代粒子集進(jìn)行采樣，而基本RBPF-SLAM中常采用運動模型作為提議分布，使得粒子退化嚴(yán)重，導(dǎo)致丟失權(quán)值較大的粒子從而創(chuàng)建的地圖精度不高。為了解決提議分布精度不高的問題，結(jié)合里程計信息和外部傳感觀測信息作為混合提議分布如式(5)所示：

然而此混合提議分布無法直接進(jìn)行采樣操作，需要目標(biāo)提議分布的一個近似化正態(tài)分布實現(xiàn)。式(6)所示的正態(tài)分布參數(shù)通過帶權(quán)重的蒙特卡羅采樣方法估計得出。

在混合提議分布下，粒子權(quán)重通過式(7)得出：

式中k表示常數(shù)。

基本粒子群優(yōu)化算法(PSO)由于粒子速度的局限性而不能在整個可行空間進(jìn)行搜索，無法保證算法全局收斂，故在重采樣過程中引入量子粒子群算法更新粒子集。量子粒子群算法是一種將量子系統(tǒng)的特點與粒子群算法相結(jié)合的新興群體智能算法，將粒子群引入量子空間并確定粒子在空間中的位置，通過量子束縛態(tài)描述粒子的聚集性，保證了粒子在整個可行解區(qū)域的搜索，保證收斂到全局最優(yōu)解。利用QPSO算法驅(qū)使粒子快速地靠近于似然函數(shù)高的區(qū)域，優(yōu)化調(diào)整機(jī)器人位姿狀態(tài)的粒子集，則粒子位置更新如式(8)所示：

同時，為了防止粒子退化以及保持粒子的多樣性，對所得的粒子集進(jìn)行優(yōu)化調(diào)整，基本思想是：根據(jù)權(quán)重閾值將粒子劃分為高權(quán)重粒子、低權(quán)重粒子以及中等權(quán)重粒子，保留中等權(quán)重粒子，對具有高權(quán)重和低權(quán)重的粒子進(jìn)行自適應(yīng)交叉變異操作。根據(jù)式(9)設(shè)置合適的高權(quán)重閾值和低權(quán)重閾值，取兩閾值之間的粒子作為中等權(quán)重的粒子。

交叉操作：在高權(quán)重和低權(quán)重的粒子集中隨機(jī)選取兩個個體作為父輩粒子進(jìn)行配對，按照式(10)所示的自適應(yīng)交叉率進(jìn)行交叉操作得到兩個新個體。

變異操作：從交叉后得到的新粒子集中隨機(jī)選擇的一個父輩粒子按照式(11)所示自適應(yīng)變異率進(jìn)行變異操作產(chǎn)生新粒子。

QPSO-RBPF-SLAM算法流程：

2) 根據(jù)式(5)計算混合提議分布，進(jìn)行采樣操作得出粒子集。 {}

4) 根據(jù)式(7)計算粒子權(quán)重，并依據(jù)設(shè)定的高低權(quán)重閾值來劃分粒子。

5) 根據(jù)式(4)重采樣條件，判斷是否需要進(jìn)行重采樣操作。若需要重采樣，則執(zhí)行6)；否則，執(zhí)行8)。

6) 保留中等權(quán)重粒子，將高權(quán)重和低權(quán)重粒子根據(jù)式(10)、式(11)進(jìn)行自適應(yīng)交叉和變異操作。

7) 中等權(quán)重粒子和交叉變異后的粒子組成新粒子集進(jìn)行重采樣，并返回3)實現(xiàn)QPSO重復(fù)優(yōu)化。

3 實驗

3.1 仿真實驗

為了說明本文改進(jìn)算法的有效性，在MATLAB平臺進(jìn)行了仿真實驗。

首先對機(jī)器人自身位姿估計。設(shè)置機(jī)器人實際行走軌跡中真實的位姿狀態(tài)，利用基本RBPF、文獻(xiàn)[15]算法和改進(jìn)RBPF在粒子數(shù)N取50和100時對真實的位姿進(jìn)行估計。其中，=0.8、=0.6、=0.1、=0.01。

由圖1和表1可知，在粒子數(shù)相同的條件下，改進(jìn)RBPF算法的均方根誤差較小，與真實狀態(tài)接近；隨著粒子數(shù)的增加，雖然算法運行時間延長，但估計的結(jié)果則更加接近真實狀態(tài)；與RBPF算法和文獻(xiàn)[15]算法采用100個粒子所獲得的估計結(jié)果相比，改進(jìn)的RBPF算法采用50個粒子能夠獲得較好的估計效果，故改進(jìn)的RBPF算法能利用較少的粒子獲得可靠且較精確的估計。

其次，比較RBPF算法、文獻(xiàn)[15]算法和改進(jìn)RBPF算法下對機(jī)器人軌跡和路標(biāo)的估計結(jié)果。如圖2所示，設(shè)定100 cm×100 cm的區(qū)域，星形表示實際路標(biāo)，紅線表示實際軌跡；黑線表示利用改進(jìn)RBPF算法得到的軌跡估計，圓形表示對應(yīng)的路標(biāo)估計；虛線表示利用文獻(xiàn)[15]算法得到的軌跡估計，三角形表示對應(yīng)的路標(biāo)估計；點線表示利用RBPF算法得到的軌跡估計，黑色小點表示對應(yīng)的路標(biāo)估計。

圖1 機(jī)器人位姿估計Fig. 1 Pose estimation of robot

表1 3種算法的對比數(shù)據(jù)Table 1 Comparison data of three algorithms

圖2 機(jī)器人軌跡估計和路標(biāo)估計Fig. 2 Robot trajectory estimation and road sign estimation

由圖2和表2可知，改進(jìn)的RBPF算法在進(jìn)行軌跡和路標(biāo)估計時所用粒子數(shù)和運行時間比RBPF算法和文獻(xiàn)[15]算法少。在軌跡估計方面，改進(jìn)的RBPF算法得到的軌跡與機(jī)器人實際軌跡誤差較小，而RBPF算法和文獻(xiàn)[15]算法得到的軌跡波動較大；在路標(biāo)估計方面，利用改進(jìn)的RBPF算法得到的路標(biāo)估計與實際路標(biāo)較為接近，而RBPF算法和文獻(xiàn)[15]算法得到的路標(biāo)估計則在一定程度上遠(yuǎn)離實際路標(biāo)。因此，與RBPF算法和文獻(xiàn)[15]算法相比，改進(jìn)的RBPF算法在機(jī)器人軌跡估計和路標(biāo)估計方面能夠得到更加滿意的效果。

表2 3種算法的對比數(shù)據(jù)Table 2 Comparison data of three algorithms

下面利用維多利亞公園數(shù)據(jù)集對RBPF算法、文獻(xiàn)[15]算法和改進(jìn)的RBPF算法的性能進(jìn)一步驗證。由于悉尼維多利亞公園數(shù)據(jù)集并未提供相關(guān)噪聲參數(shù)的信息，故將噪聲參數(shù)設(shè)置為：車輛速度控制噪聲為1.0 m/s，駕駛角控制噪聲為2.0°；路標(biāo)觀測的角度噪聲為2.5°，測距噪聲為1.6 m。3種算法分別采用20個粒子、15個粒子和10個粒子來描述車輛軌跡和環(huán)境地圖。

RBPF算法、文獻(xiàn)[15]算法和改進(jìn)的RBPF算法的仿真結(jié)果如圖3所示。其中，灰色粗線表示GPS路徑(即真實路徑)，黑色細(xì)線表示估計路徑，黑點表示估計路標(biāo)。

由圖3可知，3種算法在不同程度上估計出GPS路徑，但RBPF算法采用20個粒子得到的軌跡在部分區(qū)域出現(xiàn)明顯的不匹配現(xiàn)象，偏差較大；文獻(xiàn)[15]算法采用15個粒子得到的軌跡相比RBPF算法不匹配現(xiàn)象減少；而改進(jìn)的RBPF算法采用10個粒子得到的軌跡與GPS路徑之間的誤差較小，吻合度更高。同時，RBPF算法和文獻(xiàn)[15]算法出現(xiàn)粒子匱乏問題而導(dǎo)致估計的路標(biāo)個數(shù)不完全，而改進(jìn)的RBPF算法能精確地估計所有設(shè)定的路標(biāo)。

由上述仿真可知，RBPF算法的提議分布缺少觀測信息且所有粒子都有參與重采樣，算法整體計算過程簡單但效果不佳，會出現(xiàn)粒子退化現(xiàn)象導(dǎo)致最后創(chuàng)建的地圖精度不高；文獻(xiàn)[15]對提議分布進(jìn)行改進(jìn)，引入粒子群算法更新粒子集，并對所有權(quán)重較低粒子進(jìn)行重采樣，計算復(fù)雜度有所提升；而改進(jìn)的RBPF算法通過量子粒子群算法，只考慮粒子的位置量，且針對部分粒子進(jìn)行重采樣，整體計算復(fù)雜度介于RBPF算法和文獻(xiàn)[15]算法之間，但由于改進(jìn)的RBPF算法能以較少粒子數(shù)獲得更好的估計結(jié)果，使得算法整體運行時間降低。整體而言，改進(jìn)的RBPF算法具有更好的有效性和優(yōu)越性。

圖3 維多利亞公園數(shù)據(jù)集仿真結(jié)果Fig. 3 Simulation results based on Vitoria Park data

3.2 實際驗證

為了驗證本文改進(jìn)算法的實際性，在室內(nèi)環(huán)境下利用旅行家2號移動機(jī)器人進(jìn)行實際驗證，完成同時定位與地圖構(gòu)建。該機(jī)器人內(nèi)部有里程計，并隨身攜帶URG-hokuyo激光傳感器。在PC機(jī)上運行Liunx(Ubuntu 12.04)的ROS系統(tǒng)。

選取安徽工程大學(xué)電氣工程學(xué)院實驗室部分區(qū)域作為本次實驗的室內(nèi)環(huán)境。如圖4所示，選取的區(qū)域為8 m×1.5 m，機(jī)器人以0.2 m/s的速度移動，利用里程計信息和激光數(shù)據(jù)信息實時構(gòu)建柵格地圖。

圖4 實驗室環(huán)境Fig. 4 Laboratory environment

由圖5和表3可知，RBPF-SALM算法采用了39個粒子，粒子退化嚴(yán)重，降低粒子多樣性，創(chuàng)建的地圖不夠精確；文獻(xiàn)[15]算法采用了24個粒子，創(chuàng)建的地圖有所改善，但效果不顯著；改進(jìn)的RBPFSALM算法只使用了16個粒子獲得了比RBPF-SALM算法和文獻(xiàn)[15]算法更精確的地圖，同時軌跡和路標(biāo)估計的均方根誤差、運行時間也大大縮減。

表3 3種算法創(chuàng)建地圖數(shù)據(jù)Table 3 Map data of three algorithms

圖5 Rviz實時構(gòu)建地圖Fig. 5 Rviz building a map in real time

4 結(jié)束語

為解決RBPF算法中粒子退化和多樣性降低問題，本文提出一種QPSO-RBPF-SLAM算法。將機(jī)器人運動模型和觀測模型作為提議分布，在重采樣過程中結(jié)合量子粒子群思想和遺傳算法，利用QPSO算法更新粒子位姿，根據(jù)權(quán)值劃分粒子種類引入自適應(yīng)交叉變異操作對所得粒子集進(jìn)行優(yōu)化、調(diào)整。同時，在機(jī)器人ROS平臺上利用旅行家2號機(jī)器人進(jìn)行實驗，以較少的粒子數(shù)和較短時間在精確估計機(jī)器人位姿的同時能夠創(chuàng)建較高精度的柵格地圖。下一步，在獲得的高精度柵格地圖的基礎(chǔ)上對移動機(jī)器人進(jìn)行路徑規(guī)劃研究。