改进RRT*的路径规划算法
一、RRT算法
RRT 算法是一种基于随机采样的快速搜索算法。该算法的主要思想是通过随机采样来创建一个快速探索的树，从而生长出一条从起点到终点的路径。如图为随机树的生长过程。
- 初始化。首先，初始化起始点和目标点位置，并将起点作为根节点，建立一棵只包含起点的树 T。
- 随机采样。从地图中随机生成一个点 x_rand 作为采样点，该点在自由空间中且不在障碍物中。
- 扩展树。在树 T 中找到最近邻节点 x_near。然后，计算从 x_near 到 x_rand 的一条路径是否在和障碍物发生碰撞。如果路径没有和障碍物发生碰撞，则将 x_new 作为路径上一个新的节点，并将该节点作为新的节点加入树 T。如果路径和障碍物发生碰撞，则此次生长作废，跳转到步骤 2。
- 反复迭代。重复执行步骤 2 和步骤 3，直到树 T 中包含目标点，或者超过了指定的次数，算法停止扩展。
- 返回路径。从终点回溯到起点，得到一条可行路径。
算法伪代码如下图所示
RRT 算法的优点是可以处理高维度的状态空间，计算速度快，适用于复杂的路径规划问题。但是，由于采用随机采样的方式生成树，该算法可能会产生不必要的路径， 需要使用启发式函数或其他优化方法进行改进。使用 RRT 算法进行路径规划的仿真结果如图所示，从图中可以看到 RRT 算法规划出的路径拐点较多，且可行性较低。
二、RRT*算法
RRT*算法在RRT的基础上引入了优化策略，在随机树扩张过程中引入了重选父节点和范围内重连策略，在生长的过程中不断对路径进行优化，保证生成路径为渐进最优的。
- 初始化。初始化起始点和目标点位置，并将起点作为根节点，建立一棵只包含起点的树 T。设置邻域半径
r（用于找到节点周围的候选节点进行优化）。- 随机采样。从地图中随机生成一个点 x_rand 作为采样点，该点在自由空间中且不在障碍物中。
- 扩展树。在树 T 中找到最近邻节点
x_near。然后，计算从x_near到x_rand的路径是否在和障碍物发生碰撞。如果没有碰撞，则生成新节点x_new，并将其连接到x_near。- 重选父节点。找到
x_new邻域内的所有节点（距离小于半径r的节点），并从这些节点中找到一条最短的、无碰撞的路径来连接x_new。如果该路径的成本更低，则更新x_new的父节点为该邻域内的最优节点，并将新边添加到树中。- 节点重连。在
x_new被加入树之后，检查其邻域内的其他节点，判断是否通过x_new连接这些节点可以缩短它们的路径长度。如果可以，则更新这些节点的父节点为x_new，并调整相应的路径。- 反复迭代。重复执行步骤 2 和步骤 3，直到树 T 中包含目标点，或者超过了指定的次数，算法停止扩展。
- 返回路径。从终点回溯到起点，得到一条可行路径。
如图1为重选父节点示意图，其中橙色实线为更新的路径，蓝色虚线为舍去路径，在单位圆内存在某一节点，使得X_new选择这个节点为父节点的距离更近且无碰撞，即可更新。
图2为节点重连，在单位圆内，X_1选择X_new为父节点的距离比X_1选择X_2为父节点的距离更短且无碰撞，即可更新
图1 重选父节点 图2 节点重连
通过重选父节点与节点重连，能让算法路径生成的路径拐点数量明显减少，路径长度更短。
三、改进的RRT*算法
1.地图评估复杂度策略
Bias-RRT算法通过目标偏置策略引入了目标点作为采样点的选取，设定的偏置概率P在简单地图中能够有效提升路径规划的收敛速度。
传统RRT算法使用固定步长进行随机树的生长，但同一步长难以适应不同类型的地图。使用固定步长在不同地图上是不够灵活的。
通过计算出地图复杂程度，来动态调整最优的偏置概率p和搜索步长s。
A_obstacle表示障碍物面积；A_map表示地图面积； D_obstacle表示障碍物分布情况。将地图的长宽各缩小为原来的十分之一后，将其划分为100个相等的网格，障碍物占据的网格数量与这100个网格的比例即为障碍物的分布情况。
当地图复杂程度系数C接近1时，意味着地图的复杂程度较高，因此偏置概率P和步长S的值应相应减小。偏置概率P和步长S计算公式如下
其中α与β为所设置的常量，分别为0.3的7。α能保证偏置概率被控制在0-0.3之间。在地图较为简单时，即地图复杂度系数C接近0，偏置概率接近 0.3，从而确保算法仍具有较强的目标指向性；在地图复杂度较高时，即 C 接近1，偏置概率趋近于0，目标指向性减弱，适应复杂场景。步长S应与起点到终点的欧几里得距离相关，距离越远，初始步长应相应增加。同时，β的选取值需要在路径探索的准确性与效率之间取得平衡。经过实验验证，设定β=7可确保初始步长在大部分实际场景中足够灵活，既能够快速收敛于简单场景，又能在复杂场景中保持足够的探索能力。
2.直连终点
为了加速路径生成过程，每当生成新的节点 Qnew 时，算法即刻朝向终点方向动态地生长，并对生成的路径进行实时碰撞检测。如果在此过程中检测到碰撞，算法将直接放弃当前路径并进入下一次迭代；若未检测到碰撞，则可以直接生成一条初始可行路径。
3.可变步长扩展策略
步长过大时，会增加Q_new与 Q_nearest之间的路径经过障碍物的概率，降低采样的成功率。
引入了可变步长的随机树扩展策略。在算法初始化时，设置最小步长Smin和基本步长S两个参数
n为距离因子，如图，可以扩展到Q_2的位置
4.路径剪枝
双向剪枝，减去多余的路径。从起点Qstart开始，依次检查Qstart与初始路径中各个节点Qi之间的连线是否存在障碍物。如果路径上不存在障碍物，则将Qi的父节点直接更新为Qstart，从而简化路径；如果存在障碍物，则继续以Qi的父节点作为新的起点进行检查，并重复这一过程，直到到达终点Qgoal。
算法代码
import copy
import math
import random
import timeimport matplotlib.pyplot as plt
from matplotlib.patches import Rectangle
from matplotlib.transforms import Affine2D
from scipy.spatial.transform import Rotation as Rot
import numpy as npshow_animation = TrueC1 = 0
class RRT:def __init__(self, obstacleList, randArea,expandDis=None, goalSampleRate=None, maxIter=200, n=4):self.start = Noneself.goal = Noneself.min_rand = randArea[0]self.max_rand = randArea[1]self.step = n# 计算地图面积rand_area_size = (self.max_rand - self.min_rand) ** 2# 计算障碍物的总面积obstacle_area_size = self.calculate_obstacle_area(obstacleList)# 将地图划分为400个均等格子grid_count = 20 # 20x20的网格，总共400个格子grid_size_x = (self.max_rand - self.min_rand) / grid_countgrid_size_y = (self.max_rand - self.min_rand) / grid_count# 初始化20x20的网格，初始值为0grid = [[0 for _ in range(grid_count)] for _ in range(grid_count)]# 使用障碍物更新网格grid = self.update_grid_with_obstacles(grid, obstacleList, self.min_rand, self.max_rand, grid_count)# 计算障碍物所占格子数与总格子数400的比例total_cells = grid_count * grid_count # 总共400个格子grid_cells = sum([sum(row) for row in grid])# print(grid_cells)filled_grid_ratio = grid_cells / total_cells # 障碍物所占的比例# 计算复杂程度系数C1global C1C1 = 0.5 * (obstacle_area_size / rand_area_size) + 0.5 * filled_grid_ratio# print(f"障碍物空间分布: {filled_grid_ratio:f},C1: {C1:.2f}")# 自动生成goal_sample_rateself.goal_sample_rate = goalSampleRate if goalSampleRate is not None else ((1 - C1)*0.3)self.expand_dis = expandDisself.max_iter = maxIterself.obstacle_list = obstacleListself.node_list = Nonedef rrt_star_planning(self, start, goal, animation=True):# 调整步长distance = math.sqrt((goal[0] - start[0]) ** 2 + (goal[1] - start[1]) ** 2)global C1if self.expand_dis is None:self.expand_dis = distance / 7 * (1 - C1) # 起点到终点的欧几里得距离/7 *(1-C1)# print("Expand Dis:", self.expand_dis)# print("Goal Sample Rate:", self.goal_sample_rate)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]# steer 必须生长self.expand_dis 因此可能超过min_rand max_rand 可以酌情修改theta = 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 not noCollision:adjusted = Falsefor i in range(self.step - 1, 0, -1): # 依次尝试减小factor = self.step / i # 动态计算因子reduced_step = self.expand_dis / factorx_i = nearestNode.x + reduced_step * math.cos(theta)y_i = nearestNode.y + reduced_step * math.sin(theta)if self.check_segment_collision(x_i, y_i, nearestNode.x, nearestNode.y):newNode.x = x_inewNode.y = y_inewNode.cost += reduced_step - self.expand_dis # 需要修正花费路径距离adjusted = Truebreakif not adjusted:# 如果没有找到合适的扩展节点，则丢弃该节点并重新采样continuenearInds = self.find_near_nodes(newNode)newNode = self.choose_parent(newNode, nearInds)self.node_list.append(newNode)self.rewire(newNode, nearInds)# 从新生成的节点Xnew往终点生长while True:theta_to_goal = math.atan2(self.goal.y - newNode.y, self.goal.x - newNode.x)goalNode = self.get_new_node(theta_to_goal, len(self.node_list) - 1, newNode)noCollision = self.check_segment_collision(goalNode.x, goalNode.y, newNode.x, newNode.y)if not noCollision:break # 如果发生碰撞，退出循环self.node_list.append(goalNode)newNode = goalNode # 更新newNode为goalNode，继续生长# 判断是否到达终点if self.is_near_goal(newNode):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))path, pruned_path_length = self.prune_path(path)if animation:self.draw_graph(path)breakif animation:self.draw_graph(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))# print("剪枝前 path: {}",path)if path:path, pruned_path_length = self.prune_path(path)if animation:self.draw_graph(path)# print(path)breakif path:path, pruned_path_length = self.prune_path(path)if animation:self.draw_graph(path)# print(path)return pathdef calculate_obstacle_area(self,obstacleList):total_area = 0for obstacle in obstacleList:if len(obstacle) == 3: # 圆形障碍物 (x, y, r)r = obstacle[2]total_area += math.pi * (r ** 2)elif len(obstacle) == 4: # 矩形障碍物 (x, y, width, height)width = obstacle[2]height = obstacle[3]total_area += width * heightreturn total_areadef update_grid_with_obstacles(self,grid, obstacleList, min_rand, max_rand, grid_count):grid_size_x = (max_rand - min_rand) / grid_countgrid_size_y = (max_rand - min_rand) / grid_countfor obstacle in obstacleList:if len(obstacle) == 3: # 圆形障碍物 (x, y, r)x, y, r = obstaclemin_x = max(int((x - r - min_rand) / grid_size_x), 0)max_x = min(int((x + r - min_rand) / grid_size_x), grid_count - 1)min_y = max(int((y - r - min_rand) / grid_size_y), 0)max_y = min(int((y + r - min_rand) / grid_size_y), grid_count - 1)for i in range(min_x, max_x + 1):for j in range(min_y, max_y + 1):grid[i][j] = 1elif len(obstacle) == 4: # 矩形障碍物 (x, y, width, height)x, y, width, height = obstaclemin_x = max(int((x - width / 2 - min_rand) / grid_size_x), 0)max_x = min(int((x + width / 2 - min_rand) / grid_size_x), grid_count - 1)min_y = max(int((y - height / 2 - min_rand) / grid_size_y), 0)max_y = min(int((y + height / 2 - min_rand) / grid_size_y), grid_count - 1)for i in range(min_x, max_x + 1):for j in range(min_y, max_y + 1):grid[i][j] = 1return griddef prune_path(self, original_path):pruned_path = [original_path[0]] # 将起始节点添加到剪枝路径中current_node = original_path[0]# 从第二个节点开始遍历路径for i in range(1, len(original_path)):next_node = original_path[i]# 检查从 current_node 直接连接到 next_node 的路径是否无碰撞if self.is_collision_free_path([current_node, next_node]):# 如果无碰撞，则跳过中间节点continue# 如果存在碰撞或路径段长度较长，则将前一个节点添加到剪枝路径中，并更新当前节点pruned_path.append(original_path[i - 1])current_node = original_path[i - 1]# 最后添加终点节点到剪枝路径pruned_path.append(original_path[-1])# 计算剪枝后的路径长度pruned_path_length = self.calculate_path_length(pruned_path)original_path_length = self.calculate_path_length(original_path)# 如果剪枝后的路径没有缩短，则返回原始路径# if pruned_path_length > original_path_length:# print("Pruned path is not shorter, returning original path") # 调试信息# return original_path, original_path_lengthreturn pruned_path, pruned_path_lengthdef calculate_distance(self, point1, point2):return math.sqrt((point1[0] - point2[0]) ** 2 + (point1[1] - point2[1]) ** 2)def sample(self):if random.randint(0, 100) > self.goal_sample_rate*100:rnd = [random.uniform(self.min_rand, self.max_rand), random.uniform(self.min_rand, self.max_rand)]else: # goal point samplingrnd = [self.goal.x, self.goal.y]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))return minIndexdef get_new_node(self, theta, n_ind, nearestNode):newNode = copy.deepcopy(nearestNode)newNode.x += self.expand_dis * math.cos(theta)newNode.y += self.expand_dis * math.sin(theta)newNode.cost += self.expand_disnewNode.parent = n_indreturn 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:if self.check_segment_collision(nearNode.x, nearNode.y, newNode.x, newNode.y):nearNode.parent = n_node - 1nearNode.cost = s_costdef update_node_parent(self, node, target_node, new_node_index, distance, new_cost):"""检查并更新节点的父节点与路径成本。:param node: 需要更新的节点（nearNode或nearparentNode）:param target_node: 新节点（newNode）:param new_node_index: 新节点在节点列表中的索引:param distance: 两节点之间的距离:param new_cost: 新的路径成本:return: 是否成功更新了节点"""theta = math.atan2(target_node.y - node.y, target_node.x - node.x)if self.check_collision(node, theta, distance):node.parent = new_node_indexnode.cost = new_costreturn Truereturn Falsedef 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: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 check_line_intersection(self, x1, y1, x2, y2, x3, y3, x4, y4):d1 = (x2 - x1, y2 - y1)d2 = (x4 - x3, y4 - y3)cross = d1[0] * d2[1] - d1[1] * d2[0]if cross == 0:return Falset1 = ((x3 - x1) * d2[1] - (y3 - y1) * d2[0]) / crosst2 = ((x3 - x1) * d1[1] - (y3 - y1) * d1[0]) / crossif 0 <= t1 <= 1 and 0 <= t2 <= 1:return Truereturn Falsedef plot_pruned_path(self, path, obstacle_list, start, goal):try:# Example plotting codeplt.figure()for (ox, oy, size) in obstacle_list:circle = plt.Circle((ox, oy), size, color='b', alpha=0.5)plt.gca().add_patch(circle)path_x, path_y = zip(*path)plt.plot(path_x, path_y, marker='o', color='b')plt.plot(start[0], start[1], 'go') # Start pointplt.plot(goal[0], goal[1], 'ro') # Goal pointplt.xlabel('X')plt.ylabel('Y')plt.title('Pruned Path')plt.grid(True)plt.show()except Exception as e:print(f"Error plotting path: {e}")def calculate_path_length(self, path):"""Calculate the total length of the given path."""length = 0.0for i in range(len(path) - 1):x1, y1 = path[i]x2, y2 = path[i + 1]length += np.sqrt((x2 - x1) ** 2 + (y2 - y1) ** 2)return lengthdef is_collision_free_path(self, path):"""检查给定路径中的所有路径段是否都无碰撞。参数:path (list of lists): 需要检查的路径，每个路径点是一个坐标列表，例如[[x1, y1], [x2, y2], ...]返回:bool: 如果路径中的所有路径段都无碰撞，则返回 True；否则返回 False。"""if len(path) < 2:return True # 如果路径点少于两个，无法检测碰撞# 遍历路径中的每个路径段for i in range(len(path) - 1):segment_start = path[i]segment_end = path[i + 1]x1, y1 = segment_startx2, y2 = segment_endif not self.check_segment_collision(x1, y1, x2, y2):return False # 如果发现碰撞，返回 Falsereturn True # 如果所有路径段都无碰撞，返回 Truedef check_segment_collision(self, x1, y1, x2, y2):for obstacle in self.obstacle_list:if len(obstacle) == 3: # 圆形障碍物ox, oy, size = obstaclesegment_start = np.array([x1, y1])segment_end = np.array([x2, y2])obstacle_center = np.array([ox, oy])distance_squared = self.distance_squared_point_to_segment(segment_start, segment_end, obstacle_center)if distance_squared <= size ** 2:return Falseelif len(obstacle) == 4: # 矩形障碍物rx, ry, rw, rh = obstacleif self.check_segment_rectangle_collision(x1, y1, x2, y2, rx, ry, rw, rh):return Falsereturn Truedef distance_squared_point_to_segment(self, v, w, p):if np.array_equal(v, w):return np.sum((p - v) ** 2)segment_length_squared = np.sum((w - v) ** 2)t = max(0, min(1, np.dot(p - v, w - v) / segment_length_squared))projection = v + t * (w - v)return np.sum((p - projection) ** 2)def check_segment_rectangle_collision(self, x1, y1, x2, y2, rx, ry, rw, rh):# 获取矩形的四个顶点left = rx - rw / 2right = rx + rw / 2bottom = ry - rh / 2top = ry + rh / 2# 定义矩形的四条边为线段rectangle_edges = [(left, bottom, left, top), # 左边(left, top, right, top), # 上边(right, top, right, bottom), # 右边(right, bottom, left, bottom) # 下边]# 检查传入的线段是否与矩形的任何一条边相交for edge in rectangle_edges:if self.do_intersect(x1, y1, x2, y2, *edge):return True# 检查线段的一个端点是否在矩形内部if left < x1 < right and bottom < y1 < top:return Trueif left < x2 < right and bottom < y2 < top:return Truereturn Falsedef do_intersect(self, x1, y1, x2, y2, x3, y3, x4, y4):# 使用向量叉积来检查两个线段是否相交def ccw(A, B, C):return (C[1] - A[1]) * (B[0] - A[0]) > (B[1] - A[1]) * (C[0] - A[0])A = (x1, y1)B = (x2, y2)C = (x3, y3)D = (x4, y4)return ccw(A, C, D) != ccw(B, C, D) and ccw(A, B, C) != ccw(A, B, D)def draw_graph(self, path=None):plt.clf()plt.gcf().canvas.mpl_connect('key_release_event',lambda event: [exit(0) if event.key == 'escape' else None])# 绘制节点和边（RRT生成的路径，绿色）for node in self.node_list:if node.parent is not None:plt.plot([node.x, self.node_list[node.parent].x], [node.y, self.node_list[node.parent].y], "-g")# 绘制障碍物（黑色）for obstacle in self.obstacle_list:if len(obstacle) == 3: # 圆形障碍物ox, oy, size = obstaclecircle = plt.Circle((ox, oy), size, color="black", fill=True)plt.gca().add_patch(circle)elif len(obstacle) == 4: # 矩形障碍物rx, ry, rw, rh = obstaclerectangle = plt.Rectangle((rx - rw / 2, ry - rh / 2), rw, rh,color="black", fill=True # 去掉alpha参数，确保为纯黑色)plt.gca().add_patch(rectangle)# 保持x和y轴的比例一致plt.axis('equal')# 绘制起点（绿色圆形）plt.plot(self.start.x, self.start.y, "ob", markersize=8)# 绘制终点（红色圆形）plt.plot(self.goal.x, self.goal.y, "or", markersize=8)# 绘制最终路径（红色）if path is not None and len(path) > 1: # 确保路径至少有两个节点plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r')# 为了增加可视范围，给坐标轴增加一定的余量x_margin = abs(self.start.x - self.goal.x) * 0.1y_margin = abs(self.start.y - self.goal.y) * 0.1x_min = min(self.start.x, self.goal.x) - x_marginx_max = max(self.start.x, self.goal.x) + x_marginy_min = min(self.start.y, self.goal.y) - y_marginy_max = max(self.start.y, self.goal.y) + y_marginplt.axis([x_min, x_max, y_min, y_max])plt.grid(False)plt.pause(0.01)# 使图形保持在屏幕上plt.show(block=False)class Node:def __init__(self, x, y):self.x = xself.y = yself.cost = 0.0self.parent = Nonedef main():# print("Start rrt planning")# create obstaclesobstacleList = [(10, 9, 3), # 圆形障碍物，远离起点(15, 15, 4), # 圆形障碍物(30, 18, 4), # 圆形障碍物(40, 10, 3), # 圆形障碍物(25, 34, 4), # 圆形障碍物(19, 21, 3), # 圆形障碍物(12, 38, 2), # 圆形障碍物(5, 38, 3), # 圆形障碍物(47, 37, 3), # 圆形障碍物(30, 10, 10, 6), # 矩形障碍物(46, 18, 4, 6), # 矩形障碍物(15, 30, 8, 8), # 矩形障碍物(45, 25, 5, 5), # 矩形障碍物，远离终点(20, 8, 6, 4), # 矩形障碍物(37, 15, 6, 3), # 矩形障碍物(32, 42, 5, 7), # 矩形障碍物(8, 25, 4, 4), # 矩形障碍物(39, 32, 7, 6), # 矩形障碍物(36, 26, 6, 4), # 矩形障碍物(25, 26, 3), # 圆障碍物(25, 42, 3), # 圆形障碍物，避免覆盖终点]# obstacleList = [(5, 5, 1), (3, 6, 2), (3, 8, 2), (3, 10, 2), (7, 5, 2),# (9, 5, 2), (8, 10, 1)]# Set paramsrandArea = [0, 50]rrt = RRT(obstacleList, randArea)path = rrt.rrt_star_planning(start=[5, 5], goal=[45, 45], animation=show_animation)print("Done!!")if show_animation and path:plt.show()if __name__ == '__main__':main()