基于改進(jìn)RRT算法的窄通道路徑規(guī)劃

白利征 閻鑫 齊少璞 趙守智

摘 ? 要：RRT算法是一種經(jīng)典的路徑規(guī)劃算法，但對(duì)于存在窄通道的環(huán)境，其執(zhí)行速度較低。本文進(jìn)行了一些改進(jìn)，先縮小物體找到粗略路徑，再采用雙橋測(cè)試識(shí)別路徑附近的窄通道區(qū)域，增加其中的采樣密度，并采用動(dòng)態(tài)步長(zhǎng)，使采樣步長(zhǎng)隨區(qū)域和碰撞情況自適應(yīng)調(diào)整，提高了窄通道環(huán)境中RRT算法的運(yùn)行效率。

關(guān)鍵詞：快速擴(kuò)展隨機(jī)樹(shù) ?窄通道 ?動(dòng)態(tài)步長(zhǎng) ?雙橋測(cè)試法

中圖分類(lèi)號(hào)：TP24 ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? 文獻(xiàn)標(biāo)識(shí)碼：A ? ? ? ? ? ? ? ? ? ? ? ?文章編號(hào)：1674-098X（2019）11（b）-0027-02

快速擴(kuò)展隨機(jī)樹(shù)（rapidly exploring random tree，簡(jiǎn)稱(chēng)RRT）算法是由美國(guó)愛(ài)荷華州立大學(xué)的Steven Lavalle教授在1998年提出的[1]，在路徑規(guī)劃中已經(jīng)獲得廣泛應(yīng)用。但在有窄通道的復(fù)雜環(huán)境中，由于障礙物之間距離狹小，落在窄通道中的采樣點(diǎn)相對(duì)較少，經(jīng)典的RRT算法將難以找到路徑。

為解決窄通道環(huán)境路徑規(guī)劃的難題，國(guó)內(nèi)外已有大量研究。例如Hsu D等人提出了一種橋測(cè)試法，首先正態(tài)分布生成兩個(gè)點(diǎn)，若這兩個(gè)點(diǎn)都位于障礙物中，則檢測(cè)它們的中點(diǎn)位置，若中點(diǎn)位于自由空間中則認(rèn)為其處于窄通道中，通過(guò)大量的橋測(cè)試確定通道的形狀以便對(duì)其補(bǔ)充采樣[2]，這種方法缺點(diǎn)是容易把障礙物的拐角和凹陷誤認(rèn)為是窄通道。PARK B提出了一種自適應(yīng)環(huán)境的采樣方法，首先把環(huán)境劃分為大小不一的若干區(qū)域，不同區(qū)域間采樣點(diǎn)數(shù)目一致，提取障礙物的邊界點(diǎn)，根據(jù)邊界信息移動(dòng)采樣點(diǎn)使其分布于窄通道中[3]。但這些研究往往對(duì)環(huán)境的全部狀態(tài)空間進(jìn)行采樣，存在效率較低、難以增加有效采樣點(diǎn)的問(wèn)題。

1 ?針對(duì)窄通道問(wèn)題改進(jìn)的RRT算法

若要提高窄通道環(huán)境中的采樣質(zhì)量，需先識(shí)別出環(huán)境中的窄通道區(qū)域，本文參照文獻(xiàn)[4]中的星形試驗(yàn)法，采用正交的雙橋?qū)Σ蓸狱c(diǎn)進(jìn)行測(cè)試，從而使采樣點(diǎn)分布在窄通道中，盡可能不陷入環(huán)境中的拐角和凹陷區(qū)域。

傳統(tǒng)的單橋測(cè)試法需執(zhí)行3次碰撞檢測(cè)，雙橋測(cè)試法需執(zhí)行5次碰撞檢測(cè)，而且窄通道區(qū)域在環(huán)境中的占比很小，如果直接對(duì)環(huán)境整體采樣進(jìn)行雙橋測(cè)試，那么需要進(jìn)行巨量的碰撞計(jì)算，才能識(shí)別出窄通道內(nèi)的點(diǎn)。可以先將物體等比例縮小，用RRT算法查找出多條可行路徑，把路徑節(jié)點(diǎn)列入集合R中，在R中逐點(diǎn)對(duì)原物體進(jìn)行碰撞檢測(cè)，將無(wú)碰撞的點(diǎn)置入集合F中，再使用雙橋法對(duì)F逐點(diǎn)進(jìn)行測(cè)試，提取出窄通道內(nèi)的節(jié)點(diǎn)置入集合Z中。

