📚 课程目录第2章路径规划基础

2. 路径规划基础

在深入RRT与RRT Connect算法之前，必须先建立一套精确的几何与数学语言。路径规划的核心挑战在于：如何在包含障碍物的环境中，找到一条从起点到终点的可行通路。然而，"环境"和"通路"这两个词在机器人学中有着严格的定义——我们不是在真实物理空间中直接搜索，而是在一个抽象的配置空间（Configuration Space, C-Space）中搜索。本章将系统梳理这五大基础概念，为后续的采样算法奠定严格的形式化基础。

2.1 工作空间与配置空间：从物理世界到抽象空间

核心原理

工作空间（Workspace, W）是机器人实际作业的物理空间，通常表示为二维平面（ℝ²）或三维空间（ℝ³）。它描述的是机器人本体及其连杆在物理世界中占据的几何区域。工作空间中的障碍物（Obstacles, O）是真实存在的物理实体，如墙壁、货架、其他设备。工作空间中的自由区域定义为 W_free = W \ O。

配置空间（Configuration Space, C）是机器人所有可能配置（Configuration）的集合。一个配置 q 是能够完整描述机器人上每一个点的空间位置的一组最小参数。对于平面移动机器人，q = (x, y, θ)，其中 (x,y) 为位置，θ 为朝向，维度为3。对于六轴机械臂，q 是六个关节角度的向量 (θ₁, θ₂, ..., θ₆)，维度为6。配置空间的维度等于机器人的自由度（DOF）。

关键洞察在于：工作空间中的障碍物映射到配置空间中，会形成高维的"障碍区域" C_obs。一个配置 q 如果导致机器人本体与工作空间中的障碍物发生碰撞，则 q ∈ C_obs。反之，如果机器人处于安全状态，则 q ∈ C_free = C \ C_obs。路径规划问题因此被转化为：在 C_free 中寻找连接 q_start 和 q_goal 的连续曲线。

判断方法与操作流程

构建配置空间障碍物（C_obs）的常用方法是碰撞检测。对于每个候选配置 q，执行以下流程：

通过正运动学计算，将配置 q 映射为机器人各连杆在工作空间中的几何形状（如包围盒、三角网格）。
将机器人几何与障碍物几何进行求交测试（如分离轴定理、GJK算法）。
若存在交集，则标记 q ∈ C_obs；否则 q ∈ C_free。

在实际工程中，精确构建完整的 C_obs 边界是计算不可行的（尤其在高维空间），因此采样算法（如RRT）采用增量式碰撞检测：仅在采样点或局部路径上检测碰撞，而非预先计算整个 C_obs。

经验提醒：初学者常混淆"工作空间中的路径"与"配置空间中的路径"。例如，在二维平面中画一条直线穿过两个障碍物之间的缝隙，这看似可行，但若机器人有朝向约束（如车不能侧移），则这条直线在配置空间中可能完全位于 C_obs 内。务必始终在配置空间中思考规划问题。

案例拆解：平面移动机器人的空间映射

考虑一个在矩形仓库中作业的差速驱动机器人，其工作空间为 10m × 8m 的平面。机器人本体简化为半径 0.3m 的圆形。仓库中有两个矩形货架作为障碍物。

工作空间表示：W = [0,10] × [0,8]，障碍物 O₁ = [2,4] × [2,3]，O₂ = [6,8] × [5,7]。
配置空间构建：由于机器人是圆形，配置空间维度为3（x, y, θ）。但注意，对于圆形机器人，其碰撞状态与 θ 无关（各向同性），因此 C_obs 可简化为二维投影。将障碍物向外膨胀机器人半径 0.3m，得到 C_obs 的二维截面：O₁' = [1.7,4.3] × [1.7,3.3]，O₂' = [5.7,8.3] × [4.7,7.3]。
规划结果：在 C_free 中搜索从 q_start=(0.5, 0.5) 到 q_goal=(9.5, 7.5) 的路径。由于膨胀后的障碍物几乎将地图分割，算法需要绕行。

此案例展示了工作空间到配置空间的"膨胀"操作——这是处理机器人本体尺寸的标准方法。

常见误区与总结

