陳家寶 文家燕 謝廣明

摘? 要：在移動機器人路徑規(guī)劃中，針對傳統(tǒng)A*算法規(guī)劃的路徑搜索時間長與不平滑的問題，提出一種改進的A*算法。首先，在傳統(tǒng)A*算法的基礎(chǔ)上，混合使用4鄰域和8鄰域搜索算法，并建立切換機制，改變搜索范圍，提高路徑搜索精度，減少了路徑轉(zhuǎn)折點。其次，通過對啟發(fā)式函數(shù)引入權(quán)重，優(yōu)化估價函數(shù)，縮短了路徑規(guī)劃時間。最后，對A*算法規(guī)劃的路徑使用3次均勻B樣條曲線進行平滑處理，消除路徑拐角尖點，在保證路徑最優(yōu)的情況下，提高路徑平滑性。仿真結(jié)果表明，改進后的A*算法所規(guī)劃的路徑在長度、平滑性、搜索消耗時間等特性上都優(yōu)于傳統(tǒng)算法。

關(guān)鍵詞：移動機器人;路徑規(guī)劃;A*算法;B樣條曲線

中圖分類號：TP242? ? ? ? ? ?DOI：10.16375/j.cnki.cn45-1395/t.2022.01.012

0? ? 引言

近年來，國家高度重視和支持機器人領(lǐng)域的技術(shù)創(chuàng)新，隨著利好政策不斷出臺，我國機器人產(chǎn)業(yè)正在進入飛速發(fā)展階段[1]。在移動機器人領(lǐng)域，路徑規(guī)劃是指在有障礙物的工作環(huán)境中尋找一條由起始位置到目標位置的無碰撞路徑，使機器人可以準確無誤地到達工作崗位，這是研究的重點和難點[2-3]。

國內(nèi)外研究學者針對路徑規(guī)劃問題提出了許多有效的算法，目前主要采用粒子群算法[4]、人工勢場法[5]、A*算法[6]、Dijkstra算法[7]、蟻群算法[8]等。其中A*算法是一種啟發(fā)式搜索算法，具有簡單易操作的特點，廣泛應(yīng)用于路徑規(guī)劃問題中[9]。但在實際應(yīng)用中發(fā)現(xiàn)，規(guī)劃的路徑存在轉(zhuǎn)彎折點較多、路徑搜索不精確、不平滑、遍歷節(jié)點數(shù)較多等? ? ?缺點。

為獲得更優(yōu)的路徑，研究者們進行了許多改進。文獻[10]提出使用跳點搜索算法來改進A*算法，有效減少列表中無用節(jié)點數(shù)量，但所規(guī)劃的路徑轉(zhuǎn)折直角過多，不利于移動機器人行駛。文獻[11]提出改進雙向A*算法，使起點和目標點同時運行A*算法，用來減少尋路時間，但當起點和目標點中間存在障礙物時，會產(chǎn)生并行搜索區(qū)域，沒能減少時間反而增加了尋路時間。文獻[12]引入模擬退火法來優(yōu)化A*算法，擴展節(jié)點選擇方式，但在復(fù)雜的障礙物環(huán)境下將會產(chǎn)生大量無用的擴展節(jié)點，增加了時間。文獻[13]通過擴大A*算法的搜索鄰域為24鄰域，增加了路徑選擇方向，但應(yīng)用到障礙物復(fù)雜的環(huán)境下，會產(chǎn)生忽略障礙物的情況，導(dǎo)致路徑搜索不精確。

由以上研究可知，A*算法仍然存在路徑搜索不精確、轉(zhuǎn)折點多、不平滑等問題。本文建立了比較復(fù)雜的障礙物環(huán)境地圖，提出結(jié)合4鄰域和8鄰域搜索算法的A*算法，同時對啟發(fā)式函數(shù)引入權(quán)重，對路徑進行3次均勻B樣條曲線平滑處理。解決了路徑搜索不精確問題，在保證路徑長度最短且平滑的情況下，減少尋路時間，獲取一條最優(yōu)路徑。通過仿真試驗，驗證了所提算法的可行性與有效性。

1? ? A*算法

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

