移動(dòng)機(jī)器人路徑規(guī)劃算法仿真及實(shí)現(xiàn)

郭昭烽，謝 玲，劉紅英，邱池健

(1.光大環(huán)境科技(中國(guó))有限公司，江蘇 南京 211102；2.南京理工大學(xué)紫金學(xué)院，江蘇 南京 210046)

louisguo1983@163.com;12011833@qq.com;328392801@qq.com;865457907@qq.com

1 引言(Introduction)

路徑規(guī)劃技術(shù)是機(jī)器人領(lǐng)域研究的核心之一。路徑規(guī)劃是指按照一定的評(píng)價(jià)標(biāo)準(zhǔn)(如選擇最短的規(guī)劃路徑、最短的規(guī)劃時(shí)間或者最小的工作代價(jià)等)，在存有障礙物的環(huán)境地圖中，從起點(diǎn)到目標(biāo)點(diǎn)之間找到可行地避免碰撞的路徑。路徑規(guī)劃大致可分為全局路徑規(guī)劃和局部路徑規(guī)劃兩大類(lèi)，又有靜態(tài)、動(dòng)態(tài)兩個(gè)分支。全局靜態(tài)規(guī)劃算法從經(jīng)典的BFS、DFS發(fā)展到后來(lái)的Dijkstra算法和A*算法，當(dāng)外部環(huán)境不斷發(fā)生變化時(shí)，又由A*算法演變到D*算法。除去經(jīng)典的路徑規(guī)劃算法之外，如今人工智能相關(guān)算法也為機(jī)器人路徑規(guī)劃開(kāi)拓了另一個(gè)方向，如蟻群算法、模擬退火算法、神經(jīng)網(wǎng)絡(luò)、粒子群算法和遺傳算法等。

在機(jī)器人運(yùn)動(dòng)的過(guò)程中起到?jīng)Q定性作用的是路徑規(guī)劃技術(shù)。如何提高機(jī)器人路徑規(guī)劃的技術(shù)，從實(shí)質(zhì)上看，是如何創(chuàng)造出效率性更高的算法，或是針對(duì)機(jī)器人實(shí)際應(yīng)用場(chǎng)景，將已有的算法進(jìn)行有針對(duì)性的優(yōu)化，極大程度地促進(jìn)機(jī)器人技術(shù)的發(fā)展，這對(duì)整個(gè)機(jī)器人的發(fā)展領(lǐng)域具有極其重要的意義。

2 路徑規(guī)劃原理(Principles of path planning)

機(jī)器人路徑規(guī)劃的過(guò)程中，主要分三步進(jìn)行。

首先是根據(jù)已知的地圖信息來(lái)建模，障礙點(diǎn)、可行點(diǎn)、加權(quán)值等相關(guān)的地圖數(shù)據(jù)參數(shù)被存儲(chǔ)成可被算法識(shí)別的數(shù)據(jù)流。

其次，選擇合適的算法進(jìn)行研究，根據(jù)機(jī)器人所需要執(zhí)行的任務(wù)進(jìn)行實(shí)際優(yōu)化，這一點(diǎn)可以使機(jī)器人具有應(yīng)對(duì)單一任務(wù)的專(zhuān)一性，根據(jù)實(shí)際情況消除不必要的計(jì)算過(guò)程，提高機(jī)器人的路徑規(guī)劃效率。

最后，根據(jù)改進(jìn)的路徑規(guī)劃算法為機(jī)器人尋找一條由機(jī)器人當(dāng)前位置到目標(biāo)點(diǎn)位置的無(wú)碰撞最優(yōu)路徑，機(jī)器人根據(jù)路徑信息及配置的物理參數(shù)進(jìn)行尋路。機(jī)器人路徑規(guī)劃原理如圖1所示。

圖1 路徑規(guī)劃原理Fig.1 Principles of path planning

3 路徑規(guī)劃環(huán)境建模(Path planning environment modeling)

