改進(jìn)A*和動(dòng)態(tài)窗口法的移動(dòng)機(jī)器人路徑規(guī)劃

陳 嬌，徐 菱，陳 佳，劉 卿

(西南交通大學(xué) 交通運(yùn)輸與物流學(xué)院，四川 成都 611756)

0 引言

目前，無(wú)人車(chē)、移動(dòng)機(jī)器人等移動(dòng)智能體廣泛應(yīng)用于工業(yè)制造、物流、醫(yī)療等多個(gè)領(lǐng)域，路徑規(guī)劃問(wèn)題是亟待解決的首要問(wèn)題和難點(diǎn)。路徑規(guī)劃指用算法為移動(dòng)機(jī)器人規(guī)劃一條從起點(diǎn)到終點(diǎn)的無(wú)障礙、付出代價(jià)最小的最優(yōu)或次優(yōu)路徑，路徑規(guī)劃的核心問(wèn)題是路徑規(guī)劃算法。按照適用場(chǎng)景的不同，可將路徑規(guī)劃算法分為靜態(tài)全局路徑規(guī)劃算法和動(dòng)態(tài)局部路徑規(guī)劃算法。全局路徑規(guī)劃算法主要用于解決已知的靜態(tài)環(huán)境中的移動(dòng)機(jī)器人路徑規(guī)劃問(wèn)題，普遍為傳統(tǒng)算法，如人工勢(shì)場(chǎng)法[1]、A*[2]、快速擴(kuò)展隨機(jī)樹(shù)[3]等；局部路徑規(guī)劃算法主要解決在動(dòng)態(tài)的全局未知或部分已知環(huán)境中的移動(dòng)機(jī)器人路徑規(guī)劃問(wèn)題，如動(dòng)態(tài)窗口算法[4]、遺傳算法[5]、蟻群算法[6]、神經(jīng)網(wǎng)絡(luò)算法[7]、粒子群算法[8]等。

A*算法雖然計(jì)算速度快、路徑短，是目前普遍采用的全局路徑規(guī)劃方法，但是其規(guī)劃的路徑存在拐點(diǎn)過(guò)多、節(jié)點(diǎn)冗余、與障礙物距離近等問(wèn)題。CAO等[9]提出Any-Angle A*算法，該算法基于可視圖，在起點(diǎn)到目標(biāo)點(diǎn)的連線附近進(jìn)行搜索，減少了搜索節(jié)點(diǎn)，縮短了計(jì)算時(shí)間，然而其進(jìn)行防撞處理時(shí)未考慮移動(dòng)機(jī)器人的自身體積；ZHANG等[10]提出一種改進(jìn)的A*算法對(duì)移動(dòng)機(jī)器人的路徑進(jìn)行優(yōu)化，該方法基于A*算法規(guī)劃得到初始路徑，然后遍歷初始路徑上的所有節(jié)點(diǎn)，刪除不必要的節(jié)點(diǎn)和連接，并采用關(guān)鍵點(diǎn)選擇策略進(jìn)行二次規(guī)劃，確定自動(dòng)導(dǎo)引小車(chē)(Automated Guided Vehicle, AGV)在拐點(diǎn)處的旋轉(zhuǎn)方向和旋轉(zhuǎn)角度，該方法能夠以更短的路徑和更短的轉(zhuǎn)彎時(shí)間提供更有效的路徑規(guī)劃;WANG等[11]在openlist中添加一個(gè)閾值N來(lái)減少搜索網(wǎng)格點(diǎn)的數(shù)量，并結(jié)合Floyd算法去除冗余節(jié)點(diǎn)，從而降低斷點(diǎn)的銳度，最終通過(guò)幾何優(yōu)化過(guò)程得到一條新的路徑，該方法提高了路徑的平滑度，減少了路徑長(zhǎng)度，更適用于機(jī)器人導(dǎo)航。

