基于改進(jìn)A*算法的移動(dòng)機(jī)器人路徑規(guī)劃

摘要：針對(duì)A*算法在路徑規(guī)劃中存在遍歷節(jié)點(diǎn)數(shù)過(guò)多、轉(zhuǎn)折角度較大的問(wèn)題，提出一種能自適應(yīng)場(chǎng)景地圖的改進(jìn)A*算法。通過(guò)量化地圖場(chǎng)景信息和障礙物分布情況，引入父節(jié)點(diǎn)對(duì)當(dāng)前節(jié)點(diǎn)的影響力，增加障礙物分布率的啟發(fā)函數(shù)權(quán)重，減少遍歷節(jié)點(diǎn)數(shù)量、提高搜索速度；加入轉(zhuǎn)彎懲罰函數(shù)、擴(kuò)展鄰域優(yōu)先級(jí)搜索和冗余節(jié)點(diǎn)平滑策略對(duì)路徑進(jìn)一步優(yōu)化，避免路徑出現(xiàn)多余轉(zhuǎn)彎，降低路徑出現(xiàn)局部最優(yōu)解的可能。在相同地圖場(chǎng)景中進(jìn)行測(cè)試對(duì)比，所提算法能有效減少遍歷節(jié)點(diǎn)數(shù)量，降低總轉(zhuǎn)折角度，提高搜索速度，縮短路徑距離，獲得最優(yōu)路徑。

關(guān)鍵詞：路徑規(guī)劃；A*算法；啟發(fā)式函數(shù)；鄰域擴(kuò)展；優(yōu)先級(jí)搜索

中圖分類(lèi)號(hào)：TP242文獻(xiàn)標(biāo)志碼：A

文章編號(hào)：1001-3695（2023）01-012-0075-05

doi：10.19734/j.issn.1001-3695.2022.04.0256

Mobile robot planning based on improved A* algorithm

Shen Keyu，You Zhiyu，Liu Yongxin，Huang Tao

（Key Laboratory of Electronic Information of State Ethnic Affairs Commission，College of Electrical Engineering，Southwest Minzu University，Chengdu 610041，China）

Abstract：Aiming at the problems of too many traversed nodes and large turning angles in the path planning of the A* algorithm，this paper proposed an improved A* algorithm that could adapt to the scene map.By quantifying the map scene information and the distribution of obstacles，introducing the influence of the parent node on the current node and increasing the weight of the heuristic function of the obstacle distribution rate，this paper further reduced the number of traversed nodes and improved the search speed.By adding turning penalty function，extended neighborhood priority search and redundant node smoothing strategy further optimized the path，it avoided redundant turns on the path and reduced the possibility of local optimal solutions on the path.Finally，the simulation test in the same map scene shows that the proposed algorithm can effectively reduce the number of traversed nodes，reduce the total turning angle，improve the search speed，shorten the path distance and obtain the global optimal path.

Key words：path planning；A* algorithm；heuristic function；extended neighborhood；priority search

0引言

隨著科學(xué)技術(shù)和工業(yè)自動(dòng)化的快速發(fā)展，移動(dòng)機(jī)器人已受到世界各國(guó)的普遍關(guān)注，被廣泛應(yīng)用到工業(yè)、農(nóng)業(yè)、醫(yī)療、服務(wù)等行業(yè)，并逐漸在城市安全、國(guó)防和空間探測(cè)等領(lǐng)域替代人工作業(yè)。路徑規(guī)劃是移動(dòng)機(jī)器人自主導(dǎo)航的關(guān)鍵技術(shù)［1］，是使機(jī)器人能夠在有障礙物的工作環(huán)境中規(guī)劃出從起點(diǎn)到終點(diǎn)的暢通路徑，使機(jī)器人能夠安全、無(wú)碰撞地到達(dá)終點(diǎn)［2，3］。根據(jù)地圖場(chǎng)景信息掌握程度，移動(dòng)機(jī)器人路徑規(guī)劃可分為全局路徑規(guī)劃和局部路徑規(guī)劃［4］。全局路徑規(guī)劃是一種事前規(guī)劃，規(guī)劃路徑精度取決于獲取地圖場(chǎng)景信息的準(zhǔn)確度。地圖場(chǎng)景信息有誤或噪聲過(guò)大時(shí)，其魯棒性將變差。局部路徑規(guī)劃［5］對(duì)硬件設(shè)備的計(jì)算和信息處理能力要求較高，需要實(shí)時(shí)采集周?chē)鷪?chǎng)景信息以動(dòng)態(tài)校正方位，使機(jī)器人具有良好的避障能力，對(duì)環(huán)境誤差和噪聲有較高的魯棒性。局部路徑規(guī)劃由于缺少全局環(huán)境信息，無(wú)法保證尋路結(jié)果全局最優(yōu)，甚至出現(xiàn)尋路不完整的情況。

