周 超，谷玉海，任 斌

（1.北京信息科技大學(xué) 現(xiàn)代測控教育部重點(diǎn)實(shí)驗(yàn)室，北京 100192；（2.北京航天長征飛行器研究所，北京 100076）

隨著社會發(fā)展，機(jī)器人的應(yīng)用更為廣泛，移動機(jī)器人的路徑規(guī)劃問題［1］更是目前的一個研究熱點(diǎn)。該問題可以描述為移動機(jī)器人按照某種性能指標(biāo)在有障礙物的環(huán)境里從起點(diǎn)至終點(diǎn)間找到一條最短或最佳的安全無碰撞路徑［2］。常用的路徑規(guī)劃算法有RRT算法［3］、蟻群算法［4-5］、人工勢場算法［6］、SFLA［7］、A*算法［8］等。RRT算法搜索效率高，但得到的路徑往往和最優(yōu)路徑相差甚遠(yuǎn)。蟻群算法的收斂速度相對較慢且容易陷入局部最優(yōu)狀態(tài)。人工勢場算法雖然結(jié)構(gòu)簡單易于實(shí)現(xiàn)，但仍存在目標(biāo)不可達(dá)、障礙物前產(chǎn)生震蕩、規(guī)劃路徑不可達(dá)等問題。相較于其他路徑規(guī)劃算法，A*算法構(gòu)建模型簡單，搜索效率高，針對靜態(tài)場景所得到的解接近最優(yōu)解，對不同的場景具有很強(qiáng)的擴(kuò)展性和適應(yīng)性。A*算法同樣存在很多的不足之處，如生成路徑拐點(diǎn)較多、路徑不夠平滑、大范圍搜索情況下計算量大等問題。針對這些問題，國內(nèi)外研究人員從不同角度提出不同的改進(jìn)方案。Duchoň等［9］在文中總結(jié)了Basic theta*、Phi*、Jump Point Search這3種對A*算法的改進(jìn)方法。Basic theta*和Phi*解決了A*算法不能全方向搜索的問題，最終得到的路徑長度更接近最優(yōu)路徑，但計算效率低；Jump Point Search能夠提高搜索效率，但搜索方向仍受到限制，得到的路徑相較于另外2種搜索方法更長。Guruji A K等［10］針對A*算法在動態(tài)環(huán)境下計算量過大的問題提出減少計算啟發(fā)函數(shù)值次數(shù)的方法，僅在碰撞發(fā)生前計算A*算法啟發(fā)函數(shù)的值，能夠有效提高算法處理效率。辛煜等［11］提出一種將搜索鄰域拓展至無限個的方法使得生成路徑更短，拐點(diǎn)數(shù)更少，但搜索效率會降低。單偉等［12］、王紅衛(wèi)等［13］通過引入平滑模型對生成路徑進(jìn)行二次優(yōu)化，有效減少了所得路徑的轉(zhuǎn)折次數(shù)和轉(zhuǎn)折角度。王殿君［14］、陳若男等［15］針對移動機(jī)器人在室內(nèi)的路徑規(guī)劃問題分別從提高移動機(jī)器人調(diào)整自身姿態(tài)能力和面向障礙物搜索以提升路徑安全性對A*算法做出改進(jìn)。針對大規(guī)劃空間下機(jī)器人路徑規(guī)劃問題，任曉兵等［16］提出一種基于Voronoi圖改進(jìn)的A*算法，對初次規(guī)劃的路徑進(jìn)行二次規(guī)劃，有效減少了空間規(guī)劃面積。趙曉等［17］在A*算法的基礎(chǔ)上結(jié)合了跳點(diǎn)法，有效提高了大規(guī)模搜索下生成路徑的速度。

總體分析，A*算法的改進(jìn)方法有很多，但在移動機(jī)器人路徑規(guī)劃中仍存在以下問題：沒有充分考慮移動機(jī)器人的體積而造成的路徑安全性問題；復(fù)雜地形環(huán)境下算法生成路徑合理性問題。針對上述問題，本文基于A*算法提出以下改進(jìn)方法：

