第7章：运动规划基础

章节大纲

7.1 引言

• 运动规划的定义与目标
• 与路径规划的区别
• 在机器人控制体系中的位置

7.2 配置空间与工作空间

• 7.2.1 配置空间的数学定义
• 7.2.2 工作空间到配置空间的映射
• 7.2.3 障碍物表示与C-障碍
• 7.2.4 自由空间与连通性

7.3 采样基规划算法

• 7.3.1 概率路图法（PRM）
• 7.3.2 快速探索随机树（RRT）
• 7.3.3 RRT*与渐近最优性
• 7.3.4 双向搜索与连接策略

7.4 优化基规划方法

• 7.4.1 轨迹优化问题表述
• 7.4.2 CHOMP算法
• 7.4.3 TrajOpt与序列凸优化
• 7.4.4 梯度下降与牛顿法

7.5 案例研究：机械臂在杂乱环境中的规划

• 7.5.1 问题设定
• 7.5.2 算法选择与实现
• 7.5.3 性能分析与比较

7.6 本章小结

7.7 常见陷阱与错误

7.8 练习题

7.1 引言

运动规划是机器人控制系统的核心组件之一，它负责为机器人生成从初始配置到目标配置的可行运动轨迹。与纯粹的路径规划不同，运动规划不仅考虑几何可行性，还需要满足运动学约束、动力学限制以及任务特定的优化目标。本章将系统介绍运动规划的基础理论、主流算法及其在实际系统中的应用。

运动规划问题的本质是在高维配置空间中寻找连接起点和终点的可行轨迹。这个看似简单的问题在实际中面临诸多挑战：配置空间的维度诅咒、复杂环境中的碰撞检测、实时性要求以及解的质量保证。现代运动规划算法通过巧妙的采样策略、高效的数据结构和优化技术来应对这些挑战。

运动规划的层次架构

在现代机器人系统中，运动规划通常采用分层架构，每层解决不同时空尺度的问题：

1. 任务规划层：定义高层目标序列，如"抓取物体A，放置到位置B"
2. 运动规划层：生成满足约束的轨迹，本章的重点
3. 轨迹跟踪层：将规划轨迹转换为控制指令
4. 底层控制层：执行电机级别的闭环控制

这种分层设计允许各层独立优化，同时保持系统整体的协调性。运动规划层作为连接高层决策和底层执行的桥梁，其性能直接影响整个系统的表现。

运动规划的数学表述

形式化地，运动规划问题可以表述为：

给定：

• 机器人模型 $\mathcal{R}$（运动学、动力学）
• 环境模型 $\mathcal{E}$（障碍物、地形）
• 初始配置 $\mathbf{q}_{start} \in \mathcal{C}$
• 目标配置 $\mathbf{q}_{goal} \in \mathcal{C}$
• 约束集合 $\mathcal{G}$（避碰、关节限位等）
• 成本函数 $J: \Pi \rightarrow \mathbb{R}$

求解：轨迹 $\pi^*: [0, T] \rightarrow \mathcal{C}$ 使得： $$\pi^* = \arg\min_{\pi \in \Pi} J(\pi) \quad \text{s.t.} \quad \pi(0) = \mathbf{q}_{start}, \pi(T) = \mathbf{q}_{goal}, \pi \in \mathcal{G}$$ 其中 $\Pi$ 是所有可行轨迹的集合。这个优化问题的求解复杂度随维度指数增长，因此需要高效的近似算法。

算法分类与选择

运动规划算法可以从多个维度分类：

按完备性：

• 完备算法：保证找到解（如果存在）
• 概率完备算法：以概率1收敛到解
• 分辨率完备算法：在给定分辨率下保证找到解

按最优性：

• 最优算法：保证找到最优解
• 渐近最优算法：收敛到最优解
• 次优算法：找到满意解

按方法论：

• 组合方法：基于图搜索（A*、Dijkstra）
• 采样方法：随机探索（RRT、PRM）
• 优化方法：数值优化（CHOMP、TrajOpt）
• 学习方法：数据驱动（神经网络、强化学习）

选择合适的算法需要考虑：

• 问题维度（2D平面 vs 高维关节空间）
• 环境复杂度（稀疏 vs 密集障碍物）
• 时间约束（离线规划 vs 实时响应）
• 质量要求（可行解 vs 最优解）

7.2 配置空间与工作空间

配置空间是运动规划理论的基石，它将复杂的机器人几何和约束转化为点在抽象空间中的运动问题。这种抽象大大简化了算法设计，使得我们可以用统一的框架处理各种不同的机器人系统。