A*算法是一種啟發(fā)式搜索算法，是求解最優(yōu)路徑的有效方法[14]。它通過估價函數(shù)[f（n）]來引導(dǎo)搜索方向，從而尋找出最優(yōu)路徑。其估價函數(shù)為：

[f（n）=g（n）+h（n）]，? ? ? ? ? ? ? ? ? （1）

式中：[n]代表當前節(jié)點，[f（n）]是當前節(jié)點[n]的估價函數(shù)，[g（n）]是移動機器人從起始節(jié)點到達當前節(jié)點[n]的實際代價值，[h（n）]是從當前節(jié)點[n]到達目標節(jié)點的代價估計值，通常采用歐氏距離或曼哈頓距離表示。歐氏距離指2個節(jié)點之間的直線距離，曼哈頓距離指2個節(jié)點坐標之間橫軸和縱軸絕對值之和。由于機器人在柵格環(huán)境地圖中不能全向移動，所以在考慮實際應(yīng)用情況下，本文采用曼哈頓距離來表示，公式如下：

[h（n）=xn-xg+yn-yg]，? ? ? ? ? ? ? ?（2）

式中：[（xn， yn）]表示路徑當前節(jié)點所在柵格中心的位置坐標，[（xg， yg）]表示路徑目標節(jié)點所在柵格中心的位置坐標。

算法流程如圖1所示。

Step? 1? ? ?構(gòu)建環(huán)境地圖。

Step 2? ? 設(shè)定2個列表：CLOSED列表和OPEN列表，其中CLOSED列表用于存放已擴展節(jié)點，OPEN列表用于存放待擴展節(jié)點。初始化CLOSED列表和OPEN列表為空，將起始節(jié)點S放入OPEN列表中。

Step 3? ? 在OPEN列表中搜索[f（n）]代價最小的節(jié)點[n]，將該節(jié)點放入CLOSED列表中，并從OPEN列表中移除該節(jié)點。

Step 4? ? 判斷CLOSED列表中的節(jié)點[n]是否為目標節(jié)點G，如果是，則利用回溯算法，求解得出最短路徑，結(jié)束算法;如果不是，則以該節(jié)點為父節(jié)點向4個方向繼續(xù)進行擴展搜索，生成對應(yīng)方向的子節(jié)點[m]。

Step 5? ? 判斷子節(jié)點[m]的位置在地圖中是否有障礙物，如果是，則刪除該節(jié)點;如果不是，則對其進行下一步判斷。

Step 6? ? 判斷該節(jié)點是否在OPEN列表中，如果是，則需要計算該節(jié)點的[f（n）]，并選取最小代價的節(jié)點并保留;如果不是，則將該節(jié)點放入OPEN列表中，再計算代價。

Step 7? ? 返回Step 3，繼續(xù)循環(huán)以上步驟搜索，直至找到目標節(jié)點G，得出最短路徑，結(jié)束算法。

由于傳統(tǒng)A*算法所規(guī)劃的路徑存在一定的缺陷，易產(chǎn)生大量與最終路徑無關(guān)的節(jié)點，運動路徑轉(zhuǎn)折點多，不平滑，路徑搜索精確度低，難以滿足移動機器人的實際工況運動要求。因此，本文提出一種改進搜索鄰域和啟發(fā)函數(shù)的A*算法。

1.2? 改進搜索鄰域和啟發(fā)函數(shù)的A*算法

1.2.1? ? 改進搜索鄰域

傳統(tǒng)A*算法搜索擴展節(jié)點一般采用4鄰域或? 8鄰域搜索算法[15]，如圖2所示。其中圖2（a）所示為4鄰域搜索，即每個搜索方向之間夾角都為90°，容易造成規(guī)劃的路徑轉(zhuǎn)折點過多。圖2（b）所示為? 8鄰域搜索，在4鄰域的基礎(chǔ)上增加了4個鄰域，使得搜索方向之間的夾角變?yōu)?5°，優(yōu)化了搜索角度，擴大了搜索節(jié)點的范圍。但在較復(fù)雜的地圖環(huán)境中，會使得搜索不精確，易忽略障礙物的存在，造成規(guī)劃的路徑會穿過障礙物，不符合移動機器人實際運動路徑的要求。

