移動(dòng)機(jī)器人全局路徑規(guī)劃算法的研究

黃 靜，陳漢偉

(浙江理工大學(xué)信息學(xué)院，浙江杭州 310018)

0 引言

目前，移移動(dòng)機(jī)器人普遍存在行動(dòng)機(jī)械、決策遲鈍的問(wèn)題，無(wú)法真正實(shí)現(xiàn)模擬自然環(huán)境中生物體的行為模式。因此，如何使這些移動(dòng)機(jī)器人擁有較為真實(shí)的行為能力成為一個(gè)研究熱點(diǎn)。全局路徑規(guī)劃是移動(dòng)機(jī)器人要解決的重要問(wèn)題之一[1]，作用是移動(dòng)機(jī)器人在已知錯(cuò)綜復(fù)雜的地形的狀況下，找到一條到目標(biāo)位置較優(yōu)的路徑，不僅能夠有效避開(kāi)障礙，而且時(shí)間開(kāi)銷(xiāo)較短。由于特殊地形、障礙、多角色實(shí)時(shí)移動(dòng)等因素，使路徑規(guī)劃變得十分復(fù)雜。優(yōu)秀的路徑規(guī)劃方法不僅可以讓移動(dòng)機(jī)器人的行為讓用戶看起來(lái)更加智能，而且能夠節(jié)約大量的運(yùn)算時(shí)間。

如圖1所示，盡管兩條路徑的長(zhǎng)度相同，圖1(b)模型顯然更符合現(xiàn)實(shí)中智能體的移動(dòng)規(guī)律。

(a)路徑一

(b)路徑二圖1無(wú)障礙普通路徑規(guī)劃示意圖

文中針對(duì)移動(dòng)機(jī)器人智能化中的路徑規(guī)劃問(wèn)題，著重研究了盲目式搜索算法、A*算法和遺傳算法，使用A*算法在Unity平臺(tái)上實(shí)現(xiàn)移動(dòng)機(jī)器人的路徑規(guī)劃模塊，旨在提高路徑搜索的效率，提升移動(dòng)機(jī)器人的真實(shí)感。

1 路徑規(guī)劃算法的研究與分析

1.1 盲目式搜索算法

深度優(yōu)先搜索算法(Deep First Search, DFS)和廣度優(yōu)先搜索算法(Breadth First Search, BFS)是兩種常見(jiàn)的搜索算法[2]。

DFS沿著樹(shù)的深度遍歷地圖中的節(jié)點(diǎn)，盡可能深地搜索分支節(jié)點(diǎn)。當(dāng)一個(gè)節(jié)點(diǎn)i的所有子節(jié)點(diǎn)都已被探尋過(guò)，搜索將回溯到發(fā)現(xiàn)節(jié)點(diǎn)i的那條邊的起始節(jié)點(diǎn)。如果并沒(méi)有搜索到目標(biāo)節(jié)點(diǎn)，則選擇其中一個(gè)作為源節(jié)點(diǎn)并重復(fù)以上過(guò)程，整個(gè)進(jìn)程反復(fù)進(jìn)行直到發(fā)現(xiàn)目標(biāo)節(jié)點(diǎn)為止。深度優(yōu)先算法找到的路徑通常不是最短或最有效的路徑，在場(chǎng)景地形復(fù)雜、障礙物繁多的情況下，搜索到的路線常常偏離目標(biāo)方向，具有一定的盲目性，搜索的過(guò)程也缺乏移動(dòng)機(jī)器人應(yīng)有的智能性。圖2為深度優(yōu)先搜索的示意圖。

BFS則是從起始節(jié)點(diǎn)開(kāi)始，先訪問(wèn)起始節(jié)點(diǎn)的所有子節(jié)點(diǎn)，再訪問(wèn)子節(jié)點(diǎn)的所有子節(jié)點(diǎn)，直到搜索到目標(biāo)節(jié)點(diǎn)為止。這種搜索方式可以搜索到最短的路徑，但是算法本身消耗的時(shí)間根本無(wú)法滿足移動(dòng)機(jī)器人決策模塊對(duì)實(shí)時(shí)性的要求。圖3為廣度優(yōu)先搜索的示意圖。