1）通過在存在障礙物的柵格中拓展障礙物矩陣的方法改善移動機(jī)器人易與障礙物邊緣發(fā)生碰撞的問題。

2）引入坡度信息改進(jìn)代價函數(shù)計算方式，考慮到地形坡度對移動機(jī)器人通過性的影響，解決算法生成路徑在復(fù)雜地形中過于陡峭的問題。

1 環(huán)境模型

研究路徑規(guī)劃問題首先需要一個地圖環(huán)境模型，常見的環(huán)境地圖構(gòu)建方法有可視圖法［18］、柵格法［19］、拓?fù)鋱D法。其中柵格法是將機(jī)器人工作環(huán)境分成一系列具有二值信息的網(wǎng)格單元，模型建立簡單，易于維護(hù)，因此本文采用柵格法建立移動機(jī)器人工作環(huán)境的三維地形圖。復(fù)雜環(huán)境下，移動機(jī)器人的路徑規(guī)劃問題應(yīng)更側(cè)重于如何得到一條可以安全通過的路徑，考慮到機(jī)器人自身的體積問題，建立環(huán)境的柵格地圖時選取比移動機(jī)器人自身體積稍大的柵格地圖。同一環(huán)境下，構(gòu)建相對更大的柵格地圖會減少算法中搜索鄰域節(jié)點(diǎn)的計算量，提高運(yùn)行效率。但柵格過大會影響搜索效果，到達(dá)目標(biāo)點(diǎn)的誤差也會更大。因此，設(shè)計柵格地圖時選取機(jī)器人投影面積和每一個柵格大小之比為2∶3的比例構(gòu)建柵格地圖。

本文選取25×25的平面柵格地圖作為平面環(huán)境地圖模型，認(rèn)為每一個柵格是一個節(jié)點(diǎn)，將每個柵格劃分為4×4的小柵格。

由此方法劃分完成，得到一個100×100的二維平面地圖模型，在此基礎(chǔ)上在Matlab中生成1個100×100的高度信息矩陣，用mesh函數(shù)生成一個三維地圖模型，如圖1所示。

圖1 三維地圖模型

2 A*算法

A*算法是一種啟發(fā)式搜索算法，結(jié)合了Dijkstra和BFS 2種算法各自的優(yōu)勢，能夠在保證搜索效率的前提下得到最優(yōu)路徑［20］。其基本思想是通過一個代價函數(shù)［21］計算從起始節(jié)點(diǎn)開始到其每個鄰域節(jié)點(diǎn)的代價值來確定搜索方向。用G（n）表示從起始節(jié)點(diǎn)到當(dāng)前節(jié)點(diǎn)n的實(shí)際代價，H（n）表示從當(dāng)前節(jié)點(diǎn)n到目標(biāo)點(diǎn)的代價估計值，可得代價函數(shù)的計算方式為：A*算法的基本計算流程為：

a）初始化一個OPEN表和一個CLOSE表用于存儲節(jié)點(diǎn)信息。

b）將起點(diǎn)放入OPEN表中，設(shè)其F（n）值為0。

c）判斷OPEN表中存在節(jié)點(diǎn)則選取F（n）值最小節(jié)點(diǎn)N。

d）節(jié)點(diǎn)N是終點(diǎn)，從終點(diǎn)開始追溯每一個節(jié)點(diǎn)的父節(jié)點(diǎn)直到起點(diǎn)，取出所有父節(jié)點(diǎn)即為路徑。

e）N不是終點(diǎn)，將節(jié)點(diǎn)N從OPEN表中取出加入CLOSE表，遍歷N的鄰域節(jié)點(diǎn)，若鄰域節(jié)點(diǎn)M不在CLOSE表中且不在OPEN表中，計算M的F（n）值，并將節(jié)點(diǎn)M加入OPEN表。

