二、机器人轨迹规划

机器人轨迹规划是机器人运动控制中的关键环节，旨在为机器人在工作空间中的移动设计一条平滑且有效的路径。无论是工业领域中的机械臂，还是服务型机器人，都需要精准的轨迹规划以确保任务的高效完成。

1. 定义

机器人轨迹规划（Trajectory Planning）是指：

在满足机器人运动学、动力学约束和任务需求的条件下，为机器人从 起点状态 到 目标状态 生成一条 连续、平滑、可执行的轨迹 的过程。

其中：

• 路径（Path）：机器人在工作空间或关节空间中的几何曲线（不含时间信息）。
• 轨迹（Trajectory）：在路径的基础上引入时间参数化，包含位置、速度、加速度随时间的变化。轨迹与何时到达路径中的每个部分有关,强调了时间性。如果机器人抵达N点和P点的时间不同,则相应的轨迹也不同。

$$\text{轨迹} \; q(t) = \{ q_1(t), q_2(t), \dots, q_n(t) \}, \quad t \in [0,T]$$

---

2. 目标

在实际应用中，轨迹规划不仅要确保机器人能够按照设定的路线完成任务，还要考虑运动的平滑性、效率以及对机械系统的保护。例如，在焊接、搬运或装配等任务中，机器人的运动轨迹需要与环境、工作对象相协调，以保证精确性和安全性。同时，轨迹规划还需要结合动力学约束，避免过快或过大的运动对机器人造成损伤，或者导致控制精度下降。

---

3. 分类

3.1 按空间分类

3.1.1 关节空间轨迹（Joint Space Trajectory）

关节空间轨迹规划是指直接在 机器人各关节变量 $q_i(t)$ 上进行插值与时间分配，从而生成关节角度随时间变化的轨迹。

• 轨迹形式：

$$q(t) = \{q_1(t), q_2(t), \dots, q_n(t)\}, \quad t \in [0,T]$$

这意味着规划的对象是每个关节的角度变化，而不是机械臂末端执行器的具体空间位置。具体而言，关节空间轨迹规划只需要考虑各个关节从起始位置到终点位置的位移、速度和加速度变化，而不必关心机械臂末端在运动过程中的实际路径。

在关节空间轨迹规划中，机械臂的末端位姿只在初始和终点处被明确指定。由于中间的运动轨迹无需精确控制，因此整个轨迹规划只需对起点和终点的关节角度进行两次逆运动学解算，分别对应起始位置和目标位置的关节角度。通过这种方式，关节空间轨迹规划可以有效降低计算复杂度，尤其在大规模的多关节机械臂系统中非常有优势。

然而，关节空间轨迹规划也存在一些局限性。由于不关注机械臂末端的实际轨迹，可能会导致在实际运动中，机械臂末端经过不希望经过的区域，甚至可能产生干涉或碰撞的风险。因此，尽管关节空间轨迹规划的计算相对简单快速，但在一些需要精确控制末端执行器路径的任务中，使用这种方法可能存在不确定性。

特点：

• 轨迹直接定义在关节变量 $q(t)$ 上。
• 插值和计算相对简单。
• 优点：简单，易满足关节约束。
• 缺点：末端路径不可控。
• 作为笛卡尔空间轨迹的补充方法。

3.1.2 笛卡尔空间轨迹（Cartesian Space Trajectory）

笛卡尔空间轨迹规划是指在 任务空间/操作空间（Task Space / Cartesian Space） 中进行轨迹设计，即直接规划末端执行器的位置和姿态随时间的变化：

$$X(t) = \{ p(t), \; R(t) \}, \quad t \in [0,T]$$

其中：

• $p(t) = [x(t), y(t), z(t)]^T$ 表示末端位置；
• $R(t)$ 表示末端姿态（可用旋转矩阵、欧拉角或四元数表示）。

一般轨迹先在笛卡尔空间规划，再通过 逆运动学 (Inverse Kinematics, IK) 转换为关节轨迹 $q(t)$。