應(yīng)用動(dòng)態(tài)窗口法(Dynamic Window Approach, DWA)進(jìn)行路徑規(guī)劃雖然使移動(dòng)機(jī)器人具有很好的避障能力，而且路徑較為平滑，但是很容易陷入局部最優(yōu)解，不能沿著全局最優(yōu)路徑到達(dá)指定目標(biāo)。LI等[12]提出一種改進(jìn)的動(dòng)態(tài)窗口方法，該方法考慮移動(dòng)機(jī)器人自身尺寸與障礙物之間可行空間的關(guān)系，使算法可以求解局部極小問(wèn)題，提高了路徑的平滑度；EDUARDO等[13]在傳統(tǒng)動(dòng)態(tài)窗口算法的基礎(chǔ)上提出移動(dòng)障礙物動(dòng)態(tài)窗口法(Dynamic Window for Dynamic Obstacles, DW4DO)和移動(dòng)障礙物樹(shù)動(dòng)態(tài)窗口法(Dynamic Window for Dynamic Obstacles Tree, DW4DOT)來(lái)處理動(dòng)態(tài)障礙物，提高了算法的安全性和穩(wěn)定性；MISSURA等[14]在動(dòng)態(tài)窗口方法中加入一個(gè)動(dòng)態(tài)碰撞模型來(lái)預(yù)測(cè)未來(lái)與環(huán)境的碰撞，同時(shí)考慮到其他物體的運(yùn)動(dòng)，不僅使算法保持較高的計(jì)算效率，還減少了動(dòng)態(tài)環(huán)境下的碰撞次數(shù)。

由于移動(dòng)機(jī)器人路徑規(guī)劃的任務(wù)和場(chǎng)景越來(lái)越復(fù)雜，對(duì)路徑規(guī)劃算法的要求不斷提高，通常需要將全局路徑規(guī)劃與局部路徑規(guī)劃結(jié)合。程傳奇等[15]提出一種融合改進(jìn)A*算法和DWA的全局動(dòng)態(tài)路徑規(guī)劃方法，針對(duì)傳統(tǒng)A*算法設(shè)計(jì)了一種關(guān)鍵點(diǎn)選取策略，構(gòu)造了考慮全局最優(yōu)路徑的評(píng)價(jià)函數(shù)，再結(jié)合DWA進(jìn)行實(shí)時(shí)路徑規(guī)劃，使路徑更加平滑；ZHU等[16]提出基于評(píng)價(jià)函數(shù)的A*算法和DWA的融合算法，在保證移動(dòng)機(jī)器人局部避障能力的同時(shí)保持路徑全局最優(yōu)；王洪斌等[17]提出一種改進(jìn)A*算法與DWA相結(jié)合的混合算法，使移動(dòng)機(jī)器人能依次到達(dá)多個(gè)目標(biāo)點(diǎn)，還提出將改進(jìn)動(dòng)態(tài)窗口算法與全局路徑規(guī)劃信息相結(jié)合的在線路徑規(guī)劃法，使移動(dòng)機(jī)器人能夠在動(dòng)態(tài)復(fù)雜環(huán)境中局部避障并追擊動(dòng)態(tài)目標(biāo)點(diǎn)，提升了算法的運(yùn)行效率，有效縮短了路徑長(zhǎng)度并降低了轉(zhuǎn)折總度數(shù)。Hong等[18]提出一種結(jié)合模糊邏輯的改進(jìn)動(dòng)態(tài)窗口算法，該算法通過(guò)分析目標(biāo)和障礙物的信息，使用模糊邏輯實(shí)時(shí)確定合適的權(quán)重來(lái)實(shí)現(xiàn)，該方法使機(jī)器人的移動(dòng)更加安全、平穩(wěn);白建龍等[19]針對(duì)增強(qiáng)的蟻群算法易陷入局部最優(yōu)解的問(wèn)題，設(shè)計(jì)了具有負(fù)反饋機(jī)制的改進(jìn)的蟻群算法來(lái)解決機(jī)器人路徑規(guī)劃問(wèn)題；李鑫等[20]針對(duì)自動(dòng)導(dǎo)引小車(chē)在倉(cāng)儲(chǔ)物流搬運(yùn)系統(tǒng)中的路徑?jīng)_突問(wèn)題，提出一種基于時(shí)空沖突約束的A*算法。

