改進(jìn)A*和DWA的機(jī)器人路徑規(guī)劃研究

摘" 要： 針對復(fù)雜環(huán)境中移動機(jī)器人路徑規(guī)劃效率低、環(huán)境適應(yīng)能力差等問題，提出一種融合改進(jìn)A*算法和動態(tài)窗口法（DWA）的路徑規(guī)劃算法。首先，在全局規(guī)劃中采用自適應(yīng)參數(shù)因子和安全半徑路徑優(yōu)劣評價函數(shù)對冗余節(jié)點(diǎn)進(jìn)行優(yōu)化，提取關(guān)鍵節(jié)點(diǎn)，再使用圓弧處理法對已規(guī)劃路徑進(jìn)行平滑度優(yōu)化；然后，采用改進(jìn)安全距離評價子函數(shù)的動態(tài)窗口法進(jìn)行局部規(guī)劃，提升了移動機(jī)器人的避障性能；最后，對融合算法進(jìn)行仿真和實(shí)物實(shí)驗(yàn)。結(jié)果表明：改進(jìn)的A*算法在路徑長度和轉(zhuǎn)折角度方面均得到優(yōu)化；融合改進(jìn)動態(tài)窗口法后，移動機(jī)器人在復(fù)雜環(huán)境中能夠找到最短無障礙路徑，實(shí)時避開未知障礙，安全到達(dá)目標(biāo)點(diǎn)位，驗(yàn)證了該算法的有效性和實(shí)用性。

關(guān)鍵詞： 移動機(jī)器人； 路徑規(guī)劃； A*算法； 動態(tài)窗口法； 自適應(yīng)參數(shù)； 安全距離； 動態(tài)障礙物

中圖分類號： TN820.4?34； TP242； TP18" " " " " " " "文獻(xiàn)標(biāo)識碼： A" " " " " " " "文章編號： 1004?373X（2024）20?0177?10

Research on robot path planning based on improved A* and DWA

MA Ziyong1， 2， 3， ZHU Xingguang1， 2， MA Lidong1， 2

（1. School of Mechanical Engineering， Taiyuan University of Science and Technology， Taiyuan 030024， China;

2. Shanxi Provincial Key Laboratory of Intelligent Technology and System for Heavy?Duty Equipment Operation， Taiyuan 030024， China;

3. High?End Equipment and Rail Transit Technology Research and Development Center of Haian， Taiyuan University of Science and Technology， Haian 226600， China）

Abstract： In allusion to the problems of low efficiency and poor environmental adaptability of mobile robot path planning in complex environment， a path planning algorithm combining improved A* algorithm and dynamic window approach （DWA） is proposed. In the global planning stage， adaptive weighting factor and the safety radius path evaluation function are used to optimize the redundant node and extract the key nodes， and the arc processing method is used to optimize the smoothness of the planned path. The DWA with improved safety distance evaluation subfunction is used for local planning， which can improve the obstacle avoidance performance of the mobile robot. The fusion algorithm is simulated and tested. The results show that the improved A* algorithm is optimized in both of path length and turning angle. After integrating the improved DWA， the mobile robot can find the shortest obstacle?free path in complex environments， dynamically avoid unknown obstacles， and safely reach the target position. These findings validate the effectiveness and practicality of the proposed algorithm.

Keywords： mobile robot; path planning; A* algorithm; dynamic window approach; adaptive parameter; safe distance; dynamic obstacle

0" 引" 言

移動機(jī)器人是一種能夠在復(fù)雜環(huán)境中移動作業(yè)的智能機(jī)器人，具備自主規(guī)劃、自我組織和自適應(yīng)能力，在軍事、民用等領(lǐng)域的應(yīng)用越來越廣泛，尤其是在高危、復(fù)雜的工作環(huán)境下。路徑規(guī)劃是移動機(jī)器人自主導(dǎo)航控制系統(tǒng)的關(guān)鍵技術(shù)之一[1]，已成為影響移動機(jī)器人應(yīng)用的關(guān)鍵因素。它的主要目的是使移動機(jī)器人能夠在復(fù)雜的工作環(huán)境中尋找到一條從起始點(diǎn)到目標(biāo)點(diǎn)的最短路徑，并在運(yùn)動過程中及時避開未知障礙物[2]。路徑規(guī)劃一般分為全局路徑規(guī)劃和局部路徑規(guī)劃[3]，全局路徑規(guī)劃需要在當(dāng)前已知環(huán)境信息中規(guī)劃最優(yōu)安全路徑，其代表算法有A*算法[4]、蟻群優(yōu)化[5]、快速擴(kuò)展隨機(jī)樹算法（RRT）[6]和粒子群優(yōu)化[7]等；局部路徑規(guī)劃則是在未知、不確定的環(huán)境中，通過規(guī)劃局部路徑來避開障礙物，代表算法有動態(tài)窗口法（DWA）[8]、反演控制法（IK）[9]和人工勢場法（APF）[10]。