針對傳統(tǒng)A*算法鄰域搜索不足的問題，本文將傳統(tǒng)算法單一的鄰域搜索模式改進為結(jié)合4鄰域和8鄰域的混合搜索算法，即通過對當前節(jié)點的障礙物環(huán)境進行判斷，切換使用4鄰域和8鄰域搜索算法，再對子節(jié)點進行搜索擴展。此法改變了傳統(tǒng)算法單一的擴展子節(jié)點方式，融合了2種搜索算法的優(yōu)點，使得路徑轉(zhuǎn)折點減少，路徑搜索精度提高。

混合4鄰域和8鄰域搜索算法是一種新的搜索策略。如圖3所示，從S節(jié)點到達G節(jié)點時，當遇到障礙物，采用4個鄰域搜索擴展路徑到達B2節(jié)點，沒有障礙物則采用8個鄰域搜索擴展路徑到達G節(jié)點，得出代價最優(yōu)路徑S-B2-G。而傳統(tǒng)4鄰域搜索得到的路徑為S-B2-A-G或S-B2-B3-G，傳統(tǒng)8鄰域搜索得到的路徑為S-B3-G，顯然傳統(tǒng)鄰域搜索得到的路徑存在轉(zhuǎn)折點多和會穿過障礙物的情況，沒有混合算法優(yōu)越。

改進搜索鄰域算法流程主要分為兩部分：1）初始階段，先將起始節(jié)點放入OPEN列表中;2）擴展階段，通過判斷設(shè)定的切換觸發(fā)條件，采用4鄰域或8領(lǐng)域進行擴展。主要步驟如下：

Step 1? 當進行擴展子節(jié)點時，需要對當前節(jié)點[n]的4個方向的待擴展子節(jié)點進行判斷，并進行標記。如判斷是障礙物，則對變量賦值為1;如判斷不是障礙物，則對變量賦值為0。

Step 2? 通過確定變量值來選用搜索算法。當為1時，使用4鄰域搜索算法的A*算法進行擴展，擴展后再判斷節(jié)點是否在OPEN列表中，并對其進行進一步處理，找出最小代價節(jié)點。當為0時，使用8鄰域搜索算法的A*算法進行擴展，通過標記情況獲取信息，判斷是否計算8個方向擴展節(jié)點的代價值。當對8個方向進行計算時，需要比較、計算8個方向的代價值，判斷節(jié)點是否在OPEN列表中，并選取最小代價值確定擴展方向;否則直接進行判斷，計算代價值確定擴展方向，再保存到OPEN列表中，進行下一步處理。

Step 3? 返回Step 1，循環(huán)搜索直至尋找到目標點，得出路徑。

1.2.2? ? 啟發(fā)函數(shù)的優(yōu)化

傳統(tǒng)A*算法的估價函數(shù)只是將實際成本和啟發(fā)式成本直接相加，而實際上，實際成本和啟發(fā)式成本在最佳成本估價函數(shù)中的權(quán)重往往不相等[16]。因此，為不同環(huán)境下的啟發(fā)式代價設(shè)置一定的權(quán)重，優(yōu)化估價函數(shù)，有助于提高A*算法的運行? ?效率。

1）考慮在一種極端情況下，如果[h（n）]為0，則只有[g（n）]起作用，A*算法演變成為Dijkstra算法，這樣能夠保證尋找到最短路徑。但函數(shù)[h（n）]使用更大的啟發(fā)式算法來估計從[n]到目標的最小路徑的代價，可以限制對路徑方向的一些偏離。2）如果[h（n）]小于或等于從[n]運動到目標的實際代價，可以保證能夠?qū)ふ业揭粭l最短的路徑，但[h（n）]越小，A*算法搜索擴展的節(jié)點越多，規(guī)劃路徑的速度就越慢。3）如果[h（n）]恰好等于從[n]運動到目標點的代價，那么算法將只搜索尋找最優(yōu)路徑而不會搜索擴展任何其他節(jié)點，將會大大減少計算量，此時A*算法規(guī)劃路徑會非?？?。盡管這很難實現(xiàn)，但只要能夠提供精確的信息，在某些特殊的情況下讓它們相等，A*算法運行的效果也會很好。4）如果[h（n）]大于從[n]運動到目標的代價，就不能保證尋找到的路徑最短，但A*算法規(guī)劃路徑消耗的時間會很短。5）如果[h（n）]比[g（n）]大很多，則只有[h（n）]起作用，A*算法就會演變成廣度優(yōu)先搜索算法。