研究機(jī)器人路徑規(guī)劃，離不開(kāi)對(duì)機(jī)器人移動(dòng)環(huán)境的數(shù)據(jù)建模，環(huán)境建模是算法執(zhí)行的關(guān)鍵，算法要依據(jù)環(huán)境信息給出結(jié)果。將機(jī)器人移動(dòng)的實(shí)際三維環(huán)境空間進(jìn)行規(guī)劃劃分，提取環(huán)境的信息特征，抽象計(jì)算機(jī)可以存儲(chǔ)和操作的數(shù)字信息。算法能根據(jù)建模的環(huán)境信息進(jìn)行運(yùn)算和反饋。對(duì)于局部路徑規(guī)劃而言，環(huán)境建模需對(duì)環(huán)境出現(xiàn)的新變量進(jìn)行及時(shí)處理，能夠高效地、準(zhǔn)確地將環(huán)境信息進(jìn)行更新。這樣機(jī)器人就可以利用建立起來(lái)的地圖信息進(jìn)行路徑搜索。常見(jiàn)的環(huán)境建模方法有柵格模型、幾何模型、拓?fù)淠Ｐ汀⑷荒Ｐ偷取?/p>

本文采用柵格模型作為仿真實(shí)驗(yàn)的建模方法。柵格法環(huán)境建模的具體步驟為：首先將規(guī)劃環(huán)境柵格化，處理成統(tǒng)一的單元模型數(shù)據(jù)，根據(jù)障礙物在環(huán)境中的分布信息，將對(duì)應(yīng)的單元柵格處理成障礙柵格，其余為自由柵格。其次鏈接各個(gè)柵格處理成圖，然后在圖中搜尋一條可行路徑，將路徑的坐標(biāo)存儲(chǔ)反饋給機(jī)器人，坐標(biāo)即是單元柵格的單元格編號(hào)。最后機(jī)器人根據(jù)所得編號(hào)對(duì)應(yīng)的實(shí)際坐標(biāo)進(jìn)行移動(dòng)完成路徑搜索。其優(yōu)點(diǎn)在于需要構(gòu)成的二維陣列易于實(shí)現(xiàn)，且容易被計(jì)算機(jī)存儲(chǔ)、操作和顯示，同時(shí)易于修改和擴(kuò)大地圖規(guī)模，便于實(shí)驗(yàn)數(shù)據(jù)的獲得。

4 路徑規(guī)劃算法研究及仿真(Research and simulation of path planning algorithm)

首先設(shè)置一個(gè)尺寸為15×15柵格的障礙地圖作為每個(gè)算法的統(tǒng)一輸入源，其次設(shè)置不同尺寸的地圖，對(duì)各個(gè)算法進(jìn)行路徑搜索時(shí)間的統(tǒng)計(jì)。本文對(duì)Dijkstra、A*、D*的核心算法進(jìn)行研究，在Windows 10系統(tǒng)中使用Pycharm開(kāi)發(fā)平臺(tái)，用Python語(yǔ)言對(duì)各個(gè)算法進(jìn)行編程實(shí)現(xiàn)，以及設(shè)計(jì)GUI使算法中的各個(gè)步驟進(jìn)行可視化處理，以此進(jìn)行仿真實(shí)驗(yàn)。

4.1 環(huán)境建模

使用柵格法建立一張完全有向圖，每一個(gè)柵格可以連接上下左右四個(gè)點(diǎn)。每個(gè)柵格的基礎(chǔ)屬性為笛卡爾坐標(biāo)系的坐標(biāo)(，)和一個(gè)判斷是否為自由柵格的屬性，以及一個(gè)保存可探索的柵格坐標(biāo)的數(shù)組，其他屬性根據(jù)不同算法重新構(gòu)建。柵格模型建立的地圖如圖2所示。

圖2 算法仿真的地圖Fig.2 Algorithm simulation map

白色代表障礙柵格，用數(shù)字表示的代表自由柵格。黑色柵格代表權(quán)值為1，數(shù)字3柵格代表權(quán)值為2，數(shù)字1柵格代表權(quán)值為3，數(shù)字2柵格代表權(quán)值為4。各個(gè)柵格的位置如下：