在全局路徑規(guī)劃算法中，基于采樣的快速隨機(jī)擴(kuò)展樹(shù)法（rapidly random tree，RRT）［6］僅有概率完備性，且規(guī)劃出的路徑最優(yōu)性無(wú)法保證。A*算法［7］基于傳統(tǒng)圖搜索思維并引入了啟發(fā)智能搜索，擁有出色的路徑最優(yōu)性、尋路完備性和搜索高效性，且計(jì)算量小、搜索速度快，故被廣泛應(yīng)用到靜態(tài)地圖場(chǎng)景下的路徑規(guī)劃中。因其啟發(fā)式搜索較單一、搜索鄰域受限等問(wèn)題，使得在規(guī)劃路徑時(shí)會(huì)遍歷大量不必要的搜索節(jié)點(diǎn)且路徑上存在過(guò)大轉(zhuǎn)折角，一直是學(xué)者研究改進(jìn)的重點(diǎn)。

目前已經(jīng)提出了很多改進(jìn)A*算法。文獻(xiàn)［8］為能找到最短路徑設(shè)計(jì)了新型距離計(jì)算方式。文獻(xiàn)［9］通過(guò)改變計(jì)算方式與函數(shù)權(quán)重以降低尋路時(shí)間和冗余節(jié)點(diǎn)數(shù)量。在文獻(xiàn)［10］中則限制擴(kuò)展鄰域，預(yù)設(shè)搜索方向?yàn)樯刃危岣吡吮闅v節(jié)點(diǎn)效率。文獻(xiàn)［11］擴(kuò)展20鄰域并設(shè)計(jì)了安全距離，使規(guī)劃出的路徑更平滑。文獻(xiàn)［12］將傳統(tǒng)A*算法的可搜索鄰域個(gè)數(shù)從8個(gè)拓展為無(wú)限個(gè)，可以沿任意方向進(jìn)行搜索，這樣不僅求解出來(lái)的路徑長(zhǎng)度更短，并且大大降低了其轉(zhuǎn)折點(diǎn)的個(gè)數(shù)。文獻(xiàn)［13］則通過(guò)對(duì)算法進(jìn)行定向搜索來(lái)消除對(duì)稱搜索路徑，從而減少遍歷節(jié)點(diǎn)數(shù)。文獻(xiàn)［14］增加了搜索過(guò)程中當(dāng)前節(jié)點(diǎn)的父節(jié)點(diǎn)信息，并對(duì)該節(jié)點(diǎn)與終點(diǎn)的距離進(jìn)行評(píng)估。在此基礎(chǔ)上，文獻(xiàn)［15］又將切比雪夫距離作為啟發(fā)式函數(shù)權(quán)重使其搜索速度得到極大提升。文獻(xiàn)［16］針對(duì)復(fù)雜場(chǎng)景地圖引入安全因子，使路徑經(jīng)過(guò)的危險(xiǎn)區(qū)域更少。文獻(xiàn)［17，18］利用轉(zhuǎn)向修正函數(shù)，減少了路徑上的轉(zhuǎn)折點(diǎn)個(gè)數(shù)。文獻(xiàn)［19，20］在傳統(tǒng)A*算法中引入動(dòng)態(tài)窗口，使之能更好地避開(kāi)障礙物。上述文獻(xiàn)為改進(jìn)A*算法的理論研究提供了思路和依據(jù)。

現(xiàn)有文獻(xiàn)針對(duì)特定性能指標(biāo)對(duì)A*算法進(jìn)行了改進(jìn)，但是以犧牲其他指標(biāo)性能為代價(jià)，使得在特定地圖場(chǎng)景下獲得了期望的指標(biāo)性能。為了能有效減少遍歷節(jié)點(diǎn)數(shù)量、降低總轉(zhuǎn)折角度、降低尋路時(shí)間、縮短路徑距離、提高搜索效率，本文提出了改進(jìn)A*算法，通過(guò)對(duì)啟發(fā)式函數(shù)、擴(kuò)展鄰域和優(yōu)先級(jí)搜索策略進(jìn)行設(shè)計(jì)，使改進(jìn)算法在路徑規(guī)劃上具有更強(qiáng)的啟發(fā)性，且在遍歷節(jié)點(diǎn)和路徑平滑上達(dá)到最優(yōu)。

1A*算法

A*算法借鑒了Dijkstra算法和最佳優(yōu)先搜索算法，核心思想是在指定地圖中規(guī)劃出一條從起始位置到目標(biāo)位置的最小移動(dòng)代價(jià)路徑，從起始位置到目標(biāo)位置的移動(dòng)代價(jià)估值函數(shù)f（n）為

f（n）=g（n）+h（n）（1）

其中：n表示路徑搜索過(guò)程中的當(dāng)前位置節(jié)點(diǎn)；g（n）表示從起始位置到當(dāng)前位置節(jié)點(diǎn)的真實(shí)移動(dòng)距離；h（n）表示從當(dāng)前位置節(jié)點(diǎn)到目標(biāo)位置節(jié)點(diǎn)的啟發(fā)式函數(shù)，即預(yù)估移動(dòng)代價(jià)。路徑搜索過(guò)程中節(jié)點(diǎn)與g（n）的關(guān)系如圖1所示。

圖中S（n）表示父節(jié)點(diǎn)（n-1）到當(dāng)前節(jié)點(diǎn)n的單步實(shí)際移動(dòng)距離；S（n+1）表示當(dāng)前節(jié)點(diǎn)n到下一節(jié)點(diǎn)（n+1）的單步實(shí)際移動(dòng)距離；g（n）表示起始節(jié)點(diǎn)到（n-1）節(jié)點(diǎn)的真實(shí)移動(dòng)距離g（n-1）與從（n-1）節(jié)點(diǎn)到當(dāng)前節(jié)點(diǎn)n的單步實(shí)際移動(dòng)距離S（n）之和，即為每一段已規(guī)劃路徑距離的總和。由此可得真實(shí)移動(dòng)距離函數(shù)g（n）的表達(dá)式為

