基于雙向快速探索隨機樹的狹窄通道路徑規劃

付久鵬 曾國輝 黃勃 方志軍

摘 要：針對移動機器人路徑規劃過程中基于快速探索隨機樹（RRT）算法難以對窄道進行采樣的問題，提出一種專門用于狹窄通道路徑規劃的改進橋梁檢測算法。首先對環境地圖預處理并提取出障礙物邊緣節點集合作為橋梁檢測算法的采樣空間，從而避免了大量無效采樣點，并使窄道樣本點分布更加合理化;其次改進了橋梁端點的構建過程，提高了橋梁檢測算法的運算效率;最后使用一種輕微變異Connect算法快速擴展窄道樣本點。對于實驗中的窄道環境地圖，與原始RRT-Connect算法相比較，所提改進算法的路徑探索成功率由68%提高到92%。實驗結果表明，該算法能夠較好地完成窄道樣本點采樣并有效地提高路徑規劃效率。

關鍵詞：路徑規劃;快速探索隨機樹;狹窄通道;橋梁檢測算法;障礙物邊緣檢測

中圖分類號：TP242.6

文獻標志碼：A

Abstract： In the process of mobile robot path planning， it is difficult for the Rapidly-exploration Random Tree （RRT） algorithm to sample narrow channels. In order to deal with this problem， an improved bridge detection algorithm was proposed， which is dedicated to narrow channel sampling. Firstly， the environment map was pre-processed and the obstacle edge coordinate set was extracted as the sampling space for the bridge detection algorithm， thus avoiding a large number of invalid sampling points and making the sampling points distribution of the narrow channel more rational. Secondly， the process for bridge endpoint construction was improved， and the operation efficiency of the bridge detection algorithm was increased. Finally， a slight variant Connect algorithm was used to expand the narrow channel sample points rapidly. For the narrow channel environment map in the experiment， the improved algorithm has the success rate increased from 68% to 92% compared with the original RRT-Connect algorithm. Experimental results show that the proposed algorithm can sample the narrow channel well and improve the efficiency of path planning.

Key words：? path planning; Rapidly-exploring Random Tree （RRT）; narrow channel; bridge detection algorithm; obstacle edge detection

0 引言

隨著人工智能等新興產業的發展，有關機器人智能路徑規劃理論和技術的研究與應用引起了人們的高度關注和重視[1]。機器人路徑規劃問題可以描述為機器人在一定約束條件的情況下，依據某個或某些優化準則（如規劃時間最短、行走路徑最優、占用資源最少），在環境空間內找到一條從起始點到目標點的無障礙路徑[2]。近年來，基于快速探索隨機樹（Rapidly-exploring Random Tree， RRT）的規劃方案[3-8]由于其在高維空間中的卓越性能而備受青睞，其中，尤以RRT-Connect算法[5]性能提升最為明顯，該算法在雙向RRT算法[3]的基礎上加入了貪心啟發函數，一定程度上提升了算法的收斂速度。

盡管RRT算法在機器人路徑規劃問題上取得了一定的成功，但面對一些比較特殊的情況，尤其是當環境空間中存在大量狹窄通道時，其性能會出現大幅度下降[9]。主要原因是由于RRT算法普遍采用均勻的全局隨機采樣策略，相對于整個環境空間而言，窄道所占面積比例很小，這就導致窄道內采樣概率很低，隨機樹難以向窄道擴展;同時窄道的幾何約束限制了機器人通過時允許的方向變化，從而使隨機樹的擴展難以通過窄道到達另一側，窄道兩端無法連通，最終算法因為迭代次數達到上限而路徑探索失敗[10]。文獻[11]中提出了一種將RRT-Connect算法與橋梁檢測算法相結合的方法用于解決窄道采樣難的問題，利用橋梁兩端距離偏差不遠的特性改進原始橋梁檢測算法從而獲取第二個橋端點;盡管該算法一定程度上提高了橋梁構建速度，但仍然沒有解決橋梁檢測算法樣本空間范圍大、采樣成功率低等一些固有問題。