然后對(duì)原物體進(jìn)行RRT路徑規(guī)劃，以一定概率偏向Z中的點(diǎn)和目標(biāo)位置點(diǎn)采樣，由于復(fù)雜環(huán)境中窄通道區(qū)域常和開(kāi)闊區(qū)域并存，在算法執(zhí)行中應(yīng)將環(huán)境分為若干區(qū)域，設(shè)置步長(zhǎng)隨所在區(qū)域動(dòng)態(tài)調(diào)整。可先大致劃分區(qū)域，在不同區(qū)域設(shè)置初始步長(zhǎng)，再計(jì)算該步長(zhǎng)下，F(xiàn)中節(jié)點(diǎn)在向外拓展時(shí)與障礙物的碰撞概率，根據(jù)“開(kāi)闊區(qū)域采用較大步長(zhǎng)、窄通道附近區(qū)域采用較小步長(zhǎng)，不同區(qū)域內(nèi)F中節(jié)點(diǎn)在向外拓展時(shí)與障礙物的碰撞概率大致相同”的原則調(diào)整區(qū)域劃分和步長(zhǎng)。對(duì)于某些障礙物較多、邊界復(fù)雜的區(qū)域，可設(shè)置步長(zhǎng)為隨機(jī)數(shù)，當(dāng)物體在拓展新節(jié)點(diǎn)與障礙物發(fā)生碰撞時(shí)，以隨機(jī)的小步長(zhǎng)沿采樣點(diǎn)方向生成新的節(jié)點(diǎn)，再進(jìn)行碰撞檢測(cè)，如此可增加障礙物附近的采樣概率。

2 ?仿真分析

由于窄通道環(huán)境中RRT尋路耗時(shí)很長(zhǎng)，限于硬件配置，本文設(shè)置了圖1所示的簡(jiǎn)單窄通道環(huán)境進(jìn)行仿真實(shí)驗(yàn)，長(zhǎng)方形物體共有二維平面的移動(dòng)加旋轉(zhuǎn)3個(gè)自由度，碰撞檢測(cè)算法采用基于分離軸檢測(cè)的凸多面體碰撞算法[5]。

為對(duì)比RRT算法改進(jìn)前后的性能，設(shè)定了不同的采樣和步長(zhǎng)調(diào)整策略，各自執(zhí)行20次RRT算法，得到不同策略對(duì)應(yīng)的執(zhí)行用時(shí)（見(jiàn)表1）。

經(jīng)典RRT算法只是偏向目標(biāo)點(diǎn)采樣，由于落在窄通道中的采樣點(diǎn)很少，所以算法運(yùn)行時(shí)間很長(zhǎng)。改進(jìn)后的RRT算法在識(shí)別出窄通道區(qū)域后，以一定概率偏向目標(biāo)點(diǎn)和通道點(diǎn)采樣，增加了窄通道內(nèi)的采樣密度，拓展節(jié)點(diǎn)時(shí)步長(zhǎng)隨區(qū)域調(diào)整，由于從開(kāi)闊區(qū)域進(jìn)入窄通道時(shí)對(duì)物體位姿約束很強(qiáng)，進(jìn)入窄通道的過(guò)程往往耗時(shí)較長(zhǎng)，該區(qū)域拓展節(jié)點(diǎn)發(fā)生碰撞時(shí)采用隨機(jī)小步長(zhǎng)再次嘗試拓展，可以增加障礙物附近的采樣，加速?gòu)拈_(kāi)闊區(qū)域進(jìn)入窄通道的過(guò)程。綜合利用偏向窄通道的采樣和動(dòng)態(tài)步長(zhǎng)調(diào)整策略，如表1所示，可使RRT算法的規(guī)劃速度提高很多。

3 ?結(jié)語(yǔ)

針對(duì)有窄通道的環(huán)境路徑規(guī)劃速度過(guò)慢的問(wèn)題，本文在應(yīng)用RRT算法時(shí)進(jìn)行了一些簡(jiǎn)單的改進(jìn)，主要從識(shí)別窄通道和采樣步長(zhǎng)兩方面，增加窄通道及附近區(qū)域的采樣密度，仿真實(shí)驗(yàn)表明，改進(jìn)后RRT算法的運(yùn)行時(shí)間能縮小很多。但參數(shù)設(shè)置時(shí)還需大量人為調(diào)整，例如橋測(cè)試的線(xiàn)段長(zhǎng)度、不同區(qū)域的采樣步長(zhǎng)值等等。希望未來(lái)能結(jié)合圖像識(shí)別手段，自動(dòng)設(shè)置相關(guān)參數(shù)，使之擁有更好的環(huán)境適應(yīng)能力。

參考文獻(xiàn)

[1] LAVALLE S. Rapidly-exploring random trees： a new tool for path planning[Z]. Research Report， 1998： 293-308.

[2] HSU D，JIANG T，REIF J，et al.The bridge test for sampling narrow passages with probabilistic roadmap planners[C]// Proc of IEEE International Conference on Robotics and Automation. New York： IEEE Press，2003： 4420-4426.

[3] PARK B，CHUNG W K. Adaptive node sampling method for probabilistic roadmap planners[C]/ / Proc of IEEE / RSJ International Conference on Intelligent Robots and Systems. Piscataway，NJ： IEEE Press，2009： 4399-4405.

[4] 鐘建冬， 蘇劍波. 基于概率路標(biāo)的機(jī)器人狹窄通道路徑規(guī)劃[J]. 控制與決策， 2010， 25（12）：1831-1836.

[5] 張應(yīng)中， 范超， 羅曉芳. 凸多面體連續(xù)碰撞檢測(cè)的運(yùn)動(dòng)軌跡分離軸算法[J]. 計(jì)算機(jī)輔助設(shè)計(jì)與圖形學(xué)學(xué)報(bào)， 2013（1）：7-14.