融合優(yōu)化A*算法與動(dòng)態(tài)窗口法的動(dòng)態(tài)路徑規(guī)劃算法研究

姚進(jìn)鑫,劉麗桑,3,何棟煒,3,陳 健,王 斌,徐 輝,郭江峰,陳 煒,3

(1.福建工程學(xué)院 電子電氣與物理學(xué)院，福州 350118；2.福建工程學(xué)院 福建省工業(yè)集成自動(dòng)化行業(yè)技術(shù)開發(fā)基地，福州 350118；3.福建工程學(xué)院 電子信息與電氣技術(shù)國家級實(shí)驗(yàn)教學(xué)示范中心，福州 350118)

0 引言

路徑規(guī)劃是機(jī)器人研究領(lǐng)域必不可少的一項(xiàng)技術(shù),近年來該技術(shù)正逐漸應(yīng)用于智能物流、智能家居、海上搜救、巡航等領(lǐng)域[1-2]。所謂路徑規(guī)劃，就是讓機(jī)器人在遵循一定的評價(jià)指標(biāo)函數(shù)的前提下，在充滿障礙的環(huán)境中能夠順利避開障礙物，從而規(guī)劃出一條從起始位置到目標(biāo)位置的最優(yōu)路徑[3]。隨著無人駕駛和人工智能等新興領(lǐng)域的出現(xiàn)，國內(nèi)外的學(xué)者們研究了許多種不同的路徑規(guī)劃算法。根據(jù)規(guī)劃所針對的目標(biāo)范圍的不同，可以將其分為全局路徑規(guī)劃和局部路徑規(guī)劃。其中A*算法、Dijkstra算法、D*算法等均為較為常見的全局路徑規(guī)劃算法，而局部路徑規(guī)劃算法則有動(dòng)態(tài)窗口法、人工勢場法、遺傳算法等[4-6]。

由于標(biāo)準(zhǔn)A*算法在運(yùn)行時(shí)需要從起始點(diǎn)開始，不斷地對相鄰節(jié)點(diǎn)進(jìn)行擴(kuò)展，并利用定義的代價(jià)估計(jì)函數(shù)來尋找使函數(shù)取得最小值的節(jié)點(diǎn)，因此該算法的耗時(shí)主要在于對相鄰節(jié)點(diǎn)代價(jià)估計(jì)函數(shù)的計(jì)算以及對最小的節(jié)點(diǎn)的選擇。如此一來，標(biāo)準(zhǔn)A*算法就會(huì)存在一些不可避免的缺陷：隨著機(jī)器人所處的二維空間的增大，算法擴(kuò)展的無用節(jié)點(diǎn)不斷增加，使得計(jì)算數(shù)據(jù)量與算法搜索時(shí)間不斷增加，尋路效率不高。趙曉等[7]將跳點(diǎn)搜索法與A*算法相結(jié)合，對A*算法進(jìn)行優(yōu)化，通過篩選得到的關(guān)鍵跳點(diǎn)間的直線跳躍到達(dá)目標(biāo)點(diǎn)，得到最終路徑。這種方法雖然跳過了A*算法中擴(kuò)展的大量無用節(jié)點(diǎn)，從而減少算法的計(jì)算量，提高了路徑規(guī)劃速度，但是所規(guī)劃出來的路徑曲率變化率不連續(xù)，轉(zhuǎn)折處不夠光滑，且無法實(shí)現(xiàn)動(dòng)態(tài)避障。張敬寒等[8]通過擴(kuò)大其算法的搜索鄰域并采用最小二叉堆的方法來優(yōu)化標(biāo)準(zhǔn)A*算法，進(jìn)一步縮短和減少了其路徑長度和拐點(diǎn)的數(shù)量，并通過3次均勻B樣條曲線使路徑得到進(jìn)一步平滑，提高了路徑規(guī)劃效率，但沒有考慮到其路徑規(guī)劃的實(shí)時(shí)性和對于動(dòng)態(tài)障礙物的避障性能。而動(dòng)態(tài)窗口法是在采樣的多組速度中考慮速度與單位時(shí)間內(nèi)的速度變化量組成的約束條件，并在模擬生成的運(yùn)動(dòng)軌跡中找到使評價(jià)函數(shù)取最大值的軌跡所對應(yīng)的速度驅(qū)動(dòng)機(jī)器人。但在障礙物密集等特殊情況下，存在陷入局部區(qū)域而無法到達(dá)目標(biāo)位置的致命問題，這也是局部路徑規(guī)劃算法普遍存在的問題之一。王洪斌等[9]通過引入二次A*算法，并結(jié)合目標(biāo)成本函數(shù)對所有目標(biāo)點(diǎn)進(jìn)行優(yōu)先級判定，同時(shí)采用自適應(yīng)圓弧優(yōu)化與加權(quán)障礙物步長調(diào)節(jié)算法，有效地縮短和減少了路徑長度和轉(zhuǎn)折次數(shù)，最后通過預(yù)瞄偏差角追蹤算法改進(jìn)動(dòng)態(tài)窗口法來對動(dòng)態(tài)障礙物進(jìn)行捕捉，提升了路徑規(guī)劃效率。程傳奇等[10]則在動(dòng)態(tài)窗口法的基礎(chǔ)上，根據(jù)關(guān)鍵點(diǎn)選取策略篩選出少量轉(zhuǎn)折點(diǎn)設(shè)計(jì)了一種能夠滿足全局最優(yōu)的評價(jià)指標(biāo)，并將該指標(biāo)作為動(dòng)態(tài)窗口法的評價(jià)函數(shù)，以此來提高路徑的平滑性以及動(dòng)態(tài)避障能力。但由于其關(guān)鍵點(diǎn)選取策略無法應(yīng)對復(fù)雜場景下的實(shí)時(shí)路徑規(guī)劃，使得該算法存在一定的局限性。