從上述文獻中得到啟發，本文在采用RRT-Connect算法進行一般路徑規劃的基礎上，針對狹窄通道采樣難的問題，提出了一種用于窄道路徑規劃的改進橋梁檢測算法[12]。這種方法旨在提高橋梁檢測算法的采樣效率，并使窄道內樣本點分布合理化，從而增加窄道路徑點的連通性。該算法的核心是通過障礙物邊緣檢測，用障礙物邊緣坐標集合取代原始橋梁檢測算法的全局采樣空間，從而使橋梁的兩個端點都位于障礙物邊緣;同時新型的橋梁構建策略減少了不必要的節點采樣，提高了橋梁檢測算法的檢測效率，新的樣本點再通過輕微變異的Connect算法快速擴展出更多的新節點。將基于改進橋梁檢測算法的窄道路徑規劃與基于全局隨機采樣的RRT-Connect算法相結合，可使樣本點在環境地圖中的分布更加合理，從而提高路徑發現的成功率。實驗結果表明改進后的橋梁檢測算法在狹窄通道采樣效率有了明顯的提升，保證了RRT-Connect算法的有效性。

1 橋梁檢測算法

窄道即存在于環境空間中不同障礙物之間的狹窄通道，機器人沿著窄道移動稍有偏差就有可能與障礙物發生碰撞。窄道問題對于任何基于均勻隨機采樣的RRT路徑規劃算法都是一個難點。橋梁檢測算法是一種專門針對狹窄通道的特殊抽樣策略，通過偏向較短橋梁的連接測試，過濾掉大量開闊空間中的采樣點，使得狹窄通道中的樣本密度快速增加，提高了窄道的連通性。在橋梁檢測算法中主要通過檢測三個采樣點的配置狀態：橋梁短線段的兩個端點及其中點。如果兩個端點在障礙物中，且中點位于自由空間內，則中點被判定為處于狹窄通道內的有效樣本點，由于短線段類似于橫跨不同障礙物之間的“橋梁”，因此被稱之為橋梁檢測。

如圖1所示，第一種情況是理想條件下橋梁檢測算法成功實現窄道采樣，第二種為障礙物內無效采樣，第三種為自由空間內無效采樣。盡管橋梁檢測算法能夠有效識別狹窄通道，但當環境空間中障礙物區域分布較為集中的情況下，基于全局均勻采樣策略，其采樣點可能遠離窄道區域，導致樣本點利用率低，采樣效率下降。鑒于環境空間的連續性與采樣策略的隨機性，橋梁檢測算法不可避免地浪費了大量的時間用于后兩種無效的采樣中。

從原始橋梁檢測算法的偽代碼中可以看出：算法通過repeat循環不斷采樣新節點并添加至窄道樣本集G中。在每一次的采樣中，首先調用Rand（）函數于環境空間內隨機采樣一點x，然后根據一個特殊的概率密度函數λq隨機采樣鄰近節點x′，其中概率密度函數λq是個徑向對稱的高斯分布。接著以x、x′作為“橋梁”的兩個端點，計算橋梁xx′的中點q的坐標節點。通過三次調用碰撞檢測[13]函數CollisionFree（）確保橋梁端點x、x′為碰撞點，q為自由點，若三個節點狀態全部滿足，則可以確定q點即為所需的窄道內樣本點，將其添加到窄道樣本集G中，至此完成一次窄道采樣。

2 改進的橋梁檢測算法

橋梁檢測算法的提出為窄道內采樣提供了一種解決方案，通過檢測橋梁上三個點的配置狀態，從而獲取窄道內樣本點。但是，由于原始的橋梁檢測算法所使用的采樣策略依然是全局隨機采樣，導致該算法浪費了大量時間用于空曠空間內無效節點的采樣，從而嚴重降低了算法的檢測效率。

為提高橋梁檢測算法的窄道采樣效率，本文對原始橋梁檢測算法進行改進：使用障礙物邊緣坐標節點集合替代原始采樣空間，改進橋梁端點構建過程與新節點擴展策略，提高算法窄道采樣效率。

2.1 障礙物邊緣檢測

