李明鑫，嚴(yán)華

（四川大學(xué)電子信息學(xué)院，成都 610041）

0 引言

路徑規(guī)劃一直以來都是移動智能機器人研究領(lǐng)域的研究熱點，其任務(wù)是在地圖信息已知或未知的環(huán)境中，依據(jù)時間最優(yōu)、能耗最低、距離最短等一條或多條評價指標(biāo)規(guī)劃一條從起始位置到目標(biāo)位置的無碰撞路徑［1］。移動機器人廣泛應(yīng)用于家居、農(nóng)業(yè)、醫(yī)療、工業(yè)以及軍事等方面，而路徑規(guī)劃是移動機器人研究的重要一環(huán)，開展對路徑規(guī)劃的研究具有一定的現(xiàn)實意義。

數(shù)十年來，國內(nèi)外學(xué)者對路徑規(guī)劃進(jìn)行了大量研究，并提出了很多行之有效的路徑規(guī)劃算法。路徑規(guī)劃算法按算法策略可分為以下三類［2］：①啟發(fā)式算法；②仿生算法；③基于概率的算法。啟發(fā)式算法主要有Dijkstra 算法［3］、Greedy Best-First-Search 算法［4］和A*算法［5］。Dijkstra 算法是一種最短路徑算法，但是搜索的區(qū)域較大，時間復(fù)雜度較高。Greedy Best-First-Search 算法搜索的區(qū)域較小，但可能被“欺騙”到“C 型”障礙物區(qū)域內(nèi)部，且不能保證求得最短路徑。A*算法結(jié)合了Dijkstra 和Greedy Best-First-Search 算法，在保證可以獲得最短路徑的同時具有較低的時間復(fù)雜度。仿生算法主要有遺傳算法［6］（ge?netic algorithms）、粒子群算法（particle swarm opti?mization）和蟻群算法［7］（ant colony optimization）。遺傳算法建立在自然選擇和遺傳學(xué)的基礎(chǔ)上，吳曉濤和孫增圻［8］將遺傳算法應(yīng)用于路徑規(guī)劃問題。粒子群的概念來自于對簡化的社會系統(tǒng)的模擬，該系統(tǒng)試圖通過模擬一群鳥類的覓食運動進(jìn)行尋優(yōu)［9］，張萬緒等人［10］將柵格法與粒子群優(yōu)化結(jié)合用于路徑規(guī)劃算法。蟻群算法通過模仿蟻群根據(jù)其屬地以及食物來源的信息尋找路徑的行為進(jìn)行尋優(yōu)，朱慶保和張玉蘭［11］將蟻群算法應(yīng)用于機器人路徑規(guī)劃。基于概率的算法有概率路線圖［12］（probabilistic roadmaps）算法和快速搜索隨機樹［13］（rapidly-exploring random tree）算法。概率路線圖算法首先在自由空間隨機取點并建圖，將地圖簡化，然后使用A*等搜索算法進(jìn)行路徑規(guī)劃?？焖偎阉麟S機樹（RRT）算法是基于隨機采樣的路徑規(guī)劃算法，相對于其它算法，其優(yōu)勢在于可以將非完整約束考慮在算法內(nèi)部，從而不必考慮復(fù)雜的運動學(xué)約束。但快速搜索隨機樹算法本身存在一定的缺陷，其搜索是“漫無目的”的，導(dǎo)致尋找可行解時需要迭代的次數(shù)較多。Goal-bias RRT［14］算法針對該問題進(jìn)行了優(yōu)化，在迭代求解過程中，有一定的概率引導(dǎo)快速搜索隨機樹向目標(biāo)位置生長，使搜索具有了一定的“目的性”。但Goal-bias RRT 算法中提出的概率是固定值，適應(yīng)性較差。針對上述問題，本文通過計算地圖中障礙物的密度、方差等統(tǒng)計信息，對障礙物的分布情況進(jìn)行量化，使用量化值設(shè)置上述概率，使該概率能適應(yīng)不同地圖中障礙物分布情況，更好地引導(dǎo)快速搜索隨機樹生長，達(dá)到減少RRT 算法迭代次數(shù)的目的。

1 算法描述

1.1 地圖表示

為方便地表示地圖，將二維平面劃分為網(wǎng)格，如圖1 所示。在圖1 中，左上角為坐標(biāo)原點，向右為s軸正方向，向下為t軸正方向。灰色區(qū)域為自由空間，黑色區(qū)域為障礙物。程序中使用二維數(shù)組A存儲地圖，0 表示自由空間，1 表示障礙物。使用網(wǎng)格表示地圖，可以方便地判斷某個坐標(biāo)點是否處于障礙物區(qū)域。例如，網(wǎng)格大小為10×10，點的坐標(biāo)為（67，78），可計算得到對應(yīng)的網(wǎng)格坐標(biāo)為（7，8），然后通過查詢二維數(shù)組A即可判斷該點是否處于障礙物中。同樣大小的地圖，網(wǎng)格越小可以將地圖表示的越精細(xì)，隨之而來的是計算量的增加。應(yīng)根據(jù)實際需求進(jìn)行權(quán)衡，設(shè)置網(wǎng)格大小。

