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

槐創(chuàng)鋒，郭 龍，賈雪艷，張子昊

華東交通大學(xué) 機(jī)電與車輛工程學(xué)院，南昌330013

在移動(dòng)機(jī)器人諸多技術(shù)的研究中，路徑規(guī)劃是技術(shù)研究中的重要部分之一，是機(jī)器人研究領(lǐng)域的熱點(diǎn)，目的是在有障礙物的環(huán)境中按照某一種評(píng)價(jià)指標(biāo)尋找一條從起始點(diǎn)位置到目標(biāo)點(diǎn)位置的最優(yōu)無碰撞路徑[1-2]。

柵格法環(huán)境建模的經(jīng)典方法，通過利用柵格對環(huán)境信息進(jìn)行表示，柵格的大小決定了環(huán)境信息儲(chǔ)存量的大小以機(jī)器人進(jìn)行路徑規(guī)劃的時(shí)間長短[3-4]。A*算法[5]是路徑規(guī)劃算法中經(jīng)典的啟發(fā)式搜索算法。A*算法由Hart等[6]提出，結(jié)合Dijkstra算法和最佳優(yōu)先搜索算法的優(yōu)點(diǎn)。文獻(xiàn)[7]改進(jìn)A*算法，在啟發(fā)函數(shù)中加入余弦相似性和方向信息，做歸一化處理。文獻(xiàn)[8]提出改進(jìn)A*算法的擴(kuò)展搜索鄰域的思路，利用最小二叉堆優(yōu)化A*算法OPEN列表存儲(chǔ)結(jié)構(gòu)提高搜索效率，但無法完成動(dòng)態(tài)避障。文獻(xiàn)[9]成功地?cái)U(kuò)大搜索鄰域，通過重新定義中心節(jié)點(diǎn)的位置，在每個(gè)節(jié)點(diǎn)的周圍擴(kuò)大無限可搜索鄰域，較為快速實(shí)現(xiàn)無碰撞路徑規(guī)劃。文獻(xiàn)[10]基于A*算法，設(shè)計(jì)了優(yōu)化的啟發(fā)搜索函數(shù)，選取策略是使用關(guān)鍵點(diǎn)，剔除多余路徑點(diǎn)以及非必要的轉(zhuǎn)折點(diǎn)。文獻(xiàn)[11]通過引入二次A*算法，減少路徑軌跡冗余點(diǎn)，縮短路徑長度并且采用自適應(yīng)圓弧優(yōu)化算法與加權(quán)障礙物步長調(diào)節(jié)了算法，采用預(yù)瞄偏差角追蹤法捕捉移動(dòng)目標(biāo)點(diǎn)，有效提升路徑規(guī)劃效率。文獻(xiàn)[12]以時(shí)間耗費(fèi)作為評(píng)價(jià)指標(biāo)，假設(shè)AGV 通過每個(gè)弧或節(jié)點(diǎn)的時(shí)間成本是固定的，改進(jìn)A*算法通過搜尋最小時(shí)間耗費(fèi)來選擇最優(yōu)路徑。文獻(xiàn)[13]A*算法的改進(jìn)，創(chuàng)新了高層建筑逃生路徑規(guī)劃算法，對節(jié)點(diǎn)擴(kuò)展優(yōu)化、權(quán)值優(yōu)化方面進(jìn)行改進(jìn)。文獻(xiàn)[14]修改A*算法的地圖信息，擴(kuò)展了一層障礙物，改進(jìn)了代價(jià)函數(shù)F作二次擴(kuò)展，并且考慮機(jī)器人半徑。

針對傳統(tǒng)路徑規(guī)劃算法無法高效地、安全地完成動(dòng)態(tài)環(huán)境下的路徑規(guī)劃，將傳統(tǒng)A*算法擴(kuò)展搜索鄰域，去除了擴(kuò)展鄰域同方向的子節(jié)點(diǎn)，改進(jìn)成了7×7 的A*算法。然后基于改進(jìn)7×7的A*算法和動(dòng)態(tài)窗口法的移動(dòng)機(jī)器人在柵格法環(huán)境模型中實(shí)時(shí)仿真動(dòng)態(tài)路徑規(guī)劃。

1 改進(jìn)的A＊算法

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

