基于改進(jìn)D*算法的巡檢機(jī)器人路徑規(guī)劃

秦 旭，黃曉華，馬東明，翁亞華

(南京理工大學(xué)機(jī)械工程學(xué)院，南京 210014)

0 引言

巡檢機(jī)器人需要在工作場(chǎng)景中規(guī)劃出一條從初始位置到目標(biāo)位置的路徑[1-2]，該路徑應(yīng)滿足路程短、效率高、安全性好等一系列要求，并且必須能夠避開沿途的靜態(tài)和隨機(jī)動(dòng)態(tài)障礙物[3]，路徑規(guī)劃的好壞直接決定了巡檢任務(wù)是否能夠順利完成。因此，路徑規(guī)劃對(duì)于巡檢機(jī)器人的任務(wù)完成起到舉足輕重的作用。

目前較為常用的路徑規(guī)劃算法有Dijkstra算法、A*算法。Dijkstra算法可以在實(shí)際環(huán)境中搜索一個(gè)節(jié)點(diǎn)到其他任意節(jié)點(diǎn)的最短路徑[4]，屬于貪心算法的一種；A*算法是在Dijkstra算法基礎(chǔ)上進(jìn)行改進(jìn)，融合了貪心思想和啟發(fā)思想，使用啟發(fā)信息來(lái)引導(dǎo)搜索方向，故又稱啟發(fā)式算法[5]，并可以添加安全約束[6]。然而，Dijkstra算法與A*算法在遇到動(dòng)態(tài)障礙物時(shí)，只能進(jìn)行重復(fù)規(guī)劃，效率較低；而在巡檢機(jī)器人實(shí)際工作過程中，可能會(huì)出現(xiàn)動(dòng)態(tài)障礙，于是就產(chǎn)生了D*算法[7]，D*算法又稱動(dòng)態(tài)A*(dynamic A*)算法，與A*算法類似，不同在于是從終點(diǎn)到起點(diǎn)進(jìn)行反向搜索。

但是D*算法也存在著一些問題，D*算法以長(zhǎng)度優(yōu)先，并且向周圍8個(gè)子節(jié)點(diǎn)進(jìn)行擴(kuò)展搜索，生成的路徑會(huì)產(chǎn)生一些不必要的轉(zhuǎn)彎，會(huì)增加巡檢機(jī)器人的功耗和時(shí)間；長(zhǎng)度優(yōu)先的準(zhǔn)則也導(dǎo)致生成的路徑某些時(shí)候會(huì)從兩個(gè)障礙物之間穿過，并未考慮到機(jī)器人本身的外形尺寸[8]。對(duì)此，許多學(xué)者對(duì)D*算法也做出了一些改進(jìn)，張賀等[9]基于動(dòng)態(tài)窗口法，將 A*算法與D*算法結(jié)合，最大程度確保了路徑精度。劉曉濤等[10]提出了一種基于支持向量機(jī)的D*改進(jìn)算法，規(guī)劃出較為平滑的動(dòng)態(tài)路徑。史久根等[11]針對(duì)D*算法搜索空間較大的問題,引入了抽象分層思想,將環(huán)境結(jié)構(gòu)化為層次圖，分段搜索，提高了搜索效率。

上述文獻(xiàn)中的方法均在一定程度上改進(jìn)了D*算法，但依然存在一些弊端：①所得路徑與障礙物之間具有相對(duì)較近的距離，現(xiàn)實(shí)生活中，機(jī)器人應(yīng)時(shí)刻與障礙物留有相對(duì)適中的距離；②為了保證路徑規(guī)劃時(shí)的效率從而增加了路徑的拐點(diǎn)數(shù)，未做到兩者間的平衡。

因此，本文提出一定的優(yōu)化方案，優(yōu)化代價(jià)估計(jì)函數(shù)并引入平滑度函數(shù)，在降低所得路徑拐點(diǎn)數(shù)目的同時(shí)又提高了算法效率；控制了路徑與障礙物之間的距離，考慮到了實(shí)際應(yīng)用過程中追求的低功耗和安全性，這些在制造行業(yè)的運(yùn)維過程中都是十分必要的。

1 問題描述

1.1 地圖建模

