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

李明鑫，嚴(yán)華

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

0 引言

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

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

1 算法描述

1.1 地圖表示

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

圖1 地圖的表示

1.2 RRT和Goal-bias RRT算法描述

RRT 算法的任務(wù)是在給定的有限空間X中，找出一條從起點(diǎn)xinit?X到終點(diǎn)xgoal?X的路徑，同時(shí)要求該路徑避開障礙物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

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

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

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

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

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

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

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

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

使用方差計(jì)算公式分別計(jì)算兩個(gè)維度的方差Vars和Vart即可。

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

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

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

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

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

2.1 實(shí)驗(yàn)環(huán)境

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

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

圖2 實(shí)驗(yàn)使用地圖

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

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

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

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

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

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

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

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

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

3 結(jié)語(yǔ)

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