机器人运动规划

概述

运动规划（Motion Planning）的目标是为机器人找到一条从起始构型到目标构型的无碰撞路径或轨迹。运动规划是连接感知与控制的桥梁，也是实现自主操作的核心能力。

graph LR
    A["感知/任务描述"] --> B["构型空间建模"]
    B --> C["路径规划"]
    C --> D["轨迹优化"]
    D --> E["控制执行"]
    style A fill:#e8f4fd,stroke:#2196F3
    style C fill:#e1f5fe,stroke:#03A9F4
    style D fill:#fff3e0,stroke:#FF9800
    style E fill:#f3e5f5,stroke:#9C27B0

---

1 构型空间（C-space）

1.1 定义

构型空间 \(\mathcal{C}\) 是机器人所有可能构型的集合。对 \(n\) 自由度机器人，构型空间为 \(n\) 维空间。

• 对平面移动机器人：\(\mathcal{C} = \mathbb{R}^2\) 或 \(SE(2)\)
• 对 \(n\)-DOF 机械臂：\(\mathcal{C} = \mathbb{T}^n\)（\(n\) 维环面，每个关节角 \(\in [0, 2\pi)\)）
• 对自由飞行刚体：\(\mathcal{C} = SE(3) = \mathbb{R}^3 \times SO(3)\)

1.2 障碍物映射

工作空间中的障碍物 \(\mathcal{O}_{\text{work}}\) 映射到构型空间中形成 C-space 障碍物 \(\mathcal{C}_{\text{obs}}\)：

\[ \mathcal{C}_{\text{obs}} = \{q \in \mathcal{C} \mid \mathcal{A}(q) \cap \mathcal{O}_{\text{work}} \neq \emptyset\} \]

其中 \(\mathcal{A}(q)\) 是机器人在构型 \(q\) 下占据的空间区域。

自由空间：

\[ \mathcal{C}_{\text{free}} = \mathcal{C} \setminus \mathcal{C}_{\text{obs}} \]

高维 C-space 的挑战

对高自由度机器人，显式计算 \(\mathcal{C}_{\text{obs}}\) 是不可行的。实际中通过碰撞检测（collision checking）来隐式判断。

1.3 碰撞检测

碰撞检测是规划中最耗时的操作，常用方法：

• GJK 算法：凸体间的距离计算
• FCL (Flexible Collision Library)：支持多种几何体
• 包围盒层次结构（BVH）：AABB、OBB，用于加速

---

2 基于采样的规划

2.1 RRT（Rapidly-exploring Random Tree）

RRT 是最经典的基于采样的规划算法（LaValle, 1998）。

核心思想：从起始构型开始，通过随机采样逐步扩展搜索树，直到树到达目标区域。

算法伪代码：

RRT(q_start, q_goal, K):
    T.init(q_start)
    for k = 1 to K:
        q_rand ← RANDOM_CONFIG()
        q_near ← NEAREST(T, q_rand)
        q_new  ← STEER(q_near, q_rand, Δq)
        if COLLISION_FREE(q_near, q_new):
            T.add_vertex(q_new)
            T.add_edge(q_near, q_new)
            if ||q_new - q_goal|| < ε:
                return PATH(T, q_start, q_new)
    return FAILURE

关键操作：

操作	说明
RANDOM_CONFIG()	在 \(\mathcal{C}\) 中均匀随机采样，以概率 \(p_{\text{goal}}\) 直接采样 \(q_{\text{goal}}\)（目标偏置）
NEAREST(T, q)	找树中离 \(q\) 最近的节点
STEER(q_1, q_2, \Delta q)	从 \(q_1\) 向 \(q_2\) 方向前进步长 \(\Delta q\)
COLLISION_FREE	检查路径段是否无碰撞

性质：

• 概率完备：如果解存在，随着采样数 \(K \to \infty\)，找到解的概率趋于 1
• 不保证最优：找到的路径通常不是最短的

2.2 RRT*（渐近最优 RRT）

RRT（Karaman & Frazzoli, 2011）在 RRT 基础上增加了两个操作，使其 渐近最优*：

改进一：Choose Parent（选择最优父节点）

