文章目錄

• RRT—— 快速擴展隨機算法
• 算法步驟
• • 1. 生成隨機點 Xrand
  • 2. 尋找距離 Xrand 最近的節點 Xnear
  • 3. 生成新的節點 Xnew
  • 4. 再次采樣 Xrand，尋找距離最近的節點 Xnear
  • 5. 繼續采樣，隨著采樣的進行越來越靠近終點
  • 6. 以一定概率選擇終點最為采樣點
• class RRT
• • def `_init_` 初始化
  • def rrt_planning
  • • class Node 設置節點
    • def sample 獲取采樣點
    • def get_nearest_list_index 找到距離采樣點最近的節點下標
    • theta 求解最近節點和采樣點之間的角度，生成新節點方向
    • def get_new_node 得到新的結點
    • def check_segment_collision 判斷障礙物是否在Xnew和Xnear的線段上
    • • def distance_squared_point_to_segment 計算點到直線的距離(重難點)
    • def is_near_goal 判斷是否到了終點附近
    • • def line_cost 計算兩點間的距離
    • def get_final_course 找路徑
• RRT 完整代碼

RRT—— 快速擴展隨機算法

算法步驟

1. 生成隨機點 Xrand

2. 尋找距離 Xrand 最近的節點 Xnear

當前只有根節點，后期的話有了其他新的節點，則找最近的節點

3. 生成新的節點 Xnew

樹的具體生長：將Xnear和Xrand連接起來作為樹具體的生長方向，設置步長(stepsize)作為樹枝的長度，產生新的節點 Xnew

4. 再次采樣 Xrand，尋找距離最近的節點 Xnear

結果發現生長的枝會穿越障礙物，會拋棄這次生長

5. 繼續采樣，隨著采樣的進行越來越靠近終點

設置提前停止的方法：每次生成Xnew節點，都會把該點和終點進行連線，判斷之間的距離是否小于步長且沒有障礙物，這樣就直接把該點和終點連起來。

6. 以一定概率選擇終點最為采樣點

@[TOC]算法講解

class RRT

def _init_ 初始化

def __init__(self, obstacleList, randArea = [-2,18], expandDis = 2.0, goalSampleRate = 10, maxIter = 200):self.start = Noneself.goal = Noneself.min_rand = randArea[0] # 采樣范圍賦值給最小值和最大值self.max_rand = randArea[1] # randArea = [-2, 18]self.expand_dis = expandDis # 設定步長為2self.goal_sample_rate = goalSampleRate # 目標采樣率，有10%的概率每次將終點作為采樣點self.max_iter = maxIter # 設置最大迭代次數為200self.obstacle_list = obstacleList # 設置障礙物位置和大小self.node_list = None # 存儲RRT樹上節點

• 障礙物設置

# 設置障礙物 (x,y,r)最后參數是半徑 obstacleList = [ (3, 3, 1.5), (12, 2, 3), (3, 9, 2), (9, 11, 2),]

def rrt_planning

class Node 設置節點

class Node:def __init__(self, x, y):self.x = xself.y = yself.cost = 0.0self.parent = None

def sample 獲取采樣點

• random.randint(start, stop)：返回指定范圍內的整數
• random.uniform 返回一個隨機浮點數 N ，當 a <= b 時 a <= N <= b ，當 b < a 時 b <= N
  <= a

def sample(self):# randint 是在指定的某個區間內的一個值, 產生(0,100)的隨機整數, ,[low,high]左閉右閉if random.randint(0, 100) > self.goal_sample_rate: # goal_sample_rate = 10# random.uniform 返回一個隨機浮點數 N ，當 a <= b 時 a <= N <= b ，當 b < a 時 b <= N <= arnd = [random.uniform(self.min_rand, self.max_rand), random.uniform(self.min_rand, self.max_rand)]# random.uniform(a,b) 返回a、b之間的隨機浮點值, [a,b], min_read = -2 max_read = 18else: # goal point samplingrnd = [self.goal.x, self.goal.y] # 以終點作為采樣點，返回終點坐標，goal = [15, 12]return rnd

def get_nearest_list_index 找到距離采樣點最近的節點下標

def get_nearest_list_index(nodes, rnd):# 遍歷當前所有的節點，計算采樣點和每個節點的距離dList = [(node.x - rnd[0]) ** 2 + (node.y - rnd[1]) ** 2 for node in nodes]minIndex = dList.index(min(dList)) # 得到列表里距離值最小的下標，dList是列表格式，index最小值的下標return minIndex # 返回下標