基于笛卡尔空间的轨迹规划更侧重于控制机械臂末端执行器的运动轨迹。在这种方法中，轨迹规划的对象是末端执行器在任务空间中的位置和姿态变化，即整个运动路径中的每一个点的位移、速度和加速度都需要进行精确的规划。为了实现这一点，机械臂的末端在轨迹中的每一个位置都必须进行逆运动学解算，以将其转化为对应的关节角度，从而驱动机械臂完成轨迹。

在笛卡尔空间轨迹规划中，机械臂末端的运动路径得到高度关注，能够保证末端在空间中的每一个位置都按预期精确移动。这种方法特别适用于需要精确控制末端执行器路径的任务，例如焊接、喷涂或者装配等工业任务中，机器人必须精确地遵循预定的路径工作。

由于每一个轨迹点都要进行逆运动学的实时解算，笛卡尔空间轨迹规划的计算量比关节空间轨迹规划大得多，因此这种方法对计算资源的要求更高。同时，由于需要确保整个运动过程的平滑性和无干涉，规划过程中需要考虑更多的约束条件。这种方法尽管计算复杂，但在对轨迹精度要求较高的任务中具有不可替代的优势。

特点：

• 轨迹定义在末端执行器的位姿 $X(t)$ 上。
• 优点：路径直观，可实现直线/圆弧等特定末端运动。
• 缺点：需解逆运动学，可能出现不可解或奇异性。

3.2 按方法分类

3.2.1解析方法（Analytical / Interpolation）

1.插值法（直线、多项式、样条）。

1）线性插值（Linear Interpolation）

在关节空间内，关节角按照线性规律变化：

$$q_i(t) = q_{i,0} + (q_{i,f} - q_{i,0}) \cdot \frac{t}{T}, \quad t \in [0,T]$$

• 优点：简单、快速；
• 缺点：速度在起点和终点处不连续，加速度突变。

2）三次多项式插值（Cubic Polynomial Interpolation）

形式：

$$q_i(t) = a_0 + a_1 t + a_2 t^2 + a_3 t^3$$

边界条件：

$$q_i(0)=q_{i,0}, \quad q_i(T)=q_{i,f}, \quad \dot{q}_i(0)=v_{i,0}, \quad \dot{q}_i(T)=v_{i,f}$$

可以看出三次多项式只能保证规划的起始速度为0，无法保证起始加速度给定。

解得系数向量：

$$\begin{bmatrix}a_0 \\ a_1 \\ a_2 \\ a_3 \end{bmatrix}= \begin{bmatrix} 1 & 0 & 0 & 0 \\ 1 & T & T^2 & T^3 \\ 0 & 1 & 0 & 0 \\ 0 & 1 & 2T & 3T^2 \end{bmatrix}^{-1} \begin{bmatrix} q_{i,0} \\ q_{i,f} \\ v_{i,0} \\ v_{i,f} \end{bmatrix}$$

• 优点：保证位置和速度连续；
• 缺点：加速度可能不连续。

三次多项式轨迹规划是一种常见且简单的轨迹规划方法，主要用于生成平滑的运动轨迹。它在机器人运动中尤其适用于需要从起点平稳移动到终点的情况。三次多项式轨迹规划通过给定初始位置、终点位置、初始速度和终点速度等条件来确定运动轨迹。其最大的优势是能够确保运动过程中速度变化平滑，避免突然加速或减速带来的冲击。

在实际应用中，三次多项式广泛用于机器人关节空间的轨迹规划。通过合理设置时间和位置的边界条件，三次多项式可以生成自然且符合物理要求的运动轨迹，适用于任务简单、对轨迹平滑性要求较高的场景。由于只需考虑起点和终点的速度、位置等信息，计算相对简单，因此在工业机器人和一些轻量级机械臂的运动控制中，三次多项式是一个非常有效的方法。

3）五次多项式插值（Quintic Polynomial Interpolation）

形式：

$$q_i(t) = a_0 + a_1 t + a_2 t^2 + a_3 t^3 + a_4 t^4 + a_5 t^5$$

边界条件：

$$q_i(0)=q_{i,0}, \quad q_i(T)=q_{i,f}, \quad \dot{q}_i(0)=\dot{q}_i(T)=0, \quad \ddot{q}_i(0)=\ddot{q}_i(T)=0$$