7.2.1 配置空间的数学定义

配置空间（Configuration Space，简称C-space）是运动规划的基础概念。对于一个具有$n$个自由度的机器人系统，其配置空间$\mathcal{C}$是所有可能配置的集合： $$\mathcal{C} = \{\mathbf{q} \in \mathbb{R}^n | \mathbf{q}_{min} \leq \mathbf{q} \leq \mathbf{q}_{max}\}$$ 其中$\mathbf{q}$表示机器人的配置向量。对于旋转关节，配置空间通常具有流形结构，如$SO(2)$或$SO(3)$。完整的配置空间可以表示为： $$\mathcal{C} = \mathbb{R}^{n_t} \times SO(2)^{n_r2} \times SO(3)^{n_r3}$$ 其中$n_t$、$n_r2$、$n_r3$分别表示平移、2D旋转和3D旋转自由度的数量。

配置空间的拓扑性质

配置空间的拓扑结构对运动规划算法的设计有重要影响：

1. 连通性：配置空间可能包含多个不相连的区域。例如，具有关节限位的机械臂可能无法从某些配置连续运动到其他配置。

2. 周期性：旋转关节引入周期边界条件。角度$\theta$和$\theta + 2\pi$表示相同的物理配置，需要特殊处理： $$d_{SO(2)}(\theta_1, \theta_2) = \min(|\theta_1 - \theta_2|, 2\pi - |\theta_1 - \theta_2|)$$

3. 奇异性：某些配置可能导致雅可比矩阵秩亏，称为运动学奇异点。在这些点附近，小的工作空间运动可能需要极大的关节速度。

4. 维度：配置空间维度等于机器人自由度数。典型例子： - 2D移动机器人：3维（$x, y, \theta$） - 6轴工业机械臂：6维 - 人形机器人：30-50维 - 蛇形机器人：可达100+维

配置空间的度量

定义合适的距离度量对规划算法性能至关重要：

加权欧氏距离： $$d(\mathbf{q}_1, \mathbf{q}_2) = \sqrt{\sum_{i=1}^n w_i(q_{1,i} - q_{2,i})^2}$$ 其中权重$w_i$反映各关节的相对重要性。

位移度量：基于末端执行器的位移 $$d_{disp}(\mathbf{q}_1, \mathbf{q}_2) = |FK(\mathbf{q}_1) - FK(\mathbf{q}_2)|$$ swept volume度量：考虑机器人扫过的体积，更准确但计算昂贵。

7.2.2 工作空间到配置空间的映射

工作空间$\mathcal{W} \subseteq \mathbb{R}^3$是机器人实际运动的物理空间。前向运动学映射$FK: \mathcal{C} \rightarrow \mathcal{W}$将配置映射到工作空间： $$\mathbf{x} = FK(\mathbf{q})$$ 对于串联机械臂，前向运动学通过连续的齐次变换计算： $$\mathbf{T}_{0}^{n} = \prod_{i=1}^{n} \mathbf{T}_{i-1}^{i}(\mathbf{q}_i)$$ 逆运动学$IK: \mathcal{W} \rightarrow \mathcal{C}$通常是非线性的，可能存在多解或无解的情况。

前向运动学的计算

对于串联机械臂，Denavit-Hartenberg (DH) 参数提供了系统化的建模方法： $$\mathbf{T}_{i-1}^{i} = \begin{bmatrix} \cos\theta_i & -\sin\theta_i\cos\alpha_i & \sin\theta_i\sin\alpha_i & a_i\cos\theta_i \\ \sin\theta_i & \cos\theta_i\cos\alpha_i & -\cos\theta_i\sin\alpha_i & a_i\sin\theta_i \\ 0 & \sin\alpha_i & \cos\alpha_i & d_i \\ 0 & 0 & 0 & 1 \end{bmatrix}$$ 其中：

• $\theta_i$：关节角（对于旋转关节是变量）
• $d_i$：连杆偏移（对于移动关节是变量）
• $a_i$：连杆长度（固定参数）
• $\alpha_i$：连杆扭角（固定参数）

逆运动学求解策略

逆运动学问题的求解方法主要分为三类：

1. 解析法：对特定结构（如球形手腕）推导闭式解 - 优点：计算快速，精确 - 缺点：仅适用于特定构型

