融合安全A＊算法和改進人工勢場法的巡檢機器人路徑規劃＊

張 強，魯守銀，張家瑞，姜 哲，于世偉，李志鵬

(1.山東建筑大學信息與電氣工程學院，山東 濟南 250000；2.山東建筑大學機器人技術與智能系統研究院)

0 引言

近些年來，機器人技術不斷發展，具有自主導航能力的移動機器人也逐漸進入人們的視野。自主導航的首要條件是移動機器人能夠規劃出合理的路徑，因此路徑規劃也越來越被人們所重視。移動機器人對路徑的規劃方法有很多，其中，通過搜索來找到路徑的方法有JPS 算法[1]，A*算法等；也有使用數學概率來實現路徑搜索的方法，例如RRT 算法[2]、RRT*算法等；此外，隨著智能仿生學發展而出現的遺傳算法和蟻群算法是其中的代表[3]。

總的來說，路徑規劃的算法一部分是針對完整的地圖以及障礙物信息進行的全局規劃[4]。通過規劃可以找到最優的路線，其中代表的是A*算法和Dijkstra算法，但是對路徑中突然出現的障礙物無法及時規避，具有一定的弊端。另一種則是針對附近的障礙物信息進行的規劃，包含有動態窗口法和人工勢場法等，局部路徑規劃的優點在于對路徑上突然出現的障礙物也能有良好的避障效果，但是無法保證路徑最優。

標準A*算法本質上是一種啟發式的搜索算法，具有搜索時間短，可移植性和可重塑性強的特點[5]。但是A*算法規劃出的路徑拐點較多，難以實現機器人控制，且路徑距離障礙物較近，使機器人運動存在危險。人工勢場法在局部的路徑尋找中使用，能夠實時地避開障礙物，提高機器人的安全性，路徑也易于機器人行走。然而，人工勢場法存在局部極小值及目標不可達的問題，且規劃出的路徑往往距離較長。

針對目前方法存在的問題與不足之處，許多學者對此進行了研究改進，也取得了不錯的成效。李曉露[6]等人對A*的搜索鄰域進行改進，將父節點的搜索擴展到所用節點中，減少了路徑的冗余度。陳曉宏[7]等人從底層表征方式出發改進A*算法，通過編碼比較位的變化分段變步長尋找子節點，提高了路徑的合理性。陳繼清[8]等人提出了將人工勢場法添加到A*搜索評價中，提高了A*搜索的避障能力。詹京吳[9]等人提出了一種融合安全A*和動態窗口法的算法，使規劃的路徑更符合機器人本身的運行。鮑久圣[10]等人提出了一種用于無軌膠輪車的A*和人工勢場法融合算法，經實驗后具有良好的效果。

本文將A*算法進行改進，在代價函數添加安全函數，規劃出的路徑具有更高的安全性，并選取路徑的關鍵節點，添加到人工勢場法中，作為引力點指引機器人前進。實現了實時避障和路徑平滑，避免了人工勢場法的部分問題，優化了路徑距離。

1 安全A＊算法

1.1 A＊算法簡介

A*算法是在Dijkstra 算法的基礎上，與貪心算法融合而來，結合了廣度優先和深度優先。在搜索時通過openlist 和closelist 兩個狀態表來對節點進行選擇，基于柵格地圖進行搜索，對每個節點進行考察，將將要考察的路徑節點放到openlist表中，將已考察的節點放到closelist 表中。通過代價值評價函數對節點進行考察，找到最優的路徑，路徑評價函數為：

其中，f(n)為起點由待考察點到終點的代價值，g(n)為從起點到待考察點的實際代價值，h(n)為待考察點到終點的估計值。起點到終點為實際搜索的代價值，待考察點到終點的代價是由坐標估計而來的，距離計算方法有曼哈頓距離，切比雪夫距離和歐幾里得距離，曼哈頓距離是橫縱坐標差之和，切比雪夫距離是橫縱坐標差的最大值，而歐式距離為橫縱坐標差平方和的開根號，故本文采用更加精確的歐氏距離判斷。

搜索過程中，由父節點向周圍相鄰節點擴展，傳統的是向周圍四節點擴展，規劃出的路徑只能橫向或縱向移動，目前更多采用八節點擴展方法，可以向周圍八個點移動。八節點擴展的方法相較于四節點的方法，規劃出的路徑更優，也更為平滑。