上述改進(jìn)算法對(duì)傳統(tǒng)A*算法和DWA從不同側(cè)面進(jìn)行了改進(jìn)，但是未能完全解決所規(guī)劃路徑折點(diǎn)多、路徑冗余、路徑平滑度低及容易陷入局部最小等問(wèn)題。因此，本文對(duì)傳統(tǒng)A*算法和DWA進(jìn)行改進(jìn)，提出一種融合算法，利用A*算法進(jìn)行全局規(guī)劃后，采用關(guān)鍵折點(diǎn)提取策略剔除全局路徑的冗余拐點(diǎn)和冗余節(jié)點(diǎn)，再結(jié)合改進(jìn)DWA進(jìn)行局部路徑規(guī)劃，最終為移動(dòng)機(jī)器人規(guī)劃出路徑長(zhǎng)度最短、平滑度高的安全路徑。

1 算法基礎(chǔ)

1.1 傳統(tǒng)A*算法

A*算法是一種有效求解最短路徑的搜索方法，也是典型的啟發(fā)式算法，該方法從起點(diǎn)開(kāi)始對(duì)周?chē)付ǖ姆较蜻M(jìn)行搜索，并計(jì)算當(dāng)前節(jié)點(diǎn)周?chē)總€(gè)節(jié)點(diǎn)到起點(diǎn)的累積實(shí)際代價(jià)值和到終點(diǎn)的估計(jì)代價(jià)值，選擇總代價(jià)值最小的節(jié)點(diǎn)作為下一拓展節(jié)點(diǎn)，拓展到終點(diǎn)時(shí)停止搜索，完成路徑規(guī)劃。A*算法的啟發(fā)式函數(shù)為

f(p)=g(p)+h(p)。

(1)

式中：f(p)為當(dāng)前節(jié)點(diǎn)p的總代價(jià)值；g(p)為當(dāng)前狀態(tài)點(diǎn)p到起點(diǎn)s的實(shí)際代價(jià)值；h(p)為當(dāng)前狀態(tài)p到目標(biāo)點(diǎn)d的估計(jì)代價(jià)值。常見(jiàn)的計(jì)算代價(jià)的方法有曼哈頓距離、歐氏距離和切比雪夫距離，本文算法重點(diǎn)比較所規(guī)劃路徑的長(zhǎng)度，因此采用歐氏距離度量節(jié)點(diǎn)之間的代價(jià)，歐氏距離計(jì)算公式為

(2)

式中：(xs,ys)為起點(diǎn)s的坐標(biāo)；(xd,yd)為目標(biāo)點(diǎn)的坐標(biāo)。

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

DWA符合移動(dòng)機(jī)器人的運(yùn)動(dòng)特征、靈活性強(qiáng)，因此成為動(dòng)態(tài)環(huán)境下局部避障的主要算法。DWA的基本思想是結(jié)合移動(dòng)機(jī)器人的運(yùn)動(dòng)特性，在路徑規(guī)劃過(guò)程中實(shí)時(shí)預(yù)測(cè)時(shí)間t內(nèi)移動(dòng)機(jī)器人的速度空間(vt,wt)和狀態(tài)空間(xt,yt,yawt,vt,wt)，模擬移動(dòng)機(jī)器人在預(yù)測(cè)時(shí)間內(nèi)的運(yùn)動(dòng)軌跡，再根據(jù)評(píng)價(jià)函數(shù)確定最優(yōu)運(yùn)動(dòng)軌跡，直至到達(dá)目標(biāo)位置完成路徑規(guī)劃。假設(shè)移動(dòng)機(jī)器人在時(shí)間段(t2-t1)內(nèi)運(yùn)動(dòng)，則其位姿變化信息為：

