基于改進(jìn)A*算法和DFS算法的割草機(jī)器人遍歷路徑規(guī)劃*

王新彥，盛冠杰，張凱，易政洋

(江蘇科技大學(xué)機(jī)械工程學(xué)院，江蘇鎮(zhèn)江，212100)

0 引言

近年來(lái)，隨著移動(dòng)機(jī)器人技術(shù)的迅速發(fā)展，智能化設(shè)備已被廣泛應(yīng)用于社會(huì)的各個(gè)領(lǐng)域，給人們的生產(chǎn)生活帶來(lái)了極大的便利[1-3]。割草機(jī)器人作為農(nóng)業(yè)機(jī)器人的一種，不僅用于市政綠地、機(jī)場(chǎng)、高爾夫球場(chǎng)等場(chǎng)地的草坪修剪工作，還用于耕地和林地的除草工作，因此受到廣泛關(guān)注，而遍歷路徑規(guī)劃在割草機(jī)器人應(yīng)用過(guò)程中起著至關(guān)重要的作用[4-6]。

全覆蓋遍歷路徑規(guī)劃是一種在二維環(huán)境中特殊的路徑規(guī)劃，指在滿足某種性能指標(biāo)最優(yōu)的前提下，在設(shè)定區(qū)域內(nèi)尋找一條從起點(diǎn)到終點(diǎn)且經(jīng)過(guò)所有可達(dá)點(diǎn)的連續(xù)路徑[7-9]。該技術(shù)發(fā)展至今已涌現(xiàn)出了大量研究成果，Balch采用隨機(jī)遍歷算法進(jìn)行目標(biāo)區(qū)域遍歷，使機(jī)器人沿直線移動(dòng)，當(dāng)遇到障礙物時(shí)隨機(jī)轉(zhuǎn)動(dòng)一個(gè)角度繼續(xù)前進(jìn)，該方法原理簡(jiǎn)單、易于實(shí)現(xiàn)，但要達(dá)到高覆蓋率需要很多時(shí)間，工作效率低。張赤斌等[10]將蟻群算法應(yīng)用到遍歷路徑規(guī)劃中，通過(guò)牛耕式分解法將目標(biāo)區(qū)域分解成若干個(gè)不含障礙物的子區(qū)域，采用蟻群算法搜索出子區(qū)域的遍歷順序，該方法覆蓋率高，但算法運(yùn)算量大且跨區(qū)域轉(zhuǎn)移路徑長(zhǎng)。李楷等[11]提出了一種局部區(qū)域覆蓋和回溯機(jī)制相結(jié)合的遍歷方法，利用回溯機(jī)制對(duì)局部覆蓋過(guò)程中存在的遺漏區(qū)域進(jìn)行回溯遍歷，以此提高遍歷覆蓋率，但存在遍歷重復(fù)率較高的問(wèn)題。Le等[12]提出了一種基于螺旋生成樹(shù)的遍歷方法，該方法遍歷重復(fù)率低，但其規(guī)劃的遍歷路徑轉(zhuǎn)向次數(shù)多且易陷入死區(qū)。馬全坤等[13]提出了一種基于記憶模擬退火和A*算法的遍歷方法，采用記憶模擬退火算法確定各子區(qū)域的遍歷順序，通過(guò)A*算法規(guī)劃跨區(qū)域轉(zhuǎn)移路徑，該方法性能較優(yōu)，但跨區(qū)域轉(zhuǎn)移路徑較長(zhǎng)。

上述研究成果對(duì)遍歷路徑規(guī)劃技術(shù)的發(fā)展起到了積極的推動(dòng)作用，但仍存在遍歷重復(fù)率高、遺漏區(qū)域多、普適性弱的問(wèn)題。為此，本文提出一種改進(jìn)A*算法與DFS算法相結(jié)合的遍歷算法，通過(guò)DFS算法確定目標(biāo)區(qū)域劃分后的子區(qū)域遍歷順序，然后使用改進(jìn)A*算法進(jìn)行跨區(qū)域轉(zhuǎn)移路徑規(guī)劃，從而實(shí)現(xiàn)割草機(jī)器人的遍歷路徑規(guī)劃。

1 環(huán)境建模

1.1 建立柵格地圖