• 优点：保证位置、速度、加速度连续，轨迹平滑；
• 缺点：计算量稍大。

相比三次多项式，五次多项式轨迹规划更适合对运动过程中的加速度和冲击力有更高要求的场景。五次多项式不仅考虑了位置和速度的变化，还引入了对加速度的控制，这使得生成的轨迹在整个运动过程中更加平滑，能够避免由于加速度不连续带来的机械冲击。

五次多项式的特点是能够通过控制初始和终点的加速度来进一步优化轨迹，因此特别适合精密机械臂的运动规划。在机器人关节空间规划中，使用五次多项式可以保证每个关节的加速度变化平稳，避免出现急停或急启动的现象。这种方式通常用于要求更高的机器人任务，例如一些需要高度平滑运动的装配任务或动态作业任务。

4）样条插值（Spline Interpolation）

当轨迹需要经过多个中间点（Waypoints）时，可以使用样条插值。

• 三次样条（Cubic Spline）

  • 各区间为三次多项式；
  • 保证在节点处位置、速度、加速度连续（$C^2$ 连续性）。

• B 样条（B-spline）

  • 基于控制点，轨迹更平滑；
  • 广泛应用于工业机器人。

5)优缺点总结

• 优点：

  • 插值和时间分配简单；
  • 易于满足关节速度、加速度约束；
  • 工业机器人常用。

• 缺点：

  • 末端路径不可控；
  • 在需要特定末端轨迹（如直线、圆弧）时不适用。

特点：适合结构简单的轨迹，执行效率高。

2.时间参数化

即在给定几何轨迹后，合理分配运动时间，满足关节速度/加速度约束。

• 梯形速度规划：

  • 加速 → 匀速 → 减速；
  • 常用公式：

  $$v(t) = \begin{cases} a t, & 0 \leq t < t_a \\ v_{\max}, & t_a \leq t < T-t_d \\ v_{\max} - a (t - (T-t_d)), & T-t_d \leq t \leq T \end{cases}$$

• S 曲线规划：

  • 加速度平滑变化（加加速度受限），速度曲线呈 S 形；
  • 更加平稳，减少机械冲击。

---

3.2.2 笛卡尔姿态空间规划方法

笛卡尔空间姿态规划是直接在机器人末端执行器的运动路径上进行规划。与关节空间规划不同，笛卡尔空间轨迹规划不仅关注机械臂的末端起点和终点，还关心整个轨迹上每个时刻的位置和姿态。这种方法的优势在于可以精确控制机器人末端在整个轨迹上的运动，适用于需要对空间路径精确控制的任务。

通过笛卡尔空间轨迹规划，机器人能够精确跟踪特定的路径，并且可以控制末端执行器的位姿（位置和方向）。这对于一些如焊接、绘图或装配等任务尤为重要。在这些任务中，不仅要求机器人末端到达目标位置，还需要在运动的整个过程中保持特定的姿态，以确保工作的精确性和稳定性。

笛卡尔空间轨迹规划的挑战在于，每一个轨迹点都需要进行逆运动学计算，将末端的空间位置信息转化为机器人关节的角度。这增加了计算的复杂性，但也使得机器人在执行任务时能保持高度的精度和灵活性。

1.笛卡尔直线轨迹

末端执行器从点 $p_0$ 到 $p_f$ 沿直线路径运动：

$$p(t) = p_0 + (p_f - p_0) \cdot s(t), \quad s(t) \in [0,1]$$

其中 $s(t)$ 为归一化时间参数，一般通过 梯形速度 或 S 曲线速度 分配。

2.笛卡尔圆弧轨迹

末端执行器经过三点 $p_0, p_m, p_f$，轨迹为圆弧：

• 确定圆心 $O$ 和半径 $r$；
• 使用角度参数 $\theta(t)$ 插值：

$$p(t) = O + r \cdot \begin{bmatrix} \cos \theta(t) \\ \sin \theta(t) \\ 0 \end{bmatrix}$$

3.姿态插值

末端姿态 $R(t)$ 可采用以下方式插值：

（1）欧拉角插值（简单但可能遇到万向节锁死）；

（2）四元数插值（Slerp, Spherical Linear Interpolation），保证旋转平滑：

