蔣 美 云

(南京工業(yè)職業(yè)技術(shù)學(xué)院 江蘇 南京 210023)

0 引 言

移動機(jī)器人是一個(gè)集環(huán)境感知、動態(tài)決策與規(guī)劃、行為控制與執(zhí)行等多功能于一體的綜合系統(tǒng)，是目前科學(xué)技術(shù)發(fā)展最活躍的領(lǐng)域之一，其不僅在工業(yè)、農(nóng)業(yè)、服務(wù)業(yè)等行業(yè)中應(yīng)用廣泛，而且在城市安全、國防和空間探測領(lǐng)域等有害與危險(xiǎn)場合得到很好的應(yīng)用。作為相對獨(dú)立工作的個(gè)體，其具有能量有限、時(shí)效性要求較高、工作環(huán)境特殊等特點(diǎn)，而有效的路徑規(guī)劃算法對于提高機(jī)器人工作效率，降低機(jī)器人能量消耗，延長機(jī)器人工作時(shí)間等具有重要意義。機(jī)器人路徑規(guī)劃是機(jī)器人技術(shù)研究領(lǐng)域的熱點(diǎn)及難點(diǎn)問題，旨在解決機(jī)器人如何在工作環(huán)境中安全到達(dá)目的地的問題，圍繞該問題國內(nèi)外研究者開展了一系列研究。機(jī)器人路徑規(guī)劃過程主要是根據(jù)工作代價(jià)最小、路徑最短等準(zhǔn)則，為機(jī)器人規(guī)劃出一條從起點(diǎn)到目的點(diǎn)的連續(xù)的、無碰撞的路徑[1]。當(dāng)前，機(jī)器人路徑規(guī)劃步驟包含環(huán)境地圖構(gòu)建及機(jī)器人路徑規(guī)劃。環(huán)境地圖構(gòu)建主要有柵格地圖法[2]、可視圖法[3]、結(jié)構(gòu)空間法[4]和拓?fù)鋱D法[5]。其中：柵格地圖法是一種十分有效的地圖描述方法，越來越受到研究者的重視，應(yīng)用前景十分廣闊；可視圖法則將機(jī)器人作為工作環(huán)境中的一點(diǎn)，用直線將障礙物的頂點(diǎn)，起點(diǎn)與障礙物，障礙物與目標(biāo)點(diǎn)連接起來，并保障直線不經(jīng)過障礙物，但隨著障礙物數(shù)量的增加，可視圖中連線的數(shù)量也會成倍增加，導(dǎo)致路徑規(guī)劃效率大大下降。機(jī)器人路徑規(guī)劃方法主要有局部路徑規(guī)劃和全局路徑規(guī)劃法。局部規(guī)劃路徑方法屬于動態(tài)規(guī)劃，主要方法有遺傳算法、動態(tài)窗口法、模糊邏輯控制法和人工勢場法等[6]。A*算法[7]是全局路徑規(guī)劃方法的典型代表，由Dijkstra算法演變而來。A*算法用啟發(fā)函數(shù)評估待選路徑的優(yōu)劣，啟發(fā)函數(shù)和評估函數(shù)的選取是該算法的難點(diǎn)。在A*算法的基礎(chǔ)上，又出現(xiàn)了LPA*[8]、D*Lite[9]等算法。隨著人工智能技術(shù)的普及應(yīng)用，作為全局搜索算法的蟻群算法[10-11]在機(jī)器人路徑規(guī)劃的應(yīng)用也越來越受到關(guān)注。然而，研究及應(yīng)用結(jié)果表明，基于蟻群算法的機(jī)器人路徑規(guī)劃過程，往往需要較長的搜索時(shí)間，算法收斂速度較慢，易出現(xiàn)局部最優(yōu)解等問題，除蟻群算法自身因素外，路徑搜索空間過大也是影響路徑搜索時(shí)間和算法收斂速度的主要因素之一。