使用C/C++實(shí)現(xiàn)DFS和BFS搜索算法，解決一個(gè)20×20的矩陣中的路徑搜索問(wèn)題，起始點(diǎn)為點(diǎn)[3,3]，搜索目標(biāo)點(diǎn)為點(diǎn)DFS和BFS都是盲目式搜索算法，算法進(jìn)行搜索的時(shí)候，不會(huì)對(duì)節(jié)點(diǎn)進(jìn)行評(píng)估，且跳轉(zhuǎn)到最優(yōu)結(jié)點(diǎn)進(jìn)行下一步的搜索。當(dāng)搜索狀態(tài)最為糟糕的時(shí)候，算法很可能需要搜索整個(gè)地圖空間才能得出所需的路徑。顯然，這類(lèi)盲目型搜索算法只能適用于解決規(guī)模不大的搜索問(wèn)題。

圖2 DFS示意圖

圖3 BFS示意圖

[17,17]，結(jié)果如圖4和圖5所示。實(shí)驗(yàn)結(jié)果證明，DFS的搜索路徑十分地盲目，完全不符合智能體的決策行為，而B(niǎo)FS能夠更為有效地搜索到最短路徑，路徑較真實(shí)，但時(shí)間消耗較大。

圖4 DFS路徑搜索的實(shí)現(xiàn)

圖5 BFS路徑搜索的實(shí)現(xiàn)

1.2 A*算法

不同于DFS和BFS等傳統(tǒng)的盲目式搜索算法，啟發(fā)式搜索算法在從一個(gè)節(jié)點(diǎn)向下一個(gè)節(jié)點(diǎn)搜索時(shí)，會(huì)引入一個(gè)啟發(fā)函數(shù)，這個(gè)啟發(fā)函數(shù)為每一個(gè)子節(jié)點(diǎn)進(jìn)行評(píng)估，選擇子節(jié)點(diǎn)中評(píng)估值最低的節(jié)點(diǎn)作為路徑的節(jié)點(diǎn)。優(yōu)秀的啟發(fā)函數(shù)可以迅速地搜索到路徑規(guī)劃問(wèn)題中的最優(yōu)路徑。A*算法是啟發(fā)式搜索中比較有效的方法[3]，常用于解決最優(yōu)路徑問(wèn)題及其他一些策略性問(wèn)題。

在路徑規(guī)劃過(guò)程中，A*算法會(huì)引入一些具有啟發(fā)意義的信息，這些信息能夠引導(dǎo)算法發(fā)現(xiàn)有效的路徑。算法通常使用一個(gè)啟發(fā)函數(shù)表示這些信息，在搜索的過(guò)程中使用啟發(fā)函數(shù)對(duì)路徑規(guī)劃中的搜索位置進(jìn)行評(píng)估，選取評(píng)估值最低的節(jié)點(diǎn)作為路徑節(jié)點(diǎn)，并從這個(gè)節(jié)點(diǎn)再次進(jìn)行進(jìn)一步搜索，直到目標(biāo)節(jié)點(diǎn)被搜索到，完成整個(gè)搜索過(guò)程。A*算法避免了路徑規(guī)劃問(wèn)題中常常產(chǎn)生的大量無(wú)效搜索路徑，相對(duì)于盲目式搜索，提高了算法本身的搜索效率。

A*算法中最重要的就是算法的啟發(fā)函數(shù)：

f(n)=g(n)+h(n)

(1)

式中：f(n)是路徑中每個(gè)可能搜索節(jié)點(diǎn)的評(píng)估值，由兩部分組成：g(n)表示從起始節(jié)點(diǎn)到當(dāng)前節(jié)點(diǎn)的評(píng)估值；h(n)表示當(dāng)前節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的評(píng)估值，h(n)的優(yōu)劣會(huì)直接影響算法的性能，也決定了算法是否能夠被定義為A*算法。

