路径规划算法入门

基础概念与预备知识

在深入算法之前，必须理解几个核心概念，这是所有规划算法的基石。

配置空间（Configuration Space, \(C_{space}\)）

  • 定义：机器人所有可能位置（和姿态）的集合。
  • 例子：
    • 一个在 2D 平面上移动的点机器人：\(C_{space} = \mathbb{R}^2\) (x, y)。
    • 一个带方向的机器人：\(C_{space} = \mathbb{R}^2 \times SO(2)\) (x, y, \(\theta\))。
    • 一个 6 自由度机械臂：\(C_{space} = \mathbb{R}^6\) (6 个关节角)。
  • 重要性：我们将复杂的机器人碰撞问题转化为 \(C_{space}\) 中的一个点是否在障碍物内的问题。

自由空间与障碍空间

  • \(C_{obs}\) (Obstacle Space)：配置空间中会导致机器人与环境或自身碰撞的区域。
  • \(C_{free}\) (Free Space)：配置空间中安全的区域，\(C_{free} = C_{space} \setminus C_{obs}\)。
  • 目标：在 \(C_{free}\) 中找到一条从 \(q_{start}\) 到 \(q_{goal}\) 的连续路径。

为什么需要基于采样的算法？

  • 传统算法局限：如 A*、Dijkstra 需要构建网格（Grid）。在高维空间（如 7 自由度机械臂）中，网格数量呈指数爆炸（维数灾难）。
  • 采样优势：不依赖网格，通过在空间中随机撒点来探索 \(C_{free}\)，对高维空间更友好。
  • 代价：通常不是最优解（RRT），且是概率完备的（Probabilistically Complete，即如果有解，随着时间推移找到解的概率趋近于 1，但找不到解时无法判定无解）。

PRM

核心思想

“先建图，后查询”。PRM 不针对特定的起点和终点，而是试图构建一个覆盖整个 \(C_{free}\) 的连通图（路线图）。一旦图建好，任何起点和终点都可以快速通过图搜索找到路径。

算法流程

  1. 采样：在 \(C_{space}\) 中随机生成 \(N\) 个节点。
  2. 碰撞检测：剔除落在 \(C_{obs}\) 中的节点，保留 \(C_{free}\) 中的节点。
  3. 连接邻居：
    • 对于每个节点 \(q\)，寻找其附近的 \(k\) 个最近邻节点（或半径 \(r\) 内的所有节点）。
    • 尝试用直线（或局部规划器）连接 \(q\) 和邻居 \(q_{near}\)。
    • 关键细节：检查连接线段上是否有碰撞。如果无碰撞，则在图中添加一条边 \((q, q_{near})\)。
  4. 结果：得到一个无向图 \(G = (V, E)\)。

伪代码

def PRM_Build(N, k):
    V = [] # 节点集合
    E = [] # 边集合
    
    # 1. 采样
    while len(V) < N:
        q_rand = Sample_Config()
        if not Collision(q_rand):
            V.append(q_rand)
            
    # 2. 连接
    for q in V:
        Neighbors = Find_K_Nearest(V, q, k)
        for q_near in Neighbors:
            if not Collision_Check_Edge(q, q_near):
                E.add((q, q_near))
                
    return Graph(V, E)

优缺点分析

  • 优点：
    • 适合多查询场景（环境不变，起点终点频繁变）。
    • 计算负担主要在建图，查询非常快。
  • 缺点：
    • 窄通道问题：如果自由空间中有狭窄通道，随机采样很难正好采到通道内的点，导致图不连通。
    • 不适合动态环境（环境变了图就废了）。
  • 关键参数：
    • \(N\)（采样点数）：越多连通性越好，但构建越慢。
    • \(k\) 或 \(r\)（连接半径）：越大连通性越好，但边越多，后续搜索越慢。

RRT

核心思想

“单查询，树生长”。RRT 从起点开始，像树一样向空间生长。它利用随机采样的偏置，快速探索未知区域，特别适合高维空间和微分约束（如车辆运动学）。