圖1 為A*算法兩種不同的擴展搜索方式。分別為四節點搜索和八節點搜索。

圖1 四節點和八節點搜索方式

1.2 安全距離函數

當搜索到目標點時搜索結束，根據搜索的父節點得到搜索出的路徑，A*算法相較于Dijkstra 算法，增加了函數h(n)，在保證搜索準確度的情況下，提高了搜索效率。但是搜索的路徑因為一味力求最優，所以與障礙物距離過近，許多路徑甚至是貼著障礙物。故本文提出在路徑評價函數中添加安全距離函數k(n)，以提高機器人運行的安全性，路徑代價函數為：

引入安全距離函數k(n)評價的是節點周圍距離障礙物的距離l以及障礙物的個數s。在A*的搜索中，距離l 為節點與附近八個相鄰柵格障礙物的最近距離，障礙物個數s 為當前節點相鄰八個柵格中障礙物個數。得到安全距離函數k(n)的公式為：

其中，μ1和μ2為函數權重，l取值為0，1或者，s取值在0到8之間。

1.3 安全A＊仿真

在MATLAB中對本文的方法進行試驗分析，使用尺寸為20*20 的柵格地圖，設置起點坐標為(1,39)，用圓圈來表示，將(39,1)設置為終點，用三角形進行表示。在地圖中設置部分障礙物，設置A*搜索函數。由起點開始，計算起點相鄰八個柵格的代價值f(n)，找到代價值最小的，即為下一個父節點。然后重復計算八個相鄰節點的代價值f(n)，如果某相鄰節點已經計算過，除非新的代價值更低則進行替換，否則不予改變。當節點搜索到目標點時，A*尋路結束，此時的矩陣函數包含該節點之前搜索的所有父節點，所有節點構成從起始點到目標點的路徑，根據節點位置可以得到規劃路徑。

由圖2 可以看出，傳統A*算法規劃的路徑在所用距離上已經達到最優，只有55.25，時間也較快，1.13秒。但是最優路徑有部分是緊貼障礙物行進。因此本文提出引入安全距離函數k(n)的方法，對節點代價值f(n)進行改進，利用MATLAB進行仿真，結果如圖3。

圖2 傳統A*路徑規劃

圖3 安全A*路徑規劃

根據圖3 仿真結果，距離為57.59，時間為2.43 秒。雖然距離和時間有所增加，但是如圖2可以看出，在引入安全距離函數后可以明顯看出，規劃出的路徑與障礙物保留了部分的安全距離，保障了巡檢機器人的安全運行。A*算法的路徑優劣對柵格大小的依賴比較大，過小的柵格對路徑的距離會有縮短，但這會導致搜索時間大大增加。

2 人工勢場法

2.1 人工勢場法簡介

如果出現不在規劃地圖中的障礙物，機器人在運行過程中無法閃避，使用人工勢場法針對機器人周圍檢測到的障礙物進行實時路徑規劃，可以解決突發障礙物的問題。因其簡單效率快等優勢，成為機器人領域比較常用的一種局部路徑規劃方法。

人工勢場法的原理是求解障礙物斥力和目標點引力的合力方向作為機器人運動的方向，直至到達目標點，實現實時避障和路徑規劃。

由圖4 看出，通過計算障礙物斥力與目標點引力的合力，作為機器人運動的方向。

圖4 人工勢場法原理圖解

引力場函數為：

其中，k1為權重系數，d(n,ng)表示當前節點n與目標節點ng的歐氏距離。對勢場的函數求解，可以得到引力函數為：

斥力場函數為：

其中，k2為權重系數，dn0表示當前節點n到障礙物的歐氏距離，d0為設定的障礙物對機器人作用的最遠距離。

因此，計算移動機器人當前位置所受到的合力，判斷機器人下一步運動的方向，通過設置步長，得到機器人下一步運動的位置，從而規劃出移動路徑。

2.2 人工勢場法改進

人工勢場法的缺陷有很多，當得到的合力為零時，機器人的方向無法確定，無法前進或者在當前位置振蕩。當引力與斥力在一條線上時，容易在障礙物前也無法確定方向，無法到達目的地。當到達目標點但斥力仍然存在時，會出現機器人無法停在終點的情況。障礙物的速度也應考慮，只計算與障礙物之間的距離，也會出現與障礙物碰撞的情況。

