量子粒子群優化下的RBPF-SLAM算法研究

伍永健，陳躍東，陳孟元

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

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

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

1 RBPF-SLAM

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

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

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

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

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

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

1) 初始化：當t=0時，選取N個粒子，每個粒子的權重為。

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

2 QPSO-RBPF-SLAM

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

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

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

式中k表示常數。

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

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

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

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

QPSO-RBPF-SLAM算法流程：

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

4) 根據式(7)計算粒子權重，并依據設定的高低權重閾值來劃分粒子。

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

6) 保留中等權重粒子，將高權重和低權重粒子根據式(10)、式(11)進行自適應交叉和變異操作。

7) 中等權重粒子和交叉變異后的粒子組成新粒子集進行重采樣，并返回3)實現QPSO重復優化。

3 實驗

3.1 仿真實驗

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

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

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

其次，比較RBPF算法、文獻[15]算法和改進RBPF算法下對機器人軌跡和路標的估計結果。如圖2所示，設定100 cm×100 cm的區域，星形表示實際路標，紅線表示實際軌跡；黑線表示利用改進RBPF算法得到的軌跡估計，圓形表示對應的路標估計；虛線表示利用文獻[15]算法得到的軌跡估計，三角形表示對應的路標估計；點線表示利用RBPF算法得到的軌跡估計，黑色小點表示對應的路標估計。

圖1 機器人位姿估計Fig. 1 Pose estimation of robot

表1 3種算法的對比數據Table 1 Comparison data of three algorithms

圖2 機器人軌跡估計和路標估計Fig. 2 Robot trajectory estimation and road sign estimation

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

表2 3種算法的對比數據Table 2 Comparison data of three algorithms

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

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

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

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

圖3 維多利亞公園數據集仿真結果Fig. 3 Simulation results based on Vitoria Park data

3.2 實際驗證

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

選取安徽工程大學電氣工程學院實驗室部分區域作為本次實驗的室內環境。如圖4所示，選取的區域為8 m×1.5 m，機器人以0.2 m/s的速度移動，利用里程計信息和激光數據信息實時構建柵格地圖。

圖4 實驗室環境Fig. 4 Laboratory environment

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

表3 3種算法創建地圖數據Table 3 Map data of three algorithms

圖5 Rviz實時構建地圖Fig. 5 Rviz building a map in real time

4 結束語

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