基于ROS下移動(dòng)機(jī)器人地圖構(gòu)建與路徑規(guī)劃研究

馬舸瀚，楊 旗

（沈陽(yáng)理工大學(xué)機(jī)械工程學(xué)院，沈陽(yáng) 110000）

0 引言

近幾十年來(lái)，智能移動(dòng)機(jī)器人因其高靈活性和機(jī)動(dòng)性的特點(diǎn)而被廣泛應(yīng)用［1］。在大多數(shù)情況下，自主導(dǎo)航對(duì)于智能移動(dòng)機(jī)器人是必要的，特別是路徑規(guī)劃，是導(dǎo)航中最重要的部分之一。因此，如何在部分已知或完全未知的環(huán)境中構(gòu)建具有理想路徑規(guī)劃器的移動(dòng)機(jī)器人已成為研究熱點(diǎn)［2］。

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

針對(duì)上述問(wèn)題，研究工作提出了一種移動(dòng)機(jī)器人優(yōu)化路徑規(guī)劃模型。實(shí)驗(yàn)結(jié)果表明,該優(yōu)化后的算法模型能夠獲得比現(xiàn)有的路徑優(yōu)化算法更快、更平滑、更安全的路徑。

1 粒子濾波算法

1.1 Gmapping算法

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

1.2 基于RBPF算法的改進(jìn)

傳統(tǒng)的RBPF 算法使用粒子濾波算法來(lái)估計(jì)機(jī)器人的狀態(tài)［5］。然而，隨著粒子數(shù)量的增加，計(jì)算復(fù)雜度顯著增加。隨著算法迭代次數(shù)的增加，粒子不可避免地會(huì)發(fā)生退化。

因此，針對(duì)以上RBPF 算法的問(wèn)題，本研究提出了一種改進(jìn)的RBPF 映射算法來(lái)實(shí)現(xiàn)SLAM映射，即利用改進(jìn)的分布和自適應(yīng)重采樣算法。

（1）提議分布的改進(jìn)。本次分布信息對(duì)比了加入激光雷達(dá)觀測(cè)的分布模型p（z|x）和歷程采樣后的高斯分布模型p（x|x',u），從圖1 可以看出，兩種分布模型的差別較大，采用激光雷達(dá)觀測(cè)的分布模型可以提高機(jī)器人姿態(tài)移動(dòng)的準(zhǔn)確率，效果較好。

圖1 兩種運(yùn)動(dòng)模型概率分布

利用上述激光雷達(dá)分布模型得到最新的觀測(cè)數(shù)據(jù)，將該觀測(cè)數(shù)據(jù)應(yīng)用于新粒子的采集，可以獲得改進(jìn)的提議分布，具體如式（1）所示：

（2）目標(biāo)分布通常只有幾個(gè)峰值，在大多數(shù)情況下只有一個(gè)峰值。為了簡(jiǎn)化計(jì)算，直接收集峰值附近的值來(lái)模擬所提出的分布。

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

（3）重采樣依據(jù)。映射采用了改進(jìn)的粒子采樣分布，這意味著粒子降解較低，不需要每次都重采樣，因此采用自適應(yīng)重采樣方法來(lái)確定粒子采樣。首先，根據(jù)公式計(jì)算出當(dāng)前粒子權(quán)重分散性量度Neff。計(jì)算過(guò)程如式（3）所示：

然后將其與設(shè)置的粒子數(shù)N進(jìn)行比較，當(dāng)Neff小于N/2時(shí)需進(jìn)行重采樣。

（4）地圖的更新。為了驗(yàn)證改進(jìn)RBPF 算法的有效性，利用ROS 機(jī)器人系統(tǒng)軟件對(duì)地圖環(huán)境進(jìn)行構(gòu)建，并利用改進(jìn)的RBPF算法對(duì)機(jī)器人的地圖構(gòu)建結(jié)果進(jìn)行分析。具體如圖2所示。

圖2 地圖仿真環(huán)境

圖3和圖4 分別是傳統(tǒng)RBPF 算法和改進(jìn)的RBPF 算法的仿真結(jié)果。對(duì)比可知，改進(jìn)的RBPF 算法很好地避免了路徑周邊較多毛刺的問(wèn)題，提高了地圖構(gòu)建的清晰度。

圖3 傳統(tǒng)RBPF算法仿真

圖4 改進(jìn)RBPF算法

1.3 地圖三值化