本文采用柵格法[14]進(jìn)行環(huán)境建模，將割草機(jī)器人的工作環(huán)境全局信息分解成一系列具有二值信息的等大小正方形網(wǎng)格單元，單元格邊長(zhǎng)為割草機(jī)器人割幅。接著每個(gè)柵格被賦予環(huán)境信息值：若某個(gè)柵格內(nèi)不含障礙物，則稱(chēng)該柵格為自由柵格并賦值為0，反之被稱(chēng)為障礙柵格并賦值為1。本文結(jié)合市政綠地的現(xiàn)實(shí)情況，對(duì)環(huán)境做如下假定：(1)假定市政綠地環(huán)境全局信息已知；(2)假定灌木植物為圓形障礙物；(3)假定石塊、垃圾桶等為多邊形障礙物；(4)假定積水區(qū)、小土丘等為形狀不規(guī)則障礙物。

建立的柵格地圖如圖1所示，把環(huán)境信息賦予對(duì)應(yīng)的柵格，相當(dāng)于將環(huán)境地圖離散化，每個(gè)單元格映射出環(huán)境信息的一部分。

圖1 柵格地圖

1.2 障礙物膨脹處理

在柵格地圖中存在一些障礙物的邊緣不完全與柵格單元邊界重合，如圖1所示，此時(shí)需要進(jìn)行圖像處理，通過(guò)二值形態(tài)學(xué)中的膨脹運(yùn)算對(duì)這些障礙物的邊緣區(qū)域進(jìn)行膨脹處理，使其邊緣在柵格化的地圖中與柵格單元邊界重合，以防止割草機(jī)器人陷入死角以及智能算法陷入局部最優(yōu)[15]，以便后續(xù)遍歷算法在此基礎(chǔ)上進(jìn)行路徑規(guī)劃。

(1)

根據(jù)上述膨脹數(shù)學(xué)模型，邊緣不完全與柵格單元邊界重合的障礙物目標(biāo)經(jīng)過(guò)膨脹處理后得到的新障礙物柵格M，如圖2所示。

圖2 膨脹處理后的障礙物

1.3 目標(biāo)區(qū)域劃分

柵格地圖建立后，由于障礙物分布毫無(wú)規(guī)律，割草機(jī)器人工作時(shí)易出現(xiàn)漏割、重復(fù)遍歷、效率低等情況。為提高工作效率，采用牛耕式分解法將目標(biāo)區(qū)域劃分成多個(gè)不含障礙物的子區(qū)域，使割草機(jī)器人通過(guò)遍歷單個(gè)子區(qū)域并進(jìn)行子區(qū)域之間路徑轉(zhuǎn)移，來(lái)達(dá)到全覆蓋遍歷路徑規(guī)劃。

牛耕式分解法[16]是從梯形單元分解法[17]上改進(jìn)而來(lái)，該方法假設(shè)一條垂直的切割線從左往右掃描整個(gè)有界區(qū)域，每當(dāng)遇到障礙物上的臨界點(diǎn)或邊時(shí)便進(jìn)行分割，目標(biāo)區(qū)域中的非障礙物部分進(jìn)而被分割成多個(gè)子區(qū)域。

相比梯形單元分解法，該方法使目標(biāo)區(qū)域劃分過(guò)程中產(chǎn)生的子區(qū)域數(shù)量更少，可有效降低遍歷重復(fù)率。子區(qū)域劃分后，按照從上到下、從左到右的原則對(duì)各子區(qū)域進(jìn)行數(shù)字標(biāo)號(hào)，如圖3所示，以便后續(xù)子區(qū)域遍歷順序的規(guī)劃。

圖3 目標(biāo)區(qū)域劃分

2 基于柵格地圖的遍歷方法

目標(biāo)區(qū)域完成劃分后，割草機(jī)器人要實(shí)現(xiàn)對(duì)目標(biāo)區(qū)域的全覆蓋遍歷可通過(guò)以下兩個(gè)步驟：(1)根據(jù)子區(qū)域之間連通關(guān)系確定子區(qū)域的遍歷順序，保證每個(gè)子區(qū)域在遍歷路徑規(guī)劃中有且僅被訪問(wèn)一次；(2)按照給定的遍歷順序依次訪問(wèn)每個(gè)子區(qū)域，完成子區(qū)域間路徑轉(zhuǎn)移。