2. 数值迭代法：使用雅可比迭代 $$\Delta\mathbf{q} = \mathbf{J}^{\dagger}(\mathbf{q})\Delta\mathbf{x}$$ 其中$\mathbf{J}^{\dagger}$是雅可比伪逆： $$\mathbf{J}^{\dagger} = \mathbf{J}^T(\mathbf{J}\mathbf{J}^T + \lambda\mathbf{I})^{-1}$$

3. 优化方法：将IK表述为优化问题 $$\min_{\mathbf{q}} |FK(\mathbf{q}) - \mathbf{x}_{target}|^2 + \alpha|\mathbf{q} - \mathbf{q}_{ref}|^2$$

工作空间分析

可达工作空间：末端执行器能够到达的所有位置 $$\mathcal{W}_{reach} = \{FK(\mathbf{q}) | \mathbf{q} \in \mathcal{C}\}$$ 灵巧工作空间：末端执行器能以任意姿态到达的位置 $$\mathcal{W}_{dext} = \{\mathbf{p} | \forall \mathbf{R} \in SO(3), \exists \mathbf{q} \in \mathcal{C}, FK(\mathbf{q}) = (\mathbf{p}, \mathbf{R})\}$$ 可操作度（Manipulability）衡量机器人在特定配置下的运动能力： $$w(\mathbf{q}) = \sqrt{\det(\mathbf{J}(\mathbf{q})\mathbf{J}^T(\mathbf{q}))}$$ 当$w(\mathbf{q}) = 0$时，机器人处于奇异配置。

7.2.3 障碍物表示与C-障碍

工作空间中的障碍物$\mathcal{O} \subset \mathcal{W}$在配置空间中产生C-障碍（C-obstacle）： $$\mathcal{C}_{obs} = \{\mathbf{q} \in \mathcal{C} | \mathcal{A}(\mathbf{q}) \cap \mathcal{O} \neq \emptyset\}$$ 其中$\mathcal{A}(\mathbf{q})$表示机器人在配置$\mathbf{q}$时占据的工作空间。自由配置空间定义为： $$\mathcal{C}_{free} = \mathcal{C} \setminus \mathcal{C}_{obs}$$

障碍物的几何表示

障碍物可以用多种方式表示，每种方法在存储效率和查询速度间权衡：

1. 多边形/多面体表示： - 精确表示，适合凸体 - 支持精确的碰撞检测 - 存储效率高

2. 体素格网（Voxel Grid）： - 均匀离散化空间 - 查询时间O(1) - 内存消耗大：$O(n^3)$

3. 八叉树（Octree）： - 自适应分辨率 - 平衡存储和精度 - 支持多分辨率查询

4. 符号距离场（SDF）： $$\phi(\mathbf{x}) = \begin{cases} -d(\mathbf{x}, \partial\mathcal{O}) & \mathbf{x} \in \mathcal{O} \\ +d(\mathbf{x}, \partial\mathcal{O}) & \mathbf{x} \notin \mathcal{O} \end{cases}$$

• 提供距离和梯度信息
• 适合优化方法
• 预计算成本高

C-障碍的计算

C-障碍的精确计算是NP-hard问题，实际中使用近似方法：

闵可夫斯基和方法（适用于平移机器人）： $$\mathcal{C}_{obs} = \mathcal{O} \oplus (-\mathcal{R})$$ 其中$\oplus$表示闵可夫斯基和，$\mathcal{R}$是机器人几何。

采样检测方法：

算法：SampleBasedCObstacle

1. 在配置空间密集采样
2. 对每个样本q：
   a. 计算机器人几何A(q)
   b. 检测A(q)与O的碰撞
   c. 标记q为free或obstacle

3. 插值构建连续C-障碍

碰撞检测算法

C-障碍的计算通常通过碰撞检测算法实现，常用的方法包括：

GJK算法：基于闵可夫斯基差的凸体碰撞检测

• 时间复杂度：O(mk)，m和k是顶点数
• 仅适用于凸体
• 可扩展到计算最小距离

分离轴定理（SAT）：

• 原理：两凸体不相交当且仅当存在分离轴
• 检测所有潜在分离轴（边法向量）
• 适合2D和简单3D形状

层次包围体（BVH）：

层次结构：
     AABB (整个机器人)
    /            \
  OBB(上臂)    OBB(下臂)
  /    \        /    \
球体  球体    球体  球体

• 快速剔除不可能碰撞的部分
• 支持复杂几何
• FCL库的核心方法

连续碰撞检测（CCD）： 对于快速运动，检测整个运动过程： $$\text{CCD}(\mathbf{q}_1, \mathbf{q}_2) = \bigcup_{t \in [0,1]} \mathcal{A}((1-t)\mathbf{q}_1 + t\mathbf{q}_2)$$