本文首先基于跳點(diǎn)搜索法對A*算法進(jìn)行優(yōu)化，然后與動(dòng)態(tài)窗口法相結(jié)合，提出了一種將優(yōu)化A*算法與動(dòng)態(tài)窗口法相融合的混合路徑規(guī)劃算法。一方面由于跳點(diǎn)搜索法的加入，使得優(yōu)化后的A*算法在靜態(tài)路徑規(guī)劃的速度上得以提高，另一方面該混合算法在傳統(tǒng)動(dòng)態(tài)窗口法的基礎(chǔ)上，結(jié)合跳點(diǎn)搜索法所獲得的跳躍點(diǎn)信息提出了一個(gè)新的方位角評價(jià)指標(biāo)，經(jīng)這一評價(jià)指標(biāo)優(yōu)化后的評價(jià)函數(shù)使得機(jī)器人在動(dòng)態(tài)環(huán)境中也能尋找到一條全局最優(yōu)路徑，克服了原算法易陷入局部最優(yōu)的弊端。

1 基于跳點(diǎn)搜索的優(yōu)化A*算法

1.1 A*算法

A*算法是在靜態(tài)的二維配置空間中用于計(jì)算最優(yōu)路徑的啟發(fā)式搜索算法，它綜合了經(jīng)典的Dijkstra算法與啟發(fā)式的最佳優(yōu)先搜索(BFS)算法的優(yōu)點(diǎn)，通過代價(jià)評估函數(shù)搜索最優(yōu)路徑節(jié)點(diǎn)作為下一個(gè)需要遍歷的節(jié)點(diǎn),重復(fù)這一過程，直到發(fā)現(xiàn)目標(biāo)點(diǎn)，形成最優(yōu)路徑[11]。其中，代價(jià)評估函數(shù)為：

f(m)=h(m)+g(m)

(1)

式中:f(m)表示當(dāng)前位置m代價(jià)評估函數(shù)；g(m)表示機(jī)器人從初始位置到達(dá)當(dāng)前位置m的實(shí)際代價(jià)；h(m)表示機(jī)器人從當(dāng)前位置m到目標(biāo)位置的估計(jì)代價(jià)；h(m)的選擇直接影響了A*算法的成功率與準(zhǔn)確性，其值的大小與實(shí)際值越接近，搜索效率就越高。

如圖1所示為A*算法規(guī)劃的路徑。

圖1 A*算法規(guī)劃路徑

從圖1中的綠色網(wǎng)格開始，規(guī)劃一條到達(dá)紅色網(wǎng)格的最優(yōu)路徑，淺藍(lán)色網(wǎng)格為最終規(guī)劃出的最優(yōu)路徑，淺綠色網(wǎng)格為在規(guī)劃過程中搜索過的節(jié)點(diǎn)。可以發(fā)現(xiàn)，A*算法存在的某些不足之處[12]：由于A*算法在尋路時(shí)，需要不斷地對當(dāng)前節(jié)點(diǎn)的8個(gè)相鄰節(jié)點(diǎn)進(jìn)行搜索，并計(jì)算其估計(jì)代價(jià)，但實(shí)際上只有少量節(jié)點(diǎn)與最終生成的路徑有關(guān)，需要進(jìn)行必要的計(jì)算，而大部分的節(jié)點(diǎn)與最終生成的路徑毫無關(guān)聯(lián)，不必對這些無用節(jié)點(diǎn)進(jìn)行訪問。這一缺點(diǎn)使得這些無用節(jié)點(diǎn)隨著配置空間的增大而增多，不僅消耗內(nèi)存，也降低了尋路效率。

1.2 跳點(diǎn)搜索法