theta 求解最近節點和采樣點之間的角度，生成新節點方向

theta = math.atan2(rnd[1] - nearestNode.y, rnd[0] - nearestNode.x)

def get_new_node 得到新的結點

# 生成新的節點, n_ind表示離采樣點最近節點的下標，nearestNode表示離采樣點最近的節點(用x,y表示)def get_new_node(self, theta, n_ind, nearestNode):newNode = copy.deepcopy(nearestNode) # 拷貝最近的節點作為新的節點，然后再增加x，y軸方向的坐標# 進行生長，生長的距離(設置為2)乘以余弦正弦值，得到節點的坐標，expand_dis = 2newNode.x += self.expand_dis * math.cos(theta)newNode.y += self.expand_dis * math.sin(theta)newNode.cost += self.expand_dis # cost值等于長度，記錄長度newNode.parent = n_ind # 記錄這個新節點的父節點，就是 nearestNode 的下標return newNode

def check_segment_collision 判斷障礙物是否在Xnew和Xnear的線段上

def check_segment_collision(self, x1, y1, x2, y2): # 把生成的新節點和最近的節點輸入進來for (ox, oy, size) in self.obstacle_list: # 遍歷所有障礙物# 判斷直線和圓是否相切，只需要判斷圓心到直線的距離# 輸入 Xnew, Xnear, 障礙物圓心# array處理向量的模塊dd = self.distance_squared_point_to_segment(np.array([x1, y1]),np.array([x2, y2]),np.array([ox, oy]))if dd <= size ** 2: # 距離大于半徑則沒有碰撞return False # collisionreturn True

def distance_squared_point_to_segment 計算點到直線的距離(重難點)

• 點到直線的公式(用向量表示)
• 下面公式的值是一個比例，vw與vo的長度比例 = 向量vw的自乘和向量 vp與 vw乘積的比值

python中dot函數用法

• dot()函數可以通過NumPy庫調用，也可以由數組實例對象進行調用。例如：a.dot(b) 與 np.dot(a,b)效果相同。但矩陣積計算不遵循交換律,np.dot(a,b) 和 np.dot(b,a) 得到的結果是不一樣的

# 計算點到直線的距離def distance_squared_point_to_segment(v, w, p):# Return minimum distance between line segment vw and point p# 如果端點v,w是同一個點,則距離就是p到v/w的距離，變成點到點的距離if np.array_equal(v, w):# a.dot(b) 與 np.dot(a, b) 效果相同return (p - v).dot(p - v) # v == w case，返回距離的平方(向量(p-v)的x^2+y^2)l2 = (w - v).dot(w - v) # i.e. |w-v|^2 - avoid a sqrt 計算w和v的線段長度的平方# Consider the line extending the segment,# parameterized as v + t (w - v).# We find projection of point p onto the line.# It falls where t = [(p-v) . (w-v)] / |w-v|^2# We clamp t from [0,1] to handle points outside the segment vw.t = max(0, min(1, (p - v).dot(w - v) / l2)) # (p - v) 與 (w - v)做點積，就是向量pv與向量wv做乘積，t是個比例projection = v + t * (w - v) # Projection falls on the segment 圖中的o點return (p - projection).dot(p - projection) # 返回p 和 o點的距離的平方

def is_near_goal 判斷是否到了終點附近

def is_near_goal(self, node):d = self.line_cost(node, self.goal) # 計算新加入點和終點的距離if d < self.expand_dis: # 距離小于步長的話，則表示和終點非常接近，可以直接和終點連起來return Truereturn False

def line_cost 計算兩點間的距離

def line_cost(node1, node2):return math.sqrt((node1.x - node2.x) ** 2 + (node1.y - node2.y) ** 2)

def get_final_course 找路徑

def get_final_course(self, lastIndex): # 找路徑，傳入終點值path = [[self.goal.x, self.goal.y]]while self.node_list[lastIndex].parent is not None: # 當parent = 0是就找到終點了node = self.node_list[lastIndex]path.append([node.x, node.y])lastIndex = node.parentpath.append([self.start.x, self.start.y])return path

RRT 完整代碼

