融合改進(jìn)A*與DWA算法的機(jī)器人動(dòng)態(tài)路徑規(guī)劃

劉建娟，薛禮啟，張會(huì)娟，劉忠璞

1.河南工業(yè)大學(xué) 電氣工程學(xué)院，鄭州 450000

2.河南工業(yè)大學(xué) 機(jī)電設(shè)備及測(cè)控技術(shù)研究所，鄭州 450000

目前，路徑規(guī)劃可分為局部路徑規(guī)劃和全局路徑規(guī)劃兩大類，用于局部路徑規(guī)劃的算法包括DWA算法[1-3]、粒子群算法[4-6]、蟻群算法[7-9]、遺傳算法[10-11]等；用于全局路徑規(guī)劃的算法包括D*算法[12-13]、A*算法[14-15]、快速擴(kuò)展隨機(jī)樹(shù)[16-17]等。

A*算法廣泛應(yīng)用于移動(dòng)機(jī)器人路徑規(guī)劃，但是傳統(tǒng)A*算法在移動(dòng)機(jī)器人的應(yīng)用中存在拐點(diǎn)多、路徑不平滑等不足，不利于機(jī)器人的運(yùn)行。針對(duì)這些不足，許多學(xué)者對(duì)傳統(tǒng)A*算法進(jìn)行不同層面的改進(jìn)，也取得了不錯(cuò)的效果。文獻(xiàn)[18]利用JPS 算法對(duì)A*算法進(jìn)行改進(jìn)，優(yōu)化搜索策略，同時(shí)利用貝塞爾曲線對(duì)路徑進(jìn)行平滑優(yōu)化，由實(shí)驗(yàn)結(jié)果可知此算法明顯提升了路徑規(guī)劃效率，在大尺度環(huán)境中有較好的適應(yīng)性，但是A*算法是在全局已知的前提下進(jìn)行搜索和擴(kuò)展的，因此無(wú)法實(shí)現(xiàn)隨機(jī)障礙物的動(dòng)態(tài)避障。文獻(xiàn)[19]提出一種單邊矩形擴(kuò)展A*算法，利用受迫擴(kuò)展規(guī)則，簡(jiǎn)化終止條件，提高搜索效率和路徑規(guī)劃質(zhì)量，但是算法搜索地圖的不規(guī)則性和破碎化程度不斷加重，算法更適合在大尺度環(huán)境中的路徑規(guī)劃。DWA算法常用于移動(dòng)機(jī)器人的局部路徑規(guī)劃，但是在大尺度復(fù)雜環(huán)境中由于缺少中間引導(dǎo)，無(wú)法得到最優(yōu)路徑，甚至無(wú)法達(dá)到目標(biāo)點(diǎn)。文獻(xiàn)[20]通過(guò)移動(dòng)機(jī)器人的尺寸與障礙物之間的自由空間關(guān)系對(duì)DWA算法進(jìn)行改進(jìn)，由于對(duì)障礙物與機(jī)器人之間的距離進(jìn)行限制，在復(fù)雜環(huán)境中出現(xiàn)的狹窄區(qū)域會(huì)造成機(jī)器人反復(fù)啟停，甚至停止運(yùn)行。文獻(xiàn)[21]通過(guò)立體視覺(jué)進(jìn)行避障，嚴(yán)格定義機(jī)器人的搜索空間得到機(jī)器人最優(yōu)速度，雖然可以實(shí)現(xiàn)動(dòng)態(tài)路徑規(guī)劃，但是由于缺乏中間指引，規(guī)劃路徑很難達(dá)到最優(yōu)，而立體視覺(jué)又增加了時(shí)間和空間復(fù)雜性，對(duì)硬件設(shè)備性能提出更高的要求，經(jīng)濟(jì)性會(huì)有所降低。目前來(lái)說(shuō)，單一改進(jìn)A*算法路徑規(guī)劃能得到從起點(diǎn)到終點(diǎn)的最優(yōu)路徑，但是無(wú)法及時(shí)躲避局部障礙物；單一改進(jìn)DWA算法雖然能有效躲避局部障礙物，但是不滿足全局最優(yōu)路徑。因此，結(jié)合兩者的優(yōu)勢(shì)，對(duì)兩種算法進(jìn)行改進(jìn)并實(shí)現(xiàn)兩種算法融合是當(dāng)前研究的熱點(diǎn)。文獻(xiàn)[22]將搜索鄰域由3×3 擴(kuò)展為7×7，優(yōu)化了搜索角度，減少了路徑的轉(zhuǎn)折點(diǎn)，使得路徑更加平滑，然后將改進(jìn)后的A*算法與DWA算法融合，雖然保證了全局最優(yōu)，但是由于沒(méi)有對(duì)已知障礙物和未知?jiǎng)討B(tài)障礙物進(jìn)行區(qū)分，導(dǎo)致在動(dòng)態(tài)路徑規(guī)劃時(shí)兩者相互影響，造成動(dòng)態(tài)避障靈敏度低。文獻(xiàn)[23]通過(guò)判斷起點(diǎn)和終點(diǎn)的位置關(guān)系，將傳統(tǒng)的8搜索方向改為5搜索方向，設(shè)計(jì)新的啟發(fā)函數(shù)，實(shí)現(xiàn)無(wú)斜穿路徑，增加了路徑平滑度，實(shí)現(xiàn)算法融合。但是融合算法存在一定的不足，首先，由于沒(méi)有改進(jìn)DWA 算法動(dòng)態(tài)窗口評(píng)價(jià)函數(shù)，使得動(dòng)態(tài)避障靈敏度不高，其次，將傳統(tǒng)8 搜索方向改為5 搜索方向時(shí)，沒(méi)有考慮到剩余5搜索方向都存在障礙物的情況并加以判斷，搜索可能會(huì)陷入死區(qū)，無(wú)法實(shí)現(xiàn)路徑規(guī)劃。文獻(xiàn)[24]通過(guò)關(guān)鍵點(diǎn)提取策略對(duì)傳統(tǒng)A*算法進(jìn)行改進(jìn)，并構(gòu)建全局最優(yōu)的動(dòng)態(tài)窗口評(píng)價(jià)函數(shù)，將改進(jìn)的算法進(jìn)行融合，實(shí)現(xiàn)實(shí)時(shí)最優(yōu)路徑規(guī)劃。算法雖然對(duì)動(dòng)態(tài)窗口評(píng)價(jià)函數(shù)進(jìn)行改進(jìn)，增加機(jī)器人末端到終點(diǎn)的距離函數(shù)，其目的是不斷縮短與終點(diǎn)的距離，但是仍然沒(méi)有區(qū)分障礙物類型，既難以獲得全局最優(yōu)路徑，又無(wú)法保證動(dòng)態(tài)避障的靈敏度。

