基于雙層地圖的分段式路徑規(guī)劃*

光興嶼，蔣 林,b

(武漢科技大學(xué) a.冶金裝備及其控制教育部重點(diǎn)實(shí)驗(yàn)室；b.機(jī)器人與智能系統(tǒng)研究院，武漢 430081)

0 引言

移動(dòng)機(jī)器人的導(dǎo)航是移動(dòng)機(jī)器人研究的重要組成部分之一，而路徑規(guī)劃一直是移動(dòng)機(jī)器人研究方向上的熱點(diǎn)與難點(diǎn)，尋求一條路徑最短、收斂最快、能夠避開(kāi)障礙的最優(yōu)路徑，對(duì)解決機(jī)器人導(dǎo)航問(wèn)題具有重要意義。

按照對(duì)機(jī)器人工作環(huán)境信息的掌握程度，機(jī)器人路徑規(guī)劃可以分為全局路徑規(guī)劃和局部路徑規(guī)劃兩部分[1]。其中全局路徑規(guī)劃是指已知關(guān)于機(jī)器人工作環(huán)境的所有信息、機(jī)器人位姿信息與終點(diǎn)，根據(jù)環(huán)境地圖按照一定的算法搜索一條最優(yōu)或者近似最優(yōu)的無(wú)碰撞路徑，根據(jù)對(duì)地圖構(gòu)建的方法，分為柵格法[2]、拓?fù)浞╗3]等。局部路徑規(guī)劃是一種邊探測(cè)工作環(huán)境邊進(jìn)行路徑規(guī)劃的方法[4]，可與全局路徑規(guī)劃配合使用，用于動(dòng)態(tài)環(huán)境中的避障，局部路徑規(guī)劃主要有、遺傳算法[5]、人工勢(shì)場(chǎng)法[6]、滾動(dòng)窗口算法[7]等。

現(xiàn)有的全局路徑規(guī)劃算法在路徑規(guī)劃中一般通過(guò)加入膨脹系數(shù)來(lái)解決在規(guī)劃經(jīng)過(guò)門的路徑時(shí)，機(jī)器人會(huì)過(guò)于靠近障礙物的問(wèn)題[8]，但當(dāng)膨脹系數(shù)過(guò)低時(shí)，機(jī)器人離障礙物不會(huì)足夠遠(yuǎn)，而當(dāng)膨脹系數(shù)過(guò)高時(shí)，則會(huì)導(dǎo)致機(jī)器人無(wú)法通過(guò)一些比機(jī)器人自身略寬的門。

本文通過(guò)結(jié)合對(duì)柵格地圖的預(yù)處理，通過(guò)加入了一層門點(diǎn)圖層，對(duì)傳統(tǒng)的全局路徑規(guī)劃進(jìn)行優(yōu)化，使其在實(shí)際環(huán)境中能夠更快的到達(dá)終點(diǎn)。

1 全局路徑規(guī)劃介紹

機(jī)器人的全局路徑規(guī)劃可以分為兩部分，第一部分是對(duì)環(huán)境地圖的構(gòu) 建，第二部分是路徑規(guī)劃方法。

1.1 占用柵格地圖的建立

地圖的構(gòu)建就是將機(jī)器人的工作區(qū)域轉(zhuǎn)化為機(jī)器人可以識(shí)別的數(shù)學(xué)模型，而柵格法是將機(jī)器人移動(dòng)空間上建立二維的直角坐標(biāo)系，并以固定尺寸對(duì)機(jī)器人的工作環(huán)境進(jìn)行分割。

占用柵格的基本思想就是用一系列隨機(jī)變量來(lái)表示地圖，每個(gè)隨機(jī)變量是一個(gè)二值數(shù)據(jù)，表示該位置是否被占用，占用柵格地圖構(gòu)建算法對(duì)以上隨機(jī)變量進(jìn)行近似后驗(yàn)估計(jì)。

在通常的尺度地圖中，對(duì)于一個(gè)點(diǎn)，它要么有(Occupied狀態(tài)，下面用1來(lái)表示)障礙物，要么沒(méi)有(Free狀態(tài)，下面用0來(lái)表示)障礙物(概率值)。在占據(jù)柵格地圖中，對(duì)于一個(gè)點(diǎn)，用p(s=100 )來(lái)表示它是Free狀態(tài)的概率，用p(s=0)來(lái)表示它是Occupied狀態(tài)的概率。通過(guò)引入兩者的比值Odd(s)來(lái)作為點(diǎn)的狀態(tài)[9]：

(1)