圖2是A*算法尋路過程的一個示例。在上述計算過程中，若節(jié)點(diǎn)的一側(cè)存在障礙物，即使其斜向?yàn)榉钦系K物節(jié)點(diǎn)，其尋路方向也不可設(shè)定為斜向，這是為了避免移動機(jī)器人與障礙物發(fā)生碰撞的問題。

圖2 A*算法尋路過程示意圖

在圖2的尋路過程中是將機(jī)器人看作為一個質(zhì)點(diǎn)去進(jìn)行計算的，而實(shí)際上機(jī)器人自身是有一定體積大小的，在上圖計算過程中，認(rèn)定如節(jié)點(diǎn)的正右側(cè)存在障礙物，則當(dāng)前節(jié)點(diǎn)的右上和右下節(jié)點(diǎn)即為不可到達(dá)點(diǎn)，通過這樣的方式避免移動機(jī)器人與障礙物發(fā)生碰撞，但實(shí)際上這種方向并不可取，一是沒有考慮到柵格大小和機(jī)器人大小的關(guān)系，二是如果遇到如圖2中Nj的障礙物情況，是否將該點(diǎn)認(rèn)為是一個障礙物點(diǎn)。針對這一問題，本文提出一種節(jié)點(diǎn)拓展障礙物矩陣的搜索方法。同時考慮在復(fù)雜地形環(huán)境下，地形坡度對路徑規(guī)劃的影響，對代價函數(shù)計算方式進(jìn)行改進(jìn)。

3 優(yōu)化方法

3.1 節(jié)點(diǎn)拓展障礙物矩陣

將移動機(jī)器人在前進(jìn)過程中與周邊障礙發(fā)生碰撞的問題進(jìn)行簡化，將機(jī)器人自身體積在平面坐標(biāo)系的投影視作一個正方形，因此，僅需要考慮正方形的4個角與周邊環(huán)境是否發(fā)生碰撞來進(jìn)行判斷。為解決傳統(tǒng)路徑規(guī)劃算法產(chǎn)生的路徑會導(dǎo)致移動機(jī)器人和障礙物發(fā)生碰撞的問題，提出一種拓展柵格地圖節(jié)點(diǎn)障礙物矩陣的方法，即將柵格地圖中的每個節(jié)點(diǎn)劃分為n階的障礙物節(jié)點(diǎn)，根據(jù)一個節(jié)點(diǎn)內(nèi)障礙物的分布情況劃分為完全障礙物節(jié)點(diǎn)、不完全障礙物節(jié)點(diǎn)和無障礙節(jié)點(diǎn)。

在每次鄰域搜索時，如果當(dāng)前節(jié)點(diǎn)為不完全障礙節(jié)點(diǎn)，需要根據(jù)節(jié)點(diǎn)中障礙物分布的情況判斷目標(biāo)鄰域節(jié)點(diǎn)是否滿足可通行條件。

本文設(shè)定機(jī)器人的投影和每一個柵格大小比例為2∶3，為了便于計算移動機(jī)器人是否與障礙物邊緣存在碰撞可能性，選用4階矩陣來表示每個柵格的障礙物信息并提出如下規(guī)則對節(jié)點(diǎn)進(jìn)行分類：

a）若矩陣A中不存在非0點(diǎn)則認(rèn)為是無障礙節(jié)點(diǎn)。

b）若矩陣A中主對角線或次對角線上存在2個及以上非0點(diǎn)則認(rèn)為該節(jié)點(diǎn)為完全障礙物節(jié)點(diǎn)。

c）若矩陣A中第2、3列或第2、3行存在2個及以上非0點(diǎn)則認(rèn)為該節(jié)點(diǎn)為完全障礙物節(jié)點(diǎn)。

d）除上述情況以外的節(jié)點(diǎn)認(rèn)為是不完全障礙節(jié)點(diǎn)。