針對上述問題，本文對啟發(fā)式函數(shù)引入權(quán)重，將傳統(tǒng)A*算法的估價函數(shù)重新構(gòu)造為：

[f（n）=g（n）+w*h（n）]，[w≥1].? ? ? ? ?（3）

通過改變[w]來影響搜索效率，但如果權(quán)重過大會導(dǎo)致規(guī)劃路徑陷入局部最優(yōu)。為此，在實際應(yīng)用時，可以根據(jù)需求靈活地調(diào)節(jié)[w]，提高A*算法效率。

傳統(tǒng)A*算法和引入啟發(fā)式函數(shù)權(quán)重的A*算法在MATLAB中的路徑規(guī)劃仿真結(jié)果分別如圖4和圖5所示。選用100[×]100大小的柵格環(huán)境地圖，障礙物占比為20%。仿真圖中左上方圓圈設(shè)置為起點，右下方方塊設(shè)置為目標點，陰影區(qū)域為算法搜索節(jié)點，黑色線條為規(guī)劃所得最短路徑。對比分析2個仿真結(jié)果得出，引入啟發(fā)式函數(shù)權(quán)重的A*算法搜索區(qū)域少，搜索時間短，效率明顯比傳統(tǒng)A*算法高。

2? ? 3次均勻B樣條曲線路徑平滑處理

混合了4鄰域與8鄰域搜索并引入權(quán)重的改進A*算法，其規(guī)劃所得的路徑準確性明顯提高，規(guī)劃時間縮短，但路徑不平滑、拐角尖點的問題依然比較突出，造成機器人行駛轉(zhuǎn)彎效率低，不利于移動。因此，需要平滑處理規(guī)劃所得的路徑，有助于提高機器人運動效率。而B樣條曲線具有幾何保凸性、不變性、凸包性等優(yōu)點，被應(yīng)用于路徑平滑處理[17]。其中3次B樣條曲線在節(jié)點處具有二階連續(xù)性的特點，能夠優(yōu)化曲線，滿足機器人移動的加速度與速度連續(xù)性的要求。所以本文采用3次均勻B樣條曲線對路徑進行平滑處理，B樣條曲線表達式為：

[C（t）=i=0nPiNi， k（t）]，? ? ? ? ? ? ? ? ? ?（4）

式中：[Pi（i=0， 1， 2， …， n）]為給定控制點的坐標，[Ni， k（t）]為基函數(shù)，其表達式為：

[Ni， k（t）=1k！j=0k-i（-1）jCjk+1（t+k-i-j）k]，

[0≤t≤1]， [i=0， 1， …， k-1]，? ? ? ? ?（5）

[Cjk+1=（k+1）！j！（k+1-j）！].? ? ? ? ? ?（6）

由式（4）和式（5）可知，當[k=3]時，基函數(shù)為：

[N0，? 3（t）=16（-t3+3t2-3t+1）N1，? 3（t）=16（3t3-6t2+4）N2，? 3（t）=16（-3t3+3t2+3t+1）N3，? 3（t）=16t3]. （7）

將式（7）代入式（4），得到3次均勻B樣條曲線方程為：

[N0， 3（t）=161tt2t31410-30303-630-13-31P0P1P2P3].

（8）

使用3次均勻B樣條曲線來對A*算法的尖點路徑進行光滑處理。將獲取的相鄰坐標代入上述公式中，就能確定控制點對應(yīng)的樣條曲線方程，隨著[t]的連續(xù)取值，就可以繪制出光滑的B樣條曲線。當有[n]個控制頂點時，只需要將控制頂點移動[n-3]次，就能得到一條完整、連續(xù)的3次B樣條曲線，完成對路徑的平滑優(yōu)化處理。

3? ? 仿真及結(jié)果分析