如果要將啟發(fā)式搜索算法定義為A*算法，需要保證路徑規(guī)劃問(wèn)題中存在著從起始節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的最優(yōu)路徑，而且路徑中所有節(jié)點(diǎn)的評(píng)估值都大于零。此外，h(n)需要盡量地接近路徑規(guī)劃問(wèn)題的實(shí)際評(píng)估值h*(n)。

路徑規(guī)劃問(wèn)題存在解、節(jié)點(diǎn)評(píng)估值大于零，這些條件都比較容易滿足，但是如果要滿足h(n)接近實(shí)際評(píng)估值就需要進(jìn)行有效的設(shè)計(jì)。A*算法中的h(n)和h*(n)相差不能過(guò)大[4-5]，過(guò)小的h(n)會(huì)使整個(gè)算法的區(qū)分能力減弱，從而使算法的效率降低。優(yōu)秀的h(n)應(yīng)該盡量逼近h*(n)，但是始終小于h*(n)[6-7]。這種設(shè)計(jì)不僅可以增加啟發(fā)信息，還能減少搜索過(guò)程中需要探索的節(jié)點(diǎn)數(shù)，從而有效提高算法的搜索速度。

和盲目式搜索算法相比，A*算法搜索時(shí)探索的節(jié)點(diǎn)要少很多，而且路徑的效果較好，如圖6所示。

圖6 A*搜索算法的實(shí)現(xiàn)

1.3 遺傳算法

遺傳算法是一種模擬自然選擇和遺傳機(jī)制的隨機(jī)搜索算法[8-9]。遺傳算法模擬生物遺傳過(guò)程中發(fā)生的繁殖、交叉和基因突變現(xiàn)象，在每次迭代中都保留一組候選解，并按某種指標(biāo)從種群中選取較優(yōu)的個(gè)體，利用遺傳算子進(jìn)行選擇、交叉和變異，對(duì)這些個(gè)體進(jìn)行組合，產(chǎn)生新一代的候選解群，重復(fù)此過(guò)程，直到滿足預(yù)置收斂指標(biāo)為止。

基本過(guò)程如下：

(1)初始化。確定種群規(guī)模N、交叉概率Pc、變異概率Pm和置終止進(jìn)化準(zhǔn)則；隨機(jī)生成個(gè)體數(shù)量為N的種群作為初始種群Population(t)；種群代數(shù)t=0。

(2)個(gè)體評(píng)價(jià)。計(jì)算或估價(jià)Population(t)中各個(gè)體的適應(yīng)度。

(3)種群進(jìn)化。

①選擇親代染色體。從Population(t)中運(yùn)用選擇算子選擇出M/2對(duì)染色體(M≥N)。

②交叉。對(duì)所選擇的M/2對(duì)染色體，根據(jù)Pc的概率執(zhí)行交叉形成M個(gè)臨時(shí)個(gè)體。

③變異。M個(gè)臨時(shí)個(gè)體都有Pm的幾率發(fā)生變異，形成M個(gè)候選個(gè)體。

④選擇子代染色體。從上述所形成的M個(gè)候選個(gè)體中根據(jù)適應(yīng)度選擇出N個(gè)個(gè)體組成新一代種群Population(t+1)。

(4)終止檢驗(yàn)。如果已經(jīng)滿足終止的條件，則輸出Population(t+1)中具有最大適應(yīng)度的個(gè)體作為最優(yōu)解，終止計(jì)算；否則置t=t+1并轉(zhuǎn)至步驟(3)。

在智能體的路徑規(guī)劃中，文中將四個(gè)搜索動(dòng)作編碼為二進(jìn)制碼，如表1所示。

表1 遺傳算法二進(jìn)制編碼

