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

張 慶，劉 旭，彭 力，朱鳳增

物聯(lián)網(wǎng)技術(shù)應(yīng)用教育部工程研究中心（江南大學(xué) 物聯(lián)網(wǎng)工程學(xué)院），江蘇 無(wú)錫214122

路徑規(guī)劃是移動(dòng)機(jī)器人完成復(fù)雜任務(wù)的前提和保證，也是移動(dòng)機(jī)器人的關(guān)鍵技術(shù)之一[1-2]。目的是根據(jù)有障礙物的環(huán)境中的某些優(yōu)化條件，找到從起點(diǎn)到目標(biāo)點(diǎn)的最優(yōu)或次優(yōu)路徑。考慮移動(dòng)機(jī)器人在室內(nèi)環(huán)境中的應(yīng)用，如AGV（automated guided vehicle）小車(chē)[3]、智能家居[4]、無(wú)人機(jī)[5]等，要求機(jī)器人避開(kāi)墻壁等障礙物，選擇最優(yōu)路徑移動(dòng)。在這個(gè)過(guò)程中，小車(chē)不僅需要具備簡(jiǎn)單的避障功能，更重要的是能夠快速實(shí)現(xiàn)路徑規(guī)劃，提高效率。為此，本文研究了具有墻等障礙物的復(fù)雜室內(nèi)環(huán)境下移動(dòng)機(jī)器人的快速路徑規(guī)劃問(wèn)題。

針對(duì)移動(dòng)機(jī)器人的路徑規(guī)劃問(wèn)題，國(guó)內(nèi)外許多學(xué)者進(jìn)行了大量的研究。根據(jù)對(duì)環(huán)境信息掌握程度的不同，可分為全局路徑規(guī)劃[6-9]和局部路徑規(guī)劃[10-13]兩類。本文主要研究了全局路徑規(guī)劃問(wèn)題。目前，用于全局路徑規(guī)劃的方法有蟻群算法[14]、A*算法等。蟻群算法是一種模擬自然界中螞蟻覓食行為的仿生進(jìn)化算法。然而，該算法計(jì)算量大，收斂速度慢，求解時(shí)間長(zhǎng)，且解的質(zhì)量取決于參數(shù)設(shè)置，容易陷入局部最優(yōu)解。隨著遺傳算法種群數(shù)量的增加，也存在著計(jì)算量大、效率低的問(wèn)題，容易陷入“早熟”，使目標(biāo)點(diǎn)無(wú)法到達(dá)。A*算法相比于蟻群算法往往能更快地求出最優(yōu)路徑。但A*算法仍存在一些缺點(diǎn)。由于A*算法在尋路的過(guò)程中，要不斷將每個(gè)節(jié)點(diǎn)及周?chē)藗€(gè)鄰點(diǎn)添加到OpenList 和ClosedList 兩個(gè)列表中進(jìn)行評(píng)估，尋找代價(jià)值最低的節(jié)點(diǎn)，A*算法主要的計(jì)算量在于對(duì)節(jié)點(diǎn)的評(píng)估和代價(jià)最小節(jié)點(diǎn)的選擇。當(dāng)應(yīng)用的場(chǎng)景較大時(shí)，由于計(jì)算量巨大導(dǎo)致計(jì)算速度慢，內(nèi)存占用大，尋路效率低下。針對(duì)此問(wèn)題，文獻(xiàn)[15]提出了將A*算法與迭代加深算法融合，優(yōu)化了狀態(tài)判重和估價(jià)排序，提高了運(yùn)算效率；文獻(xiàn)[16]提出了提高評(píng)估函數(shù)的啟發(fā)強(qiáng)度對(duì)算法進(jìn)行加速，相較于傳統(tǒng)A*算法速度大幅度提高，但存在較高的空間復(fù)雜度，使得算法對(duì)內(nèi)存占用嚴(yán)重；文獻(xiàn)[17]利用無(wú)限領(lǐng)域的思想解決了A*算法局限八運(yùn)動(dòng)方向的問(wèn)題，但降低了運(yùn)算速度；Harabor等人提出跳點(diǎn)搜索算法，對(duì)于A*算法路徑上的節(jié)點(diǎn)進(jìn)行修剪，然后實(shí)現(xiàn)較長(zhǎng)距離的跳躍，大幅度提高了運(yùn)算速度[18]。