式(1)中，、代表該柵格在笛卡爾坐標(biāo)系下的坐標(biāo)位置，、代表柵格的像素寬度和高度，將在繪制GUI的時(shí)候使用。

4.2 Dijkstra算法

Dijkstra算法是從一個(gè)頂點(diǎn)到其余各頂點(diǎn)的最短路徑算法，解決的是有權(quán)圖中最短路徑問(wèn)題。其主要特點(diǎn)是從起始點(diǎn)開(kāi)始，采用貪心算法的策略，每次遍歷到始點(diǎn)距離最近且未訪問(wèn)過(guò)的頂點(diǎn)的鄰接節(jié)點(diǎn)，直到擴(kuò)展到終點(diǎn)為止。本文采用的是對(duì)BFS算法的改寫(xiě)，BFS算法中使用的地圖，默認(rèn)權(quán)值都為1，即從一個(gè)單元柵格到另外的節(jié)點(diǎn)單元柵格默認(rèn)距離都為1。單元柵格的距離默認(rèn)為1，這種情況表示在平地上運(yùn)動(dòng)，而現(xiàn)實(shí)的環(huán)境情況下，平地只是環(huán)境因素的一部分，還有山坡、低洼或者沼澤地等其他不同的環(huán)境條件，而在這些有坡度的環(huán)境因素下，機(jī)器人通過(guò)這些地方就要改變其速度，或者以時(shí)間為代價(jià)通過(guò)。也就是說(shuō)，BFS得出的路徑在有權(quán)圖中并不是最優(yōu)路徑。因此我們通過(guò)給每個(gè)自由柵格增加一個(gè)權(quán)的屬性，表示當(dāng)前柵格到其他單元柵格的距離值，在有權(quán)圖中采用Dijkstra算法求出最短路徑。

本文使用一個(gè)優(yōu)先隊(duì)列存儲(chǔ)，將這個(gè)要探索的單元格添加到隊(duì)列中，而單元柵格到單元柵格的距離為公式(2)所得：

作為隊(duì)列的優(yōu)先級(jí)編號(hào)，距離越短，優(yōu)先級(jí)越高，即優(yōu)先隊(duì)列會(huì)將當(dāng)前單元柵格到附近最短距離的單元柵格出隊(duì)，判斷其當(dāng)前的狀態(tài)和后續(xù)節(jié)點(diǎn)的狀態(tài)。

在實(shí)現(xiàn)Dijkstra算法時(shí)，我們引用Python.queue創(chuàng)建一個(gè)PriorityQueue，該隊(duì)列是一個(gè)優(yōu)先搜索隊(duì)列，不同于普通隊(duì)列，該隊(duì)列的出入隊(duì)是根據(jù)隊(duì)列中對(duì)象的優(yōu)先級(jí)而定的。該優(yōu)先隊(duì)列有兩個(gè)參數(shù)，即index和object，index為優(yōu)先隊(duì)列對(duì)象object的優(yōu)先值，優(yōu)先隊(duì)列會(huì)根據(jù)對(duì)象的優(yōu)先值進(jìn)行排序，每次得到的隊(duì)頭的對(duì)象都是優(yōu)先值最高的，對(duì)應(yīng)的index為最低數(shù)值。優(yōu)先隊(duì)列的存儲(chǔ)和讀取函數(shù)為push()和get()，將對(duì)象入隊(duì)：PriorityQueue.push([index,object])；獲取隊(duì)頭對(duì)象：PriorityQueue.get()。算法首先根據(jù)傳入的地圖信息初始化distance，將不是終點(diǎn)的節(jié)點(diǎn)對(duì)向的權(quán)值設(shè)置為無(wú)窮大。算法執(zhí)行到最后將返回兩個(gè)字典對(duì)象parent和distance，機(jī)器人將通過(guò)遍歷parent字典尋找最短路徑。而distance記錄點(diǎn)到點(diǎn)的距離，可以通過(guò)訪問(wèn)該字典求得最短路徑的長(zhǎng)度。