$$q(t) = \frac{\sin((1-s)\theta)}{\sin \theta} q_0 + \frac{\sin(s\theta)}{\sin \theta} q_f$$

其中：

• $q_0, q_f$ 为初始和目标四元数；

• $\theta = \arccos(q_0 \cdot q_f)$；

• $s = \frac{t}{T}$。

（3）基于等效轴角（Axis-Angle Representation）的姿态规划

1）姿态表示：轴角法（Axis-Angle Representation）

任意旋转可以用一个单位向量 $ \hat{u} $（旋转轴）和一个旋转角度 $\theta$ 表示：

$$R = e^{\theta \, [\hat{u}]_\times}$$

其中：

• $\hat{u} = [u_x, u_y, u_z]^T$，$\|\hat{u}\| = 1$；
• $[\hat{u}]_\times$ 为反对称矩阵：

$$[\hat{u}]_\times = \begin{bmatrix} 0 & -u_z & u_y \\ u_z & 0 & -u_x \\ -u_y & u_x & 0 \end{bmatrix}$$

• $R \in SO(3)$ 为旋转矩阵。

2）初始与目标姿态差异

已知初始姿态 $R_0$ 和目标姿态 $R_f$，它们的相对旋转为：

$$R_{rel} = R_0^T R_f$$

对 $R_{rel}$ 做轴角分解，得到旋转轴 $\hat{u}$ 和旋转角度 $\theta$：

$$\theta = \arccos \left( \frac{\text{trace}(R_{rel}) - 1}{2} \right)$$

旋转轴 $\hat{u}$ 由 $R_{rel}$ 的反对称部分提取：

$$\hat{u} = \frac{1}{2 \sin \theta} \begin{bmatrix} R_{32} - R_{23} \\ R_{13} - R_{31} \\ R_{21} - R_{12} \end{bmatrix}$$

3）姿态插值方法

为了实现平滑姿态变化，采用归一化时间参数 $s(t) \in [0,1]$，定义插值旋转：

$$R(t) = R_0 \, e^{(s(t)\theta) [\hat{u}]_\times}$$

其中：

• $s(t)$ 可由时间分配函数决定（如线性、梯形速度或 S 曲线）。
• 当 $s(t)=0$ 时，$R(t)=R_0$；当 $s(t)=1$ 时，$R(t)=R_f$。

4）优点

• 姿态变化在几何意义上为 绕固定轴的匀速/加速/减速旋转；
• 避免欧拉角万向节锁死问题；
• 与四元数 Slerp 插值本质等价（数学上等价于球面插值）。

基于旋转矩阵有9个元素，各行各列之间满足正交关系。无法进行插值规划。

4.时间参数化

在几何轨迹 $p(s), R(s)$ 上加入时间 $t$：

• 速度约束：

$$\|\dot{p}(t)\| \leq v_{\max}$$

• 加速度约束：

$$\|\ddot{p}(t)\| \leq a_{\max}$$

• 常用方法：
  • 匀速运动（恒定 $\dot{s}$）；
  • 梯形速度规划；
  • S 曲线速度规划。

5.优缺点总结

• 优点：
  • 末端轨迹可控，适合需要严格路径跟踪的任务。
  • 可直接保证任务空间轨迹的几何特性（直线/圆弧）。
• 缺点：
  • 需要逆运动学求解，可能无解或出现奇异性。
  • 计算复杂度高于关节空间规划。

---

3.2.3 采样/搜索方法（Sampling / Search Based）

这里先简单了解，下一篇详细学习

1.RRT（快速扩展随机树）

（1）基本思想

RRT（Rapidly-exploring Random Tree)是一种 采样式路径规划算法，适用于高维、复杂约束的环境。
其基本思想是：

• 从起点开始，逐步向随机采样点扩展树结构；
• 树快速探索空间，直到找到目标区域。

特点：不需要完整的空间建模，特别适合高维空间和障碍物复杂的情况。扩展效率高，探索性强。

（2）性能分析

• 优点：
  • 适用于高维空间；
  • 不需要显式构造环境网格；
  • 扩展效率高，探索性强。