由表1中編碼組成的二進(jìn)制序列，如00100111，就是遺傳算法在解決路徑規(guī)劃時(shí)的一個(gè)染色體。若要評(píng)估這條染色體的適應(yīng)度，則從起始點(diǎn)開(kāi)始，根據(jù)染色體中的編碼進(jìn)行搜索。若其中的一個(gè)編碼使本次搜索遇到了障礙，則忽略該編碼，使用下一個(gè)編碼繼續(xù)搜索，直到歷遍所有編碼或者抵達(dá)目標(biāo)位置。適應(yīng)度f(wàn)itness由這次搜索的最終位置距離目標(biāo)位置的距離d給出：

(2)

圖7、圖8為使用遺傳算法進(jìn)行路徑搜索的實(shí)驗(yàn)結(jié)果。從圖中易知，遺傳算法能夠有效地搜索到目標(biāo)位置，且規(guī)劃的路徑較符合自然環(huán)境中智慧生物的決策行為。然而，隨著地圖面積的增大，遺傳算法在解決此類(lèi)問(wèn)題時(shí)就需要增長(zhǎng)染色體的長(zhǎng)度，增加種群的規(guī)模，因此算法的計(jì)算量往往變得十分龐大，這顯然無(wú)法滿足移動(dòng)機(jī)器人的高實(shí)時(shí)性需求[10]。當(dāng)虛擬場(chǎng)景的地圖較大，起始位置和目標(biāo)位置相聚較遠(yuǎn)時(shí)，遺傳算法并不是最合適的路徑規(guī)劃算法。

圖7 進(jìn)化4次后的路徑

圖8 進(jìn)化11次后的路徑

1.4 算法總體分析

盲目式的搜索算法不僅運(yùn)算時(shí)間長(zhǎng)，而且搜索得到的路徑真實(shí)性差；遺傳算法得到的路徑較符合現(xiàn)實(shí)中智能體的路徑選擇方式，但運(yùn)算量大，耗時(shí)長(zhǎng)，無(wú)法滿足實(shí)時(shí)性要求；啟發(fā)式的搜索算法能夠有效利用路徑中的啟發(fā)信息，搜索得到的路徑真實(shí)，運(yùn)算效率較好。A*算法能夠有效解決傳統(tǒng)虛擬環(huán)境路徑搜索算法造成的避障不及時(shí)，原地徘徊等移動(dòng)機(jī)器人反應(yīng)不靈活等問(wèn)題，而且效率較好。

2 A*路徑規(guī)劃的應(yīng)用及其優(yōu)化

本文利用Unity平臺(tái)[11-12]虛擬移動(dòng)機(jī)器人路徑規(guī)劃模塊，針對(duì)移動(dòng)機(jī)器人行動(dòng)遲鈍，行動(dòng)時(shí)模型重疊等問(wèn)題，選取最適用于移動(dòng)機(jī)器人路徑規(guī)劃的A*算法設(shè)計(jì)移動(dòng)機(jī)器人的路徑規(guī)劃模塊，并且使用貪婪平滑算法進(jìn)行優(yōu)化，旨在使虛擬移動(dòng)機(jī)器人能夠靈活避開(kāi)障礙。

A*算法需要維持兩張列表：存放未評(píng)估節(jié)點(diǎn)的Open表和存放已評(píng)估節(jié)點(diǎn)的Closed表，具體實(shí)現(xiàn)方式如下：

(1)把起始節(jié)點(diǎn)添加到Open表。

(2)考察Open表

①如果Open表中的元素為空，則表示搜索失??；尋找Open表中f(n)值最低的節(jié)點(diǎn)作為當(dāng)前的搜索節(jié)點(diǎn)，并把它移入Closed表中。

②如果當(dāng)前點(diǎn)為終點(diǎn)，則路徑規(guī)劃成功，轉(zhuǎn)驟(4)。

(3) 對(duì)相鄰的上下左右4個(gè)方向的每個(gè)節(jié)點(diǎn)進(jìn)行如下操作。

