簡(jiǎn)介

RRT 算法（快速擴(kuò)展隨機(jī)樹(shù)，rapidly exploring random tree）是一種隨機(jī)性算法，它可以直接應(yīng)用于非完整約束系統(tǒng)的規(guī)劃，不需進(jìn)行路徑轉(zhuǎn)換，所以它的算法復(fù)雜度較小，尤為適用于高維多自由度的系統(tǒng)。

缺點(diǎn)是得到的路徑質(zhì)量不是很好。需要后處理進(jìn)一步優(yōu)化。
思想是快速擴(kuò)張一群像樹(shù)一樣的路徑以探索（填充）空間的大部分區(qū)域，伺機(jī)找到可行的路徑。

RRT 的基本步驟是：
　　1. 起點(diǎn)作為一顆種子，從它開(kāi)始生長(zhǎng)枝丫；
　　2. 在機(jī)器人的“構(gòu)型”空間中，生成一個(gè)隨機(jī)點(diǎn)X；
　　3. 在樹(shù)上找到距離X最近的那個(gè)點(diǎn)，記為Y吧；
　　4. 朝著X的方向生長(zhǎng)，如果沒(méi)有碰到障礙物就把生長(zhǎng)后的樹(shù)枝和端點(diǎn)添加到樹(shù)上，返回 2；

?

　　

?

　六維空間的RRT

實(shí)際效果如圖。

?

偽代碼

function BuildRRT(qinit, K, Δq)T.init(qinit)for k = 1 to Kqrand = Sample() -- chooses a random configurationqnearest = Nearest(T, qrand) -- selects the node in the RRT tree that is closest to qrandif Distance(qnearest, qgoal) < Threshold thenreturn trueqnew = Extend(qnearest, qrand, Δq) -- moving from qnearest an incremental distance in the direction of qrandif qnew ≠ NULL thenT.AddNode(qnew)return falsefunction Sample() -- Alternatively,one could replace Sample with SampleFree(by using a collision detection algorithm to reject samples in C_obstaclep = Random(0, 1.0)if 0 < p < Prob thenreturn qgoalelseif Prob < p < 1.0 thenreturn RandomNode()

?初始化時(shí)隨機(jī)樹(shù)T只包含一個(gè)節(jié)點(diǎn)：根節(jié)點(diǎn)qinit。首先Sample函數(shù)從狀態(tài)空間中隨機(jī)選擇一個(gè)采樣點(diǎn)qrand（4行）；

然后Nearest函數(shù)從隨機(jī)樹(shù)中選擇一個(gè)距離qrand最近的節(jié)點(diǎn)qnearest（5行）；

最后Extend函數(shù)通過(guò)從qnearest向qrand擴(kuò)展一段距離，得到一個(gè)新的節(jié)點(diǎn)qnew（8行）。如果qnew與障礙物發(fā)生碰撞，則Extend函數(shù)返回空，放棄這次生長(zhǎng)，否則將qnew加入到隨機(jī)樹(shù)中。重復(fù)上述步驟直到qnearest和目標(biāo)點(diǎn)qgaol距離小于一個(gè)閾值，則代表隨機(jī)樹(shù)到達(dá)了目標(biāo)點(diǎn)，算法返回成功（6~7行）。為了使算法可控，可以設(shè)定運(yùn)行時(shí)間上限或搜索次數(shù)上限（3行）。如果在限制次數(shù)內(nèi)無(wú)法到達(dá)目標(biāo)點(diǎn)，則算法返回失敗。

為了加快隨機(jī)樹(shù)到達(dá)目標(biāo)點(diǎn)的速度，簡(jiǎn)單的改進(jìn)方法是：在隨機(jī)樹(shù)每次的生長(zhǎng)過(guò)程中，根據(jù)隨機(jī)概率來(lái)決定qrand是目標(biāo)點(diǎn)還是隨機(jī)點(diǎn)。在Sample函數(shù)中設(shè)定參數(shù)Prob （probability），每次得到一個(gè)0到1.0的隨機(jī)值p，當(dāng)0<p<Prob的時(shí)候，隨機(jī)樹(shù)朝目標(biāo)點(diǎn)生長(zhǎng)；當(dāng)Prob<p<1.0時(shí)，隨機(jī)樹(shù)朝一個(gè)隨機(jī)方向生長(zhǎng)。

原文鏈接：https://blog.csdn.net/a735148617/article/details/103644670

?

?

?

上圖說(shuō)明了RRT在有障礙物和沒(méi)有障礙物的情況下探索自由空間的能力。這種特性通常被稱為RRT的Voronoi偏差。作為均勻采樣的結(jié)果，規(guī)劃器更有可能在更大的Voronoi區(qū)域中選擇樣本，并且樹(shù)朝著那個(gè)自由空間遞增地快速增長(zhǎng)。

?

RRT路徑規(guī)劃算法

https://blog.csdn.net/aoyousihaiqiuqihuang/article/details/100147478? ?matlab實(shí)現(xiàn)，包括后處理smooth

- 關(guān)于后處理，有不同的策略，文中采用的貪心策略，是相對(duì)簡(jiǎn)單的一種

?

一種后處理smooth策略：

#!/usr/bin/python # -*- coding: UTF-8 -*- import numpy as np import random import mathclass Smoother(object):"""@desc; 在規(guī)劃好的path中，隨機(jī)選擇兩個(gè)point(中間至少間隔一個(gè)point)，對(duì)選中的兩個(gè)point采用固定長(zhǎng)度的線性插值，若中間插值點(diǎn)未發(fā)生碰撞，則用該path替換之前兩點(diǎn)之間的path"""def __init__(self):passdef __linecdchecker(self, start, goal):""":param start::param goal::return:"""nps = np.array(start).reshape(-1,1)npg = np.array(goal).reshape(-1,1)nele = math.ceil((abs(npg-nps)/self.__expanddis).max())ratio = np.linspace(0, 1, nele, endpoint=False)jointslist = (nps+(npg-nps)*ratio).T.tolist()for joints in jointslist:iscollided = self.__iscollidedcallback(joints, self.__obstaclelist, self.__robot, self.__cdchecker)if iscollided:return False, []return True, jointslistdef pathsmoothing(self, path, planner, maxiter):"""the path and planner are necessary parametersthe following member variables of planner will be used for smoothing1. jointlimits2. iscollidedcallback3. cdchecker4. robot5. expanddis6. obstaclelist:param path::param planner::return:"""self.__jointlimits = planner.jointlimitsself.__iscollidedcallback = planner.iscollidedcallbackself.__cdchecker = planner.cdcheckerself.__robot = planner.robotself.__expanddis = planner.expanddisself.__obstaclelist = planner.obstaclelistpathlength = len(path)if pathlength <= 3:return pathfor i in range(maxiter):pickpoint0 = random.randint(0, pathlength-3)pickpoint1 = random.randint(pickpoint0+1, pathlength-1)result, addpath = self.__linecdchecker(path[pickpoint0], path[pickpoint1])if result:path = path[:pickpoint0]+addpath+path[pickpoint1:]pathlength = len(path)return path

smooth效果：

?

?

總結(jié)

以上是生活随笔為你收集整理的RRT算法的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問(wèn)題。

如果覺(jué)得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。