xt2=xt1+vΔtcos(θΔt)；

yt2=yt1+vΔtsin(θΔt)；

θt2=θt1+wΔt。

(3)

式中：xt2和yt2分別為t2處移動(dòng)機(jī)器人的x，y坐標(biāo)位置，θt2為t2處移動(dòng)機(jī)器人的航向角；v為移動(dòng)機(jī)器人的線速度；w為移動(dòng)機(jī)器人的角速度；Δt為t1～t2的時(shí)間間隔。

對(duì)預(yù)測(cè)時(shí)間t內(nèi)的移動(dòng)機(jī)器人速度空間采樣后，采用評(píng)價(jià)函數(shù)評(píng)價(jià)預(yù)測(cè)軌跡。DWA的評(píng)價(jià)函數(shù)通常根據(jù)實(shí)際需要確定，一般情況下主要由目標(biāo)距離代價(jià)、速度代價(jià)、障礙物代價(jià)、方位角代價(jià)等代價(jià)函數(shù)組成，本文設(shè)定DWA的評(píng)價(jià)函數(shù)如式(4)所示，規(guī)定在路徑規(guī)劃過(guò)程中，總代價(jià)越小越優(yōu)。

C(v,w)=

αD(v,w)+βS(v,w)+λO(v,w)。

(4)

式中：C(v,w)為速度采樣空間的總代價(jià)；D(v,w)為速度采樣空間到目標(biāo)點(diǎn)位置的距離代價(jià)；S(v,w)為速度采樣的速度代價(jià)；O(v,w)為速度采樣空間到障礙物的距離代價(jià)；α，β，λ分別為目標(biāo)距離代價(jià)、速度代價(jià)、障礙物距離代價(jià)的單位代價(jià)增益。

各代價(jià)函數(shù)D(v,w),S(v,w),O(v,w)的具體函數(shù)如下：

S(v,w)=vmax-vt；

i=1,2,3,…,n。

(5)

式中：(xt,yt)為預(yù)測(cè)節(jié)點(diǎn)的(x,y)坐標(biāo)位置；(xd,yd)為目標(biāo)點(diǎn)的(x,y)坐標(biāo)位置；vmax為移動(dòng)機(jī)器人配置的最大速度；vt為預(yù)測(cè)速度空間的速度；(xoi,yoi)為第i個(gè)障礙物的(x,y)坐標(biāo)位置。

2 融合算法

傳統(tǒng)的A*算法應(yīng)用范圍廣、適應(yīng)性強(qiáng)，但是其規(guī)劃的路徑往往折點(diǎn)較多，路徑平滑度極低。DWA規(guī)劃的路徑平滑度高，能實(shí)現(xiàn)實(shí)時(shí)避障，但是極易陷入局部最優(yōu)，尋路成功率低。本文提出一種基于改進(jìn)A*算法和DWA的融合算法，充分利用兩種算法的優(yōu)勢(shì)，實(shí)現(xiàn)路徑長(zhǎng)度、平滑度和安全性的三重優(yōu)化。

2.1 環(huán)境模型描述

本文采用柵格法創(chuàng)建路徑規(guī)劃的環(huán)境地圖(如圖2)，將環(huán)境劃分為若干大小均等的柵格，根據(jù)實(shí)際環(huán)境將對(duì)應(yīng)柵格設(shè)置為空閑狀態(tài)和占據(jù)狀態(tài)，其中空閑柵格用白色表示，占據(jù)柵格用黑色表示。結(jié)合二維平面直角坐標(biāo)系，柵格橫軸用x坐標(biāo)表示，數(shù)值從左到右依次遞增，縱軸用y表示，數(shù)值從下到上依次遞增，柵格的具體位置用P(xi,yj)(i,j=1,2,3,…,n)表示。

假設(shè)移動(dòng)機(jī)器人路徑規(guī)劃的起點(diǎn)為S(1,1)，終點(diǎn)為D(14,13)，分別采用傳統(tǒng)A*算法和DWA在圖2所示的柵格地圖上進(jìn)行路徑規(guī)劃，得到的路徑如圖3所示。