分別設(shè)置起始點(diǎn)(0,0)、終點(diǎn)(9,9)和起始點(diǎn)(0,0)、終點(diǎn)(14,14)進(jìn)行仿真，該算法仿真結(jié)果如圖3所示。

圖3 Dijkstra算法路徑搜索Fig.3 Dijkstra algorithm path search

根據(jù)仿真結(jié)果可以看出，第一組參數(shù)得出的最短路徑為13，第二組參數(shù)得出的最短路徑為26。通過(guò)仿真的路徑搜索可視化過(guò)程可以看出，Dijkstra算法與BFS算法類(lèi)似，都是層層逼近終點(diǎn)，但是由于有優(yōu)先值的加入，使得相對(duì)較遠(yuǎn)的子節(jié)點(diǎn)成為優(yōu)先選擇的對(duì)象，而不是靠近當(dāng)前的節(jié)點(diǎn)作為下一步搜索的節(jié)點(diǎn)。

4.3 A＊算法

A*算法引入了啟發(fā)式搜索這個(gè)概念，也就是增加了一個(gè)啟發(fā)函數(shù)，該函數(shù)能夠計(jì)算得出一個(gè)值，該值代表當(dāng)前柵格到目的柵格移動(dòng)的優(yōu)先級(jí)，數(shù)值越高優(yōu)先級(jí)越低?？梢酝ㄟ^(guò)下面的函數(shù)來(lái)表示節(jié)點(diǎn)的優(yōu)先級(jí)：

其中，()是節(jié)點(diǎn)的最終優(yōu)先級(jí)，當(dāng)算法在選取下輪遍歷的節(jié)點(diǎn)時(shí)，選取當(dāng)前節(jié)點(diǎn)列表中優(yōu)先級(jí)最高的節(jié)點(diǎn)作為下次循環(huán)的起點(diǎn)；()是節(jié)點(diǎn)到目標(biāo)點(diǎn)的距離；()為節(jié)點(diǎn)到目標(biāo)點(diǎn)的預(yù)估代價(jià)。()啟發(fā)式函數(shù)如下：

式(4)是曼哈頓距離的計(jì)算，式(5)是歐拉距離的計(jì)算，對(duì)于不同的地圖模型，將選取不同的啟發(fā)式函數(shù)。

首先建立兩個(gè)列表OPENSET和CLOSESET，列表存儲(chǔ)的是節(jié)點(diǎn)對(duì)象，OPENSET將存放可以被選擇移動(dòng)的節(jié)點(diǎn)，CLOSESET存放被選擇過(guò)或不可選擇的節(jié)點(diǎn)。根據(jù)Point類(lèi)中的屬性進(jìn)行判斷，即Point.obs和Point.close中有一個(gè)為T(mén)rue，則這個(gè)節(jié)點(diǎn)將會(huì)被保存在CLOSESET中。算法從起始點(diǎn)開(kāi)始逐層搜索，將從OPENSET中選取代價(jià)最小的節(jié)點(diǎn)開(kāi)始探索，根據(jù)當(dāng)前節(jié)點(diǎn)中臨近節(jié)點(diǎn)列表中的節(jié)點(diǎn)的代價(jià)，選取最小代價(jià)的點(diǎn)，添加到OPENSET中，作為下一次探索的起始點(diǎn)。在退出當(dāng)前節(jié)點(diǎn)時(shí)，會(huì)將當(dāng)前節(jié)點(diǎn)的now_Point.close設(shè)置為T(mén)rue并添加到CLOSESET中。

分別設(shè)置起始點(diǎn)(0,0)、終點(diǎn)(9,9)和起始點(diǎn)(0,0)、終點(diǎn)(14,14)，以及分別采用曼哈頓距離和歐拉距離作為啟發(fā)式，仿真結(jié)果如圖4所示。

圖4 A*算法路徑搜索Fig.4 A* algorithm path search