對(duì)于一個(gè)點(diǎn)，新來(lái)了一個(gè)測(cè)量值(Measurement，z～{0,1})之后需要更新它的狀態(tài)。

(2)

具體實(shí)現(xiàn)通過(guò)貝葉斯公式實(shí)現(xiàn)：

(3)

同時(shí)對(duì)兩邊進(jìn)行對(duì)數(shù)運(yùn)算：

(4)

含有測(cè)量值的項(xiàng)就只剩下了第一項(xiàng)，這個(gè)比值即為測(cè)量值的模型，一個(gè)點(diǎn)狀態(tài)數(shù)值越大，就表示越肯定他是occupied狀態(tài)，相反數(shù)值越小，就表示越肯定它是free狀態(tài)。

在最終建成的地圖中，地圖上的柵格被分為三種，一是障礙物柵格，無(wú)法通過(guò)，對(duì)此類柵格賦值為99，二是未知柵格，對(duì)此類柵格賦值為-1，三是可行駛區(qū)域柵格，對(duì)此類柵格賦值為0，這樣機(jī)器人就可以根據(jù)柵格的灰度值區(qū)分障礙物和可通行區(qū)域。相比于拓?fù)浞?，柵格法具有?jiǎn)單易于實(shí)現(xiàn)等優(yōu)點(diǎn)，是目前解決路徑規(guī)劃問(wèn)題最常用的環(huán)境建模方法[10]。

1.2 A*算法介紹

A*算法在機(jī)器人全局路徑尋優(yōu)和規(guī)劃方面應(yīng)用非常廣泛。A*算法是一種啟發(fā)式搜索算法，通過(guò)在傳統(tǒng)的Dijkstra算法的基礎(chǔ)上引入了啟發(fā)式函數(shù)，使其相較于Dijkstra路徑搜尋效率更高，是一種在靜態(tài)路徑規(guī)劃求解最短路徑最有效的方法[11]，其估價(jià)函數(shù)為[12]f(n)=g(n)+h(n)。其中h(n)為啟發(fā)函數(shù)，值為n到目標(biāo)點(diǎn)的估計(jì)代價(jià)，一般選取兩點(diǎn)間的曼哈頓距離作為代價(jià)值，如下式所示：

h(n)=abs(xi-xj)+abs(yi-yj)

(5)

其中,點(diǎn)(xi,yi)為當(dāng)前節(jié)點(diǎn)n，點(diǎn)(xj,yj)為目標(biāo)節(jié)點(diǎn)。g(n)為從初始節(jié)點(diǎn)到當(dāng)前節(jié)點(diǎn)n的實(shí)際代價(jià)，柵格地圖中任意節(jié)點(diǎn)都有8個(gè)相鄰節(jié)點(diǎn)，如圖1所示。

圖1 每一步所增加的代價(jià)值

規(guī)劃的路徑通過(guò)前后左右4個(gè)柵格則g(n)=g(n)+1，若通過(guò)斜向的4個(gè)柵格則g(n)=g(n)+1.4，從而得到g(n)的值。

1.3 A*算法實(shí)現(xiàn)流程

A*算法具體的實(shí)現(xiàn)流程圖如圖2所示。

圖2 A*算法流程圖

圖中符號(hào)定義如下：

N為路徑規(guī)劃中已規(guī)劃過(guò)的某一個(gè)節(jié)點(diǎn)；g(n)為初始節(jié)點(diǎn)s到n的實(shí)際代價(jià)；h(n)為啟發(fā)函數(shù)，n到目標(biāo)點(diǎn)的啟發(fā)代價(jià)；f(n)—g(n)+h(n)，為節(jié)點(diǎn)n到目標(biāo)點(diǎn)的估價(jià)函數(shù)；OPEN LIST為保存所有已生成而未考察的節(jié)點(diǎn)；CLOSED LIST為記錄已訪問(wèn)過(guò)的所有節(jié)點(diǎn)。

1.4 實(shí)際環(huán)境中存在的缺陷