本文基于A*算法，但傳統(tǒng)的A*算法難以處理復(fù)雜的工業(yè)大場(chǎng)景，且搜索過(guò)程中存在大量冗余節(jié)點(diǎn)。

因此，本文從兩方面對(duì)傳統(tǒng)A*算法進(jìn)行了設(shè)計(jì)和改進(jìn)：

（1）改進(jìn)啟發(fā)函數(shù)的具體計(jì)算方式，使啟發(fā)式函數(shù)精確地等于實(shí)際最佳路徑，減少A*拓展的節(jié)點(diǎn)。

（2）使用JPS 搜索策略篩選出跳點(diǎn)添加到OpenList 和ClosedList 中代替A*算法中大量不必要的鄰節(jié)點(diǎn)，通過(guò)篩選跳點(diǎn)不斷進(jìn)行拓展，減少了尋路過(guò)程拓展的節(jié)點(diǎn)，提高了搜索效率。

1 問(wèn)題的描述

1.1 環(huán)境的表示方法

在移動(dòng)機(jī)器人路徑規(guī)劃之前，首先建立一個(gè)機(jī)器人坐標(biāo)系系統(tǒng)，把機(jī)器人各傳感器信息傳遞到統(tǒng)一坐標(biāo)系，基于不同的傳感器分別建立里程計(jì)運(yùn)動(dòng)模型、激光雷達(dá)觀測(cè)模型和IMU 運(yùn)動(dòng)模型，最后建立了創(chuàng)建地圖所需的柵格地圖模型。

本文將移動(dòng)機(jī)器人視為二維柵格地圖上的質(zhì)點(diǎn)，移動(dòng)機(jī)器人的位置以坐標(biāo)表示，如圖1 所示。柵格地圖模型將機(jī)器人工作環(huán)境用若干連續(xù)同一尺度的柵格表示，用兩種狀態(tài)表示環(huán)境中的可通行區(qū)域和障礙區(qū)域。圖中空白柵格表示移動(dòng)機(jī)器人可以通過(guò)，即為空閑狀態(tài)；黑色柵格表示不可通行的障礙物，為占據(jù)狀態(tài)。

機(jī)器人實(shí)際運(yùn)行中，會(huì)有多個(gè)方向的移動(dòng)和控制精度導(dǎo)致的偏差。但考慮到模型的復(fù)雜性，A*算法中規(guī)定，移動(dòng)機(jī)器人在不處于邊界和四周有障礙物的情況下，可以向周?chē)? 個(gè)方向的柵格移動(dòng)。由于是二維柵格，不考慮機(jī)器人及環(huán)境高度，如圖2 所示。

1.2 傳統(tǒng)A*尋路算法引入

Fig.1 Grid map圖1 柵格地圖

Fig.2 Motion direction of mobile robot圖2 機(jī)器人運(yùn)動(dòng)方向

A*算法是通過(guò)使用啟發(fā)式函數(shù)來(lái)引導(dǎo)尋路，從而確保有效地找到最優(yōu)路徑。其基本思想是，通過(guò)一個(gè)估價(jià)函數(shù)來(lái)確定搜索的方向，朝著目標(biāo)點(diǎn)開(kāi)始拓展。再利用評(píng)價(jià)函數(shù)計(jì)算得到每個(gè)節(jié)點(diǎn)的代價(jià)值添加到OpenList 中并選擇代價(jià)最小的鄰節(jié)點(diǎn)作為下一個(gè)拓展節(jié)點(diǎn)，重復(fù)這一過(guò)程直到將目標(biāo)點(diǎn)添加進(jìn)OpenList中，最后通過(guò)節(jié)點(diǎn)的父輩關(guān)系反向生成最終路徑。A*算法的評(píng)價(jià)函數(shù)f(n)表示為：

式中，n是當(dāng)前正在拓展的節(jié)點(diǎn)；f(n)表示從起始點(diǎn)經(jīng)過(guò)當(dāng)前節(jié)點(diǎn)n到達(dá)目標(biāo)點(diǎn)的評(píng)價(jià)函數(shù)；g(n)表示起點(diǎn)到節(jié)點(diǎn)n的實(shí)際代價(jià)；h(n)表示節(jié)點(diǎn)n到目標(biāo)點(diǎn)的估計(jì)代價(jià)，h(n)的正確選取能夠提升A*算法的效率和準(zhǔn)確性。傳統(tǒng)A*算法通常采用歐氏距離來(lái)計(jì)算g(n)和h(n)。

