6.1 运动规划：构型空间、采样与优化方法

6.1 运动规划：构型空间、采样与优化方法

给机器人一个起点和一个终点，它怎么找到一条不撞障碍物的路径？这就是运动规划问题。看似简单，实则计算复杂度极高（PSPACE-hard），需要专门的算法。

6.1.1 运动规划问题定义

运动规划（Motion Planning） 问题形式化：

• 机器人：由 n 个关节描述，构型 q \in \mathcal{C}（构型空间）。
• 障碍物：工作空间中的几何体。
• 起点 q_{start} 与 终点 q_{goal}。
• 目标：找到连续路径 \sigma: [0,1] \to \mathcal{C}_{free}，满足 \sigma(0) = q_{start}，\sigma(1) = q_{goal}，且 \sigma(s) \in \mathcal{C}_{free} 对所有 s。

其中 \mathcal{C}_{free} 是 自由构型空间——机器人不与障碍物碰撞、不违反关节限制的所有构型集合。

6.1.2 构型空间（C-space）：运动规划的舞台

为什么规划在构型空间而非工作空间？举个简单例子：

一个 2 自由度机械臂，工作空间是 2D 平面，但它的状态由两个关节角 (θ1, θ2) 决定。构型空间就是 (θ1, θ2) 平面上的一个区域：

<svg viewBox="0 0 600 360" xmlns="http://www.w3.org/2000/svg">
  <!-- 工作空间 -->
  <text x="100" y="20" font-size="12" font-weight="bold">工作空间（物理世界）</text>
  <rect x="40" y="40" width="180" height="240" fill="#f9fafb" stroke="#9ca3af"/>
  <!-- 障碍物 -->
  <rect x="120" y="120" width="60" height="60" fill="#94a3b8" stroke="#475569"/>
  <text x="150" y="155" text-anchor="middle" font-size="10">障碍</text>
  <!-- 机械臂示意 -->
  <line x1="60" y1="260" x2="150" y2="180" stroke="#2563eb" stroke-width="4"/>
  <line x1="150" y1="180" x2="200" y2="100" stroke="#2563eb" stroke-width="4"/>
  <circle cx="60" cy="260" r="6" fill="#dc2626"/>
  <circle cx="150" cy="180" r="5" fill="#dc2626"/>
  <circle cx="200" cy="100" r="6" fill="#16a34a"/>
  <text x="50" y="285" font-size="10">基座</text>
  <text x="210" y="95" font-size="10">末端</text>

  <!-- C-space -->
  <text x="380" y="20" font-size="12" font-weight="bold">构型空间 C-space (θ1, θ2)</text>
  <rect x="320" y="40" width="240" height="240" fill="#f9fafb" stroke="#9ca3af"/>
  <text x="430" y="305" text-anchor="middle" font-size="10">θ1</text>
  <text x="305" y="165" text-anchor="middle" font-size="10" transform="rotate(-90 305 165)">θ2</text>
  <!-- 障碍区域 C-space obstacle -->
  <path d="M 380 90 Q 420 80 460 110 Q 470 150 440 180 Q 400 200 370 160 Q 360 120 380 90 Z" fill="#94a3b8" stroke="#475569" opacity="0.7"/>
  <text x="415" y="145" text-anchor="middle" font-size="9">C-obstacle</text>
  <!-- 起点 -->
  <circle cx="350" cy="250" r="6" fill="#dc2626"/>
  <text x="335" y="270" font-size="9">q_start</text>
  <!-- 终点 -->
  <circle cx="530" cy="70" r="6" fill="#16a34a"/>
  <text x="515" y="60" font-size="9">q_goal</text>
  <!-- 路径 -->
  <path d="M 350 250 Q 320 180 360 130 Q 410 80 480 60 Q 510 65 530 70" fill="none" stroke="#2563eb" stroke-width="2" stroke-dasharray="4 3"/>
</svg>
​

工作空间中机械臂要避开障碍物，对应 C-space 中要避开「C-obstacle」区域。规划就是从 q_start 到 q_goal 找一条不穿过 C-obstacle 的路径。