①如果該節(jié)點(diǎn)不可訪問(wèn)，或者已經(jīng)存在于Closed表中，忽略該節(jié)點(diǎn)。

②如果該節(jié)點(diǎn)不在Open表中，把它添加進(jìn)去，并記錄它的f(n)、g(n)和h(n)值。

③如果該節(jié)點(diǎn)已經(jīng)在Open表中，用f(n)值作為參考檢查新的路徑是否更好，即有更低的f(n)值，并且重新計(jì)算該點(diǎn)的f(n)值。

④ 如果所有方向都已搜索完畢，則轉(zhuǎn)向驟(2)；否則，轉(zhuǎn)向步驟(3)。

(4) 保存路徑，根據(jù)Closed表中的節(jié)點(diǎn)信息，進(jìn)行逆向提取，便能夠得到一條從起始節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的路徑。

場(chǎng)景中的小型圓柱體需要需找一條從A點(diǎn)到達(dá)B點(diǎn)，并且能繞過(guò)障礙物的有效路徑。場(chǎng)景設(shè)計(jì)如圖9所示，路徑搜索的實(shí)驗(yàn)結(jié)果如圖10所示。實(shí)驗(yàn)結(jié)果表明，A*算法能夠有效地避開(kāi)路徑中的障礙物抵達(dá)目標(biāo)，而且能夠滿足決策模塊對(duì)實(shí)時(shí)性要求。

圖9 實(shí)驗(yàn)場(chǎng)景設(shè)計(jì)

(a) 障礙一

(b) 障礙二圖10 在不同障礙場(chǎng)景中的路徑搜索結(jié)果

然而，從實(shí)驗(yàn)結(jié)果也容易發(fā)現(xiàn)，A*算法搜索產(chǎn)生的路徑存在一些多余路徑，這是由于算法的啟發(fā)函數(shù)并不一直最佳。因此，需要對(duì)A*算法得到的路徑進(jìn)行優(yōu)化。在規(guī)劃路徑的過(guò)程中，如果相鄰的兩條直線路徑之間沒(méi)有障礙物，就使用一條路徑代替這兩條路徑。

文中采用貪婪平滑算法改進(jìn)A*算法[13-14]規(guī)劃生成的路徑。將A*生成的路徑按下列步驟進(jìn)行平滑：

(1)將路徑中的第一點(diǎn)作為貪婪平滑算法的起點(diǎn)；

(2)如果路徑中從起點(diǎn)開(kāi)始后續(xù)沒(méi)有連續(xù)的3個(gè)點(diǎn)，則轉(zhuǎn)到步驟(4)；否則每次從路徑當(dāng)前起點(diǎn)開(kāi)始連續(xù)取3個(gè)點(diǎn)，分別為記為a，b，c，如果ac直線路徑上沒(méi)有障礙并且ac長(zhǎng)度小于ab長(zhǎng)度加上bc長(zhǎng)度，則在路徑中刪除b點(diǎn)，保持起點(diǎn)不變，重新執(zhí)行步驟(2)；否則執(zhí)行步驟(3)；

(3)將起點(diǎn)的下一點(diǎn)作為新的計(jì)算起點(diǎn)，轉(zhuǎn)到步驟(2)執(zhí)行；

(4)路徑剩余的點(diǎn)就是平滑后的新路徑。

算法優(yōu)化前后的效果如圖11和圖12所示。

圖11 普通A*算法的路徑圖

圖12 優(yōu)化后的A*算法的路徑

通過(guò)貪婪平滑算法改進(jìn)的A*算法，能夠使移動(dòng)機(jī)器人的行動(dòng)路徑更加接近真實(shí)智慧生命體的路徑選擇方式，并且依然居然較好的算法執(zhí)行效率。

3 結(jié)論