7.2.4 自由空间与连通性

配置空间的拓扑结构决定了运动规划问题的复杂度。两个配置$\mathbf{q}_{start}$和$\mathbf{q}_{goal}$之间存在可行路径的充要条件是它们位于$\mathcal{C}_{free}$的同一连通分量中。

路径和轨迹的形式化定义

路径的定义为连续映射$\pi: [0,1] \rightarrow \mathcal{C}_{free}$，满足：

• $\pi(0) = \mathbf{q}_{start}$
• $\pi(1) = \mathbf{q}_{goal}$
• $\forall t \in [0,1], \pi(t) \in \mathcal{C}_{free}$

轨迹包含时间信息：$\tau: [0, T] \rightarrow \mathcal{C}_{free}$，还需满足动力学约束。

连通性分析

路径存在性定理： 设$\mathcal{C}_{free}$是配置空间的自由部分，则从$\mathbf{q}_{start}$到$\mathbf{q}_{goal}$存在路径当且仅当： $$\exists \gamma: [0,1] \rightarrow \mathcal{C}_{free}, \gamma(0) = \mathbf{q}_{start}, \gamma(1) = \mathbf{q}_{goal}, \gamma \text{ 连续}$$ 狭窄通道问题： 当$\mathcal{C}_{free}$包含狭窄通道时，随机采样方法效率急剧下降。通道的"狭窄度"可定义为： $$\epsilon = \min_{\mathbf{q} \in \text{passage}} d(\mathbf{q}, \mathcal{C}_{obs})$$

配置空间的分解

为了分析和规划，常将配置空间分解为简单区域：

1. 精确分解：将$\mathcal{C}_{free}$分解为不相交的凸区域
2. 近似分解：使用规则格网或自适应分解
3. 概率路图：构建$\mathcal{C}_{free}$的稀疏表示

配置空间示意图（2D机械臂）：

     C-space                    Workspace
   θ₂ ↑                           y ↑
      |████  ████                   |   ╱▓▓▓╱
      |████  ████                   |  ╱▓▓▓╱  
      |          ← C-obstacle      |╱▓▓▓╱← obstacle
      |██████████                   +---→ x
      +----------→ θ₁              

狭窄通道在C-space中的表现：
      |████╔═╗████|  ← 狭窄通道
      |████║ ║████|     (narrow passage)
      |████╚═╝████|

配置空间的复杂度分析：

• 2D工作空间中的多边形机器人：C-障碍复杂度$O(mn)$
• 3D中的多面体：C-障碍复杂度$O(m^3n^3)$
• 高维空间：精确表示不可行，必须使用近似方法

7.3 采样基规划算法

采样基方法通过在配置空间中随机采样来构建连通图或树结构，避免了显式构建C-障碍的计算复杂度。这类算法具有概率完备性，即当采样数趋于无穷时，如果存在解则一定能找到。

采样基方法的理论基础

采样基规划的核心思想是用离散采样逼近连续空间。根据概率论，如果采样分布覆盖整个自由空间，那么： $$P(\text{找到解}) \rightarrow 1 \quad \text{当} \quad n \rightarrow \infty$$ 这种方法的优势在于：

• 不需要显式计算C-障碍
• 对高维空间有效（维度诅咒影响较小）
• 可以处理复杂约束
• 算法简单，易于实现

关键挑战是如何高效采样和连接样本点。

7.3.1 概率路图法（PRM）

PRM算法分为两个阶段：学习阶段和查询阶段。

学习阶段算法细节

学习阶段：

算法：PRM_Learning(n, k)
输入：采样数n，近邻数k
输出：路图G = (V, E)

1. V ← ∅, E ← ∅
2. while |V| < n do
3.    q_rand ← RandomConfiguration()
4.    if CollisionFree(q_rand) then
5.       V ← V ∪ {q_rand}
6. for each v ∈ V do
7.    N_v ← k-NearestNeighbors(v, V)
8.    for each u ∈ N_v do
9.       if (v,u) ∉ E and LocalPlanner(v, u) then
10.         E ← E ∪ {(v,u)}

查询阶段：

1. 将$\mathbf{q}_{start}$和$\mathbf{q}_{goal}$连接到路图
2. 使用图搜索算法（如A*）寻找最短路径

采样策略

均匀采样： 最简单但在狭窄通道处效率低 $$P(\mathbf{q}) = \frac{1}{|\mathcal{C}|}$$ 高斯采样： 在障碍物附近增加采样密度