在進(jìn)行路徑規(guī)劃前，首先要對(duì)巡檢機(jī)器人所處的真實(shí)環(huán)境進(jìn)行地圖建模，常用的地圖主要有柵格地圖、拓?fù)涞貓D、語(yǔ)義地圖等。其中柵格地圖數(shù)據(jù)結(jié)構(gòu)簡(jiǎn)單、能有效表達(dá)空間的可變性[12]。故本文采用柵格法對(duì)巡檢機(jī)器人的工作環(huán)境進(jìn)行二維模型搭建。

為了驗(yàn)證最終算法的通用性，用randi函數(shù)隨機(jī)生成靜態(tài)和動(dòng)態(tài)障礙物(生成的柵格地圖見后續(xù)仿真圖)。

不同類型的柵格區(qū)域的含義如表1所示。

表1 柵格類型含義

1.2 D*算法

1.2.1 D*算法簡(jiǎn)介

D*算法又稱動(dòng)態(tài)A*算法，是一種動(dòng)態(tài)逆向扇形搜索算法，逆向的搜索機(jī)制保留了地圖成本，避免了回溯的高計(jì)算成本，這也是D*算法最大的優(yōu)點(diǎn)。相比于A*算法，D*算法在某些動(dòng)態(tài)環(huán)境下搜索效率更高，與另一些啟發(fā)式算法相比，D*算法計(jì)算量小，實(shí)現(xiàn)起來(lái)比較簡(jiǎn)單，所以D*算法在路徑規(guī)劃和人工智能等方面得到了廣泛的應(yīng)用。

首先，D*算法的核心思想之一就是路徑優(yōu)劣評(píng)價(jià)公式，表達(dá)式如下：

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

(1)

式中，n為當(dāng)前狀態(tài)；f(n)為從初始狀態(tài)經(jīng)由狀態(tài)n到目標(biāo)狀態(tài)的代價(jià)估計(jì)；g(n)為初始狀態(tài)到狀態(tài)n的實(shí)際代價(jià)；h(n)為狀態(tài)n到目標(biāo)狀態(tài)的代價(jià)估計(jì)，又稱啟發(fā)函數(shù)。

對(duì)于兩個(gè)狀態(tài)之間的代價(jià)，在實(shí)際應(yīng)用過程中，通常以距離作為代價(jià)值；對(duì)于二維空間內(nèi)的兩個(gè)點(diǎn)(x1,y1)、(x2,y2)，又有幾種不同的計(jì)算機(jī)制作為距離的度量，主要有歐氏距離、曼哈頓距離，這些度量機(jī)制相應(yīng)的表達(dá)式分別如下：

(2)

dManhattan=|x2-x1|+|y2-y1|

(3)

在D*算法中，常用的是歐式距離作為代價(jià)值。

其次，與A*算法類似，D*算法同樣定義了兩個(gè)列表合集：openList與closeList；openList表由待考察的節(jié)點(diǎn)組成，closeList表由已經(jīng)考察過的節(jié)點(diǎn)組成，類似于Dijkstra算法中的U集合和S集合，當(dāng)closeList表中出現(xiàn)目標(biāo)節(jié)點(diǎn)時(shí)，程序停止。

對(duì)于上文提到的啟發(fā)函數(shù)h，在D*算法中，若Y為X的父節(jié)點(diǎn)，設(shè)兩個(gè)節(jié)點(diǎn)X，Y之間的代價(jià)為C(X,Y)，則有h(X)=h(Y)+C(X,Y)；當(dāng)X狀態(tài)發(fā)生變化后(如前進(jìn)過程中原有自由空間遭遇動(dòng)態(tài)障礙物)，那么h(X)會(huì)發(fā)生改變，設(shè)k(X)取變化前后最小的值。

最后，在D*算法的流程中，還定義了兩個(gè)非常重要的狀態(tài)：Lower態(tài)和Raise態(tài)；Lower態(tài)代表在機(jī)器人行進(jìn)過程中并未發(fā)生突發(fā)情況，地圖信息如同初始規(guī)劃一樣，即h(X)=k(X)；若在原始規(guī)劃的路徑上的一個(gè)或多個(gè)節(jié)點(diǎn)變?yōu)榱藙?dòng)態(tài)障礙物，如圖2所示，則可知此時(shí)C(X,Y)=∞，此時(shí)h(X)>k(X)，稱為Raise態(tài)。此外，算法將每一個(gè)節(jié)點(diǎn)的狀態(tài)分為3類：未被遍歷到的節(jié)點(diǎn)為new；在openList表中的為open；被移入到closeList表中的為closed。

