Lec 5 轨迹规划 (Trajectory Planning)

第五章：轨迹规划 (Trajectory Planning)

核心概念

轨迹规划的目标是生成一系列随时间变化的变量，用于描述机器人末端执行器（end-effector）在满足各种约束条件下的位置和姿态。

关键公式预览:

关节空间位置的n阶多项式表示:
$q(t) = a_0 + a_1t + a_2t^2 + \dots + a_nt^n$
关节空间速度与操作空间速度的关系 (通过逆雅可比矩阵):
$\dot{q} = J^{-1}\dot{X}$

路径规划 vs. 轨迹规划

路径规划 (Path Planning)

什么是路径？

路径 (Path) 是指机器臂在关节空间或操作空间中，为了完成指定运动所必须经过的一系列点的集合。

核心任务: 找到一条从起始点（S）到目标点（G）的可行路径，通常需要避开障碍物（O1, O2, O3）。

|310

纯几何概念: 路径规划只关心“去哪里”和“怎么走”，不关心“何时到达”或“走多快”。它不回答机器人关节应该如何运动到目标角度的具体时间问题。

轨迹规划 (Trajectory Planning)

什么是轨迹？

轨迹 (Trajectory) 是在一条给定的路径上，为其上的每一个点都指定了时间信息（例如速度、加速度）。

核心任务: 生成一个关于时间的函数序列，该序列描述了末端执行器在满足一系列约束条件 (Constraints) 下的位置和姿态变化。
输入: 路径 (Path) + 约束 (Constraints)
输出:
- 操作空间 (Operational Space): 描述末端执行器的位置 $p(t)$、速度 $v(t)$ 和加速度 $a(t)$。
- 关节空间 (Joint Space): 描述机器人各关节的角度 $q(t)$、角速度 $\dot{q}(t)$ 和角加速度 $\ddot{q}(t)$。

常见的约束条件包括:

路径约束 (Path)
边界条件 (Boundary)
无碰撞 (No collision)
运动学约束 (Kinematics)
连续性 (Continuity)
平滑性 (Smoothness)
等等 (etc.)

|425

5.1 在关节空间中规划 (Planning in Joint Space)

操作空间规划 vs. 关节空间规划

两种规划方式

机器人末端执行器的轨迹可以在操作空间（也叫笛卡尔空间, Cartesian Space）或关节空间中进行规划。

操作空间规划:
- 优点: 非常直观。因为机器人的任务（比如画一条直线）通常是在操作空间中定义的。
- 实现: 在操作空间规划好轨迹后，通过求解逆运动学 (inverse kinematics) 问题来获得对应的关节角度。
- 缺点: 可能会遇到不连续、奇异点 (singularity) 甚至无解的问题。例如，当机器人手臂完全伸直时，就会出现奇异点。
关节空间规划:
- 优点: 可以直接得到关于时间的关节函数，避免了逆运动学带来的问题。因此，在大多数情况下，规划是在关节空间中完成的。
- 实现: 如果我们已知机器人所有关节的目标角度，就可以直接在关节空间中为每个关节规划轨迹函数。

|500

轨迹的模型 (Models of a Trajectory)

问题定义

对于一个给定的关节，已知其在初始时间 $t_0$ 的角度为 $\theta_i(t_0)$，在最终时间 $t_f$ 的角度为 $\theta_i(t_f)$。能够将关节从初始角度转移到最终角度的、随时间变化的函数 $\theta_i(t)$ 就被称为一条轨迹。

非唯一性: 从起点到终点的轨迹不是唯一的。
常用函数模型: 理论上，任何满足约束条件的函数都可以用来建模轨迹。常见的有：
- 多项式函数 (Polynomial function)
- 谐波函数 (Harmonic function)
- 指数函数 (Exponential function)
- 摆线函数 (Cycloid functions)

函数示例:

多项式: $q(t) = a_0 + a_1t + a_2t^2 + \dots + a_nt^n$
谐波: $q(t) = a_0 + a_1\cos(a_2t) + a_3\sin(a_2t)$
其他: $q(t) = a_0 + a_1t - a_2\sin(a_3t)$