算法流程

  1. 初始化：树 \(T\) 只包含根节点 \(q_{start}\)。
  2. 循环（直到找到目标或达到最大迭代次数）：
    • 采样：随机生成一个点 \(q_{rand}\)。通常有 5% 的概率直接采样 \(q_{goal}\)，以引导树向目标生长（称为 Goal Bias）。
    • 最近邻：在树 \(T\) 中找到距离 \(q_{rand}\) 最近的节点 \(q_{near}\)。
    • 转向：从 \(q_{near}\) 向 \(q_{rand}\) 方向移动一个步长 \(\eta\)，得到新节点 \(q_{new}\)。公式：\(q_{new} = q_{near} + \eta \cdot \frac{q_{rand} - q_{near}}{\|q_{rand} - q_{near}\|}\)。
    • 碰撞检测：检查 \(q_{near}\) 到 \(q_{new}\) 的连线是否安全。
    • 添加节点：如果安全，将 \(q_{new}\) 加入树 \(T\)，并添加边 \((q_{near}, q_{new})\)。
    • 目标检查：如果 \(\|q_{new} - q_{goal}\| < \text{threshold}\)，则连接目标，成功。

伪代码

def RRT(q_start, q_goal, max_iter, step_size):
    T = Tree(q_start)
    
    for i in range(max_iter):
        # 1. 采样 (带目标偏置)
        if random() < 0.05:
            q_rand = q_goal
        else:
            q_rand = Sample_Random()
            
        # 2. 寻找最近节点
        q_near = Nearest_Neighbor(T, q_rand)
        
        # 3. 转向 (限制步长)
        q_new = Steer(q_near, q_rand, step_size)
        
        # 4. 碰撞检测
        if not Collision_Check(q_near, q_new):
            T.add_node(q_new)
            T.add_edge(q_near, q_new)
            
            # 5. 检查是否到达目标
            if Distance(q_new, q_goal) < threshold:
                return Path(T, q_goal)
                
    return Failure

注意事项

  1. Voronoi 偏置：RRT 倾向于向大空间生长。因为树节点周围的 Voronoi 区域越大，随机点落在该区域的概率就越大。这使得 RRT 能快速探索未访问区域。
  2. 步长（\(\eta\)）：
    • 太大：容易穿过狭窄通道导致碰撞检测失败，路径曲折。
    • 太小：树生长极慢，收敛时间长。
  3. 距离度量：通常使用欧几里得距离。但在机械臂中，可能需要考虑关节权重的加权距离。

优缺点分析

  • 优点：
    • 实现简单，速度快。
    • 天然适合处理微分约束（通过修改 Steer 函数）。
    • 单查询效率高。
  • 缺点：
    • 路径质量差：生成的路径非常曲折（锯齿状），通常不是最优路径。
    • 贪婪生长：一旦树长偏了，很难修正。

RRT*

核心思想

RRT* 是 RRT 的改进版，引入了重连 机制。

  • 目标：实现渐进最优性。即随着采样点数 \(N \to \infty\)，RRT* 找到的路径代价收敛到全局最优解。
  • 区别：RRT 只关心“能不能连上”，RRT* 关心“连上后代价是不是更小”。

算法流程（与 RRT 的区别）

在找到 \(q_{new}\) 且确认无碰撞后，RRT* 多了两步操作：

  1. 选择最佳父节点：
    • 在 \(q_{new}\) 附近寻找所有邻居节点 \(Q_{near}\)。
    • 遍历 \(Q_{near}\)，计算从 \(q_{start}\) 经过 \(q_{near}\) 到达 \(q_{new}\) 的总代价。
    • 选择使总代价最小的 \(q_{near}\) 作为 \(q_{new}\) 的父节点（而不仅仅是距离 \(q_{rand}\) 最近的点）。
  2. 重连：
    • 再次遍历 \(Q_{near}\)。
    • 检查如果将 \(q_{new}\) 作为 \(q_{near}\) 的父节点，是否能减小 \(q_{near}\) 到起点的代价。
    • 如果能减小，则断开 \(q_{near}\) 原来的父连接，将其父节点改为 \(q_{new}\)。

