4.2.1 点对点运动

点对点（point-to-point）是约束最少的一类问题：只规定起点、终点和总时间，中途怎么走不管。约束少 ⇒ 解无穷多 ⇒ 必须补一个性能指标，教材补的正是最小能量——它恰好把答案锁死成三次多项式。工业上另有更省算力的做法：梯形速度曲线。本节完整推导的就是这两种方案，以及它们之间的折中。录播 0:39:00–1:10:50。

4.2.1.0 从「无穷多解」到「唯一解」：为什么要三次多项式

问题的提法（0:41:04、教材 §4.2.1）：机械手要在规定的 $t_f$ 时间内，把关节从初始位形 $q_i$ 移到终点位形 $q_f$ ；不关心实际的末端执行器路径，只要求在满足若干一般性要求的前提下，最优化某个性能指标。

为什么可以只研究单关节。 $q$ 其实是 $n$ 个关节的向量 $(q_1,\dots,q_n)$ ；但对同一台机械手，「哪个关节它在用的过程中，时间都是一样的」——所有关节共享同一时间轴，规划方法也完全相同，因此单关节 $q(t)$ 的规划完全可以推广到其他关节（0:40:27）。以下不失一般性，只写单关节。

无穷多解与优化指标。 只知道 $q_i$ 、 $q_f$ 和 $t_f$ ，「初始到终止怎么过去的，它是有很多方式的……它是有无数多个解法」（0:42:03）；只有再补一个优化目标，解才唯一。教材选的指标是电机能量消耗最小：

定理 4.2.A · 最小能量解是三阶多项式（教材 §4.2.1）

考虑一个绕定轴转动的刚体，转动惯量为 $I$ ，转动由电机力矩 $\tau$ 提供。令 $\dot q=\omega$ ，求解微分方程

$I\dot\omega=\tau$

并服从条件

$\int_0^{t_f}\omega(t)\,\mathrm dt=q_f-q_i ,$

使性能指标

$\int_0^{t_f}\tau^2(t)\,\mathrm dt$

最小化。其解的形式为

$\omega(t)=at^2+bt+c .$

即速度是二阶多项式 ⇒ 位置是三阶（三次）多项式、加速度是线性函数。

这条结论的适用范围比推导本身更宽（0:45:00）：上面的推导把问题简化成了定轴转动，也不计摩擦。即使关节动力学不能这样简单地描述（事实上，绕关节轴的转动惯量本身就是机械手位形的函数），选择三阶多项式来生成关节轨迹仍然是该问题的有效解——只是系数会不同。

老师还补了一句方法论上的说明（0:44:23）：这个问题本身是个泛函问题（变分法 / 现代控制理论里的同类内容），所以「具体的求解过程我们就不介绍了，直接给结论」。

4.2.1.1 三次多项式：4 个约束定 4 个系数

定义 4.2.1 · 三次多项式时间律

为确定一个关节运动，取三次多项式

$q(t)=a_3t^3+a_2t^2+a_1t+a_0 \tag{4.1}$

其导数为抛物线形式的速度曲线与线性形式的加速度曲线：

$\dot q(t)=3a_3t^2+2a_2t+a_1,\qquad \ddot q(t)=6a_3t+2a_2 .$

4 个系数需要 4 个约束。除初始与终点位置 $q_i$ 、 $q_f$ 外，通常再利用初始与终点速度 $\dot q_i$ 、 $\dot q_f$ （一般设为 0）——「因为一般的初始肯定是从零开始加速，终止完成任务一般也是回到零的」（0:46:30）。解出下面这个方程组，4 个系数就都定了：

$a_0=q_i,\qquad a_1=\dot q_i,\qquad a_3t_f^3+a_2t_f^2+a_1t_f+a_0=q_f,\qquad 3a_3t_f^2+2a_2t_f+a_1=\dot q_f .$

老师特别强调这类问题的通用解法（0:16:25）：整个过程都是「解方程 / 待定系数法」——根据任务找合适的约束（约束就是方程），再把多项式的系数解出来。方程多了就超定（想办法提高阶次或改解析式形式）；方程少了就多解（补上优化约束或工程约束，让未知数个数与方程个数相同）。