針對(duì)這些問(wèn)題，將傳統(tǒng)A*算法的啟發(fā)函數(shù)中引入環(huán)境信息，實(shí)現(xiàn)對(duì)A*算法啟發(fā)函數(shù)的自適應(yīng)調(diào)整；設(shè)計(jì)路徑平滑優(yōu)化算法，刪除冗余節(jié)點(diǎn)、減少轉(zhuǎn)折、提高路徑平滑度；提取改進(jìn)A*算法規(guī)劃路徑的關(guān)鍵點(diǎn)作為DWA算法的中間目標(biāo)點(diǎn)做全局引導(dǎo)，在全局最優(yōu)的基礎(chǔ)上實(shí)現(xiàn)改進(jìn)A*與DWA算法融合。

1 傳統(tǒng)A*與DWA算法

首先對(duì)現(xiàn)有文獻(xiàn)中傳統(tǒng)A*算法及DWA 算法的基本原理進(jìn)行介紹。分析算法存在的不足及形成原因，為第2章中提出算法改進(jìn)和算法融合提供理論依據(jù)。

1.1 傳統(tǒng)A*算法基本原理

A*算法廣泛應(yīng)用于二維地圖的路徑規(guī)劃，是一種典型的啟發(fā)式搜索型路徑規(guī)劃算法，利用評(píng)價(jià)函數(shù)來(lái)指導(dǎo)節(jié)點(diǎn)的搜索與擴(kuò)展，因此評(píng)價(jià)函數(shù)影響搜索空間的大小和算法的速度。傳統(tǒng)A*算法的評(píng)價(jià)函數(shù)如公式（1）所示：

