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

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

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

0 引言

路徑規(guī)劃是機器人研究領(lǐng)域必不可少的一項技術(shù),近年來該技術(shù)正逐漸應(yīng)用于智能物流、智能家居、海上搜救、巡航等領(lǐng)域[1-2]。所謂路徑規(guī)劃，就是讓機器人在遵循一定的評價指標(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ī)劃算法則有動態(tài)窗口法、人工勢場法、遺傳算法等[4-6]。

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

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

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

1.1 A*算法

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

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

(1)

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

如圖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é)點?？梢园l(fā)現(xiàn)，A*算法存在的某些不足之處[12]：由于A*算法在尋路時，需要不斷地對當(dāng)前節(jié)點的8個相鄰節(jié)點進(jìn)行搜索，并計算其估計代價，但實際上只有少量節(jié)點與最終生成的路徑有關(guān)，需要進(jìn)行必要的計算，而大部分的節(jié)點與最終生成的路徑毫無關(guān)聯(lián)，不必對這些無用節(jié)點進(jìn)行訪問。這一缺點使得這些無用節(jié)點隨著配置空間的增大而增多，不僅消耗內(nèi)存，也降低了尋路效率。

1.2 跳點搜索法

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

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

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

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

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

(2)

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

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

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

(3)

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

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

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

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

(4)

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

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

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

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

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

(5)

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

(6)

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

(7)

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

2 動態(tài)窗口法

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

2.1 機器人的運動模型

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

(8)

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

2.2 機器人的速度采樣

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

(9)

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

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

(10)

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

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

(11)

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

2.3 評價函數(shù)

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

(12)

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

3 融合算法

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

(13)

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

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

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

4 仿真與實驗

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

4.2 實驗與結(jié)果分析

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

圖11 環(huán)境地圖

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

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

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

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

5 結(jié)論

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