例 4.2.B（原书图 4.1）· 0 → π 的三次轨迹

取 $q_i=0$ ， $q_f=\pi$ ， $t_f=1$ ， $\dot q_i=\dot q_f=0$ 。代入上式得

$a_0=0,\quad a_1=0,\quad a_3+a_2=\pi,\quad 3a_3+2a_2=0 \;\Longrightarrow\; a_2=3\pi,\ a_3=-2\pi,$

即 $q(t)=-2\pi t^3+3\pi t^2$ ， $\dot q(t)=-6\pi t^2+6\pi t$ ， $\ddot q(t)=-12\pi t+6\pi$ 。

读图要点（老师 0:47:28 逐条点出）：位置平滑且连续；速度连续但不平滑—— $t=0$ 与 $t=1$ 两处速度曲线各有一个折点；加速度在这两点上跳跃（从 0 跳到 $6\pi$ ，再从 $-6\pi$ 跳回 0）。这正是「位置/速度光滑、加速度有跳跃」那一档的典型代表。

课堂幻灯片 · 00:54:18：三次多项式时间律——位置（三次）、速度（抛物线）、加速度（线性，两端跳跃）

4.2.1.2 五次多项式：把加速度也管住

定义 4.2.2 · 五次多项式时间律

如果还要同时指定初始与最终的加速度，约束就增加到 6 个，至少需要五阶多项式：

$q(t)=a_5t^5+a_4t^4+a_3t^3+a_2t^2+a_1t+a_0 \tag{4.2}$

与前一情形一样，系数由 $t=0$ 与 $t=t_f$ 处对 $q(t)$ 及其前两阶导数施加的条件解出：位置、速度、加速度的初终值一共给出 6 个方程，正好解 6 个系数。

五次多项式的代价（0:53:45）：三次多项式最多只能列 4 个约束方程，一旦要指定初终加速度就不够用了，必须升到五次；但选式 (4.2) 就等于放弃对性能指标的最小化——「通过理论推导，如果想要能量消耗最小的话，它只能是三阶的一个多项式，所以采用五阶多项式肯定不是最小的」（0:54:42）。何况阶次越高，计算量越大，系数求解的数值精度也越低。

所以工业上一般不直接采用五次，而是用混合多项式（blended polynomial）的时间律——「允许直接验证机械手是否在物理上支持所得的速度和加速度」（教材 §4.2.1）。下一小节要讲的梯形速度曲线（LSPB），正是这类时间律中最常用的代表。

4.2.1.3 梯形速度曲线（LSPB）：工业上最常用的折中

定义 4.2.3 · 梯形速度曲线（LSPB，抛物线混合）

指定一条梯形速度曲线：起始段为常值加速度，中段维持巡航速度，末段为常值减速度。对速度积分得到位置轨迹：它由一条直线段和在其两端分别连接到初始位置与最终位置的两段抛物线组成，因此又称抛物线混合（linear segments with parabolic blends）。

课件 0:56:47 把它类比电梯拖动：「先匀加速、再巡航、再匀减速，正好到零停下来」。

两条标准假设（教材 §4.2.1）：

初始速度与最终速度都为零；
两个常加速度分段的时间长度相同（因此两段加速度等量），轨迹在 $t_m=t_f/2$ 时刻关于平均值点 $q_m=(q_f+q_i)/2$ 对称。

老师解释了「为什么取相等」（0:59:48）：两段设成相等，可以减少需要解的参数与方程。实际过程中这种计算可能要做千万次以上，「你节省一点计算的时间，累积起来就非常多了」。

课堂幻灯片 · 00:56:47：梯形速度曲线——速度梯形、位置为直线加两段抛物线、加速度分段常值

（a）情形一：指定加速度 $\ddot q_c$ 。 抛物线分段与线性分段必须速度衔接（不许跳跃），也就是抛物线分段终点的速度要等于线性分段的常值速度（0:58:00）：

定理 4.2.C · 指定加速度：

t_c

的二次方程与加速度下界

设 $q_c$ 为抛物线分段终点 $t_c$ 时刻的关节变量值——该段以常加速度 $\ddot q_c$ 从静止启动（ $\dot q(0)=0$ ）加速到此，则