在 \(q_{\text{new}}\) 的邻域 \(r_n\) 内搜索最优父节点：

\[ q_{\text{parent}}^* = \arg\min_{q \in \text{Near}(T, q_{\text{new}}, r_n)} \left[ \text{Cost}(q) + c(q, q_{\text{new}}) \right] \]

其中邻域半径：

\[ r_n = \gamma \left(\frac{\log n}{n}\right)^{1/d} \]

改进二：Rewire（重新连接）

检查邻域内的节点是否可以通过 \(q_{\text{new}}\) 获得更短路径，如果是则更新父节点。

for q_near in Near(T, q_new, r_n):
    if Cost(q_new) + c(q_new, q_near) < Cost(q_near):
        if COLLISION_FREE(q_new, q_near):
            T.rewire(q_near, parent=q_new)

2.3 PRM（Probabilistic Roadmap）

PRM 适用于 多次查询 场景（同一环境中规划多条路径）。

两阶段：

1. 构建阶段（Learning Phase）：在自由空间中采样 \(N\) 个节点，连接邻近节点形成路图
2. 查询阶段（Query Phase）：将起点和终点连接到路图，用 A* 或 Dijkstra 搜索路径

BUILD_ROADMAP(N, k):
    V ← {}
    E ← {}
    for i = 1 to N:
        q ← RANDOM_FREE_CONFIG()
        V.add(q)
        neighbors ← K_NEAREST(V, q, k)
        for q_n in neighbors:
            if COLLISION_FREE(q, q_n):
                E.add(q, q_n)
    return Graph(V, E)

2.4 方法比较

方法	查询模式	最优性	计算开销	适用场景
RRT	单次	否	低	高维、快速探索
RRT*	单次	渐近最优	中	需要优质路径
PRM	多次	渐近最优	预处理高	静态环境多查询
Bi-RRT	单次	否	低	狭窄通道

---

3 基于搜索的规划

3.1 A* 算法

在离散化的 C-space（网格）上搜索最短路径：

\[ f(n) = g(n) + h(n) \]

• \(g(n)\)：从起点到 \(n\) 的实际代价
• \(h(n)\)：启发式估计（需可容许，即 \(h(n) \leq h^*(n)\)）

3.2 Hybrid A*

结合连续状态空间与离散搜索，常用于车辆运动规划。节点扩展时考虑运动学约束（如最小转弯半径）。

---

4 轨迹优化

基于采样的方法生成的路径通常不平滑、不高效。轨迹优化通过连续优化将粗路径精化为高质量轨迹。

4.1 问题形式化

\[ \min_{\xi} \; \mathcal{J}[\xi] = \int_0^T F(\xi(t), \dot{\xi}(t), \ddot{\xi}(t)) \, dt \]

\[ \text{s.t.} \quad \xi(0) = q_{\text{start}}, \quad \xi(T) = q_{\text{goal}}, \quad \xi(t) \in \mathcal{C}_{\text{free}} \; \forall t \]

典型的代价泛函包括：

• 平滑性：\(\int \|\ddot{\xi}\|^2 dt\)（最小化加速度）
• 障碍物代价：\(\int c(\xi(t)) dt\)（距障碍物越近代价越大）
• 时间最优：\(\min T\)

4.2 CHOMP（Covariant Hamiltonian Optimization for Motion Planning）

CHOMP（Ratliff et al., 2009）使用协变梯度下降直接优化轨迹。

代价函数：

\[ \mathcal{U}[\xi] = \underbrace{\frac{1}{2} \int_0^1 \|\dot{\xi}(t)\|^2 dt}_{\text{平滑性代价}} + \lambda \underbrace{\int_0^1 c(\xi(t)) \|\dot{\xi}(t)\| dt}_{\text{障碍物代价}} \]

障碍物代价场 \(c(x)\)：

\[ c(x) = \begin{cases} -d(x) + \frac{1}{2}\epsilon & \text{if } d(x) < 0 \\ \frac{1}{2\epsilon}(d(x) - \epsilon)^2 & \text{if } 0 \leq d(x) \leq \epsilon \\ 0 & \text{if } d(x) > \epsilon \end{cases} \]