本文為了降低路徑搜索空間大小，提高路徑搜索效率，在基于柵格地圖法對環(huán)境進(jìn)行構(gòu)建的基礎(chǔ)上，以起點(diǎn)和目的點(diǎn)間的連線為吸引路徑，采用精英螞蟻沿著靠近吸引路徑的方向探索出初始路徑封閉區(qū)域，后續(xù)螞蟻則在初始路徑封閉區(qū)域內(nèi)搜索出最終路徑。本文算法有效降低了路徑搜索空間大小，能夠以更快的速度探索出有效路徑。

1 環(huán)境建模及相關(guān)定義

1.1 基于柵格法的環(huán)境建模

設(shè)定機(jī)器人工作的環(huán)境由m×n個(gè)大小相等的柵格構(gòu)成，其中任一柵格單元可表示為Cxy，柵格地圖G可表示為:

G={Cxy|x∈[1,m],y∈[1,n]}

(1)

Cxy=0表示該柵格為無障礙物柵格區(qū)域，Cxy=1表示該柵格為存在障礙物柵格區(qū)域。此外，令G0為無障礙物柵格區(qū)域的集合，令G1為有障礙物柵格區(qū)域的集合，則柵格地圖G也可表示為:

G=G0∪G1

(2)

圖1為5×5的柵格地圖，無障礙物及存在障礙的柵格已在圖中標(biāo)出。

圖1 5×5柵格地圖

1.2 相關(guān)定義

為便于進(jìn)行后續(xù)算法描述，特給出如下定義：

定義1吸引路徑：機(jī)器人出發(fā)點(diǎn)S與目的點(diǎn)T間的連線ST。

定義2拐點(diǎn)：改變機(jī)器人前進(jìn)方向的點(diǎn)。

定義3機(jī)器人當(dāng)前位置：無障礙柵格的中心。

定義4機(jī)器人移動方向(見圖2)：機(jī)器人由當(dāng)前無障礙柵格中心移動到下一無障礙柵格中心的方向，可為東、南、西、北、東北、東南、西南、西北八個(gè)方向。

圖2 機(jī)器人移動方向

定義5機(jī)器人單步移動距離：機(jī)器人從當(dāng)前無障礙柵格中心移動到下一無障礙柵格中心所經(jīng)過的路徑長度。

2 算法設(shè)計(jì)

2.1 算法思想

如上文所述，除蟻群算法自身因素外，路徑搜索空間過大也是導(dǎo)致算法執(zhí)行效率較低的主要原因之一。在蟻群算法優(yōu)化方面，已有眾多研究者提出有效的改進(jìn)算法，本文則從降低路徑搜索空間大小的角度出發(fā)，基于平面上兩點(diǎn)間直線距離最短的原則，以起點(diǎn)和目的點(diǎn)間的連線為吸引路徑，從起點(diǎn)出發(fā)采用精英螞蟻分別在吸引路徑的兩側(cè)沿著靠近吸引路徑的方向朝目的點(diǎn)前進(jìn)，而后通過吸引路徑兩側(cè)的精英螞蟻探索出初始路徑，構(gòu)建起初始路徑封閉區(qū)域，有效降低后續(xù)螞蟻路徑搜索空間的大小，進(jìn)而有效降低路徑搜索時(shí)間。

2.2 目標(biāo)函數(shù)

設(shè)定機(jī)器人在柵格地圖中的起點(diǎn)為S(xs,ys)、目的點(diǎn)為T(xt,yt)，從當(dāng)前位置Pi(x,y)到下一位置Pi+1(x,y)的距離為di(Pi→Pi+1)，機(jī)器人從起點(diǎn)S(xs,ys)經(jīng)過N段路徑后到達(dá)目的點(diǎn)T(xt,yt)的路徑距離之和表示為：

(3)

且Pi(x,y)∈G0。定義目標(biāo)函數(shù)F為：

(4)

由式(4)可知機(jī)器人路徑目標(biāo)函數(shù)可描述為：從起始點(diǎn)S(xs,ys)到目的點(diǎn)T(xt,yt)為機(jī)器人找到一條最短且無碰撞的路徑。

2.3 初始路徑探索算法