1. 采样点$\mathbf{q}_1 \sim \text{Uniform}(\mathcal{C})$
2. 采样$\mathbf{q}_2 \sim \mathcal{N}(\mathbf{q}_1, \sigma^2\mathbf{I})$
3. 如果$\mathbf{q}_1 \in \mathcal{C}_{free} \oplus \mathbf{q}_2 \in \mathcal{C}_{obs}$或相反，保留自由空间中的点

桥采样（Bridge Sampling）： 专门针对狭窄通道

1. 采样两个障碍物内的点$\mathbf{q}_1, \mathbf{q}_2 \in \mathcal{C}_{obs}$
2. 计算中点$\mathbf{q}_{mid} = (\mathbf{q}_1 + \mathbf{q}_2)/2$
3. 如果$\mathbf{q}_{mid} \in \mathcal{C}_{free}$，添加到路图

连接策略

k-近邻连接： 连接k个最近的节点，平衡连通性和效率 $$\text{Neighbors}(\mathbf{q}) = \{\mathbf{q}' \in V : d(\mathbf{q}, \mathbf{q}') \leq d_k(\mathbf{q})\}$$ 半径连接： 连接固定半径内的所有节点 $$\text{Neighbors}(\mathbf{q}) = \{\mathbf{q}' \in V : d(\mathbf{q}, \mathbf{q}') \leq r\}$$ 混合策略： $$k(n) = k_{PRM} \cdot \log n$$ 确保随着节点增加，连接度适当增长。

PRM的渐近最优性

PRM*通过调整连接半径实现渐近最优性： $$r_n = \gamma \left(\frac{\log n}{n}\right)^{1/d}$$ 其中$\gamma$是与问题相关的常数，$d$是配置空间维度。

定理（Karaman & Frazzoli, 2011）： 如果连接半径满足上述条件，则： $$\lim_{n \to \infty} P(\text{cost}(\pi_n^*) = \text{cost}(\pi^*)) = 1$$ 其中$\pi_n^*$是n个采样点时的最优路径，$\pi^*$是真实最优路径。

7.3.2 快速探索随机树（RRT）

RRT通过增量构建空间填充树来探索配置空间：

算法 RRT(q_start, q_goal, K)

1. T.init(q_start)
2. for k = 1 to K do
3.    q_rand ← RandomSample()
4.    q_near ← Nearest(T, q_rand)
5.    q_new ← Steer(q_near, q_rand, δ)
6.    if CollisionFree(q_near, q_new) then
7.       T.addVertex(q_new)
8.       T.addEdge(q_near, q_new)
9.    if Distance(q_new, q_goal) < ε then
10.      return ExtractPath(T)
11. return failure

RRT的关键特性：

• Voronoi偏向：树自然向未探索区域生长
• 概率完备性：当迭代次数趋于无穷时必然找到解
• 快速探索：对高维空间特别有效

7.3.3 RRT*与渐近最优性

RRT*通过引入重连接操作实现渐近最优：

RRT*的重连接步骤：

1. 找到q_new的r-邻域内所有节点
2. 选择使cost(q_start → q_new)最小的父节点
3. 重新连接邻域内的节点，如果通过q_new的路径更短

重连接半径的选择： $$r_{RRT*} = \min\left\{\gamma_{RRT*}\left(\frac{\log n}{n}\right)^{1/d}, \eta\right\}$$ 其中$\gamma_{RRT*} = 2\left(1 + \frac{1}{d}\right)^{1/d}\left(\frac{\mu(\mathcal{C}_{free})}{\zeta_d}\right)^{1/d}$，$\zeta_d$是单位球体积。

7.3.4 双向搜索与连接策略

双向RRT (Bi-RRT) 同时从起点和终点生长两棵树：

双向搜索示意：
    Start Tree              Goal Tree

        *                      *
       /|\                    /|\
      / | \                  / | \

     *  *  *                *  *  *
      \ | /                  \ | /
       \|/     Connect        \|/

        * ←---------------→    *

连接策略包括：

• 贪心连接：每次迭代尝试连接两棵树的最近节点
• 平衡生长：交替扩展较小的树
• 启发式偏向：向对方树的方向偏向采样

7.4 优化基规划方法

优化基方法将运动规划问题表述为连续优化问题，通过迭代改进初始轨迹来获得最优解。这类方法能够自然地处理各种成本函数和约束条件，生成平滑且高质量的轨迹。

7.4.1 轨迹优化问题表述