式中，(xs,ys)和(xt,yt)分別是起始點(diǎn)Ps坐標(biāo)和目標(biāo)點(diǎn)Pt坐標(biāo)。可以從評(píng)價(jià)函數(shù)清楚看出，A*算法是將Dijkstra 算法和最佳優(yōu)先搜索（best-first-search，BFS）算法相結(jié)合。當(dāng)g(n)=0 的時(shí)候，A*算法就等價(jià)于使用貪心策略的BFS 算法，算法啟發(fā)性更強(qiáng)即偏向于接近目標(biāo)點(diǎn)的節(jié)點(diǎn)，雖然計(jì)算速度最快，但不一定能得到最優(yōu)解；當(dāng)h(n)=0 時(shí)，A*算法就等價(jià)于廣度優(yōu)先搜索的Dijkstra算法，更偏向于接近起點(diǎn)的節(jié)點(diǎn)，雖然一定能得到最優(yōu)解，但計(jì)算量大，效率低下。A*算法結(jié)合了二者的優(yōu)點(diǎn)，評(píng)價(jià)函數(shù)綜合了g(n)和h(n)，在保證找到最優(yōu)路徑的前提下，提高了搜索效率。因此，合理的計(jì)算方式會(huì)調(diào)整A*算法評(píng)價(jià)函數(shù)的權(quán)重比例，能有效提高算法的效果。

1.3 A*算法的實(shí)施

A*算法在進(jìn)行路徑規(guī)劃時(shí)會(huì)定義兩個(gè)鏈表：一個(gè)是ClosedList，用來(lái)存放已經(jīng)遍歷過(guò)的節(jié)點(diǎn)；另一個(gè)是OpenList，用來(lái)存放未遍歷并將要被遍歷的節(jié)點(diǎn)。算法執(zhí)行過(guò)程中，從OpenList中選擇代價(jià)最低節(jié)點(diǎn)加入到ClosedList 同時(shí)把該節(jié)點(diǎn)周?chē)泥徆?jié)點(diǎn)添加到OpenList，并且更新起點(diǎn)到各個(gè)節(jié)點(diǎn)的g(n)和各個(gè)節(jié)點(diǎn)到目標(biāo)點(diǎn)的h(n)，重復(fù)以上步驟直到目標(biāo)點(diǎn)也被添加到OpenList中，算法結(jié)束。A*算法的尋路過(guò)程如圖3 所示。

Fig.3 Path-finding process of A*algorithm圖3 A*算法尋路過(guò)程

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

為了解決A*算法路徑規(guī)劃過(guò)程中存在的計(jì)算量大，內(nèi)存耗費(fèi)嚴(yán)重等問(wèn)題，本文改進(jìn)評(píng)價(jià)函數(shù)計(jì)算方式并結(jié)合JPS 跳點(diǎn)算法對(duì)傳統(tǒng)A*算法進(jìn)行改進(jìn)。

2.1 改進(jìn)評(píng)價(jià)函數(shù)計(jì)算方式

啟發(fā)式函數(shù)作為A*搜索算法的核心，對(duì)算法的行為有重大的影響。當(dāng)啟發(fā)式函數(shù)h(n)始終為0 時(shí)，則將由從起點(diǎn)到節(jié)點(diǎn)n的距離g(n)決定，此時(shí)A*算法將等效于Dijkstra 算法；當(dāng)h(n)始終小于等于節(jié)點(diǎn)n到目標(biāo)點(diǎn)的實(shí)際代價(jià)，則A*算法一定可以求出最優(yōu)解，但是h(n)的值越小，算法需要拓展的節(jié)點(diǎn)越多，計(jì)算量越大；當(dāng)h(n)大于節(jié)點(diǎn)n到目標(biāo)點(diǎn)的實(shí)際代價(jià)，則A*不能保證找到一條最短路徑，但計(jì)算速度更快。

傳統(tǒng)A*算法都是采用歐氏距離計(jì)算h(n)，但是算法定義了機(jī)器人移動(dòng)是8 方向的，導(dǎo)致規(guī)劃出的路徑會(huì)大于歐氏距離計(jì)算值，如圖4 所示，由于歐氏距離計(jì)算出的h(n)小于節(jié)點(diǎn)n到目標(biāo)點(diǎn)的實(shí)際代價(jià)，導(dǎo)致拓展的節(jié)點(diǎn)數(shù)量增加。