人工勢場法的局部最優和目標不可達問題都是由于勢場函數的缺陷所造成的，無法完全避免問題的出現，因此首先應對勢場函數進行改進。局部最優和目標不達的情況原因主要是因為在目標點時合力不為零，而在未達到時可能會出現合力為零的情況。因此在斥力勢場函數中添加距離函數，dng為當前點到目標點的距離，保證了機器人只有在到達目標點時引力和斥力同時為零。因此，改進后的斥力勢場函數及斥力函數為：

2.3 改進人工勢場法仿真

利用MATLAB對實驗進行仿真，同樣設置柵格地圖，柵格大小設置為20*20，起始點坐標設置為(1,39)，用圓圈進行表示，目標點坐標設置為(39,1)，用三角形進行表示。在環境中添加與A*算法位置相同的障礙物，A*算法的障礙物為柵格，而人工勢場法障礙物為以柵格為圓心直徑為1 的圓。基于此環境下，根據力的公式進行設置，得到當前位置的合力，設置特定的步長，機器人根據方向和步長到達下一位置，在下一位置繼續重復之前的計算，直至到達目標點并停下。

由圖5 的MATLAB 運行結果可以得出，距離為57.59，時間為0.32秒。人工勢場法因為省去了大量的搜索時間，因此在時間上有所提升。因為障礙物復雜度低的原因，路徑也保持了較高的合理性。

圖5 改進人工勢場法路徑規劃

3 融合安全A＊的人工勢場法

3.1 安全A＊關鍵節點

對于巡檢機器人來說，所需巡檢的位置基本是不變的，采用A*算法初步對路徑進行規劃。由于A*算法規劃的結果只考慮當前存在的障礙物，且規劃后無法改變，故在運行過程中采用人工勢場法，以應對A*的不足。

為了提高機器人運行的安全性，本文采用設計的安全A*算法進行規劃，得出在安全情況下的最優路徑。對最優安全路徑進行采樣，選取其中部分節點，節點的選取主要為A*路徑上的關鍵節點，同斜率的冗余節點不予采用。將提取出來的A*關鍵節點添加到人工勢場法的地圖中，在勢場中添加A*關鍵節點的引力函數，以當前點指向A*關鍵節點作為引力添加到合力中，引導下一步的機器人運動。A*關鍵節點的添加，可以指引人工勢場法向著最優路徑前進，彌補了傳統人工勢場法規劃的缺陷。

提取的關鍵節點數為a，在引力場中設置每個節點對機器人具有不同的吸引力，為了保證機器人向著目標點前進，因此應關鍵節點的引力是逐漸變大的。設當前的關鍵節點為第b個，則第b個節點對障礙物的引力系數為bk1/ak2，經過實驗，設置為b3/a2，因此，A*關鍵節點對機器人的引力合力為：

3.2 融合算法仿真

在MATLAB中進行仿真，建立與之前同等障礙物的20*20 柵格地圖，首先用安全A*算法進行全局仿真，提取安全A*路徑關鍵節點，添加到引力場中，建立勢場函數，將融合安全A*關鍵節點的人工勢場法進行仿真，得到如圖6所示的結果。

圖6 融合算法路徑規劃

4 實驗驗證

利用紙箱模擬障礙物進行實驗，采用rplidar A1M8 與下位機連接，上位機通過虛擬網絡控制臺觀測運行狀況，并獲取地圖。在Ubuntu18.04 系統上，采用hector slam算法[11]對現場進行建圖（圖7、圖8）。

圖7 模擬障礙物環境

圖8 障礙物環境掃描建圖

根據圖7、圖8，對雷達構建的地圖使用本文融合算法進行實驗，得出結果如圖9所示。

圖9 障礙物環境路徑規劃

5 結果分析

由圖6 仿真結果得知，融合算法的路徑長度為56.60，時間為0.31秒，在路徑距離和時間進一步縮短。如表1所示。

表1 路徑規劃算法比較

由表1 對比得知，安全A*算法相較于傳統A*算法，雖然犧牲了一部分路徑距離和計算時間，但在機器人安全性上有了很大提高。融合安全A*和改進人工勢場算法相則在力求路徑更優的情況下做到了實時避障，在固定路徑的巡檢下，具有優異的性能。由圖9可得，在紙箱模擬障礙物的實驗中，本文所述融合算法也具有良好的避障性能。