g（n）=∑ni=1S（i）（2）

若h（n）的評(píng)估值遠(yuǎn)小于g（n），則f（n）將近似等于g（n），此時(shí)A*算法近似于Dijkstra 算法，遍歷節(jié)點(diǎn)會(huì)增多，搜索效率將大大降低；若h（n）遠(yuǎn)大于g（n），此時(shí)A*算法逐漸演變成最佳優(yōu)先搜索算法，路徑規(guī)劃速度變快，但容易出現(xiàn)局部最優(yōu)解的情況。因此A*算法的性能優(yōu)劣取決于啟發(fā)式函數(shù)h（n）的選取。常用h（n）的預(yù)估方法有歐幾里德距離、曼哈頓距離和對(duì)角線距離。

假設(shè)起點(diǎn)坐標(biāo)為（s1，s2），終點(diǎn)坐標(biāo)為（g1，g2）。

歐幾里德距離的啟發(fā)函數(shù)為

h=（s1-g1）2+（s2-g2）2（3）

曼哈頓距離的啟發(fā)函數(shù)為

h=|s1-g1|+|s2-g2|（4）

對(duì)角線距離的啟發(fā)函數(shù)為

h=1.4×Diagonal+（Straight-2×Diagonal）（5）

其中：Diagonal=min（|s1-g1| +|s2-g2|），Straight=|s1-g1| +|s2-g2|。

由于歐幾里德距離的計(jì)算精度高于曼哈頓和對(duì)角線距離，得出最優(yōu)路徑的可能性更大，故本文選取歐幾里德距離作為啟發(fā)式函數(shù)h（n）進(jìn)行移動(dòng)代價(jià)預(yù)估。

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

傳統(tǒng)A*算法及文獻(xiàn)改進(jìn)A*算法在任意地圖中自適應(yīng)性較差，在路徑規(guī)劃時(shí)存在遍歷節(jié)點(diǎn)數(shù)多、總轉(zhuǎn)折角度大、規(guī)劃路徑不夠平滑、尋路時(shí)間較長(zhǎng)等問(wèn)題。本文綜合文獻(xiàn)［7～18］所提改進(jìn)思路，在設(shè)計(jì)思路時(shí)需要避免出現(xiàn)因提高某一方面性能指標(biāo)導(dǎo)致其他性能降低的情況。通過(guò)設(shè)計(jì)自適應(yīng)啟發(fā)式函數(shù)、轉(zhuǎn)彎懲罰函數(shù)、擴(kuò)展鄰域優(yōu)先級(jí)搜索和路徑冗余平滑策略來(lái)改進(jìn)A*算法，保證最終規(guī)劃出的路徑相對(duì)最優(yōu)，提高算法的魯棒性和搜索效率，減少遍歷節(jié)點(diǎn)數(shù)和總轉(zhuǎn)折角度，從而緩解移動(dòng)機(jī)器人的存儲(chǔ)壓力，降低能源損耗。

2.1自適應(yīng)啟發(fā)式函數(shù)策略

通過(guò)引入父節(jié)點(diǎn)（n-1）的影響力，增強(qiáng)h（n）啟發(fā)性，減少算法遍歷節(jié)點(diǎn)數(shù)量和循環(huán)遍歷次數(shù)，提高搜索速度。但在引入父節(jié)點(diǎn)之后，h（n）與g（n）有可能會(huì)失去平衡，導(dǎo)致搜索步長(zhǎng)越界而出現(xiàn)局部最優(yōu)解。因此，需要相應(yīng)降低引入父節(jié)點(diǎn)影響力之后的權(quán)重，使之能根據(jù)地圖變化后的量化信息，自適應(yīng)調(diào)整權(quán)重值。設(shè)計(jì)的自適應(yīng)啟發(fā)式函數(shù)h′（n）為

h′（n）=（1-P）×［h（n）+h（n-1）］（6）

其中：（1-P）為自適應(yīng)啟發(fā)式函數(shù)h′（n）的自適應(yīng)權(quán)重因子；P為起始節(jié)點(diǎn)和目標(biāo)節(jié)點(diǎn)直角坐標(biāo)所形成的矩形地圖障礙物覆蓋率，其值為矩形地圖中所有障礙物占地面積與地圖總面積的比值，取值為［0，1］。地圖障礙物越多，自適應(yīng)權(quán)重因子會(huì)相應(yīng)降低，從而避免局部最優(yōu)解情況的出現(xiàn)。