算法主要步驟如下：

Step1以起點(diǎn)S(xs,ys)與目的點(diǎn)T(xt,yt)間的連線ST為吸引路徑。

Step2將精英螞蟻A和B置于起點(diǎn)S(xs,ys)，兩只精英螞蟻均靠近并沿吸引路徑方向向目的點(diǎn)T(xt,yt)前進(jìn)。

Step3當(dāng)遇到障礙柵格時(shí)，螞蟻A在吸引路徑的右側(cè)靠近并沿吸引路徑方向前進(jìn)；螞蟻B則在吸引路徑的左側(cè)靠近并沿吸引路徑方向前進(jìn)；螞蟻A和螞蟻B在前進(jìn)過程中均記錄其路徑拐點(diǎn)PA→i(x,y)和PB→j(x,y)；根據(jù)吸引路徑的吸引效應(yīng)積極向吸引路徑靠攏，并沿吸引路徑方向前進(jìn)。

Step4螞蟻A或螞蟻B繼續(xù)靠近并沿吸引路徑方向朝目的點(diǎn)T(xt,yt)前進(jìn)，遇到障礙柵格時(shí)，執(zhí)行Step3。

Step5螞蟻A或螞蟻B未到達(dá)目的點(diǎn)時(shí)，重復(fù)執(zhí)行Step3和Step4，直到螞蟻A和B均到達(dá)目的點(diǎn)，螞蟻A和B獲得的包含拐點(diǎn)的路徑分別表示為：

(5)

式中：GAi表示螞蟻A從起始點(diǎn)S出發(fā)遇到的第i個(gè)拐點(diǎn)且1≤i≤m；GBj表示螞蟻B從起始點(diǎn)S出發(fā)遇到的第j個(gè)拐點(diǎn)且1≤j≤n。

2.4 初始路徑平滑算法

采用2.3節(jié)算法得到的路徑雖保證了盡量靠近吸引路徑，但仍存在較多的拐點(diǎn)導(dǎo)致機(jī)器人付出較高的轉(zhuǎn)彎代價(jià)，且路徑長度也有待優(yōu)化。下面以平滑螞蟻A的路徑PathA=(S,GA1,GA2,…,GAm,T)為例，為便于描述，不妨令出發(fā)點(diǎn)S為路徑上的第0個(gè)拐點(diǎn)即S=GA0，目的點(diǎn)T為路徑上的第m+1個(gè)拐點(diǎn)即T=GA(m+1)，則原始路徑可表示為：

PathA=(GA0,GA1,GA2,…,GAm,GA(m+1))

(6)

初始路徑平滑算法步驟如下：

Step1以路徑PathA的第一個(gè)拐點(diǎn)GA0為基準(zhǔn)，分別構(gòu)建GA0與其不相鄰拐點(diǎn)GAi間的路徑LA0Ai=d(GA0,GAi)，其中2≤i≤m+1。

Step2當(dāng)存在拐點(diǎn)GAi滿足max(LA0Ai)且路徑LA0Ai上無障礙柵格時(shí)，將拐點(diǎn)GA0存入已優(yōu)化路徑(GA0)，構(gòu)建待優(yōu)化路徑：

(7)

(8)

令精英螞蟻B的初始路徑為：

PathB=(S,GB1,GB2,…,GBn,T)

(9)

同樣在采用初始路徑平滑算法后，可得到精英螞蟻B的優(yōu)化路徑：

(10)

結(jié)論1地圖中的點(diǎn)S和T間只存在一個(gè)障礙柵格區(qū)域時(shí)，其最短路徑包含在精英螞蟻A和螞蟻B初始路徑形成的封閉區(qū)域內(nèi)。

證明：構(gòu)建如圖3所示柵格地圖，根據(jù)2.3節(jié)中提出的算法，其中一只精英螞蟻的路徑可表示為：

Path0=(S,A,B,D,E,T)

(11)

圖3 證明圖例

采用2.4節(jié)中優(yōu)化算法后，得到的優(yōu)化路徑為：