在評(píng)價(jià)函數(shù)公式（1）中，n表示當(dāng)前節(jié)點(diǎn)，F(xiàn)(n)表示從起點(diǎn)經(jīng)由節(jié)點(diǎn)n到達(dá)目標(biāo)點(diǎn)的評(píng)價(jià)函數(shù)；G(n)表示從起點(diǎn)到達(dá)節(jié)點(diǎn)n的實(shí)際代價(jià)；H(n)為估計(jì)值，表示從節(jié)點(diǎn)n到目標(biāo)點(diǎn)估計(jì)代價(jià)，又叫啟發(fā)函數(shù)。由啟發(fā)函數(shù)H(n)的選取，可以控制節(jié)點(diǎn)的搜索與擴(kuò)展，進(jìn)而可以控制算法的速度和精度。傳統(tǒng)A*算法常用的啟發(fā)函數(shù)有曼哈頓距離、切比雪夫距離、歐幾里德距離三種，三種距離的示意圖如圖1所示。因?yàn)閱l(fā)函數(shù)H(n)與實(shí)際距離的關(guān)系會(huì)直接影響算法的速度和精度，為了提高算法的精度和靈活性，需要對(duì)啟發(fā)函數(shù)進(jìn)行改進(jìn)。

圖1 三種啟發(fā)函數(shù)距離示意圖Fig.1 Distance diagram of three heuristic functions

1.2 DWA算法基本原理

1.2.1 運(yùn)動(dòng)學(xué)模型

本文選用Turtlebot2移動(dòng)機(jī)器人平臺(tái)作為實(shí)驗(yàn)平臺(tái)進(jìn)行實(shí)驗(yàn)驗(yàn)證。由于DWA算法要將位置控制轉(zhuǎn)化為速度控制，因此需要對(duì)Turtlebot2 的運(yùn)動(dòng)模型進(jìn)行分析，Turtlebot2的兩輪差速運(yùn)動(dòng)的運(yùn)動(dòng)學(xué)模型如圖2所示。

圖2 Turtlebot2差速運(yùn)動(dòng)學(xué)模型Fig.2 Differential kinematics model of Turnlebot2

假設(shè)v(t)和ω(t)分別表示Turtlebot2 在世界坐標(biāo)系下t時(shí)刻的線速度和角速度，在采樣周期Δt內(nèi)，位移較小，近似做勻速直線運(yùn)動(dòng)，則此運(yùn)動(dòng)學(xué)模型的數(shù)學(xué)表達(dá)式可表示為公式（2）：

1.2.2 速度采樣

DWA算法將動(dòng)態(tài)避障問(wèn)題描述為速度空間中三個(gè)帶約束的優(yōu)化問(wèn)題，要根據(jù)實(shí)際情況對(duì)速度采樣范圍進(jìn)行約束。

（1）移動(dòng)機(jī)器人速度約束

機(jī)器人的采樣速度應(yīng)控制在移動(dòng)機(jī)器人最佳角速度和線速度區(qū)間內(nèi)，則約束為公式（3）：

式中，vmax、vmin指機(jī)器人的最大、最小線速度，ωmax、ωmin指機(jī)器人最大、最小角速度。

（2）移動(dòng)機(jī)器人電機(jī)加減速約束

機(jī)器人的采樣速度應(yīng)考慮電機(jī)性能，速度應(yīng)保持在當(dāng)前速度在最大減速度和最大加速度條件下，速度可變范圍內(nèi)，則約束為公式（4）：

式中，vc、ωc指當(dāng)前時(shí)刻線速度和角速度，admax、aimax指機(jī)器人最大減速度和最大加速度，αdmax、αimax指機(jī)器人的最大角減速度和最大角加速度。

（3）移動(dòng)機(jī)器人安全距離約束

機(jī)器人在進(jìn)行局部路徑規(guī)劃時(shí)，為了保證其安全，在約束條件（2）最大減速度條件下，使得移動(dòng)機(jī)器人到達(dá)障礙物之前線速度和角速度都降為零，則約束為公式（5）：

式中，dist(v,ω) 指模擬軌跡末端距離障礙物的最近距離，admax、αdmax指機(jī)器人最大減速度和最大加速度。

最終的動(dòng)態(tài)窗口的速度為以上三個(gè)約束條件的交集，動(dòng)態(tài)窗口速度Vw滿足公式（6）：

1.2.3 評(píng)價(jià)函數(shù)

DWA 算法的評(píng)價(jià)函數(shù)指標(biāo)有偏向角、線速度及模擬軌跡末端與距障礙物的最近距離。由于這三個(gè)指標(biāo)的度量單位不同，為避免某一個(gè)量的數(shù)值過(guò)大而占比過(guò)大，需要對(duì)每個(gè)指標(biāo)進(jìn)行歸一化處理，歸一化處理后得到的軌跡評(píng)價(jià)函數(shù)如公式（7）：