一般的轨迹优化问题可以表述为： $$\begin{aligned} \min_{\xi} \quad & J(\xi) = \int_0^T L(\mathbf{q}(t), \dot{\mathbf{q}}(t)) dt + \Phi(\mathbf{q}(T)) \\ \text{s.t.} \quad & \mathbf{q}(0) = \mathbf{q}_{start} \\ & \mathbf{q}(T) = \mathbf{q}_{goal} \\ & \mathbf{q}(t) \in \mathcal{C}_{free}, \quad \forall t \in [0, T] \\ & g(\mathbf{q}(t), \dot{\mathbf{q}}(t), \ddot{\mathbf{q}}(t)) \leq 0 \end{aligned}$$ 其中$\xi$表示轨迹参数化，$L$是运行成本，$\Phi$是终端成本，$g$表示动力学和运动学约束。

轨迹的参数化方式：

• 离散路点：$\xi = [\mathbf{q}_0, \mathbf{q}_1, ..., \mathbf{q}_N]$
• 多项式系数：$\mathbf{q}(t) = \sum_{i=0}^{n} \mathbf{a}_i t^i$
• B样条控制点：$\mathbf{q}(t) = \sum_{i} \mathbf{c}_i B_i(t)$

7.4.2 CHOMP算法

协变哈密顿优化运动规划（CHOMP）将轨迹优化表述为函数梯度下降： $$J_{CHOMP} = \lambda \cdot \text{smoothness}(\xi) + \text{obstacle}(\xi)$$ 平滑性成本使用有限差分近似加速度： $$\text{smoothness}(\xi) = \sum_{i=1}^{N-1} |\mathbf{q}_{i+1} - 2\mathbf{q}_i + \mathbf{q}_{i-1}|^2$$ 障碍物成本使用符号距离场： $$\text{obstacle}(\xi) = \sum_{i=0}^{N} \sum_{b \in \text{body}} c(d(\mathbf{x}_b^i))$$ 其中$c(d)$是成本函数，当$d < 0$（碰撞）时急剧增加。

CHOMP的梯度更新： $$\xi^{k+1} = \xi^k - \alpha \mathbf{A}^{-1} \nabla J(\xi^k)$$ 其中$\mathbf{A}$是平滑性成本的Hessian矩阵（三对角矩阵），可以高效求逆。

7.4.3 TrajOpt与序列凸优化

TrajOpt使用序列二次规划（SQP）求解非凸轨迹优化问题。核心思想是在每次迭代中将问题线性化/二次化：

算法 TrajOpt-SQP

1. 初始化轨迹 ξ⁰
2. for k = 0 to max_iter do
3.    构建凸近似：
      - 线性化碰撞约束
      - 二次化目标函数
4.    求解QP子问题：
      min Δξ  ½Δξᵀ∇²J Δξ + ∇Jᵀ Δξ
      s.t.    Aₑₑ Δξ = bₑₑ  (等式约束)
              Aᵢₙₑ Δξ ≤ bᵢₙₑ (不等式约束)

5.    信赖域更新：ξᵏ⁺¹ = ξᵏ + βΔξ
6.    if converged then return ξᵏ⁺¹

碰撞约束的凸化使用支撑超平面： $$d(\mathbf{x}) \geq d(\mathbf{x}_0) + \nabla d(\mathbf{x}_0)^T(\mathbf{x} - \mathbf{x}_0)$$

7.4.4 梯度下降与牛顿法

基础梯度下降： $$\xi^{k+1} = \xi^k - \alpha_k \nabla J(\xi^k)$$ 步长选择策略：

• 固定步长：$\alpha_k = \alpha_0$
• 线搜索：Armijo条件或Wolfe条件
• 自适应步长：Adam、RMSprop等

牛顿法利用二阶信息： $$\xi^{k+1} = \xi^k - \alpha_k [\nabla^2 J(\xi^k)]^{-1} \nabla J(\xi^k)$$ 拟牛顿方法（BFGS、L-BFGS）避免显式计算Hessian： $$\mathbf{B}_{k+1} = \mathbf{B}_k + \frac{\mathbf{y}_k \mathbf{y}_k^T}{\mathbf{y}_k^T \mathbf{s}_k} - \frac{\mathbf{B}_k \mathbf{s}_k \mathbf{s}_k^T \mathbf{B}_k}{\mathbf{s}_k^T \mathbf{B}_k \mathbf{s}_k}$$ 其中$\mathbf{s}_k = \xi^{k+1} - \xi^k$，$\mathbf{y}_k = \nabla J(\xi^{k+1}) - \nabla J(\xi^k)$。