圖2 改進(jìn)D*算法流程圖

1.2.2 D*算法流程

D*算法主要分為兩個(gè)部分，第一部分為Process_state，是基于A*算法從目標(biāo)點(diǎn)往起始點(diǎn)搜索，主要用于環(huán)境信息已知，障礙物為靜態(tài)的路徑規(guī)劃；第二部分為Modify_cost，主要用于修正出現(xiàn)動(dòng)態(tài)障礙物時(shí)導(dǎo)致代價(jià)值發(fā)生變化時(shí)的節(jié)點(diǎn)信息，屬于動(dòng)態(tài)避障搜索階段。D*算法仿真結(jié)果如圖1所示，圖1a表示當(dāng)障礙物為靜態(tài)時(shí)，地圖信息如同初始規(guī)劃一樣，即h(X)=k(X)；若在路徑中遭遇動(dòng)態(tài)障礙物為黑色柵格時(shí)，節(jié)點(diǎn)狀態(tài)變?yōu)镽aise態(tài)，灰色柵格為再規(guī)劃路線，可以看到未受影響的節(jié)點(diǎn)仍在再規(guī)劃路徑中。

(a) 原始路徑 (b) 遭遇動(dòng)態(tài)障礙物時(shí)的路徑

在圖1中可以直觀的看出，D*算法僅僅需要重新計(jì)算從當(dāng)前節(jié)點(diǎn)到繞過動(dòng)態(tài)障礙物的新路徑，避免了回溯的高計(jì)算成本。然而D*算法是向周圍8個(gè)子節(jié)點(diǎn)進(jìn)行擴(kuò)展搜索，忽略了動(dòng)態(tài)障礙物導(dǎo)致最終路徑存在的非必要轉(zhuǎn)彎問題；使用歐式距離指標(biāo)進(jìn)行評(píng)價(jià)，在長(zhǎng)度優(yōu)先的前提下，會(huì)從兩個(gè)障礙物之間穿過，對(duì)巡檢過程中的安全性產(chǎn)生了一定的威脅。

2 D*算法改進(jìn)

針對(duì)上述問題，本文對(duì)傳統(tǒng)D*算法進(jìn)行了改進(jìn)：優(yōu)化了子節(jié)點(diǎn)的選取方式，以局部環(huán)境關(guān)鍵節(jié)點(diǎn)為主，舍棄無(wú)用節(jié)點(diǎn)和危險(xiǎn)節(jié)點(diǎn)；又改進(jìn)了代價(jià)估計(jì)函數(shù)，針對(duì)傳統(tǒng)的歐式距離作為代價(jià)值進(jìn)行了修正，新增了平滑度函數(shù)對(duì)規(guī)劃路線偏離理想路徑的程度進(jìn)行懲罰，避免巡檢機(jī)器人在小范圍區(qū)域內(nèi)較高的轉(zhuǎn)彎頻率，以增加路徑的平滑度。

2.1 節(jié)點(diǎn)選取的改進(jìn)

在D*算法中，是以當(dāng)前節(jié)點(diǎn)為基準(zhǔn)，選擇其8個(gè)鄰近子節(jié)點(diǎn)完成擴(kuò)展操作，由于使用歐氏距離作為代價(jià)的評(píng)估，易知在斜方向上代價(jià)值較低，這也就間接導(dǎo)致了路徑會(huì)在兩個(gè)障礙物之間穿過的情形，本文將子節(jié)點(diǎn)的選取分為兩個(gè)級(jí)別：高級(jí)子節(jié)點(diǎn)和低級(jí)子節(jié)點(diǎn)，為避免穿過障礙物之間的狹縫，將如圖3所示的水平豎直方向上的標(biāo)識(shí)為(2,4,6,8)的子節(jié)點(diǎn)作為高級(jí)子節(jié)點(diǎn)，斜方向上的標(biāo)識(shí)為(1,3,5,7)的子節(jié)點(diǎn)作為低級(jí)子節(jié)點(diǎn)；選用高級(jí)子節(jié)點(diǎn)作為首選擴(kuò)展方向，并按照以下準(zhǔn)則選用低級(jí)子節(jié)點(diǎn)：