Fig.4 Euclidean distance and actual path圖4 歐氏距離與實(shí)際路徑

本文采用切比雪夫距離代替歐氏距離，切比雪夫距離是指兩個(gè)點(diǎn)之間的距離定義為其各坐標(biāo)數(shù)值差的最大值。在向量空間中又稱為L(zhǎng)∞范數(shù)。在直角坐標(biāo)系下，兩點(diǎn)之間的切比雪夫距離為：

因此切比雪夫的啟發(fā)式函數(shù)為：

式中，D是直線移動(dòng)1 格的代價(jià)，(xn,yn)和(xt,yt)分別是當(dāng)前節(jié)點(diǎn)Pn坐標(biāo)和目標(biāo)點(diǎn)Pt坐標(biāo)。

圖4 中，使用歐氏距離計(jì)算出的代價(jià)值低于實(shí)際路徑成本，相當(dāng)于在啟發(fā)函數(shù)前加了一個(gè)縮放的系數(shù)，這樣就導(dǎo)致啟發(fā)性降低，算法更偏向于Dijkstra算法。使用切比雪夫距離計(jì)算的代價(jià)值等價(jià)于實(shí)際路徑成本，算法的啟發(fā)性增強(qiáng)，需要評(píng)估的節(jié)點(diǎn)減少，這樣就能使得算法在保證最優(yōu)路徑的前提下也能提高速度。

如圖5所示，藍(lán)色為起始點(diǎn)，紫色為目標(biāo)點(diǎn)，黃色是添加到OpenList 中的節(jié)點(diǎn)，紅色是添加到ClosedList中參與評(píng)估的節(jié)點(diǎn)，綠色是最終路徑。在同一規(guī)劃任務(wù)下，切比雪夫距離改進(jìn)的算法參與評(píng)估的節(jié)點(diǎn)大大減少，規(guī)劃效果明顯優(yōu)于傳統(tǒng)A*算法。

Fig.5 Simulation results of two kinds of heuristic functions圖5 兩種啟發(fā)函數(shù)的仿真結(jié)果

2.2 JPS 策略下改進(jìn)A*

A*算法結(jié)合JPS 策略是在算法執(zhí)行前增加一個(gè)預(yù)處理的過(guò)程，所謂預(yù)處理就是通過(guò)JPS 迭代跳點(diǎn)搜索函數(shù)，尋找當(dāng)前節(jié)點(diǎn)的繼承跳點(diǎn)，修剪掉中間無(wú)用的節(jié)點(diǎn)，連接跳點(diǎn)實(shí)現(xiàn)兩點(diǎn)間的長(zhǎng)距離跳躍。JPS 算法將會(huì)利用兩個(gè)規(guī)則：修剪規(guī)則和跳躍規(guī)則。

2.2.1 修剪規(guī)則

修剪的基本思想就是拓展到節(jié)點(diǎn)x的時(shí)候，在鄰居集合（即neighbours(x)）中篩選出不需要添加到OpenList 進(jìn)行評(píng)估的任何節(jié)點(diǎn)n，以便最快地到達(dá)目標(biāo)點(diǎn)。通過(guò)比較兩條路徑的代價(jià)值來(lái)進(jìn)行篩選：路徑L1從父節(jié)點(diǎn)p(x)出發(fā)經(jīng)過(guò)節(jié)點(diǎn)x到達(dá)節(jié)點(diǎn)n；另一條路徑L2從父節(jié)點(diǎn)p(x)出發(fā)不經(jīng)過(guò)節(jié)點(diǎn)x到達(dá)節(jié)點(diǎn)n。此外，兩條路徑上的每個(gè)節(jié)點(diǎn)都屬于neighbours(x)。

根據(jù)當(dāng)前節(jié)點(diǎn)的鄰節(jié)點(diǎn)是否存在障礙物，對(duì)修剪規(guī)則分兩種情況進(jìn)行討論。

情況1節(jié)點(diǎn)周?chē)淮嬖谡系K物