7.5 案例研究：机械臂在杂乱环境中的规划

7.5.1 问题设定

考虑一个7自由度Franka Emika Panda机械臂在装配工作站中的拾取任务。环境包含：

• 工作台（1.2m × 0.8m）
• 多个静态障碍物（工具、零件盒）
• 目标物体位于狭窄空间内

任务要求：

1. 从初始配置移动到预抓取位置
2. 避免与所有障碍物碰撞
3. 最小化执行时间和能量消耗
4. 保证轨迹平滑性（加速度连续）

环境布局（俯视图）：
┌─────────────────────────┐
│  ┌──┐    Target    ┌──┐ │
│  │█ │      ★       │█ │ │
│  └──┘              └──┘ │
│                         │
│  ┌────┐  Robot  ┌────┐ │
│  │Tool│    ▲    │Part│ │
│  └────┘         └────┘ │
└─────────────────────────┘

7.5.2 算法选择与实现

阶段1：全局路径规划（RRT-Connect）

使用双向RRT快速找到初始可行路径：

参数设置：

- 步长 δ = 0.2 rad
- 目标偏向概率 p_goal = 0.1
- 最大迭代 K = 5000
- 碰撞检测分辨率 ε_col = 0.05 rad

RRT-Connect实现要点：

1. 高效最近邻搜索：使用KD-Tree，复杂度O(log n)
2. 延迟碰撞检测：先检测端点，再二分检测路径
3. 关节限位处理：Steer函数中裁剪超限关节

阶段2：路径优化（TrajOpt）

将RRT路径作为初始猜测，使用TrajOpt优化：

目标函数： $$J = w_1 \sum_{i} |\mathbf{q}_{i+1} - \mathbf{q}_i|^2 + w_2 \sum_{i} |\ddot{\mathbf{q}}_i|^2 + w_3 T$$ 约束条件：

• 碰撞避免：$d_{min} \geq 0.02$ m
• 关节限位：$\mathbf{q}_{min} \leq \mathbf{q} \leq \mathbf{q}_{max}$
• 速度限制：$|\dot{\mathbf{q}}| \leq \dot{\mathbf{q}}_{max}$
• 加速度限制：$|\ddot{\mathbf{q}}| \leq \ddot{\mathbf{q}}_{max}$

优化策略：

1. 热启动：使用RRT路径插值生成密集路点
2. 信赖域调整：根据约束违反程度自适应调整
3. 并行碰撞检测：多线程计算各路点的距离梯度

7.5.3 性能分析与比较

实验设置：

• 100个随机场景（不同障碍物配置）
• Intel i7-9700K CPU，16GB RAM
• 比较算法：RRT、RRT*、PRM、CHOMP、TrajOpt

性能指标：

| 算法 | 成功率 | 平均规划时间 | 路径长度 | 平滑性评分 |

*PRM查询时间，不包括路图构建

关键发现：

1. 采样vs优化的权衡： - 采样方法（RRT系列）成功率高，但路径质量较差 - 优化方法（CHOMP、TrajOpt）路径质量好，但易陷入局部极小

2. 混合策略的优势： - RRT+TrajOpt结合了全局搜索和局部优化的优点 - 两阶段方法在成功率和质量间取得良好平衡

3. 计算瓶颈分析： - 碰撞检测占总时间的60-70% - 使用符号距离场可加速3-5倍 - GPU并行化可进一步提升10倍性能

4. 狭窄通道问题： - 纯随机采样在狭窄通道效率低 - 引导采样（如中轴变换）可显著改善

7.6 本章小结

本章系统介绍了运动规划的基础理论和主要算法。核心要点包括：

基础概念：

• 配置空间$\mathcal{C}$是运动规划的基本框架，将复杂的机器人几何简化为点的运动
• C-障碍$\mathcal{C}_{obs}$的计算是规划的关键，通常通过碰撞检测隐式实现
• 自由空间的连通性决定了问题的可解性

采样基方法：

• PRM适合多查询场景，构建可重用的路图
• RRT系列算法快速探索未知空间，具有概率完备性
• RRT*通过重连接实现渐近最优，收敛速度与维度相关：$O(n^{1/d})$

优化基方法：

• 将规划问题表述为非线性优化：$\min J(\xi)$ s.t. 约束
• CHOMP使用函数梯度快速收敛到局部最优
• TrajOpt通过序列凸优化处理复杂约束

关键公式汇总：