圖3 子節(jié)點(diǎn)擴(kuò)展

(1)若2為障礙物，禁選節(jié)點(diǎn)1、3。即不生成0-1、0-3路徑。

(2)若4為障礙物，禁選節(jié)點(diǎn)3、5。即不生成0-3、0-5路徑。

(3)若6為障礙物，禁選節(jié)點(diǎn)5、7。即不生成0-5、0-7路徑。

(4)若8為障礙物，禁選節(jié)點(diǎn)7、1。即不生成0-7、0-1路徑。

2.2 代價(jià)估計(jì)函數(shù)的改進(jìn)

上文中提到，D*算法常用歐氏距離作為代價(jià)值。由式(2)可知，需要進(jìn)行開方處理，降低了程序的運(yùn)行速度，并且歐氏距離即為二維空間下兩點(diǎn)間的直線距離，并未考慮到巡檢機(jī)器人在運(yùn)行過程中的角度信息，故而最終會(huì)規(guī)劃出多次轉(zhuǎn)彎的路徑。

因此，本文首先對(duì)代價(jià)估計(jì)函數(shù)中代價(jià)值的選取做出優(yōu)化，選用切比雪夫距離取代歐氏距離，在二維空間內(nèi)，兩個(gè)點(diǎn)(x1,y1)、(x2,y2)之間的切比雪夫距離的表達(dá)式如下：

dChebyshev=max||x2-x1|，|y2-y1||

(4)

本文在研究中設(shè)置垂直水平方向的代價(jià)值為10，對(duì)角線方向的代價(jià)值為14，對(duì)于使得路徑產(chǎn)生不利拐角的最優(yōu)節(jié)點(diǎn)，既人為增加其啟發(fā)值，使得其變?yōu)榇巫顑?yōu)節(jié)點(diǎn)；又避免了開方運(yùn)算，從而提高算法速度。此外，令：

hdiagonal=min(|x2-x1|,|y2-y1|)

(5)

hstraight=||x2-x1|-|y2-y1||

(6)

將代價(jià)值帶入，可得到優(yōu)化后的代價(jià)估計(jì)函數(shù)為：

h(n)=14min(|x2-x1|，|y2-y1|)+10||x2-x1|-|y2-y1||

(7)

2.3 平滑度函數(shù)的引入

本文在改進(jìn)代價(jià)估計(jì)函數(shù)的基礎(chǔ)上再引入平滑度函數(shù)f(θ)對(duì)規(guī)劃路徑偏離理想路徑(初始點(diǎn)與目標(biāo)點(diǎn)的直線)的程度進(jìn)行懲罰，篩選出每一次擴(kuò)展時(shí)偏移程度相對(duì)較小的路徑，從而最大可能性上避免移動(dòng)機(jī)器人的無(wú)效轉(zhuǎn)彎。定義路徑平滑度函數(shù)[12]如下：

(8)

式中，θ為待定子節(jié)點(diǎn)與父節(jié)點(diǎn)之間形成的路徑與理想路徑之間的夾角；μ為環(huán)境因子。

2.3.1 平滑度函數(shù)的分段

圖4 路徑移動(dòng)方向

由圖4可知，當(dāng)路徑為兩個(gè)柵格的對(duì)角線時(shí)，存在4個(gè)代價(jià)值相同的子節(jié)點(diǎn)且關(guān)于X軸對(duì)稱，因此須將X軸上2個(gè)子節(jié)點(diǎn)和X軸下2個(gè)子節(jié)點(diǎn)分開即可，故將θ平均分為3段。

2.3.2μ值的選取

μ值描述不同環(huán)境下障礙物對(duì)環(huán)境的影響，與障礙物所占環(huán)境地圖面積比率直接相關(guān)[12]。在MATLAB代碼中，隨機(jī)生成的障礙物概率為20%～30%，結(jié)合實(shí)驗(yàn)與實(shí)踐，本文取μ=2。