如圖6（a）所示，當(dāng)前節(jié)點(diǎn)x是從父節(jié)點(diǎn)p(x)直線向右拓展而至，此時(shí)評(píng)估灰色節(jié)點(diǎn)是毫無(wú)意義的，因?yàn)閺膒(x)經(jīng)過(guò)x到達(dá)這些節(jié)點(diǎn)的代價(jià)一定不會(huì)比從p(x)直接到這些節(jié)點(diǎn)的代價(jià)更低；圖6（b）是當(dāng)前節(jié)點(diǎn)x從父節(jié)點(diǎn)p(x)對(duì)角拓展而至，同樣的，此時(shí)評(píng)估灰色節(jié)點(diǎn)會(huì)導(dǎo)致計(jì)算量大且毫無(wú)意義，為了篩選出跳點(diǎn)，需要修剪掉這些灰色節(jié)點(diǎn)。

Fig.6 Pruning rules without an adjacent obstacle圖6 節(jié)點(diǎn)周?chē)鸁o(wú)障礙物時(shí)修剪規(guī)則

因此，在節(jié)點(diǎn)周?chē)淮嬖谡系K物時(shí)，對(duì)于直線移動(dòng)的拓展，有以下修剪條件：

（1）直線移動(dòng)時(shí)，修剪任何滿足下式的節(jié)點(diǎn)：

（2）對(duì)角移動(dòng)時(shí)，修剪任何滿足下式的節(jié)點(diǎn)：

當(dāng)A*算法拓展到當(dāng)前節(jié)點(diǎn)x時(shí)，根據(jù)上述修剪條件對(duì)不必要鄰節(jié)點(diǎn)進(jìn)行刪除操作，這時(shí)定義x的剩余鄰節(jié)點(diǎn)為自然鄰節(jié)點(diǎn)。

情況2節(jié)點(diǎn)周?chē)嬖谡系K物

如圖7 所示，當(dāng)拓展到節(jié)點(diǎn)x時(shí)，若neighbours(x)存在障礙物，則無(wú)法修剪所有非自然鄰節(jié)點(diǎn)。對(duì)于圖7 中綠色的節(jié)點(diǎn)，不存在一條不經(jīng)過(guò)節(jié)點(diǎn)x的代價(jià)最小路徑可以從p(x)出發(fā)而達(dá)到。若要達(dá)到它們，則必須經(jīng)過(guò)節(jié)點(diǎn)x，否則就不是代價(jià)最低路徑。對(duì)于每一個(gè)這樣的被迫評(píng)估的鄰節(jié)點(diǎn)，給出篩選條件：

（1）n∈neighbours(x)且不是自然鄰節(jié)點(diǎn)；

滿足篩選條件的neighbours(x)定義為強(qiáng)制鄰節(jié)點(diǎn)，在拓展過(guò)程中這些強(qiáng)制鄰節(jié)點(diǎn)將作為跳點(diǎn)進(jìn)行操作。

Fig.7 Pruning rules with adjacent obstacle圖7 節(jié)點(diǎn)周?chē)姓系K物時(shí)修剪規(guī)則

2.2.2 跳躍規(guī)則

根據(jù)修剪規(guī)則，篩選出了所有的自然鄰節(jié)點(diǎn)和強(qiáng)制鄰節(jié)點(diǎn)，這些就是跳點(diǎn)。將所有的跳點(diǎn)添加到OpenList中進(jìn)行代價(jià)評(píng)估，這樣減少了A*算法尋路過(guò)程中對(duì)大量中間節(jié)點(diǎn)的計(jì)算。

相比于傳統(tǒng)A*算法每次只能拓展一格的策略，改進(jìn)后的A*算法可以通過(guò)連接跳點(diǎn)，實(shí)現(xiàn)較長(zhǎng)距離的“跳躍”。在尋路的過(guò)程中只需要花費(fèi)很短的時(shí)間進(jìn)行預(yù)處理，就可以減少大量不必要的節(jié)點(diǎn)，降低了計(jì)算過(guò)程中的內(nèi)存開(kāi)銷(xiāo)，極大地提高了尋路的效率。