1. 配置空间映射：$FK: \mathcal{C} \rightarrow \mathcal{W}$
2. RRT*连接半径：$r = \gamma(\frac{\log n}{n})^{1/d}$
3. 轨迹优化通用形式：$\min_{\xi} J(\xi) = \int_0^T L dt + \Phi(T)$
4. CHOMP更新：$\xi^{k+1} = \xi^k - \alpha \mathbf{A}^{-1}\nabla J$
5. SQP子问题：$\min_{\Delta\xi} \frac{1}{2}\Delta\xi^T\mathbf{H}\Delta\xi + \mathbf{g}^T\Delta\xi$

算法选择指南：

• 高维空间、单查询：RRT/RRT-Connect
• 多查询、固定环境：PRM
• 需要最优性：RRT* + 后处理优化
• 密集障碍物：优化方法（CHOMP/TrajOpt）
• 实时应用：预计算PRM或简化RRT

7.7 常见陷阱与错误

陷阱1：忽视配置空间的拓扑结构

错误：将关节角度简单当作欧氏空间处理 后果：旋转关节在±π处的不连续导致规划失败 正确做法：使用SO(2)流形距离，处理角度回绕

陷阱2：碰撞检测精度不足

错误：使用过大的碰撞检测步长 后果：细长物体可能"穿越"薄障碍物 正确做法：基于物体几何和速度自适应选择检测分辨率

陷阱3：RRT步长选择不当

错误：固定使用较大步长以加快探索 后果：在狭窄通道附近反复碰撞，无法通过 正确做法：自适应步长或使用RRT-Connect的贪心延伸

陷阱4：优化初值选择不当

错误：使用直线插值作为TrajOpt初始轨迹 后果：初值在障碍物内部，优化无法逃离 正确做法：先用采样方法获得可行初值

陷阱5：忽略动力学约束

错误：规划的路径加速度不连续 后果：实际执行时产生振动或超过驱动器能力 正确做法：在优化中加入速度/加速度约束

陷阱6：过度依赖单一算法

错误：期望一个算法解决所有场景 后果：特定场景下性能极差 正确做法：根据问题特征选择或组合算法

调试技巧：

1. 可视化C-空间：对2-3维问题，直接绘制C-障碍
2. 记录采样分布：检查是否均匀覆盖空间
3. 监控优化收敛：绘制成本函数和约束违反度
4. 增量测试：从简单场景逐步增加复杂度
5. 性能剖析：识别计算瓶颈（通常是碰撞检测）

7.8 练习题

基础题

题1：证明对于点机器人在2D平面中的运动规划，多边形障碍物的C-障碍也是多边形。

提示：考虑闵可夫斯基和的性质

题2：给定RRT算法，如果将采样空间从$\mathcal{C}$改为$\mathcal{C}_{free}$（只在自由空间采样），算法是否仍然具有概率完备性？解释原因。

提示：考虑窄通道场景

题3：设计一个简单的启发式函数来改进RRT在已知目标位置时的收敛速度。

提示：考虑目标偏向策略

修改RandomSample()函数：
if random() < p_goal:
    return q_goal
else:
    return UniformSample()

题4：计算3-DOF平面机械臂（链接长度分别为$l_1=1, l_2=0.8, l_3=0.6$）的可达工作空间面积。

提示：考虑最大和最小可达半径

挑战题

题5：分析RRT*算法的渐近最优性。给定$n$个采样点，证明连接半径$r_n$的选择如何影响算法的最优性保证。

提示：使用概率论和图论的结果

题6：设计一个混合算法，结合PRM的多查询优势和RRT的单查询效率。描述算法框架并分析其性能特征。

提示：考虑懒惰评估和增量构建

题7：考虑一个机械臂需要在移动障碍物环境中规划。设计一个时空规划算法，将时间作为额外维度处理。分析算法复杂度并讨论实时性挑战。

提示：构建时空配置空间

Steer_ST(s_near, s_rand):
    Δt = max(0, t_rand - t_near)
    Δq = q_rand - q_near
    v = Δq / Δt
    if |v| > v_max:
        Δt = |Δq| / v_max
    return (q_near + v·δt, t_near + δt)

题8：优化基方法（如CHOMP）容易陷入局部极小。设计一个逃离策略，并证明其能够以一定概率找到更好的解。

提示：考虑随机扰动和模拟退火

1. 运行标准CHOMP直到收敛
2. if 检测到局部极小:
   a. 计算逃离方向 d_escape
   b. 添加随机扰动 ξ' = ξ + αd_escape + βnoise
   c. 以概率p(ΔJ)接受新解

3. 重复直到满足终止条件