基于ROS下移動機器人地圖構建與路徑規劃研究

馬舸瀚，楊 旗

（沈陽理工大學機械工程學院，沈陽 110000）

0 引言

近幾十年來，智能移動機器人因其高靈活性和機動性的特點而被廣泛應用［1］。在大多數情況下，自主導航對于智能移動機器人是必要的，特別是路徑規劃，是導航中最重要的部分之一。因此，如何在部分已知或完全未知的環境中構建具有理想路徑規劃器的移動機器人已成為研究熱點［2］。

目前，在機器人路徑規劃方面出現了很多算法，典型的機器人全局路徑規劃算法，如A*算法、Dijkstra 算法、蟻群算法、BFS 算法和RBPF 算法等［3］。在路徑規劃方面，這些算法具有較高的成功率，但需要很長時間，網格越精確，效率越低。

針對上述問題，研究工作提出了一種移動機器人優化路徑規劃模型。實驗結果表明,該優化后的算法模型能夠獲得比現有的路徑優化算法更快、更平滑、更安全的路徑。

1 粒子濾波算法

1.1 Gmapping算法

Gmapping 算法［4］是目前應用最為廣泛的一種2D 激光SLAM 算法，與卡爾曼濾波相比，局限性較小，應用范圍廣。

1.2 基于RBPF算法的改進

傳統的RBPF 算法使用粒子濾波算法來估計機器人的狀態［5］。然而，隨著粒子數量的增加，計算復雜度顯著增加。隨著算法迭代次數的增加，粒子不可避免地會發生退化。

因此，針對以上RBPF 算法的問題，本研究提出了一種改進的RBPF 映射算法來實現SLAM映射，即利用改進的分布和自適應重采樣算法。

（1）提議分布的改進。本次分布信息對比了加入激光雷達觀測的分布模型p（z|x）和歷程采樣后的高斯分布模型p（x|x',u），從圖1 可以看出，兩種分布模型的差別較大，采用激光雷達觀測的分布模型可以提高機器人姿態移動的準確率，效果較好。

圖1 兩種運動模型概率分布

利用上述激光雷達分布模型得到最新的觀測數據，將該觀測數據應用于新粒子的采集，可以獲得改進的提議分布，具體如式（1）所示：

（2）目標分布通常只有幾個峰值，在大多數情況下只有一個峰值。為了簡化計算，直接收集峰值附近的值來模擬所提出的分布。

歸一化參數η的求取方法如式（2）所示：

（3）重采樣依據。映射采用了改進的粒子采樣分布，這意味著粒子降解較低，不需要每次都重采樣，因此采用自適應重采樣方法來確定粒子采樣。首先，根據公式計算出當前粒子權重分散性量度Neff。計算過程如式（3）所示：

然后將其與設置的粒子數N進行比較，當Neff小于N/2時需進行重采樣。

（4）地圖的更新。為了驗證改進RBPF 算法的有效性，利用ROS 機器人系統軟件對地圖環境進行構建，并利用改進的RBPF算法對機器人的地圖構建結果進行分析。具體如圖2所示。

圖2 地圖仿真環境

圖3和圖4 分別是傳統RBPF 算法和改進的RBPF 算法的仿真結果。對比可知，改進的RBPF 算法很好地避免了路徑周邊較多毛刺的問題，提高了地圖構建的清晰度。

圖3 傳統RBPF算法仿真

圖4 改進RBPF算法

1.3 地圖三值化

所謂三值化［6］，就是將0～255 的灰度映射為0，128，255，從狀態估計的角度來說就是將一個概率密度集中到這三個點上。

圖像處理的步驟是先三值化，然后分別提取x軸和y軸方向的障礙物，再合并x軸和y軸兩張圖，最后通過鄰域過濾孤立點。實驗結果如圖5和圖6所示。

圖5 原灰度圖

圖6 三值化圖像

2 機器人路徑規劃

2.1 A*算法

目前，成熟的路徑規劃方法包括A*算法、快速隨機樹、人工勢場法和人工力矩法。而A*算法在全局路徑規劃算法中更有效。傳統的A*算法是一種典型的啟發式算法。A*算法的成本函數定義如式（4）所示：

其中：n是當前節點；f(n)是賦值函數的總和；g(n)是從起點到節點的實際路徑替代值；h(n)是從節點到終點的估計替代值。

h(n) 可以由節點與終點之間的歐氏距離計算得出：

其中：（x1,y1）、（x2,y2）分別表示兩節點n1,n2的坐標。

圖7 A*算法仿真

2.2 蟻群算法

蟻群算法是由Dorigo 等［7］于1992 年提出的，該算法基于模擬螞蟻尋找食物的先進過程。具體表示如圖8所示。

圖8 蟻群算法路徑規劃仿真

2.3 蟻群算法的實現流程

（1）狀態轉移概率規則。蟻群算法主要是通過狀態轉移概率規則來對下一節點進行選擇，這是蟻群算法的核心思想，狀態轉移概率公式如式（6）所示：

（2）信息素的更新。蟻群算法的主要特點是，每次迭代時信息素的值都會得到更新，并會自行生成一個解。信息素關聯兩個區域節點邊緣，并通過公式（7）更新：

其中：τ0為信息素初始值；ρ為［0, 1］區間的可調參數。

螞蟻完成最短路徑信息素更新的過程如式（8）所示：

其中：Δτi,j=1/L*，L*為最短路徑行走的長度；ρ為［0,1］區間的可調參數。

蟻群算法的實現流程如圖9所示。

圖9 蟻群算法的實現流程

3 算法融合

3.1 A*和蟻群算法融合的意義

傳統的A*算法生成的路徑非最優的問題已經引起了許多學者的關注。對于此問題，可以利用蟻群算法對A*算法進行優化，來提高A*算法在路徑規劃方面的迭代效率，改進A*算法的尋徑速度以及路徑規劃的準確率。

3.2 A*和蟻群算法融合的仿真

蟻群算法與融合算法的仿真結果分別如圖10 和圖11 所示。由仿真結果可知，融合之后的算法路徑規劃的迭代次數在80 回左右，而未融合的算法的迭代次數已經超出了100次。為了防止結果的偶然性，繼續對算法進行仿真實驗。

圖10 蟻群算法仿真

圖11 融合算法仿真

實驗結果表明，未融合的蟻群算法的迭代次數遠遠在100次以上，而融合之后的算法路徑規劃的迭代次數在90 次左右，可見改進后的算法具有更好的結果，可以更快的規劃出一條最優路徑，再次證明了改進算法的可行性。

圖12 單蟻群算法仿真

圖13 A＊與蟻群融合算法仿真

4 結語

本文以SLAM 作為研究內容，以基于ROS系統的移動機器人系統為研究對象，主要完成以下工作：

（1）根據真實的實驗環境構建了仿真環境模型，并在gazebo 和rviz 仿真軟件中對原始的RBPF 算法和改進后的算法進行仿真建圖實驗對比。

（2）對圖像進行了三值化處理，去除了仿真地圖周圍的毛刺和棱角，解決了地圖的重影問題，提高了地圖的精度。

（3）將傳統的蟻群算法與A*算法相融合，改進后的算法與原算法相比收斂速度更快。