Path1=(S,B,D,T)

(12)

此時(shí)，機(jī)器人所經(jīng)過的路徑之和為：

L1=d(S,B)+d(B,D)+d(D,T)

(13)

如圖3所示，在拐點(diǎn)B處，機(jī)器人除沿拐點(diǎn)D的方向運(yùn)動外，剩余可運(yùn)動方向?yàn)樗椒较?，假定其沿水平方向運(yùn)動到拐點(diǎn)C處，此時(shí)拐點(diǎn)C與目的點(diǎn)T間存在直接路徑，拐點(diǎn)C和T間不存在拐點(diǎn)使得拐點(diǎn)間路徑更短，則路徑可表示為：

Path2=(S,C,T)

(14)

機(jī)器人所經(jīng)過的路徑之和為：

L2=d(S,C)+d(C,T)

(15)

假定每個(gè)柵格的寬度和長度均為1，則有：

(16)

顯然L1

綜上，采用優(yōu)化算法后，可確保精英螞蟻所構(gòu)建的初始路徑封閉區(qū)域外不存在更短的從起點(diǎn)至目的點(diǎn)的路徑。因此，最短路徑只能包含在初始路徑封閉區(qū)域內(nèi)。

證畢。

2.5 最終路徑規(guī)劃

執(zhí)行初始路徑及其優(yōu)化算法后，由精英螞蟻A和B的路徑可獲得包含吸引路徑的封閉區(qū)域，不妨令機(jī)器人轉(zhuǎn)彎次數(shù)n為精英螞蟻A和B路徑拐點(diǎn)數(shù)的均值。當(dāng)障礙柵格密集分布于封閉區(qū)域內(nèi)時(shí)，可能會導(dǎo)致優(yōu)化后的路徑中仍存在大量的拐點(diǎn)且最優(yōu)路徑被封閉區(qū)域隔離，直接體現(xiàn)為機(jī)器人轉(zhuǎn)彎代價(jià)的增加。在此，給出基于機(jī)器人轉(zhuǎn)彎代價(jià)的最終路徑規(guī)劃方法：

(1) 當(dāng)機(jī)器人轉(zhuǎn)彎代價(jià)α>t時(shí)，表明采用初始路徑優(yōu)化算法后，路徑中仍存在較多拐點(diǎn)，導(dǎo)致機(jī)器人轉(zhuǎn)彎代價(jià)過大且路徑長度較長。因此，在全局范圍內(nèi)，采用文獻(xiàn)[11]提出的改進(jìn)蟻群算法進(jìn)行路徑搜索，并進(jìn)一步采用2.4節(jié)的路徑平滑算法對路徑進(jìn)行優(yōu)化。

(2) 當(dāng)機(jī)器人轉(zhuǎn)彎代價(jià)α≤t時(shí)，表明障礙柵格在吸引路徑上分布較為稀疏。此時(shí)，將封閉區(qū)域以外的柵格統(tǒng)一標(biāo)記為障礙柵格，而后續(xù)螞蟻的尋路過程也將在此封閉區(qū)域內(nèi)進(jìn)行。后續(xù)路徑搜索采用文獻(xiàn)[11]算法，在此不再贅述。當(dāng)通過蟻群算法在初始區(qū)域中尋找到最優(yōu)路徑后，進(jìn)一步采用2.4節(jié)的路徑平滑算法對路徑進(jìn)行優(yōu)化。

特別地，轉(zhuǎn)彎代價(jià)門限值t可根據(jù)本文算法與傳統(tǒng)算法的對比實(shí)驗(yàn)予以確定。

3 仿真實(shí)驗(yàn)

3.1 相關(guān)設(shè)置

算法仿真過程在MATLAB環(huán)境中實(shí)現(xiàn)。為便于展示，首先構(gòu)建15×15的柵格地圖，起點(diǎn)S及目的點(diǎn)T如圖4所示。同時(shí)，隨機(jī)生成50幅100×100的柵格地圖，根據(jù)本文算法與文獻(xiàn)[11]算法的對比結(jié)果，設(shè)定轉(zhuǎn)彎代價(jià)門限值為t=0.61，以確保采用本文算法與文獻(xiàn)[11]算法均能搜索到最優(yōu)路徑。在相同的參數(shù)和環(huán)境下，對文獻(xiàn)[11]算法和本文算法進(jìn)行了對比分析。