三次多项式轨迹 (Cubic Polynomial Trajectory)

为什么用三次多项式？

三次多项式有4个系数 ($a_0, a_1, a_2, a_3$)，正好可以满足4个边界条件：初始位置、初始速度、最终位置、最终速度。这在机器人运动控制中非常常用。

位置: $q(t) = a_0 + a_1t + a_2t^2 + a_3t^3$
速度: $\dot{q}(t) = a_1 + 2a_2t + 3a_3t^2$ (抛物线速度曲线)
加速度: $\ddot{q}(t) = 2a_2 + 6a_3t$ (线性加速度曲线)

为了求解这4个系数，我们可以建立如下方程组：
$q(t_0) = q_0$
$\dot{q}(t_0) = \dot{q}_0$
$q(t_f) = q_f$
$\dot{q}(t_f) = \dot{q}_f$

其矩阵形式为：

$$
\begin{bmatrix}
1 & t_0 & t_0^2 & t_0^3 \
0 & 1 & 2t_0 & 3t_0^2 \
1 & t_f & t_f^2 & t_f^3 \
0 & 1 & 2t_f & 3t_f^2
\end{bmatrix}
\begin{bmatrix} a_0 \ a_1 \ a_2 \ a_3 \end{bmatrix}
=

约 2760 个字 17 张图片预计阅读时间 9 分钟

\begin{bmatrix} q_0 \ \dot{q}_0 \ q_f \ \dot{q}_f \end{bmatrix}
$$

系数求解

|625

当初始时间 $t_0 = 0$ 时，系数可以简化为：
$a_0 = q_0$
$a_1 = \dot{q}_0$
$a_2 = \frac{3(q_f - q_0) - (2\dot{q}_0 + \dot{q}_f)t_f}{t_f^2}$
$a_3 = \frac{2(q_0 - q_f) + (\dot{q}_0 + \dot{q}_f)t_f}{t_f^3}$

|650

Ex 5-1: "静止-静止"三次轨迹

问题: 假设 $q(0) = 10$, $q(1) = 45$, 并且 $\dot{q}(0) = \dot{q}(1) = 0$。找到这条"静止-静止" (rest-to-rest) 的三次轨迹。

解:
这里 $t_0 = 0, t_f = 1$。
$q_0 = 10, \dot{q}_0 = 0$
$q_f = 45, \dot{q}_f = 0$

代入简化公式计算系数：
$a_0 = q_0 = 10$
$a_1 = \dot{q}_0 = 0$
$a_2 = \frac{3(45 - 10) - (2(0) + 0) \cdot 1}{1^2} = 3 \times 35 = 105$
$a_3 = \frac{2(10 - 45) + (0 + 0) \cdot 1}{1^3} = 2 \times (-35) = -70$

最终轨迹方程:
$q(t) = 10 + 105t^2 - 70t^3$

|346

三次多项式轨迹的缺点

可以用多个三次多项式连接成经过多个点的轨迹。
缺点: 在多段轨迹的连接点处，加速度不连续，会产生无限大的加加速度 (Jerk)。这会导致机器人在物理上产生振动和冲击。
位置: 平滑 (smooth)
速度: 连续 (continuous) 但不平滑
加速度: 不连续 (not continuous)

|450

五次多项式轨迹 (Quintic Polynomial Trajectory)

为什么要用五次多项式？

为了解决加速度不连续的问题，我们需要更高阶的多项式。五次多项式有6个系数，可以满足6个边界条件：初始/最终的位置、速度和加速度。通过将初始和最终的加速度都设为0，我们就能保证轨迹在起点和终点是平滑的。

位置: $q(t) = a_0 + a_1t + a_2t^2 + a_3t^3 + a_4t^4 + a_5t^5$
6个边界条件:
$q(t_0) = q_0, \dot{q}(t_0) = \dot{q}_0, \ddot{q}(t_0) = \ddot{q}_0$
$q(t_f) = q_f, \dot{q}(t_f) = \dot{q}_f, \ddot{q}(t_f) = \ddot{q}_f$