所謂三值化［6］，就是將0～255 的灰度映射為0，128，255，從狀態(tài)估計(jì)的角度來(lái)說(shuō)就是將一個(gè)概率密度集中到這三個(gè)點(diǎn)上。

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

圖5 原灰度圖

圖6 三值化圖像

2 機(jī)器人路徑規(guī)劃

2.1 A*算法

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

其中：n是當(dāng)前節(jié)點(diǎn)；f(n)是賦值函數(shù)的總和；g(n)是從起點(diǎn)到節(jié)點(diǎn)的實(shí)際路徑替代值；h(n)是從節(jié)點(diǎn)到終點(diǎn)的估計(jì)替代值。

h(n) 可以由節(jié)點(diǎn)與終點(diǎn)之間的歐氏距離計(jì)算得出：

其中：（x1,y1）、（x2,y2）分別表示兩節(jié)點(diǎn)n1,n2的坐標(biāo)。

圖7 A*算法仿真

2.2 蟻群算法

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

圖8 蟻群算法路徑規(guī)劃仿真

2.3 蟻群算法的實(shí)現(xiàn)流程

（1）狀態(tài)轉(zhuǎn)移概率規(guī)則。蟻群算法主要是通過(guò)狀態(tài)轉(zhuǎn)移概率規(guī)則來(lái)對(duì)下一節(jié)點(diǎn)進(jìn)行選擇，這是蟻群算法的核心思想，狀態(tài)轉(zhuǎn)移概率公式如式（6）所示：

（2）信息素的更新。蟻群算法的主要特點(diǎn)是，每次迭代時(shí)信息素的值都會(huì)得到更新，并會(huì)自行生成一個(gè)解。信息素關(guān)聯(lián)兩個(gè)區(qū)域節(jié)點(diǎn)邊緣，并通過(guò)公式（7）更新：

其中：τ0為信息素初始值；ρ為［0, 1］區(qū)間的可調(diào)參數(shù)。

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

其中：Δτi,j=1/L*，L*為最短路徑行走的長(zhǎng)度；ρ為［0,1］區(qū)間的可調(diào)參數(shù)。

蟻群算法的實(shí)現(xiàn)流程如圖9所示。

圖9 蟻群算法的實(shí)現(xiàn)流程

3 算法融合

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

傳統(tǒng)的A*算法生成的路徑非最優(yōu)的問(wèn)題已經(jīng)引起了許多學(xué)者的關(guān)注。對(duì)于此問(wèn)題，可以利用蟻群算法對(duì)A*算法進(jìn)行優(yōu)化，來(lái)提高A*算法在路徑規(guī)劃方面的迭代效率，改進(jìn)A*算法的尋徑速度以及路徑規(guī)劃的準(zhǔn)確率。

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

蟻群算法與融合算法的仿真結(jié)果分別如圖10 和圖11 所示。由仿真結(jié)果可知，融合之后的算法路徑規(guī)劃的迭代次數(shù)在80 回左右，而未融合的算法的迭代次數(shù)已經(jīng)超出了100次。為了防止結(jié)果的偶然性，繼續(xù)對(duì)算法進(jìn)行仿真實(shí)驗(yàn)。

圖10 蟻群算法仿真

圖11 融合算法仿真

實(shí)驗(yàn)結(jié)果表明，未融合的蟻群算法的迭代次數(shù)遠(yuǎn)遠(yuǎn)在100次以上，而融合之后的算法路徑規(guī)劃的迭代次數(shù)在90 次左右，可見(jiàn)改進(jìn)后的算法具有更好的結(jié)果，可以更快的規(guī)劃出一條最優(yōu)路徑，再次證明了改進(jìn)算法的可行性。

圖12 單蟻群算法仿真

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

4 結(jié)語(yǔ)

本文以SLAM 作為研究?jī)?nèi)容，以基于ROS系統(tǒng)的移動(dòng)機(jī)器人系統(tǒng)為研究對(duì)象，主要完成以下工作：

（1）根據(jù)真實(shí)的實(shí)驗(yàn)環(huán)境構(gòu)建了仿真環(huán)境模型，并在gazebo 和rviz 仿真軟件中對(duì)原始的RBPF 算法和改進(jìn)后的算法進(jìn)行仿真建圖實(shí)驗(yàn)對(duì)比。

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

（3）將傳統(tǒng)的蟻群算法與A*算法相融合，改進(jìn)后的算法與原算法相比收斂速度更快。