式中，α、β、γ是每一項(xiàng)的加權(quán)系數(shù)。Head(v,ω)為模擬軌跡終點(diǎn)方向和目標(biāo)點(diǎn)之間方向角偏差；Vel(v,ω)用來(lái)評(píng)價(jià)移動(dòng)機(jī)器人在當(dāng)前估計(jì)中運(yùn)行的速度大??；Dist(v,ω) 是指模擬軌跡終點(diǎn)與障礙物的最近距離。

對(duì)傳統(tǒng)DWA算法的原理介紹可知，傳統(tǒng)DWA算法存在兩點(diǎn)不足：第一，只有一個(gè)目標(biāo)點(diǎn)，缺少中間指引，在大尺度環(huán)境中無(wú)法得到最優(yōu)路徑；第二，未將已知障礙物和未知障礙物進(jìn)行區(qū)分，導(dǎo)致動(dòng)態(tài)避障靈敏度低。

2 算法改進(jìn)與算法融合

針對(duì)上文提到的傳統(tǒng)算法存在的不足，提出自己算法的改進(jìn)和融合思路。首先，量化環(huán)境信息，將環(huán)境信息引入傳統(tǒng)A*算法的評(píng)價(jià)函數(shù)，實(shí)現(xiàn)了評(píng)價(jià)函數(shù)中啟發(fā)函數(shù)的自適應(yīng)改變，提高了算法的靈活性，其次設(shè)計(jì)路徑平滑算法對(duì)路徑進(jìn)行平滑優(yōu)化，使得路徑更加適合機(jī)器人的運(yùn)動(dòng)。最后改進(jìn)DWA 算法的評(píng)價(jià)函數(shù)，減少已知障礙物和未知障礙物的影響，并將改進(jìn)后的兩種算法進(jìn)行融合。

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

2.1.1 量化環(huán)境信息

傳統(tǒng)A*算法的評(píng)價(jià)函數(shù)由實(shí)際代價(jià)函數(shù)G(n)和啟發(fā)函數(shù)H(n)組成。其中，啟發(fā)函數(shù)H(n)主導(dǎo)傳統(tǒng)A*算法的搜索性能。當(dāng)起點(diǎn)與終點(diǎn)沒(méi)有障礙物時(shí)，選用歐幾里德距離公式估計(jì)的路徑與實(shí)際路徑相同，此時(shí)算法速度快、準(zhǔn)確率高，但是實(shí)際應(yīng)用中往往存在障礙物，導(dǎo)致搜索空間變大、效率降低、產(chǎn)生較多冗余節(jié)點(diǎn)。因此，當(dāng)障礙物較少時(shí)，可以適當(dāng)增大啟發(fā)函數(shù)H(n)權(quán)重，減小搜索范圍、提高搜索效率。當(dāng)障礙物較多時(shí)，可以適當(dāng)減小啟發(fā)函數(shù)H(n)權(quán)重，提高搜索范圍避免陷入局部最優(yōu)。障礙物的存在使得產(chǎn)生多條可選路徑，最短路徑也會(huì)大于起點(diǎn)和終點(diǎn)之間的歐幾里德距離。為了估計(jì)最短路徑，引入環(huán)境障礙物比率K抽象反應(yīng)環(huán)境復(fù)雜度。假設(shè)移動(dòng)機(jī)器人起點(diǎn)為(xs,ys)終點(diǎn)為(xt,yt)，起點(diǎn)和終點(diǎn)組成的矩形柵格障礙物柵格數(shù)為N，那么障礙物的比率K可表示為公式（8）：

2.1.2 環(huán)境信息引入評(píng)價(jià)函數(shù)

環(huán)境障礙物比率K由障礙物柵格數(shù)與起點(diǎn)終點(diǎn)共同決定，K的大小隨起點(diǎn)終點(diǎn)的改變而改變。將環(huán)境障礙物比率K引入評(píng)價(jià)函數(shù)F(n)，改變啟發(fā)函數(shù)H(n)的權(quán)重，實(shí)現(xiàn)評(píng)價(jià)函數(shù)F(n)的自適應(yīng)調(diào)整。啟發(fā)函數(shù)H(n)選取歐幾里德距離如公式（9）所示，改進(jìn)后的評(píng)價(jià)函數(shù)F(n)如公式（10）所示。

由公式（10）可知，當(dāng)環(huán)境中障礙物較少時(shí)，啟發(fā)函數(shù)H(n)的系數(shù)(1-lnK)大，算法縮小了搜索空間，提高了搜索效率。當(dāng)環(huán)境中障礙物多時(shí)，啟發(fā)函數(shù)H(n)的系數(shù)(1-lnK)小，算法擴(kuò)大了搜索空間，降低搜索速度，避免陷入局部最優(yōu)。