$\ddot q_c t_c=\frac{q_m-q_c}{t_m-t_c} \tag{4.3}$

其中 $t_m=t_f/2$ 、 $q_m=(q_f+q_i)/2$ ，而

$q_c=q_i+\tfrac12\ddot q_c t_c^2 . \tag{4.4}$

合并 (4.3)(4.4) 得关于 $t_c$ 的二次方程

$\ddot q_c t_c^2-\ddot q_c t_f t_c+q_f-q_i=0 . \tag{4.5}$

通常以 $\operatorname{sgn}\ddot q_c=\operatorname{sgn}(q_f-q_i)$ 指定 $\ddot q_c$ ；于是对给定的 $t_f,q_i,q_f$ ，取 $t_c\le t_f/2$ 的根

$t_c=\frac{t_f}{2}-\frac12\sqrt{\frac{t_f^2\ddot q_c-4(q_f-q_i)}{\ddot q_c}} . \tag{4.6}$

加速度的下界（根号内非负）为

$|\ddot q_c|\ \ge\ \frac{4\,|q_f-q_i|}{t_f^2} . \tag{4.7}$

当 (4.7) 取等号时，轨迹不再有常值速度段，只剩加速段与减速段——速度曲线退化为三角形（0:59:30）。 $\ddot q_c$ 的正负由位移方向定：「终点位置和初始位置的差是正的，加速度取正的；是负的，加速度取负」（1:03:01）。

确定 $t_c$ 后，生成如下的多项式序列（教材式 4.8）：

$q(t)=\begin{cases} q_i+\tfrac12\ddot q_c t^2, & 0\le t\le t_c\[1mm] q_i+\ddot q_c t_c\left(t-\dfrac{t_c}{2}\right), & t_c<t\le t_f-t_c\[1mm] q_f-\tfrac12\ddot q_c(t_f-t)^2, & t_f-t_c<t\le t_f \end{cases} \tag{4.8}$

课堂幻灯片 · 01:04:22：由 (4.8) 生成的分段多项式序列与三角形极限

（b）情形二：指定巡航速度 $\dot q_c$ 。 指定加速度并不是确定梯形速度曲线的唯一途径（1:06:46）——换个思路，也可以直接指定巡航速度：

定义 4.2.4 · 指定巡航速度

除 $q_i,q_f,t_f$ 外，还可以指定巡航速度 $\dot q_c$ ，它须服从

$\frac{|q_f-q_i|}{t_f}<|\dot q_c|\le\frac{2|q_f-q_i|}{t_f} . \tag{4.9}$

注意到 $\dot q_c=\ddot q_c t_c$ ，由 (4.5) 解出

$t_c=\frac{q_i-q_f+\dot q_c t_f}{\dot q_c},\qquad \ddot q_c=\frac{\dot q_c^2}{q_i-q_f+\dot q_c t_f} . \tag{4.10,4.11}$

由此得到的 $t_c,\ddot q_c$ 同样代入 (4.8) 生成轨迹。

两端取等号的几何含义（1:07:12）：右边取等号相当于三角形（没有直线段），左边取等号相当于矩形（没有抛物线混合段）。但矩形在实际中不可能出现——「速度不可能在零时间内从零加速到某个速度」，所以左边必须是严格不等号。

课堂幻灯片 · 01:07:48：指定巡航速度的约束 (4.9) 与 (4.10)(4.11)，以及 12.5% 的能量代价

代价是多少？ 采用梯形速度曲线，性能指标 $\int_0^{t_f}\tau^2,\mathrm dt$ 比最优（三次）情形增加 12.5%（教材 §4.2.1）。老师 1:10:14 的权衡是：「但是折中考虑，它更简单、计算速度更快、轨迹没那么复杂，最后也是可接受的」——这正是工业上普遍采用梯形速度曲线的原因。

把三种时间律放在一起对比，各自的「光滑档位」一目了然：

三种点对点时间律的对比（本教程绘）：阶次越高越光滑，代价是能量与算力。三次最小能量但加速度两端跳跃；五次能指定初终加速度、三阶连续；梯形速度曲线（LSPB）加速度分段常值、峰值可由

v_{\max}/a_{\max}

直接控制，是工业默认选择。

