一、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 time
import 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 np
show_animation = True
C1 = 0
class RRT:
def __init__(self, obstacleList, randArea,
expandDis=None, goalSampleRate=None, maxIter=200, n=4):
self.start = None
self.goal = None
self.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_count
grid_size_y = (self.max_rand - self.min_rand) / grid_count
# 初始化20x20的网格，初始值为0
grid = [[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 # 障碍物所占的比例
# 计算复杂程度系数C1
global C1
C1 = 0.5 * (obstacle_area_size / rand_area_size) + 0.5 * filled_grid_ratio
# print(f"障碍物空间分布: {filled_grid_ratio:f},C1: {C1:.2f}")
# 自动生成goal_sample_rate
self.goal_sample_rate = goalSampleRate if goalSampleRate is not None else ((1 - C1)*0.3)
self.expand_dis = expandDis
self.max_iter = maxIter
self.obstacle_list = obstacleList
self.node_list = None
def rrt_star_planning(self, start, goal, animation=True):
# 调整步长
distance = math.sqrt((goal[0] - start[0]) ** 2 + (goal[1] - start[1]) ** 2)
global C1
if 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 = None
lastPathLength = 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 = False
for i in range(self.step - 1, 0, -1): # 依次尝试减小
factor = self.step / i # 动态计算因子
reduced_step = self.expand_dis / factor
x_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_i
newNode.y = y_i
newNode.cost += reduced_step - self.expand_dis # 需要修正花费路径距离
adjusted = True
break
if not adjusted:
# 如果没有找到合适的扩展节点，则丢弃该节点并重新采样
continue
nearInds = 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) - 1
tempPath = self.get_final_course(lastIndex)
tempPathLen = self.get_path_len(tempPath)
if lastPathLength > tempPathLen:
path = tempPath
lastPathLength = tempPathLen
print("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)
break
if 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) - 1
tempPath = self.get_final_course(lastIndex)
tempPathLen = self.get_path_len(tempPath)
if lastPathLength > tempPathLen:
path = tempPath
lastPathLength = tempPathLen
print("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)
break
if path:
path, pruned_path_length = self.prune_path(path)
if animation:
self.draw_graph(path)
# print(path)
return path
def calculate_obstacle_area(self,obstacleList):
total_area = 0
for 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 * height
return total_area
def update_grid_with_obstacles(self,grid, obstacleList, min_rand, max_rand, grid_count):
grid_size_x = (max_rand - min_rand) / grid_count
grid_size_y = (max_rand - min_rand) / grid_count
for obstacle in obstacleList:
if len(obstacle) == 3: # 圆形障碍物 (x, y, r)
x, y, r = obstacle
min_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] = 1
elif len(obstacle) == 4: # 矩形障碍物 (x, y, width, height)
x, y, width, height = obstacle
min_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] = 1
return grid
def 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_length
return pruned_path, pruned_path_length
def 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 sampling
rnd = [self.goal.x, self.goal.y]
return rnd
def choose_parent(self, newNode, nearInds):
if len(nearInds) == 0:
return newNode
dList = []
for i in nearInds:
dx = newNode.x - self.node_list[i].x
dy = newNode.y - self.node_list[i].y
d = 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 newNode
newNode.cost = minCost
newNode.parent = minInd
return newNode
def 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) ** 2
for node in self.node_list]
near_inds = [d_list.index(i) for i in d_list if i <= r ** 2]
return near_inds
def 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) + xCenter
rnd = [rnd[(0, 0)], rnd[(1, 0)]]
else:
rnd = self.sample()
return rnd
@staticmethod
def sample_unit_ball():
a = random.random()
b = random.random()
if b < a:
a, b = b, a
sample = (b * math.cos(2 * math.pi * a / b),
b * math.sin(2 * math.pi * a / b))
return np.array([[sample[0]], [sample[1]], [0]])
@staticmethod
def get_path_len(path):
pathLen = 0
for 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
@staticmethod
def line_cost(node1, node2):
return math.sqrt((node1.x - node2.x) ** 2 + (node1.y - node2.y) ** 2)
@staticmethod
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))
return minIndex
def 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_dis
newNode.parent = n_ind
return newNode
def is_near_goal(self, node):
d = self.line_cost(node, self.goal)
if d < self.expand_dis:
return True
return False
def 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 + d
if nearNode.cost > s_cost:
if self.check_segment_collision(nearNode.x, nearNode.y, newNode.x, newNode.y):
nearNode.parent = n_node - 1
nearNode.cost = s_cost
def 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_index
node.cost = new_cost
return True
return False
def check_collision(self, nearNode, theta, d):
tmpNode = copy.deepcopy(nearNode)
end_x = tmpNode.x + math.cos(theta) * d
end_y = tmpNode.y + math.sin(theta) * d
return 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.parent
path.append([self.start.x, self.start.y])
return path
def 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)
@staticmethod
def plot_ellipse(xCenter, cBest, cMin, e_theta): # pragma: no cover
a = math.sqrt(cBest ** 2 - cMin ** 2) / 2.0
b = cBest / 2.0
angle = math.pi / 2.0 - e_theta
cx = 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 False
t1 = ((x3 - x1) * d2[1] - (y3 - y1) * d2[0]) / cross
t2 = ((x3 - x1) * d1[1] - (y3 - y1) * d1[0]) / cross
if 0 <= t1 <= 1 and 0 <= t2 <= 1:
return True
return False
def plot_pruned_path(self, path, obstacle_list, start, goal):
try:
# Example plotting code
plt.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 point
plt.plot(goal[0], goal[1], 'ro') # Goal point
plt.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.0
for 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 length
def 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_start
x2, y2 = segment_end
if not self.check_segment_collision(x1, y1, x2, y2):
return False # 如果发现碰撞，返回 False
return True # 如果所有路径段都无碰撞，返回 True
def check_segment_collision(self, x1, y1, x2, y2):
for obstacle in self.obstacle_list:
if len(obstacle) == 3: # 圆形障碍物
ox, oy, size = obstacle
segment_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 False
elif len(obstacle) == 4: # 矩形障碍物
rx, ry, rw, rh = obstacle
if self.check_segment_rectangle_collision(x1, y1, x2, y2, rx, ry, rw, rh):
return False
return True
def 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 / 2
right = rx + rw / 2
bottom = ry - rh / 2
top = 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 True
if left < x2 < right and bottom < y2 < top:
return True
return False
def 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 = obstacle
circle = plt.Circle((ox, oy), size, color="black", fill=True)
plt.gca().add_patch(circle)
elif len(obstacle) == 4: # 矩形障碍物
rx, ry, rw, rh = obstacle
rectangle = 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.1
y_margin = abs(self.start.y - self.goal.y) * 0.1
x_min = min(self.start.x, self.goal.x) - x_margin
x_max = max(self.start.x, self.goal.x) + x_margin
y_min = min(self.start.y, self.goal.y) - y_margin
y_max = max(self.start.y, self.goal.y) + y_margin
plt.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 = x
self.y = y
self.cost = 0.0
self.parent = None
def main():
# print("Start rrt planning")
# create obstacles
obstacleList = [
(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 params
randArea = [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()
