运动规划是机器人控制系统的核心组件之一，它负责为机器人生成从初始配置到目标配置的可行运动轨迹。与纯粹的路径规划不同，运动规划不仅考虑几何可行性，还需要满足运动学约束、动力学限制以及任务特定的优化目标。本章将系统介绍运动规划的基础理论、主流算法及其在实际系统中的应用。
运动规划问题的本质是在高维配置空间中寻找连接起点和终点的可行轨迹。这个看似简单的问题在实际中面临诸多挑战：配置空间的维度诅咒、复杂环境中的碰撞检测、实时性要求以及解的质量保证。现代运动规划算法通过巧妙的采样策略、高效的数据结构和优化技术来应对这些挑战。
在现代机器人系统中，运动规划通常采用分层架构，每层解决不同时空尺度的问题：
这种分层设计允许各层独立优化，同时保持系统整体的协调性。运动规划层作为连接高层决策和底层执行的桥梁，其性能直接影响整个系统的表现。
形式化地，运动规划问题可以表述为：
给定：
求解：轨迹 $\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$ 是所有可行轨迹的集合。这个优化问题的求解复杂度随维度指数增长，因此需要高效的近似算法。
运动规划算法可以从多个维度分类：
按完备性：
按最优性：
按方法论：
选择合适的算法需要考虑：
配置空间是运动规划理论的基石，它将复杂的机器人几何和约束转化为点在抽象空间中的运动问题。这种抽象大大简化了算法设计，使得我们可以用统一的框架处理各种不同的机器人系统。
配置空间（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旋转自由度的数量。
配置空间的拓扑结构对运动规划算法的设计有重要影响：
连通性：配置空间可能包含多个不相连的区域。例如，具有关节限位的机械臂可能无法从某些配置连续运动到其他配置。
周期性：旋转关节引入周期边界条件。角度$\theta$和$\theta + 2\pi$表示相同的物理配置，需要特殊处理： \(d_{SO(2)}(\theta_1, \theta_2) = \min(|\theta_1 - \theta_2|, 2\pi - |\theta_1 - \theta_2|)\)
奇异性：某些配置可能导致雅可比矩阵秩亏，称为运动学奇异点。在这些点附近，小的工作空间运动可能需要极大的关节速度。
维度：配置空间维度等于机器人自由度数。典型例子：
定义合适的距离度量对规划算法性能至关重要：
加权欧氏距离： \(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度量：考虑机器人扫过的体积，更准确但计算昂贵。
工作空间$\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}\]其中：
逆运动学问题的求解方法主要分为三类：
数值迭代法：使用雅可比迭代 \(\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}\)
可达工作空间：末端执行器能够到达的所有位置 \(\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$时，机器人处于奇异配置。
工作空间中的障碍物$\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}\]障碍物可以用多种方式表示，每种方法在存储效率和查询速度间权衡：
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算法：基于闵可夫斯基差的凸体碰撞检测
分离轴定理（SAT）：
层次包围体（BVH）：
层次结构：
AABB (整个机器人)
/ \
OBB(上臂) OBB(下臂)
/ \ / \
球体 球体 球体 球体
连续碰撞检测（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)\)
配置空间的拓扑结构决定了运动规划问题的复杂度。两个配置$\mathbf{q}{start}$和$\mathbf{q}{goal}$之间存在可行路径的充要条件是它们位于$\mathcal{C}_{free}$的同一连通分量中。
路径的定义为连续映射$\pi: [0,1] \rightarrow \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})\)
为了分析和规划，常将配置空间分解为简单区域：
配置空间示意图（2D机械臂）：
C-space Workspace
θ₂ ↑ y ↑
|████ ████ | ╱▓▓▓╱
|████ ████ | ╱▓▓▓╱
| ← C-obstacle |╱▓▓▓╱← obstacle
|██████████ +---→ x
+----------→ θ₁
狭窄通道在C-space中的表现：
|████╔═╗████| ← 狭窄通道
|████║ ║████| (narrow passage)
|████╚═╝████|
配置空间的复杂度分析：
采样基方法通过在配置空间中随机采样来构建连通图或树结构，避免了显式构建C-障碍的计算复杂度。这类算法具有概率完备性，即当采样数趋于无穷时，如果存在解则一定能找到。
采样基规划的核心思想是用离散采样逼近连续空间。根据概率论，如果采样分布覆盖整个自由空间，那么：
\[P(\text{找到解}) \rightarrow 1 \quad \text{当} \quad n \rightarrow \infty\]这种方法的优势在于：
关键挑战是如何高效采样和连接样本点。
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)}
查询阶段：
均匀采样： 最简单但在狭窄通道处效率低 \(P(\mathbf{q}) = \frac{1}{|\mathcal{C}|}\)
高斯采样： 在障碍物附近增加采样密度
桥采样（Bridge Sampling）： 专门针对狭窄通道
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*通过调整连接半径实现渐近最优性：
\[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^$是真实最优路径。
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的关键特性：
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$是单位球体积。
双向RRT (Bi-RRT) 同时从起点和终点生长两棵树：
双向搜索示意：
Start Tree Goal Tree
* *
/|\ /|\
/ | \ / | \
* * * * * *
\ | / \ | /
\|/ Connect \|/
* ←---------------→ *
连接策略包括：
优化基方法将运动规划问题表述为连续优化问题，通过迭代改进初始轨迹来获得最优解。这类方法能够自然地处理各种成本函数和约束条件，生成平滑且高质量的轨迹。
一般的轨迹优化问题可以表述为：
\[\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$表示动力学和运动学约束。
轨迹的参数化方式：
协变哈密顿优化运动规划（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矩阵（三对角矩阵），可以高效求逆。
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)\]基础梯度下降：
\[\xi^{k+1} = \xi^k - \alpha_k \nabla J(\xi^k)\]步长选择策略：
牛顿法利用二阶信息：
\[\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自由度Franka Emika Panda机械臂在装配工作站中的拾取任务。环境包含：
任务要求：
环境布局（俯视图）：
┌─────────────────────────┐
│ ┌──┐ Target ┌──┐ │
│ │█ │ ★ │█ │ │
│ └──┘ └──┘ │
│ │
│ ┌────┐ Robot ┌────┐ │
│ │Tool│ ▲ │Part│ │
│ └────┘ └────┘ │
└─────────────────────────┘
阶段1：全局路径规划（RRT-Connect）
使用双向RRT快速找到初始可行路径：
参数设置：
- 步长 δ = 0.2 rad
- 目标偏向概率 p_goal = 0.1
- 最大迭代 K = 5000
- 碰撞检测分辨率 ε_col = 0.05 rad
RRT-Connect实现要点：
阶段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\)
约束条件：
| 速度限制：$ | \dot{\mathbf{q}} | \leq \dot{\mathbf{q}}_{max}$ |
| 加速度限制：$ | \ddot{\mathbf{q}} | \leq \ddot{\mathbf{q}}_{max}$ |
优化策略：
实验设置：
性能指标：
| 算法 | 成功率 | 平均规划时间 | 路径长度 | 平滑性评分 |
|---|---|---|---|---|
| RRT | 92% | 0.8s | 12.4 rad | 2.3 |
| RRT* | 94% | 5.2s | 9.1 rad | 2.5 |
| PRM | 88% | 0.3s* | 10.2 rad | 2.1 |
| CHOMP | 76% | 1.2s | 8.3 rad | 4.8 |
| TrajOpt | 83% | 0.9s | 8.7 rad | 4.6 |
| RRT+TrajOpt | 95% | 1.7s | 8.5 rad | 4.7 |
*PRM查询时间，不包括路图构建
关键发现：
本章系统介绍了运动规划的基础理论和主要算法。核心要点包括：
基础概念：
采样基方法：
优化基方法：
关键公式汇总：
算法选择指南：
错误：将关节角度简单当作欧氏空间处理 后果：旋转关节在±π处的不连续导致规划失败 正确做法：使用SO(2)流形距离，处理角度回绕
错误：使用过大的碰撞检测步长 后果：细长物体可能”穿越”薄障碍物 正确做法：基于物体几何和速度自适应选择检测分辨率
错误：固定使用较大步长以加快探索 后果：在狭窄通道附近反复碰撞，无法通过 正确做法：自适应步长或使用RRT-Connect的贪心延伸
错误：使用直线插值作为TrajOpt初始轨迹 后果：初值在障碍物内部，优化无法逃离 正确做法：先用采样方法获得可行初值
错误：规划的路径加速度不连续 后果：实际执行时产生振动或超过驱动器能力 正确做法：在优化中加入速度/加速度约束
错误：期望一个算法解决所有场景 后果：特定场景下性能极差 正确做法：根据问题特征选择或组合算法
题1：证明对于点机器人在2D平面中的运动规划，多边形障碍物的C-障碍也是多边形。
提示：考虑闵可夫斯基和的性质
题2：给定RRT算法，如果将采样空间从$\mathcal{C}$改为$\mathcal{C}_{free}$（只在自由空间采样），算法是否仍然具有概率完备性？解释原因。
提示：考虑窄通道场景
题3：设计一个简单的启发式函数来改进RRT在已知目标位置时的收敛速度。
提示：考虑目标偏向策略
题4：计算3-DOF平面机械臂（链接长度分别为$l_1=1, l_2=0.8, l_3=0.6$）的可达工作空间面积。
提示：考虑最大和最小可达半径
题5：分析RRT*算法的渐近最优性。给定$n$个采样点，证明连接半径$r_n$的选择如何影响算法的最优性保证。
提示：使用概率论和图论的结果
题6：设计一个混合算法，结合PRM的多查询优势和RRT的单查询效率。描述算法框架并分析其性能特征。
提示：考虑懒惰评估和增量构建
题7：考虑一个机械臂需要在移动障碍物环境中规划。设计一个时空规划算法，将时间作为额外维度处理。分析算法复杂度并讨论实时性挑战。
提示：构建时空配置空间
题8：优化基方法（如CHOMP）容易陷入局部极小。设计一个逃离策略，并证明其能够以一定概率找到更好的解。
提示：考虑随机扰动和模拟退火