|475

方程解如下，可以作为公式：

|575

Ex 5-2: "静止-静止"且加速度为零的轨迹

问题: 找到一条"静止-静止"且在起点和终点加速度都为零的轨迹。
条件:
$q(0) = 10$ deg
$q(1) = 45$ deg
$\dot{q}(0) = 0, \ddot{q}(0) = 0$
$\dot{q}(1) = 0, \ddot{q}(1) = 0$

解:
通过求解矩阵方程，得到系数：
$a_0 = 10$
$a_1 = 0$
$a_2 = 0$
$a_3 = 350$
$a_4 = -525$
$a_5 = 210$

最终轨迹方程:
$q(t) = 10 + 350t^3 - 525t^4 + 210t^5$

|300

零加加速度轨迹 (Zero Jerk Trajectory)

更平滑的运动

如果我们希望轨迹在起点和终点的加加速度 (Jerk) 也为零，就需要使用七次多项式，它有8个系数，可以满足8个边界条件。

8个边界条件: 初始/最终的位置、速度、加速度、加加速度均为零（或指定值）。
位置: $q(t) = a_0 + a_1t + \dots + a_7t^7$

Example

条件: $q(0) = 10$ deg, $q(1) = 45$ deg, 且在 $t=0$ 和 $t=1$ 时的速度、加速度、加加速度均为0。

最终轨迹方程:
$q(t) = 10 + 1225t^4 - 2940t^5 + 2450t^6 - 700t^7$

|600

抛物线轨迹 (Parabolic Trajectory)

核心思想

抛物线轨迹使用两段二次多项式 (parabola) 来构建整个运动。由于二次多项式的二阶导数是常数，这意味着整个运动过程由两段恒定加速度的阶段组成：先是恒定正加速度，然后是恒定负加速度。这种轨迹的速度曲线呈三角形。

约束条件:
- 加速度大小为已知的常数： $|\ddot{q}(t_0)| = a_c$ (这是一个已知的参数)
- 初始位置： $q_1(0) = q_0$
- 初始速度 (从静止开始)： $\dot{q}_1(0) = 0$

第一部分：加速阶段 ($0 < t \le \frac{1}{2}t_f$)

在运动的前半段，机器人以恒定的正加速度 $a_c$ 运动。
通过对加速度积分，我们可以得到速度和位置的方程：
- 速度: $\dot{q}_1(t) = a_c t$
- 位置: $q_1(t) = \frac{1}{2}a_c t^2 + q_0$

第二部分：减速阶段 ($\frac{1}{2}t_f < t < t_f$)

在运动的后半段，我们使用一个新的二次多项式来描述轨迹，它有3个未知系数 ($a_0, a_1, a_2$)：
- 位置: $q_2(t) = a_0 + a_1t + a_2t^2$

求解第二部分的系数

|500

为了求解这3个未知系数，我们需要3个独立的约束条件：

最终位置: 在 $t_f$ 时刻，轨迹必须到达终点 $q_f$。
$q_2(t_f) = q_f$
最终速度: 在 $t_f$ 时刻，轨迹必须停止，速度为0。
$\dot{q}_2(t_f) = 0$
中间点位置连续: 在中间时刻 $t = \frac{t_f}{2}$，两段轨迹必须平滑连接，即位置相等。
$q_1(\frac{t_f}{2}) = q_2(\frac{t_f}{2}) = \frac{1}{8}a_c t_f^2 + q_0$

这三个条件可以构成一个线性方程组来求解 $[a_0, a_1, a_2]^T$：

\[ \begin{bmatrix} 1 & t_f & t_f^2 \\ 0 & 1 & 2t_f \\ \dots & \dots & \dots \end{bmatrix} \begin{bmatrix} a_0 \\ a_1 \\ a_2 \end{bmatrix} = \begin{bmatrix} q_f \\ 0 \\ \frac{1}{8}a_c t_f^2 + q_0 \end{bmatrix} \]