原始橋梁檢測算法首先需要采樣橋梁的其中一個端點，端點通過均勻的全局隨機采樣方法獲得并調用碰撞檢測函數進行配置狀態判斷。這種采樣方法有一個明顯的弊端，即采樣的總體范圍太大導致樣本點利用率低。原因如下：全局隨機采樣的采樣范圍為整個環境空間，即使經碰撞檢測函數過濾，其樣本點也可能為障礙空間內任意一點，假使該點距離窄道較遠，不適合作為橋梁的端點，則繼續操作并無任何意義。因此，為了縮小待檢測橋梁端點采樣范圍，提高樣本點利用率，本文提出了一種障礙物邊緣檢測方法[14-15]，將障礙物邊緣樣本點提取出來，作為橋梁檢測的樣本集來替代原先的采樣空間。相對于原始的橋梁檢測算法，障礙物邊緣樣本集的獲取有效地降低了采樣空間，提高了采樣質量，同時還使得橋梁的構建更加合理。

障礙物邊緣檢測需要對整個環境地圖所有節點進行預處理，主要分檢測當前節點配置狀態和檢測當前節點的周邊節點配置狀態兩步。如圖2所示，如果當前節點x處于障礙物空間內并且周邊節點（序號1～8）至少有一個位于自由空間內，則可判定當前節點為障礙物邊緣節點。

在障礙物邊緣檢測函數偽代碼中，碰撞檢測函數CollisionFree（）返回True表示節點處于自由空間內，返回False表示節點處于障礙物空間。Near（）用于獲取該節點周邊8個節點集合y，根據節點x與周邊節點集合y的配置狀態，從而判斷節點x是否為障礙物邊緣節點。整個函數通過循環檢測整個環境地圖所有節點的配置狀態，提取出所有滿足要求的節點并添加于障礙物邊緣節點集合B′中。

2.2 橋梁端點的構建

原橋梁檢測算法中橋梁的長度由一個服從高斯分布概率密度函數λq決定，過大或過小的λq都會使窄道采樣性能下降，同時還需要對每個自由度都要設計一個不同的高斯方差，且高斯方差依據窄道的寬度得到，這就使得算法的計算難度很大[16]。為避免因λq帶來的算法復雜度上升以及采樣性能下降等問題，本文提出了一種改進橋梁端點構建策略。其結構示意圖如圖3所示

首先根據環境地圖尺寸大小獲得一個相對窄道寬度，并以此為半徑r，對障礙物邊緣節點集合進行隨機采樣從而獲得橋梁其中一個端點x，以端點坐標x為圓心，繪制一個圓形區域[17]，則該圓形區域內所有障礙物邊緣節點集合W即為橋梁第二個端點的采樣范圍。

圓形區域的半徑r可表示為：

其中：Length表示整個環境地圖的長度;Width表示整個環境地圖的寬度;α、 β分別為Length、Width相對應的比例系數，其大小可根據需要人為設定，一般使半徑r的大小略大于窄道最大寬度。

其次，對W中節點根據W中節點到橋梁端點x的歐氏距離由大到小降序排序。

由圖3可知，圓形區域內所有障礙物邊緣節點集合W可表示為：

其中： p表示節點坐標;ab表示線段ab; p∈ab則表示線段ab上的所有節點; p∈cd表示線段cd上的所有節點。圓形區域內所有障礙物邊緣節點集合W由線段ab上的節點與線段cd上的節點兩部分組成。經降序排序[18-19]后，由于圓形區域內圓周上的節點距離圓心x最遠，因此a、b、c、d四個節點排序靠前，當選擇c點或d點任意一點作為橋梁的另一個端點，由圖3可知，橋梁中點q必然位于窄道內中心位置，屬于自由空間，符合橋梁檢測算法要求，將其添加于窄道樣本集G中，至此，成功實現一次窄道樣本點采樣。

改進的橋梁檢測算法偽代碼如下所示：

對比改進后的算法與原始橋梁檢測算法的偽代碼，一方面改進后的算法在橋梁端點的采樣范圍上由整個環境空間縮小為障礙物邊緣節點集合B′，從而減少了兩次碰撞檢測;另一方面，算法改進最大的地方還體現在偽代碼的第2）～6）行，其中：Radius（C）函數用于根據環境空間C的尺寸獲取半徑大小r;NearCircle（x，r，B′）函數用于獲取以x為圓心的圓形區域內所有障礙物邊緣節點集合W;Sort（W）函數用于對W中節點按其到圓心x的距離進行降序排序;BridgeInspection（x，W）表示利用橋梁檢測方法對x與W中節點依次經行檢測，查找所需窄道樣本點。