在實(shí)際的是室內(nèi)環(huán)境中，情況會(huì)變得復(fù)雜。在大部分情況下A*算法在柵格地圖上規(guī)劃出的路徑是理論上的最佳路徑，但在規(guī)劃經(jīng)過(guò)門路徑時(shí)，理論上的最佳路徑經(jīng)常需要機(jī)器人靠近障礙物，但在依靠激光雷達(dá)進(jìn)行定位的機(jī)器人上，因?yàn)榧す饫走_(dá)在探測(cè)近距離時(shí)存在盲區(qū),導(dǎo)致移動(dòng)機(jī)器人在接近障礙物的時(shí)候，經(jīng)常無(wú)法準(zhǔn)確的進(jìn)行定位，從而在原地旋轉(zhuǎn)，導(dǎo)致導(dǎo)航的耗時(shí)增加，從而影響了導(dǎo)航的效率，而加入了膨脹系數(shù)后，雖然一定程度上緩解了上述問(wèn)題，卻會(huì)導(dǎo)致很多時(shí)候移動(dòng)機(jī)器人在面對(duì)明顯比自己要寬的門時(shí)無(wú)法通過(guò)，并且規(guī)劃的路徑還存在轉(zhuǎn)折次數(shù)過(guò)多和轉(zhuǎn)折角過(guò)大等問(wèn)題。為了解決上述問(wèn)題，本文提出了一種基于多層地圖的分段式導(dǎo)航算法，通過(guò)對(duì)原始的柵格地圖加入一次預(yù)處理的流程，并加入一層門點(diǎn)圖層，以完成對(duì)路徑規(guī)劃的優(yōu)化。

2 基于雙層地圖的分段式路徑規(guī)劃

2.1 對(duì)柵格地圖的預(yù)處理

柵格地圖具有簡(jiǎn)單易維護(hù)的特點(diǎn)，但因其結(jié)構(gòu)的局限性，只能通過(guò)數(shù)字(1～100)來(lái)表示障礙物的代價(jià)值，而無(wú)法表示障礙物以外的物體類型，因而在原圖層的基礎(chǔ)上再引入一張圖層，來(lái)表示地圖上門的位置。首先，需要得到一張完整的環(huán)境的柵格地圖，并對(duì)地圖進(jìn)行二值化處理，從而提取出地圖上障礙物的形狀。將二值化后的地圖視為一個(gè)由0與255組成的列表，其中障礙物柵格的灰度為0，其余柵格灰度為255。并對(duì)地圖上的障礙物進(jìn)行空心化處理。分別從4個(gè)方向?qū)Χ祷蟮臇鸥竦貓D進(jìn)行縮進(jìn)處理，C即為需要?jiǎng)h除的點(diǎn)，1為障礙物柵格，0為灰度為255的柵格，空白處為任意柵格，如圖3所示。

圖3 柵格地圖縮進(jìn)示意圖

最后將分別4張縮放所得到的地圖進(jìn)行疊加，得到最終的空心化后的柵格地圖。

接下來(lái)進(jìn)行對(duì)門點(diǎn)圖層的制作。對(duì)空心化后的柵格地圖進(jìn)行處理。取空心化后柵格地圖上灰度為0的任意點(diǎn)x，設(shè)點(diǎn)x的坐標(biāo)為[i,j]，將，通過(guò)下列公式：

img[i,j+1]+img[i-1,j+1]+img[i-1,j]+
img[i-1,j-1]+img[i,j-1]+img[i+1,j-1]+
img[i+1,j]+img[i+1,j+1]=255

(6)

判斷，式中的img[i,j]即為圖像img中坐標(biāo)為[i,j]的像素點(diǎn)的灰度值。再對(duì)符合條件的點(diǎn)進(jìn)行配對(duì)，將最近的4個(gè)點(diǎn)進(jìn)行匹配為一組4個(gè)元素的列表，通過(guò)設(shè)定一個(gè)閾值，即為每個(gè)列表中若有任意兩點(diǎn)的距離大于閾值，則刪除此列表，從而篩選出正確的門點(diǎn)，再將每個(gè)列表中的4個(gè)點(diǎn)分別連接，并將四邊形填充為灰度0，從而得到門點(diǎn)圖層。

2.2 對(duì)路徑的優(yōu)化

通過(guò)將表示門的圖層與原始地圖同時(shí)載入，可以將表示門的圖層用于移動(dòng)機(jī)器人的導(dǎo)航。首先，需要對(duì)門點(diǎn)的坐標(biāo)進(jìn)行處理，在現(xiàn)實(shí)環(huán)境中，門在地圖中的朝向不確定，通過(guò)門的4個(gè)點(diǎn)的坐標(biāo)來(lái)對(duì)原有的路徑進(jìn)行分段。設(shè)定地圖中代表門的4個(gè)點(diǎn)的坐標(biāo)分別為(x1,y1)、(x2,y2)、(x3,y3)、(x4,y4)。因?yàn)榇蟛糠智闆r下門的形狀可以視為一個(gè)長(zhǎng)方形來(lái)處理，且大部分情況下長(zhǎng)方形的長(zhǎng)邊都是可以通過(guò)的邊，而短邊是不能通過(guò)的，因此，取四邊形[(x1,y1),(x2,y2),(x3,y3),(x4,y4)]最短的兩條邊，假設(shè)所求得最短的兩條邊分別為[(x1,y1),(x2,y2)]與[(x3,y3),(x4,y4)]，設(shè)(x5,y5)與(x6,y6)分別為長(zhǎng)方形的兩條短邊的中點(diǎn)，再通過(guò)公式：