圖4 仿真實(shí)驗(yàn)柵格地圖

3.2 初始路徑規(guī)劃算法仿真結(jié)果

采用本文提出的初始路徑探索算法后，獲得了如圖5所示的初始路徑，精英螞蟻A和B經(jīng)過的初始路徑存在較多拐點(diǎn)，路徑有待平滑優(yōu)化。精英螞蟻A和B經(jīng)過的初始路徑長度分別為23.14、23.73，拐點(diǎn)個(gè)數(shù)分別為15、13。

圖5 初始路徑規(guī)劃算法執(zhí)行結(jié)果

3.3 初始路徑規(guī)劃優(yōu)化算法仿真結(jié)果

采用本文算法的初始路徑優(yōu)化結(jié)果如圖6所示，精英螞蟻A和B平滑優(yōu)化后的初始路徑長度分別為19.97、21.03，拐點(diǎn)數(shù)分別為6、8，平滑優(yōu)化后的初始路徑長度減少了13.70%、11.38%，拐點(diǎn)數(shù)量減少了60%、38%。初始路徑封閉區(qū)域如圖7所示，其大小占柵格地圖的36%，這表明本文算法有效降低了路徑搜索空間的大小。

圖6 初始路徑平滑結(jié)果

圖7 初始路徑封閉區(qū)域

3.4 最終路徑規(guī)劃結(jié)果

最終路徑規(guī)劃過程中，由精英螞蟻A和B確定的機(jī)器人轉(zhuǎn)彎代價(jià)為t≈0.39，小于門限值t，故在初始路徑封閉區(qū)域內(nèi)搜索最終路徑，其結(jié)果如圖8所示。最終路徑長度為19.01，較初始路徑規(guī)劃過程中精英螞蟻A平滑優(yōu)化后的初始路徑長度19.97減少了0.96。圖9為本文算法與文獻(xiàn)[11]算法的收斂性對比結(jié)果?？梢钥闯?，迭代次數(shù)達(dá)到19次時(shí)，本文算法已找到最優(yōu)路徑，耗時(shí)0.490 2 s。由于路徑搜索空間較大，文獻(xiàn)[11]算法在迭代次數(shù)達(dá)到40時(shí)，才能找到最優(yōu)路徑，耗時(shí)1.966 2 s，約為本文算法的4倍。此外，在前面隨機(jī)生成的50幅100×100柵格地圖中，有48幅柵格地圖的最終路徑包含在初始路徑封閉區(qū)域內(nèi)，而剩余2個(gè)柵格地圖的機(jī)器人轉(zhuǎn)彎代價(jià)均大于門限值0.61，也根據(jù)全局搜索算法確定了最終路徑。

圖8 初始路徑封閉區(qū)域內(nèi)的最終路徑

4 結(jié) 語

針對柵格地圖環(huán)境中的機(jī)器人路徑規(guī)劃問題，從減少路徑搜索空間大小的角度出發(fā)，確定了基于初始路徑封閉區(qū)域的路徑規(guī)劃思想。為獲得初始路徑封閉區(qū)域，首先采用精英螞蟻沿起始點(diǎn)與目的點(diǎn)間的吸引路徑前進(jìn)，再采用路徑平滑算法優(yōu)化初始路徑探尋結(jié)果。最終路徑搜索過程中，根據(jù)機(jī)器人轉(zhuǎn)彎代價(jià)，決定后續(xù)螞蟻的路徑探尋區(qū)域是否限定在初始路徑封閉區(qū)域內(nèi)。與傳統(tǒng)算法在仿真環(huán)境下的對比結(jié)果表明，在96%的情況下本文算法能夠在更短的時(shí)間內(nèi)找到最終路徑。