由圖3a可見(jiàn)，傳統(tǒng)A*算法規(guī)劃的路徑存在較多冗余節(jié)點(diǎn)和路段，其轉(zhuǎn)折角度大，路徑未達(dá)到最短，且路徑曲率不連續(xù)，不符合移動(dòng)機(jī)器人的運(yùn)行規(guī)則。由圖3b可見(jiàn)，DWA規(guī)劃的路徑整體比較平滑，但是存在大量冗余路段，通過(guò)長(zhǎng)途繞路規(guī)劃的路徑并非最優(yōu)。

2.2 全局路徑優(yōu)化

針對(duì)A*算法規(guī)劃的路徑存在較多冗余節(jié)點(diǎn)和冗余路段等缺點(diǎn)，提出一種關(guān)鍵節(jié)點(diǎn)提取算法過(guò)濾A*算法所規(guī)劃路徑的冗余節(jié)點(diǎn)，保留路徑必經(jīng)的關(guān)鍵折點(diǎn)，使優(yōu)化后的路徑更短、折點(diǎn)更少，全局優(yōu)化的具體步驟如下：

(1)獲取A*算法所規(guī)劃路徑的全部節(jié)點(diǎn)集U{Pi,1≤i≤n}，其中P1表示規(guī)劃路徑的起點(diǎn)，Pn表示規(guī)劃路徑的終點(diǎn)。創(chuàng)建關(guān)鍵節(jié)點(diǎn)集V，用于存放算法優(yōu)化后的關(guān)鍵轉(zhuǎn)折點(diǎn)，集合初始值為V{P1,Pn}，即路徑起點(diǎn)P1和終點(diǎn)Pn。

(2)從P1開(kāi)始作直線，依次連接P3,P4,…,Pm，判斷直線P1Pm是否經(jīng)過(guò)障礙物，是則節(jié)點(diǎn)Pm-1為路徑的必經(jīng)節(jié)點(diǎn)，且必為關(guān)鍵轉(zhuǎn)折點(diǎn)，將節(jié)點(diǎn)Pm-1添加至集合V；否則，判定節(jié)點(diǎn)P2,…,Pm-1為冗余節(jié)點(diǎn)，從Pm繼續(xù)向后連接節(jié)點(diǎn)集U中的節(jié)點(diǎn)，直到連接到路徑的終點(diǎn)Pn，將所有關(guān)鍵節(jié)點(diǎn)均添加至集合V。關(guān)鍵節(jié)點(diǎn)選取完成后，集合V{P1,Pm-1,…,Pm+k,Pn}中就包含了所有關(guān)鍵節(jié)點(diǎn)，設(shè)該關(guān)鍵節(jié)點(diǎn)數(shù)為m。

(3)依次連接集合V中的所有節(jié)點(diǎn)，完成對(duì)路徑的全局優(yōu)化，具體步驟如圖4所示。

比較圖3a和圖4c可知，全局優(yōu)化后的路徑長(zhǎng)度從20.90縮短到18.91，優(yōu)化了9.52%；折點(diǎn)從17個(gè)減少到5個(gè)，優(yōu)化了70.59%；路徑的累積轉(zhuǎn)彎角度從270°降低到140.03°，優(yōu)化了48.14%。綜上所述，利用關(guān)鍵節(jié)點(diǎn)提取算法對(duì)路徑進(jìn)行全局優(yōu)化后實(shí)現(xiàn)了對(duì)路徑長(zhǎng)度和路段冗余的優(yōu)化，然而路徑仍然存在平滑度低、與障礙物距離過(guò)近等缺點(diǎn)。

2.3 局部路徑優(yōu)化