2.3 窄道樣本點擴展

在通過改進的橋梁檢測算法獲取一定數量的窄道樣本點之后，需要建立起樣本點之間的聯系，簡而言之，就是通過確立每個樣本點的父節點與子節點，試圖構建一條連接窄道空間的隨機樹。本文根據窄道環境下特有的線性結構，同時考慮到窄道樣本點位置的合理分布，提出了一種輕微變異的Connect算法，原始Connect算法是一種貪婪啟發函數，在遇到障礙物或者到達目標區域之前，通過反復調用Extend（）擴展函數不斷生成新節點進而擴展隨機樹。與原始Connect算法不同，本文算法只有當遇到障礙物的情況下才會停止新節點擴展。如圖4所示，當采用改進的橋梁檢測算法獲得窄道內兩個樣本點q和q′后，需要在兩節點之間擴展出更多的新節點，原始Connect算法由節點q向節點q′僅能夠擴展出3個新節點，而應用本文所提出的改進Connect算法則可以快速擴展出更多的新節點，當然新節點不能與已存在的窄道樣本點相同，節點擴展效率得到較大提升，同時，這種新的擴展方式能夠使窄道內樣本點突破窄道范圍采樣限制，擴展出許多處于窄道外的樣本點并保持與窄道內樣本的連通性。

以下是改進的Connect算法的偽代碼，與原始的Connect算法相比，改進部分主要體現在第3）行對終止條件的判斷，由原先的兩個停止條件變為一個停止條件，即只有遇到障礙物的情況下才停止擴展新節點。

3 仿真與分析

3.1 地圖構建與預處理

為驗證本文所提出的改進橋梁檢測算法的有效性，在CPU為IntelCorei5-4300U的聯想thinkpadX240型筆記本上進行了實驗仿真，同時還與原始RRT-Connect進行了對比實驗，驗證了該算法的優越性。

圖5為本次仿真實驗所使用的環境地圖，地圖大小為500×800。圖5中：黑色區域為障礙物空間;白色區域為自由空間;起始點坐標設置為（10，10），位于地圖左上角;目標點坐標設置為（490，790），位于地圖右下角。機器人需由起始點經白色自由空間找到一條通向目標點的無障礙路徑。為了體現改進橋梁檢測算法對于狹窄通道采樣的適用性，在環境地圖的中心區域特別地設置了一條Z型狹窄通道，且該通道是連接起始點與目標點之間的必經之路。

圖6為障礙物邊緣檢測處理后的環境地圖，圖中粗線部分即為障礙物邊緣坐標所表示的集合，相對于對整個環境空間，處理后的障礙物邊緣的樣本集合要小很多，同時樣本點采樣更為有效。

3.2 仿真分析

本文所有實驗均基于Matlab仿真平臺，設定參數如下：步長為10，橋梁半徑為25，橋梁樣本點采樣為500次，最大迭代次數為5000，實驗次數50。實驗結果如圖7～8所示。圖7為原始RRT-Connect在本實驗環境地圖下仿真結果，從中可看出：由原始RRT-Connect算法分別于起始點與目標點生成的兩棵隨機樹在各自所處的空曠區域浪費了大量時間進行采樣，盡管如此，仍然有很大概率無法找到窄道入口;同時，當其中一棵隨機樹進入窄道，Z型窄道結構又使得其容易陷入窄道內無法擴展。因此，當且僅當兩棵隨機樹同時擴展進入窄道內，RRT-Connect算法才有可能探索成功。圖8為結合改進橋梁檢測算法后的RRT-Connect算法的仿真結果，從中可看出：在采樣點的數量上，改進后的算法具有非常大的優勢，樣本點減少的同時也意味著算法消耗時間更短。盡管改進后算法在環境地圖的預處理上花費了一點時間，但障礙物邊緣節點集合的提取有效提高了窄道樣本點的采樣效率與利用率，又因為窄道樣本點均處于窄道中心位置，配合輕微變異的Connect算法能夠快速擴展新的窄道樣本點，這些樣本點又很容易經窄道入口擴展到外部自由區域，同時保持與窄道內樣本點的連接關系，窄道的連通性得到增強，從而使得RRT-Connect的兩棵隨機樹不需要刻意地尋找窄道入口，在擴展一定數量的節點之后，兩棵隨機樹能夠連接上這些外部窄道樣本點，則路徑探索成功。