伪代码

        # ... (采样、最近邻、转向、碰撞检测同 RRT) ...
        
        if not Collision_Check(q_near, q_new):
            # 1. 寻找附近的邻居 (半径 r)
            Q_near = Find_Neighbors(T, q_new, radius)
            
            # 2. 选择最佳父节点
            q_min = q_near # 默认
            min_cost = Cost(q_near) + Line_Cost(q_near, q_new)
            
            for q_cand in Q_near:
                cost = Cost(q_cand) + Line_Cost(q_cand, q_new)
                if cost < min_cost and not Collision_Check(q_cand, q_new):
                    min_cost = cost
                    q_min = q_cand
            
            # 3. 添加节点 (连接到最佳父节点)
            T.add_node(q_new, parent=q_min)
            
            # 4. 重连 (Rewire)
            for q_cand in Q_near:
                cost_new = Cost(q_new) + Line_Cost(q_new, q_cand)
                if cost_new < Cost(q_cand) and not Collision_Check(q_new, q_cand):
                    T.rewire(q_cand, new_parent=q_new)

注意事项

  1. 邻居半径（\(r\)）：
    • RRT* 中 \(r\) 不能是常数。理论证明，\(r\) 必须随采样数 \(N\) 变化：
    • \(r_N = \gamma (\frac{\log N}{N})^{1/d}\)，其中 \(d\) 是空间维度，\(\gamma\) 是常数。
    • 工程实践：通常取一个固定的较大值也能获得不错的效果，但理论最优性需要动态半径。
  2. 计算代价：Cost(q) 表示从起点 \(q_{start}\) 沿树边到达节点 \(q\) 的累积距离（或代价）。
  3. 收敛速度：RRT* 收敛到最优解的速度很慢。通常需要比 RRT 多几个数量级的迭代次数才能看到明显的路径优化。

优缺点分析

  • 优点：
    • 渐进最优，路径更平滑、更短。
    • 保留了 RRT 的采样优势。
  • 缺点：
    • 计算量大（每次插入节点都要搜索邻居并尝试重连）。
    • 收敛慢，实时性不如 RRT。

算法对比总结

特性 PRM RRT RRT*
数据结构 图 树 树
查询类型 多查询 单查询 单查询
最优性 取决于图搜索（通常近似最优） 非最优 (可行即可) 渐进最优
主要阶段 建图 + 搜索 生长 + 连接 生长 + 连接 + 重连
适用场景 静态环境，多次规划 动态环境，高维，实时性要求高 对路径质量有要求，离线规划
计算瓶颈 建图时的碰撞检测 最近邻搜索 重连时的邻居搜索与碰撞检测
窄通道能力 差（需特殊采样策略） 中 中

工程实现细节与优化技巧

如果你打算自己写代码实现这些算法，以下细节决定了代码的性能和可用性。

最近邻搜索

  • 问题：随着节点数增加，线性搜索 \(O(N)\) 太慢。
  • 解决方案：使用空间索引数据结构。
    • KD-Tree：最常用，低维空间（\(d < 20\)）效率极高，查询 \(O(\log N)\)。
    • Ball Tree：在高维空间表现通常优于 KD-Tree。
    • 库推荐：Python 使用 scipy.spatial.KDTree 或 sklearn.neighbors；C++ 使用 FLANN 或 nanoflann。

碰撞检测

  • 瓶颈：这是最耗时的操作，占据了 90% 以上的计算时间。
  • 优化：
    1. 早期退出：在检查线段碰撞时，一旦发现一点碰撞立即返回，不要检查完整个线段。
    2. 分层检测：先用简单的包围盒（AABB）或包围球进行粗略检测，只有通过后再进行精确的网格检测。
    3. 缓存：如果环境静态，某些碰撞结果可以缓存。

路径平滑

  • 问题：采样算法生成的路径由许多短线段组成，呈锯齿状，机器人难以执行。
  • 后处理方法：
    1. 剪枝：尝试直接连接路径上的非相邻节点（如 \(P_0\) 连 \(P_5\)），如果无碰撞，则删除中间节点。
    2. 贝塞尔曲线/样条：将折线拟合为平滑曲线（需注意曲线可能会偏离原路径进入障碍物，需再次校验）。
    3. 梯度优化：将路径视为弹性带，通过优化算法最小化路径长度和曲率。

高级变体

  • RRT-Connect：双向生长（从起点和终点同时长树），速度比单向 RRT 快很多。
  • Informed RRT*：在找到初始解后，限制采样空间在一个椭圆区域内（焦点为起点和终点），加速收敛到最优解。
  • Anytime RRT*：允许算法在任意时间中断，返回当前最好的解，随着时间推移解的质量不断提高。
posted @ 2026-03-28 17:05  AFewMoon  阅读(7)  评论(0)    收藏  举报