A*算法是啟發(fā)式的搜索算法，可以實(shí)現(xiàn)全局路徑規(guī)劃。根據(jù)定義的估價(jià)函數(shù)在靜態(tài)環(huán)境模型中搜索理論上的最優(yōu)路徑，則代價(jià)函數(shù)表示為：

式中，n代表當(dāng)前節(jié)點(diǎn)；f(n)是當(dāng)前節(jié)點(diǎn)n的代價(jià)函數(shù)；g(n)為移動(dòng)機(jī)器人起始節(jié)點(diǎn)到達(dá)當(dāng)前節(jié)點(diǎn)n的實(shí)際代價(jià)值；h(n)是從當(dāng)前節(jié)點(diǎn)n到達(dá)目標(biāo)點(diǎn)的代價(jià)估計(jì)值，是代價(jià)函數(shù)中最重要的部分，正確地選擇h(n)能夠提升A*算法的成功率和準(zhǔn)確性。選取歐氏距離作為h(n)的代價(jià)函數(shù)，函數(shù)表示為：

式中，(xn,yn)代表當(dāng)前路徑節(jié)點(diǎn)所在柵格中心的坐標(biāo)，(xg,yg)代表目標(biāo)點(diǎn)節(jié)點(diǎn)所在柵格中心坐標(biāo)。傳統(tǒng)的A*算法搜索從起始點(diǎn)到達(dá)目標(biāo)點(diǎn)的最優(yōu)路徑過程中，規(guī)劃的全局路徑存在多余節(jié)點(diǎn)，運(yùn)動(dòng)軌跡折線較多，在路徑節(jié)點(diǎn)處存在轉(zhuǎn)折角度過大以致于不符合移動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)原理等缺陷，為此提出一種改進(jìn)的A*算法。

1.2 改進(jìn)7×7的A＊算法

傳統(tǒng)A*算法搜索擴(kuò)展節(jié)點(diǎn)采用3×3 鄰域算法，如圖1 所示。任意搜索方向之間夾角均為0.25π，節(jié)點(diǎn)轉(zhuǎn)折角均為0.25π 的倍數(shù)，因此傳統(tǒng)A*算法的規(guī)劃路徑不是理論最短，而且規(guī)劃路徑上因?yàn)檗D(zhuǎn)折點(diǎn)較多、轉(zhuǎn)折角過大不夠平滑。

圖1 傳統(tǒng)3×3的A*算法鄰域搜索圖

針對傳統(tǒng)A*算法3×3 的鄰域搜索缺陷，提出擴(kuò)展A*算法搜索鄰域。以圖1中7×7正方柵格圖為例，當(dāng)前節(jié)點(diǎn)n位于中心節(jié)點(diǎn)，其周圍第一層（3×3 的鄰域）中8個(gè)節(jié)點(diǎn)是傳統(tǒng)A*算法待擴(kuò)展的節(jié)點(diǎn)。改進(jìn)算法除了采用3×3鄰域搜索算法外，還采用當(dāng)前節(jié)點(diǎn)n周圍第二層即5×5 的搜索鄰域，共計(jì)16 個(gè)節(jié)點(diǎn)納入算法待擴(kuò)展節(jié)點(diǎn)，算法待搜索鄰域節(jié)點(diǎn)數(shù)即為24個(gè)，節(jié)點(diǎn)移動(dòng)方向是16個(gè)方向，改進(jìn)之處在于去除同方向的多余子節(jié)點(diǎn)，則改進(jìn)5×5的A*算法待搜索節(jié)點(diǎn)個(gè)數(shù)減少為16個(gè)。這樣既擴(kuò)大了搜索鄰域，優(yōu)化了搜索角度，又提高了算法效率，如圖2 所示。當(dāng)A*算法的擴(kuò)展搜索鄰域到達(dá)第三層，如圖3所示為7×7的搜索鄰域，去除同方向的多余子節(jié)點(diǎn)后，待搜索鄰域個(gè)數(shù)為32，可移動(dòng)方向?yàn)?2，則將傳統(tǒng)A*算法成功改進(jìn)為7×7的A*算法。

圖2 改進(jìn)5×5的A*算法鄰域搜索示意圖

圖3 改進(jìn)7×7的A*算法鄰域搜索示意圖

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