2.1 子區(qū)域遍歷順序規(guī)劃

子區(qū)域的遍歷順序規(guī)劃采用DFS算法[18]，該算法是圖算法中一種著名的搜索算法，經(jīng)常被用來(lái)遍歷連通圖中每個(gè)節(jié)點(diǎn)，找到一條詳盡可靠的覆蓋路徑，算法步驟如表1所示。

將目標(biāo)區(qū)域劃分得到的每個(gè)子區(qū)域看作連通圖中節(jié)點(diǎn)，按照子區(qū)域間相鄰關(guān)系建立連通圖，如圖4(a)所示。根據(jù)目標(biāo)區(qū)域劃分中切割線移動(dòng)方向，將①設(shè)置為遍歷起始區(qū)域，圖4(b)是使用DFS算法規(guī)劃的子區(qū)域遍歷順序圖。

表1 DFS算法步驟Tab. 1 DFS algorithm steps

(a) 連通圖

(b) 遍歷順序圖

2.2 子區(qū)域間路徑轉(zhuǎn)移

為了使割草機(jī)器人有序地完成對(duì)每個(gè)子區(qū)域的遍歷，就需要解決子區(qū)域之間路徑轉(zhuǎn)移的問(wèn)題。針對(duì)該問(wèn)題，本文采用改進(jìn)A*算法規(guī)劃從上一個(gè)子區(qū)域遍歷終點(diǎn)到下一個(gè)子區(qū)域遍歷起點(diǎn)的最短路徑。

2.2.1 A*算法的評(píng)價(jià)函數(shù)

A*算法是人工智能中一種典型的啟發(fā)式搜索算法，被廣泛應(yīng)用于靜態(tài)路網(wǎng)中兩點(diǎn)間最優(yōu)路徑的求解。其評(píng)價(jià)函數(shù)如式(2)所示。

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

(2)

式中：f(n)——從起始節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的評(píng)價(jià)函數(shù)；

g(n)——起始節(jié)點(diǎn)到當(dāng)前節(jié)點(diǎn)n的實(shí)際代價(jià)函數(shù)；

h(n)——當(dāng)前節(jié)點(diǎn)n到目標(biāo)節(jié)點(diǎn)的啟發(fā)代價(jià)函數(shù)。

在二維地圖中，代價(jià)函數(shù)的值通常指兩個(gè)節(jié)點(diǎn)之間的歐幾里得距離，即

(3)

(4)

式中： (xs,ys)——起始點(diǎn)位置；

(xn,yn)——當(dāng)前點(diǎn)位置；

(xt,yt)——目標(biāo)點(diǎn)位置。

2.2.2 改進(jìn)A*算法

A*算法規(guī)劃所得到的是一系列相鄰的路徑節(jié)點(diǎn)，將各相鄰節(jié)點(diǎn)用直線連接起來(lái)即為所得到的路徑。由于A*算法規(guī)劃的路徑存在冗余的路徑節(jié)點(diǎn)且轉(zhuǎn)向次數(shù)多[19]，導(dǎo)致路徑平滑性較差且路徑長(zhǎng)度不是最短，同時(shí)路徑穿過(guò)障礙物邊界點(diǎn)時(shí)易發(fā)生碰撞。針對(duì)上述問(wèn)題，本文通過(guò)路徑平滑性優(yōu)化和添加防碰撞安全間距對(duì)A*算法進(jìn)行改進(jìn)，改進(jìn)A*算法的流程圖如圖5所示。

圖5 改進(jìn)A*算法流程圖