全局路徑規(guī)劃中，A*算法具有高效性、易于實(shí)現(xiàn)、應(yīng)用場景廣泛等優(yōu)點(diǎn)，受到廣大學(xué)者的關(guān)注。王小紅等基于A*算法，在24鄰域搜索的基礎(chǔ)上引入新的啟發(fā)式函數(shù)，提升了算法搜索效率，但該算法未對路徑長度進(jìn)行優(yōu)化且路徑轉(zhuǎn)折點(diǎn)多[11]。王中玉等通過改進(jìn)評價函數(shù)的計算方式和函數(shù)權(quán)重比例，以降低路徑搜索時間和全局路徑的長度。該算法忽略了路徑平滑度優(yōu)化，路徑轉(zhuǎn)彎角度過大[12]。劉子豪等通過跳躍點(diǎn)搜索策略增強(qiáng)A*算法的節(jié)點(diǎn)擴(kuò)展，顯著降低了內(nèi)存消耗和搜索規(guī)模，然而未考慮移動機(jī)器人的自身體積[13]。上述研究通過改進(jìn)評價函數(shù)來提高路徑規(guī)劃的效率，但是在復(fù)雜環(huán)境充滿不確定性的情況下，需借助局部路徑規(guī)劃確保移動機(jī)器人作業(yè)的安全性。DWA算法具有高效、實(shí)時避障和可擴(kuò)展性強(qiáng)的優(yōu)勢。王永雄等提出一種自適應(yīng)環(huán)境動態(tài)變化調(diào)整目標(biāo)函數(shù)的權(quán)值，來提升路徑的平滑性與移動機(jī)器人運(yùn)動的安全性，雖然該算法能有效地局部避障，但是不符合全局最優(yōu)路徑[14]。卞永明等針對無法避開“C”型障礙物的問題，通過在軌跡評價函數(shù)中引入轉(zhuǎn)點(diǎn)函數(shù)，提升了移動機(jī)器人的運(yùn)動效率，但是在復(fù)雜環(huán)境中容易陷入局部最優(yōu)[15]。文獻(xiàn)[16?18]針對不同的應(yīng)用場景，將A*算法與DWA算法進(jìn)行了融合，都達(dá)到了較好的避障效果。

針對目前復(fù)雜環(huán)境中移動機(jī)器人路徑規(guī)劃效率低、路徑轉(zhuǎn)折點(diǎn)多、路徑平滑度差、安全性差等問題，本文提出一種融合改進(jìn)A*算法和動態(tài)窗口法的路徑規(guī)劃算法，利用自適應(yīng)參數(shù)因子和安全半徑路徑優(yōu)劣評價函數(shù)對路徑進(jìn)行優(yōu)化。首先，通過改進(jìn)安全距離評價子函數(shù)的動態(tài)窗口法進(jìn)行局部避障；接著，對改進(jìn)A*算法、傳統(tǒng)A*算法、跳點(diǎn)A*算法、文獻(xiàn)[19]算法、文獻(xiàn)[20]算法、融合算法進(jìn)行仿真實(shí)驗(yàn)對比，驗(yàn)證改進(jìn)A*算法的優(yōu)越性以及融合算法的避障能力；最后，在真實(shí)環(huán)境下建立柵格地圖，完成實(shí)物機(jī)器人自主導(dǎo)航避障任務(wù)，以驗(yàn)證算法的有效性和實(shí)用性。

1" 基于改進(jìn)A*算法的全局路徑規(guī)劃

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

A*算法是一種用于靜態(tài)路網(wǎng)中求解最短路徑的搜索算法，具有高效的直接搜索能力和智能的啟發(fā)式搜索指導(dǎo)。在已知障礙物、起始點(diǎn)、目標(biāo)點(diǎn)和地圖環(huán)境信息后，A*算法通過路徑優(yōu)劣評價公式判斷父節(jié)點(diǎn)周圍擴(kuò)展節(jié)點(diǎn)代價值，代價值最小的作為下一個父節(jié)點(diǎn)，重復(fù)上述過程直到目標(biāo)點(diǎn)為父節(jié)點(diǎn)，結(jié)束A*算法的路徑搜索過程，生成從起始點(diǎn)到目標(biāo)點(diǎn)的路徑。傳統(tǒng)A*算法的路徑優(yōu)劣評價公式為：

[f（n）=gn+hn] （1）

式中：[gn]為在狀態(tài)空間中從起始點(diǎn)到節(jié)點(diǎn)[n]的實(shí)際代價；[hn]為從節(jié)點(diǎn)[n]到目標(biāo)狀態(tài)的最佳路徑的估計代價；[fn]為從起始點(diǎn)經(jīng)節(jié)點(diǎn)[n]到目標(biāo)點(diǎn)的代價估計。

常用的移動代價計算方法有歐氏距離、切比雪夫距離、曼哈頓距離，可分別表示為：

[h1（n）=x1-x22+y1-y22] （2）

[h2（n）=max（x1-x2，y1-y2）] （3）

[h3（n）=x1-x2+y1-y2] （4）

式中：[x1，y1]和[x2，y2]分別為當(dāng)前節(jié)點(diǎn)和目標(biāo)節(jié)點(diǎn)的坐標(biāo)；[h1（n）]、[h2（n）]、[h3（n）]分別為采用歐氏距離、切比雪夫距離、曼哈頓距離計算的兩點(diǎn)之間的估計代價值。

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

1.2.1" 路徑優(yōu)劣評價函數(shù)優(yōu)化

傳統(tǒng)A*算法的路徑優(yōu)劣評價函數(shù)是以恒定參數(shù)因子來平衡實(shí)際代價與估計代價，但在復(fù)雜障礙環(huán)境中，路徑優(yōu)劣評價函數(shù)還需要進(jìn)一步改進(jìn)，以提高路徑規(guī)劃的效率。本文在路徑優(yōu)劣評價函數(shù)中加入自適應(yīng)參數(shù)因子，通過計算搜索過程中遇到障礙物的概率動態(tài)調(diào)整參數(shù)因子，以滿足移動機(jī)器人在復(fù)雜環(huán)境下高效路徑規(guī)劃的應(yīng)用需求。優(yōu)化后的估計代價函數(shù)[Hn]公式為：

[H（n）=ek·h1n] （5）

式中[k]為：

[k=i=xminxmaxj=yminymax1-Pi，jMx+My] （6）