风险提示：不要忽略机器人的几何形状而将机器人视为质点。若将上述案例中的机器人视为质点（半径=0），则规划出的路径可能紧贴货架边缘，实际执行时必然碰撞。正确做法是始终在配置空间（已膨胀障碍物）中规划，或使用精确的碰撞检测。

总结：工作空间是物理现实，配置空间是决策空间。RRT算法之所以在配置空间中工作，是因为它天然支持高维空间，且能统一处理各种机器人类型（机械臂、移动机器人、无人机）。

2.2 障碍物表示：几何、栅格与采样

核心原理

障碍物的表示方式直接影响碰撞检测的效率和精度。在路径规划中，常见的表示方法有三类：几何表示（精确但计算复杂）、栅格表示（离散但简单）、采样表示（适用于大规模或传感器数据）。

表示方法	描述	优点	缺点	适用场景
几何表示	多边形、多面体、CSG（构造实体几何）	精确、无分辨率损失	碰撞检测计算量大	CAD模型、结构化环境
栅格表示	将空间离散为网格，每个格子标记占据/空闲	查询O(1)、易于实现	分辨率受限、内存占用高	2D地图、SLAM建图
采样表示	点云、OctoMap（八叉树概率地图）	内存效率高、支持动态更新	碰撞检测需近邻搜索	3D感知、无人机导航

操作流程：栅格地图的构建

确定地图边界和分辨率（如 0.05m/格）。
将传感器数据（激光雷达、深度相机）投影到栅格坐标系。
对每个栅格，根据命中次数和穿过次数更新占据概率 p。
设定阈值（如 p>0.7 为占据，p<0.3 为空闲），生成二值地图。

对于RRT算法，碰撞检测通常调用isCollision(q)函数，该函数内部根据障碍物表示执行不同的查询：几何表示用求交测试，栅格表示查表，采样表示用KD-Tree近邻搜索。

工程实践：在仓储机器人项目中，我们使用2D栅格地图（分辨率5cm），障碍物膨胀半径设为机器人半径+安全余量（0.3m+0.1m）。碰撞检测函数平均耗时约0.02ms，满足RRT算法每秒上千次采样的需求。若使用精确几何表示，单次碰撞检测需0.5ms，将严重拖慢规划速度。

案例拆解：动态障碍物的处理

在AGV调度场景中，障碍物并非静态。我们采用双层地图策略：静态层使用几何表示（精确的货架位置），动态层使用栅格表示（实时更新的行人、叉车）。RRT规划时，先查询静态层，再叠加动态层，若任一图层判定碰撞，则拒绝该采样点。这种混合表示在保证安全性的同时，维持了规划实时性。

常见误区

风险提示：栅格分辨率的选择是权衡。分辨率过高（如1mm）会导致内存爆炸（100m×100m地图需10^10格）；分辨率过低（如1m）会丢失窄通道信息，导致规划失败。建议根据机器人尺寸和通道宽度选择分辨率，通常为机器人最小尺寸的1/5至1/10。

2.3 路径与轨迹：几何与时间的区别

核心原理

路径（Path）是配置空间中的连续几何曲线，定义为 τ(s): [0,1] → C_free，其中 s 是归一化弧长参数。路径只关心"走哪条路"，不关心"何时到达"。

轨迹（Trajectory）是带时间信息的路径，定义为 π(t): [0,T] → C_free，其中 t 是时间。轨迹不仅包含几何信息，还包含速度、加速度等运动学约束。轨迹通常由路径+时间规划（Time Scaling）生成。

RRT和RRT Connect算法输出的是路径（一系列配置点序列），而非轨迹。后续需要经过轨迹生成模块（如梯形速度规划、S型曲线规划）将路径转化为可执行的轨迹。

判断方法：何时需要区分

若仅需验证可达性（如"能否到达目标点"），只需路径。
若需实际执行（如机械臂抓取），必须生成轨迹，且需满足关节速度/加速度限制。
若涉及动态避障（移动障碍物），必须在轨迹层面进行时间维度的规划。

经验提醒：RRT生成的路径往往包含大量冗余转折点，直接用于轨迹生成会导致速度剧烈波动。通常先使用路径平滑算法（如样条插值、捷径剪枝）简化路径，再进行时间规划。

案例拆解：机械臂的路径与轨迹