針對(duì)DWA易陷入局部最優(yōu)和成功率低等缺點(diǎn)，以及全局優(yōu)化后路徑仍然存在平滑度低等問(wèn)題，繼續(xù)對(duì)路徑進(jìn)行局部?jī)?yōu)化。用2.2節(jié)全局優(yōu)化得到的關(guān)鍵節(jié)點(diǎn)對(duì)路徑進(jìn)行分段，再用改進(jìn)DWA進(jìn)行局部路徑優(yōu)化，改善傳統(tǒng)DWA容易陷入局部最優(yōu)的問(wèn)題，并提高整體路徑的平滑度和安全性。

由DWA的原理可知，路徑的影響因素主要是移動(dòng)機(jī)器人的航向角和角速度，因?yàn)榻撬俣韧ǔ楣潭ㄅ渲弥?，所以影響路徑平滑程度和路徑長(zhǎng)度的主要因素是航向角。航向角偏差越大，所需的轉(zhuǎn)彎距離越長(zhǎng)，越容易造成路徑冗余，因此本文主要對(duì)航向角進(jìn)行改進(jìn)和優(yōu)化，具體步驟如下：

(1)從起點(diǎn)P1開(kāi)始，依次將關(guān)鍵節(jié)點(diǎn)集V{P1,Pm-1,…,Pm+k,Pn}中除終點(diǎn)Pn外的所有節(jié)點(diǎn)作為局部路徑規(guī)劃的起點(diǎn)S1,S2,…,Sm-1；從第2個(gè)節(jié)點(diǎn)Pm-1開(kāi)始，依次將集合V{P1,Pm-1,…,Pm+k,Pn}中的所有節(jié)點(diǎn)作為局部路徑規(guī)劃的終點(diǎn)D1,D2,…,Dm-1，將全局路徑分為S1D1,S2D2,…,Sm-1Dm-1共m-1段路。

(2)計(jì)算路徑S1D1,即原線段P1Pm-1的傾斜角度

(6)

式中：xS1,yS1,xD1,yD1分別為第1段路徑起點(diǎn)S1和終點(diǎn)D1對(duì)應(yīng)的x軸、y軸坐標(biāo)值。

(3)將路徑的傾斜角度轉(zhuǎn)換為弧度值，作為移動(dòng)機(jī)器人的初始航向角。傾斜角度轉(zhuǎn)化弧度值的公式為

yaw=α×180°/π。

(7)

(4)初始化移動(dòng)機(jī)器人的狀態(tài)參數(shù)集L{l}，其中l(wèi)(x,y,yaw,v,w)記錄移動(dòng)機(jī)器人路徑規(guī)劃過(guò)程中的狀態(tài)參數(shù)，包括位置、航向角、線速度、角速度。

(5)按照實(shí)際環(huán)境需要，設(shè)定移動(dòng)機(jī)器人的初始線速度v(單位：m/s)、初始角速度w(單位：rad/s)，結(jié)合前3步得到起點(diǎn)S1(單位：m)、終點(diǎn)D1(單位：m)、初始航向角yaw(單位：rad)，綜合得到l1(x1,y1,yaw1,v1,w1)。

(6)采用DWA進(jìn)行局部路徑規(guī)劃，并將機(jī)器人的所有狀態(tài)參數(shù)li更新到到狀態(tài)參數(shù)集L中，記錄路徑的所有節(jié)點(diǎn)和機(jī)器人的位姿信息，每段路徑完成后狀態(tài)參數(shù)集變成L{l1,l2,…,li}，li(xi,yi,yawi,vi,wi)表示移動(dòng)機(jī)器人的最新?tīng)顟B(tài)參數(shù)。

(7)按照式(2)計(jì)算下一段路徑的傾斜角度α，同時(shí)將機(jī)器人上一狀態(tài)參數(shù)l中的yaw轉(zhuǎn)換為角度值β，弧度值轉(zhuǎn)化為角度值的公式為

β=yaw×π/180°。

(8)

(8)重復(fù)步驟(7)，直至移動(dòng)機(jī)器人到達(dá)第m-1路段的終點(diǎn)Dm-1，得到記錄移動(dòng)機(jī)器人的所有狀態(tài)參數(shù)集L{l1,l2,…,li,li+1,…,ln}，完成局部路徑優(yōu)化。