💡 C-space 的威力：把「机械臂在工作空间避障」这个看似 2D 的问题，转换为「一个点在 C-space 中避障」——大大简化了问题。代价是 C-space 维度等于自由度，高自由度机器人 C-space 是高维空间，规划难度激增。

6.1.3 运动规划的两大流派

下面分别展开。

6.1.4 RRT：快速随机探索树

RRT（Rapidly-exploring Random Tree） 是最经典的采样规划算法。它的核心思想：在 C-space 中随机撒点，逐步扩展一棵树覆盖空间。

def RRT(q_start, q_goal, max_iter):
    tree = [q_start]
    for i in range(max_iter):
        # 1. 随机采样一个构型（10% 概率采样 q_goal，引导探索）
        q_rand = random_sample() or q_goal
        
        # 2. 找树中离 q_rand 最近的节点
        q_near = nearest(tree, q_rand)
        
        # 3. 从 q_near 向 q_rand 扩展一步（受步长限制）
        q_new = steer(q_near, q_rand, step_size)
        
        # 4. 检查 (q_near → q_new) 是否无碰撞
        if not collision(q_near, q_new):
            tree.append(q_new)
            # 5. 若到达 q_goal 邻域，返回路径
            if distance(q_new, q_goal) < epsilon:
                return trace_path(tree, q_new)
    return None  # 未找到
​

RRT 的特点

• 概率完备：只要解存在，迭代次数足够多必能找到。
• 不最优：找到的路径通常很绕，需要后续平滑。
• 高维有效：在 7-30 DoF 空间仍可用，是高维规划的主力。

6.1.5 RRT*：渐近最优的升级版

RRT*（Karaman & Frazzoli 2010）在 RRT 基础上做关键改进：重新连接邻居节点，让路径渐近最优。

RRT* 关键改动:
  新节点 q_new 加入后：
  1. 找 q_new 邻域内所有节点（半径 r 内）
  2. 检查「从哪个邻居到 q_new 路径更短」→ 改 q_new 的父节点
  3. 检查「q_new 到邻居是否比邻居现有路径更短」→ 重连邻居的父节点为 q_new
​

这样树的连接方式不断优化，路径越来越短。理论上 RRT* 渐近最优——迭代时间足够长，路径收敛到最优。

代价是每次扩展成本高于 RRT（要做邻居搜索和重连）。实际工程中，常用 RRT* + 时间限制（如 5 秒）作为折中。

6.1.6 PRM：多查询的概率路图

PRM（Probabilistic Roadmap） 与 RRT 的区别：它先离线构建一张「路图」（roadmap），多次查询复用：

# 离线阶段：构建路图
def PRM_build(num_samples):
    samples = [random_config() for _ in range(num_samples)]
    roadmap = graph()
    for s in samples:
        neighbors = find_neighbors(s, samples, radius)
        for n in neighbors:
            if not collision(s, n):
                roadmap.add_edge(s, n)
    return roadmap

# 在线阶段：查询
def PRM_query(roadmap, q_start, q_goal):
    # 把 q_start, q_goal 连入路图
    connect_to_roadmap(roadmap, q_start)
    connect_to_roadmap(roadmap, q_goal)
    # 在路图上跑 Dijkstra/A*
    return graph_search(roadmap, q_start, q_goal)
​

PRM 适合多次查询同一环境（如工厂固定布局）。RRT 适合单次查询、动态环境。

6.1.7 优化方法：CHOMP 与 TrajOpt

优化方法不从随机采样开始，而是从初始猜测轨迹出发，通过迭代优化使其避开障碍、变光滑。

CHOMP（Covariant Hamiltonian Optimization for Motion Planning）

CHOMP 把轨迹定义为一系列构型 \xi = (q_0, q_1, ..., q_T)，定义目标函数：

• 平滑项惩罚轨迹不光滑（高加加速度）。
• 障碍项 c(q) 是构型 q 接近障碍的代价（距离场表示）。