為校驗(yàn)自適應(yīng)啟發(fā)式函數(shù)對(duì)遍歷節(jié)點(diǎn)和搜索時(shí)間的影響，在圖2所示30×30地圖中進(jìn)行測(cè)試驗(yàn)證。圖2（a）是傳統(tǒng)A*算法的規(guī)劃結(jié)果，圖2（b）是僅引入父節(jié)點(diǎn)影響力的規(guī)劃結(jié)果，圖2（c）是本文設(shè)計(jì)的自適應(yīng)啟發(fā)式函數(shù)規(guī)劃結(jié)果。圖中黑色方格為障礙物，起始節(jié)點(diǎn)與目標(biāo)節(jié)點(diǎn)坐標(biāo)圍成的矩形地圖障礙物覆蓋率P為35.59%，黃色實(shí)心圓為起始節(jié)點(diǎn)，藍(lán)色實(shí)心五角星為目標(biāo)節(jié)點(diǎn)，紅色節(jié)點(diǎn)為路徑規(guī)劃過(guò)程中訪問(wèn)并保存過(guò)的節(jié)點(diǎn)，綠色節(jié)點(diǎn)為路徑規(guī)劃過(guò)程中丟棄的節(jié)點(diǎn)，黑色實(shí)線為算法最終規(guī)劃出的路徑（見(jiàn)電子版）。規(guī)劃性能指標(biāo)如表1所示。

從圖2和表1可以看出，引入父節(jié)點(diǎn)的啟發(fā)函數(shù)在規(guī)劃路徑時(shí)遍歷節(jié)點(diǎn)數(shù)最少，尋路時(shí)間最短，但由于其過(guò)大的搜索步長(zhǎng)，導(dǎo)致尋路精度較差，規(guī)劃出的路徑存在局部最優(yōu)解，路徑距離較長(zhǎng)，路徑并非最優(yōu)。圖2（c）引入自適應(yīng)權(quán)重因子后，相對(duì)避免了局部最優(yōu)解的出現(xiàn)，且在遍歷節(jié)點(diǎn)和尋路時(shí)間上均優(yōu)于傳統(tǒng)A*算法，但此時(shí)路徑存在冗余轉(zhuǎn)角，還需要對(duì)其作進(jìn)一步優(yōu)化改進(jìn)。

2.2轉(zhuǎn)彎懲罰函數(shù)策略

當(dāng)路徑規(guī)劃中出現(xiàn)局部最優(yōu)解時(shí)，意味著算法規(guī)劃的路徑存在不必要轉(zhuǎn)彎，因此可以引入轉(zhuǎn)彎懲罰函數(shù)修正路徑多余轉(zhuǎn)角。

首先根據(jù)兩點(diǎn)之間線段最短原理，確定從起始節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的相對(duì)位置，并由兩點(diǎn)的直角坐標(biāo)得到初始向量。隨后連接當(dāng)前節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)，得出搜索向量。最后通過(guò)向量平行原理來(lái)判斷初始向量與搜索向量的位置關(guān)系，采用轉(zhuǎn)彎懲罰的方式來(lái)保證待搜索路徑向最優(yōu)路徑擬合，從而減少不必要的轉(zhuǎn)彎。判斷向量平行的原理如圖3所示。

圖中黃色實(shí)心圓為起始節(jié)點(diǎn)，灰色圓形表示當(dāng)前節(jié)點(diǎn)，藍(lán)五角星為目標(biāo)節(jié)點(diǎn)，黑色方格為障礙物，紅色射線表示從當(dāng)前節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的搜索向量，黑色射線表示起始節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的初始向量（見(jiàn)電子版）。

設(shè)當(dāng)前節(jié)點(diǎn)坐標(biāo)為（n1，n2），起始節(jié)點(diǎn)（s1，s2），目標(biāo)節(jié)點(diǎn)（g1，g2），則式（7）成立。

dx1=|g1-n1|，dy1=|g2-n2|

dx2=|g1-s1|，dy2=|g2-s2|（7）

由向量平行原理，推出轉(zhuǎn)彎懲罰函數(shù)：

cross（n）=|dx1×dy2-dx2×dy1|×K（8）

其中：K為常數(shù)，取值為（0，1），用于進(jìn)一步提高f（n）的計(jì)算精度。此時(shí)的f（n）如式（9）所示。

f（n）=g（n）+h′（n）+cross（n）（9）

在初始向量與搜索向量平行時(shí)路徑不轉(zhuǎn)彎，此時(shí)cross（n）為0；反之cross（n）相應(yīng)增加，其值作為搜索轉(zhuǎn)彎的懲罰代價(jià)，從而盡可能減少規(guī)劃路徑中不必要的轉(zhuǎn)彎，使搜索方向向目標(biāo)節(jié)點(diǎn)擬合，以保證最終規(guī)劃出的路徑全局最優(yōu)或相對(duì)最優(yōu)。

2.3擴(kuò)展鄰域優(yōu)先級(jí)搜索策略

傳統(tǒng)A*算法的搜索鄰域?yàn)?鄰域或8鄰域，導(dǎo)致其搜索空間受限，搜索路徑無(wú)法保證最優(yōu)，因此需要針對(duì)具體地圖場(chǎng)景對(duì)搜索鄰域范圍進(jìn)行擴(kuò)展。當(dāng)鄰域擴(kuò)展時(shí)，規(guī)劃出的路徑較為平滑，但搜索速度將變慢；當(dāng)鄰域縮減時(shí)，搜索速度將提高，但規(guī)劃出的路徑存在過(guò)多轉(zhuǎn)折角，在復(fù)雜地圖中甚至有可能尋路失敗。本文充分考慮到擴(kuò)展和縮減鄰域的優(yōu)缺點(diǎn)，設(shè)計(jì)了一種基于優(yōu)先級(jí)的擴(kuò)展鄰域搜索策略，保證最終規(guī)劃出的路徑相對(duì)平滑，并減少因擴(kuò)展鄰域而增加的尋路時(shí)間。