[Pi，j=1，" " （i，j）為障礙物點(diǎn)0，" " 其他] （7）

式中：[ek]為自適應(yīng)參數(shù)因子；[Mx]、[My]分別為移動機(jī)器人沿x軸和y軸方向遍歷的節(jié)點(diǎn)數(shù)；[Pi，j]為二進(jìn)制指示變量，用于判斷位置[i，j]處是否為障礙物，是則為1，否則為0；[1-Pi，j]為起始節(jié)點(diǎn)與目標(biāo)節(jié)點(diǎn)之間無障礙節(jié)點(diǎn)數(shù)。

為保證機(jī)器人在運(yùn)動中的安全性，使規(guī)劃全局路徑與障礙物之間保持一個安全距離，本文引入一個安全半徑定義，增大轉(zhuǎn)彎半徑，公式如式（8）所示。若當(dāng)前節(jié)點(diǎn)與下一節(jié)點(diǎn)形成的路徑與障礙物之間的最小距離小于安全半徑，需要考慮該節(jié)點(diǎn)安全代價，反之則不考慮安全代價。安全半徑考慮了移動機(jī)器人的大小和周圍環(huán)境限制，能夠使移動機(jī)器人在運(yùn)動時避免發(fā)生安全事故。

[G（n）=i=1ndi+η·max0，r-min（di）] （8）

式中：[di]為從起點(diǎn)節(jié)點(diǎn)到當(dāng)前節(jié)點(diǎn)[i]的實(shí)際代價距離；[r]為移動機(jī)器人的安全半徑；[min（di）]為當(dāng)前節(jié)點(diǎn)與下個節(jié)點(diǎn)的路徑到距離最近障礙物的最小距離；[η]為安全代價權(quán)重。

綜上，本文設(shè)計路徑優(yōu)劣評價函數(shù)如下：

[Fn=G（n）+H（n）] （9）

1.2.2" 冗余節(jié)點(diǎn)優(yōu)化

在已知障礙物空間中，通過優(yōu)化路徑優(yōu)劣評價函數(shù)來評估A*算法能否保證路徑搜索效率，完成初始階段全局規(guī)劃的要求。但是這種算法得到的路徑轉(zhuǎn)折點(diǎn)數(shù)量過多，路徑平滑度較差，初步規(guī)劃出的路徑也過于復(fù)雜。在復(fù)雜環(huán)境中，傳統(tǒng)A*算法難以保證移動機(jī)器人運(yùn)動過程中的安全性，因此，本文采用一種冗余節(jié)點(diǎn)優(yōu)化方法。該方法能夠剔除所有冗余轉(zhuǎn)折點(diǎn)，得到更加平滑且安全的路徑。

沉余節(jié)點(diǎn)優(yōu)化方法如圖1所示，[NS]為起始點(diǎn)，[NT]為目標(biāo)點(diǎn)，傳統(tǒng)A*算法規(guī)劃的路徑為[NS，N1，N2，N3，N4，N5，N6，][N7，NT]，其路徑存在多個節(jié)點(diǎn)。為解決上述問題，本文對A*算法的冗余節(jié)點(diǎn)進(jìn)行優(yōu)化，優(yōu)化的基本思路為：在傳統(tǒng)A*算法規(guī)劃的路徑上，如果非相鄰節(jié)點(diǎn)之間的距離小于規(guī)劃節(jié)點(diǎn)間的距離、非相鄰節(jié)點(diǎn)路徑組成的直線不與障礙物相撞且該直線與障礙物的垂直距離大于等于安全半徑距離，則中間節(jié)點(diǎn)屬于冗余節(jié)點(diǎn)，可以刪除，只保存初始節(jié)點(diǎn)、中間拐點(diǎn)和目標(biāo)節(jié)點(diǎn)，保留路徑為[NS，N3，N8，N5，NT]，重復(fù)上述循環(huán)，最后形成新的優(yōu)化路徑為[NS，N3，NT]；若直線與障礙物相撞則跳過該非相鄰節(jié)點(diǎn)的檢測，對下一輪非相鄰節(jié)點(diǎn)繼續(xù)進(jìn)行檢測。

1.2.3" 路徑平滑度優(yōu)化

經(jīng)過圓弧處理后的優(yōu)化路徑如圖2所示，新優(yōu)化路徑為[S，N3，T]。其路徑長度、轉(zhuǎn)折點(diǎn)及安全性能都得到了提升，但依然存在平滑度較差的問題。這是因?yàn)橐苿訖C(jī)器人在運(yùn)動過程中轉(zhuǎn)向較大導(dǎo)致移動機(jī)器人漂移，不利于對移動機(jī)器人實(shí)際控制。為解決上述問題，對本文改進(jìn)A*算法規(guī)劃的路徑進(jìn)行平滑度優(yōu)化。常見的路徑平滑度優(yōu)化方法有貝塞爾曲線法、三次樣條曲線、B樣條曲線法和圓弧處理法等。在復(fù)雜環(huán)境中，通過對轉(zhuǎn)折點(diǎn)進(jìn)行圓弧處理，使得路徑長度減小且轉(zhuǎn)折度降低，滿足移動機(jī)器人的幾何特性。

路徑平滑度優(yōu)化具體步驟如下。

已知起始點(diǎn)S、轉(zhuǎn)折點(diǎn)N3、目標(biāo)點(diǎn)T坐標(biāo)分別為[m1，n1]、[m2，n2]、[m3，n3]。在移動機(jī)器人轉(zhuǎn)彎時考慮安全因素不與障礙物發(fā)生碰撞，在此設(shè)立圓弧AB的半徑AO、BO為[R]，由幾何模型可得以下關(guān)系：