(7)

可求得目標(biāo)點(diǎn)的斜率k，再通過(guò)設(shè)定距離L，可以求得與點(diǎn)(x7,y7)在方向k上相距離分別為L(zhǎng)與-L的兩個(gè)點(diǎn)，即為所求目標(biāo)點(diǎn)，其中點(diǎn)(x7,y7)為列表中4個(gè)點(diǎn)的中點(diǎn),所需目標(biāo)點(diǎn)由下列公式求得：

(8)

如圖4所示。

圖4 對(duì)門點(diǎn)的處理

在傳統(tǒng)A*算法規(guī)劃的路徑上先計(jì)算規(guī)劃出的路徑在門點(diǎn)圖層上的代價(jià)值，若當(dāng)前規(guī)劃路徑在門點(diǎn)圖層上的代價(jià)值為0，則表示該路徑并沒(méi)有經(jīng)過(guò)門，這不需要啟用路徑優(yōu)化，若該路徑在門點(diǎn)圖層上的代價(jià)值大于99，即當(dāng)前規(guī)劃出的路徑經(jīng)過(guò)了門，需要啟用分段路徑規(guī)劃?rùn)C(jī)制，并對(duì)當(dāng)前的全局路徑規(guī)劃進(jìn)行重新規(guī)劃。通過(guò)對(duì)每個(gè)門周圍的目標(biāo)點(diǎn)距離為L(zhǎng)的范圍進(jìn)行判斷，若范圍內(nèi)存在軌跡，則將目標(biāo)點(diǎn)添加至列表List中，并通過(guò)計(jì)算List中離目標(biāo)點(diǎn)最近的各點(diǎn)在軌跡中的排序，對(duì)列表中的點(diǎn)依次進(jìn)行排序，并對(duì)路徑進(jìn)行重新規(guī)劃，將起點(diǎn)和中點(diǎn)分別加入列表的第一位和末位，并分別通過(guò)A*算法，將各個(gè)點(diǎn)按列表的順序連接起來(lái)，生成新的路徑，從而完成對(duì)路徑的優(yōu)化。

3 仿真分析

本次仿真實(shí)驗(yàn)的仿真環(huán)境基于開(kāi)源的機(jī)器人仿真軟件gazebo，機(jī)器人的控制平臺(tái)則選用了Ubuntu16.04+開(kāi)源機(jī)器人操作系統(tǒng)Ros Kinetic，建立的仿真環(huán)境如圖5所示。

圖5 仿真環(huán)境

通過(guò)基于2D激光雷達(dá)的Slam-Gmapping[14]算法建立出的環(huán)的柵格地圖如圖6所示。

圖6 柵格地圖

對(duì)所建好的柵格地圖空心化后，如圖7所示。

圖7 空心化后的柵格地圖

最終所獲得的門點(diǎn)圖層如圖8所示。

圖8 門點(diǎn)圖層

通過(guò)Ros下的map_server節(jié)點(diǎn)將原始柵格地圖與門點(diǎn)地圖同時(shí)載入，其中原始柵格地圖載入的話題名為/map，而門點(diǎn)圖層載入的話題名為/map1，在可視化圖形界面Rviz下顯示如圖9所示。

圖9 Rviz界面

我們通過(guò)ros下的navigation下的導(dǎo)航包導(dǎo)航出的路徑與我們優(yōu)化后的路徑進(jìn)行對(duì)比，其中路徑A為未優(yōu)化的路徑，而路徑B為我們優(yōu)化后的路徑，對(duì)比如圖10所示。

圖10 結(jié)果比對(duì)

可見(jiàn)相比于原始路徑A，優(yōu)化后的路徑B，在通過(guò)門的時(shí)候，不存在過(guò)于靠近障礙物的問(wèn)題，從而避免了進(jìn)入激光雷達(dá)的盲區(qū)導(dǎo)致的機(jī)器人定位丟失的問(wèn)題。

4 試驗(yàn)驗(yàn)證與分析

4.1 試驗(yàn)軟硬件設(shè)計(jì)