2.3.1擴(kuò)展鄰域

針對(duì)擴(kuò)展鄰域?qū)е滤阉魉俣冉档偷膯?wèn)題，將擴(kuò)展24鄰域與傳統(tǒng)8鄰域重合的方向刪除，形成圖4所示的16鄰域擴(kuò)展方向。圖中1～4為傳統(tǒng)4鄰域，1～8為傳統(tǒng)8鄰域，9～16為本文提出的擴(kuò)展鄰域。

在擴(kuò)展鄰域之后，共有16個(gè)搜索方向。記dx為相對(duì)于當(dāng)前節(jié)點(diǎn)的橫軸坐標(biāo)變化數(shù)值，dy表示縱軸坐標(biāo)變化數(shù)值，S（i）為式（2）中的單步移動(dòng)距離，則擴(kuò)展鄰域后的動(dòng)態(tài)規(guī)劃矩陣Motion為

Motion=［dx，dy，S（i）］（10）

2.3.2分配優(yōu)先級(jí)

為減少因擴(kuò)展鄰域而增加的搜索時(shí)間，本文對(duì)搜索鄰域分配優(yōu)先級(jí)，以增強(qiáng)g（n）的啟發(fā)性，使之能在保證尋路向最優(yōu)擬合的同時(shí)提升搜索速度。圖5中虛線表示從起始節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的搜索向量a，實(shí)線為從起始節(jié)點(diǎn)向子節(jié)點(diǎn)的移動(dòng)向量b，a與b之間的夾角記做θ。

記a=（X，Y），b=（M，N），夾角θ的余弦為

cos θ=a·b|a|·|b|=XM+YNX2+Y2×M2+N2（11）

在確定搜索向量與移動(dòng)向量的相對(duì)位置關(guān)系后，再對(duì)擴(kuò)展后的16個(gè)鄰域分配優(yōu)先級(jí)。從圖5和式（11）可知，搜索向量與移動(dòng)向量間的夾角θ越小，其移動(dòng)方向與最優(yōu)路徑越擬合，但此時(shí)cos θ較大，因此通過(guò)改進(jìn)式（10）中的S（i）系數(shù)來(lái)調(diào)整搜索鄰域時(shí)的單步移動(dòng)距離，從而使得優(yōu)先搜索更趨向于搜索向量的鄰域，增強(qiáng)g（n）的啟發(fā)性和導(dǎo)向性，提高搜索速度。改進(jìn)后的S′（i）為

S′（i）=（2-cos θ）×S（i）（12）

此時(shí)擴(kuò)展鄰域優(yōu)先級(jí)搜索的動(dòng)態(tài)規(guī)劃矩陣為

Motion=［dx，dy，S′（i）］（13）

故最終移動(dòng)代價(jià)估值函數(shù)f（n）為

f（n）=∑ni=1S′（i）+h′（n）+cross（n）（14）

2.4冗余節(jié)點(diǎn)平滑策略

針對(duì)移動(dòng)機(jī)器人存儲(chǔ)的路徑節(jié)點(diǎn)數(shù)過(guò)多和轉(zhuǎn)折角度過(guò)大的問(wèn)題，本文提出冗余節(jié)點(diǎn)平滑策略，刪除額外拐點(diǎn)和節(jié)點(diǎn)優(yōu)化路徑。

首先計(jì)算初始路徑中鄰近三個(gè)節(jié)點(diǎn)間的位置關(guān)系，通過(guò)向量平行原理，判斷并存儲(chǔ)路徑中的拐點(diǎn)；之后依次判斷鄰近四個(gè)拐點(diǎn)是否可以直連，直連后路徑若未經(jīng)過(guò)障礙物且總距離最短，則記錄拐點(diǎn)位置，并刪除初始路徑中位于直連拐點(diǎn)之間的冗余節(jié)點(diǎn)；接著以記錄拐點(diǎn)為起點(diǎn)，再與隨后三個(gè)鄰近拐點(diǎn)進(jìn)行直連判斷，直到目標(biāo)節(jié)點(diǎn)為止。更新路徑節(jié)點(diǎn)，再次循環(huán)判斷，直到路徑上沒(méi)有可直連的冗余節(jié)點(diǎn)為止，此時(shí)的路徑即為冗余節(jié)點(diǎn)平滑處理后的最優(yōu)路徑。刪除冗余節(jié)點(diǎn)的原理示意圖及其測(cè)試如圖6所示。圖6（a）中A→B→C→D為規(guī)劃出的初始路徑，黃色實(shí)心圓A、B、C、D是路徑中的拐點(diǎn)，紅色虛線表示兩點(diǎn)間無(wú)障礙物可直連，藍(lán)色虛線表示兩點(diǎn)間有障礙物不可直連（見(jiàn)電子版）。由于圖6（a）中A→D和B→D均經(jīng)過(guò)障礙物不可直連，所以采用冗余節(jié)點(diǎn)平滑策略后得到的路徑為A→C→D。若圖中A→D、A→C和B→D的路徑都沒(méi)有經(jīng)過(guò)障礙物，則根據(jù)兩點(diǎn)之間線段最短原理，由冗余節(jié)點(diǎn)平滑策略直得出A→D為最優(yōu)路徑；若圖中B→D也連通，則比較A→C→D與A→B→D兩條路徑距離，選擇最短距離路徑。

