基于地圖統計信息的優化RRT算法

李明鑫，嚴華

（四川大學電子信息學院，成都 610041）

0 引言

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

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

1 算法描述

1.1 地圖表示

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

圖1 地圖的表示

1.2 RRT和Goal-bias RRT算法描述

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

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

1.3 基于地圖統計信息的優化RRT算法描述

Goal-bias RRT 算法未考慮障礙物的分布情況，而是設定一個固定的概率值p，不能適應任意的地圖。基于地圖統計信息的優化RRT 算法對RANDOM_STATE（）函數進行了進一步優化，通過計算障礙物的密度以及均值和方差，進而按照一定地策略利用上述統計信息計算出一個引導概率值p，一定程度上增加了對不同地圖的適應性。為了使得概率值p反映對路徑求解有較大影響的區域的障礙物的分布情況，計算障礙物密度以及均值和方差時，只考慮以xinit和xgoal為對角線的矩形內部的障礙物，而認為該區域以外的障礙物對路徑求解影響是較小的。

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

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

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

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

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

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

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

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

算法2基于地圖統計信息的優化RRT 算法中選擇目標點的算法

2 實驗驗證

2.1 實驗環境

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

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

圖2 實驗使用地圖

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

2.2 統計信息和引導概率

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

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

表1 對10幅地圖求得的統計信息以及引導概率

2.3 迭代次數和路徑長度

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

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

表2 迭代次數和路徑長度結果對比

3 結語

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