通过梯度下降迭代优化，轨迹逐渐远离障碍、变光滑。

TrajOpt（Trajectory Optimization）

TrajOpt（Schulman et al. 2014）用序列二次规划（SQP），支持硬约束：

• 关节限制、速度限制、加速度限制。
• 碰撞约束。
• 任务约束（如保持杯子水平）。

TrajOpt 比 CHOMP 更精确处理约束，是工业部署的主流方案之一。

优化方法的优势

• 路径光滑：天然生成平滑路径，无需后处理。
• 支持约束：可处理任务约束（保持姿态、避开禁区）。
• 可加入目标：除了避障，还可优化时间、能量等。

优化方法的劣势

• 需要初始猜测：从一个粗糙路径开始优化。
• 局部最优：可能陷入局部最优，找不到全局最优。
• 数值稳定性：约束满足依赖求解器。

💡 工程实践：常用「RRT* 找粗略路径 → CHOMP/TrajOpt 优化平滑」的两阶段方案。前者保证找得到，后者保证路径质量。

6.1.8 碰撞检测：规划的基础设施

无论采样还是优化，都需要频繁判断「这段路径是否碰撞」。碰撞检测（Collision Detection） 是规划的计算瓶颈。

主流算法：

工业级规划库（OMPL、MoveIt、FCL）都内置高效碰撞检测。

6.1.9 任务约束规划

很多任务不仅要求无碰撞，还有任务约束：

• 保持姿态：端着杯子要保持水平（末端旋转约束）。
• 直线运动：焊接要沿直线（末端位置约束）。
• 避免特定区域：靠近人的区域不能进入。

约束规划扩展了自由度概念：用 Task Space 而非纯 Joint Space 描述约束。代表方法如 TSR（Task Space Region）、AtlasRRT。

约束规划比无约束规划难度高，但工业需求广泛（焊接、喷涂、装配）。

6.1.10 实时规划与重规划

静态规划假设环境不变，真实环境动态变化（人走动、物体被移）。实时规划要求规划在毫秒-秒级完成：

Reactive 控制更进一步：不显式规划，根据当前观测直接反应（如人工势场法）。它快但可能陷入局部最优（如在 C 字形障碍中卡住）。

⚠️ 工程现实：生产级机器人系统常用「全局规划 + 局部 reactive」结合：RRT* 算全局粗路径，局部用速度障碍（RVO/DWA）实时避让人和动态障碍。这是自动驾驶、室内导航的标准架构。

6.1.11 学习型运动规划

最近几年，学习型运动规划兴起：

• 神经网络预测路径：用 CVAE/GAN 学习从 (start, goal, scene) 到路径的映射。
• 强化学习规划：用 RL 在仿真中学习避障策略。
• Diffusion-based 规划：用扩散模型生成多模态路径。
• VLA 直接出动作：第 5 章 VLA 本质绕过了显式规划，直接从图像到动作。

学习型规划的优势是快（推理一次即可），劣势是泛化性、可解释性、安全保证都弱于传统规划。当前趋势是传统规划 + 学习增强——传统保证安全，学习提升效率。

本节小结

• 运动规划在构型空间（C-space）进行，把工作空间避障转化为 C-space 点避障。
• 三大流派：采样（RRT*/PRM）、优化（CHOMP/TrajOpt）、搜索（A*）。
• RRT 概率完备但不最优；RRT* 渐近最优但每步成本高；PRM 适合多查询场景。
• CHOMP 用梯度下降优化轨迹平滑+障碍项；TrajOpt 用 SQP 支持硬约束。
• 两阶段方案「RRT* + CHOMP」是工业实践常用组合。
• 碰撞检测是规划的基础设施，BVH/SDF 是主流加速结构。
• 任务约束规划（保持姿态、直线等）扩展了自由度概念。
• 实时规划用全局+局部 reactive 结合，学习型规划是新兴方向。

下一节《6.2 轨迹优化与平滑》将讨论怎么把粗糙路径变成时间最优、加加速度可控的轨迹。