路徑局部?jī)?yōu)化步驟如圖5所示。

比較傳統(tǒng)DWA和局部?jī)?yōu)化后的路徑圖以及兩條路徑的相關(guān)數(shù)據(jù)可知，路徑長(zhǎng)度從26.81縮短到20.25，優(yōu)化了24.47%，改善了傳統(tǒng)算法易陷入局部最優(yōu)、路段冗余等問(wèn)題，而且基本實(shí)現(xiàn)路徑最短。綜上所述，改進(jìn)A*算法和改進(jìn)DWA的融合算法，一方面成功改善了傳統(tǒng)A*算法所規(guī)劃的路徑平滑度和安全性低、不符合移動(dòng)機(jī)器人運(yùn)動(dòng)特征等問(wèn)題，另一方面成功改善了動(dòng)態(tài)窗口算法規(guī)劃路徑時(shí)易陷入局部最優(yōu)、路徑過(guò)長(zhǎng)等缺點(diǎn)，所規(guī)劃的路徑平滑度、安全性、長(zhǎng)度3方面均得到較大程度的改善與優(yōu)化。

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

為了驗(yàn)證本文所提融合算法的有效性，采用傳統(tǒng)A*算法、DWA和本文融合算法進(jìn)行了多組仿真對(duì)比實(shí)驗(yàn)。

3.1 對(duì)比仿真實(shí)驗(yàn)

在文獻(xiàn)[15]中規(guī)模為20×20的柵格地圖上，采用3種算法對(duì)移動(dòng)機(jī)器人的路徑規(guī)劃進(jìn)行仿真，為保持良好的對(duì)比效果，在實(shí)驗(yàn)過(guò)程中，移動(dòng)機(jī)器人的最大線速度、最大角速度、最大線加速度、最大角加速度、線速度分辨率、角速度分辨率、時(shí)間分辨率、預(yù)測(cè)周期等參數(shù)均與文獻(xiàn)[15]所述的仿真參數(shù)相同，分別為1 m/s，20°/s，0.2 m/s2，50°/s2，0.01 m/s，1°/s，0.1 s，3 s，路徑規(guī)劃的起點(diǎn)與終點(diǎn)也相同，分別為S(2.5,1.5)，D(11.5,19.5)。實(shí)驗(yàn)環(huán)境為運(yùn)行內(nèi)存8 GB的64位WIN 10操作系統(tǒng)，實(shí)驗(yàn)平臺(tái)為Python 3.7。

采用傳統(tǒng)A*算法進(jìn)行路徑規(guī)劃得到的路徑如圖6a所示，此時(shí)路徑的長(zhǎng)度為22.899 5，共有21個(gè)節(jié)點(diǎn)，轉(zhuǎn)彎次數(shù)為4，累積轉(zhuǎn)彎角度為180°。對(duì)路徑進(jìn)行全局優(yōu)化得到的路徑如圖6b所示，此時(shí)路徑的長(zhǎng)度為20.208 135，共有3個(gè)節(jié)點(diǎn)，轉(zhuǎn)彎次數(shù)為3，累積轉(zhuǎn)彎角度為63.43°。采用傳統(tǒng)DWA進(jìn)行路徑規(guī)劃，算法迭代2 000次后得到的路徑如圖6c所示，此時(shí)算法陷入局部最優(yōu)，尋路失敗。本文改進(jìn)融合算法規(guī)劃的路徑如圖6d所示，此時(shí)路徑長(zhǎng)度為21.579 0。

