伍永健，陳躍東，陳孟元

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

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

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

1 RBPF-SLAM

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

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

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

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

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

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

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

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

2 QPSO-RBPF-SLAM

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

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

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

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

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

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

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

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

QPSO-RBPF-SLAM算法流程：

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

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

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)實(shí)現(xiàn)QPSO重復(fù)優(yōu)化。

3 實(shí)驗(yàn)

3.1 仿真實(shí)驗(yàn)

為了說(shuō)明本文改進(jìn)算法的有效性，在MATLAB平臺(tái)進(jìn)行了仿真實(shí)驗(yàn)。

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

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

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

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

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

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

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

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

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

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

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

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

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

3.2 實(shí)際驗(yàn)證

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

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

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

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

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

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

4 結(jié)束語(yǔ)

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