[y1=k1x1-m1n2-n1m2m2-m1] （10）

[y2=k2x2-m3n2-n3m2m2-m3] （11）

[α=arctan k1] （12）

式中：[k1]為直線SN3的斜率；[k2]為直線N3T的斜率；[α]為線段SN3與水平面夾角。由勾股定理可得AN3的距離[lAN3]為：

[lAN3=Rtanβ2] （13）

式中：[β]為線段SN3與線段N3T夾角；[β=arctank2-k11+k1k2]。同理可得AS距離為：[lAS=m2-m12+n2-n12-lAN3] （14）

當(dāng)[lAD≥r]，[lBE≥r]且[βgt;90]°時，對路徑進(jìn)行平滑度優(yōu)化，否則跳過路徑平滑度優(yōu)化。聯(lián)立式（10）～式（12）可得：

[Ax=m1+cosα·lSAAy=n2-n1m2-m1Ax-m1n2-n1m2m2-m1] （15）

式中（[Ax]，[Ay]）為切點(diǎn)坐標(biāo)。同理可得切點(diǎn)B坐標(biāo)為（[Bx]，[By]），由幾何模型可得：

[Ox=Ax+sin α·ROy=Ax-cos α·R] （16）

式中（[Ox]，[Oy]）為圓心O的坐標(biāo)。

綜上可得移動機(jī)器人路徑優(yōu)化后的軌跡為：

[y=n2-n1m2-m1x-m1n2-n1m2m2-m1，" x≤AxR2-x-Ox2+Oy，" " " " " Axlt;xlt;Bxn3-n2m3-m2x-m2n3-n2m3m3-m2，" "x≥Bx] （17）

2" 動態(tài)窗口法

動態(tài)窗口法（DWA）在速度空間中使用基于采樣的方法為移動機(jī)器人生成模擬運(yùn)動軌跡，該算法考慮機(jī)器人的運(yùn)動學(xué)約束和環(huán)境信息來定義可行速度的“動態(tài)窗口”，從而在短時間范圍內(nèi)生成一組模擬軌跡。每個軌跡都使用一個目標(biāo)函數(shù)進(jìn)行評估，通過不斷重復(fù)這個過程，移動機(jī)器人可以在適應(yīng)環(huán)境變化的同時，規(guī)劃出一條通往目標(biāo)點(diǎn)的安全高效的路徑。

2.1" 建立移動機(jī)器人運(yùn)動學(xué)模型

DWA算法是依據(jù)移動機(jī)器人線速度、角速度、運(yùn)動方向等信息來確定其位置信息的算法。為了更好地研究該算法效果，需要分析移動機(jī)器人運(yùn)動模型，兩輪差速運(yùn)動學(xué)模型如圖3所示。

假設(shè)移動機(jī)器人軌跡為一段弧線時，此運(yùn)動方程為：

[Xk=pkvkT] （18）

[pkvk=pk-1⊕vk-1Δtkvk-1] （19）

式中：[pk]為在任意全局坐標(biāo)框架下的機(jī)器人姿態(tài)，由移動機(jī)器人坐標(biāo)和運(yùn)動方向的偏移角度決定；[vk]為速度向量，由[vk]與[vk-1]相等可以看出該運(yùn)動方程假設(shè)移動機(jī)器人為勻速；[Δtk]為[k-1]時刻到[k]時刻的時間增量；[vk-1Δtk]為在此時間段內(nèi)，移動機(jī)器人的軌跡估計。在二維情況下，對[pk]展開說明：

[xkykλk=xk-1+vxk-1Δtkcosλk-1-vyk-1Δtksinλk-1yk-1+vxk-1Δtksinλk-1-vyk-1Δtkcosλk-1λk-1+wλk-1Δtk] （20）

式中：（[xk]，[yk]）為在[k]時刻時移動機(jī)器人的位置坐標(biāo)；[λk]為移動機(jī)器人在[k]時刻時運(yùn)動方向的偏移角度；[v]、[w]分別為移動機(jī)器人的線速度和角速度。

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

移動機(jī)器人在運(yùn)動過程中躲避障礙物時的速度受多個因素影響，其中主要因素為環(huán)境和自身因素，因此把問題歸結(jié)為移動機(jī)器人在速度空間中約束的優(yōu)化問題。兩輪差速型移動機(jī)器人使用速度空間來搜索最佳速度。

1） 移動機(jī)器人自身速度約束為：

[vm={v，wv∈[vmin，vmax]，w∈[wmin，wmax]}] （21）

式中[vm]為移動機(jī)器人速度空間中線速度、角速度的集合。

2） 在移動機(jī)器人正常運(yùn)動中，受電機(jī)轉(zhuǎn)矩約束，由于加速度大小達(dá)不到理想狀態(tài)，使其最大速度受到約束。其約束方程為：

[vd=v，wv∈vc-vbΔt，vc+vaΔt，]

[w∈wc-wbΔt，wc+waΔt] （22）

式中：[vd]為移動機(jī)器人在實(shí)際運(yùn)動過程中能達(dá)到的速度范圍；[vc]、[wc]分別為當(dāng)前移動機(jī)器人線速度、角速度；[va]、[wa]分別為最大線加速度、角加速度；[vb]、[wb]分別為最大線減速度、角減速度。

3） 為保證移動機(jī)器人在運(yùn)動過程中能夠有效躲避障礙，即移動機(jī)器人在快要與障礙物碰撞時速度降為0，對移動機(jī)器人安全制動距離進(jìn)行約束，公式如下：

[va=v，wv≤2?distv，w?vb12，]

[w≤2?distv，w?wb12] （23）

式中[distv，w]為移動機(jī)器人模擬軌跡和最近障礙物之間的距離。

