張 強(qiáng)，魯守銀，張家瑞，姜 哲，于世偉，李志鵬

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

0 引言

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

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

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

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

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

1 安全A＊算法

1.1 A＊算法簡(jiǎn)介

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

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

搜索過(guò)程中，由父節(jié)點(diǎn)向周?chē)噜徆?jié)點(diǎn)擴(kuò)展，傳統(tǒng)的是向周?chē)墓?jié)點(diǎn)擴(kuò)展，規(guī)劃出的路徑只能橫向或縱向移動(dòng)，目前更多采用八節(jié)點(diǎn)擴(kuò)展方法，可以向周?chē)藗€(gè)點(diǎn)移動(dòng)。八節(jié)點(diǎn)擴(kuò)展的方法相較于四節(jié)點(diǎn)的方法，規(guī)劃出的路徑更優(yōu)，也更為平滑。

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

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

1.2 安全距離函數(shù)

當(dāng)搜索到目標(biāo)點(diǎn)時(shí)搜索結(jié)束，根據(jù)搜索的父節(jié)點(diǎn)得到搜索出的路徑，A*算法相較于Dijkstra 算法，增加了函數(shù)h(n)，在保證搜索準(zhǔn)確度的情況下，提高了搜索效率。但是搜索的路徑因?yàn)橐晃读η笞顑?yōu)，所以與障礙物距離過(guò)近，許多路徑甚至是貼著障礙物。故本文提出在路徑評(píng)價(jià)函數(shù)中添加安全距離函數(shù)k(n)，以提高機(jī)器人運(yùn)行的安全性，路徑代價(jià)函數(shù)為：

引入安全距離函數(shù)k(n)評(píng)價(jià)的是節(jié)點(diǎn)周?chē)嚯x障礙物的距離l以及障礙物的個(gè)數(shù)s。在A*的搜索中，距離l 為節(jié)點(diǎn)與附近八個(gè)相鄰柵格障礙物的最近距離，障礙物個(gè)數(shù)s 為當(dāng)前節(jié)點(diǎn)相鄰八個(gè)柵格中障礙物個(gè)數(shù)。得到安全距離函數(shù)k(n)的公式為：

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

1.3 安全A＊仿真

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

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

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

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

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

2 人工勢(shì)場(chǎng)法

2.1 人工勢(shì)場(chǎng)法簡(jiǎn)介

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

人工勢(shì)場(chǎng)法的原理是求解障礙物斥力和目標(biāo)點(diǎn)引力的合力方向作為機(jī)器人運(yùn)動(dòng)的方向，直至到達(dá)目標(biāo)點(diǎn)，實(shí)現(xiàn)實(shí)時(shí)避障和路徑規(guī)劃。

由圖4 看出，通過(guò)計(jì)算障礙物斥力與目標(biāo)點(diǎn)引力的合力，作為機(jī)器人運(yùn)動(dòng)的方向。

圖4 人工勢(shì)場(chǎng)法原理圖解

引力場(chǎng)函數(shù)為：

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

斥力場(chǎng)函數(shù)為：

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

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

2.2 人工勢(shì)場(chǎng)法改進(jìn)

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

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

2.3 改進(jìn)人工勢(shì)場(chǎng)法仿真

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

由圖5 的MATLAB 運(yùn)行結(jié)果可以得出，距離為57.59，時(shí)間為0.32秒。人工勢(shì)場(chǎng)法因?yàn)槭∪チ舜罅康乃阉鲿r(shí)間，因此在時(shí)間上有所提升。因?yàn)檎系K物復(fù)雜度低的原因，路徑也保持了較高的合理性。

圖5 改進(jìn)人工勢(shì)場(chǎng)法路徑規(guī)劃

3 融合安全A＊的人工勢(shì)場(chǎng)法

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

對(duì)于巡檢機(jī)器人來(lái)說(shuō)，所需巡檢的位置基本是不變的，采用A*算法初步對(duì)路徑進(jìn)行規(guī)劃。由于A*算法規(guī)劃的結(jié)果只考慮當(dāng)前存在的障礙物，且規(guī)劃后無(wú)法改變，故在運(yùn)行過(guò)程中采用人工勢(shì)場(chǎng)法，以應(yīng)對(duì)A*的不足。

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

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

3.2 融合算法仿真

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

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

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

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

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

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

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

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

5 結(jié)果分析

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

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

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