跳點(diǎn)搜索法(jump point search)是由Daniel Har-abor和Alban Grastien提出的一種新的尋路算法[13]。與A*算法不同的是，該算法中引入了跳點(diǎn)的概念，通過一定的篩選規(guī)則選出那些表征方向變化的特定節(jié)點(diǎn)后，機(jī)器人只按照這些特定節(jié)點(diǎn)進(jìn)行跳躍式前進(jìn),這些特定節(jié)點(diǎn)也稱為跳點(diǎn)。對于跳點(diǎn)的搜索，刪除空間網(wǎng)格中的那些無用節(jié)點(diǎn)是跳點(diǎn)搜索法的核心，也叫剪枝，陶凱[14]提出了許多剪枝規(guī)則。根據(jù)節(jié)點(diǎn)的8鄰域障礙物的有無，可以將其大致分為2種情況進(jìn)行闡述。

1.2.1節(jié)點(diǎn)鄰域沒有障礙物

當(dāng)某一節(jié)點(diǎn)的周圍8鄰域節(jié)點(diǎn)中不存在障礙物時(shí)，則該節(jié)點(diǎn)將沿直線和對角線方向搜索，如圖2所示。當(dāng)該節(jié)點(diǎn)向直線方向搜索至當(dāng)前節(jié)點(diǎn)x時(shí)，如圖2(a)所示。圖中點(diǎn)p為當(dāng)前位置節(jié)點(diǎn)x的父節(jié)點(diǎn)，會(huì)發(fā)現(xiàn)一個(gè)現(xiàn)象：若以p點(diǎn)為起點(diǎn)，直接抵達(dá)網(wǎng)格標(biāo)為灰色的節(jié)點(diǎn)(這里假設(shè)抵達(dá)節(jié)點(diǎn)n)，得到路徑(圖中虛線箭頭)所花費(fèi)的時(shí)間要優(yōu)于同樣從p點(diǎn)出發(fā)的一條途徑當(dāng)前位置x到達(dá)灰色節(jié)點(diǎn)的路徑長度。如果我們在此過程中去評估這條路徑p→x→n則會(huì)增加其所帶來的耗時(shí)成本。因此，當(dāng)由p點(diǎn)水平向右搜索到當(dāng)前位置x時(shí)，只需考慮y節(jié)點(diǎn)即可。A*算法中正是因?yàn)樵u估了大量類似灰色網(wǎng)格這種毫無意義的節(jié)點(diǎn)，無形中增加了計(jì)算量，降低了搜索效率。所以，假設(shè)在當(dāng)前位置節(jié)點(diǎn)x的8鄰域內(nèi)任取一節(jié)點(diǎn)n為目標(biāo)節(jié)點(diǎn),若要規(guī)劃一條起點(diǎn)為p、目標(biāo)為n且耗時(shí)成本最優(yōu)的路徑，在向水平和垂直方向直線擴(kuò)展時(shí)，顯然，只有滿足以下約束所生成的路徑p→x→y才是最優(yōu)路徑[15-16]。

圖2 節(jié)點(diǎn)的8鄰域沒有障礙物

length(p,…,n|x)>length(p,x,n)

(2)

length(p,x,n)衡量的是從出發(fā)點(diǎn)p到目標(biāo)點(diǎn)n且經(jīng)過x的路徑長度；length(p,…,n|x)衡量的同樣是從出發(fā)點(diǎn)p到目標(biāo)點(diǎn)n的路徑長度，但這條路徑繞過了x。

同樣，當(dāng)節(jié)點(diǎn)的擴(kuò)展為對角線方向時(shí)，如圖2(b)所示。同樣，只有滿足以下約束所生成的路徑p→x→y才是最優(yōu)路徑[7]。

length(p,…,n|x)≥length(p,x,n)

(3)

由此，可得到一條剪枝規(guī)則，即當(dāng)節(jié)點(diǎn)領(lǐng)域內(nèi)不存在障礙物時(shí)，應(yīng)剪去不滿足約束條件(2)和(3)的灰色網(wǎng)格節(jié)點(diǎn)，而滿足約束條件白色網(wǎng)格(如圖中的y，y1，y2，y3)則被稱作節(jié)點(diǎn)x的自然鄰節(jié)點(diǎn)。

1.2.2節(jié)點(diǎn)鄰域有障礙物