考点 · 老师原话（录播时间点）

本节重心是梯形速度曲线，不是三次/五次。 三次讲完时老师明说：「咱们五次（多项式）上会简单地介绍一下，然后重点介绍混合梯形」（0:48:11）。三次、五次是「解方程」的基础练习，梯形速度曲线（LSPB）才是工业与考试的重点。

一切归结为解方程。 「整个都是解方程、待定系数法的过程——根据任务来找合适的约束，把三次或者五次多项式的系数解出来」（0:16:25）。

LSPB 的工程价值。 「在工业上其实一般采用混合多项式……允许直接验证机械手是否在物理上支持所得的速度和加速度」（0:55:28）——这是它取代五次多项式的理由。

末端速度必须归零的物理原因。 「如果不减为零的话，它还有惯性，它可能再往回走」（0:59:32）。

两段加速度取相等的动机是省算力。 「为了简化计算……实际过程中我们的计算可能是千万次，你节省一点计算时间，累积起来就非常多了」（0:59:48）。

12.5% 的取舍。 「采用梯形速度曲线，性能指标增加了 12.5%，但是折中考虑它更简单、计算速度更快，最后也是可接受的」（1:10:14）。

默写点 · 合上书再写（主动回忆）

不看正文与速查卡，先写出下面几条，再展开核对。写不出来就说明这一节还没「拿走」。

写出三次多项式的四个约束方程。
梯形速度曲线的两个「衔接条件」是什么？由此得到的 $t_c$ 方程是几次的？
默写加速度下界 (4.7) 与巡航速度约束 (4.9)，并说明各自的等号对应什么形状。

核对

$a_0=q_i$ ； $a_1=\dot q_i$ ； $a_3t_f^3+a_2t_f^2+a_1t_f+a_0=q_f$ ； $3a_3t_f^2+2a_2t_f+a_1=\dot q_f$ 。（ $q(t)$ 在 $t=0,t_f$ 处取初终位置； $\dot q(t)$ 在两端取初终速度。）

衔接条件即 (4.3)：抛物线分段终点速度 $\ddot q_c t_c$ 等于线性分段常值速度 $(q_m-q_c)/(t_m-t_c)$ ；配合 (4.4) $q_c=q_i+\tfrac12\ddot q_c t_c^2$ 得到关于 $t_c$ 的二次方程 (4.5)。

动手实验 · 同一段位移，三张速度剖面

三次（最小能量）、五次（管住两端加速度）与 LSPB（匀加速—匀速—匀减速）解决的是同一个问题，代价却各不相同：三次的加速度在起停瞬间跳变；五次的加速度两端为零，但峰值更高；LSPB 把加速度压成方波，峰值可直接由 $\ddot q_{\max}$ 验证。切换运动律，拖动 $T$ 与巡航速度系数，比较三条曲线在端部的行为。

4.2.1 小结与常见错误

一条主线：点对点约束少 ⇒ 无穷多解 ⇒ 用最小能量指标锁死 ⇒ 三阶多项式（速度抛物线、加速度线性）；要管初终加速度 ⇒ 五次（6 约束，放弃最优）；工业折中 ⇒ 梯形速度曲线（LSPB），峰值可直接由 $a_{\max},v_{\max}$ 验证，代价 12.5%。
两个可指定量：LSPB 可以指定加速度（情形一，得到 $t_c$ 二次方程）或指定巡航速度（情形二，得到 (4.10)(4.11)）——二选一，都可以。
一句话：「整个都是解方程、待定系数法的过程——根据任务找约束，把系数解出来。」

常见错误清单 · 自查三条

约束数 ≠ 系数个数：三次有 4 个系数，所以只能给 4 个条件；一旦任务要求指定初终加速度，三次就不够，必须升五次——不要硬凑。
把 $t_c$ 当成半个总时间： $t_c\le t_f/2$ 只是上界；三角形极限（ $t_c=t_f/2$ ）才没有巡航段。
忘方向： $\ddot q_c$ 的符号必须与 $q_f-q_i$ 一致（ $\operatorname{sgn}\ddot q_c=\operatorname{sgn}(q_f-q_i)$ ），否则 (4.6) 的根无意义。