由圖6a可見(jiàn)，傳統(tǒng)A*算法規(guī)劃的全局路徑存在較多轉(zhuǎn)折點(diǎn)和冗余路段，路徑曲率不連續(xù)，且路徑上有些節(jié)點(diǎn)與障礙物距離過(guò)近，安全性較低。由圖6d可見(jiàn)，融合算法規(guī)劃的路徑轉(zhuǎn)折明顯減少，路徑曲率連續(xù)，平滑度高，且路徑各節(jié)點(diǎn)都與障礙物保持適當(dāng)?shù)陌踩嚯x。對(duì)比傳統(tǒng)A*算法和融合算法的仿真數(shù)據(jù)可得，路徑長(zhǎng)度優(yōu)化了5.77%，路徑平滑度優(yōu)化了64.76%，路徑安全性優(yōu)化了100%，融合算法同時(shí)實(shí)現(xiàn)了路徑長(zhǎng)度、路徑平滑度和路徑安全性的三重優(yōu)化。

3.2 實(shí)際環(huán)境仿真實(shí)驗(yàn)

為驗(yàn)證本文融合算法在實(shí)際應(yīng)用中的有效性，在機(jī)器人操作系統(tǒng)(Robot Operating System, ROS)中建立16×16×3的實(shí)際仿真環(huán)境，如圖7所示。采用即時(shí)定位與地圖構(gòu)建(Simultaneous Localization and Mapping, SLAM)掃描仿真環(huán)境建立相應(yīng)的地圖，如圖8所示。實(shí)驗(yàn)環(huán)境為運(yùn)行內(nèi)存為4 GB的64位Ubuntu 16.04操作系統(tǒng)，實(shí)驗(yàn)平臺(tái)為ROS(Kinetic)，實(shí)驗(yàn)引入的移動(dòng)機(jī)器人為T(mén)urtlebot3-Burger。路徑規(guī)劃實(shí)驗(yàn)為從起點(diǎn)位置(1,1)到目標(biāo)點(diǎn)(14,2)處獲取物品。實(shí)驗(yàn)中Burger的參數(shù)設(shè)置與文獻(xiàn)[15]相同。

采用ROS系統(tǒng)默認(rèn)的導(dǎo)航算法和本文所提融合算法在環(huán)境中進(jìn)行仿真實(shí)驗(yàn)，得到Burger的路徑如圖9和圖10所示。

綜上可知，采用ROS默認(rèn)的導(dǎo)航算法規(guī)劃的路徑比較平滑且曲率連續(xù)，符合移動(dòng)機(jī)器人的運(yùn)動(dòng)規(guī)則，然而該路徑有較多轉(zhuǎn)折路段，且轉(zhuǎn)折角度較大，導(dǎo)致存在多段冗余路徑。采用本文所提融合算法成功規(guī)劃出了無(wú)障礙路徑，而且在保證路徑平滑程度的基礎(chǔ)上減少了路徑轉(zhuǎn)折和冗余路段，實(shí)現(xiàn)了對(duì)路徑平滑度和長(zhǎng)度的雙重優(yōu)化。

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

針對(duì)傳統(tǒng)A*算法所規(guī)劃路徑平滑度和安全性低、DWA所規(guī)劃路徑易陷入局部最優(yōu)等問(wèn)題，本文提出一種綜合采用改進(jìn)A*算法和DWA的融合算法，其結(jié)合兩種算法優(yōu)勢(shì)，避免了路徑規(guī)劃中的缺陷。最后通過(guò)傳統(tǒng)A*算法和DWA的仿真對(duì)比實(shí)驗(yàn)，證明本文改進(jìn)融合算法實(shí)現(xiàn)了對(duì)路徑長(zhǎng)度、平滑性和安全性的優(yōu)化；同時(shí)結(jié)合ROS的實(shí)際環(huán)境仿真驗(yàn)證了本文改進(jìn)融合算法能高效完成實(shí)際環(huán)境的路徑規(guī)劃任務(wù)，優(yōu)化路徑長(zhǎng)度和平滑度。未來(lái)將在多種應(yīng)用場(chǎng)景中研究多移動(dòng)機(jī)器人的路徑規(guī)劃與協(xié)調(diào)算法，推動(dòng)移動(dòng)機(jī)器人的算法研究和實(shí)踐應(yīng)用。