在算法運(yùn)行過程中，總會(huì)遇到鄰域內(nèi)存在障礙物的節(jié)點(diǎn)。如圖3(a)和3(b)所示，節(jié)點(diǎn)同樣可以向直線方向和對角線2個(gè)方向進(jìn)行搜索，且同樣需要剪去圖中的灰色節(jié)點(diǎn)。不同的是，圖2中節(jié)點(diǎn)n屬于非自然鄰節(jié)點(diǎn)，而圖3中節(jié)點(diǎn)n卻不必執(zhí)行剪枝操作，需將其標(biāo)為白色。因?yàn)樵趫D3中的節(jié)點(diǎn)x上方遇到了障礙物，此時(shí)只有以p點(diǎn)為起點(diǎn)，并經(jīng)過當(dāng)前位置x到達(dá)節(jié)點(diǎn)n得到的路徑才是所有可行路徑中的耗時(shí)成本最優(yōu)路徑，因此，將這一節(jié)點(diǎn)叫作強(qiáng)制鄰節(jié)點(diǎn)[15-16]。于是，得到強(qiáng)制鄰節(jié)點(diǎn)需要滿足的條件：

圖3 節(jié)點(diǎn)的8鄰域有障礙物

(4)

由此可得到在遭遇障礙物時(shí)的剪枝規(guī)則，即當(dāng)擴(kuò)展的節(jié)點(diǎn)領(lǐng)域內(nèi)遭遇障礙物時(shí)，應(yīng)剪去不滿足約束條件(2)和(3)且除去滿足約束條件(4)的強(qiáng)制鄰節(jié)點(diǎn)n以及障礙物節(jié)點(diǎn)以外的剩余灰色節(jié)點(diǎn)。滿足該條件的節(jié)點(diǎn)x就是算法中所要尋找的跳點(diǎn)。也就是說，一個(gè)所謂的跳點(diǎn)其實(shí)就是一個(gè)鄰域內(nèi)含有強(qiáng)制鄰節(jié)點(diǎn)的網(wǎng)格節(jié)點(diǎn)[16]。

1.3 基于跳點(diǎn)搜索的優(yōu)化A*算法

在剪掉那些無意義的節(jié)點(diǎn)后，便得到了一系列的跳躍點(diǎn)。對這些跳躍點(diǎn)執(zhí)行標(biāo)準(zhǔn)A*算法的操作后，便可快速地得到全局路徑。優(yōu)化的A*算法的流程如圖4所示。

圖4 優(yōu)化A*算法流程框圖

對于優(yōu)化A*算法中距離的度量方式，使用較多的度量方式有歐式幾何距離或曼哈頓距離。在平面直角坐標(biāo)系中任取兩點(diǎn)(pi,qi)和(pj,qj)，則這兩點(diǎn)之間的最短直線距離DEij稱為歐式幾何距離；兩點(diǎn)橫坐標(biāo)之差的絕對值與縱坐標(biāo)之差的絕對值之和DMij稱為曼哈頓距離。即

(5)

DMij=|pi-pj|+|qi-qj|

(6)

為了方便機(jī)器人的操控，結(jié)合以上2種度量方式,本文為估計(jì)代價(jià)h(m)設(shè)計(jì)了一種更接近實(shí)際的新的距離函數(shù)[10]：

(7)

式中，dx(m)=|pi-pj|,dy(m)=|qi-qj|，(pi,qi)、(pj,qj)分別為起始點(diǎn)和目標(biāo)點(diǎn)的坐標(biāo)。

2 動(dòng)態(tài)窗口法

動(dòng)態(tài)窗口法是在由速度和加速度組成的速度矢量空間中對多組速度進(jìn)行采樣操作，并綜合考慮速度和加減速性能的約束，用機(jī)器人的運(yùn)動(dòng)模型模擬這些速度在一段時(shí)間間隔內(nèi)的運(yùn)動(dòng)軌跡，并依據(jù)評價(jià)指標(biāo)對所獲得的軌跡進(jìn)行評價(jià)，最后選取評價(jià)最高的軌跡所對應(yīng)的速度和加速度作為機(jī)器人的驅(qū)動(dòng)速度參數(shù)。該算法的核心就是將路徑規(guī)劃問題轉(zhuǎn)化成帶約束的速度矢量空間的優(yōu)化問題[11,17]。

2.1 機(jī)器人的運(yùn)動(dòng)模型

實(shí)現(xiàn)動(dòng)態(tài)窗口法的第一步需要獲取機(jī)器人的運(yùn)動(dòng)模型，這也是模擬其運(yùn)動(dòng)軌跡的前提條件。假設(shè)機(jī)器人的運(yùn)動(dòng)軌跡由一段微小的圓弧軌跡組成，每段圓弧軌跡都對應(yīng)著唯一的速度矢量(vt,ωt)。由于每段圓弧軌跡生成的時(shí)間間隔Δt很短，可以近似將這段軌跡看成是一段直線軌跡。于是，假設(shè)機(jī)器人在一段時(shí)間間隔Δt內(nèi)以恒定的速度作直線運(yùn)動(dòng)，得到機(jī)器人的運(yùn)動(dòng)模型為

(8)