如圖4所示，兩個(gè)啟發(fā)式得到的路徑結(jié)果都相同，圖4(a)、圖4(c)的距離為22，圖4(b)、圖4(d)的距離為26。但是我們可以很明顯地看出，曼哈頓距離在柵格模型中搜索的范圍相對(duì)于采用歐拉距離作為啟發(fā)式的搜索范圍更少，也就是說(shuō)，A*算法在柵格模型中采用曼哈頓距離作為啟發(fā)式方程，將會(huì)縮小時(shí)間復(fù)雜度，提高算法的效率。

4.4 D＊算法

D*算法及其延伸算法廣泛應(yīng)用于移動(dòng)機(jī)器人和自主車(chē)輛導(dǎo)航中。D*算法的核心與Dijkstra算法和A*算法類(lèi)似，需要維護(hù)一張OPENSET表。該列表記錄每一個(gè)單元柵格的五個(gè)狀態(tài)：NEW、OPEN、CLOSED、RAISE、LOWER。NEW代表該節(jié)點(diǎn)從未加入OPENSET中。OPEN代表該節(jié)點(diǎn)當(dāng)前處于OPENSET表中。CLOSED代表該節(jié)點(diǎn)已被OPENSET列表移除。而RAISE和LOWER則記錄移動(dòng)成本的代價(jià)，RAISE代表當(dāng)前移動(dòng)成本比未改變地圖信息時(shí)的成本高；LOWER代表當(dāng)前移動(dòng)成本比未改變地圖信息時(shí)的成本低。有了這五個(gè)狀態(tài)，算法可以在改變地圖信息后，對(duì)路徑進(jìn)行重新預(yù)估和修正。

當(dāng)障礙點(diǎn)的增加數(shù)量過(guò)多的時(shí)候，還會(huì)引發(fā)另一個(gè)問(wèn)題：死鎖的出現(xiàn)，即障礙點(diǎn)的出現(xiàn)，導(dǎo)致自由柵格不能通過(guò)臨近節(jié)點(diǎn)找到一條新的路線(xiàn)。而解決這個(gè)問(wèn)題的方法，則是保存當(dāng)前路徑數(shù)據(jù)，然后使算法重新規(guī)劃路徑。

首先建立一張維護(hù)列表OPENLIST記錄節(jié)點(diǎn)的狀態(tài)，D*算法是反向搜索，所以將終點(diǎn)先添加到列表中。每一個(gè)單元節(jié)點(diǎn)有三個(gè)狀態(tài)：NEW、OPEN和CLOSED，NEW代表當(dāng)前節(jié)點(diǎn)未被加入OPENLIST中；OPEN代表當(dāng)前節(jié)點(diǎn)在OPENLIST中；CLOSED代表當(dāng)前節(jié)點(diǎn)不在OPENLIST中。此時(shí)的CLOSED不再是被移除的意思，即該節(jié)點(diǎn)曾被列入列表中。算法的搜索是擴(kuò)張過(guò)程，即每次從維護(hù)列表中選取一個(gè)節(jié)點(diǎn)，對(duì)其節(jié)點(diǎn)的移動(dòng)代價(jià)進(jìn)行預(yù)估，將移動(dòng)代價(jià)最小的節(jié)點(diǎn)保存到OPENLIST中，并修改其狀態(tài)屬性，同時(shí)將當(dāng)前節(jié)點(diǎn)和選取節(jié)點(diǎn)的變化狀態(tài)傳播到臨近節(jié)點(diǎn)，更新其對(duì)應(yīng)的各個(gè)臨近節(jié)點(diǎn)的權(quán)值。D*算法從目標(biāo)點(diǎn)開(kāi)始搜索，即每個(gè)節(jié)點(diǎn)都有一個(gè)反向指針，該指針由下一步移動(dòng)的節(jié)點(diǎn)指向當(dāng)前節(jié)點(diǎn)，最終的路徑只要按照反向指針指向的節(jié)點(diǎn)搜索，就能得到路徑。