綜上所述，移動機(jī)器人速度空間要符合這三個約束條件，速度大小范圍可表示為：

[vr=vm?vd?va] （24）

2.3" 評價函數(shù)

移動機(jī)器人線速度和角速度在與其運(yùn)動學(xué)模型結(jié)合后，其速度采樣在速度空間內(nèi)模擬出無數(shù)個路徑。需要評價函數(shù)對每個路徑進(jìn)行評估，從中選擇得分最高路徑作為機(jī)器人行進(jìn)路徑；然后把對應(yīng)速度組合傳遞給移動機(jī)器人運(yùn)動控制系統(tǒng)，以實(shí)現(xiàn)機(jī)器人運(yùn)動。傳統(tǒng)的評價函數(shù)如下所示：

[Gv，w=α?headingv，w+β?distancev，w+" " " " " " " " " "γ?velocityv，w] （25）

式中：[headingv，w]為方位偏轉(zhuǎn)角評價子函數(shù)，表示移動機(jī)器人前進(jìn)方向與目標(biāo)點(diǎn)方向之間的角度差；[distancev，w]為距離評價子函數(shù)，表示移動機(jī)器人當(dāng)前位置與目標(biāo)位置之間的距離；[velocityv，w]為速度評價函數(shù)，表示移動機(jī)器人在當(dāng)前狀態(tài)下的速度；[α]、[β]、[γ]為權(quán)重系數(shù)，用于調(diào)整移動機(jī)器人運(yùn)動方向、距離和速度等因素對評估函數(shù)結(jié)果的影響。

3" 融合算法

在移動機(jī)器人所構(gòu)建的全局地圖中獲得完整環(huán)境和障礙物信息的情況下，改進(jìn)A*算法可以很好地進(jìn)行導(dǎo)航。然而，在復(fù)雜環(huán)境中可能會出現(xiàn)一些突發(fā)情況，例如遇到其他移動機(jī)器人或未知的障礙物等。如果不采取適當(dāng)措施，移動機(jī)器人很容易與障礙物發(fā)生碰撞。由于DWA算法是基于移動機(jī)器人當(dāng)前狀態(tài)下的局部最優(yōu)解來實(shí)時規(guī)劃路徑，因此沒有考慮全局最優(yōu)解。為實(shí)現(xiàn)移動機(jī)器人的實(shí)時避障且不使移動機(jī)器人陷入局部最優(yōu)的局面，本文將改進(jìn)A*算法與具有局部避障能力的DWA算法融合。為保證移動機(jī)器人在復(fù)雜環(huán)境中工作的安全性，本文改進(jìn)了DWA算法評價函數(shù)，公式如下：

[Gv，w=σα?headingv，w+β?Distv，w+]" " " " " " " " " " " " " " [ε?penalty+γ?velocityv，w] （26）

式中：[Distv，w+ε?penalty]為安全距離評價子函數(shù)，其中[Distv，w]為全局已知障礙物、未知動態(tài)障礙物及靜態(tài)障礙物與模擬路徑之間的最短距離；[ε?penalty]為安全懲罰函數(shù)，表示當(dāng)移動機(jī)器人與障礙物距離小于2倍安全半徑時對評價函數(shù)進(jìn)行懲罰，以避免移動機(jī)器人與障礙物發(fā)生碰撞；[σ]表示將函數(shù)結(jié)果歸一化處理，使路徑相對平滑。

通過在改進(jìn)A*算法的全局路徑上，融合改進(jìn)安全距離評價子函數(shù)的動態(tài)窗口法進(jìn)行局部路徑規(guī)劃，最終實(shí)現(xiàn)移動機(jī)器人動態(tài)避障，具體步驟如下。

步驟1：建立環(huán)境柵格地圖，設(shè)置移動機(jī)器人起始點(diǎn)和目標(biāo)點(diǎn)。

步驟2：利用改進(jìn)A*算法進(jìn)行全局路徑規(guī)劃，輸出最短無障礙路徑的關(guān)鍵節(jié)點(diǎn)。

步驟3：在全局地圖上設(shè)置未知靜態(tài)障礙物、未知動態(tài)障礙物。

步驟4：利用移動機(jī)器人攜帶的激光雷達(dá)等傳感器采集周圍環(huán)境信息，更新動態(tài)窗口內(nèi)部環(huán)境，判斷是否存在未知靜態(tài)和動態(tài)障礙物。

步驟5：如果沒有障礙物，繼續(xù)沿最短無障礙路徑行駛；如果有障礙物，進(jìn)行碰撞預(yù)測，實(shí)行避障措施。

步驟6：使用改進(jìn)安全距離評價子函數(shù)的動態(tài)窗口法進(jìn)行局部路徑規(guī)劃，以規(guī)避障礙物。移動機(jī)器人沿局部規(guī)劃路徑運(yùn)動，避障完成后返回最優(yōu)路徑行駛。

步驟7：判斷移動機(jī)器人是否到達(dá)目標(biāo)點(diǎn)，如果到達(dá)目標(biāo)點(diǎn)，則算法結(jié)束，否則執(zhí)行步驟4。

基于改進(jìn)A*算法和動態(tài)窗口法融合的移動機(jī)器人路徑規(guī)劃算法流程如圖4所示。

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

計算機(jī)配置以及仿真實(shí)驗(yàn)平臺為： Windows 11系統(tǒng)專業(yè)版，基于x64處理器Intel? CoreTM" i7?10700 CPU 2.90 GHz ，機(jī)帶RAM 16 GB，仿真軟件為Matlab 2018b。

4.1" 仿真實(shí)驗(yàn)與結(jié)果分析

4.1.1" 改進(jìn)A*算法路徑仿真分析