傳統(tǒng)DWA 算法進(jìn)行規(guī)劃時(shí)，缺少全局規(guī)劃路徑的指引，只有目的地位置方向的指引，而中間具體路徑是對環(huán)境中的障礙物及時(shí)避障實(shí)時(shí)規(guī)劃的路徑[15]。然而，在障礙物較多情況下，移動(dòng)過程容易陷入局部最優(yōu)，導(dǎo)致全局路徑距離變大。將改進(jìn)的7×7的A*算法與動(dòng)態(tài)窗口算法進(jìn)行融合，在局部路徑規(guī)劃時(shí)，機(jī)器人對臨時(shí)設(shè)置的未知?jiǎng)討B(tài)障礙物成功規(guī)避，安全高效地到達(dá)目標(biāo)點(diǎn)位置，完成了全局路徑規(guī)劃。

動(dòng)態(tài)窗口法，是基于機(jī)器人運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)的一種局部路徑規(guī)劃算法，將路徑規(guī)劃問題轉(zhuǎn)化成為速度矢量空間上的約束優(yōu)化問題。動(dòng)態(tài)窗口法的目的是：在速度二維空間中采樣多組速度（線速度、角速度），在一定時(shí)間間隔內(nèi)，同時(shí)模擬移動(dòng)機(jī)器人在這些速度下的軌跡。獲取多組可行軌跡后，根據(jù)設(shè)計(jì)的評(píng)價(jià)函數(shù)，選取最優(yōu)軌跡所對應(yīng)的速度（線速度、角速度），驅(qū)動(dòng)機(jī)器人完成局部路徑規(guī)劃。

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

動(dòng)態(tài)窗口算法針對窗口區(qū)域內(nèi)移動(dòng)機(jī)器人的速度進(jìn)行空間采樣，并且模擬出移動(dòng)機(jī)器人的運(yùn)動(dòng)軌跡。機(jī)器人的運(yùn)動(dòng)狀態(tài)由機(jī)器人的線速度和角速度來反饋，(vt,wt)表示軌跡，通過評(píng)價(jià)函數(shù)在所有可行軌跡里選取最佳軌跡，在時(shí)間間隔Δt內(nèi)，假設(shè)機(jī)器人作勻速直線運(yùn)動(dòng)，運(yùn)動(dòng)學(xué)模型為：

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

在速度空間中存在無窮多組(v,ω)，要依據(jù)機(jī)器人的實(shí)際情況對采樣速度的范圍進(jìn)行約束。

（1）機(jī)器人的速度約束為：

（2）機(jī)器人電機(jī)加減速約束。動(dòng)態(tài)窗口移動(dòng)時(shí)間間隔內(nèi)，機(jī)器人加速度所帶來的最大、最小速度為：

式中，vc、wc代表當(dāng)前速度；va、wa代表機(jī)器人最大加速度；vb、wb代表機(jī)器人最大減速度。

機(jī)器人制動(dòng)距離約束。在局部路徑規(guī)劃時(shí)為了確保機(jī)器人安全，在最大減速度條件要求下，機(jī)器人在撞擊障礙物之前速度減為0，則：

式中，dist(v,w)是(v,w)對應(yīng)軌跡距離障礙物的最近距離。

2.3 機(jī)器人評(píng)價(jià)函數(shù)

速度空間存在若干組的采樣速度是可行的，于是設(shè)計(jì)評(píng)價(jià)函數(shù)以便選取機(jī)器人最優(yōu)軌跡。評(píng)價(jià)函數(shù)設(shè)計(jì)準(zhǔn)則為局部路徑規(guī)劃，機(jī)器人應(yīng)盡量避開障礙物，用最短時(shí)間到達(dá)目標(biāo)點(diǎn)位置。設(shè)計(jì)的評(píng)價(jià)函數(shù)為：

式中，方位角評(píng)價(jià)函數(shù)head(v,w)，代表了當(dāng)前速度下模擬軌跡終點(diǎn)位置方向與目標(biāo)位置之間的方位角偏差；軌跡與靜態(tài)障礙物的最近距離為dist(v,w),vel(v,w)為當(dāng)前模擬速度大小的評(píng)價(jià)函數(shù)；σ為平滑系數(shù)；α、β、γ為3項(xiàng)的加權(quán)系數(shù)。

3 融合算法

傳統(tǒng)DWA 算法進(jìn)行規(guī)劃時(shí)，缺少全局規(guī)劃路徑的指引，只有目的地位置方向的指引，故采用改進(jìn)7×7 A*算法進(jìn)行全局路徑規(guī)劃，融合動(dòng)態(tài)窗口法進(jìn)行動(dòng)態(tài)避障，提升動(dòng)態(tài)規(guī)劃路徑的全局最優(yōu)性。