防碰撞預(yù)處理是通過(guò)計(jì)算障礙物邊界點(diǎn)到路徑的垂直距離l，與設(shè)置的安全間距η進(jìn)行對(duì)比，判斷路徑是否安全。防碰撞安全間距η的大小與柵格單元邊長(zhǎng)γ有關(guān)，滿足η∈(0,0.5γ),η的值可根據(jù)實(shí)際情況進(jìn)行調(diào)節(jié)。如圖6所示，路徑節(jié)點(diǎn)A、B坐標(biāo)分別為(xa,ya)、(xb,yb)，障礙物邊界點(diǎn)H坐標(biāo)為(xe,ye)，直線BC垂直于H點(diǎn)所在的橫軸，垂足為C點(diǎn)，連接CH，直線AB與直線CH相交于F點(diǎn)，H點(diǎn)到直線AB的垂直距離DH即為l的長(zhǎng)度，線段FH長(zhǎng)度為t，θ為直線AB與直線BC所夾銳角。由于A、B、H三點(diǎn)坐標(biāo)已知，可求解障礙物邊界點(diǎn)H到路徑AB的垂直距離l。

直線AB與直線BC所夾銳角

(5)

線段FH長(zhǎng)度

(6)

障礙物邊界點(diǎn)H到路徑AB的垂直距離

l=t·cosθ

(7)

圖6 防碰撞路徑圖

A*算法的改進(jìn)是在獲取A*算法規(guī)劃的路徑節(jié)點(diǎn)后進(jìn)行，如圖7所示，Ni(i=1～13)是A*算法規(guī)劃的路徑節(jié)點(diǎn)，i為路徑節(jié)點(diǎn)的標(biāo)號(hào)。改進(jìn)算法的步驟如下。

步驟1：從起點(diǎn)G往終點(diǎn)E方向，取初始路徑的第一個(gè)節(jié)點(diǎn)Ni(i=1)。

步驟2：若節(jié)點(diǎn)Ni+1是終點(diǎn)E，轉(zhuǎn)至步驟6；若節(jié)點(diǎn)Ni+1既不是終點(diǎn)E，也不是起點(diǎn)G，轉(zhuǎn)至步驟3；若節(jié)點(diǎn)Ni+1是起點(diǎn)G，轉(zhuǎn)至步驟7。

步驟3：判斷直線NiNi+1與直線Ni+1Ni+2斜率是否相等。若斜率相等，節(jié)點(diǎn)Ni、Ni+1、Ni+2共線，刪除節(jié)點(diǎn)Ni+1并使Ni后面節(jié)點(diǎn)的標(biāo)號(hào)均減1，返回步驟2；若斜率不相等，節(jié)點(diǎn)Ni、Ni+1、Ni+2不共線，轉(zhuǎn)至步驟4。

步驟4：判斷節(jié)點(diǎn)Ni和Ni+2之間是否存在障礙物。若有障礙物，路徑節(jié)點(diǎn)不變，轉(zhuǎn)至步驟5；若無(wú)障礙物，由式(5)～式(7)計(jì)算障礙物邊界點(diǎn)到路徑NiNi+2的垂直距離l并判斷與安全間距η的大小：如果l≤η，路徑節(jié)點(diǎn)不變，轉(zhuǎn)至步驟5；如果l>η，刪除節(jié)點(diǎn)Ni+1并使Ni后面節(jié)點(diǎn)的標(biāo)號(hào)均減1，返回步驟2。

步驟5：令i=i+1，更新i的值并返回步驟2。

步驟6：從終點(diǎn)E往起點(diǎn)G方向，取初始路徑的第一個(gè)節(jié)點(diǎn)Ni(i=1)，返回步驟2。

步驟7：輸出優(yōu)化路徑Path，算法結(jié)束。如圖7所示，優(yōu)化后路徑為：G→N6→M1→E。從起點(diǎn)G往終點(diǎn)E正向取點(diǎn)，處理后路徑為：G→N6→N11→E；從終點(diǎn)E往起點(diǎn)G反向取點(diǎn)，處理后路徑為：E→N7→N4→N3→G。

圖7 路徑平滑性處理

3 遍歷路徑規(guī)劃仿真

通過(guò)使用牛耕式分解法將目標(biāo)區(qū)域劃分成多個(gè)不含障礙物的子區(qū)域；其次，使用DFS算法確定子區(qū)域的遍歷順序；然后，采用改進(jìn)A*算法搜索出最短、無(wú)碰撞的跨區(qū)域轉(zhuǎn)移路徑并對(duì)子區(qū)域內(nèi)部進(jìn)行往復(fù)前進(jìn)式遍歷，最終實(shí)現(xiàn)對(duì)整個(gè)柵格地圖的遍歷路徑規(guī)劃。