import copy import math import random import timeimport matplotlib.pyplot as plt from scipy.spatial.transform import Rotation as Rot import numpy as npshow_animation = Trueclass RRT:def __init__(self, obstacleList, randArea = [-2,18], expandDis = 2.0, goalSampleRate = 10, maxIter = 200):self.start = Noneself.goal = Noneself.min_rand = randArea[0] # 采樣范圍賦值給最小值和最大值self.max_rand = randArea[1] # randArea = [-2, 18]self.expand_dis = expandDis # 設定步長為2self.goal_sample_rate = goalSampleRate # 目標采樣率，有10%的概率每次將終點作為采樣點self.max_iter = maxIter # 設置最大迭代次數為200self.obstacle_list = obstacleList # 設置障礙物位置和大小self.node_list = None # 存儲RRT樹上節點# start = [0, 0], goal = [15, 12], animation = show_animation (開頭設置show_animation = True)def rrt_planning(self, start, goal, animation = True):start_time = time.time() # 進行計時，統計時間self.start = Node(start[0], start[1]) # 將起點和終點以node節點的方式存儲起來,start[0]表示起點x值self.goal = Node(goal[0], goal[1]) # Node 是一個類 class Nodeself.node_list = [self.start] # 將起點加入到node列表中，起點作為根節點path = Nonefor i in range(self.max_iter): # 進行for循環，到最大迭代次數200rnd = self.sample() # 獲取采樣點rnd# 求采樣點和節點的距離，返回采樣點和各個節點距離值最小的節點的下標 n_indn_ind = self.get_nearest_list_index(self.node_list, rnd)nearestNode = self.node_list[n_ind] # 使用下標找到距離采樣點最近的節點 Xnear# steertheta = math.atan2(rnd[1] - nearestNode.y, rnd[0] - nearestNode.x) # 求解最近節點和采樣點之間的角度newNode = self.get_new_node(theta, n_ind, nearestNode) # 生長過程，生成新的節點noCollision = self.check_segment_collision(newNode.x, newNode.y, nearestNode.x, nearestNode.y)if noCollision: # 沒有碰撞，就可以把新的節點加入到樹里面self.node_list.append(newNode)if animation:self.draw_graph(newNode, path)# 判斷是否到了終點附近if self.is_near_goal(newNode):if self.check_segment_collision(newNode.x, newNode.y,self.goal.x, self.goal.y): # 判斷路徑是否與障礙物發生碰撞lastIndex = len(self.node_list) - 1 # 沒有碰撞的話就找到終點，下標從0開始，長度減1path = self.get_final_course(lastIndex)pathLen = self.get_path_len(path)print("current path length: {}, It costs {} s".format(pathLen, time.time()-start_time))if animation:self.draw_graph(newNode, path)return pathdef rrt_star_planning(self, start, goal, animation=True):start_time = time.time()self.start = Node(start[0], start[1])self.goal = Node(goal[0], goal[1])self.node_list = [self.start]path = NonelastPathLength = float('inf')for i in range(self.max_iter): # 迭代次數rnd = self.sample() # 獲取采樣點# 找離采樣點最近的節點，最開始找的離初始點近的結點(有步長)n_ind = self.get_nearest_list_index(self.node_list, rnd)nearestNode = self.node_list[n_ind] # 由索引得到距離最近的節點# steertheta = math.atan2(rnd[1] - nearestNode.y, rnd[0] - nearestNode.x) # 求解采樣點和最近的節點的角度值newNode = self.get_new_node(theta, n_ind, nearestNode) # 由角度、最近點的父節點的索引值、最近節點得到新的節點# 判斷是否有障礙物noCollision = self.check_segment_collision(newNode.x, newNode.y, nearestNode.x, nearestNode.y)if noCollision:nearInds = self.find_near_nodes(newNode)newNode = self.choose_parent(newNode, nearInds)self.node_list.append(newNode)self.rewire(newNode, nearInds)if animation:self.draw_graph(newNode, path)if self.is_near_goal(newNode):if self.check_segment_collision(newNode.x, newNode.y,self.goal.x, self.goal.y):lastIndex = len(self.node_list) - 1tempPath = self.get_final_course(lastIndex)tempPathLen = self.get_path_len(tempPath)if lastPathLength > tempPathLen:path = tempPathlastPathLength = tempPathLenprint("current path length: {}, It costs {} s".format(tempPathLen, time.time()-start_time))return pathdef informed_rrt_star_planning(self, start, goal, animation=True):start_time = time.time()self.start = Node(start[0], start[1])self.goal = Node(goal[0], goal[1])self.node_list = [self.start]# max length we expect to find in our 'informed' sample space,# starts as infinitecBest = float('inf')path = None# Computing the sampling spacecMin = math.sqrt(pow(self.start.x - self.goal.x, 2)+ pow(self.start.y - self.goal.y, 2))xCenter = np.array([[(self.start.x + self.goal.x) / 2.0],[(self.start.y + self.goal.y) / 2.0], [0]])a1 = np.array([[(self.goal.x - self.start.x) / cMin],[(self.goal.y - self.start.y) / cMin], [0]])e_theta = math.atan2(a1[1], a1[0])# 論文方法求旋轉矩陣（2選1）# first column of identity matrix transposed# id1_t = np.array([1.0, 0.0, 0.0]).reshape(1, 3)# M = a1 @ id1_t# U, S, Vh = np.linalg.svd(M, True, True)# C = np.dot(np.dot(U, np.diag(# [1.0, 1.0, np.linalg.det(U) * np.linalg.det(np.transpose(Vh))])),# Vh)# 直接用二維平面上的公式（2選1）C = np.array([[math.cos(e_theta), -math.sin(e_theta), 0],[math.sin(e_theta), math.cos(e_theta), 0],[0, 0, 1]])for i in range(self.max_iter):# Sample space is defined by cBest# cMin is the minimum distance between the start point and the goal# xCenter is the midpoint between the start and the goal# cBest changes when a new path is foundrnd = self.informed_sample(cBest, cMin, xCenter, C)n_ind = self.get_nearest_list_index(self.node_list, rnd)nearestNode = self.node_list[n_ind]# steertheta = math.atan2(rnd[1] - nearestNode.y, rnd[0] - nearestNode.x)newNode = self.get_new_node(theta, n_ind, nearestNode)noCollision = self.check_segment_collision(newNode.x, newNode.y, nearestNode.x, nearestNode.y)if noCollision:nearInds = self.find_near_nodes(newNode)newNode = self.choose_parent(newNode, nearInds)self.node_list.append(newNode)self.rewire(newNode, nearInds)if self.is_near_goal(newNode):if self.check_segment_collision(newNode.x, newNode.y,self.goal.x, self.goal.y):lastIndex = len(self.node_list) - 1tempPath = self.get_final_course(lastIndex)tempPathLen = self.get_path_len(tempPath)if tempPathLen < cBest:path = tempPathcBest = tempPathLenprint("current path length: {}, It costs {} s".format(tempPathLen, time.time()-start_time))if animation:self.draw_graph_informed_RRTStar(xCenter=xCenter,cBest=cBest, cMin=cMin,e_theta=e_theta, rnd=rnd, path=path)return pathdef sample(self):# randint 是在指定的某個區間內的一個值, 產生(0,100)的隨機整數, ,[low,high]左閉右閉if random.randint(0, 100) > self.goal_sample_rate: # goal_sample_rate = 10# random.uniform 返回一個隨機浮點數 N ，當 a <= b 時 a <= N <= b ，當 b < a 時 b <= N <= arnd = [random.uniform(self.min_rand, self.max_rand), random.uniform(self.min_rand, self.max_rand)]# random.uniform(a,b) 返回a、b之間的隨機浮點值, [a,b], min_read = -2 max_read = 18else: # goal point samplingrnd = [self.goal.x, self.goal.y] # 以終點作為采樣點，返回終點坐標，goal = [15, 12]return rnddef choose_parent(self, newNode, nearInds):if len(nearInds) == 0:return newNodedList = []for i in nearInds:dx = newNode.x - self.node_list[i].xdy = newNode.y - self.node_list[i].yd = math.hypot(dx, dy)theta = math.atan2(dy, dx)if self.check_collision(self.node_list[i], theta, d):dList.append(self.node_list[i].cost + d)else:dList.append(float('inf'))minCost = min(dList)minInd = nearInds[dList.index(minCost)]if minCost == float('inf'):print("min cost is inf")return newNodenewNode.cost = minCostnewNode.parent = minIndreturn newNodedef find_near_nodes(self, newNode):n_node = len(self.node_list)r = 50.0 * math.sqrt((math.log(n_node) / n_node))d_list = [(node.x - newNode.x) ** 2 + (node.y - newNode.y) ** 2for node in self.node_list]near_inds = [d_list.index(i) for i in d_list if i <= r ** 2]return near_indsdef informed_sample(self, cMax, cMin, xCenter, C):if cMax < float('inf'):r = [cMax / 2.0,math.sqrt(cMax ** 2 - cMin ** 2) / 2.0,math.sqrt(cMax ** 2 - cMin ** 2) / 2.0]L = np.diag(r)xBall = self.sample_unit_ball()rnd = np.dot(np.dot(C, L), xBall) + xCenterrnd = [rnd[(0, 0)], rnd[(1, 0)]]else:rnd = self.sample()return rnd@staticmethoddef sample_unit_ball():a = random.random()b = random.random()if b < a:a, b = b, asample = (b * math.cos(2 * math.pi * a / b),b * math.sin(2 * math.pi * a / b))return np.array([[sample[0]], [sample[1]], [0]])@staticmethoddef get_path_len(path):pathLen = 0for i in range(1, len(path)):node1_x = path[i][0]node1_y = path[i][1]node2_x = path[i - 1][0]node2_y = path[i - 1][1]pathLen += math.sqrt((node1_x - node2_x)** 2 + (node1_y - node2_y) ** 2)return pathLen@staticmethoddef line_cost(node1, node2):return math.sqrt((node1.x - node2.x) ** 2 + (node1.y - node2.y) ** 2)@staticmethoddef get_nearest_list_index(nodes, rnd):# 遍歷當前所有的節點，計算采樣點和每個節點的距離dList = [(node.x - rnd[0]) ** 2 + (node.y - rnd[1]) ** 2 for node in nodes]minIndex = dList.index(min(dList)) # 得到列表里距離值最小的下標，dList是列表格式，index最小值的下標return minIndex # 返回下標# 生成新的節點, n_ind表示離采樣點最近節點的下標，nearestNode表示離采樣點最近的節點(用x,y表示)def get_new_node(self, theta, n_ind, nearestNode):newNode = copy.deepcopy(nearestNode) # 拷貝最近的節點作為新的節點，然后再增加x，y軸方向的坐標# 進行生長，生長的距離(設置為2)乘以余弦正弦值，得到節點的坐標，expand_dis = 2newNode.x += self.expand_dis * math.cos(theta)newNode.y += self.expand_dis * math.sin(theta)newNode.cost += self.expand_dis # cost值等于長度，記錄長度newNode.parent = n_ind # 記錄這個新節點的父節點，就是 nearestNode 的下標return newNodedef is_near_goal(self, node):d = self.line_cost(node, self.goal) # 計算新加入點和終點的距離if d < self.expand_dis: # 距離小于步長的話，則表示和終點非常接近，可以直接和終點連起來return Truereturn Falsedef rewire(self, newNode, nearInds):n_node = len(self.node_list)for i in nearInds:nearNode = self.node_list[i]d = math.sqrt((nearNode.x - newNode.x) ** 2+ (nearNode.y - newNode.y) ** 2)s_cost = newNode.cost + dif nearNode.cost > s_cost:theta = math.atan2(newNode.y - nearNode.y,newNode.x - nearNode.x)if self.check_collision(nearNode, theta, d):nearNode.parent = n_node - 1nearNode.cost = s_cost@staticmethod# 計算點到直線的距離def distance_squared_point_to_segment(v, w, p):# Return minimum distance between line segment vw and point p# 如果端點v,w是同一個點,則距離就是p到v/w的距離，變成點到點的距離if np.array_equal(v, w):# a.dot(b) 與 np.dot(a, b) 效果相同return (p - v).dot(p - v) # v == w case，返回距離的平方(向量(p-v)的x^2+y^2)l2 = (w - v).dot(w - v) # i.e. |w-v|^2 - avoid a sqrt 計算w和v的線段長度的平方# Consider the line extending the segment,# parameterized as v + t (w - v).# We find projection of point p onto the line.# It falls where t = [(p-v) . (w-v)] / |w-v|^2# We clamp t from [0,1] to handle points outside the segment vw.t = max(0, min(1, (p - v).dot(w - v) / l2)) # (p - v) 與 (w - v)做點積，就是向量pv與向量wv做乘積，t是個比例projection = v + t * (w - v) # Projection falls on the segment 圖中的o點return (p - projection).dot(p - projection) # 返回p 和 o點的距離的平方def check_segment_collision(self, x1, y1, x2, y2): # 把生成的新節點和最近的節點輸入進來for (ox, oy, size) in self.obstacle_list: # 遍歷所有障礙物# 判斷直線和圓是否相切，只需要判斷圓心到直線的距離# 輸入 Xnew, Xnear, 障礙物圓心# array處理向量的模塊# dd 表示兩點距離的平方dd = self.distance_squared_point_to_segment(np.array([x1, y1]),np.array([x2, y2]),np.array([ox, oy]))if dd <= size ** 2: # 距離的平方大于半徑的平方則沒有碰撞return False # collisionreturn Truedef check_collision(self, nearNode, theta, d):tmpNode = copy.deepcopy(nearNode)end_x = tmpNode.x + math.cos(theta) * dend_y = tmpNode.y + math.sin(theta) * dreturn self.check_segment_collision(tmpNode.x, tmpNode.y, end_x, end_y)def get_final_course(self, lastIndex): # 找路徑，傳入終點值path = [[self.goal.x, self.goal.y]]while self.node_list[lastIndex].parent is not None: # 當parent = 0是就找到終點了node = self.node_list[lastIndex]path.append([node.x, node.y])lastIndex = node.parentpath.append([self.start.x, self.start.y])return pathdef draw_graph_informed_RRTStar(self, xCenter=None, cBest=None, cMin=None, e_theta=None, rnd=None, path=None):plt.clf()# for stopping simulation with the esc key.plt.gcf().canvas.mpl_connect('key_release_event',lambda event: [exit(0) if event.key == 'escape' else None])if rnd is not None:plt.plot(rnd[0], rnd[1], "^k")if cBest != float('inf'):self.plot_ellipse(xCenter, cBest, cMin, e_theta)for node in self.node_list:if node.parent is not None:if node.x or node.y is not None:plt.plot([node.x, self.node_list[node.parent].x], [node.y, self.node_list[node.parent].y], "-g")for (ox, oy, size) in self.obstacle_list:plt.plot(ox, oy, "ok", ms=30 * size)if path is not None:plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r')plt.plot(self.start.x, self.start.y, "xr")plt.plot(self.goal.x, self.goal.y, "xr")plt.axis([-2, 18, -2, 15])plt.grid(True)plt.pause(0.01)@staticmethoddef plot_ellipse(xCenter, cBest, cMin, e_theta): # pragma: no covera = math.sqrt(cBest ** 2 - cMin ** 2) / 2.0b = cBest / 2.0angle = math.pi / 2.0 - e_thetacx = xCenter[0]cy = xCenter[1]t = np.arange(0, 2 * math.pi + 0.1, 0.1)x = [a * math.cos(it) for it in t]y = [b * math.sin(it) for it in t]rot = Rot.from_euler('z', -angle).as_matrix()[0:2, 0:2]fx = rot @ np.array([x, y])px = np.array(fx[0, :] + cx).flatten()py = np.array(fx[1, :] + cy).flatten()plt.plot(cx, cy, "xc")plt.plot(px, py, "--c")def draw_graph(self, rnd=None, path=None):plt.clf()# for stopping simulation with the esc key.plt.gcf().canvas.mpl_connect('key_release_event',lambda event: [exit(0) if event.key == 'escape' else None])if rnd is not None:plt.plot(rnd.x, rnd.y, "^k")for node in self.node_list:if node.parent is not None:if node.x or node.y is not None:plt.plot([node.x, self.node_list[node.parent].x], [node.y, self.node_list[node.parent].y], "-g")for (ox, oy, size) in self.obstacle_list:# self.plot_circle(ox, oy, size)plt.plot(ox, oy, "ok", ms=30 * size)plt.plot(self.start.x, self.start.y, "xr")plt.plot(self.goal.x, self.goal.y, "xr")if path is not None:plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r')plt.axis([-2, 18, -2, 15])plt.grid(True)plt.pause(0.01)class Node:def __init__(self, x, y):self.x = xself.y = yself.cost = 0.0self.parent = Nonedef main():print("Start rrt planning")# create obstaclesobstacleList = [ # 設置障礙物 (x,y,r)最后參數是半徑(3, 3, 1.5),(12, 2, 3),(3, 9, 2),(9, 11, 2),]# obstacleList = [(5, 5, 1), (3, 6, 2), (3, 8, 2), (3, 10, 2), (7, 5, 2),# (9, 5, 2), (8, 10, 1)]# Set params 類RRT,采樣范圍[-2,18]，障礙物，最大迭代次數rrt = RRT(randArea = [-2, 18], obstacleList = obstacleList, maxIter = 200)path = rrt.rrt_planning(start = [0, 0], goal = [15, 12], animation = show_animation) # 傳入起點和終點# path = rrt.rrt_star_planning(start=[0, 0], goal=[15, 12], animation=show_animation)# path = rrt.informed_rrt_star_planning(start=[0, 0], goal=[15, 12], animation=show_animation)print("Done!!")if show_animation and path:plt.show()if __name__ == '__main__':main()

總結

以上是生活随笔為你收集整理的3 基于采样的路径规划 —— RRT算法的全部內容，希望文章能夠幫你解決所遇到的問題。

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