2.1.3 路徑平滑優(yōu)化

傳統(tǒng)A*算法路徑規(guī)劃是由連續(xù)柵格中心點(diǎn)連接組成，存在許多冗余節(jié)點(diǎn)，路徑轉(zhuǎn)折次數(shù)多，路徑不平滑。針對(duì)這些問(wèn)題，基于Floyd 算法思想設(shè)計(jì)路徑平滑優(yōu)化算法。

路徑平滑優(yōu)化原理如圖3 所示，以此為例，傳統(tǒng)A*算法規(guī)劃的路徑規(guī)劃由柵格中心點(diǎn)的連線組成，優(yōu)化前的路徑為（S，1，2，…，13，T），存在許多冗余節(jié)點(diǎn)。在考慮安全距離D的基礎(chǔ)上，對(duì)路徑進(jìn)行平滑優(yōu)化，使得路徑的選擇不再局限于過(guò)柵格的中心位置。優(yōu)化后的路徑平滑度增加，路徑長(zhǎng)度和轉(zhuǎn)折點(diǎn)減少，路徑平滑優(yōu)化步驟如下：

圖3 路徑平滑優(yōu)化原理圖Fig.3 Optimization of path smoothing

步驟1遍歷所有節(jié)點(diǎn)，刪除每一段路徑中間的冗余節(jié)點(diǎn)，保留起始點(diǎn)和拐點(diǎn)。刪除中間節(jié)點(diǎn)后剩下S，2，6，8，9，10，11，13，T五個(gè)節(jié)點(diǎn)。

步驟2遍歷起始點(diǎn)和拐點(diǎn)，從起點(diǎn)開(kāi)始將每一個(gè)節(jié)點(diǎn)都與后面的節(jié)點(diǎn)相連作為備選路徑，判斷每條路徑與障礙物柵格的距離di與安全距離D的關(guān)系。若di≤D，則刪除路徑，若di>D，保留路徑，刪除路徑之間的拐點(diǎn)。刪除不必要拐點(diǎn)后剩余S，6，8，T三個(gè)節(jié)點(diǎn)。

步驟3提取剩余節(jié)點(diǎn)，輸出優(yōu)化路徑，算法結(jié)束。

2.2 DWA算法評(píng)價(jià)函數(shù)優(yōu)化

為了提高融合算法在復(fù)雜環(huán)境中的避障性能，需要對(duì)傳統(tǒng)DWA 算法的評(píng)價(jià)函數(shù)進(jìn)行優(yōu)化。在傳統(tǒng)DWA算法路徑規(guī)劃時(shí)，評(píng)價(jià)函數(shù)中Dist(v,ω)的權(quán)重，決定軌跡與障礙物的距離。若增大Dist(v,ω)系數(shù)，則生成路徑長(zhǎng)度增大，達(dá)不到全局最優(yōu)；若減小Dist(v,ω)系數(shù)，則遇到未知障礙物時(shí)，不能及時(shí)避障。為了減少全局已知障礙物與全局未知障礙物之間的相互干擾，將評(píng)價(jià)函數(shù)中Dist(v,ω)區(qū)分為模擬軌跡終點(diǎn)與已知障礙物的最近距離Dist_s(v,w)和模擬軌跡終點(diǎn)與未知障礙物的最近距離Dist_d(v,w)。則優(yōu)化后的DWA 算法評(píng)價(jià)函數(shù)如公式（11）：公式（11）中，α、β、γ、φ是每一項(xiàng)的加權(quán)系數(shù)。Heading(v,w)為模擬軌跡終點(diǎn)和子目標(biāo)點(diǎn)之間方向角偏差，隨著路徑的前進(jìn)，按照提取的關(guān)鍵點(diǎn)順序不斷更新子目標(biāo)點(diǎn)；Vel(v,ω) 用來(lái)評(píng)價(jià)移動(dòng)機(jī)器人在當(dāng)前估計(jì)中運(yùn)行的速度大小；Dist_s(v,w)表示模擬軌跡終點(diǎn)與已知障礙物的最近距離，控制已知障礙物對(duì)局部路徑規(guī)劃的干擾；Dist_d(v,w)表示模擬軌跡終點(diǎn)與未知障礙物的最近距離，用來(lái)控制避障的靈敏度。

2.3 融合算法

