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

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

中圖分類號(hào)： TN820.4?34； TP242； TP18" " " " " " " "文獻(xiàn)標(biāo)識(shí)碼： A" " " " " " " "文章編號(hào)： 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" 引" 言

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

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

針對(duì)目前復(fù)雜環(huán)境中移動(dòng)機(jī)器人路徑規(guī)劃效率低、路徑轉(zhuǎn)折點(diǎn)多、路徑平滑度差、安全性差等問(wèn)題，本文提出一種融合改進(jìn)A*算法和動(dòng)態(tài)窗口法的路徑規(guī)劃算法，利用自適應(yīng)參數(shù)因子和安全半徑路徑優(yōu)劣評(píng)價(jià)函數(shù)對(duì)路徑進(jìn)行優(yōu)化。首先，通過(guò)改進(jìn)安全距離評(píng)價(jià)子函數(shù)的動(dòng)態(tài)窗口法進(jìn)行局部避障；接著，對(duì)改進(jìn)A*算法、傳統(tǒng)A*算法、跳點(diǎn)A*算法、文獻(xiàn)[19]算法、文獻(xiàn)[20]算法、融合算法進(jìn)行仿真實(shí)驗(yàn)對(duì)比，驗(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*算法通過(guò)路徑優(yōu)劣評(píng)價(jià)公式判斷父節(jié)點(diǎn)周圍擴(kuò)展節(jié)點(diǎn)代價(jià)值，代價(jià)值最小的作為下一個(gè)父節(jié)點(diǎn)，重復(fù)上述過(guò)程直到目標(biāo)點(diǎn)為父節(jié)點(diǎn)，結(jié)束A*算法的路徑搜索過(guò)程，生成從起始點(diǎn)到目標(biāo)點(diǎn)的路徑。傳統(tǒng)A*算法的路徑優(yōu)劣評(píng)價(jià)公式為：

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

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

常用的移動(dòng)代價(jià)計(jì)算方法有歐氏距離、切比雪夫距離、曼哈頓距離，可分別表示為：

[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）]分別為采用歐氏距離、切比雪夫距離、曼哈頓距離計(jì)算的兩點(diǎn)之間的估計(jì)代價(jià)值。

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

1.2.1" 路徑優(yōu)劣評(píng)價(jià)函數(shù)優(yōu)化

傳統(tǒng)A*算法的路徑優(yōu)劣評(píng)價(jià)函數(shù)是以恒定參數(shù)因子來(lái)平衡實(shí)際代價(jià)與估計(jì)代價(jià)，但在復(fù)雜障礙環(huán)境中，路徑優(yōu)劣評(píng)價(jià)函數(shù)還需要進(jìn)一步改進(jìn)，以提高路徑規(guī)劃的效率。本文在路徑優(yōu)劣評(píng)價(jià)函數(shù)中加入自適應(yīng)參數(shù)因子，通過(guò)計(jì)算搜索過(guò)程中遇到障礙物的概率動(dòng)態(tài)調(diào)整參數(shù)因子，以滿足移動(dòng)機(jī)器人在復(fù)雜環(huán)境下高效路徑規(guī)劃的應(yīng)用需求。優(yōu)化后的估計(jì)代價(jià)函數(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]分別為移動(dòng)機(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)之間無(wú)障礙節(jié)點(diǎn)數(shù)。

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

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

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

綜上，本文設(shè)計(jì)路徑優(yōu)劣評(píng)價(jià)函數(shù)如下：

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

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

在已知障礙物空間中，通過(guò)優(yōu)化路徑優(yōu)劣評(píng)價(jià)函數(shù)來(lái)評(píng)估A*算法能否保證路徑搜索效率，完成初始階段全局規(guī)劃的要求。但是這種算法得到的路徑轉(zhuǎn)折點(diǎn)數(shù)量過(guò)多，路徑平滑度較差，初步規(guī)劃出的路徑也過(guò)于復(fù)雜。在復(fù)雜環(huán)境中，傳統(tǒng)A*算法難以保證移動(dòng)機(jī)器人運(yùn)動(dòng)過(guò)程中的安全性，因此，本文采用一種冗余節(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]，其路徑存在多個(gè)節(jié)點(diǎn)。為解決上述問(wèn)題，本文對(duì)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]；若直線與障礙物相撞則跳過(guò)該非相鄰節(jié)點(diǎn)的檢測(cè)，對(duì)下一輪非相鄰節(jié)點(diǎn)繼續(xù)進(jìn)行檢測(cè)。

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

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

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

已知起始點(diǎn)S、轉(zhuǎn)折點(diǎn)N3、目標(biāo)點(diǎn)T坐標(biāo)分別為[m1，n1]、[m2，n2]、[m3，n3]。在移動(dòng)機(jī)器人轉(zhuǎn)彎時(shí)考慮安全因素不與障礙物發(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]°時(shí)，對(duì)路徑進(jìn)行平滑度優(yōu)化，否則跳過(guò)路徑平滑度優(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)。