3 仿真結(jié)果與分析

3.1 仿真環(huán)境與測(cè)試對(duì)象

為了對(duì)改進(jìn)后的D*算法的性能進(jìn)行驗(yàn)證，對(duì)于傳統(tǒng)的D*算法與改進(jìn)后的D*算法進(jìn)行對(duì)比仿真，算法代碼在MATLAB R2019a平臺(tái)進(jìn)行編譯。

建立不同尺寸的柵格地圖模型，為了保證仿真實(shí)驗(yàn)結(jié)果的普遍性，避免特定環(huán)境帶來(lái)的影響，采用不同數(shù)據(jù)在同一臺(tái)計(jì)算機(jī)上分別進(jìn)行多次仿真，分別建立不同尺寸的柵格地圖，并隨機(jī)生成靜態(tài)和動(dòng)態(tài)障礙物，并選取不同的障礙物覆蓋率進(jìn)行多次對(duì)比。

在實(shí)際仿真中，對(duì)同一障礙物覆蓋率下的路徑規(guī)劃進(jìn)行多次仿真，由于篇幅限制，本文只附上10×20的柵格地圖環(huán)境下20%、25%、30%的障礙物覆蓋率時(shí)的各一份對(duì)比仿真圖。分別如圖5～圖7所示。

(a) 傳統(tǒng)D*算法 (b) 改進(jìn)D*算法

(a) 傳統(tǒng)D*算法 (b) 改進(jìn)D*算法

(a) 傳統(tǒng)D*算法 (b) 改進(jìn)D*算法

3.2 仿真結(jié)果分析

對(duì)于每種柵格地圖，采用不同的障礙物覆蓋率和障礙物分布，進(jìn)行數(shù)十次的仿真并記錄下所得路徑的平均拐點(diǎn)數(shù)和算法平均規(guī)劃時(shí)間，所得結(jié)果如表2和表3所示。

表2 不同環(huán)境下拐點(diǎn)數(shù)目

表3 不同環(huán)境下規(guī)劃時(shí)間

由上述表格可知，改進(jìn)后的D*算法相較于傳統(tǒng)D*算法所規(guī)劃的路徑拐點(diǎn)數(shù)量更少，減少了約30%；可以在較小的迭代次數(shù)內(nèi)獲得最優(yōu)解，從而使得規(guī)劃時(shí)間更少，時(shí)間節(jié)約了約20%；在地圖環(huán)境越復(fù)雜時(shí)，越能體現(xiàn)該算法的優(yōu)越性。因此，本文建立的改進(jìn)D*算法可有效地避免傳統(tǒng)D*算法的轉(zhuǎn)彎次數(shù)多的缺陷，并且提高了算法效率，規(guī)劃所得路徑也具有更高的安全性，有效提升了路徑質(zhì)量。

4 結(jié)束語(yǔ)

本文針對(duì)傳統(tǒng)D*算法所得的路徑不平滑、生成路徑與障礙物之間的距離相對(duì)較近等問題，優(yōu)化了子節(jié)點(diǎn)的選取方式，對(duì)子節(jié)點(diǎn)進(jìn)行了優(yōu)先級(jí)的分配，舍棄無(wú)用節(jié)點(diǎn)和危險(xiǎn)節(jié)點(diǎn)，從而在較大程度上提高了路徑安全性；又改進(jìn)了代價(jià)估計(jì)函數(shù)；新增了平滑度函數(shù)，降低了路徑的拐點(diǎn)數(shù)。

仿真處理結(jié)果表明，改進(jìn)的D*算法均發(fā)揮了一定的優(yōu)勢(shì)，改善了生成的路徑質(zhì)量，使得拐點(diǎn)數(shù)目減少了約30%；提高了規(guī)劃效率，使規(guī)劃時(shí)間節(jié)約了約20%；增加了與障礙物間的距離，保障了機(jī)器人工作過程中的安全性。

本算法仍有改進(jìn)之處，下一階段可嘗試向更遠(yuǎn)節(jié)點(diǎn)擴(kuò)展，細(xì)化擴(kuò)展方向的角度；在后續(xù)研究中也可找出路徑長(zhǎng)度、安全性與平滑度三者之間的平衡。