• 缺点：
  • 生成路径不光滑；
  • 路径可能冗余、过长；
  • 需要后处理（如 B-spline 平滑、时间参数化）。

（3）改进版本

• RRT*：在 RRT 基础上增加重连（rewire）步骤，能渐进逼近最优解。

• RRT-Connect：双向扩展，提高收敛效率。

• Informed-RRT*：在椭圆区域内采样，提高找到最优路径的速度。

• 蓝色细线：RRT 扩展树

• 灰色块：障碍物

• 红色粗线：最终可行路径

• 绿色点：起点

• 红色点：目标

进一步可以用 平滑曲线（如 B 样条） 来优化这条路径

（4）应用场景

• 移动机器人避障导航

• 机械臂高维空间运动规划

• 无人机路径规划

• 自主驾驶路径搜索

2.PRM（概率路网）

PRM 是一种基于 采样 的路径规划方法，适合静态、复杂环境。

主要分为 两阶段：

(1)学习阶段（Learning Phase / Roadmap Construction）

• 在自由空间 $\mathcal{C}_{free}$ 中随机采样一系列点 $q_i$；
• 将这些点通过局部规划器（Local Planner，例如直线连接）连接成无碰撞的边，构成一张 无向图（roadmap）。

(2)查询阶段（Query Phase）

• 给定起点 $q_{start}$ 和目标 $q_{goal}$；
• 将它们与 roadmap 中的最近节点相连；
• 在 roadmap 上搜索最短路径（如 Dijkstra 或 A*）。

(5) 性能特点

优点：

• 学习阶段与查询阶段分离 → 一次建图，多次查询；

• 适用于静态环境；

• 适合多次路径查询任务（如工厂车间重复任务）。

缺点：

• 路径质量依赖于采样密度；

• 在高维空间效率下降；

• 构图需要全局连通性，计算量可能大。

(6)改进版本

• Lazy-PRM：边不提前检测，搜索时才做碰撞检测，提高效率。

• PRM*：渐进逼近最优路径，类似 RRT*。

• SPARS：稀疏采样，提高图的稀疏性和路径质量。

(7)应用场景

• 静态环境下的多次路径查询（工厂机械臂、AGV 导航）；

• 仓储机器人反复搬运任务；

• 虚拟环境角色导航。

特点： 适合高维复杂空间，能处理障碍物。

---

4. 约束条件

机器人轨迹规划需满足：

1. 运动学约束

• 最大关节速度

$$|\dot{q}_i(t)| \leq v_{i,\max}$$

• 最大关节加速度

$$|\ddot{q}_i(t)| \leq a_{i,\max}$$

2. 动力学约束

• 关节力矩

$$|\tau_i(t)| \leq \tau_{i,\max}$$

3. 任务约束
   • 避障要求
   • 工艺约束（如焊接、打磨需保持姿态）
   • 平滑性约束

---

5. 与路径规划的区别

• 路径规划（Path Planning）：回答 “走哪条路？” → 给出几何路径，未涉及时间。
• 轨迹规划（Trajectory Planning）：回答 “怎么走？” → 给出随时间的运动规律。

📌 简而言之：

• 路径 = 空间上的线
• 轨迹 = 带时间的运动

---

6. 小结

机器人轨迹规划是机器人运动控制中的关键环节，连接了 高层任务规划 与 底层控制执行。

• 路径 → 几何形状
• 轨迹 → 时间化运动
• 目标 → 平滑、安全、可控、满足约束

7.各类轨迹/路径规划方法对比分析