綜上可得移動(dòng)機(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" 動(dòng)態(tài)窗口法

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

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

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

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

[Xk=pkvkT] （18）

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

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

[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]時(shí)刻時(shí)移動(dòng)機(jī)器人的位置坐標(biāo)；[λk]為移動(dòng)機(jī)器人在[k]時(shí)刻時(shí)運(yùn)動(dòng)方向的偏移角度；[v]、[w]分別為移動(dòng)機(jī)器人的線速度和角速度。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

3" 融合算法

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

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

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

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

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

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

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

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

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

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

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

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

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

計(jì)算機(jī)配置以及仿真實(shí)驗(yàn)平臺(tái)為： 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)境地圖來(lái)模擬全局路徑規(guī)劃。首先建立20×20柵格環(huán)境地圖，在相同地圖環(huán)境中對(duì)傳統(tǒng)A*算法、文獻(xiàn)[19]算法、文獻(xiàn)[20]算法、本文改進(jìn)A*算法、圓弧處理法進(jìn)行對(duì)比仿真實(shí)驗(yàn)，結(jié)果如圖5所示。圖中五角星為目標(biāo)點(diǎn)，三角形為起始點(diǎn)，黑色代表障礙物，算法性能指標(biāo)對(duì)比如表1所示。

由結(jié)果可得，在20×20柵格地圖中，本文改進(jìn)A*算法的路徑總長(zhǎng)相較傳統(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%。說(shuō)明本文改進(jìn)A*算法提升了移動(dòng)機(jī)器人工作效率，在路徑長(zhǎng)度有一定的改善。

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

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

由表2實(shí)驗(yàn)結(jié)果可以得出：在20×20簡(jiǎn)單柵格環(huán)境地圖中，本文改進(jìn)A*算法的路徑總長(zhǎng)相較于傳統(tǒng)A*算法、跳點(diǎn)A*算法、無(wú)[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*算法的路徑總長(zhǎng)相較于傳統(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%。通過(guò)算法對(duì)比可以看出，改進(jìn)A*算法的安全性、轉(zhuǎn)折角度、平滑度得到明顯提升，且路徑更短。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

1） 在全局規(guī)劃中，通過(guò)在傳統(tǒng)A*算法的路徑優(yōu)劣評(píng)價(jià)函數(shù)中引入自適應(yīng)參數(shù)因子和安全半徑，對(duì)全局路徑進(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%的路徑長(zhǎng)度和55.55%的轉(zhuǎn)折角度；與傳統(tǒng)A*算法、跳點(diǎn)A*算法、無(wú)[ek]因子A*算法相比，改進(jìn)A*算法的安全性、轉(zhuǎn)折角度、平滑度得到明顯提升，且路徑更短。

2） 在局部規(guī)劃中，建立移動(dòng)機(jī)器人的運(yùn)動(dòng)學(xué)模型，采用改進(jìn)安全距離評(píng)價(jià)子函數(shù)的動(dòng)態(tài)窗口法，使得移動(dòng)機(jī)器人避開(kāi)未知障礙物的能力提升；通過(guò)將改進(jìn)A*算法和動(dòng)態(tài)窗口法融合，使得移動(dòng)機(jī)器人的動(dòng)態(tài)避障能力提升。經(jīng)實(shí)驗(yàn)驗(yàn)證，融合改進(jìn)動(dòng)態(tài)窗口法后，改進(jìn)A*算法具備了在復(fù)雜環(huán)境中找到最短無(wú)障礙路徑的能力，實(shí)現(xiàn)了移動(dòng)機(jī)器人實(shí)時(shí)避開(kāi)未知障礙，實(shí)現(xiàn)了無(wú)碰撞到達(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].電子測(cè)量技術(shù)，2019，42（12）：122?128.

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

[4] 趙曉，王錚，黃程侃，等.基于改進(jìn)A*算法的移動(dòng)機(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] 林依凡，陳彥杰，何炳蔚，等.無(wú)碰撞檢測(cè)RRT*的移動(dòng)機(jī)器人運(yùn)動(dòng)規(guī)劃方法[J].儀器儀表學(xué)報(bào)，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ì)算機(jī)測(cè)量與控制，2018，26（7）：282?286.

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

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

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

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

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

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

[18] 楊桂華，衛(wèi)嘉樂(lè).基于改進(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ì)算機(jī)應(yīng)用，2019，39（4）：1006?1011.

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

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

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

馬立東（1980—），男，河北遷安人，博士研究生，教授，博士生導(dǎo)師，主要研究方向?yàn)橹悄軝C(jī)器人系統(tǒng)、視覺(jué)檢測(cè)技術(shù)、先進(jìn)檢測(cè)技術(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)目：國(guó)家自然科學(xué)基金面上項(xiàng)目（52274389）；山西省重點(diǎn)研發(fā)計(jì)劃項(xiàng)目（202102010101003）；山西省重點(diǎn)研發(fā)計(jì)劃項(xiàng)目（202202150401010）；山西省科技合作交流專項(xiàng)項(xiàng)目（202104041101031）；山西省留學(xué)人員科技活動(dòng)擇優(yōu)資助項(xiàng)目（20220028）；

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