首先討論當(dāng)前節(jié)點(diǎn)自身的障礙物分布情況，已知每個節(jié)點(diǎn)都存在8個鄰域節(jié)點(diǎn)，用坐標(biāo)的方式來表示，將當(dāng)前點(diǎn)視為［0，0］，設(shè)定當(dāng)前點(diǎn)0°的方向?yàn)椋?，0］，則其8個鄰域的表示方法如表1所示，每個鄰域搜索方向都有不同的判斷條件，判斷當(dāng)前節(jié)點(diǎn)在其鄰域搜索方向上是否滿足通行條件的順序如圖3所示。

圖3 障礙物矩陣順序示意圖

為方便表示，將節(jié)點(diǎn)自身的障礙物矩陣按照Nobs所示為每一項編號，在進(jìn)行鄰域搜索時，對應(yīng)搜索目標(biāo)鄰域需滿足除表1中對應(yīng)忽略項外其余項均為0，可認(rèn)定當(dāng)前節(jié)點(diǎn)在該方向上可以進(jìn)行搜索。

表1 節(jié)點(diǎn)自身障礙物矩陣忽略項

在滿足節(jié)點(diǎn)自身能夠搜索該鄰域的條件時，判斷目標(biāo)鄰域節(jié)點(diǎn)是否滿足可通行條件。以目標(biāo)鄰域節(jié)點(diǎn)為右上節(jié)點(diǎn)時為例，N0為當(dāng)前節(jié)點(diǎn)的軌跡起始點(diǎn)，在目標(biāo)鄰域節(jié)點(diǎn)不存在障礙物的情況下存在如圖4所示5種情況，此時除考慮N0和當(dāng)前目標(biāo)節(jié)點(diǎn)Ni的障礙物分布，同時也需要考慮其鄰域的障礙物分布情況是否滿足可通行條件，即如圖4中3條實(shí)線構(gòu)成的區(qū)域其障礙物矩陣項的值均為0。

圖4 目標(biāo)鄰域節(jié)點(diǎn)為無障礙節(jié)點(diǎn)時不同起始點(diǎn)的軌跡

當(dāng)目標(biāo)鄰域節(jié)點(diǎn)邊緣存在障礙物時，需要判斷目標(biāo)鄰域節(jié)點(diǎn)的障礙物分布情況，由圖4分析，當(dāng)目標(biāo)鄰域節(jié)點(diǎn)為右上節(jié)點(diǎn)時，考慮機(jī)器人在柵格地圖中移動的軌跡，將滿足可通行條件的障礙物情況分為2種：

a）目標(biāo)鄰域節(jié)點(diǎn)障礙物分布為左上側(cè)時：即Nobs中1，2，5區(qū)域的任意區(qū)域存在障礙，其余區(qū)域均不存在障礙時，可將軌跡中心點(diǎn)進(jìn)行偏移以獲取可通行的路徑。

b）目標(biāo)鄰域節(jié)點(diǎn)障礙物分布為右下側(cè)時：即Nobs中12，15，16區(qū)域的任意區(qū)域存在障礙，其余區(qū)域均不存在障礙時，可將軌跡中心點(diǎn)進(jìn)行偏移以獲取可通行的路徑。

如圖5為節(jié)點(diǎn)左上側(cè)存在障礙區(qū)域，目標(biāo)鄰域節(jié)點(diǎn)Ni的障礙物矩陣中1，2，5為不可通行區(qū)域，而其余區(qū)域滿足可通行的條件，進(jìn)而再判斷相鄰鄰域節(jié)點(diǎn)是否具有可通行的條件，即N0右側(cè)和上方的節(jié)點(diǎn)的障礙物矩陣中被虛線劃過的區(qū)域均不存在障礙物，稱其滿足“借道條件”，則將目標(biāo)鄰域節(jié)點(diǎn)的“中心點(diǎn)”偏移，避開邊角存在的障礙物，從而取得一條從起始點(diǎn)到終點(diǎn)代價較低的安全路徑。圖6為節(jié)點(diǎn)右下側(cè)存在障礙區(qū)域的情況。

