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

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

0 引言

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

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

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

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

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

1 安全A＊算法

1.1 A＊算法簡介

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

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

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

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

圖1 四節(jié)點和八節(jié)點搜索方式

1.2 安全距離函數(shù)

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

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

其中，μ1和μ2為函數(shù)權(quán)重，l取值為0，1或者，s取值在0到8之間。

1.3 安全A＊仿真

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

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

圖2 傳統(tǒng)A*路徑規(guī)劃

圖3 安全A*路徑規(guī)劃

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

2 人工勢場法

2.1 人工勢場法簡介

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

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

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

圖4 人工勢場法原理圖解

引力場函數(shù)為：

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

斥力場函數(shù)為：

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

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

2.2 人工勢場法改進

人工勢場法的缺陷有很多，當?shù)玫降暮狭榱銜r，機器人的方向無法確定，無法前進或者在當前位置振蕩。當引力與斥力在一條線上時，容易在障礙物前也無法確定方向，無法到達目的地。當?shù)竭_目標點但斥力仍然存在時，會出現(xiàn)機器人無法停在終點的情況。障礙物的速度也應(yīng)考慮，只計算與障礙物之間的距離，也會出現(xiàn)與障礙物碰撞的情況。

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

2.3 改進人工勢場法仿真

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

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

圖5 改進人工勢場法路徑規(guī)劃

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

3.1 安全A＊關(guān)鍵節(jié)點

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

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

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

3.2 融合算法仿真

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

圖6 融合算法路徑規(guī)劃

4 實驗驗證

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

圖7 模擬障礙物環(huán)境

圖8 障礙物環(huán)境掃描建圖

根據(jù)圖7、圖8，對雷達構(gòu)建的地圖使用本文融合算法進行實驗，得出結(jié)果如圖9所示。

圖9 障礙物環(huán)境路徑規(guī)劃

5 結(jié)果分析

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

表1 路徑規(guī)劃算法比較

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