其中 \(d(x)\) 是点 \(x\) 到最近障碍物的有符号距离。

协变梯度更新：

\[ \xi \leftarrow \xi - \frac{1}{\eta} A^{-1} \bar{\nabla} \mathcal{U} \]

其中 \(A\) 是平滑性度量矩阵，\(A^{-1}\) 的使用确保更新本身是平滑的。

4.3 TrajOpt（Sequential Convex Optimization）

TrajOpt（Schulman et al., 2014）使用序列凸优化处理非凸约束。

核心思想：将非凸碰撞避免约束线性化，在信赖域内求解凸子问题。

\[ \min_{\xi} \; f_{\text{smooth}}(\xi) \]

\[ \text{s.t.} \quad \hat{c}_j(\xi) \leq 0, \quad j = 1, \ldots, m \quad \text{(线性化碰撞约束)} \]

\[ \|\xi - \xi_k\| \leq \Delta \quad \text{(信赖域约束)} \]

每步求解一个二次规划（QP），逐步逼近可行最优解。

4.4 方法比较

方法	优化变量	碰撞处理	收敛性
CHOMP	路径点序列	代价场梯度	局部最优，需良好初始化
TrajOpt	路径点序列	凸约束	局部最优，收敛快
STOMP	路径点序列	随机采样	无需梯度
iLQR/DDP	控制序列	代价项	局部最优，考虑动力学

---

5 动态环境下的规划

5.1 在线重规划

• D / D Lite：增量搜索算法，环境变化时只更新受影响的部分
• ERRT：扩展 RRT，复用前一次规划的信息

5.2 速度空间方法

• Velocity Obstacles (VO)：在速度空间中计算碰撞锥
• ORCA：最优互惠碰撞避免，适用于多智能体

5.3 反应式规划

• 人工势场法：目标产生引力场，障碍物产生斥力场

\[ F_{\text{total}} = -\nabla U_{\text{att}}(q) - \nabla U_{\text{rep}}(q) \]

局部极小问题

势场法存在局部极小值问题，机器人可能陷入非目标点。可通过随机扰动或结合全局规划器解决。

---

6 与学习的结合

运动规划与强化学习的结合是当前热门方向（详见 强化学习在机器人中的应用）。

6.1 学习启发式

• 使用神经网络预测采样分布，引导 RRT/PRM 在有希望的区域采样
• MPNet（Qureshi et al., 2019）：学习从当前构型到目标的局部规划策略

6.2 扩散模型用于规划

• Diffuser（Janner et al., 2022）：将轨迹生成建模为去噪扩散过程
• Planning as Inference：将规划问题转化为概率推断

6.3 端到端策略

直接学习从观测到动作的映射，跳过显式规划：

• 模仿学习 + 轨迹优化
• 基于视觉的运动规划（ViNT, NoMaD）

---

7 实用考虑

7.1 规划流水线

典型的机器人运动规划流水线：

1. 任务规划：高层决策（先抓取再放置）
2. 运动规划：生成无碰撞路径
3. 轨迹参数化：添加时间信息（梯形速度曲线、S-curve）
4. 平滑与后处理：B-spline 拟合、shortcutting
5. 控制执行：关节空间跟踪

7.2 常用工具

工具	语言	特点
OMPL	C++/Python	开源规划库，含多种采样算法
MoveIt	C++/Python	ROS 集成，工业级运动规划
Drake	C++/Python	含轨迹优化与控制
CuRobo	Python/CUDA	NVIDIA 开发，GPU 加速规划

---

参考文献

1. LaValle, S. M. (2006). Planning Algorithms. Cambridge University Press.
2. Karaman, S. & Frazzoli, E. (2011). Sampling-based algorithms for optimal motion planning. IJRR.
3. Ratliff, N. et al. (2009). CHOMP: Gradient optimization techniques for efficient motion planning. ICRA.
4. Schulman, J. et al. (2014). Motion planning with sequential convex optimization and convex collision checking. IJRR.
5. Lynch, K. M. & Park, F. C. (2017). Modern Robotics. Cambridge University Press.

---

评论