式中：xt、yt、θt為機(jī)器人在t時(shí)刻所處的坐標(biāo)位置與方位角；xt+1、yt+1、θt+1為機(jī)器人在t+1時(shí)刻所處的坐標(biāo)位置與方位角；vt和ωt分別為t時(shí)刻機(jī)器人的平移速度與旋轉(zhuǎn)角速度。

2.2 機(jī)器人的速度采樣

為求解上述的機(jī)器人運(yùn)動(dòng)模型，則需要對機(jī)器人的速度進(jìn)行采樣并代入模型公式求解，推算出模擬軌跡。在動(dòng)態(tài)窗口法中定義了3種不同的約束來限制采樣速度的范圍，第1種是受機(jī)器人所處環(huán)境以及自身物理結(jié)構(gòu)的限制，機(jī)器人所能達(dá)到的速度范圍為[17-18]：

(9)

式中：vmax和vmin為機(jī)器人所能達(dá)到的最大、最小的線速度；ωmax和ωmin為機(jī)器人所能達(dá)到的最大、最小角速度。

第2種約束考慮了遭遇障礙物時(shí)的情景。當(dāng)機(jī)器人采用最大減速度vb′、ωb′進(jìn)行緊急制動(dòng)時(shí)，為了使機(jī)器人能在碰撞之前停下來，保證其安全性，必須設(shè)定其容許的速度范圍：

(10)

式中：dist(v,ω)表示速度矢量(v,ω)模擬的軌跡距最近障礙物的距離。

第3種約束是在一個(gè)模擬的時(shí)間間隔Δt內(nèi)受電機(jī)所允許的最大加速度va′、ωa′和最大減速度vb′、ωb′的限制，所能達(dá)到的速度范圍：

(11)

式中：vc、ωc表示機(jī)器人當(dāng)前的線速度和角速度。

2.3 評價(jià)函數(shù)

在多組采樣速度組成的集合下，通過模擬可以得到多組可行的軌跡。要從這些可行軌跡中選擇最優(yōu)軌跡，則需要從多個(gè)維度對其進(jìn)行評估，使得機(jī)器人能夠沿著最優(yōu)軌跡安全、快速到達(dá)目標(biāo)位置。在傳統(tǒng)的動(dòng)態(tài)窗口法中設(shè)計(jì)了方位角評價(jià)函數(shù)head(v,ω)、障礙物間隙評價(jià)函數(shù)dist(v,ω)和速度評價(jià)函數(shù)vel(v,ω)3個(gè)加權(quán)項(xiàng)。其評價(jià)函數(shù)為：

(12)

式中：head(v,ω)是衡量當(dāng)前選擇的采樣速度下所產(chǎn)生的模擬軌跡末端方向與目標(biāo)位置方向的角度偏差，偏差量越大，head(v,ω)的評分越低；dist(v,ω)是衡量速度矢量(v,ω)模擬的軌跡距最近障礙物的距離，評分越低，機(jī)器人與障礙物相撞的幾率越大；vel(v,ω)是衡量機(jī)器人在當(dāng)前軌跡下朝目標(biāo)點(diǎn)的行駛速度；σ、β、γ是這3個(gè)評價(jià)指標(biāo)的加權(quán)系數(shù)。

3 融合算法

優(yōu)化的A*算法能夠快速獲得全局路徑信息，能夠很好地應(yīng)對僅有靜態(tài)障礙物的簡單環(huán)境，但對于復(fù)雜環(huán)境中出現(xiàn)的動(dòng)態(tài)障礙物卻無法進(jìn)行實(shí)時(shí)避障。而傳統(tǒng)的動(dòng)態(tài)窗口法有著不錯(cuò)的動(dòng)態(tài)避障性能，但因其評價(jià)函數(shù)指標(biāo)head(v,ω)中只評估最終目標(biāo)點(diǎn)這一個(gè)方面，容易出現(xiàn)陷入局部區(qū)域無法到達(dá)目標(biāo)的致命缺陷，使得它難以根據(jù)全局環(huán)境信息規(guī)劃出一條全局最優(yōu)的路徑。針對上述2種算法存在的問題，本文采用上文所提的優(yōu)化A*算法所獲得的全局路徑跳躍點(diǎn)信息與傳統(tǒng)的動(dòng)態(tài)窗口法相融合，提出新的方位角評價(jià)指標(biāo)JPHead(v,ω)，并對傳統(tǒng)動(dòng)態(tài)窗口法的評價(jià)函數(shù)進(jìn)行優(yōu)化，使得優(yōu)化后的動(dòng)態(tài)窗口法評價(jià)函數(shù)在動(dòng)態(tài)避障的同時(shí)充分考慮其全局最優(yōu)性。優(yōu)化后的評價(jià)函數(shù)表達(dá)式為

