融合改進(jìn)A*算法與DWA算法的機(jī)器人實(shí)時路徑規(guī)劃

張 振，張華良，鄧永勝，白士宇

((1.東北大學(xué) 機(jī)械工程與自動化學(xué)院，遼寧 沈陽 110819；2.中國科學(xué)院沈陽自動化研究所 工業(yè)控制網(wǎng)絡(luò)與系統(tǒng)研究室，遼寧 沈陽 110015；3.中國科學(xué)院機(jī)器人與智能制造創(chuàng)新研究院，遼寧 沈陽 110000；4.沈陽理工大學(xué) 自動化與電氣工程學(xué)院，遼寧 沈陽 110168)

0 引言

路徑規(guī)劃是移動機(jī)器人完成導(dǎo)航功能的基本環(huán)節(jié)之一[1-2]，根據(jù)對環(huán)境信息掌握程度的不同分為全局路徑規(guī)劃和局部路徑規(guī)劃[3-4]。應(yīng)用于全局路徑規(guī)劃的算法有：Dijkstra算法[5]、A*算法[6-7]和快速擴(kuò)展隨機(jī)樹(Rapidly-exploring Random Trees，RRT)算法[8]等；應(yīng)用于局部路徑規(guī)劃的算法有：TEB(Time Elastic Band)算法[9]、人工勢場法[10]和動態(tài)窗口(Dynamic Window Approach，DWA)算法[11-12]等。

A*算法是應(yīng)用較廣泛的一種全局路徑規(guī)劃方法，已經(jīng)有許多學(xué)者對傳統(tǒng)A*算法進(jìn)行改進(jìn)。為提高搜索效率，文獻(xiàn)[13]將A*算法與跳點(diǎn)搜索算法結(jié)合，將篩選出的跳點(diǎn)添加至Open集與Close集中，減少尋路拓展節(jié)點(diǎn)。文獻(xiàn)[14]以指數(shù)衰減的方式對估計(jì)函數(shù)加權(quán)，并對優(yōu)化后的路徑進(jìn)行5次多項(xiàng)式平滑處理得到光滑曲線。為提高路徑安全性，文獻(xiàn)[15]通過引入鄰域矩陣進(jìn)行障礙搜索，有效提升了路徑安全性；文獻(xiàn)[16]通過在障礙物邊緣設(shè)置“緩沖區(qū)”使機(jī)器人遠(yuǎn)離障礙物。

以上研究都是在對全局環(huán)境信息已知的情況下進(jìn)行尋路，無法使機(jī)器人規(guī)避未知障礙物，而DWA算法具有良好的局部避障能力，文獻(xiàn)[17]根據(jù)障礙物稠密程度自適應(yīng)調(diào)整目標(biāo)函數(shù)權(quán)值，兼顧路徑的平滑性與安全性；文獻(xiàn)[18]提出了一種較優(yōu)方位角范圍約束,通過軌跡評價(jià)函數(shù)確定最優(yōu)軌跡對應(yīng)的速度。然而，僅應(yīng)用DWA算法進(jìn)行路徑規(guī)劃往往會陷入局部最優(yōu)的困境，將A*算法與DWA算法融合進(jìn)行混合路徑規(guī)劃可以獲得較好的避障效果[19-20]。

綜上所述，本文提出了一種改進(jìn)A*算法與DWA算法相融合的實(shí)時路徑規(guī)劃算法。該算法的主要創(chuàng)新點(diǎn)有：優(yōu)化A*算法評價(jià)函數(shù)，加入周圍環(huán)境及父節(jié)點(diǎn)的影響，以指數(shù)加權(quán)的方式提高尋路效率；增加節(jié)點(diǎn)安全擴(kuò)展策略保證路徑安全性；使用關(guān)鍵點(diǎn)提取策略剔除冗余節(jié)點(diǎn)；將提取的關(guān)鍵點(diǎn)信息加入DWA算法的評價(jià)函數(shù)中，實(shí)現(xiàn)算法融合。

1 A*算法描述與改進(jìn)

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

A*算法是一種常見的路徑查找和圖形遍歷算法，是Dijkstra算法的擴(kuò)展。相比于Dijkstra算法，A*算法增加了一種啟發(fā)函數(shù)的引導(dǎo)，從而使機(jī)器人的尋路過程具有方向性，極大地提升了算法效率[21]。用傳統(tǒng)A*算法評價(jià)函數(shù)來計(jì)算遍歷節(jié)點(diǎn)的優(yōu)先級，評價(jià)函數(shù)核心表達(dá)式為：

F(n)=G(n)+H(n)，

(1)

式中,F(n)表示由起始點(diǎn)S至目標(biāo)點(diǎn)G的估計(jì)代價(jià)值；G(n)表示由起始點(diǎn)S至當(dāng)前節(jié)點(diǎn)C的實(shí)際代價(jià)值；H(n)表示由當(dāng)前節(jié)點(diǎn)C至目標(biāo)點(diǎn)G的估代價(jià)值，也稱作啟發(fā)式搜索函數(shù)。常用的啟發(fā)函數(shù)有曼哈頓距離、切比雪夫距離和歐式距離等，其中歐式距離應(yīng)用較廣泛，表達(dá)式為：

(2)

A*算法使用評價(jià)函數(shù)F(n)來計(jì)算上一節(jié)點(diǎn)擴(kuò)展至當(dāng)前節(jié)點(diǎn)C的代價(jià)值，并從中選取F(n)最小(優(yōu)先級最高)的節(jié)點(diǎn)作為下一個待遍歷的節(jié)點(diǎn)。

傳統(tǒng)A*算法的不足主要表現(xiàn)為：

① 重復(fù)搜索節(jié)點(diǎn)，使尋路效率下降；

② 啟發(fā)函數(shù)H(n)代價(jià)值小于實(shí)際值，導(dǎo)致搜索空間變大、路徑冗余；

③ 將機(jī)器人簡化為質(zhì)點(diǎn)，導(dǎo)致機(jī)器人與障礙物距離過近，甚至斜穿障礙物；

④ 路徑拐點(diǎn)較多，對機(jī)器人的電機(jī)不利。

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

1.2.1 評價(jià)函數(shù)優(yōu)化

(1)在評價(jià)函數(shù)F(n)的啟發(fā)項(xiàng)中加入其父節(jié)點(diǎn)對當(dāng)前節(jié)點(diǎn)擴(kuò)展的影響，有效減少了節(jié)點(diǎn)的往返搜索現(xiàn)象，將優(yōu)化后的啟發(fā)函數(shù)記為H(n)′，具體形式為：

(3)

式中，H(p)為當(dāng)前節(jié)點(diǎn)C的父節(jié)點(diǎn)P到目標(biāo)點(diǎn)G的距離；Px，Py分別為父節(jié)點(diǎn)P的橫縱坐標(biāo)值。

(2)引入環(huán)境障礙物率K，量化環(huán)境中的障礙物稠密程度，表達(dá)式如式(4)所示，根據(jù)環(huán)境障礙物率K對優(yōu)化后的啟發(fā)函數(shù)H(n)′進(jìn)行指數(shù)加權(quán)，實(shí)現(xiàn)啟發(fā)項(xiàng)的快速自適應(yīng)調(diào)整，提高機(jī)器人在柵格地圖中不同位置的尋路效率和靈活度。

(4)

式中，N表示由當(dāng)前節(jié)點(diǎn)C到目標(biāo)點(diǎn)G為頂點(diǎn)所組成的矩形區(qū)域中障礙物的數(shù)量。評價(jià)函數(shù)F(n)優(yōu)化為：

F(n)=G(n)+exp[H(n)/K]*(H(p)+H(n))。

(5)

由式(5)可知，當(dāng)前節(jié)點(diǎn)離目標(biāo)點(diǎn)較遠(yuǎn)或環(huán)境中障礙物較少時，評價(jià)函數(shù)中的啟發(fā)項(xiàng)權(quán)重變大，機(jī)器人尋路效率提高，遍歷節(jié)點(diǎn)減少；同理，當(dāng)前節(jié)點(diǎn)離目標(biāo)點(diǎn)較近或環(huán)境中障礙物較多時，機(jī)器人降低尋路速度，保證目標(biāo)點(diǎn)可達(dá)。

1.2.2 安全擴(kuò)展策略

采用八方向的搜索方式進(jìn)行節(jié)點(diǎn)擴(kuò)展，具體如圖1(a)所示，在節(jié)點(diǎn)擴(kuò)展時進(jìn)行安全檢測，根據(jù)父節(jié)點(diǎn)周圍的環(huán)境信息自適應(yīng)地調(diào)整擴(kuò)展方向。安全擴(kuò)展策略的具體步驟為：

步驟①：以父節(jié)點(diǎn)與待擴(kuò)展的子節(jié)點(diǎn)為頂點(diǎn)構(gòu)建矩形柵格區(qū)域，如圖1(b)紅色矩形框所示；

(a)節(jié)點(diǎn)八方向擴(kuò)展

步驟②：檢測矩形柵格區(qū)域內(nèi)除父節(jié)點(diǎn)與子節(jié)點(diǎn)外的柵格是否是障礙物區(qū)域；

步驟③：若存在障礙物區(qū)域，則將此子節(jié)點(diǎn)的評價(jià)函數(shù)F(n)設(shè)為無窮大，A*算法就會轉(zhuǎn)而擴(kuò)展其他評價(jià)函數(shù)值較小的子節(jié)點(diǎn)，使機(jī)器人遠(yuǎn)離障礙物；

步驟④：若不存在障礙物區(qū)域，則對此子節(jié)點(diǎn)進(jìn)行正常擴(kuò)展。

1.2.3 關(guān)鍵點(diǎn)提取策略

提出關(guān)鍵點(diǎn)提取策略減少路徑節(jié)點(diǎn)，重新分配拐點(diǎn)來減少路徑長度及轉(zhuǎn)折角度；引入安全閾值d來評價(jià)優(yōu)化后路線的安全性，安全閾值d即該段路線與周邊相鄰障礙物頂點(diǎn)之間的距離。

關(guān)鍵點(diǎn)提取策略如圖2所示，圖2中由節(jié)點(diǎn)S，1～7，G所組成的路徑為優(yōu)化前的路徑，可以看出這條路徑均經(jīng)過柵格中心點(diǎn)，拐點(diǎn)較多。下面對這條路線進(jìn)行平滑化處理，并重新提取關(guān)鍵點(diǎn)：

步驟①：由起始點(diǎn)S遍歷所有節(jié)點(diǎn)，直到遍歷至終點(diǎn)G結(jié)束。檢測由起始點(diǎn)S到節(jié)點(diǎn)1所組成的矩形區(qū)域內(nèi)是否存在柵格障礙物，不存在障礙物則舍棄此節(jié)點(diǎn)。依次進(jìn)行起始點(diǎn)S至節(jié)點(diǎn)2及以下節(jié)點(diǎn)矩形區(qū)域的障礙檢測。當(dāng)檢測至節(jié)點(diǎn)5時，發(fā)現(xiàn)矩形區(qū)域內(nèi)出現(xiàn)障礙物，則計(jì)算矩形區(qū)域內(nèi)障礙物頂點(diǎn)相距路線S5的最小距離d5,若d5≥d則舍棄節(jié)點(diǎn)5，繼續(xù)進(jìn)行節(jié)點(diǎn)6及以下節(jié)點(diǎn)矩形區(qū)域的障礙檢測；若d5

圖2 關(guān)鍵點(diǎn)提取策略

步驟②：對初步優(yōu)化后的關(guān)鍵點(diǎn)進(jìn)行共線檢測，若存在共線點(diǎn)，則保留首尾節(jié)點(diǎn)，剔除不必要的中間點(diǎn)，對關(guān)鍵點(diǎn)進(jìn)行二次優(yōu)化，進(jìn)一步減少節(jié)點(diǎn)數(shù)。

步驟③：提取剩余的關(guān)鍵點(diǎn)，輸出二次優(yōu)化后的路徑，算法結(jié)束。

2 DWA算法優(yōu)化

2.1 機(jī)器人航跡推算

對本文所使用四輪全向機(jī)器人建立運(yùn)動學(xué)模型，具體如圖3所示。

圖3 全向移動機(jī)器人運(yùn)動學(xué)模型

圖中，vx,vy分別為四輪全向機(jī)器人的水平方向及豎直方向的速度；ω為移動機(jī)器人繞其中心Or運(yùn)動的角速度；θt為機(jī)器人的航向角。設(shè)移動機(jī)器人在相鄰時刻之間勻速運(yùn)動，而時間間隔Δt很短，所以將移動機(jī)器人從t時刻到t+Δt時刻的運(yùn)動近似為一條直線，從而可以得出機(jī)器人的航跡推算公式：

(6)

2.2 速度矢量空間采樣

由式(6)可知，對于當(dāng)前時刻機(jī)器人的位姿，都可以通過每一個速度矢量(vx,vy,ω)推算出采樣周期內(nèi)的位移位姿及其運(yùn)動軌跡。因此，DWA算法只需要對機(jī)器人的速度矢量空間進(jìn)行采樣，然后評價(jià)產(chǎn)生軌跡的好壞即可確定最佳軌跡。在實(shí)際環(huán)境中，速度矢量空間采樣會受到機(jī)器人本身性能及環(huán)境的限制，具體約束表現(xiàn)為：

(1)由于機(jī)器人硬件性能對機(jī)器人最小和最大速度都有限制，所以移動機(jī)器人速度約束表示為：

Vm={vx∈[vmin,vmax],vy∈[vmin,vmax],

w∈[wmin,wmax]}，

(7)

式中，vmax,vmin分別為最大、最小線速度限制；ωmax,ωmin為分別為最大、最小角速度限制。

(2)由于機(jī)器人電機(jī)性能的約束，機(jī)器人應(yīng)保證在電機(jī)力矩可承受范圍內(nèi)進(jìn)行速度矢量空間采樣，即機(jī)器人電機(jī)加速度約束：

(8)

式中，axmin,axmax分別為移動機(jī)器人x軸方向的最大減加速度和最大加速度；aymin,aymax分別為移動機(jī)器人y軸方向的最大減加速度和最大加速度；aωmin,aωmax分別為移動機(jī)器人角速度的最大減加速度和最大加速度。

(3)當(dāng)機(jī)器人遇到動態(tài)障礙物時，應(yīng)該保證機(jī)器人與障礙物之間保持安全距離，即移動機(jī)器人安全距離約束：

(9)

式中，dist(vx,vy,ω)為機(jī)器人軌跡距離障礙物的最小距離。

同時滿足以上3個約束即可作為采樣空間內(nèi)的速度Vr，具體形式如下：

Vr∈Vm∩Vd∩Va。

(10)

2.3 評價(jià)函數(shù)優(yōu)化

在進(jìn)行速度采樣后會得到多組可行的機(jī)器人軌跡，因此需要采用評價(jià)函數(shù)找出對于機(jī)器人最可行的軌跡，傳統(tǒng)DWA算法的評價(jià)函數(shù)為：

G(vx,vy,ω)=σ[αhead(vx,vy,ω)+βdist(vx,vy,ω)+

γvel(vx,vy,ω)]，

(11)

式中，head(vx,vy,ω)為軌跡末端點(diǎn)與目標(biāo)點(diǎn)的方位角偏差；vel(vx,vy,ω)為此軌跡對應(yīng)的線速度值；α，β，γ為夾角、距離及速度各子函數(shù)的加權(quán)系數(shù)；σ為對此3項(xiàng)子函數(shù)的歸一化參數(shù)。

由式(11)可知，傳統(tǒng)DWA算法只單純設(shè)置一個目標(biāo)點(diǎn)對機(jī)器人進(jìn)行引導(dǎo)，會導(dǎo)致機(jī)器人在障礙物較多或路線較長時陷入局部最優(yōu)。因此將使用改進(jìn)A*算法優(yōu)化后的關(guān)鍵點(diǎn)對機(jī)器人進(jìn)行局部引導(dǎo)，計(jì)算各個采樣速度所對應(yīng)的模擬軌跡末端點(diǎn)與引導(dǎo)點(diǎn)的距離，將其作為引導(dǎo)點(diǎn)評價(jià)子函數(shù)guid(vx,vy,ω)加入對模擬軌跡的評價(jià)函數(shù)中；此外，由于四輪全向機(jī)器人可以全向運(yùn)動，所以不需要考慮軌跡末端點(diǎn)與目標(biāo)點(diǎn)的方位角偏差head(vx,vy,ω)，經(jīng)過優(yōu)化后的評價(jià)函數(shù)為：

G′(vx,vy,ω)=σ[αguid(vx,vy,ω)+βdist(vx,vy,ω)+

γvel(vx,vy,ω)]，

(12)

式中，guid(vx,vy,ω)為模擬軌跡末端點(diǎn)到引導(dǎo)點(diǎn)的距離；α,β,γ分別為3項(xiàng)子函數(shù)的加權(quán)系數(shù)；σ為對此3項(xiàng)子函數(shù)的歸一化參數(shù)。

3 融合算法路徑規(guī)劃

由于全局和局部路徑規(guī)劃都有各自的缺陷，A*算法能獲取全局范圍內(nèi)的最佳路徑卻無法躲避動態(tài)障礙物，DWA算法能躲避動態(tài)障礙物卻容易陷入局部最優(yōu)。本文將上述2種方法融合，提取改進(jìn)后A*算法的全局關(guān)鍵點(diǎn)作為改進(jìn)DWA算法的引導(dǎo)點(diǎn)，通過融合算法產(chǎn)生的路徑能在保證全局最優(yōu)的前提下實(shí)時躲避動態(tài)障礙物，算法流程如圖4所示。

圖4 融合算法流程

4 仿真分析

為了驗(yàn)證融合算法的有效性，在Pycharm中進(jìn)行仿真實(shí)驗(yàn)，所用電腦配置為AMD A6-6310 APU @1.8 GHz，搭建了20×20及40×40的環(huán)境柵格地圖，黑色部分代表已知障礙物，灰色部分代表未知障礙物，白色部分代表可通行區(qū)域，綠色部分代表訪問節(jié)點(diǎn)區(qū)域，●代表位于左上角的起始點(diǎn)S，▲代表位于右下角的目標(biāo)點(diǎn)G。

4.1 改進(jìn)A*算法仿真對比實(shí)驗(yàn)

改進(jìn)A*算法對評價(jià)函數(shù)進(jìn)行優(yōu)化，設(shè)置安全閾值d=0.5，實(shí)驗(yàn)所用地圖為20×20柵格地圖，分別使用傳統(tǒng)A*算法、文獻(xiàn)[14]改進(jìn)A*算法、文獻(xiàn)[15]改進(jìn)A*算法及本文改進(jìn)A*算法進(jìn)行對比實(shí)驗(yàn)，算法性能指標(biāo)對比如表1所示,仿真結(jié)果如圖5所示。

表1 簡單情況下路徑規(guī)劃性能指標(biāo)對比

(a)傳統(tǒng)A*

為驗(yàn)證本文改進(jìn)A*算法的可靠性，在40×40隨機(jī)柵格地圖對傳統(tǒng)A*算法與本文改進(jìn)的A*算法進(jìn)行仿真實(shí)驗(yàn)，實(shí)驗(yàn)結(jié)果如圖6所示，算法性能指標(biāo)對比如表2所示。

(a)傳統(tǒng)A*

表2 復(fù)雜情況下路徑規(guī)劃性能指標(biāo)對比

由上述實(shí)驗(yàn)結(jié)果可知，在20×20的簡單柵格環(huán)境地圖中，本文改進(jìn)A*算法的尋路時間相比于傳統(tǒng)A*算法降低了81%，相比于文獻(xiàn)[15]改進(jìn)A*算法下降了84%；同時，本文改進(jìn)A*算法的安全性相比于傳統(tǒng)A*算法和文獻(xiàn)[14]改進(jìn)A*算法有大幅提高，使機(jī)器人在轉(zhuǎn)彎時與障礙物保持安全距離；而且本文改進(jìn)A*算法在保證路徑較短的同時，最大程度減小了轉(zhuǎn)折角度和次數(shù)，使規(guī)劃路線更加平滑。在40×40的復(fù)雜隨機(jī)柵格環(huán)境地圖中，由于地圖環(huán)境較復(fù)雜且同時要考慮路徑的安全性，導(dǎo)致本文改進(jìn)A*算法較傳統(tǒng)A*算法的路徑長度及轉(zhuǎn)折角度略有上升，但本文改進(jìn)A*算法規(guī)劃的路徑距離障礙物的平均距離更遠(yuǎn)，也不會出現(xiàn)斜穿障礙物的情況，而且相比于傳統(tǒng)A*算法的尋路時間降低了89%，從而保證機(jī)器人可以安全、快速地避開環(huán)境內(nèi)的已知障礙物。

4.2 融合導(dǎo)航算法仿真實(shí)驗(yàn)

實(shí)驗(yàn)所用全向移動機(jī)器人運(yùn)動學(xué)參數(shù)如表3所示。

表3 全向移動機(jī)器人運(yùn)動學(xué)參數(shù)

為驗(yàn)證機(jī)器人對于未知障礙物的動態(tài)避障能力，使用20×20柵格地圖作為實(shí)驗(yàn)環(huán)境，在改進(jìn)A*算法規(guī)劃的路線上添加3個突然出現(xiàn)的障礙物，融合算法改進(jìn)后的評價(jià)函數(shù)各項(xiàng)系數(shù)分別為：α=0.15,β=0.4,γ=0.4，融合算法的路徑仿真結(jié)果如圖7所示，在圖中以灰色柵格表示突然出現(xiàn)的障礙物，*代表由改進(jìn)A*算法所得到全局關(guān)鍵點(diǎn)，藍(lán)色虛線表示由改進(jìn)A*算法得到的路徑，紅色實(shí)線表示由融合算法得到的路徑。

由圖7可知，當(dāng)環(huán)境中不存在未知障礙物時，改進(jìn)A*算法和融合算法均能順著關(guān)鍵點(diǎn)的指引到達(dá)目標(biāo)點(diǎn)，具體如圖7(a)所示;此時在機(jī)器人的行進(jìn)路線上分別添加1～3個未知障礙物，分別如圖7(b)～(d)所示，可以看出改進(jìn)A*算法所規(guī)劃的路徑并沒有規(guī)避未知障礙物，而融合導(dǎo)航算法在全局路徑的引導(dǎo)下對未知障礙物進(jìn)行合理規(guī)避，并與障礙物之間始終保持安全距離。由上述仿真結(jié)果可知，融合導(dǎo)航算法能在保證全局最優(yōu)的基礎(chǔ)上，安全、快速地實(shí)現(xiàn)動態(tài)避障功能。

(a)無未知障礙物

5 實(shí)驗(yàn)驗(yàn)證

為驗(yàn)證融合導(dǎo)航算法在真實(shí)環(huán)境中的動態(tài)避障效果，使用四輪全向移動機(jī)器人進(jìn)行實(shí)驗(yàn)，具體結(jié)構(gòu)如圖8所示，在機(jī)器人上搭載的主要器件有：1個Jetson Nano B01控制器、1個STM32F103運(yùn)動控制器、4個驅(qū)動電機(jī)、1個激光雷達(dá)和1個深度相機(jī)。在機(jī)器人控制器和個人PC中預(yù)裝基于Ubuntu18.04的機(jī)器人操作系統(tǒng)——Melodic。

實(shí)驗(yàn)條件如圖8所示。以個人PC為主機(jī)、機(jī)器人控制器為從機(jī)進(jìn)行局域網(wǎng)內(nèi)主從機(jī)網(wǎng)絡(luò)配置，在主機(jī)端開啟鍵盤控制節(jié)點(diǎn)、從機(jī)端開啟Cartographer算法建圖節(jié)點(diǎn)構(gòu)建如圖9所示的環(huán)境地圖。

(a)機(jī)器人實(shí)物

圖9 實(shí)驗(yàn)場地柵格地圖

控制機(jī)器人完成在真實(shí)環(huán)境下的導(dǎo)航功能。在如圖8(b)所示的真實(shí)環(huán)境中分別對原始A*算法、改進(jìn)A*算法以及融合算法進(jìn)行導(dǎo)航實(shí)驗(yàn)。

打開Rviz可視化界面，完成相應(yīng)顯示配置，進(jìn)行初始位姿估計(jì)并指定目標(biāo)點(diǎn)，基于建好的環(huán)境地圖對原始A*算法、改進(jìn)A*算法進(jìn)行導(dǎo)航實(shí)驗(yàn)，運(yùn)行結(jié)果如圖10和圖11所示。

(a)時刻“1”

(a)時刻“1”

從實(shí)驗(yàn)結(jié)果可以看出，改進(jìn)A*算法規(guī)劃的路線較原始A*算法規(guī)劃的路線轉(zhuǎn)折更小、平滑性更好。

保持以上起始點(diǎn)和目標(biāo)點(diǎn)不變，在改進(jìn)A*算法規(guī)劃的全局路徑中加入2個計(jì)算機(jī)主機(jī)箱作為未知障礙物基于融合算法進(jìn)行導(dǎo)航實(shí)驗(yàn)，加入未知障礙物的實(shí)驗(yàn)場地如圖12所示，實(shí)驗(yàn)結(jié)果如圖13所示。

圖12 加入未知障礙物后的實(shí)驗(yàn)場地

(a)時刻“1”

從實(shí)驗(yàn)結(jié)果可以看出，雖然融合算法在遇到未知障礙物時轉(zhuǎn)折角度會變大，但成功地避開了環(huán)境中的2個未知障礙物。

6 結(jié)束語

傳統(tǒng)A*算法規(guī)劃的路徑存在效率低、不平滑、靠近環(huán)境中已知障礙物及不能躲避環(huán)境中未知障礙物等缺點(diǎn)。在傳統(tǒng)A*算法的評價(jià)函數(shù)中加入環(huán)境信息及父節(jié)點(diǎn)信息加快尋路效率；提出安全擴(kuò)展策略自適應(yīng)改變擴(kuò)展方向避免機(jī)器人距離障礙物過近；提出關(guān)鍵點(diǎn)提取策略剔除冗余節(jié)點(diǎn)使路徑平滑；將改進(jìn)A*算法輸出的關(guān)鍵點(diǎn)信息加入到改進(jìn)DWA算法的軌跡評價(jià)函數(shù)中完成算法融合；設(shè)置仿真實(shí)驗(yàn)與實(shí)物實(shí)驗(yàn)進(jìn)行理論驗(yàn)證。最終實(shí)驗(yàn)結(jié)果證明，改進(jìn)A*算法的多項(xiàng)性能指標(biāo)相比于傳統(tǒng)A*算法都有了大幅提升，且融合改進(jìn)A*算法與DWA算法規(guī)劃的路徑能成功避開環(huán)境中的未知障礙物，使機(jī)器人安全達(dá)到目標(biāo)點(diǎn)。