簡介

RRT 算法（快速擴展隨機樹，rapidly exploring random tree）是一種隨機性算法，它可以直接應用于非完整約束系統的規劃，不需進行路徑轉換，所以它的算法復雜度較小，尤為適用于高維多自由度的系統。

缺點是得到的路徑質量不是很好。需要后處理進一步優化。
思想是快速擴張一群像樹一樣的路徑以探索（填充）空間的大部分區域，伺機找到可行的路徑。

RRT 的基本步驟是：
　　1. 起點作為一顆種子，從它開始生長枝丫；
　　2. 在機器人的“構型”空間中，生成一個隨機點X；
　　3. 在樹上找到距離X最近的那個點，記為Y吧；
　　4. 朝著X的方向生長，如果沒有碰到障礙物就把生長后的樹枝和端點添加到樹上，返回 2；

?

　　

?

　六維空間的RRT

實際效果如圖。

?

偽代碼

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()

?初始化時隨機樹T只包含一個節點：根節點qinit。首先Sample函數從狀態空間中隨機選擇一個采樣點qrand（4行）；

然后Nearest函數從隨機樹中選擇一個距離qrand最近的節點qnearest（5行）；

最后Extend函數通過從qnearest向qrand擴展一段距離，得到一個新的節點qnew（8行）。如果qnew與障礙物發生碰撞，則Extend函數返回空，放棄這次生長，否則將qnew加入到隨機樹中。重復上述步驟直到qnearest和目標點qgaol距離小于一個閾值，則代表隨機樹到達了目標點，算法返回成功（6~7行）。為了使算法可控，可以設定運行時間上限或搜索次數上限（3行）。如果在限制次數內無法到達目標點，則算法返回失敗。

為了加快隨機樹到達目標點的速度，簡單的改進方法是：在隨機樹每次的生長過程中，根據隨機概率來決定qrand是目標點還是隨機點。在Sample函數中設定參數Prob （probability），每次得到一個0到1.0的隨機值p，當0<p<Prob的時候，隨機樹朝目標點生長；當Prob<p<1.0時，隨機樹朝一個隨機方向生長。

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

?

?

?

上圖說明了RRT在有障礙物和沒有障礙物的情況下探索自由空間的能力。這種特性通常被稱為RRT的Voronoi偏差。作為均勻采樣的結果，規劃器更有可能在更大的Voronoi區域中選擇樣本，并且樹朝著那個自由空間遞增地快速增長。

?

RRT路徑規劃算法

https://blog.csdn.net/aoyousihaiqiuqihuang/article/details/100147478? ?matlab實現，包括后處理smooth

- 關于后處理，有不同的策略，文中采用的貪心策略，是相對簡單的一種

?

一種后處理smooth策略：

#!/usr/bin/python # -*- coding: UTF-8 -*- import numpy as np import random import mathclass Smoother(object):"""@desc; 在規劃好的path中，隨機選擇兩個point(中間至少間隔一個point)，對選中的兩個point采用固定長度的線性插值，若中間插值點未發生碰撞，則用該path替換之前兩點之間的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效果：

?

?

總結

以上是生活随笔為你收集整理的RRT算法的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。