(13)

JPHead(v,ω)與原方位角評價(jià)指標(biāo)head(v,ω)的區(qū)別在于，JPHead(v,ω)衡量的是模擬軌跡末端與距離當(dāng)前軌跡最近的跳點(diǎn)之間的方位角偏差，可有效避免陷入局部最優(yōu)，保證系統(tǒng)可到達(dá)最終目標(biāo)點(diǎn)。

該融合算法保證了在進(jìn)行動(dòng)態(tài)路徑規(guī)劃時(shí)能夠沿著全局最優(yōu)路徑點(diǎn)規(guī)劃出一條平滑的全局最優(yōu)路徑。改進(jìn)的動(dòng)態(tài)窗口法流程如圖5所示。

圖5 優(yōu)化的動(dòng)態(tài)窗口法流程框圖

4 仿真與實(shí)驗(yàn)

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

為了驗(yàn)證提出的基于優(yōu)化的A*算法與動(dòng)態(tài)窗口法的融合算法的有效性與泛化性，本文在Matlab中建立了一個(gè)15×15的網(wǎng)格環(huán)境，并分別對傳統(tǒng)的A*算法和與跳點(diǎn)搜索法相結(jié)合的優(yōu)化A*算法、傳統(tǒng)的動(dòng)態(tài)窗口法和本文融合算法進(jìn)行了4組對比仿真實(shí)驗(yàn)。另外，針對融合算法又進(jìn)行了第5組實(shí)驗(yàn)，來驗(yàn)證本文算法在動(dòng)態(tài)環(huán)境下的實(shí)時(shí)避障能力。

第1、2組仿真實(shí)驗(yàn)結(jié)果如圖6、7所示。其中，起始節(jié)點(diǎn)用綠色圓圈表示，其坐標(biāo)分別為(3,14)和(3,3)；目標(biāo)節(jié)點(diǎn)則用紅色圓圈表示，其坐標(biāo)分別為(15,4)和(15,10)；形狀大小不一的多個(gè)黑色障礙物分別置于圖中的不同位置，圖6(a)與圖7(a)中用黑色圓圈標(biāo)記的節(jié)點(diǎn)表示在傳統(tǒng)A*算法搜索過程中所遍歷的節(jié)點(diǎn)，圖6(b)與圖7(b)中用黑色十字標(biāo)記的節(jié)點(diǎn)表示在優(yōu)化A*算法搜索過程中所發(fā)現(xiàn)的跳點(diǎn)，藍(lán)色的折線為2種算法在不同環(huán)境下所得到的最優(yōu)路徑。表1為2種算法在不同環(huán)境下的路徑規(guī)劃性能對比。

圖7 在實(shí)驗(yàn)環(huán)境2下傳統(tǒng)A*算法與優(yōu)化A*算法的仿真結(jié)果

通過圖6、7與表1可以看出，與傳統(tǒng)A*算法相比，經(jīng)過跳點(diǎn)搜索法優(yōu)化后的A*算法在不同的實(shí)驗(yàn)環(huán)境下擴(kuò)展節(jié)點(diǎn)數(shù)有顯著減少，分別減少了80.90%和82.14%。在路徑長度方面，實(shí)驗(yàn)環(huán)境1下2種算法得到的路徑長度相同，而實(shí)驗(yàn)環(huán)境2下反而還比傳統(tǒng)A*算法要稍長一些，即便如此，在2個(gè)不同環(huán)境下的搜索時(shí)間卻分別減少了5.41%與18.37%，且在實(shí)驗(yàn)環(huán)境2下的轉(zhuǎn)折次數(shù)也有所減少。綜合來看，優(yōu)化后的A*算法在得到與傳統(tǒng)A*算法基本相同的全局路徑的基礎(chǔ)上，其搜索路徑的時(shí)間和速度，甚至是轉(zhuǎn)折次數(shù)上還要優(yōu)于傳統(tǒng)A*算法。盡管如此，但是優(yōu)化A*算法始終沒有解決路徑轉(zhuǎn)折處不夠光滑的問題。

表1 傳統(tǒng)A*算法與優(yōu)化A*算法在不同環(huán)境下的路徑規(guī)劃性能

圖6 在實(shí)驗(yàn)環(huán)境1下傳統(tǒng)A*算法與優(yōu)化A*算法的仿真結(jié)果