為驗證本文所設(shè)計算法的可行性和有效性，在MATLAB R2020b中建立比較復(fù)雜的障礙物柵格環(huán)境地圖。針對移動機器人路徑規(guī)劃問題，分別對傳統(tǒng)A*算法和本文混合4鄰域與8鄰域搜索同時引入權(quán)重的A*算法進行仿真。

3.1? ?路徑規(guī)劃仿真試驗

構(gòu)建的柵格環(huán)境地圖大小為30[×]30。如圖6所示，環(huán)境地圖中左上角的圓點為機器人起點，右下角的圓點為目標點，障礙物為黑色方格，可行路徑為白色方格。

在相同的障礙物環(huán)境地圖中（圖6），分別采用傳統(tǒng)A*算法4鄰域搜索、8鄰域搜索和本文的改進鄰域搜索并引入權(quán)重的A*算法進行路徑規(guī)劃仿真試驗，獲得的仿真結(jié)果分別如圖7—圖9所示。

分析圖7—圖9可得，相同環(huán)境下，4鄰域A*算法規(guī)劃的路徑長度不是最優(yōu)，路徑轉(zhuǎn)折點多。? ?8鄰域A*算法規(guī)劃的路徑出現(xiàn)了從2個障礙物之間穿過去的情況，所得路徑不符合移動機器人的運動要求。本文改進鄰域搜索并引入權(quán)重的A*算法所規(guī)劃的路徑明顯比4鄰域A*算法規(guī)劃的路徑轉(zhuǎn)折點少，遇到障礙物會選擇使用4鄰域A*算法，沒有障礙物時會使用8鄰域A*算法，解決了路徑會穿過障礙物的問題，同時引入啟發(fā)式函數(shù)權(quán)重，提高了效率，路徑長度得到減小。

傳統(tǒng)A*算法和改進鄰域搜索并引入權(quán)重的A*算法性能如表1所示。由表1可知，改進鄰域搜索并引入權(quán)重的A*算法消耗的時間和規(guī)劃的路徑長度明顯優(yōu)于傳統(tǒng)A*算法。

3.2? ?路徑平滑處理仿真試驗

采用3次均勻B樣條曲線對路徑進行平滑處理，仿真結(jié)果如圖10所示，局部放大如圖11所示。從圖中可以看出，路徑經(jīng)過3次均勻B樣條曲線處理后變得十分平滑。對比改進鄰域搜索并引入權(quán)重A*算法規(guī)劃的路徑可知，3次均勻B樣條曲線消除了路徑上的拐角尖點，使路徑更加平滑順暢，同時在保證路徑最優(yōu)情況下，減小了路徑的長度。機器人在實際移動時遇到直角轉(zhuǎn)向會造成卡頓，而路徑經(jīng)過平滑處理后，機器人可以保持加速度與速度的連續(xù)性，使機器人移動更加流暢。仿真結(jié)果表明本文算法對路徑平滑處理的可行性與有效性。

4? ? 結(jié)論

本文針對移動機器人路徑規(guī)劃問題，提出了一種改進A*算法：混合使用4鄰域與8鄰域搜索算法，調(diào)節(jié)算法搜索鄰域，從而提高了路徑搜索精確度，減少規(guī)劃路徑的轉(zhuǎn)折點;引入啟發(fā)式函數(shù)權(quán)重，改變啟發(fā)式成本在成本估價函數(shù)中的權(quán)重，縮短了路徑規(guī)劃的時間。對規(guī)劃路徑進行3次均勻B樣條曲線平滑處理，消除了路徑拐角尖點，使機器人可以平滑地移動到目標點。仿真結(jié)果表明，混合4鄰域與8鄰域搜索同時引入權(quán)重的A*算法，結(jié)合3次均勻B樣條曲線平滑處理后，應(yīng)用于移動機器人路徑規(guī)劃上是可行和有效的。

參考文獻

[1]? ? ?吳青鴻，李健，劉歡，等.基于模糊PID下肢外骨骼機器人的控制技術(shù)[J].廣西科技大學學報，2020，31（4）：104-111.

[2]? ? ?卜新蘋，蘇虎，鄒偉，等.基于非均勻環(huán)境建模與三階Bezier曲線的平滑路徑規(guī)劃[J].自動化學報，2017，43（5）：710-724.