為檢驗(yàn)冗余節(jié)點(diǎn)平滑策略的作用，對(duì)圖2所示地圖場(chǎng)景進(jìn)行冗余節(jié)點(diǎn)平滑處理，得到圖6（b）所示的結(jié)果。圖中黑色實(shí)線為規(guī)劃出的初始路徑，紅色實(shí)線是對(duì)初始路徑進(jìn)行冗余節(jié)點(diǎn)平滑處理后的最終路徑，黃色方格為路徑上的拐點(diǎn)（見(jiàn)電子版）。圖6（b）表明通過(guò)對(duì)路徑進(jìn)行冗余節(jié)點(diǎn)平滑處理后，使得算法規(guī)劃出的路徑更為平滑，經(jīng)過(guò)的節(jié)點(diǎn)數(shù)更少，僅需要記憶幾個(gè)拐點(diǎn)就可以規(guī)劃出完整的路徑，有效緩解了移動(dòng)機(jī)器人的存儲(chǔ)壓力，降低了移動(dòng)機(jī)器人與障礙物發(fā)生碰撞的幾率，也解決了機(jī)器人在轉(zhuǎn)彎時(shí)額外產(chǎn)生的能源損耗和時(shí)間等待問(wèn)題。

2.5改進(jìn)A*算法流程

為使傳統(tǒng)A*算法能在任意地圖中規(guī)劃出最優(yōu)路徑，有效減少遍歷節(jié)點(diǎn)數(shù)量、降低總轉(zhuǎn)折角度、提高搜索速度、縮短路徑距離，本文融合提出的自適應(yīng)啟發(fā)式函數(shù)策略、轉(zhuǎn)彎懲罰函數(shù)策略、擴(kuò)展鄰域優(yōu)先級(jí)搜索策略和冗余節(jié)點(diǎn)平滑策略，最終形成本文所提的改進(jìn)A*算法。算法流程如圖7所示。

3仿真與分析

為檢驗(yàn)本文算法的性能，在Intel CoreTM i5-7300HQ CPU @ 2.50 GHz計(jì)算機(jī)上利用MATLAB 2020b對(duì)傳統(tǒng)A*算法、文獻(xiàn)［9］改進(jìn)A*算法、文獻(xiàn)［11］改進(jìn)A*算法和本文所提算法在相同場(chǎng)景中進(jìn)行仿真驗(yàn)證。柵格地圖尺寸為30×30，測(cè)試結(jié)果如圖8所示。

圖中黃色圓形為起點(diǎn)，藍(lán)色五角星為終點(diǎn)，黑色柵格為障礙物，起點(diǎn)與終點(diǎn)所圍地圖的障礙物覆蓋率為31.33%，紅色區(qū)域表示尋路過(guò)程中遍歷并保存的節(jié)點(diǎn)，綠色為丟棄的節(jié)點(diǎn)，黑色實(shí)線為最終規(guī)劃出的路徑，黃色節(jié)點(diǎn)為路徑上的拐點(diǎn)（見(jiàn)電子版）。各算法在路徑規(guī)劃時(shí)的相關(guān)性能參數(shù)如表2所示。

文獻(xiàn)［9］通過(guò)改進(jìn)距離計(jì)算方法，人為調(diào)整權(quán)重系數(shù)關(guān)系并選擇最優(yōu)權(quán)重比例，使得規(guī)劃出的路徑在總轉(zhuǎn)折角度、路徑長(zhǎng)度上優(yōu)于傳統(tǒng)A*算法，但該算法每次更換場(chǎng)景地圖時(shí)需要多次實(shí)驗(yàn)尋找最優(yōu)權(quán)重比例，故搜索效率不高，魯棒性較弱。文獻(xiàn)［11］引入20鄰域和安全距離，使得規(guī)劃出的路徑更為平滑，能較好地避開(kāi)障礙物，但是尋路時(shí)間大幅增加，遍歷節(jié)點(diǎn)數(shù)也較多，搜索效率較低。相比于文獻(xiàn)［9，11］的改進(jìn)算法，本文算法規(guī)劃的路徑距離稍微增加，但在尋路時(shí)間和總轉(zhuǎn)折角等方面均有較大提升。

通過(guò)統(tǒng)計(jì)分析表2中的性能參數(shù)可知，本文提出的改進(jìn)A*算法比傳統(tǒng)A*算法更為優(yōu)越，主要表現(xiàn)在以下方面：遍歷節(jié)點(diǎn)數(shù)減少85.71%，路徑節(jié)點(diǎn)數(shù)減少72.73%，路徑拐點(diǎn)數(shù)減少64.71%，總轉(zhuǎn)折角度降低68.77%，尋路時(shí)間降低49.74%，路徑距離減少1.67%。以上數(shù)據(jù)充分證明本文提出的改進(jìn)A*算法在路徑規(guī)劃時(shí)尋路時(shí)間更短，遍歷節(jié)點(diǎn)數(shù)更少且僅需記憶有限幾個(gè)拐點(diǎn)便可以得到完整路徑，規(guī)劃出的路徑更平滑、距離較優(yōu)，更適合移動(dòng)機(jī)器人日常工作。

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