為了直觀、清晰地顯示仿真結(jié)果，柵格地圖由23×23 個(gè)柵格組成，其中黑色區(qū)域?yàn)檎系K物占據(jù)的柵格，其他白色區(qū)域是自由柵格。圖8為遍歷路徑規(guī)劃仿真結(jié)果圖，由圖可知，不論在上文建立的普通環(huán)境模型中，還是在特殊的復(fù)雜環(huán)境模型中，本文提出的遍歷路徑規(guī)劃算法的覆蓋率都達(dá)到100%且遍歷重復(fù)率為0。

圖9為A*算法改進(jìn)前后規(guī)劃的跨區(qū)域轉(zhuǎn)移路徑對(duì)比圖，其中每個(gè)子區(qū)域的遍歷起點(diǎn)用“●”表示，每個(gè)子區(qū)域的遍歷終點(diǎn)用“★”表示。遍歷整個(gè)柵格地圖需進(jìn)行九次跨區(qū)域路徑轉(zhuǎn)移，這里主要對(duì)比其中三條轉(zhuǎn)移路徑：⑩—⑨區(qū)域、⑨—⑥區(qū)域、⑥—③區(qū)域，對(duì)應(yīng)路徑在圖9中用加粗黑線表示。

結(jié)合圖9、表2和表3可以看出，改進(jìn)后的A*算法所規(guī)劃的路徑刪除了冗余的路徑節(jié)點(diǎn)，使路徑長(zhǎng)度縮短了3.26%，轉(zhuǎn)向次數(shù)減少了62.5%，提高了路徑的平滑性，同時(shí)，由于添加了防碰撞安全間距，改進(jìn)后的A*算法所規(guī)劃的路徑不會(huì)出現(xiàn)穿過(guò)障礙物邊界點(diǎn)的情況，路徑安全性得到提高。

(a) 普通環(huán)境

(b) 復(fù)雜環(huán)境

(a) A*算法

(b) 改進(jìn)A*算法

表2 路徑轉(zhuǎn)移距離對(duì)比Tab. 2 Comparison of path transfer distance

表3 路徑轉(zhuǎn)移轉(zhuǎn)向次數(shù)對(duì)比Tab. 3 Comparison of the number of times of path transfer

4 結(jié)論

針對(duì)割草機(jī)器人大面積作業(yè)時(shí)遍歷路徑規(guī)劃覆蓋率低、重復(fù)率高、普適性弱的問(wèn)題，本文提出了一種改進(jìn)A*算法與DFS算法相結(jié)合的遍歷算法。

1) 針對(duì)目標(biāo)區(qū)域劃分后的子區(qū)域間路徑轉(zhuǎn)移，采用改進(jìn)A*算法規(guī)劃轉(zhuǎn)移路徑。通過(guò)路徑平滑性優(yōu)化與添加防碰撞安全間距對(duì)A*算法進(jìn)行改進(jìn)，使之規(guī)劃的路徑更平滑、更安全，路徑長(zhǎng)度更短。MATLAB仿真試驗(yàn)表明，采用改進(jìn)A*算法所規(guī)劃的跨區(qū)域轉(zhuǎn)移路徑長(zhǎng)度和轉(zhuǎn)向次數(shù)比A*算法分別減少了3.26%和62.5%，路徑不會(huì)穿過(guò)障礙物邊界點(diǎn)。

2) 針對(duì)割草機(jī)器人遍歷路徑規(guī)劃，提出了一種改進(jìn)A*算法與DFS算法相結(jié)合的遍歷算法。通過(guò)DFS算法確定目標(biāo)區(qū)域劃分后的子區(qū)域遍歷順序，然后使用改進(jìn)A*算法進(jìn)行跨區(qū)域轉(zhuǎn)移路徑規(guī)劃。通過(guò)仿真試驗(yàn)表明，遍歷路徑規(guī)劃覆蓋率達(dá)到100%，遍歷重復(fù)率為0，本文遍歷算法具備有效性和可行性。由于本文算法只適用于靜態(tài)環(huán)境，下一步將在此工作基礎(chǔ)上進(jìn)一步展開(kāi)動(dòng)態(tài)環(huán)境中遍歷路徑規(guī)劃的探討與研究。