為驗(yàn)證本文改進(jìn)A*算法的優(yōu)越性，設(shè)置安全半徑為0.5，建立二維柵格環(huán)境地圖來模擬全局路徑規(guī)劃。首先建立20×20柵格環(huán)境地圖，在相同地圖環(huán)境中對傳統(tǒng)A*算法、文獻(xiàn)[19]算法、文獻(xiàn)[20]算法、本文改進(jìn)A*算法、圓弧處理法進(jìn)行對比仿真實(shí)驗(yàn)，結(jié)果如圖5所示。圖中五角星為目標(biāo)點(diǎn)，三角形為起始點(diǎn)，黑色代表障礙物，算法性能指標(biāo)對比如表1所示。

由結(jié)果可得，在20×20柵格地圖中，本文改進(jìn)A*算法的路徑總長相較傳統(tǒng)A*算法、文獻(xiàn)[19]算法、文獻(xiàn)[20]算法分別降低了0.8%、5.1%、1.54%，在轉(zhuǎn)折點(diǎn)數(shù)量方面分別降低了57.1%、70%、25%，在總轉(zhuǎn)折角度方面分別降低了53.4%、67.4%、43.7%。說明本文改進(jìn)A*算法提升了移動機(jī)器人工作效率，在路徑長度有一定的改善。

如表1所示，為了保證移動機(jī)器人在運(yùn)動過程中的流暢性，減少拐彎對電機(jī)的磨損程度，本文改進(jìn)A*算法在安全性能、轉(zhuǎn)折角度和轉(zhuǎn)折點(diǎn)數(shù)量方面相較傳統(tǒng)A*算法、文獻(xiàn)[19]算法、文獻(xiàn)[20]算法有一定提高。該算法可以根據(jù)移動機(jī)器人實(shí)際體積來調(diào)整安全半徑，使移動機(jī)器人在運(yùn)動過程中避免與障礙物發(fā)生碰撞。

為進(jìn)一步驗(yàn)證算法的穩(wěn)定性，分別建立20×20簡單柵格環(huán)境地圖和25×25一般柵格環(huán)境地圖，在相同地圖環(huán)境中對傳統(tǒng)A*算法、跳點(diǎn)A*算法、無[ek]因子A*算法、本文改進(jìn)A*算法、圓弧處理法進(jìn)行仿真實(shí)驗(yàn)，結(jié)果如圖6、圖7所示。圖a）為傳統(tǒng)A*算法生成的路徑，圖b）為跳點(diǎn)A*算法生成的路徑，圖c）為無[ek]因子A*算法生成的路徑，圖d）為本文改進(jìn)A*算法生成的路徑，圖e）為圓弧處理后生成的路徑。不同環(huán)境下算法性能指標(biāo)對比結(jié)果如表2所示。

由表2實(shí)驗(yàn)結(jié)果可以得出：在20×20簡單柵格環(huán)境地圖中，本文改進(jìn)A*算法的路徑總長相較于傳統(tǒng)A*算法、跳點(diǎn)A*算法、無[ek]因子A*算法分別降低1.04%、3.1%、5.31%，在轉(zhuǎn)折點(diǎn)數(shù)量方面分別降低62.5%、62.5%、50%，在總轉(zhuǎn)折角度方面分別降低64.7%、64.7%、55.9%；在25×25一般柵格環(huán)境地圖中，本文改進(jìn)A*算法的路徑總長相較于傳統(tǒng)A*算法、跳點(diǎn)A*算法分別降低2.53%、3.1%，在轉(zhuǎn)折點(diǎn)數(shù)量方面分別降低69.3%、63.6%，在總轉(zhuǎn)折角度方面分別降低68.3%、60.2%。通過算法對比可以看出，改進(jìn)A*算法的安全性、轉(zhuǎn)折角度、平滑度得到明顯提升，且路徑更短。

4.1.2" 融合算法路徑仿真分析

為了保證移動機(jī)器人在復(fù)雜環(huán)境下工作的安全性和可靠性，分別在25×25一般柵格環(huán)境地圖、30×30復(fù)雜柵格環(huán)境地圖中對融合算法進(jìn)行仿真實(shí)驗(yàn)，測試融合算法面對突發(fā)障礙物時的避障能力。

實(shí)驗(yàn)中，設(shè)定五角星為目標(biāo)點(diǎn)，三角形為起始點(diǎn)，黑色方塊為障礙物，灰色方塊為未知靜止障礙物，帶虛線的方塊為動態(tài)障礙物，實(shí)線為融合算法規(guī)劃的路徑，虛線為改進(jìn)后A*算法規(guī)劃的路徑。移動機(jī)器人初始線速度、角速度大小都為0，評價函數(shù)中參數(shù)為：[α]=0.05，[β]=0.1，[γ]=0.2，[σ]=0.4，[ε]=0.06。移動機(jī)器人運(yùn)動學(xué)參數(shù)設(shè)置如表3所示。

25×25一般柵格環(huán)境中融合算法的避障路徑規(guī)劃結(jié)果如圖8所示，灰色路徑直接穿過未知障礙物，而黑色路徑則可以重新規(guī)劃一條安全路徑，說明改進(jìn)后A*算法僅適用于全局規(guī)劃，但不能躲避未知障礙物；而本文提出的融合算法能夠按照全局最優(yōu)路徑軌跡運(yùn)動并避開未知障礙物。

30×30復(fù)雜柵格環(huán)境中融合算法的避障路徑規(guī)劃結(jié)果如圖9所示，當(dāng)移動機(jī)器人遇到障礙物時，完成局部避障后返回最優(yōu)全局規(guī)劃，說明融合算法路徑平滑性較好，且與全局規(guī)劃路徑重合度高，符合移動機(jī)器人的運(yùn)動特性。