針對(duì)傳統(tǒng)A*算法在路徑規(guī)劃中存在的不足，本文提出了具有地圖場(chǎng)景自適應(yīng)、轉(zhuǎn)彎懲罰、擴(kuò)展鄰域優(yōu)先級(jí)搜索和冗余節(jié)點(diǎn)平滑的改進(jìn)A*算法。該算法通過(guò)增強(qiáng)父節(jié)點(diǎn)影響力并量化地圖障礙物信息得到自適應(yīng)啟發(fā)式函數(shù)來(lái)提高搜索速度，減少遍歷節(jié)點(diǎn)，規(guī)避局部最優(yōu)解的出現(xiàn)；隨后設(shè)計(jì)轉(zhuǎn)彎懲罰函數(shù)，進(jìn)一步降低路徑中出現(xiàn)多余轉(zhuǎn)彎的可能；接著利用擴(kuò)展鄰域優(yōu)先級(jí)搜索和冗余節(jié)點(diǎn)平滑策略，使搜索方向向目標(biāo)節(jié)點(diǎn)擬合，以規(guī)劃出更快、更平滑的路徑。

通過(guò)實(shí)驗(yàn)對(duì)比分析，本文提出的改進(jìn)A*算法遍歷節(jié)點(diǎn)數(shù)更少、路徑更為平滑，尋路時(shí)間和路徑距離更短，證明了所提算法的優(yōu)越性和可行性。接下來(lái)將結(jié)合其他智能算法，研究如何在動(dòng)態(tài)環(huán)境中快速準(zhǔn)確地規(guī)劃出移動(dòng)機(jī)器人的最優(yōu)路徑。

參考文獻(xiàn)：

［1］劉華軍，楊靜宇，陸建峰，等.移動(dòng)機(jī)器人運(yùn)動(dòng)規(guī)劃研究綜述［J］.中國(guó)工程科學(xué)，2006（1）：85-94.（Liu Huajun，Yang Jingyu，Lu Jianfeng，et al.Research on mobile robots motion planning：a survey［J］.Strategic Study of CAE，2006（1）：85-94.）

［2］陸新華，張桂林.室內(nèi)服務(wù)機(jī)器人導(dǎo)航方法研究［J］.機(jī)器人，2003，25（1）：80-87.（Lu Xinhua，Zhang Guilin.Summarization on indoor service robot navigation［J］.Robot，2003，25（1）：80-87.）

［3］王殿君.基于改進(jìn)A*算法的室內(nèi)移動(dòng)機(jī)器人路徑規(guī)劃［J］.清華大學(xué)學(xué)報(bào) ：自然科學(xué)版，2012，52（8）：1085-1089.（Wang Dianjun.Indoor mobile-robot path planning based on an improved A* algorithm［J］.Journal of Tsinghua University：Science and Technology，2012，52（8）：1085-1089.）

［4］梁軍，韓冬冬，盤(pán)朝奉，等.基于移動(dòng)機(jī)器人的智能車(chē)庫(kù)關(guān)鍵技術(shù)綜述［J］.機(jī)械工程學(xué)報(bào)，2022，58（3）：1-20.（Liang Jun，Han Dongdong，Pan Chaofeng，et al.Review on critical technologies of robot-based intelligent garages［J］.Journal of Mechanical Enginee-ring，2022，58（3）：1-20.）

［5］鮑慶勇，李舜酩，沈峘，等.自主移動(dòng)機(jī)器人局部路徑規(guī)劃綜述［J］.傳感器與微系統(tǒng)，2009，28（9）：1-4，11.（Bao Qingyong，Li Shunming，Shen Huan，et al.Survey of local path planning for autonomous mobile robot［J］.Transducer and Microsystem Technologies，2009，28（9）：1-4，11.）

［6］余卓平，李奕姍，熊璐.無(wú)人車(chē)運(yùn)動(dòng)規(guī)劃算法綜述［J］.同濟(jì)大學(xué)學(xué)報(bào) ：自然科學(xué)版，2017，45（8）：1150-1159.（Yu Zhuoping，Li Yi-shan，Xiong Lu.A review of the motion planning problem of autonomous vehicle［J］.Journal of Tongji University：Natural Science，2017，45（8）：1150-1159.）

［7］Hart P E，Nilsson N J，Raphael B.A formal basis for the heuristic determination of minimum cost paths［J］.IEEE Trans on Systems Science and Cybernetics，1968，4（2）：100-107.

［8］Ju Chunyu，Luo Qinghua，Yan Xiaozhen.Path planning using an improved A* algorithm［C］//Proc of the 11th International Conference on Prognostics and System Health Management.Piscataway，NJ：IEEE Press，2020：23-26.

［9］王中玉，曾國(guó)輝，黃勃，等.改進(jìn)A*算法的機(jī)器人全局最優(yōu)路徑規(guī)劃［J］.計(jì)算機(jī)應(yīng)用，2019，39（9）：2517-2522.（Wang Zhongyu，Zeng Guohui，Huang Bo，et al.Global optimal path planning for robots with improved A* algorithm［J］.Journal of Computer Applications，2019，39（9）：2517-2522.）