最终求解得到系数为：
$$
\begin{bmatrix} a_0 \ a_1 \ a_2 \end{bmatrix}
=
\begin{bmatrix}
q_f - t_f(q_0 + \frac{1}{8}a_c t_f^2) \
2q_0 + \frac{1}{4}a_c t_f^2 \
-\frac{1}{t_f}(q_0 + \frac{1}{8}a_c t_f^2)
\end{bmatrix}
$$

抛物线轨迹实例

问题: 已知 $q_0 = 10$ deg, $q_f = 45$ deg, $t_f = 1$ s, 且加速度 $a_c = 200$ deg/s²。求这条抛物线轨迹。

解:
将给定的数值代入上述公式，可以计算出两段轨迹的具体方程。

第一段: $q_1(t) = \frac{1}{2}(200)t^2 + 10 = 100t^2 + 10$
第二段: 通过计算得到 $a_0, a_1, a_2$ 的值，代入 $q_2(t) = a_0 + a_1t + a_2t^2$ 即可。

最终的轨迹、速度、加速度曲线如下图所示。注意加速度是一个阶跃函数（从+200突变到-200），速度是一个三角形，位置由两段抛物线平滑拼接而成。

|375

经过中间点的轨迹 (Point Sequence Trajectory)

Note

如果我们希望轨迹在指定时间经过一系列中间点（via-points），同样可以用高阶多项式实现。

例如: 轨迹需要经过4个点 $q_0, q_1, q_2, q_3$，分别在时间 $t_0, t_1, t_2, t_3$ 到达。同时，我们通常还会施加初始和最终的速度、加速度约束。

这样总共有8个条件：

$q(t_0) = q_0$
$q(t_1) = q_1$
$q(t_2) = q_2$
$q(t_3) = q_3$
$\dot{q}(t_0) = 0$
$\ddot{q}(t_0) = 0$
$\dot{q}(t_3) = 0$
$\ddot{q}(t_3) = 0$

这8个条件可以用一个七次多项式来满足。

|450

5.2 梯形速度轨迹 (Trapezoidal Velocity Trajectory)

另一种常用方法

与使用单一的高阶多项式不同，我们可以用几段低阶多项式（通常是二次）来拼接成一条轨迹。这种方法的优点是速度和加速度有明确的边界，并且计算相对简单。

核心思想: 让关节以最大速度工作，同时满足所有约束，这通常是时间最优 (time-optimal) 的轨迹。

速度曲线呈梯形，包含三个阶段：

加速阶段 ($t_0 \to t_1$): 恒定正加速度，速度线性增加，位置是二次曲线。
匀速阶段 ($t_1 \to t_2$): 零加速度，速度恒定，位置是线性的。
减速阶段 ($t_2 \to t_f$): 恒定负加速度，速度线性减小，位置是二次曲线。

|325

分段方程

段 1 & 3 (加速/减速):
$q(t) = a_0 + a_1t + a_2t^2$
$\dot{q}(t) = a_1 + 2a_2t$
$\ddot{q}(t) = 2a_2$ (恒定加速度)
段 2 (匀速):
$q(t) = b_0 + b_1t$
$\dot{q}(t) = b_1$ (恒定速度)
$\ddot{q}(t) = 0$

|425

规划中的两种情况

根据已知条件，通常分为两种情况进行计算：

情况 1: 已知最大速度 $\dot{q}_m$ 和总时间 $T$。需要求解加速度 $\ddot{q}$ 以及切换时间点 $t_1, t_2$。
情况 2: 已知最大速度 $\dot{q}_m$ 和加速度 $\ddot{q}$。需要求解总时间 $T$ 以及切换时间点 $t_1, t_2$。

Ex 5-4: 梯形轨迹计算

问题: 假设 $q(0)=10, q(10)=60, \dot{q}(0)=2, \dot{q}(10)=4, \dot{q}_m=6$。求对应的梯形轨迹。