方法	主要思想	优点	缺点	典型应用场景
关节空间轨迹规划	在关节空间 $q(t)$ 插值（直线、多项式、样条）	计算简单，稳定可靠，不依赖逆解	末端路径不可控（末端轨迹可能弯曲）	机械臂点到点运动
笛卡尔空间轨迹规划	在任务空间 $p(t), R(t)$ 插值（直线、圆弧、Slerp）	末端轨迹直观可控，适合严格工艺轨迹	需要逆运动学，可能无解或奇异	焊接、喷涂、装配、医疗机器人
欧拉角轨迹规划	使用欧拉角 $(\phi, \theta, \psi)$ 描述姿态，并逐分量插值	数学形式直观，便于人工理解	存在万向节锁死，插值可能不平滑	低维任务空间的简单姿态变化
轴角姿态插值	使用旋转轴 $\hat{u}$ 和角度 $\theta$ 进行平滑插值	姿态变化自然，几何意义直观，避免欧拉角奇异	需要矩阵对数/指数运算	末端姿态规划、无人机/航天器姿态控制
RRT	树结构随机扩展，快速探索空间	适合高维、复杂环境；无需建模	路径不光滑，需后处理；单次查询为主	移动机器人、无人机、自动驾驶路径搜索
PRM	采样构建无向图（roadmap），再做图搜索	学习阶段与查询阶段分离；可多次查询	高维性能差；需大量采样保证连通性	多次重复任务（工厂机械臂、仓储 AGV）

---

总结

• 运动学轨迹规划：

  • 关节空间：简单可靠，但末端路径不可控；
  • 笛卡尔空间：路径直观，但依赖逆运动学；
  • 欧拉角：易于理解，但有奇异性；
  • 轴角：几何直观，无奇异，常与四元数等价使用。

• 采样式路径规划：

  • RRT：适合单次路径搜索，快速但粗糙；
  • PRM：适合多次查询任务，需预处理建图。

---

Q&A

1.为什么在关节空间直线运动和在笛卡尔空间直线运动会产生不同的轨迹？

• 关节空间直线运动

  • 每个关节角度 $q_i(t)$ 按照直线/多项式插值变化。
  • 优点：实现简单，计算量小，不依赖逆运动学。
  • 缺点：末端执行器的路径在笛卡尔空间往往是弯曲的，不是直线。

• 笛卡尔空间直线运动

  • 直接规定末端执行器的位姿 $(p(t), R(t))$ 在笛卡尔空间中沿直线/圆弧插值。
  • 优点：末端路径直观可控，满足工艺要求（焊接、打磨、喷涂）。
  • 缺点：需要不断解逆运动学，有时无解或在奇异点处难以实现。

• 根本原因：
  机械臂运动学是非线性映射：

  $$x = f(q), \quad q = (q_1, q_2, ..., q_n)$$

  其中 $x$ 是末端位置。即使 $q(t)$ 是直线插值，经过 $f(q)$ 映射后的 $x(t)$ 也未必是直线；
  反过来在 $x$ 空间直线插值，映射回关节空间 $q(t)$ 就是非线性的。

---

2.轨迹规划时，为什么要限制速度和加速度？

• 安全性：过高的速度或加速度可能导致机械冲击，损坏设备或工件。

• 可实现性：实际电机/驱动器有最大转速和最大转矩（对应最大加速度）。

• 平滑性：如果轨迹不约束速度/加速度，关节可能出现剧烈抖动。

• 控制稳定性：平滑的速度/加速度曲线（如梯形速度、S曲线）有利于控制器跟踪，减少超调。

简言之：速度/加速度约束是为了安全、可控和可实现。

---

3.如果规定两秒钟必须完成一次动作，应该如何安排轨迹？

这是一个 时间参数化问题 (time-parameterization)。

方法一：均匀分布（简单直线插值）

• 总时间 $T = 2s$ 均分，每个采样点均匀分布。
• 缺点：起点和终点的速度会突变，不平滑。

方法二：梯形速度规划（最常用）

• 分为 加速段 - 匀速段 - 减速段

• 轨迹函数示例：

  $$s(t) = \begin{cases} \frac{1}{2} a t^2 & 0 \leq t < t_a \\ v_{\max}(t - \tfrac{t_a}{2}) & t_a \leq t < T - t_a \\ S - \frac{1}{2} a (T-t)^2 & T-t_a \leq t \leq T \end{cases}$$

  其中 $a$ 是加速度，$S$ 是总位移。

方法三：三次多项式插值（Cubic Polynomial）

• 设定边界条件（初始/末端位置、速度）

• 解三次多项式：

  $$q(t) = a_0 + a_1 t + a_2 t^2 + a_3 t^3$$

• 能保证速度在起点、终点为零，轨迹平滑。

---

参考

1. 机器人基础-机器人轨迹规划 ↩
2. 机器人学导论（原书第3版） ↩