無障礙物和存在障礙物時移動機(jī)器人姿態(tài)角度、線速度、角速度仿真結(jié)果分別如圖10、圖11所示。

如圖10a）、圖11a）所示，移動機(jī)器人的姿態(tài)角度發(fā)生變化，表明移動機(jī)器人在第40個控制節(jié)點(diǎn)遇到第1個動態(tài)障礙物，在第514個控制節(jié)點(diǎn)處遇到第1個靜態(tài)障礙物。如圖10b）、圖11b）所示，當(dāng)移動機(jī)器人接近障礙物時線速度穩(wěn)定在[0.4，0.48]，避障時速度較小，保證了移動機(jī)器人的安全，且符合障礙物附近減速原則；移動機(jī)器人角速度幅值穩(wěn)定在[0.3，-0.2]，表明在接近障礙物時，移動機(jī)器人角度調(diào)整較穩(wěn)定，抖動程度小，能夠順利完成避障任務(wù)。

4.2" 實(shí)物實(shí)驗(yàn)與結(jié)果分析

為驗(yàn)證本文融合算法在真實(shí)環(huán)境下動態(tài)避障的有效性和實(shí)用性，使用搭載RS?Lidar?16線激光雷達(dá)的Buffalo履帶移動機(jī)器人進(jìn)行實(shí)驗(yàn)。實(shí)驗(yàn)環(huán)境與設(shè)備如圖12所示，其中已知障礙物2個，未知障礙物1個。在機(jī)器人端和個人PC端預(yù)裝基于Ubuntu 20.04的ROS機(jī)器人操作系統(tǒng)——Noetic。

在實(shí)驗(yàn)環(huán)境下，通過個人計算機(jī)連接小車WiFi后建立ROS與小車的通信；然后在計算機(jī)端遠(yuǎn)程控制小車，啟動電機(jī)驅(qū)動節(jié)點(diǎn)、IMU節(jié)點(diǎn)、激光雷達(dá)節(jié)點(diǎn)和建圖節(jié)點(diǎn)，構(gòu)建如圖13所示的柵格地圖。

融合算法動態(tài)避障路徑如圖14所示。在相同的實(shí)驗(yàn)條件下，本文融合算法較原始融合算法的路徑轉(zhuǎn)折點(diǎn)數(shù)量降低40%，工作時間降低3.8%，說明融合算法的路徑平滑度、規(guī)劃效率得到了提升。lt;E：＼2023＼m20＼2024年20期＼Image＼28t12.tifgt;

如圖15所示，運(yùn)行本文融合算法的移動機(jī)器人在完成自主導(dǎo)航避障實(shí)驗(yàn)過程中，原始算法的線速度、角速度在140～170 s范圍內(nèi)出現(xiàn)7次劇烈波動，而本文融合算法在全過程中僅出現(xiàn)2次波動，表明應(yīng)用該算法時機(jī)器人的避障穩(wěn)定性更好。

表4對本文所述8種算法進(jìn)行比較，從表中可以看出，本文提出的融合改進(jìn)A*算法和動態(tài)窗口法（DWA）的路徑規(guī)劃算法同時考慮了全局最優(yōu)性和局部最優(yōu)性，能夠在未知環(huán)境中引導(dǎo)移動機(jī)器人安全穩(wěn)定地避開障礙物，安全到達(dá)目標(biāo)點(diǎn)位。

5" 結(jié)" 語

移動機(jī)器人是復(fù)雜環(huán)境中移動作業(yè)的重要設(shè)備，需要在運(yùn)動過程中具備規(guī)劃簡單平滑的運(yùn)動路徑和避開未知障礙物的能力。針對以上兩個需求，本文提出一種改進(jìn)A*算法和動態(tài)窗口法（DWA）融合的路徑規(guī)劃算法。

1） 在全局規(guī)劃中，通過在傳統(tǒng)A*算法的路徑優(yōu)劣評價函數(shù)中引入自適應(yīng)參數(shù)因子和安全半徑，對全局路徑進(jìn)行冗余節(jié)點(diǎn)優(yōu)化和圓弧處理。經(jīng)實(shí)驗(yàn)驗(yàn)證，本文改進(jìn)A*算法相較于其他文獻(xiàn)算法（文獻(xiàn)[19]、文獻(xiàn)[20]）平均減少了3.32%的路徑長度和55.55%的轉(zhuǎn)折角度；與傳統(tǒng)A*算法、跳點(diǎn)A*算法、無[ek]因子A*算法相比，改進(jìn)A*算法的安全性、轉(zhuǎn)折角度、平滑度得到明顯提升，且路徑更短。

2） 在局部規(guī)劃中，建立移動機(jī)器人的運(yùn)動學(xué)模型，采用改進(jìn)安全距離評價子函數(shù)的動態(tài)窗口法，使得移動機(jī)器人避開未知障礙物的能力提升；通過將改進(jìn)A*算法和動態(tài)窗口法融合，使得移動機(jī)器人的動態(tài)避障能力提升。經(jīng)實(shí)驗(yàn)驗(yàn)證，融合改進(jìn)動態(tài)窗口法后，改進(jìn)A*算法具備了在復(fù)雜環(huán)境中找到最短無障礙路徑的能力，實(shí)現(xiàn)了移動機(jī)器人實(shí)時避開未知障礙，實(shí)現(xiàn)了無碰撞到達(dá)目標(biāo)點(diǎn)位。

參考文獻(xiàn)

[1] TELEWECK P E， CHANDRASEKARAN B. Path planning algorithms and their use in robotic navigation systems [J]. Journal of physics： conference series， 2019， 1207（1）： 012018.

