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

付久鵬 曾國(guó)輝 黃勃 方志軍

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

關(guān)鍵詞：路徑規(guī)劃;快速探索隨機(jī)樹;狹窄通道;橋梁檢測(cè)算法;障礙物邊緣檢測(cè)

中圖分類號(hào)：TP242.6

文獻(xiàn)標(biāo)志碼：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 引言

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

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

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

1 橋梁檢測(cè)算法

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

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

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

2 改進(jìn)的橋梁檢測(cè)算法

橋梁檢測(cè)算法的提出為窄道內(nèi)采樣提供了一種解決方案，通過檢測(cè)橋梁上三個(gè)點(diǎn)的配置狀態(tài)，從而獲取窄道內(nèi)樣本點(diǎn)。但是，由于原始的橋梁檢測(cè)算法所使用的采樣策略依然是全局隨機(jī)采樣，導(dǎo)致該算法浪費(fèi)了大量時(shí)間用于空曠空間內(nèi)無效節(jié)點(diǎn)的采樣，從而嚴(yán)重降低了算法的檢測(cè)效率。

為提高橋梁檢測(cè)算法的窄道采樣效率，本文對(duì)原始橋梁檢測(cè)算法進(jìn)行改進(jìn)：使用障礙物邊緣坐標(biāo)節(jié)點(diǎn)集合替代原始采樣空間，改進(jìn)橋梁端點(diǎn)構(gòu)建過程與新節(jié)點(diǎn)擴(kuò)展策略，提高算法窄道采樣效率。

2.1 障礙物邊緣檢測(cè)

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

障礙物邊緣檢測(cè)需要對(duì)整個(gè)環(huán)境地圖所有節(jié)點(diǎn)進(jìn)行預(yù)處理，主要分檢測(cè)當(dāng)前節(jié)點(diǎn)配置狀態(tài)和檢測(cè)當(dāng)前節(jié)點(diǎn)的周邊節(jié)點(diǎn)配置狀態(tài)兩步。如圖2所示，如果當(dāng)前節(jié)點(diǎn)x處于障礙物空間內(nèi)并且周邊節(jié)點(diǎn)（序號(hào)1～8）至少有一個(gè)位于自由空間內(nèi)，則可判定當(dāng)前節(jié)點(diǎn)為障礙物邊緣節(jié)點(diǎn)。

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

2.2 橋梁端點(diǎn)的構(gòu)建

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

首先根據(jù)環(huán)境地圖尺寸大小獲得一個(gè)相對(duì)窄道寬度，并以此為半徑r，對(duì)障礙物邊緣節(jié)點(diǎn)集合進(jìn)行隨機(jī)采樣從而獲得橋梁其中一個(gè)端點(diǎn)x，以端點(diǎn)坐標(biāo)x為圓心，繪制一個(gè)圓形區(qū)域[17]，則該圓形區(qū)域內(nèi)所有障礙物邊緣節(jié)點(diǎn)集合W即為橋梁第二個(gè)端點(diǎn)的采樣范圍。

圓形區(qū)域的半徑r可表示為：

其中：Length表示整個(gè)環(huán)境地圖的長(zhǎng)度;Width表示整個(gè)環(huán)境地圖的寬度;α、 β分別為L(zhǎng)ength、Width相對(duì)應(yīng)的比例系數(shù)，其大小可根據(jù)需要人為設(shè)定，一般使半徑r的大小略大于窄道最大寬度。

其次，對(duì)W中節(jié)點(diǎn)根據(jù)W中節(jié)點(diǎn)到橋梁端點(diǎn)x的歐氏距離由大到小降序排序。

由圖3可知，圓形區(qū)域內(nèi)所有障礙物邊緣節(jié)點(diǎn)集合W可表示為：

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

改進(jìn)的橋梁檢測(cè)算法偽代碼如下所示：

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

2.3 窄道樣本點(diǎn)擴(kuò)展

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

以下是改進(jìn)的Connect算法的偽代碼，與原始的Connect算法相比，改進(jìn)部分主要體現(xiàn)在第3）行對(duì)終止條件的判斷，由原先的兩個(gè)停止條件變?yōu)橐粋€(gè)停止條件，即只有遇到障礙物的情況下才停止擴(kuò)展新節(jié)點(diǎn)。

3 仿真與分析

3.1 地圖構(gòu)建與預(yù)處理

為驗(yàn)證本文所提出的改進(jìn)橋梁檢測(cè)算法的有效性，在CPU為IntelCorei5-4300U的聯(lián)想thinkpadX240型筆記本上進(jìn)行了實(shí)驗(yàn)仿真，同時(shí)還與原始RRT-Connect進(jìn)行了對(duì)比實(shí)驗(yàn)，驗(yàn)證了該算法的優(yōu)越性。

圖5為本次仿真實(shí)驗(yàn)所使用的環(huán)境地圖，地圖大小為500×800。圖5中：黑色區(qū)域?yàn)檎系K物空間;白色區(qū)域?yàn)樽杂煽臻g;起始點(diǎn)坐標(biāo)設(shè)置為（10，10），位于地圖左上角;目標(biāo)點(diǎn)坐標(biāo)設(shè)置為（490，790），位于地圖右下角。機(jī)器人需由起始點(diǎn)經(jīng)白色自由空間找到一條通向目標(biāo)點(diǎn)的無障礙路徑。為了體現(xiàn)改進(jìn)橋梁檢測(cè)算法對(duì)于狹窄通道采樣的適用性，在環(huán)境地圖的中心區(qū)域特別地設(shè)置了一條Z型狹窄通道，且該通道是連接起始點(diǎn)與目標(biāo)點(diǎn)之間的必經(jīng)之路。

圖6為障礙物邊緣檢測(cè)處理后的環(huán)境地圖，圖中粗線部分即為障礙物邊緣坐標(biāo)所表示的集合，相對(duì)于對(duì)整個(gè)環(huán)境空間，處理后的障礙物邊緣的樣本集合要小很多，同時(shí)樣本點(diǎn)采樣更為有效。

3.2 仿真分析

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

為了驗(yàn)證本文所提出的改進(jìn)后RRT-Connect算法的規(guī)劃性能，在所設(shè)定的障礙物環(huán)境下，除原始RRT-Connect算法外，還加入了文獻(xiàn)[11]算法方法進(jìn)行對(duì)比，表1顯示了經(jīng)50次仿真后的實(shí)驗(yàn)結(jié)果的平均值。

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

[11] 莫棟成， 劉國(guó)棟. 改進(jìn)的RRT-Connect雙足機(jī)器人路徑規(guī)劃算法[J]. 計(jì)算機(jī)應(yīng)用， 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] 段瑞玲， 李慶祥， 李玉和. 圖像邊緣檢測(cè)方法研究綜述[J]. 光學(xué)技術(shù)， 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] 付妍， 朱曉明， 周秉鋒. 基于圓形參數(shù)域和重要性采樣的三維模型網(wǎng)格重建[J]. 計(jì)算機(jī)學(xué)報(bào)， 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] 霍紅衛(wèi)， 許進(jìn). 快速排序算法研究[J]. 微電子學(xué)與計(jì)算機(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.