第3、4組仿真實(shí)驗(yàn)結(jié)果如圖8、9所示。2種環(huán)境下的起始節(jié)點(diǎn)和目標(biāo)節(jié)點(diǎn)的設(shè)置以及障礙物分布與第1、2組實(shí)驗(yàn)相同，起始節(jié)點(diǎn)和目標(biāo)節(jié)點(diǎn)同樣用綠色圓圈和紅色圓圈表示。設(shè)置圖6(b)中機(jī)器人的最大線速度和最大角速度為3.5 m/s和40 rad/s；最大線加速度和最大角加速度為0.35 m/s2和60 rad/s；線速度分辨率為0.01 m/s，角速度分辨率為1 rad/s。評價(jià)函數(shù)的3個(gè)加權(quán)系數(shù)σ、β、γ設(shè)置為0.05、0.2、0.1。表2為2種算法在不同環(huán)境下的路徑規(guī)劃性能對比。另外，通過將優(yōu)化A*算法與融合算法形成的2條路徑分別劃分成相同間隔相同等份的線段，通過計(jì)算各段斜率的平均值即平滑度，來衡量路徑的平滑程度，平滑度越小，代表其平滑性越好。如表3所示。

表2 傳統(tǒng)動(dòng)態(tài)窗口法與融合算法在不同環(huán)境下的路徑規(guī)劃性能

表3 優(yōu)化A*算法與融合算法在不同環(huán)境下的平滑度

從圖8、9以及表2、3可以看出，在相差無幾的時(shí)間內(nèi)與相同的實(shí)驗(yàn)環(huán)境1下，融合算法規(guī)劃的路徑長度比傳統(tǒng)動(dòng)態(tài)窗口法更短。而在相同的實(shí)驗(yàn)環(huán)境2下，融合算法很好地解決了傳統(tǒng)動(dòng)態(tài)窗口法容易陷入局部最優(yōu)而無法到達(dá)目標(biāo)點(diǎn)的缺陷，成功地抵達(dá)目標(biāo)點(diǎn)，完成路徑規(guī)劃。不僅如此，融合算法還解決了優(yōu)化A*算法路徑轉(zhuǎn)折處不夠光滑的問題，使之不僅與優(yōu)化A*算法一樣具有全局路徑規(guī)劃能力，且其規(guī)劃的路徑比優(yōu)化A*算法更加平滑。

圖8 在實(shí)驗(yàn)環(huán)境1下傳統(tǒng)動(dòng)態(tài)窗口法與融合算法的仿真結(jié)果

圖9 在實(shí)驗(yàn)環(huán)境2下傳統(tǒng)動(dòng)態(tài)窗口法與融合算法的仿真結(jié)果

第5組的動(dòng)態(tài)仿真實(shí)驗(yàn)過程如圖10所示，圖中新增了黑色障礙物與紅色障礙物用以模擬動(dòng)態(tài)障礙，它們的半徑均為0.3 m，其初始位置分別為(6,13.5)和(11.5,7)。令黑色障礙物以0.15 m/s的速度向右方移動(dòng)，紅色障礙物則以0.25 m/s的速度向上方移動(dòng)。從圖10(b)和10(c)中可以看到，當(dāng)遇到正在緩慢向右移動(dòng)的黑色障礙物時(shí)，機(jī)器人會(huì)主動(dòng)避開它并選擇向其移動(dòng)方向的左側(cè)繞行，避免與障礙物發(fā)生碰撞。接著，機(jī)器人會(huì)繼續(xù)朝著目標(biāo)點(diǎn)方向前進(jìn)，當(dāng)?shù)竭_(dá)如圖10(d)所示的位置時(shí)，機(jī)器人與紅色障礙物交叉相遇，而此時(shí)在其移動(dòng)方向的左側(cè)又存在著一個(gè)靜態(tài)障礙物，且2個(gè)障礙物之間的距離十分接近，使得機(jī)器人無法安全地向左繞行進(jìn)行避障，最終，機(jī)器人選擇向移動(dòng)方向的右側(cè)繞過這2個(gè)障礙物進(jìn)行避障，如圖10(e)所示。從動(dòng)態(tài)仿真實(shí)驗(yàn)的過程可以看到，無論是追及黑色障礙物還是與紅色障礙物交叉相遇，機(jī)器人都能很好地避開障礙物，且其規(guī)劃的路徑依然滿足全局最優(yōu)性。

圖10 在動(dòng)態(tài)環(huán)境下融合算法的仿真實(shí)驗(yàn)結(jié)果

4.2 實(shí)驗(yàn)與結(jié)果分析