本次試驗(yàn)使用課題組自主研發(fā)的機(jī)器人平臺(tái)，機(jī)器人采用單激光雷達(dá)進(jìn)行定位建圖與導(dǎo)航。所用的激光雷達(dá)傳感器型號(hào)為SICK LMS111，激光雷達(dá)的探測(cè)角度為180°，角度分辨率為0.5°。控制器則采用了型號(hào)為intel nuc6i7KYK的mini PC，mini PC的cpu型號(hào)為intel core i7-6770HQ，內(nèi)存8g。試驗(yàn)所用的軟件平臺(tái)為ubuntu16.04+ROS kinetic。試驗(yàn)用的移動(dòng)機(jī)器人平臺(tái)如圖11所示。

圖11 實(shí)驗(yàn)用的移動(dòng)機(jī)器人

4.2 試驗(yàn)的結(jié)果及分析

通過(guò)紙箱子的擺放來(lái)模擬房間中的門，試驗(yàn)所用的室內(nèi)環(huán)境如圖12所示。

圖12 實(shí)驗(yàn)環(huán)境

通過(guò)基于ros的google cartographer[15]算法對(duì)房間建圖,對(duì)所得到的柵格地圖進(jìn)行處理，如圖13所示。

圖13 對(duì)柵格地圖進(jìn)行處理

將所得到的門點(diǎn)圖層載入后，任意規(guī)劃一條經(jīng)過(guò)門的路徑，最終得到的路徑與未經(jīng)過(guò)如圖14所示。

(a) 全局路徑規(guī)劃 (b) 優(yōu)化后的軌跡 圖14 優(yōu)化后的軌跡與傳統(tǒng)A*算法實(shí)驗(yàn)對(duì)比

圖14a中的軌跡為傳統(tǒng)A*算法全局路徑規(guī)劃，圖14b中的軌跡為經(jīng)過(guò)優(yōu)化后的軌跡，由實(shí)驗(yàn)結(jié)果可知，當(dāng)規(guī)劃經(jīng)過(guò)門的路徑時(shí)，算法檢測(cè)到路徑在門點(diǎn)圖層上的代價(jià)值大于99，即路徑經(jīng)過(guò)門，從而對(duì)路徑進(jìn)行了重新規(guī)劃?？梢院苊黠@的看到，生成的路徑雖然相比原始路徑要長(zhǎng)，但對(duì)于機(jī)器人更加安全，機(jī)器人不會(huì)出現(xiàn)離障礙物過(guò)近的情況。

5 結(jié)論

針對(duì)機(jī)器人移動(dòng)機(jī)器人通過(guò)傳統(tǒng)A*算法路徑規(guī)劃經(jīng)過(guò)門時(shí)因規(guī)劃的路徑靠近障礙物，導(dǎo)致定位丟失的問(wèn)題，提出了基于雙層地圖的路徑規(guī)劃而的方法。傳統(tǒng)的A*算法規(guī)劃的路徑距離最短(前提是路徑存在)，但路徑常沿障礙物邊緣前進(jìn)，在經(jīng)過(guò)門時(shí)會(huì)因?yàn)榧す饫走_(dá)的近角盲區(qū)導(dǎo)致定位丟失甚至發(fā)生危險(xiǎn)?；诙鄬拥貓D的分段路徑規(guī)劃可以使移動(dòng)機(jī)器人在經(jīng)過(guò)門時(shí)不會(huì)離障礙物過(guò)避免了移動(dòng)機(jī)器人因?yàn)殡x障礙物過(guò)近而無(wú)法定位的問(wèn)題。首先對(duì)已經(jīng)建立的柵格地圖進(jìn)行空心化，從而提取出柵格地圖上的門的坐標(biāo)，并對(duì)門進(jìn)行填充，制作成門點(diǎn)圖層，再將原始柵格地圖與門點(diǎn)圖層同時(shí)用于移動(dòng)機(jī)器人的定位，當(dāng)移動(dòng)機(jī)器人規(guī)劃的路徑需要經(jīng)過(guò)門點(diǎn)時(shí)，則啟動(dòng)路徑優(yōu)化機(jī)制，對(duì)原始路徑進(jìn)行優(yōu)化。仿真實(shí)驗(yàn)與現(xiàn)實(shí)環(huán)境中的實(shí)驗(yàn)表明基于多層地圖的分段路徑規(guī)劃規(guī)劃出的路徑可以使移動(dòng)機(jī)器人不會(huì)離障礙物過(guò)近，降低了移動(dòng)機(jī)器人在導(dǎo)航時(shí)丟失位置的幾率，具有跟高的品質(zhì)。