為了提升實(shí)時(shí)避障在路徑轉(zhuǎn)彎處的能力，提高路徑平滑性，在避障過程中，需要實(shí)時(shí)預(yù)估路徑的彎曲程度，遇到急彎時(shí)提前減速。為每個(gè)子目標(biāo)點(diǎn)Gi添加一個(gè)參數(shù)Δθ表示全局路徑在該子目標(biāo)點(diǎn)的彎曲程度，Δθ是Gi分別與前后兩個(gè)相鄰子目標(biāo)點(diǎn)的夾角。當(dāng)Δθ接近π時(shí)，相鄰兩個(gè)子目標(biāo)點(diǎn)連線的方向近乎平行，機(jī)器人以較高的速度運(yùn)行，當(dāng)Δθ接近直角時(shí)，路徑的彎曲程度較大，機(jī)器人應(yīng)降低速度規(guī)劃平滑路徑準(zhǔn)備轉(zhuǎn)彎。當(dāng)運(yùn)動(dòng)軌跡近乎直線時(shí)，機(jī)器人線速度較大，當(dāng)路徑需要避障轉(zhuǎn)彎時(shí)，線速度降低，增加路徑平滑度。

為避免動(dòng)態(tài)窗口算法陷入局部最優(yōu)，設(shè)計(jì)了一種全局最優(yōu)路徑的動(dòng)態(tài)窗口評(píng)價(jià)函數(shù)：

式中，Zhead(v,w,Gi)為模擬軌跡終點(diǎn)方向與子目標(biāo)點(diǎn)之間的方位角偏差。Δθi是Gi分別與前后兩個(gè)相鄰子目標(biāo)點(diǎn)的夾角，子目標(biāo)點(diǎn)Gi是機(jī)器人靜態(tài)規(guī)劃下的前進(jìn)方向上距離當(dāng)前點(diǎn)最近的靜態(tài)環(huán)境下的全局最優(yōu)路徑點(diǎn)。此評(píng)價(jià)函數(shù)使得局部路徑規(guī)劃遵循全局路徑規(guī)劃路徑序列點(diǎn)，從而保證全局路徑最佳。

3.1 完成全局規(guī)劃和臨時(shí)動(dòng)態(tài)避障

為驗(yàn)證所提融合算法在復(fù)雜環(huán)境中進(jìn)行動(dòng)態(tài)路徑規(guī)劃的有效性，在所建的柵格圖環(huán)境模型中，首先標(biāo)定目標(biāo)點(diǎn)的位置和機(jī)器人起始點(diǎn)的位置，應(yīng)用算法進(jìn)行仿真驗(yàn)證，圖4柵格地圖環(huán)境中可以看出機(jī)器人避開所有靜態(tài)障礙物到達(dá)目標(biāo)點(diǎn)位置，完成了靜態(tài)環(huán)境下路徑規(guī)劃。

圖4 融合算法靜態(tài)環(huán)境路徑規(guī)劃

為了驗(yàn)證動(dòng)態(tài)環(huán)境下的路徑規(guī)劃，在機(jī)器人規(guī)劃好的路徑上臨時(shí)設(shè)置動(dòng)態(tài)障礙，紅色的方塊即為設(shè)置的動(dòng)態(tài)障礙物。圖5中的運(yùn)動(dòng)軌跡是機(jī)器人規(guī)劃好的路徑，在路徑上放置障礙物，綠色的箭頭為機(jī)器人的運(yùn)動(dòng)方向。圖6 為移動(dòng)機(jī)器人初始到結(jié)束動(dòng)態(tài)路徑規(guī)劃的過程圖，顯示了移動(dòng)機(jī)器人成功地避開動(dòng)態(tài)障礙物到達(dá)目標(biāo)點(diǎn)位置。從圖5、6 可以看出，改進(jìn)的融合算法，可以實(shí)現(xiàn)局部路徑規(guī)劃，躲避動(dòng)態(tài)障礙物，還能得到平滑而且安全的規(guī)劃路徑。

圖5 臨時(shí)設(shè)置動(dòng)態(tài)障礙物

3.2 輸出運(yùn)動(dòng)參數(shù)