解: 这是一个情况1的变种，因为总时间 $T=10$ 和最大速度 $\dot{q}_m=6$ 已知。通过求解一系列边界条件方程，可以得到：

切换时间: $t_1 = 4, t_2 = 8$
加速度: $\ddot{q} = 1$

分段系数:

段1 ($0 \le t < 4$): $a_0 = 10, a_1 = 2, a_2 = 0.5$
段2 ($4 \le t < 8$): $b_0 = 2, b_1 = 6$
段3 ($8 \le t \le 10$): $c_0 = -30, c_1 = 14, c_2 = -0.5$

最终轨迹方程:
$

q(t) =
\begin{cases}
10 + 2t + 0.5t^2 & 0 \le t < 4 \
2 + 6t & 4 \le t < 8 \
-30 + 14t - 0.5t^2 & 8 \le t \le 10
\end{cases}
$

[请在此处插入第 32 页的图片]

Ex 5-5: 时间最优轨迹

问题: 已知边界条件 $\dot{q}_m=8, \ddot{q}=4$，找到一条从 $q(0)=10$ 到 $q(t_f)=60$ 的时间最优 "静止-静止" 轨迹。

解: 这是一个情况2的例子。通过公式计算可得：

切换时间: $t_1 = 4, t_2 = 6.25$
总时间: $T = 8.25$

分段系数:

段1: $a_0 = 10, a_1 = 0, a_2 = 2$
段2: $b_0 = 2, b_1 = 8$
段3: $c_0 = -76.125, c_1 = 33, c_2 = -2$

[请在此处插入第 34 页的图片]

5.3 在操作空间中规划 (Planning in Operational Space)

Important

虽然关节空间规划可以避免奇异点等问题，但缺点是末端执行器的实际运动路径不直观。当我们需要机器人严格遵循某个几何路径（如直线、圆弧）时，就必须在操作空间中规划。

基本流程:

在操作空间中规划末端执行器的轨迹，得到 $X(t)$。
在实时控制中，利用逆运动学或逆雅可比矩阵算法，根据 $X(t)$ 反解出对应的关节变量 $q(t)$。

方法1：使用逆运动学

Ex 5-3: 2R机械臂直线运动

问题: 一个2R平面机械臂，从点 (1, 1.5) 运动到点 (-1, 1.5)，路径为直线 $Y=1.5$。求关节的控制函数。

解:

规划操作空间轨迹:
- Y坐标恒定: $Y(t) = 1.5$
- X坐标使用三次多项式从1变到-1: $X(t) = 1 - 6t^2 + 4t^3$
应用逆运动学:
将 $X(t)$ 和 $Y(t)$ 代入2R机械臂的逆运动学公式，就可以得到关节角 $\theta_1(t)$ 和 $\theta_2(t)$ 随时间变化的函数。

$\theta_2 = \pm 2 \cdot \text{atan2} \sqrt{\frac{(l_1+l_2)^2 - (X^2+Y^2)}{(X^2+Y^2) - (l_1-l_2)^2}}$

$\theta_1 = \text{atan2} \frac{X(l_1+l_2\cos\theta_2) + Yl_2\sin\theta_2}{Y(l_1+l_2\cos\theta_2) - Xl_2\sin\theta_2}$

将 $X(t)$ 和 $Y(t)$ 代入即可。正负号代表了"手肘向上"和"手肘向下"两种构型。

[请在此处插入第 37 和 38 页的图片]

方法2：使用逆雅可比矩阵

速度层面的控制

如果操作空间的速度轨迹 $\dot{X}$ 已知，我们可以用逆雅可比矩阵 $J^{-1}$ 来直接求解关节的速度轨迹 $\dot{q}$。

$\dot{q} = J^{-1}\dot{X}$

得到 $\dot{q}(t)$ 后，通过积分就可以得到关节位置 $q(t)$。这个方法在解析解存在时非常高效。

[请在此处插入第 41 和 42 页的图片和例子]