3.2 代價函數(shù)的優(yōu)化

計算三維坐標(biāo)系下的距離信息，對啟發(fā)值的計算通常采用歐式距離計算方法：

采用該方法計算時，坡度信息沒有完全的體現(xiàn)在代價函數(shù)的計算值中，因此雖然得到了一條可行路徑，但路徑過于“坎坷”，在復(fù)雜地形環(huán)境下，移動機(jī)器人的路徑規(guī)劃問題更需要考慮坡度因素所帶來的影響，上坡和下坡都會一定程度影響機(jī)器人的運(yùn)動，因?yàn)樾枰x取一條更為平緩的路徑，為實(shí)現(xiàn)這一目標(biāo)，對代價函數(shù)的計算方式進(jìn)行修改。

圖5 目標(biāo)鄰域節(jié)點(diǎn)障礙分布為“左上側(cè)”

圖6 目標(biāo)鄰域節(jié)點(diǎn)障礙分布為“右下側(cè)”

根據(jù)參考文獻(xiàn)［22］中提出的在山地行走時速度和坡度的關(guān)系：

式中：Slope為坡度信息，計算公式為

式中：H為當(dāng)前節(jié)點(diǎn)中最高的高度信息值；L為水平寬度。

對代價函數(shù)中G（n）和H（n）的計算方式進(jìn)行改進(jìn)，仍選取歐式距離的計算方法

式中：ρ為2個節(jié)點(diǎn)間的平面歐式距離，對于存在坡度的路面計算其等效平面距離，根據(jù)式（3）中坡度和速度的關(guān)系，取其等效平面距離為

可得G（n）的計算公式：

相應(yīng)的H（n）也需要考慮和終點(diǎn)間的估計代價值，仍選取歐式距離作為代價函數(shù)的計算方式，在已知當(dāng)前節(jié)點(diǎn)和目標(biāo)點(diǎn)之間的平面歐式距離的基礎(chǔ)上，假設(shè)從當(dāng)前節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的坡度信息均為Slope（n），參照式（6）得到其等效平面距離：

在此基礎(chǔ)上再考慮目標(biāo)點(diǎn)的坡度信息和當(dāng)前節(jié)點(diǎn)坡度信息的差的等效平面距離，可得H（n）的計算方式：

4 仿真分析

無障礙情況下，在不考慮高度信息的情況下由傳統(tǒng)A*算法得到的最短路徑如圖7虛線所示，這條路徑雖然是理論上存在的最短路徑，但沒有考慮復(fù)雜地形對于移動機(jī)器人行動的影響，在此種地形環(huán)境中，路徑規(guī)劃問題應(yīng)更多考慮到實(shí)際地形因素。如圖7實(shí)線所示為改進(jìn)了代價函數(shù)計算方式的A*算法，相較于前者，對地形條件的考慮更為充分，雖然路徑更長，但生成的路徑更為平緩，通過性更好。

圖7 改進(jìn)算法生成路徑

在改進(jìn)代價函數(shù)的基礎(chǔ)上，進(jìn)一步驗(yàn)證在考慮移動機(jī)器人本身體積的情況下在該路徑下是否和障礙物發(fā)生碰撞的問題，在目標(biāo)鄰域節(jié)點(diǎn)為水平方向和對角線方向時進(jìn)行討論，圖8為存在障礙物情況下的路徑規(guī)劃情況，虛線表示改進(jìn)代價函數(shù)的A*算法的避開障礙物的路徑，實(shí)線為拓展障礙物矩陣方法改進(jìn)后生成的路徑，由結(jié)果分析，如果不考慮移動機(jī)器人和障礙物體積而得到的規(guī)劃路徑是一條危險路徑，且在斜方向?yàn)槟繕?biāo)鄰域節(jié)點(diǎn)的情況下，移動機(jī)器人會與障礙物發(fā)生碰撞，現(xiàn)實(shí)情況中移動機(jī)器人的路徑規(guī)劃問題通常會采用局部路徑規(guī)劃和全局路徑規(guī)劃相結(jié)合的方式進(jìn)行實(shí)時避障，在遇到無法通過的障礙物的情況時會進(jìn)行局部的二次規(guī)劃，為減少重復(fù)規(guī)劃的次數(shù)，在進(jìn)行全局的路徑規(guī)劃時應(yīng)得到一條當(dāng)前靜態(tài)環(huán)境下更為安全的路徑。

