4. 全局路径规划
任务目标
实现RRT（Rapidly-exploring Random Tree）算法，在包含障碍物的二维平面环境中规划一条从起点到终点的可行路径，并可视化随机树的生长过程与最终路径。
环境与依赖
本案例使用Python 3.8+，仅依赖numpy和matplotlib。安装命令：
pip install numpy matplotlib核心实现
以下代码完整实现了RRT算法：定义障碍物环境、随机采样扩展树、碰撞检测、路径回溯，并输出可视化结果。
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):
self.start = np.array(start, dtype=float)
self.goal = np.array(goal, dtype=float)
self.obstacles = obstacle_list # 每个障碍: (x, y, w, h) 或 (x, y, r)
self.bounds = bounds # [xmin, xmax, ymin, ymax]
self.step = step_size
self.max_iter = max_iter
self.nodes = [self.start]
self.parent = [-1]
self.path = []
def _collision_free(self, p1, p2):
"""检查线段p1-p2是否与任何障碍物碰撞"""
for obs in self.obstacles:
if len(obs) == 3: # 圆形障碍
cx, cy, r = obs
# 计算线段到圆心的最短距离
d = np.cross(p2 - p1, np.array([cx, cy]) - p1) / np.linalg.norm(p2 - p1)
if abs(d) < r:
# 检查垂足是否在线段上
t = np.dot(np.array([cx, cy]) - p1, p2 - p1) / np.linalg.norm(p2 - p1)**2
if 0 <= t <= 1:
return False
else: # 矩形障碍 (x, y, w, h)
x, y, w, h = obs
# 线段与矩形四条边分别检测
rect_pts = [(x, y), (x+w, y), (x+w, y+h), (x, y+h)]
for i in range(4):
a = np.array(rect_pts[i])
b = np.array(rect_pts[(i+1) % 4])
if self._segments_intersect(p1, p2, a, b):
return False
return True
def _segments_intersect(self, p1, p2, p3, p4):
"""判断两条线段是否相交"""
def ccw(A, B, C):
return np.cross(B - A, C - A) > 0
return (ccw(p1, p3, p4) != ccw(p2, p3, p4)) and (ccw(p1, p2, p3) != ccw(p1, p2, p4))
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距离"""
direction = to_sample - from_node
dist = np.linalg.norm(direction)
if dist < self.step:
return to_sample
return from_node + direction / dist * self.step
def plan(self):
"""执行RRT搜索，返回路径点列表"""
for _ in range(self.max_iter):
# 以10%概率直接采样目标点，引导树向终点生长
if np.random.rand() < 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)
nearest_node = self.nodes[nearest_idx]
new_node = self._steer(nearest_node, sample)
# 检查新节点是否在边界内且路径无碰撞
if (self.bounds[0] <= new_node[0] <= self.bounds[1] and
self.bounds[2] <= new_node[1] <= self.bounds[3] and
self._collision_free(nearest_node, new_node)):
self.nodes.append(new_node)
self.parent.append(nearest_idx)
# 到达目标附近则回溯路径
if np.linalg.norm(new_node - self.goal) < self.step:
self.nodes.append(self.goal)
self.parent.append(len(self.nodes) - 2)
self._extract_path()
return self.path
return None # 达到最大迭代次数仍未找到路径
def _extract_path(self):
"""从目标节点回溯到起点"""
idx = len(self.nodes) - 1
while idx != -1:
self.path.append(self.nodes[idx])
idx = self.parent[idx]
self.path.reverse()
def visualize(self):
"""绘制障碍物、随机树和最终路径"""
fig, ax = plt.subplots(figsize=(8, 8))
# 绘制障碍物
for obs in self.obstacles:
if len(obs) == 3:
ax.add_patch(Circle((obs[0], obs[1]), obs[2], color='gray', alpha=0.7))
else:
ax.add_patch(Rectangle((obs[0], obs[1]), obs[2], obs[3],
color='gray', alpha=0.7))
# 绘制树结构
for i, node in enumerate(self.nodes):
if self.parent[i] != -1:
px, py = self.nodes[self.parent[i]]
ax.plot([px, node[0]], [py, node[1]], 'lightblue', linewidth=0.5)
# 绘制最终路径
if self.path:
path_arr = np.array(self.path)
ax.plot(path_arr[:, 0], path_arr[:, 1], 'r-', linewidth=2.5, label='Path')
# 绘制起点和终点
ax.plot(*self.start, 'go', markersize=10, label='Start')
ax.plot(*self.goal, 'ro', markersize=10, label='Goal')
ax.set_xlim(self.bounds[0], self.bounds[1])
ax.set_ylim(self.bounds[2], self.bounds[3])
ax.set_aspect('equal')
ax.legend()
ax.set_title('RRT Path Planning')
plt.show()
# 使用示例
if __name__ == "__main__":
# 定义环境：起点、终点、障碍物列表、边界
start = (0.5, 0.5)
goal = (9.0, 9.0)
obstacles = [(3, 3, 1.5, 1.5), (6, 2, 1.0, 2.0), (2, 6, 2.0, 1.0), (7, 7, 1.0, 1.0)]
bounds = [0, 10, 0, 10]
rrt = RRT(start, goal, obstacles, bounds, step_size=0.6, 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})")
else:
print("未找到可行路径")
rrt.visualize()
运行与验证
运行方式：将上述代码保存为rrt_planning.py，在终端执行python rrt_planning.py。
验证方法：
- 程序输出“路径找到”并打印路径点序列，说明RRT成功规划出可行路径；
- 弹出的可视化窗口应显示：灰色障碍物、浅蓝色随机树、红色最终路径、绿色起点和红色终点；
- 红色路径应连续且不穿过任何灰色障碍物，从起点连通至终点。
判定标准：若输出“未找到可行路径”，可增大max_iter或step_size后重新运行；若路径穿过障碍物，说明碰撞检测有误，需检查障碍物参数定义。
关键说明
算法采用概率完备的RRT策略：每次迭代随机采样，扩展树向采样点生长，并以10%概率直接采样目标点以加速收敛。碰撞检测支持矩形和圆形两种障碍物类型，通过线段相交和点到线段距离判断。本实现为教学演示版本，未包含路径平滑处理，实际应用中可在RRT基础上增加剪枝或B样条平滑优化路径质量。