為了進(jìn)一步驗(yàn)證提出的融合算法在實(shí)際環(huán)境中的實(shí)現(xiàn)效果，搭建了一輛搭載有ROS系統(tǒng)的智能小車，同時(shí)該智能小車還配備有LeTMC-520深度攝像頭，減速比為30的24 V電動(dòng)機(jī)以及RPLIDAR-A2激光雷達(dá)傳感器等硬件設(shè)備，并采用搭載Intel I3處理器的工控機(jī)作為小車的主控制器。在所有實(shí)驗(yàn)開始之前，需要對智能小車的角速度，線速度和IMU進(jìn)行校正，并整定電動(dòng)機(jī)控制模塊的PID參數(shù)，以確保所有實(shí)驗(yàn)?zāi)軌蝽樌M(jìn)行。然后，使用儲(chǔ)物箱和紙箱作為靜態(tài)障礙物搭建了一個(gè)簡單的實(shí)驗(yàn)環(huán)境模型，再將小車置于該實(shí)驗(yàn)環(huán)境模型中，通過上位機(jī)來控制小車以及其搭載的傳感器進(jìn)行實(shí)驗(yàn)環(huán)境信息的采集。最后，通過采集到的環(huán)境信息數(shù)據(jù)在上位機(jī)的Rviz平臺(tái)中構(gòu)建出如圖11所示環(huán)境地圖，并將構(gòu)建好的環(huán)境地圖及時(shí)保存至工控機(jī)中，以方便進(jìn)行以下實(shí)驗(yàn)。

圖11 環(huán)境地圖

實(shí)際環(huán)境下的實(shí)驗(yàn)同樣分為靜態(tài)實(shí)驗(yàn)和動(dòng)態(tài)實(shí)驗(yàn)兩部分。在靜態(tài)實(shí)驗(yàn)中，首先，在地圖中指定了一個(gè)起始點(diǎn)和一個(gè)目標(biāo)點(diǎn)，分別位于如圖12(a)和圖12(e)所示的位置。然后，讓智能小車根據(jù)本文融合算法進(jìn)行路徑規(guī)劃，并到達(dá)指定的目標(biāo)點(diǎn)。從圖12所示的實(shí)驗(yàn)過程可以看到，智能小車從圖12(a)所示的起始位置開始行駛，并在如圖12(b)，12(c)和12(d)所示的整個(gè)行駛過程中平穩(wěn)地避開了所有靜態(tài)障礙物，最后沿著全局最優(yōu)路徑安全地到達(dá)目標(biāo)點(diǎn)，如圖12(e)所示。

圖12 本文融合算法在靜態(tài)實(shí)驗(yàn)環(huán)境下的實(shí)驗(yàn)過程

與靜態(tài)實(shí)驗(yàn)環(huán)境不同的是，在動(dòng)態(tài)實(shí)驗(yàn)環(huán)境中添加了一輛朝著左上角方向移動(dòng)的藍(lán)色小車作為動(dòng)態(tài)障礙物。實(shí)驗(yàn)開始后，智能小車同樣從與圖12(a)中相同的起始點(diǎn)開始行駛，如圖13(a)所示。當(dāng)在行駛過程中遇到藍(lán)色小車時(shí)，智能小車則及時(shí)向右轉(zhuǎn)彎并繞過藍(lán)色小車以避免與之發(fā)生碰撞，如圖13(b)和13(c)所示。隨后，智能小車又返回到融合算法規(guī)劃的最佳路徑上并繼續(xù)行駛，如圖13(d)所示。

圖13 本文融合算法在動(dòng)態(tài)實(shí)驗(yàn)環(huán)境下的實(shí)驗(yàn)過程

5 結(jié)論

本文在跳點(diǎn)搜索的基礎(chǔ)上對A*算法進(jìn)行優(yōu)化，設(shè)計(jì)了由曼哈頓和歐氏距離結(jié)合的新的距離評估函數(shù)獲取全局路徑信息；將優(yōu)化的A*算法應(yīng)用于動(dòng)態(tài)窗口法中評價(jià)函數(shù)的優(yōu)化上，提出了新的方位角評價(jià)指標(biāo)JPHead(v,ω)，形成了新的融合算法，避免了原算法易陷入局部最優(yōu)的缺陷。設(shè)計(jì)了具有動(dòng)、靜態(tài)障礙物的若干不同的環(huán)境并進(jìn)行仿真實(shí)驗(yàn)，結(jié)果表明：在相同的實(shí)驗(yàn)環(huán)境下，新的融合算法能在不改變優(yōu)化A*算法規(guī)劃的原路徑的基礎(chǔ)上，增加了規(guī)劃路徑在轉(zhuǎn)折處的平滑程度，使機(jī)器人在復(fù)雜環(huán)境中的移動(dòng)更順滑，同時(shí)縮短了路徑長度；而在不同的實(shí)驗(yàn)環(huán)境下，融合算法解決了傳統(tǒng)動(dòng)態(tài)窗口法陷入局部區(qū)域的問題，并以平滑的路徑到達(dá)目標(biāo)點(diǎn)。通過一系列實(shí)驗(yàn)可以看到，本文融合算法有效地提高了機(jī)器人在復(fù)雜場景中路徑規(guī)劃能力，但其運(yùn)行速度還有待提高，這也是下一步的工作重點(diǎn)。