改進(jìn)A*算法能夠獲取全局路徑規(guī)劃中的最優(yōu)解，在靜態(tài)工作環(huán)境中能很好地完成全局路徑規(guī)劃能力，但是對(duì)環(huán)境中出現(xiàn)的未知障礙物無(wú)法及時(shí)避障進(jìn)行局部路徑規(guī)劃。DWA 算法路徑規(guī)劃缺少全局指引，只有目的地一個(gè)方向指引，在障礙物較多的環(huán)境中，容易陷入局部最優(yōu)，導(dǎo)致全局路徑變大，甚至路徑規(guī)劃失敗。針對(duì)上述兩種算法的問(wèn)題，首先提取改進(jìn)A*算法規(guī)劃路徑上的關(guān)鍵點(diǎn)，然后將提取的關(guān)鍵點(diǎn)作為DWA 算法的中間目標(biāo)點(diǎn)，引導(dǎo)局部路徑規(guī)劃，融合算法又將已知障礙物和未知障礙物進(jìn)行分類，減少已知障礙物對(duì)路徑的影響，融合算法流程圖如圖4所示。

圖4 融合算法流程圖Fig.4 Flow chart of fusion algorithm

3 仿真驗(yàn)證

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

仿真實(shí)驗(yàn)在MATLAB 2016b 環(huán)境下進(jìn)行，為驗(yàn)證算法的有效性和適應(yīng)性，將對(duì)比實(shí)驗(yàn)的地圖設(shè)計(jì)為迷宮式柵格地圖。在柵格地圖環(huán)境中，每個(gè)柵格設(shè)置為長(zhǎng)為1 m的正方形，黑色柵格表示障礙物區(qū)域，白色柵格表示空白可移動(dòng)區(qū)域，△表示機(jī)器人初始位置，○表示目標(biāo)點(diǎn)。改進(jìn)A*算法中障礙物比重參數(shù)K隨起點(diǎn)和終點(diǎn)的位置變化而變化，安全距離D設(shè)置為0.8 m。傳統(tǒng)A*算法與改進(jìn)A*算法的仿真結(jié)果如圖5所示，性能比較如表1所示。

圖5 傳統(tǒng)A*算法與改進(jìn)A*算法仿真結(jié)果對(duì)比圖Fig.5 Comparison of simulation results of traditional A* algorithm and improved A* algorithm

表1 傳統(tǒng)A*算法與改進(jìn)A*算法性能指標(biāo)對(duì)比Table 1 Performance comparison between traditional A*algorithm and improved A* algorithm

由圖5和表1可知，通過(guò)改進(jìn)算法的評(píng)價(jià)函數(shù)，引入環(huán)境信息，在環(huán)境信息的引導(dǎo)下搜索更有方向性，使得路徑規(guī)劃時(shí)間減少0.8 s，轉(zhuǎn)彎次數(shù)減少48.0%，轉(zhuǎn)折角度減少了42.1%。通過(guò)借鑒Floyd算法思想對(duì)路徑進(jìn)行平滑優(yōu)化，使得路徑長(zhǎng)度減少8.0%。由此可見(jiàn)改進(jìn)算法不僅提高了算法的效率，而且降低了路徑長(zhǎng)度，提高了平滑度。

3.2 融合算法仿真實(shí)驗(yàn)

融合算法仿真實(shí)驗(yàn)環(huán)境地圖在改進(jìn)A*算法仿真實(shí)驗(yàn)的基礎(chǔ)上做兩點(diǎn)改變：（1）在改進(jìn)A*算法規(guī)劃路徑上隨機(jī)加上四個(gè)未知靜態(tài)障礙物；（2）在仿真環(huán)境中隨機(jī)加入一個(gè)未知?jiǎng)討B(tài)障礙物。融合算法的評(píng)價(jià)函數(shù)的各項(xiàng)系數(shù)α=0.3、β=0.4、γ=0.2、φ=0.2，融合算法仿真實(shí)驗(yàn)機(jī)器人運(yùn)動(dòng)學(xué)參數(shù)如表2所示。

表2 機(jī)器人運(yùn)動(dòng)學(xué)參數(shù)Table 2 Kinematic parameters of robot

