4. RRT快速探索随机树
任务目标
实现RRT（Rapidly-exploring Random Tree）路径规划算法，在包含障碍物的二维复杂环境中，从起点出发搜索并生成一条连接目标点的可行路径。算法需支持自定义障碍物集合，并输出最终路径点序列供后续轨迹平滑或控制模块使用。
核心实现
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.patches import Rectangle, Circle
class RRT:
def __init__(self, start, goal, obstacle_list, bounds,
step_size=0.5, max_iter=2000, goal_tolerance=0.3):
self.start = np.array(start, dtype=float)
self.goal = np.array(goal, dtype=float)
self.obstacles = obstacle_list # [(x_min,y_min,x_max,y_max), ...]
self.bounds = bounds # [x_min, x_max, y_min, y_max]
self.step_size = step_size
self.max_iter = max_iter
self.goal_tol = goal_tolerance
self.nodes = [self.start]
self.parent = [-1]
def _collision_free(self, p1, p2):
"""检查线段p1-p2是否与任何障碍物碰撞"""
for (x_min, y_min, x_max, y_max) in self.obstacles:
# 粗略检查：线段包围盒与障碍物包围盒是否相交
seg_min_x = min(p1[0], p2[0])
seg_max_x = max(p1[0], p2[0])
seg_min_y = min(p1[1], p2[1])
seg_max_y = max(p1[1], p2[1])
if seg_max_x < x_min or seg_min_x > x_max or \
seg_max_y < y_min or seg_min_y > y_max:
continue
# 精细检查：线段与矩形四条边是否相交
edges = [
((x_min, y_min), (x_max, y_min)),
((x_max, y_min), (x_max, y_max)),
((x_max, y_max), (x_min, y_max)),
((x_min, y_max), (x_min, y_min))
]
for e1, e2 in edges:
if self._segments_intersect(p1, p2, e1, e2):
return False
# 线段完全在矩形内部的情况
if x_min < p1[0] < x_max and y_min < p1[1] < y_max:
return False
return True
@staticmethod
def _segments_intersect(p1, p2, p3, p4):
"""判断两条线段是否相交"""
def cross(o, a, b):
return (a[0]-o[0])*(b[1]-o[1]) - (a[1]-o[1])*(b[0]-o[0])
d1 = cross(p3, p4, p1)
d2 = cross(p3, p4, p2)
d3 = cross(p1, p2, p3)
d4 = cross(p1, p2, p4)
if ((d1 > 0 and d2 < 0) or (d1 < 0 and d2 > 0)) and \
((d3 > 0 and d4 < 0) or (d3 < 0 and d4 > 0)):
return True
# 处理共线重叠情况
if d1 == 0 and self._on_segment(p3, p4, p1): return True
if d2 == 0 and self._on_segment(p3, p4, p2): return True
if d3 == 0 and self._on_segment(p1, p2, p3): return True
if d4 == 0 and self._on_segment(p1, p2, p4): return True
return False
@staticmethod
def _on_segment(p, q, r):
return (min(p[0],q[0]) <= r[0] <= max(p[0],q[0]) and
min(p[1],q[1]) <= r[1] <= max(p[1],q[1]))
def _nearest_node(self, sample):
"""返回距离采样点最近的节点索引"""
dists = np.linalg.norm(np.array(self.nodes) - sample, axis=1)
return int(np.argmin(dists))
def _steer(self, from_node, to_sample):
"""从from_node向to_sample方向扩展step_size距离"""
vec = to_sample - from_node
dist = np.linalg.norm(vec)
if dist < self.step_size:
return to_sample
return from_node + vec / dist * self.step_size
def plan(self):
"""执行RRT搜索，返回路径点列表；失败返回None"""
for _ in range(self.max_iter):
# 以10%概率直接采样目标点，引导树向目标生长
if np.random.random() < 0.1:
sample = self.goal
else:
sample = np.array([
np.random.uniform(self.bounds[0], self.bounds[1]),
np.random.uniform(self.bounds[2], self.bounds[3])
])
nearest_idx = self._nearest_node(sample)
new_node = self._steer(self.nodes[nearest_idx], sample)
# 边界约束
new_node[0] = np.clip(new_node[0], self.bounds[0], self.bounds[1])
new_node[1] = np.clip(new_node[1], self.bounds[2], self.bounds[3])
if self._collision_free(self.nodes[nearest_idx], new_node):
self.nodes.append(new_node)
self.parent.append(nearest_idx)
# 检查是否到达目标
if np.linalg.norm(new_node - self.goal) < self.goal_tol:
return self._extract_path(len(self.nodes)-1)
return None
def _extract_path(self, goal_idx):
"""回溯父节点生成路径"""
path = []
idx = goal_idx
while idx != -1:
path.append(self.nodes[idx])
idx = self.parent[idx]
return path[::-1]
def visualize(start, goal, obstacles, path=None):
"""可视化规划结果"""
fig, ax = plt.subplots(figsize=(8, 8))
for (x_min, y_min, x_max, y_max) in obstacles:
ax.add_patch(Rectangle((x_min, y_min), x_max-x_min, y_max-y_min,
color='gray', alpha=0.7))
ax.plot(start[0], start[1], 'go', markersize=12, label='Start')
ax.plot(goal[0], goal[1], 'r*', markersize=15, label='Goal')
if path:
path = np.array(path)
ax.plot(path[:, 0], path[:, 1], 'b-', linewidth=2, label='RRT Path')
ax.set_xlim(0, 10); ax.set_ylim(0, 10)
ax.set_aspect('equal'); ax.legend(); ax.grid(True)
plt.show()
if __name__ == '__main__':
# 定义复杂环境：起点、终点、多个矩形障碍物
start = (0.5, 0.5)
goal = (9.0, 9.0)
obstacles = [
(2, 0, 3, 7), # 左侧竖墙
(5, 2, 6, 8), # 中央竖墙
(7, 0, 8, 5), # 右侧下墙
(1, 8, 4, 9), # 左上横墙
(4, 4, 5, 5) # 中央小方块
]
bounds = [0, 10, 0, 10]
rrt = RRT(start, goal, obstacles, bounds, step_size=0.4, max_iter=3000)
path = rrt.plan()
if path:
print(f"路径规划成功，共 {len(path)} 个路径点")
print("路径点序列:")
for p in path:
print(f" ({p[0]:.2f}, {p[1]:.2f})")
visualize(start, goal, obstacles, path)
else:
print("规划失败：达到最大迭代次数仍未找到路径")
visualize(start, goal, obstacles)
运行与验证
环境依赖：NumPy、Matplotlib（仅用于可视化验证）。
运行方式：
python rrt_planner.py
验证方法：
- 程序输出路径点序列，首点应为起点(0.5, 0.5)，末点应落在以目标(9.0, 9.0)为中心、半径0.3的圆内；
- 可视化窗口中蓝色路径不应穿过任何灰色障碍物矩形；
- 路径点之间最大间距不超过step_size（0.4），且所有点均在[0,10]×[0,10]边界内。
判定标准：满足上述三点即视为规划成功。由于RRT采用随机采样，每次运行生成的路径可能不同，但均应满足无碰撞且连通起点与终点。若输出“规划失败”，可增大max_iter或适当增大step_size后重试。