文中基于Unity3D平臺(tái)，針對(duì)移動(dòng)機(jī)器人智能化中的全局路徑規(guī)劃問(wèn)題，研究了盲目式搜索算法、遺傳算法和A*算法的優(yōu)劣：遺傳算法雖然夠有效地搜索到目標(biāo)位置，但并不適合地圖場(chǎng)景較大的情況；使用A*算法實(shí)現(xiàn)移動(dòng)機(jī)器人的路徑規(guī)劃模塊，相比傳統(tǒng)的盲目搜索算法，能夠有效尋找當(dāng)前位置到目標(biāo)位置成本最小的路徑，然而實(shí)驗(yàn)結(jié)果表明，A*算法的啟發(fā)函數(shù)并不總是最佳的，路徑搜索過(guò)程中會(huì)產(chǎn)生一些拐角或者其他形式的多余路徑，需要進(jìn)行路徑優(yōu)化。文中利用貪婪平滑算法對(duì)A*算法進(jìn)行優(yōu)化，算法效率較好，移動(dòng)機(jī)器人的智能性較強(qiáng)。

參考文獻(xiàn)：

[1] 王麗. 移動(dòng)機(jī)器人路徑規(guī)劃方法研究：[學(xué)位論文]. 西安: 西北工業(yè)大學(xué), 2007.

[2] 曲道奎,杜振軍,徐殿國(guó),等. 移動(dòng)機(jī)器人路徑規(guī)劃方法研究. 機(jī)器人, 2008(2): 97-101.

[3] 史輝, 曹聞, 朱述龍，等. A*算法的改進(jìn)及其在路徑規(guī)劃中的應(yīng)用. 測(cè)繪與空間地理信息, 2009(6): 208-211.

[4] 陳剛, 付少鋒, 周利華. A*算法在游戲地圖尋徑中的幾種改進(jìn)策略研究. 科學(xué)技術(shù)與工程, 2007(15): 3731-3736.

[5] 邱磊. 基于A～*算法的游戲地圖尋路實(shí)現(xiàn)及性能比較. 陜西科技大學(xué)學(xué)報(bào)(自然科學(xué)版), 2011(6): 89-93.

[6] 王紅衛(wèi), 馬勇, 謝勇，等. 基于平滑A*算法的移動(dòng)機(jī)器人路徑規(guī)劃. 同濟(jì)大學(xué)學(xué)報(bào)(自然科學(xué)版), 2010, (11): 1647-1650.

[7] 劉浩, 鮑遠(yuǎn)律. A*算法在矢量地圖最優(yōu)路徑搜索中的應(yīng)用. 計(jì)算機(jī)仿真, 2008(4): 253-257.

[8] 陳華華,杜歆,顧偉康. 基于遺傳算法的靜態(tài)環(huán)境全局路徑規(guī)劃. 浙江大學(xué)學(xué)報(bào)(理學(xué)版), 2005(1): 49-53.

[9] 馬永杰, 云文霞. 遺傳算法研究進(jìn)展. 計(jì)算機(jī)應(yīng)用研究, 2012(4): 1201-1206.

[10] 唐文艷. 結(jié)構(gòu)優(yōu)化中的遺傳算法研究和應(yīng)用：[學(xué)位論文].大連：大連理工大學(xué), 2002.

[11] 陳俊鋒. 基于Unity3D的跨平臺(tái)手機(jī)網(wǎng)絡(luò)游戲的研究與實(shí)現(xiàn)：[學(xué)位論文]. 廣州：中山大學(xué), 2013.

[12] 鄭磊. 基于三維網(wǎng)頁(yè)技術(shù)的Unity3D教學(xué)管理系統(tǒng)的設(shè)計(jì)與實(shí)現(xiàn)：[學(xué)位論文]. 上海：上海交通大學(xué), 2013.

[13] ZHANG Z Y，ZHAO Z P. A multiple mobile robots path planning algorithm based on a-star and dijkstra algorithm．International Journal of Smart Home, 2014, 8(3)：75-86.

[14] LI J，SUN X X. Route planning's method for unmanned aerial vehicles based on improved a-star algorithm. Acta Armamentarii, 2008, 29(7), 788-792