[3]? ? ?王洪斌，尹鵬衡，鄭維，等.基于改進的A*算法與動態(tài)窗口法的移動機器人路徑規(guī)劃[J].機器人，2020，42（3）：346-353.

[4]? ? ?皮倩瑛，葉洪濤.一種動態(tài)調(diào)節(jié)慣性權(quán)重的粒子群算法[J].廣西科技大學學報，2016，27（3）：26-32.

[5]? ? ?丁佳宇，王茂森，戴勁松.基于前饋控制人工勢場法的無人機群系統(tǒng)設(shè)計[J].傳感器與微系統(tǒng)，2021，40（5）：114-117.

[6]? ? ?田華亭，李濤，秦穎.基于A*改進算法的四向移動機器人路徑搜索研究[J].控制與決策，2017，32（6）：1007-1012.

[7]? ? ?SUNITA，GARG D.Dynamizing dijkstra：a solution to dynamic shortest path problem through retroactive priority queue[J].Journal of King Saud University-Computer and Information Sciences，2021，33（3）：364-373.

[8]? ? ?LIU J H，YANG J G，LIU H P，et al.An improved ant colony algorithm for robot path planning[J].Soft Computing，2017，21（19）：5829-5839.

[9]? ? ?張紅梅，李明龍，楊樂.基于改進A*算法的移動機器人安全路徑規(guī)劃[J].計算機仿真，2018，35（4）：319-324.

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

[11]? ?王中玉，曾國輝，黃勃.基于改進雙向A*的移動機器人路徑規(guī)劃算法[J].傳感器與微系統(tǒng)，2020，39（11）：141-143.

[12]? ?祁玄玄，黃家駿，曹建安.基于改進A*算法的無人車路徑規(guī)劃[J].計算機應(yīng)用，2020，40（7）：2021-2027.

[13]? ?張敬寒，陶兆勝，彭澎，等.基于擴大搜索鄰域A*算法的平滑路徑規(guī)劃[J].長春理工大學學報（自然科學版），2018，41（6）：124-127.

[14]? ?楊瑤，付克昌，蔣濤，等.一種改進A*算法在智能車中的應(yīng)用研究[J].重慶理工大學學報（自然科學版），2021，35（3）：71-79.

[15]? ?張志文，張鵬，毛虎平，等.改進A*算法的機器人路徑規(guī)劃研究[J].電光與控制，2021，28（4）：21-25.

[16]? ?LIN M X，YUAN K，SHI C Z，et al.Path planning of mobile robot based on improved A* algorithm[C]//29th Chinese Control and Decision Conference，2017：3570-3576.

[17]? ?胡中華，許昕，陳中.無人機三維航跡非均勻3次B樣條平滑算法[J].控制工程，2020，27（7）：1259-1266.

Mobile robot path planning based on improved A* algorithm

CHEN Jiabao1， WEN Jiayan*1，2， XIE Guangming1，3

（1. School of Electrical，Electronic and Computer Science， Guangxi University of Science and Technology，

Liuzhou 545616， China; 2. Guangxi Key Laboratory of Automobile Components and Vehicle Technology（Guangxi University of Science and Technology）， Liuzhou 545006， China; 3. College of Engineering，

Peking University， Beijing 100871， China）

Abstract： In the path planning of mobile robots， an improved A* algorithm is proposed to solve the problem of long and unsmooth path search time subjected to the traditional A* algorithm. Firstly， on the basis of the traditional A* algorithm， the 4-neighbor and 8-neighbor search algorithms are mixed， and a switching mechanism is established to improve the path search accuracy and reduce path turning points. Secondly， by introducing weights to the heuristic function， the evaluation function is optimized， and the path planning time is shortened. Finally， the path planned by the A* algorithm is smooth using cubic uniform B-spline curves to eliminate the corners of the path and improve the smoothness of the path while ensuring the optimal path. The simulation results show that the path planned by the improved A* algorithm is superior to the traditional algorithm in terms of length， smoothness， and searching time consumption.

Key words： mobile robot; path planning; A* algorithm; B-spline curve

（責任編輯：黎? 婭）