當(dāng)在指定路徑上有障礙物產(chǎn)生時(shí)，將檢測(cè)到障礙物的附近節(jié)點(diǎn)放入OPENLIST中，此次將這些節(jié)點(diǎn)的預(yù)估值進(jìn)行重新計(jì)算，只保留預(yù)估值最小的節(jié)點(diǎn)，并修改其狀態(tài)，同時(shí)將節(jié)點(diǎn)的狀態(tài)變化傳播到臨近節(jié)點(diǎn)。如果遇到堵塞，則按照Dijkstra算法從節(jié)點(diǎn)列表中選取最優(yōu)的點(diǎn)進(jìn)行回溯，并更新列表中的節(jié)點(diǎn)信息。

設(shè)置起點(diǎn)為(0,0)、終點(diǎn)為(14,14)，設(shè)置兩組不同后續(xù)生成的障礙點(diǎn)分別為：第一組：(0,5)，(3,5)，(13,10)，(14,10)；第二組：(5,10)，(5,0)，(5,2)，(12,12)。新生成的障礙點(diǎn)將渲染成數(shù)字2柵格，與原有障礙白色柵格形成對(duì)比，仿真結(jié)果如圖5所示。

圖5 D*算法路徑搜索Fig.5 D* algorithm path search

通過(guò)仿真結(jié)果可以看出，機(jī)器人在地圖信息改變之后，在排除鎖死情況發(fā)生的狀況下，大致會(huì)沿著原來(lái)的路徑前進(jìn)，當(dāng)遇到障礙物時(shí)，會(huì)選擇繞開(kāi)障礙物，走臨近的可移動(dòng)節(jié)點(diǎn)，直至終點(diǎn)。圖5(b)和圖5(d)中的數(shù)字1柵格表示上一次路徑的選擇，也就是說(shuō)障礙生成之后，數(shù)字1柵格不是當(dāng)前路徑規(guī)劃中的最優(yōu)點(diǎn)。這就是D*算法不同于靜態(tài)環(huán)境下的路徑搜索算法的優(yōu)點(diǎn)，即能在動(dòng)態(tài)環(huán)境中實(shí)時(shí)修改前進(jìn)路線(xiàn)，保證自身的安全。

4.5 算法仿真數(shù)據(jù)分析

首先建立尺寸不同的地圖,因?yàn)榭梢暬惴ㄟ^(guò)程中有大量的圖形輸出，會(huì)影響算法的時(shí)間復(fù)雜度，所以對(duì)于算法運(yùn)行時(shí)間的數(shù)據(jù)搜集，采取封閉式測(cè)試，只保留算法的核心計(jì)算代碼，僅保留路徑輸出作為終點(diǎn)，采用python.time.process_time()函數(shù)，分別在算法起始點(diǎn)和終點(diǎn)設(shè)立start_time、end_time，最后將end_time與start_time相減得出的結(jié)果作為算法運(yùn)行的時(shí)間，取各個(gè)算法仿真20 次所得的time，去掉最大值、最小值，取平均數(shù)作為最后的數(shù)據(jù)來(lái)源。在每輪實(shí)驗(yàn)中引入一組隨機(jī)的障礙點(diǎn)添加到地圖中，確保封閉測(cè)試得到數(shù)據(jù)的隨機(jī)性和準(zhǔn)確性。測(cè)試20 輪得到數(shù)據(jù)如表1、表2和表3所示。

表1 路徑規(guī)劃算法的運(yùn)行時(shí)間Tab.1 Running time of the path planning algorithm

表2 A＊算法和D＊算法歐拉公式啟發(fā)式運(yùn)行時(shí)間Tab.2 Heuristic running time of A* algorithm and D*algorithm Euler formula

表3 A＊算法和D＊算法曼哈頓公式啟發(fā)式運(yùn)行時(shí)間Tab.3 Heuristic running time of A* algorithm and D*algorithm Manhattan formula