融合算法仿真實(shí)驗(yàn)結(jié)果如圖6所示，圖6（a）中粉色虛線為改進(jìn)A*算法在靜態(tài)環(huán)境中的全局路徑規(guī)劃，路徑上的*為提取的關(guān)鍵點(diǎn)，作為融合算法的中間目標(biāo)點(diǎn)。圖6（b）中紅色方塊用來(lái)模擬全局路徑上隨機(jī)加入的未知靜態(tài)障礙物，黃色方塊用來(lái)模擬環(huán)境中隨機(jī)出現(xiàn)的動(dòng)態(tài)障礙物，且正在繞過(guò)第一個(gè)靜態(tài)障礙物。圖6（c）表示已繞過(guò)第二個(gè)靜態(tài)障礙物，而后與動(dòng)態(tài)障礙物相遇開(kāi)始局部動(dòng)態(tài)路徑規(guī)劃。圖6（d）表示機(jī)器人在保證全局最優(yōu)的基礎(chǔ)上完成了起點(diǎn)到終點(diǎn)的動(dòng)態(tài)路徑規(guī)劃。在路徑規(guī)劃的過(guò)程中軌跡末端綠色短曲線為模擬軌跡，黑色虛線為動(dòng)態(tài)障礙物的移動(dòng)軌跡，藍(lán)色實(shí)線為融合算法規(guī)劃路徑。

圖6 融合算法路徑規(guī)劃仿真結(jié)果Fig.6 Simulation results of path planning based on fusion algorithm

由圖6融合算法仿真結(jié)果可知，提取改進(jìn)A*算法路徑上的關(guān)鍵點(diǎn)，作為DWA算法的中間目標(biāo)點(diǎn)，使得規(guī)劃路徑靠近改進(jìn)A*算法靜態(tài)最優(yōu)路徑；融合算法又將已知障礙物和未知障礙物進(jìn)行分類，減少已知障礙物對(duì)路徑的影響，使得規(guī)劃路徑及時(shí)有效地避開(kāi)未知?jiǎng)屿o態(tài)障礙物。

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

實(shí)驗(yàn)選取Turtlebot2移動(dòng)機(jī)器人平臺(tái)進(jìn)行物理平臺(tái)實(shí)驗(yàn)驗(yàn)證。為滿足實(shí)驗(yàn)需要，除Turtlebot2 自身所帶的傳感器外，還配備了思嵐A2激光雷達(dá)和Kinect v1深度相機(jī)來(lái)感知周圍環(huán)境。為了方便調(diào)試和機(jī)器人的自主導(dǎo)航，在Turtlebot2上配備一臺(tái)小型工控機(jī)作為主機(jī)，個(gè)人PC作為從機(jī)，主機(jī)和從機(jī)都安裝64位ubuntu16.04操作系統(tǒng)和機(jī)器人操作系統(tǒng)ROS（Kinetic）并在局域網(wǎng)內(nèi)完成主機(jī)與從機(jī)的網(wǎng)絡(luò)配置，通過(guò)IP 實(shí)現(xiàn)個(gè)人PC 對(duì)Turtlebot2 的遠(yuǎn)程控制。為了驗(yàn)證算法的可行性，用Turtlebot2 移動(dòng)機(jī)器人，在同一環(huán)境中對(duì)三種算法進(jìn)行對(duì)比驗(yàn)證。Turtlebot2 移動(dòng)機(jī)器人的實(shí)驗(yàn)參數(shù)設(shè)置如下：最大線速度0.5 m/s，最大角速度0.25 rad/s，線速度采樣個(gè)數(shù)為10，角速度采樣個(gè)數(shù)為20，采樣周期0.2 s。實(shí)驗(yàn)場(chǎng)地如圖7所示。

圖7 實(shí)驗(yàn)場(chǎng)地地圖Fig.7 Experimental site map

通過(guò)從機(jī)打開(kāi)從機(jī)終端，啟動(dòng)底盤，啟動(dòng)Gmapping構(gòu)圖算法。打開(kāi)從機(jī)終端，啟動(dòng)鍵盤節(jié)點(diǎn)，打開(kāi)Rviz可視化工具，通過(guò)鍵盤控制節(jié)點(diǎn)控制機(jī)器人運(yùn)動(dòng)，構(gòu)建環(huán)境地圖如圖8所示。

圖8 實(shí)驗(yàn)場(chǎng)地建圖Fig.8 Construction of experimental site

4.1 傳統(tǒng)A*算法實(shí)驗(yàn)驗(yàn)證

在ROS 系統(tǒng)下通過(guò)主機(jī)遠(yuǎn)程控制軟件登錄，實(shí)現(xiàn)對(duì)Turtlebot2 的遠(yuǎn)程控制。啟動(dòng)底盤控制節(jié)點(diǎn)，啟動(dòng)定位并選定地圖，打開(kāi)Rviz可視化界面，啟動(dòng)傳統(tǒng)A*算法路徑規(guī)劃，選定起點(diǎn)和終點(diǎn)。運(yùn)行結(jié)果如圖9所示。

