馬舸瀚,楊 旗
(沈陽(yáng)理工大學(xué)機(jī)械工程學(xué)院,沈陽(yáng) 110000)
近幾十年來(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)化算法更快、更平滑、更安全的路徑。
Gmapping 算法[4]是目前應(yīng)用最為廣泛的一種2D 激光SLAM 算法,與卡爾曼濾波相比,局限性較小,應(yīng)用范圍廣。
傳統(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算法
所謂三值化[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 三值化圖像
目前,成熟的路徑規(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*算法仿真
蟻群算法是由Dorigo 等[7]于1992 年提出的,該算法基于模擬螞蟻尋找食物的先進(jìn)過(guò)程。具體表示如圖8所示。
圖8 蟻群算法路徑規(guī)劃仿真
(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)流程
傳統(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)確率。
蟻群算法與融合算法的仿真結(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*與蟻群融合算法仿真
本文以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)后的算法與原算法相比收斂速度更快。