A*算法在將歐拉距離和曼哈頓距離分別作為不同的啟發(fā)式的時(shí)候，采用曼哈頓距離的時(shí)間消耗少于歐拉距離，因此在現(xiàn)實(shí)環(huán)境中，機(jī)器人使用A*算法路徑規(guī)劃時(shí)，要根據(jù)現(xiàn)實(shí)環(huán)境來(lái)選擇采用這兩者中的哪一個(gè)作為啟發(fā)式函數(shù)。在A*算法仿真實(shí)驗(yàn)中，我們提出了兩個(gè)不同的啟發(fā)式。由于采用柵格模型建模，柵格模型類(lèi)似于城市網(wǎng)格，可以類(lèi)比，在設(shè)計(jì)公交路線(xiàn)系統(tǒng)、搜索城市中點(diǎn)對(duì)點(diǎn)的最小路徑時(shí)，便可以采用啟發(fā)式為曼哈頓距離的A*算法。而本文所實(shí)現(xiàn)的A*算法，其中OPENLIST和CLOSELIST都是采用List的方式存儲(chǔ)數(shù)據(jù)，Python中的列表是基于數(shù)組的類(lèi)型，所以可以改用二叉堆這種數(shù)據(jù)結(jié)構(gòu)，使其節(jié)點(diǎn)的添加和刪除的操作速度更快，降低耗時(shí)。

D*算法彌補(bǔ)了A*算法不適合在動(dòng)態(tài)環(huán)境中規(guī)劃路徑的缺陷。D*算法在未出現(xiàn)新的障礙源時(shí)，路徑規(guī)劃算法基本與A*算法一致。在出現(xiàn)障礙源時(shí)，D*算法會(huì)重新對(duì)存儲(chǔ)的地圖信息進(jìn)行刪除和插入操作，需要更多的計(jì)算，所以消耗的時(shí)間較多。

5 移動(dòng)機(jī)器人路徑規(guī)劃實(shí)現(xiàn)(Implementation of mobile robot path planning)

路徑規(guī)劃實(shí)現(xiàn)如圖6所示。

圖6 路徑規(guī)劃實(shí)現(xiàn)Fig.6 Path planning realization

5.1 開(kāi)發(fā)環(huán)境

使用的機(jī)器人小車(chē)采用JetbotNano作為開(kāi)發(fā)主板，如圖6(a)所示，CPU為ARM的A57，操作系統(tǒng)為Unbuntu 18.10，開(kāi)發(fā)平臺(tái)為JupyterLab，開(kāi)發(fā)語(yǔ)言為Python 3.0。

通過(guò)WiFi模板進(jìn)行PC和機(jī)器人之間的數(shù)據(jù)傳輸，以及搭載四個(gè)直流電機(jī)控制車(chē)輪的移動(dòng)。通過(guò)對(duì)GPIO口的編程，控制直流電機(jī)的運(yùn)動(dòng)方向和前進(jìn)速度。

5.2 具體實(shí)現(xiàn)

通過(guò)分析仿真實(shí)驗(yàn)數(shù)據(jù)報(bào)告可以看到，由于我們?cè)陟o態(tài)環(huán)境下實(shí)現(xiàn)該小車(chē)的路徑規(guī)劃功能，因此選取了有代表性的A*算法來(lái)為機(jī)器人規(guī)劃行動(dòng)路徑。

首先需要導(dǎo)入地圖數(shù)據(jù)，將二維空間地圖抽象成算法可以處理的數(shù)據(jù)；再根據(jù)算法得到的結(jié)果，編程控制小車(chē)前進(jìn)。地圖的構(gòu)建方式也是采用柵格模型，將實(shí)際地圖抽象成二維平面，將每個(gè)單元柵格的信息采用一組二維數(shù)組來(lái)保存。此次實(shí)驗(yàn)地圖如圖6(b)所示，數(shù)字1單元格為機(jī)器人起點(diǎn)位置，數(shù)字2單元格為終點(diǎn)，黑色單元格為地圖上的障礙。

在得到A*算法得出的路徑列表之后，通過(guò)調(diào)用集成的開(kāi)發(fā)庫(kù)，使用主板上的i2c總線(xiàn)對(duì)四個(gè)直流驅(qū)動(dòng)電機(jī)進(jìn)行控制。調(diào)用Jetbot.Robot庫(kù)創(chuàng)建一個(gè)機(jī)器人控制對(duì)象。Robot庫(kù)的forward()、backward()、left()、right()、stop()分別控制機(jī)器人前進(jìn)、倒退、左轉(zhuǎn)、右轉(zhuǎn)、停止。根據(jù)地圖的參數(shù)以及實(shí)際地面情況，設(shè)置對(duì)應(yīng)的前進(jìn)速度和時(shí)間。