所改進(jìn)的融合算法可實(shí)時(shí)輸出機(jī)器人的控制參數(shù)，有利于機(jī)器人的自動(dòng)反饋控制，機(jī)器人線速度的變化、機(jī)器人姿態(tài)（弧度）的變化、機(jī)器人角速度的變化結(jié)果如圖7 所示。圖7 反映了機(jī)器人動(dòng)態(tài)路徑規(guī)劃時(shí)，控制參數(shù)的變化，當(dāng)機(jī)器人局部路徑躲避障礙物時(shí)，線速度減小，弧度也減小。

3.3 驗(yàn)證算法高效性和適用性，對比其他算法

圖6 動(dòng)態(tài)避障路徑規(guī)劃圖

圖7 機(jī)器人的控制參數(shù)變化圖

為了驗(yàn)證基于改進(jìn)7×7的A*算法與動(dòng)態(tài)窗口法的融合算法的高效性，與標(biāo)準(zhǔn)A*算法、改進(jìn)7×7的A*算法在相同的仿真環(huán)境中作出對比，仿真實(shí)驗(yàn)結(jié)果如圖8所示。每組重復(fù)仿真10 次，取平均值記錄路徑規(guī)劃的時(shí)間和路徑長度，各種算法的性能指標(biāo)如表1所示。

圖8 三種算法全局路徑規(guī)劃仿真圖

表1 三種算法性能指標(biāo)的對比

由圖8、表1可知，三種算法都能夠在靜態(tài)環(huán)境下規(guī)劃出從起始點(diǎn)到目標(biāo)點(diǎn)的路徑。傳統(tǒng)A*算法路徑轉(zhuǎn)折角多，轉(zhuǎn)折角度大，改進(jìn)的7×7的A*算法平滑性最好，路徑最短，用時(shí)最少。

將3×3的搜索鄰域擴(kuò)展為7×7后，同時(shí)去除擴(kuò)展鄰域同方向的多余子節(jié)點(diǎn)，實(shí)質(zhì)上搜索方向由8個(gè)增加到32個(gè)，優(yōu)化了搜索角度；從表中可以看出改進(jìn)7×7的A*算法距離為40.563 4 mm，時(shí)間為0.038 546 s。傳統(tǒng)A*算法路徑距離為40.870 1 mm，時(shí)間為0.060 966 s。改進(jìn)7×7的A*算法增加了搜索方向，并沒有因?yàn)樵黾铀惴ㄓ?jì)算量而延長路徑規(guī)劃時(shí)間，相反因?yàn)閮?yōu)化了搜索角度，取消了節(jié)點(diǎn)移動(dòng)方向僅為0.25π的整數(shù)倍的限制，提高了搜索效率，因此減少了路徑規(guī)劃時(shí)間；并且改進(jìn)7×7的A*算法優(yōu)化了全局路徑，減少了路徑長度，增加了路徑平滑性，可以表明改進(jìn)7×7 的A*算法比傳統(tǒng)A*算法更具有適用性。

相比傳統(tǒng)A*算法和改進(jìn)7×7 的A*算法，基于改進(jìn)7×7的A*算法與動(dòng)態(tài)窗口算法的融合算法，規(guī)劃路徑更短為38.612 2 mm，平滑性更好，路徑規(guī)劃更好。

4 結(jié)論

將傳統(tǒng)A*算法，擴(kuò)展其搜索鄰域同時(shí)去除了同方向的多余子節(jié)點(diǎn)，改進(jìn)為一種7×7的A*算法，有效地消除了傳統(tǒng)路徑轉(zhuǎn)折角多、轉(zhuǎn)折角度大的缺陷。為提升在動(dòng)態(tài)復(fù)雜環(huán)境下移動(dòng)機(jī)器人路徑規(guī)劃效率，提出一種基于改進(jìn)7×7的A*算法與動(dòng)態(tài)窗口法的融合算法，設(shè)計(jì)了一種全局最優(yōu)路徑的動(dòng)態(tài)窗口評(píng)價(jià)函數(shù)，采用動(dòng)態(tài)窗口算法實(shí)現(xiàn)路徑的局部規(guī)劃，成功規(guī)避了擋住已規(guī)劃好路徑上的動(dòng)態(tài)障礙物，機(jī)器人規(guī)劃出了從起始點(diǎn)到目標(biāo)點(diǎn)的最優(yōu)路徑。通過與多種算法仿真分析比較，結(jié)果表明了提出的融合算法具有可行性和高效性。