圖1 地圖的表示

1.2 RRT和Goal-bias RRT算法描述

RRT 算法的任務(wù)是在給定的有限空間X中，找出一條從起點xinit?X到終點xgoal?X的路徑，同時要求該路徑避開障礙物Xobs?X。

RRT算法描述如下：

算法1RRT算法描述

GENERATE_RRT（xinit，K，Δt）

1T.init（xinit）；

2 fork=1 toKdo

3 if（||xnew-xgoal||

4 break；

5xrand←RANDOM_STATE（）；

6xnear←NEAREST_NEIGHBOR（xrand，T）；

7u←SELECT_INPUT（xrand，xnear）；

8 xnew←NEW_STATE（xnear，u，Δt）；

9 if（judge（xnew）==false）

10 continue；

11T.add_vertex（xnew）；

12T.add_edge（xnear，xnew，u）；

13 returnT

初始化時將xinit加入RRT 結(jié)果集T，經(jīng)過最多K次迭代，若成功求解，則返回結(jié)果，否則求解失敗。其中K是人為設(shè)定的預(yù)期最多的迭代次數(shù)，防止問題無解造成結(jié)果天然地不收斂，進(jìn)而導(dǎo)致算法無法停止。在每次迭代中，首先驗證是否求解完成，完成則停止迭代并返回結(jié)果，否則繼續(xù)迭代。迭代時首先RANDOM_STATE()在給定的有限空間X中選擇隨機點xrand；然后NEAREST_NEIGHBOR(xrand,T) 從T中 選 擇 距 離xrand最近的點xnear；然后SELECT_INPUT(xrand,xnear)和NEW_STATE(xnear,u,Δt) 產(chǎn)生一個新的點xnew；最終judge(xnew)判斷xnew是否滿足非完整約束以及xnear到xnew的路徑上是否存在障礙物Xobs，若存在障礙物則放棄點xnew，否則將xnew以及xnear與xnew形成的邊加入結(jié)果集T。

RRT 算法中RANDOM_STATE（）函數(shù)是從有限空間X 中隨機選取一個點xrand作為每次迭代的目標(biāo)點，該搜索算法是漫無目的的，效率較低。為了解決該問題，LaValle 和Kuffner 提出了Goalbias RRT 算法，對RANDOM_STATE（）函數(shù)進(jìn)行了優(yōu)化，通過設(shè)定一個概率值p，每次迭代以概率p選中xgoal?X作為該次迭代的目標(biāo)點，以概率1-p選擇隨機點作為該次迭代的目標(biāo)點，增加了算法一定的目的性。

1.3 基于地圖統(tǒng)計信息的優(yōu)化RRT算法描述

Goal-bias RRT 算法未考慮障礙物的分布情況，而是設(shè)定一個固定的概率值p，不能適應(yīng)任意的地圖?；诘貓D統(tǒng)計信息的優(yōu)化RRT 算法對RANDOM_STATE（）函數(shù)進(jìn)行了進(jìn)一步優(yōu)化，通過計算障礙物的密度以及均值和方差，進(jìn)而按照一定地策略利用上述統(tǒng)計信息計算出一個引導(dǎo)概率值p，一定程度上增加了對不同地圖的適應(yīng)性。為了使得概率值p反映對路徑求解有較大影響的區(qū)域的障礙物的分布情況，計算障礙物密度以及均值和方差時，只考慮以xinit和xgoal為對角線的矩形內(nèi)部的障礙物，而認(rèn)為該區(qū)域以外的障礙物對路徑求解影響是較小的。

障礙物密度反映了障礙物的數(shù)量，障礙物越多則從xinit到xgoal前進(jìn)時遇到障礙物的概率越大，故障礙物的密度與p值負(fù)相關(guān)。對于二維空間X，分為兩個維度獨立地考察障礙物的均值和方差。若s 軸和t 軸的障礙物分布的均值越接近xinit和xgoal在兩個軸上的中點，從xinit到xgoal前進(jìn)時遇到障礙物的概率越大，故障礙物均值與中點的接近程度與p值負(fù)相關(guān)。障礙物的方差越大，說明障礙物分布越散亂，阻礙xinit到xgoal的路徑的概率越大，故障礙物的方差與p值負(fù)相關(guān)。具體計算方法見式（1）—（5）。