設(shè)地圖軸坐標(biāo)對(duì)應(yīng)為正東，反方向?yàn)檎?；軸坐標(biāo)對(duì)應(yīng)為正南，反方向?yàn)檎薄C(jī)器人起始點(diǎn)車(chē)頭對(duì)應(yīng)方向?yàn)檎?。根?jù)路徑列表，機(jī)器人首先調(diào)整車(chē)頭方向，調(diào)整的方向?yàn)榧磳⑶斑M(jìn)的下一個(gè)單元格的方向。設(shè)置一個(gè)變量保存當(dāng)前機(jī)器人車(chē)頭正對(duì)的方向信息，隨后機(jī)器人前進(jìn)一個(gè)單元格。方向參數(shù)設(shè)正南為(0,1)，正西為(-1,0)，正北為(0，-1)，正東為(1,0)，標(biāo)記數(shù)值按照正南方順時(shí)針?lè)较蛟O(shè)置為1、2、3、4，并保存在一個(gè)判斷列表中。車(chē)頭方向調(diào)整偽代碼如下:

通過(guò)對(duì)路徑列表的循環(huán)遍歷，初始化小車(chē)車(chē)頭方向，然后前行，直至到達(dá)終點(diǎn)。實(shí)驗(yàn)結(jié)果如圖6(c)所示。數(shù)字3單元格代表機(jī)器人前進(jìn)的路線(xiàn)。實(shí)驗(yàn)結(jié)果是機(jī)器人成功實(shí)現(xiàn)掉頭、轉(zhuǎn)彎、前行等功能，按照A*算法所得的路徑列表移動(dòng)，準(zhǔn)確在終點(diǎn)停止。

本次實(shí)物演示的地圖為一張根據(jù)機(jī)器人車(chē)身定制的2 m×2 m的塑料布材質(zhì)的地圖，內(nèi)部大小為20 cm×20 cm的正方形柵格。地圖在空地上展開(kāi)，在保持平整的情況下，完成路徑規(guī)劃的各項(xiàng)參數(shù)為：前進(jìn)電機(jī)速度：0.45(驅(qū)動(dòng)電路參數(shù)，需要根據(jù)具體環(huán)境進(jìn)行測(cè)試和調(diào)節(jié))；左轉(zhuǎn)電機(jī)參數(shù)：0.585；右轉(zhuǎn)電機(jī)參數(shù)：5.585；延時(shí)：1 s。利用Jetbot小車(chē)自身所帶的攝像頭，使小車(chē)在每次前進(jìn)、掉頭時(shí)，攝像頭根據(jù)比對(duì)地圖的數(shù)據(jù)，保證機(jī)器人小車(chē)能夠時(shí)刻居中，路線(xiàn)不偏移。

6 結(jié)論(Conclusion)

基于機(jī)器人路徑搜索算法的研究，本文對(duì)Dijkstra算法、A*算法、D*算法進(jìn)行研究，仿真實(shí)現(xiàn)了各個(gè)算法在路徑搜索上的演示，通過(guò)對(duì)比各算法的時(shí)間復(fù)雜度和搜索出的最優(yōu)路徑以及對(duì)應(yīng)的數(shù)據(jù)，確定在移動(dòng)機(jī)器人實(shí)體上采用A*算法。而通過(guò)實(shí)驗(yàn)可知，在柵格網(wǎng)絡(luò)中，A*算法采用曼哈頓距離啟發(fā)式，仿真可以得到更加優(yōu)異的結(jié)果。同時(shí)也提出了如何優(yōu)化A*算法的思路，即使用二叉樹(shù)堆代替數(shù)組來(lái)降低算法的時(shí)間復(fù)雜度。最后在移動(dòng)機(jī)器人上實(shí)現(xiàn)路徑規(guī)劃算法的演示，證明了該算法的可行性。