如圖8 所示，先從起點(diǎn)S開(kāi)始向水平和豎直方向遞歸拓展（從父節(jié)點(diǎn)到當(dāng)前節(jié)點(diǎn)的方向是拓展的方向，若無(wú)父節(jié)點(diǎn)，則先向水平和豎直方向拓展），如果遞歸過(guò)程中遇到了障礙物，那么遞歸停止，且失敗路徑上的所有節(jié)點(diǎn)都被忽略，JPS 不會(huì)產(chǎn)生任何東西。否則，遞歸將繼續(xù)并導(dǎo)向后繼節(jié)點(diǎn)xn，xn是強(qiáng)制鄰節(jié)點(diǎn)或目標(biāo)點(diǎn)，此時(shí)搜索從當(dāng)前節(jié)點(diǎn)x直接跳到xn，并不會(huì)將沿途的任何一個(gè)節(jié)點(diǎn)添加到OpenList。當(dāng)水平和豎直方向都遞歸失敗時(shí)，才會(huì)在對(duì)角線上進(jìn)行遞歸，這樣可以確保不會(huì)錯(cuò)過(guò)任何一個(gè)轉(zhuǎn)折點(diǎn)。當(dāng)沿著對(duì)角線遞歸到新的節(jié)點(diǎn)時(shí)，重新沿著水平和豎直方向遞歸拓展。

Fig.8 Path-finding process of improved A*algorithm圖8 改進(jìn)A*算法尋路過(guò)程

圖8 中，從起始點(diǎn)S直接跳轉(zhuǎn)到節(jié)點(diǎn)X，繼續(xù)向水平方向遞歸到節(jié)點(diǎn)Y，由于是一個(gè)非失敗的直線式跳躍，遞歸停止；由于節(jié)點(diǎn)Z的原因，若遞歸繼續(xù)，則會(huì)失去一個(gè)轉(zhuǎn)向的機(jī)會(huì)。當(dāng)發(fā)現(xiàn)目標(biāo)節(jié)點(diǎn)G時(shí)，停止遞歸，根據(jù)節(jié)點(diǎn)的父輩關(guān)系，反推出最終路徑。

3 仿真及實(shí)驗(yàn)驗(yàn)證

3.1 仿真分析

為了驗(yàn)證改進(jìn)算法的效果，本文對(duì)傳統(tǒng)A*算法和JPS 策略下改進(jìn)的A*算法進(jìn)行仿真分析。選取了5個(gè)不同尺寸、障礙物密度相同的柵格地圖，在CPU 為i7-9750，操作系統(tǒng)為Windows 10，主頻2.6 GHz，運(yùn)行內(nèi)存為16 GB 的計(jì)算機(jī)上進(jìn)行仿真。

如圖9 所示，兩種算法在40×40 柵格地圖上進(jìn)行仿真實(shí)驗(yàn)，左上角的藍(lán)色格點(diǎn)為起始節(jié)點(diǎn)，右下角的紫色格點(diǎn)為目標(biāo)節(jié)點(diǎn)，黑色為障礙物節(jié)點(diǎn)，綠色節(jié)點(diǎn)為最終生成的路徑。在圖9（a）中，黃色節(jié)點(diǎn)為A*算法添加到ClosedList 評(píng)估的節(jié)點(diǎn)，灰色節(jié)點(diǎn)為添加到OpenList 的節(jié)點(diǎn)；圖9（b）中，黃色節(jié)點(diǎn)為JPS 算法篩選出的跳點(diǎn)，灰色節(jié)點(diǎn)為修剪掉的節(jié)點(diǎn)。可以看出，相同環(huán)境下兩種算法生成的路徑區(qū)別不大，且JPS 策略下改進(jìn)的A*算法在尋路過(guò)程中參與評(píng)估的節(jié)點(diǎn)數(shù)量遠(yuǎn)少于A*算法。

Fig.9 Simulation results of A*algorithm and improved A*algorithm圖9 A*算法和改進(jìn)A*算法的仿真結(jié)果

表1 給出了兩種算法在不同尺寸柵格地圖中尋路的搜索時(shí)間和評(píng)估節(jié)點(diǎn)數(shù)量的對(duì)比。可以清楚地看出，改進(jìn)后的A*算法生成的路徑長(zhǎng)度和A*算法生成的路徑長(zhǎng)度基本相等，且尋路過(guò)程中評(píng)估的節(jié)點(diǎn)數(shù)量大大減少，計(jì)算時(shí)間也明顯降低，減少了尋路過(guò)程中對(duì)內(nèi)存的耗費(fèi)，隨著地圖尺寸的擴(kuò)大，這種效果更加明顯。

仿真結(jié)果表明，JPS 策略下改進(jìn)的A*算法不僅尋路速度遠(yuǎn)超傳統(tǒng)A*算法，對(duì)內(nèi)存的消耗也遠(yuǎn)低于傳統(tǒng)A*算法。