［10］陳萬(wàn)通，刁天茹，賈吉慶，等.基于扇形領(lǐng)域擴(kuò)展的同步雙向A*算法［J］.計(jì)算機(jī)應(yīng)用研究，2022，39（1）：118-122，127.（Chen Wantong，Diao Tianru，Jia Jiqing，et al.Research on synchronous bi-directional A* algorithm based on sector field expansion［J］.Application Research of Computers，2022，39（1）：118-122，127.）

［11］Yu Junwei，Hou Jing，Chen Guang.Improved safety-first A* algorithm for autonomous vehicles［C］//Proc of the 5th International Conference on Advanced Robotics and Mechatronics.Piscataway，NJ：IEEE Press，2020：706-710.

［12］辛煜，梁華為，杜明博，等.一種可搜索無(wú)限個(gè)鄰域的改進(jìn)A*算法［J］.機(jī)器人，2014，36（5）：627-633.（Xin Yu，Liang Huawei，Du Mingbo，et al.An improved A* algorithm for searching infinite neighborhoods［J］.Robot，2014，36（5）：627-633.）

［13］陳靖輝，崔巖，劉興林，等.基于改進(jìn)A*算法的移動(dòng)機(jī)器人路徑規(guī)劃方法［J］.計(jì)算機(jī)應(yīng)用研究，2020，37（S1）：118-119.（Chen Jinghui，Cui Yan，Liu Xinglin，et al.Path planning method of mobile robot based on improved A* algorithm［J］.Application Research of Computers，2020，37（S1）：118-119.）

［14］張翼，唐國(guó)金，陳磊.時(shí)相關(guān)車(chē)輛路徑規(guī)劃問(wèn)題的改進(jìn)A*算法［J］.控制工程，2012，19（5）：750-752，756.（Zhang Yi，Tang Guojin，Chen Lei.Improved A* algorithm when related to vehicle routing problems［J］.Control Engineering of China，2012，19（5）：750-752，756.）

［15］劉生偉，馬鉞，孟樹(shù)峰，等.改進(jìn)A*算法的AGV路徑規(guī)劃［J］.計(jì)算機(jī)應(yīng)用，2019，39（S2）：41-44.（Liu Shengwei，Ma Yue，Meng Shufeng，et al.Improved A* algorithm for path planning of AGV［J］.Journal of Computer Applications，2019，39（S2）：41-44.）

［16］Zhang Hongmei，Li Minglong，Yang Le.Safe path planning of mobile robot based on improved A* algorithm in complex terrains［J］.Algorithms，2018，11（4）：44.

［17］于赫年，白樺，李超.倉(cāng)儲(chǔ)式多AGV系統(tǒng)的路徑規(guī)劃研究及仿真［J］.計(jì)算機(jī)工程與應(yīng)用，2020，56（2）：233-241.（Yu Henian，Bai Hua，Li Chao.Research and simulation on path planning of warehouse multi-AGV system［J］.Computer Engineering and Applications，2020，56（2）：233-241.）

［18］耿宏飛，神健杰.A*算法在AGV路徑規(guī)劃上的改進(jìn)與驗(yàn)證［J］.計(jì)算機(jī)應(yīng)用與軟件，2022，39（1）：282-286.（Geng Hongfei，Shen Jianjie.Improvement and verification of A* algorithm in AGV path planning［J］.Computer Applications and Software，2022，39（1）：282-286.）

［19］王洪斌，尹鵬衡，鄭維，等.基于改進(jìn)的A*算法與動(dòng)態(tài)窗口法的移動(dòng)機(jī)器人路徑規(guī)劃［J］.機(jī)器人，2020，42（3）：346-353.（Wang Hongbin，Yin Pengheng，Zheng Wei，et al.Mobile robot path planning based on improved A* algorithm and dynamic window method［J］.Robot，2020，42（3）：346-353.）

［20］Li Xiaoxiao，Hu Xiaoguang，Wang Ziqiang，et al.Path planning based on combination of improved A* algorithm and DWA algorithm［C］//Proc of the 2nd International Conference on Artificial Intelligence and Advanced Manufacture.Piscataway，NJ：IEEE Press，2020：99-103.

收稿日期：2022-04-16；修回日期：2022-06-04基金項(xiàng)目：西南民族大學(xué)中央高校基本科研業(yè)務(wù)費(fèi)專(zhuān)項(xiàng)資金資助項(xiàng)目（2021NYYXS42）

作者簡(jiǎn)介：沈克宇（1997-），男，江蘇徐州人，碩士研究生，主要研究方向?yàn)闄C(jī)器人路徑規(guī)劃；游志宇（1980-），男（通信作者），四川武勝人，講師，碩導(dǎo)，博士，主要研究方向?yàn)槟茉醋儞Q與智能控制（youzhiyu@swun.cn）；劉永鑫（1993-），男，四川瀘州人，碩士研究生，主要研究方向?yàn)橐苿?dòng)機(jī)器人室內(nèi)定位與建圖；黃濤（1999-），男，江西吉安人，碩士研究生，主要研究方向?yàn)橛?jì)算機(jī)視覺(jué)．