基于采样的路径规划算法
指在配置空间中进行随机采样，再在采样得到的点间进行路径规划，好处是避免了高维复杂空间中基于搜索式算法的路径规划方法需要对整个空间进行建模，效率较高。
但是基于采样的路径规划算法得到的路径不能保证是最优路径，需要进行路径优化。
Probabilistic Road Map概率路线图
概率路线图（Probabilistic Road Map，PRM） 是一种基于采样的路径规划算法，特别适用于高维空间（如机械臂运动规划）或复杂环境中的路径搜索。它通过随机采样和构建图结构来高效解决传统方法难以处理的问题。
核心思想
PRM的核心分为两个阶段：
- 构建阶段（Learning Phase）：
- 在配置空间（C-Space）中随机采样节点，并通过局部规划器连接邻近节点，形成图结构（Roadmap）。
- 查询阶段（Query Phase）：
- 在图结构上使用图搜索算法（如A*、Dijkstra）找到起点到终点的路径。
核心优势：避免显式建模整个配置空间，特别适合处理高维问题和狭窄通道场景。
算法步骤
1. 构建阶段（构建路线图）
-
随机采样：
- 在自由空间（无障碍区域）随机生成大量节点（称为“里程碑”，Milestones）。
- 采样策略可以是均匀随机采样，或针对狭窄通道优化（如桥测试、高斯采样）。
-
邻近节点连接：
- 对每个节点 q q q，找到其 k k k个最近邻节点（或固定半径内的所有节点）。
- 尝试用局部规划器（如直线连接）检查是否与障碍物碰撞,若无碰撞，则在图中添加边 ( q , q neighbor ) (q, q_{\text{neighbor}}) (q,qneighbor)。
- 临近点连接还可以考虑很多其他规则例如机器人的运动学约束等。最简单的就是只考虑距离和可行性
-
构建图结构：
- 最终得到一个无向图 G = ( V , E ) G = (V, E) G=(V,E)，节点表示可行位置，边表示可行路径。
2. 查询阶段（路径搜索）
- 插入起点和终点：
- 将起点 q start q_{\text{start}} qstart和终点 q goal q_{\text{goal}} qgoal连接到图中（同样检查碰撞）。
- 搜索路径：
- 使用图搜索算法（如A*、Dijkstra）在图中找到连接起点和终点的最短路径。
实现与优化
1. 狭窄通道处理
- 问题：均匀采样在狭窄通道区域可能采样不足，导致路线图无法连通。
- 解决方案：
- 桥测试（Bridge Test）：在障碍物附近生成一对对称采样点，检测中间点是否在自由空间。
- 高斯采样：倾向于在障碍物边界附近生成采样点。
2. 采样策略优化
- 均匀采样：简单但效率低。
- 启发式采样：根据环境特征调整采样密度（如狭窄区域增加采样）。
3. 概率完备性
- PRM是概率完备的：当采样数趋近于无穷大时，若能存在路径，则算法一定能找到。
代码：
import numpy as np
import networkx as nx
import matplotlib.pyplot as plt
from scipy.spatial import KDTree
from matplotlib.patches import Rectangle
def is_point_in_obstacle(point, box_list):
"""检测点是否在障碍物矩形内 (轴对齐矩形检测)"""
x, y = point
for (x1, y1, x2, y2) in box_list:
if (min(x1, x2) <= x <= max(x1, x2)) and (min(y1, y2) <= y <= max(y1, y2)):
return True
return False
def is_collision_line(p1, p2, box_list):
"""改进碰撞检测：分10段采样检测线段与障碍物相交"""
for t in np.linspace(0, 1, 10):
x = p1[0] + t*(p2[0]-p1[0])
y = p1[1] + t*(p2[1]-p1[1])
if is_point_in_obstacle((x,y), box_list):
return True
return False
def prm_algorithm(height, width, box_list, start, end, n_samples=200, k_neighbors=8, radius=50):
"""支持起终点的PRM主函数"""
# 验证起终点合法性
if is_point_in_obstacle(start, box_list) or is_point_in_obstacle(end, box_list):
raise ValueError("Start/End points are in obstacle!")
# 1. 包含起终点的采样点集合
samples = [start, end] # 保证起终点存在
while len(samples) < n_samples + 2:
x = np.random.uniform(0, width)
y = np.random.uniform(0, height)
if not is_point_in_obstacle((x,y), box_list):
samples.append((x, y))
# 2. 构建KDTree加速邻域搜索
kd_tree = KDTree(samples)
# 3. 创建网络图
G = nx.Graph()
for i, pos in enumerate(samples):
G.add_node(i, pos=pos)
# 4. 连接所有节点（含起终点）
for i, point in enumerate(samples):
distances, indices = kd_tree.query(point, k=k_neighbors+1)
for idx in indices[1:]: # 排除自身
neighbor = samples[idx]
if not is_collision_line(point, neighbor, box_list):
dist = np.linalg.norm(np.array(point)-np.array(neighbor))
if dist < radius:
G.add_edge(i, idx, weight=dist)
return G
def query_path(G, start, end, box_list):
"""在PRM图中查询起终点路径"""
# 找到起终点对应的节点ID
start_id = None
end_id = None
for node in G.nodes(data='pos'):
if np.allclose(node[1], start):
start_id = node[0]
if np.allclose(node[1], end):
end_id = node[0]
# 使用Dijkstra算法寻路
try:
path_nodes = nx.shortest_path(G, start_id, end_id, weight='weight')
path = [G.nodes[n]['pos'] for n in path_nodes]
return path
except nx.NetworkXNoPath:
return None
def visualize_prm(G, box_list, start, end, path=None):
"""增强可视化：显示起终点与规划路径"""
plt.figure(figsize=(10, 10))
ax = plt.gca()
# 绘制障碍物
for (x1, y1, x2, y2) in box_list:
ax.add_patch(Rectangle(
(min(x1, x2), min(y1, y2)),
abs(x2 - x1), abs(y2 - y1),
edgecolor='black', facecolor='gray', alpha=0.7
))
# 绘制所有节点与边
pos = nx.get_node_attributes(G, 'pos')
nx.draw_networkx_nodes(G, pos, node_size=20, node_color='blue', ax=ax)
nx.draw_networkx_edges(G, pos, edge_color='lightgray', width=0.5, ax=ax)
# 高亮起终点
nx.draw_networkx_nodes(G, pos, nodelist=[0], node_color='green', node_size=100, ax=ax)
nx.draw_networkx_nodes(G, pos, nodelist=[1], node_color='red', node_size=100, ax=ax)
# 绘制规划路径
if path:
for i in range(len(path)-1):
x1, y1 = path[i]
x2, y2 = path[i+1]
plt.plot([x1, x2], [y1, y2], 'r-', linewidth=2)
plt.xlim(0, width)
plt.ylim(0, height)
plt.grid(True)
plt.title("PRM Path Planning")
plt.show()
if __name__ == "__main__":
# 定义地图与障碍物
height, width = 100, 100
box_list = [
(20, 20, 40, 40), # 障碍物1,对角点坐标
(60, 10, 80, 90), # 障碍物2
(10, 70, 30, 90) # 障碍物3
]
start = (5, 5) # 起点坐标
end = (95, 80) # 终点坐标
# 生成PRM图
prm_graph = prm_algorithm(height, width, box_list, start, end, n_samples=150)
# 查询路径
path = query_path(prm_graph, start, end, box_list)
# 可视化结果
visualize_prm(prm_graph, box_list, start, end, path)
其中基于搜索的训练算法可以参考上一篇blog：基于搜索的路径规划算法
优缺点
优点：
- 高维适用性：适合机械臂、人形机器人等多自由度系统。
- 预处理优势：路线图可离线构建，在线查询高效。
- 无需显式建模障碍物：仅依赖碰撞检测。
缺点：
- 对参数敏感：采样数量和连接策略影响成功率。
- 可能遗漏路径：采样不足时无法找到可行路径。
- 动态环境不友好：障碍物变化后需重新构建路线图。
- 不够高效：构建阶段检测采样点是否在障碍物中比较费时间
改进变种
- PRM*：渐进式优化，通过增加采样逐步优化路径。
- Lazy PRM：延迟碰撞检查，减少计算开销。
- 动态PRM：结合增量式更新处理动态障碍物。
PRM通过随机采样+图构建将高维路径规划问题转化为图搜索问题，结合其变种和改进策略，可适应不同场景需求。
RRT算法
RRT算法（Rapidly-exploring Random Tree，快速探索随机树）是一种基于采样的路径规划算法，特别适合解决高维空间（如机械臂、无人机）和复杂障碍物环境中的路径搜索问题。其核心思想是通过随机采样和树形扩展，快速探索未知空间，最终找到可行路径。
核心思想
RRT通过构建一棵从起点出发的树，不断向随机方向扩展，直到接近目标点。它不依赖全局地图，适合动态或高维环境，牺牲路径最优性以换取搜索速度。
算法步骤
-
初始化
- 创建一棵树 T T T，根节点为起点 q start q_{\text{start}} qstart。
-
随机采样
- 在配置空间（C-Space）中随机生成一个点 q rand q_{\text{rand}} qrand（自由空间或全空间，取决于实现）。
-
寻找最近节点
- 在树 T T T中找到距离 q rand q_{\text{rand}} qrand最近的节点 q near q_{\text{near}} qnear。
-
扩展新节点
- 从 q near q_{\text{near}} qnear向 q rand q_{\text{rand}} qrand方向移动一个固定步长 δ \delta δ，得到新节点 q new q_{\text{new}} qnew。
- 碰撞检测：检查路径
q
near
→
q
new
q_{\text{near}} \to q_{\text{new}}
qnear→qnew是否与障碍物碰撞。
- 若无碰撞，将 q new q_{\text{new}} qnew加入树 T T T，并记录父节点为 q near q_{\text{near}} qnear。
-
终止条件
- 当 q new q_{\text{new}} qnew进入目标区域（如距离终点小于阈值），或达到最大迭代次数时结束。
-
路径回溯
- 从终点反向追踪父节点至起点，得到最终路径。
关键机制
1. 偏向目标采样（Goal Bias）
- 为了加速收敛，以一定概率（如5%）直接采样目标点 q goal q_{\text{goal}} qgoal作为 q rand q_{\text{rand}} qrand，而非完全随机采样。
2. 步长控制
- 固定步长 δ \delta δ平衡探索速度与精度。步长过大会增加碰撞风险，过小则效率低下。
3. 最近邻搜索优化
- 使用空间数据结构（如KD-Tree）加速最近节点的查找。
实现
代码实现：
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.patches import Rectangle
from scipy.spatial import KDTree
class RRTPlanner:
def __init__(self, start, end, height, width, box_list):
"""初始化参数"""
self.start = np.array(start)
self.end = np.array(end)
self.map_height = height
self.map_width = width
self.obstacles = box_list # 障碍物对角顶点列表 [[(x1,y1),(x3,y3)], ...]
# 算法参数
self.step_size = 15 # 扩展步长
self.goal_sample_rate = 0.1 # 目标偏向概率
self.max_iter = 2000 # 最大迭代次数
self.goal_radius = 10 # 终点接受半径
# 可视化初始化
self.fig, self.ax = plt.subplots(figsize=(10,10))
self.ax.set_xlim(0, width)
self.ax.set_ylim(0, height)
self._draw_obstacles()
def _is_collision(self, p1, p2):
"""改进碰撞检测：线段与多边形障碍物相交检测"""
# 将线段离散化为多个检测点
points = zip(np.linspace(p1[0], p2[0], 20),
np.linspace(p1[1], p2[1], 20))
for (x,y) in points:
for box in self.obstacles:
(x1,y1), (x3,y3) = box
x_min = min(x1, x3)
x_max = max(x1, x3)
y_min = min(y1, y3)
y_max = max(y1, y3)
if x_min <= x <= x_max and y_min <= y <= y_max:
return True
return False
def _draw_obstacles(self):
"""绘制障碍物"""
for box in self.obstacles:
(x1,y1), (x3,y3) = box
width = abs(x3 - x1)
height = abs(y3 - y1)
self.ax.add_patch(Rectangle(
(min(x1,x3), min(y1,y3)), width, height,
edgecolor='black', facecolor='#2F4F4F', alpha=0.7
))
def plan(self):
"""RRT主算法"""
# 初始化树结构
tree = KDTree([self.start])
node_list = [self.start]
parent_list = [-1]
plt.plot(self.start[0], self.start[1], 'go', markersize=15, zorder=3) # 起点
plt.plot(self.end[0], self.end[1], 'ro', markersize=15, zorder=3) # 终点
for _ in range(self.max_iter):
# 随机采样（目标偏向策略[5](@ref)）
if np.random.rand() < self.goal_sample_rate:
rand_point = self.end
else:
rand_point = np.random.rand(2) * [self.map_width, self.map_height]
# 寻找最近节点
_, nearest_idx = tree.query(rand_point)
nearest_node = node_list[nearest_idx]
# 扩展新节点
direction = rand_point - nearest_node
distance = np.linalg.norm(direction)
direction_unit = direction / distance if distance > 0 else 0
new_node = nearest_node + direction_unit * self.step_size
# 边界约束
new_node = np.clip(new_node, [0,0], [self.map_width, self.map_height])
# 碰撞检测
if not self._is_collision(nearest_node, new_node):
# 动态可视化
self.ax.plot([nearest_node[0], new_node[0]],
[nearest_node[1], new_node[1]],
color='gray', linewidth=0.5, alpha=0.3)
plt.pause(0.001)
# 更新树结构
node_list.append(new_node)
parent_list.append(nearest_idx)
tree = KDTree(node_list)
# 检查是否到达目标区域
if np.linalg.norm(new_node - self.end) < self.goal_radius:
return self._extract_path(node_list, parent_list)
return None # 未找到路径
def _extract_path(self, nodes, parents):
"""回溯生成路径"""
path = [self.end]
current_idx = len(nodes)-1
while current_idx != 0:
path.append(nodes[current_idx])
current_idx = parents[current_idx]
path.append(self.start)
path.reverse()
# 绘制最终路径
x_vals = [p[0] for p in path]
y_vals = [p[1] for p in path]
self.ax.plot(x_vals, y_vals, 'b-', linewidth=2, zorder=2)
return path
if __name__ == "__main__":
start = (10, 10)
end = (90, 90)
height, width = 100, 100
obstacles = [
[ (30,30), (70,70) ], # 正方形障碍物
[ (20,60), (40,80) ] # 矩形障碍物
]
# 创建规划器
planner = RRTPlanner(start, end, height, width, obstacles)
# 执行规划
path = planner.plan()
# 显示结果
plt.title("RRT Path Planning")
plt.show()
参数选择建议
- 步长 δ \delta δ：通常为配置空间范围的5%~10%。
- 目标偏向概率：5%~10%。
- 最大迭代次数：根据环境复杂度调整（通常1e4~1e6次）。
效果：
优缺点
优点：
- 高效探索：适合高维空间和复杂障碍物环境。
- 无需预处理：直接在线规划，适应动态变化。
- 概率完备性：采样数足够时，若存在路径则一定能找到。
缺点：
- 路径非最优：找到的路径可能迂回。
- 随机性影响：不同次运行结果可能不一致。
- 狭窄通道困难：随机采样可能难以进入狭窄通道。
改进变种
- RRT*：
- 通过重连优化逐步逼近最优路径，具备渐近最优性。
- Informed RRT*：
- 在找到初始路径后，限制采样区域，加速优化。
- RRT-Connect：
- 同时从起点和终点构建两棵树，双向扩展以提升效率。
- Dynamic RRT：
- 支持动态障碍物，实时更新树结构。
总结：RRT算法通过随机采样和树形扩展，在高维和动态环境中实现了高效的路径探索。尽管路径质量可能不佳，但其速度和适应性使其成为机器人、自动驾驶等领域的核心算法之一。结合RRT*等优化变种，可进一步提升实用性。
扫一扫
660
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