3.2 實(shí)驗(yàn)驗(yàn)證

為了驗(yàn)證改進(jìn)后的A*算法在移動(dòng)機(jī)器人實(shí)際運(yùn)行中的有效性，將改進(jìn)后的算法應(yīng)用到本人研發(fā)的麥克納姆移動(dòng)機(jī)器人上，如圖10（b）所示。移動(dòng)機(jī)器人采用激光測(cè)距雷達(dá)RPLIDAR A1 獲取二維點(diǎn)云數(shù)據(jù)，融合陀螺儀姿態(tài)和里程計(jì)數(shù)據(jù)后利用Gmaping 功能包完成定位和二維地圖的構(gòu)建，最后利用Move_base 功能包中的global_planner 完成機(jī)器人的全局路徑規(guī)劃。本文的實(shí)驗(yàn)場(chǎng)景為實(shí)驗(yàn)室內(nèi)一段過(guò)道，如圖10（b）所示。

如圖11 所示，麥克納姆機(jī)器人在實(shí)驗(yàn)室場(chǎng)地進(jìn)行兩次路徑規(guī)劃，規(guī)劃結(jié)果利用可視化工具Rviz 顯示出來(lái)，其中藍(lán)色方塊為小車(chē)模型，紅色點(diǎn)為激光雷達(dá)數(shù)據(jù)，綠色線條為規(guī)劃出的路徑。

表2 給出了4 種算法在實(shí)驗(yàn)室環(huán)境下兩次路徑規(guī)劃的尋路時(shí)間對(duì)比。可以清楚地看出，本文改進(jìn)后的算法相較于傳統(tǒng)A*算法，同樣能規(guī)劃出近似的路徑，但是速度卻遠(yuǎn)超傳統(tǒng)A*算法；ACO 和GA 算法規(guī)劃的路徑雖然更優(yōu)，但規(guī)劃的時(shí)間太長(zhǎng)，不能滿足移動(dòng)機(jī)器人的實(shí)時(shí)路徑規(guī)劃，因此本文算法更適合移動(dòng)機(jī)器人路徑規(guī)劃。

實(shí)驗(yàn)結(jié)果表明，在JPS 策略下改進(jìn)的A*算法能夠有效提高移動(dòng)機(jī)器人路徑規(guī)劃的速度，減少了內(nèi)存的消耗，提高了尋路效率。

Table 1 Comparison between traditional A*algorithm and improved A*algorithm in this paper表1 傳統(tǒng)A*算法和本文改進(jìn)A*算法的對(duì)比

Fig.10 Mobile robots and experimental environments圖10 移動(dòng)機(jī)器人與實(shí)驗(yàn)環(huán)境

Fig.11 Path planning result of improved A*algorithm圖11 改進(jìn)A*算法的路徑規(guī)劃結(jié)果

Table 2 Comparison of pathfinding time of 4 algorithms under 2 paths表2 2 種路徑下4 種算法尋路時(shí)間對(duì)比

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

本文針對(duì)傳統(tǒng)A*算法在工廠場(chǎng)景較大的柵格地圖路徑規(guī)劃時(shí)，遍歷很多冗余的節(jié)點(diǎn)導(dǎo)致尋路算法內(nèi)存消耗大、計(jì)算速度慢等問(wèn)題，提出了一種融合JPS 跳點(diǎn)算法與A*算法的改進(jìn)策略。通過(guò)對(duì)不同尺寸柵格地圖的路徑規(guī)劃分析與對(duì)比，證明了改進(jìn)算法的有效性。仿真結(jié)果表明，本文算法不僅能很大程度地加速A*算法，而且使系統(tǒng)運(yùn)行時(shí)消耗的內(nèi)存也明顯地減少。

本文也尚存在不足之處，例如本文改進(jìn)的A*算法雖然在路徑規(guī)劃的速度上有了巨大的提升，但最終生成的路徑不夠平滑。在實(shí)際機(jī)器人工作時(shí)，平滑的路徑可以減少電機(jī)的加減速，進(jìn)而提高機(jī)器人的能耗。針對(duì)這個(gè)問(wèn)題將會(huì)引入不同的平滑函數(shù)對(duì)算法進(jìn)行改進(jìn)，進(jìn)一步提高該算法的實(shí)際應(yīng)用價(jià)值。