六轴机械臂从 A 点抓取工件到 B 点放置。RRT算法在6维配置空间中找到一条无碰撞路径，包含50个路径点。若直接执行：

每个路径点作为关节位置指令，机械臂将出现"走走停停"现象，冲击大。
正确做法：使用三次样条对50个点进行插值，生成平滑路径。
再应用梯形速度规划，设定最大关节速度 30°/s，加速度 60°/s²，生成轨迹 π(t)。
轨迹包含每个时刻的关节位置、速度、加速度，可直接发送给伺服控制器。

结果：执行时间从原始路径的 15s（含停顿）缩短至 8s（连续运动），且末端振动显著降低。

常见误区

风险提示：不要将路径点直接作为轨迹点。路径点之间是线性插值，在关节空间可能穿过奇异位形或产生速度突变。必须进行平滑和时间参数化。

2.4 规划问题的数学定义与形式化

核心原理

路径规划问题的标准数学定义如下：

给定：
  - 配置空间 C（通常为 ℝⁿ 的子集）
  - 障碍区域 C_obs ⊂ C，自由空间 C_free = C \ C_obs
  - 初始配置 q_start ∈ C_free
  - 目标配置 q_goal ∈ C_free
  - 约束条件（可选）：路径长度、曲率、能量等

求：
  连续函数 τ: [0,1] → C_free
  满足：
    τ(0) = q_start
    τ(1) = q_goal
  且对于所有 s ∈ [0,1]，τ(s) ∈ C_free

优化目标（可选）：
  min ∫₀¹ ||τ'(s)|| ds  （最短路径）
  或 min max_s κ(s)     （最小曲率）
  或 min ∫₀¹ ||τ''(s)||² ds （最平滑）

RRT算法解决的是可行性问题（是否存在路径），而RRT*等变体解决最优性问题（找到最短路径）。RRT Connect通过双向搜索加速可行性判断。

操作流程：形式化在算法实现中的体现

采样：在 C 中随机采样一个配置 q_rand。采样分布可以是均匀分布，也可以偏向目标（如以5%概率直接采样 q_goal）。
最近邻：在已有树中找到距离 q_rand 最近的节点 q_near（使用欧氏距离或加权距离）。
扩展：从 q_near 向 q_rand 方向步进 Δq（步长），生成新节点 q_new。
碰撞检测：检查 q_new 以及 q_near→q_new 之间的路径是否在 C_free 内。若碰撞，则放弃该次扩展。
终止条件：当 q_new 与 q_goal 距离小于阈值，或树中包含 q_goal 附近节点时，提取路径。

数学视角：RRT算法实际上是在 C_free 上构建一个递增的连通图。树的每个节点是一个配置，每条边是局部路径。算法的概率完备性（Probabilistic Completeness）保证：当采样次数趋于无穷时，找到路径的概率趋近于1。这一性质源于 C_free 的连通分量被采样的概率随采样点增加而趋近于1。

案例拆解：二维导航中的数学定义

设移动机器人配置空间 C = [0,10] × [0,8] × [0,2π)，障碍物为两个矩形膨胀区域。定义：

q_start = (0.5, 0.5, 0)
q_goal = (9.5, 7.5, π/2)
距离度量：d(q₁,q₂) = √(Δx² + Δy²) + 0.5·|Δθ| （加权欧氏距离，权重0.5用于平衡位置与角度）
步长 Δq = 0.3m

RRT算法运行5000次采样后，找到一条从起点到终点的路径，包含120个节点。路径总长 12.3m，而直线距离为 11.4m，说明路径略有绕行。通过RRT*优化后，路径缩短至 11.6m。

常见误区与总结

风险提示：距离度量的选择对算法效率影响巨大。若忽略朝向维度（权重为0），算法可能频繁生成朝向不合理的节点，导致后续扩展困难。若权重过大，则位置收敛慢。建议根据机器人运动学特性调整权重。

总结：数学定义将物理问题抽象为拓扑空间中的连通性问题。RRT系列算法通过随机采样隐式地探索 C_free 的连通性，避免了显式构建 C_obs 的困难。理解这些基础概念，是掌握后续算法优化（如采样偏置、步长自适应、双向扩展）的前提。