圖9 傳統(tǒng)A*算法路徑規(guī)劃Fig.9 Traditional A* algorithm for path planning

傳統(tǒng)A*算法實(shí)驗(yàn)結(jié)果由圖9，圖中紅色箭頭擺動(dòng)幅度反應(yīng)拐彎的角度。由實(shí)驗(yàn)結(jié)果可以看出，傳統(tǒng)A*算可以實(shí)現(xiàn)在靜態(tài)環(huán)境中的全局路徑規(guī)劃，但是轉(zhuǎn)彎的折點(diǎn)多，路徑不夠平滑，不利于機(jī)器人的行駛。

4.2 改進(jìn)A*算法實(shí)驗(yàn)驗(yàn)證

保持起點(diǎn)和終點(diǎn)位置不變，啟動(dòng)改進(jìn)A*算法路徑規(guī)劃，實(shí)驗(yàn)結(jié)果如圖10所示。

圖10 改進(jìn)A*算法路徑規(guī)劃Fig.10 Improved A* algorithm for path planning

改進(jìn)A*算法實(shí)驗(yàn)結(jié)果由圖10，圖中紅色箭頭擺動(dòng)幅度反應(yīng)拐彎的角度。由實(shí)驗(yàn)結(jié)果可知，相比于傳統(tǒng)A*算法，改進(jìn)A*算法的路徑規(guī)劃算法運(yùn)動(dòng)更加平滑，但是改進(jìn)A*算法是全局靜態(tài)路徑規(guī)劃，無(wú)法及時(shí)躲避環(huán)境中出現(xiàn)的障礙物。

4.3 融合算法實(shí)驗(yàn)驗(yàn)證

實(shí)驗(yàn)場(chǎng)景和設(shè)備與改進(jìn)A*算法實(shí)驗(yàn)保持一致，在實(shí)驗(yàn)環(huán)境中站立三個(gè)人靜止不動(dòng)，作為靜態(tài)未知障礙物，在小車運(yùn)行過(guò)程中走動(dòng)拍攝者作為動(dòng)態(tài)未知障礙物。保持起點(diǎn)和終點(diǎn)位置不變，啟動(dòng)融合算法路徑規(guī)劃，實(shí)驗(yàn)結(jié)果如圖11所示。

圖11 融合算法路徑規(guī)劃Fig.11 Path planning based on fusion algorithm

改進(jìn)A*算法實(shí)驗(yàn)結(jié)果由圖11，圖中紅色箭頭擺動(dòng)幅度反應(yīng)拐彎的角度。由實(shí)驗(yàn)結(jié)果可知，融合算法在全局最優(yōu)的基礎(chǔ)上，又能及時(shí)躲避環(huán)境中出現(xiàn)的動(dòng)靜態(tài)障礙物，提高了移動(dòng)機(jī)器人在復(fù)雜環(huán)境中的適應(yīng)能力。

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

傳統(tǒng)A*算法路徑規(guī)劃存在效率低、路徑規(guī)劃不平滑等缺點(diǎn)，不能滿足機(jī)器人的實(shí)際要求。這篇文章將環(huán)境信息引入傳統(tǒng)A*算法的評(píng)價(jià)函數(shù)，優(yōu)化搜索策略，使得路徑的搜索隨著環(huán)境復(fù)雜度的變化而自適應(yīng)調(diào)整，提高了算法的效率和靈活性；基于Floyd 算法思想設(shè)計(jì)路徑平滑算法，刪除冗余節(jié)點(diǎn)和不必要的拐點(diǎn)，使得路徑更加平滑，實(shí)驗(yàn)結(jié)果表明，改進(jìn)A*算法在大尺度復(fù)雜環(huán)境中不僅能生成最優(yōu)路徑，效率也有所提高。提取改進(jìn)A*算法的關(guān)鍵點(diǎn)作為DWA算法的中間目標(biāo)點(diǎn)，在全局最優(yōu)的基礎(chǔ)上實(shí)現(xiàn)了算法融合。實(shí)驗(yàn)結(jié)果表明，融合算法規(guī)劃路徑圍繞全局最優(yōu)路徑輕微浮動(dòng)，且成功對(duì)比全局路徑上出現(xiàn)的障礙物及環(huán)境中隨機(jī)出現(xiàn)的動(dòng)態(tài)障礙物。通過(guò)仿真驗(yàn)證和實(shí)驗(yàn)驗(yàn)證，結(jié)果表明了融合算法在機(jī)器人路徑規(guī)劃中的可行性。