令denseCnt表示障礙物的格子數(shù)，totalCnt表示總體的格子數(shù)，則障礙物密度為：

對于每個維度上障礙物均值與xinit和xgoal中點的接近程度，以s 軸為例，首先求得以xinit和xgoal為對角線的矩形以內(nèi)的障礙物在s軸的均值為aves，則在s 軸上障礙物的均值與中點的接近程度可以由式（2）衡量。

計算可知，式（2）的最大值在aves=(xinits+xgoals)/2 處取得，即最大值在中點處取得，而越偏離中點則值越小，故CloseWithCenter可以正確反映均值與中點的接近程度。t軸同理。

使用方差計算公式分別計算兩個維度的方差Vars和Vart即可。

對于CloseWithCenter和Var，由于其取值會大于1，為使其值分布到0-1 之間，需要使用式（3）和（4）進(jìn)行歸一化處理。以s軸為例，t軸同理。

在上文的分析中表明，障礙物密度、均值與中點的接近程度和方差三項指標(biāo)均與引導(dǎo)概率p呈負(fù)相關(guān)。但上述三項指標(biāo)與p的相關(guān)性不同，故需要增加系數(shù)以區(qū)分相關(guān)性。實驗表明，使用式（5）計算引導(dǎo)概率p效果較好。

求得引導(dǎo)概率p后，將算法1 中算法的步驟5使用算法2代替，其中random（）函數(shù)產(chǎn)生一個0～1之間的隨機數(shù)。

算法2基于地圖統(tǒng)計信息的優(yōu)化RRT 算法中選擇目標(biāo)點的算法

2 實驗驗證

2.1 實驗環(huán)境

在Linux 平臺使用Qt5.5 編寫了交互式GUI 程序進(jìn)行實驗驗證，該程序可以方便地繪制地圖以及使用提出的算法、RRT算法和Goal-bias RRT 算法進(jìn)行路徑規(guī)劃并顯示迭代次數(shù)和路徑長度。使用10幅地圖進(jìn)行實驗，記錄了本文提出的算法求得的各項統(tǒng)計信息以及引導(dǎo)概率，并對比了本文提出的算法、RRT算法和Goal-bias RRT 算法的迭代次數(shù)和路徑長度。

實驗所使用的地圖如圖2 所示，地圖的大小為900×600，網(wǎng)格大小為30×30。第一行從左到右編號為1—5，第二行從左到右編號為6—10。圖中左上角紅色的點為起點xinit，右下角綠色的點為終點xgoal，黑色為障礙物Xobs。

圖2 實驗使用地圖

地圖大小為900×600，網(wǎng)格大小為30×30。第一行從左到右編號為1—5，第二行從左到右編號為6—10

2.2 統(tǒng)計信息和引導(dǎo)概率

表1 顯示了本文提出的算法對圖2 中的10 幅地圖求得的各項統(tǒng)計信息以及引導(dǎo)概率。

從表1 中可以看出，地圖中障礙物分布情況越復(fù)雜，求得的引導(dǎo)概率值p越小，且p值之間差異較大，較為均勻地分布在0～1 之間，符合1.3中的理論分析。

表1 對10幅地圖求得的統(tǒng)計信息以及引導(dǎo)概率

2.3 迭代次數(shù)和路徑長度

使用圖2中的10幅地圖分別做20次實驗，使用本文提出的算法、RRT算法和Goal-bias RRT 算法進(jìn)行路徑規(guī)劃。對于三種算法，同一幅地圖保證實驗中起點xinit和終點xgoal保持不變，記錄迭代次數(shù)和路徑長度。實驗結(jié)果的算術(shù)平均值見表2。

從表2 可以看出，在迭代次數(shù)方面，本文提出的算法相對于RRT 算法有較大的優(yōu)勢，相對于Goal-bias RRT 算法也有一定的優(yōu)勢；在路徑長度方面，本文提出的算法相對于RRT 算法和Goalbias RRT 算法都有所縮短。迭代次數(shù)的減小使得路徑規(guī)劃算法時間復(fù)雜度有所降低，路徑長度的減小使得機器人移動的時間有所減少。

表2 迭代次數(shù)和路徑長度結(jié)果對比

3 結(jié)語

本文提出了一種基于地圖統(tǒng)計信息的優(yōu)化RRT 算法，該算法通過計算地圖中障礙物的統(tǒng)計信息，量化地圖中障礙物的分布情況，進(jìn)而得到能夠適應(yīng)不同地圖的搜索引導(dǎo)概率，以決定每步迭代向終點方向步進(jìn)的概率。從實驗結(jié)果來看，在迭代次數(shù)和路徑長度方面，本文提出的算法要優(yōu)于RRT算法和Goal-bias RRT算法。