為了驗證本文所提出的改進后RRT-Connect算法的規劃性能，在所設定的障礙物環境下，除原始RRT-Connect算法外，還加入了文獻[11]算法方法進行對比，表1顯示了經50次仿真后的實驗結果的平均值。

從表1可以更為直觀地看出：相對于傳統RRT-Connect，改進后的RRT-Connect在尋路時間上減少了63.1%，在迭代次數上減少了77.8%，特別值得注意的是在路徑探索的成功率上面，由原來的68%提升到92%，很好地說明了改進后的橋梁檢測算法對于窄道路徑規劃的有效性。與此同時，與文獻[11]方法進行比較，本文提出的改進后的RRT-Connect方法在尋路時間、迭代次數與路徑探索成功率方面均有不同程度上的優勢，這一點主要得益于本文改進算法將橋梁算法端點采樣范圍重新定義為障礙物邊緣區域，縮小了采樣范圍，從而避免了文獻[11]算法采樣過程中可能出現的大量空曠區域內橋梁檢測算法采樣無效的情況，提高了采樣效率。實驗結果表明改進后的RRT-Connect算法所采取的障礙物邊緣采樣策略，克服了基本RRT方法隨機性強、采樣效率低下的缺點，配合輕微變異的Connect節點擴展方法，使得即使在環境地圖中存在大量窄道的情況下，算法也能高效地對窄道進行識別采樣，從而保證了路徑規劃的有效性。

[11] 莫棟成， 劉國棟. 改進的RRT-Connect雙足機器人路徑規劃算法[J]. 計算機應用， 2013， 33（8）： 2289-2292. （MO D C， LIU G D. Improved RRT-Connect path planning algorithm for biped robot[J]. Journal of Computer Applications， 2013， 33（8）： 2289-2292.）

[12] SUN Z， HSU D， JIANG T， et al. Narrow passage sampling for probabilistic roadmap planning[J]. IEEE Transactions on Robotics， 2005， 21（6）： 1105-1115.

[13] BASTA R A， MEHROTRA R， VARANASI M R. Collision detection for planning collision-free motion of two robot arms[C]// Proceedings of the 1998 IEEE International Conference on Robotics and Automation. Piscataway： IEEE， 1988： 638-640.

[14] 段瑞玲， 李慶祥， 李玉和. 圖像邊緣檢測方法研究綜述[J]. 光學技術， 2005， 31（3）： 415-419. （DUAN R L， LI Q X， LI Y H. Summary of image edge detection[J]. Optical Technique， 2005， 31（3）： 415-419.）

[15] JOSHI S R， KOJU R. Study and comparison of edge detection algorithms[C]// Proceedings of the 3rd Asian Himalayas International Conference on Internet. Piscataway： IEEE， 2012： 1-5.

[16] NADARAJAH S， KOTZ S. Exact distribution of the max/min of two Gaussian random variables[J]. IEEE Transactions on Very Large Scale Integration Systems， 2008， 16（2）： 210-212.

[17] 付妍， 朱曉明， 周秉鋒. 基于圓形參數域和重要性采樣的三維模型網格重建[J]. 計算機學報， 2007， 30（12）： 2124-2131. （FU Y， ZHU X M， ZHOU B F. 3D model remeshing using circular parameter domain and importance sampling[J]. Chinese Journal of Computers， 2007， 30（12）： 2124-2131.）

[18] 霍紅衛， 許進. 快速排序算法研究[J]. 微電子學與計算機， 2002（6）： 6-9. （HUO H W， XU J. A study on quicksort algorithm[J]. Microelectronics and Computer， 2002（6）： 6-9.）

[19] YANG Y， YU P， GAN Y. Experimental study on the five sort algorithms[C]// Proceedings of the 2nd International Conference on Mechanic Automation and Control Engineering. Piscataway： IEEE， 2011： 1314-1317.