圖8 拓展障礙物矩陣方法生成路徑

由圖9能夠看出，采用節(jié)點(diǎn)拓展障礙物矩陣的方法之后生成的路徑和障礙物之間留有足夠的安全距離，按照設(shè)定移動機(jī)器人本體大小和柵格地圖比例為2∶3，路徑為移動機(jī)器人中心點(diǎn)的軌跡，圖中路徑與障礙物的最短距離為1個網(wǎng)格長度，而機(jī)器人大小所占據(jù)面積的1/2長度小于1個網(wǎng)格距離，改進(jìn)后的算法能夠解決在目標(biāo)鄰域節(jié)點(diǎn)為對角線方向時，生成的路徑會與障礙物的邊界發(fā)生碰撞的問題。

圖9 障礙物區(qū)域生成路徑對比

圖10為優(yōu)化前后生成的路徑節(jié)點(diǎn)的F（n）值變化曲線，可以看出優(yōu)化前后F（n）變化趨勢基本一致，坡度因素對F（n）的值影響較大，在后半部分的曲線基本重合（由于優(yōu)化后的路線搜索節(jié)點(diǎn)相比于優(yōu)化前多一個，所以圖像后半部分后移一個單位），總體來看，優(yōu)化前后F（n）的變化趨勢基本一致，而優(yōu)化后的算法不會對總的路徑造成較大的影響，而是對存在障礙物的局部區(qū)域進(jìn)行重新規(guī)劃，提高生成路徑安全性的同時保證其路徑最優(yōu)。

圖10 生成路徑的代價估計值曲線

為進(jìn)一步驗(yàn)證算法在遇到柵格邊緣存在障礙物的情況，在原有路徑上添加不完全障礙物節(jié)點(diǎn)，如圖11所示，實(shí)線部分為原路徑即不存在該不完全障礙物節(jié)點(diǎn)時所生成的路徑，虛線為添加不完全障礙物節(jié)點(diǎn)的情況下生成的路徑，相較于不存在該障礙物的情況下向下偏移一個網(wǎng)格的距離，使得生成路徑的安全性更高。

圖11 存在不完全障礙物節(jié)點(diǎn)情況路徑

5 結(jié)論

對傳統(tǒng)A*算法在移動機(jī)器人路徑規(guī)劃應(yīng)用中的問題進(jìn)行改進(jìn)，通過拓展柵格地圖節(jié)點(diǎn)的障礙物矩陣的方法解決了因忽略移動機(jī)器人自身體積和柵格地圖劃分柵格大小造成的生成路徑安全性問題，且生成的路徑更為合理。同時針對在復(fù)雜地形條件下，設(shè)計了新的代價函數(shù)計算方法，引入坡度信息，將陡坡上移動的距離簡化為平面等效距離計算每一個節(jié)點(diǎn)總的代價值，使得生成路徑更趨于平緩。通過仿真結(jié)果驗(yàn)證本文提出的改進(jìn)A*算法能夠在復(fù)雜地形條件下得到一條平緩的路徑且通過拓展節(jié)點(diǎn)障礙物矩陣的方法能夠有效減少移動機(jī)器人與障礙物發(fā)生碰撞的概率。但最后生成的路徑不夠平滑，針對這一問題，下一步工作將從路徑規(guī)劃效率和使生成路徑呈現(xiàn)出更平滑的曲線方面進(jìn)行優(yōu)化。