[2] 何壯壯，丁德銳.基于D?star和DWA的改進(jìn)機(jī)器人導(dǎo)航方法[J].電子測量技術(shù)，2019，42（12）：122?128.

[3] 劉公緒.共融機(jī)器人導(dǎo)航技術(shù)綜述[J].無線電工程，2020，50（12）：1007?1015.

[4] 趙曉，王錚，黃程侃，等.基于改進(jìn)A*算法的移動機(jī)器人路徑規(guī)劃[J].機(jī)器人，2018，40（6）：903?910.

[5]" WU L， HUANG X， CUI J， et al. Modified adaptive ant colony optimization algorithm and its application for solving path planning of mobile robot [J]. Expert systems with applications， 2023， 215： 119410.

[6] 林依凡，陳彥杰，何炳蔚，等.無碰撞檢測RRT*的移動機(jī)器人運(yùn)動規(guī)劃方法[J].儀器儀表學(xué)報，2020，41（10）：257?267.

[7] BLINDHEIM S， JOHANSEN T A. Particle swarm optimization for dynamic risk?aware path following for autonomous ships [J]. IFAC， 2022， 55（31）： 70?77.

[8] YANG W， WU P， ZHOU X， et al. Improved artificial potential field and dynamic window method for amphibious robot fish path planning [J]. Applied sciences， 2021， 11（5）： 2114.

[9] DUPAC M. Mathematical modeling and simulation of the inverse kinematic of a redundant robotic manipulator using azimuthal angles and spherical polar piecewise interpolation [J]. Mathematics and computers in simulation， 2023， 209： 282?298.

[10] ZHU Z， YIN Y， Lü H. Automatic collision avoidance algorithm based on route?plan?guided artificial potential field method [J]. Ocean engineering， 2023， 271： 113737.

[11] 王小紅，葉濤.基于改進(jìn)A*算法機(jī)器人路徑規(guī)劃研究[J].計算機(jī)測量與控制，2018，26（7）：282?286.

[12] 王中玉，曾國輝，黃勃，等.改進(jìn)A*算法的機(jī)器人全局最優(yōu)路徑規(guī)劃[J].計算機(jī)應(yīng)用，2019，39（9）：2517?2522.

[13] 劉子豪，趙津，劉暢，等.基于改進(jìn)A*算法室內(nèi)移動機(jī)器人路徑規(guī)劃[J].計算機(jī)工程與應(yīng)用，2021，57（2）：186?190.

[14] 王永雄，田永永，李璇，等.穿越稠密障礙物的自適應(yīng)動態(tài)窗口法[J].控制與決策，2019，34（5）：927?936.

[15] 卞永明，季鵬成，周怡和，等.基于改進(jìn)型DWA的移動機(jī)器人避障路徑規(guī)劃[J].中國工程機(jī)械學(xué)報，2021，19（1）：44?49.

[16] 王子靜，陳熙源.基于改進(jìn)A*和DWA的無人艇路徑規(guī)劃算法[J].傳感技術(shù)學(xué)報，2021，34（2）：249?254.

[17] 勞彩蓮，李鵬，馮宇.基于改進(jìn)A*與DWA算法融合的溫室機(jī)器人路徑規(guī)劃[J].農(nóng)業(yè)機(jī)械學(xué)報，2021，52（1）：14?22.

[18] 楊桂華，衛(wèi)嘉樂.基于改進(jìn)A*與DWA算法的物流機(jī)器人路徑規(guī)劃[J].科學(xué)技術(shù)與工程，2022，22（34）：15213?15220.

[19] 陳若男，文聰聰，彭玲，等.改進(jìn)A*算法在機(jī)器人室內(nèi)路徑規(guī)劃中的應(yīng)用[J].計算機(jī)應(yīng)用，2019，39（4）：1006?1011.

[20] 張振，張華良，鄧永勝，等.融合改進(jìn)A～*算法與DWA算法的機(jī)器人實(shí)時路徑規(guī)劃[J].無線電工程，2022，52（11）：1984?1993.

作者簡介：馬自勇（1987—），男，四川南部人，博士研究生，副教授，碩士生導(dǎo)師，主要研究方向?yàn)橄冗M(jìn)檢測技術(shù)、齒類零件近凈成形技術(shù)。

朱星光（1999—），男，河南平頂山人，在讀碩士研究生，主要研究方向?yàn)橐苿訖C(jī)器人建圖與導(dǎo)航、激光SLAM。

馬立東（1980—），男，河北遷安人，博士研究生，教授，博士生導(dǎo)師，主要研究方向?yàn)橹悄軝C(jī)器人系統(tǒng)、視覺檢測技術(shù)、先進(jìn)檢測技術(shù)、智能化彎曲與矯直工藝及裝備。

DOI：10.16652/j.issn.1004?373x.2024.20.028

引用格式：馬自勇，朱星光，馬立東.改進(jìn)A*和DWA的機(jī)器人路徑規(guī)劃研究[J].現(xiàn)代電子技術(shù)，2024，47（20）：177?186.

收稿日期：2024?03?02" " " " " "修回日期：2024?04?05

基金項(xiàng)目：國家自然科學(xué)基金面上項(xiàng)目（52274389）；山西省重點(diǎn)研發(fā)計劃項(xiàng)目（202102010101003）；山西省重點(diǎn)研發(fā)計劃項(xiàng)目（202202150401010）；山西省科技合作交流專項(xiàng)項(xiàng)目（202104041101031）；山西省留學(xué)人員科技活動擇優(yōu)資助項(xiàng)目（20220028）；

山西省省籌資助留學(xué)回國人員項(xiàng)目（2022?160）