(a) 路径 = 纯几何（经过哪些点、连成什么形状）；轨迹 = 路径 + 时间律（每点的速度与加速度）；运动规划 = 轨迹 + 避障与优化（动态过程，含动力学）（定义 4.1.1–定义 4.1.3）。

(b) 因为底层伺服需要的是参考输入序列：只有位置，伺服无法前馈速度/加速度，跟踪会滞后并产生大的误差；而且 定理 4.1.A 的两条硬约束（力矩饱和、结构谐振）都是对速度和加速度提的，只给位置就无从检验。

(c) 障碍物的位置在操作空间描述最自然，其关节空间像是非线性的、难以计算（0:35:36）；而驱动器只能按关节变量驱动，所以规划结果必须经逆运动学（或示教再现）落到关节空间（0:37:39）。

任务	约束数	最小阶次
(a) 位置 ×2 + 时间	2	一次（但需补优化指标才有唯一解）
(b) 再加初终速度 ×2	4	三次（4 个系数）
(c) 再加初终加速度 ×2	6	五次（6 个系数）
(d) 5 个路径点	5	单个四次多项式不可用（见 为什么不用单个高阶多项式 的五个缺点），改用 4 段三次 + 虚拟点 / 抛物线混合

一句话：方程（约束）的个数必须等于未知系数（阶次+1）的个数；少了多解、多了超定（0:16:25）。

路线	位置	速度	加速度	代价
① 指定路径点速度	是 平滑	是 连续	否 跳跃	用户指定的速度可能引起超调（例 4.2.E）
② 按规则计算速度	是 平滑	是 连续	否 跳跃	极值点被强制零速（例 4.2.F）
③ 三次样条	是 平滑	是 平滑	是 连续	需虚拟点 + 解三对角系统
④ 抛物线混合	是 平滑	是 连续	否 跳跃	轨迹不经过路径点（经由点）

只有 ③ 保证加速度连续——这是样条的定义性特征（定义 4.2.6）。

(a) 弧长的时间律 $s(t)$——位置 $p_e=f(s)$ 由路径定几何，$s(t)$ 定时间（定义 4.3.4）。

(b) 转角的时间律 $\theta(t)$——等效轴 $r'$ 固定不变，姿态轨迹完全由 $\theta$ 决定（定理 4.3.F）。

(c) 因为 $s$ 与 $\theta$ 都是一维标量，其时间律问题与「单关节 $q(t)$」同构，于是三次 / 五次 / 梯形 / 样条全部可以直接套用（0:20:12、2:27:32）。

思路：把问题写成变分形式——设 $\dot q=\omega$，约束 $I\dot\omega=\tau$ 与 $\int_0^{t_f}\omega\,\mathrm dt=q_f-q_i$，目标 $\min\int_0^{t_f}\tau^2\mathrm dt=\min I^2\int_0^{t_f}\dot\omega^2\mathrm dt$。

关键步：这是关于 $\omega$ 的泛函极值问题（Euler–Lagrange 方程），其解满足 $\mathrm d^4\omega/\mathrm dt^4=0$——即 $\omega$ 是三次函数的积分形式……更直接地，教材给出的结论是

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

结论：$\omega$ 二阶 ⇒ 积分得 $q(t)$ 为三阶（三次）多项式，求导得 $\ddot q(t)=6a_3t+2a_2$ 为线性加速度曲线（定理 4.2.A）。即使关节动力学不能这样简化（转动惯量是位形的函数），选三阶多项式仍是有效解。

思路：把 (4.1) 中的 $t$ 换成无量纲 $\eta=t/t_f$，$q$ 换成 $\tilde q=(q-q_i)/(q_f-q_i)$。

关键步：条件变成 $\tilde q(0)=0,\ \tilde q(1)=1,\ \tilde q'(0)=\tilde q'(1)=0$（这里 $'$ 对 $\eta$）。设 $\tilde q=b_3\eta^3+b_2\eta^2+b_1\eta+b_0$：$b_0=0$，$b_1=0$，$b_3+b_2=1$，$3b_3+2b_2=0$ ⇒ $b_3=-2,\ b_2=3$。

结论：$\tilde q(\eta)=3\eta^2-2\eta^3$，与数据无关；回到原变量

$$q(t)=q_i+(q_f-q_i)\left[3\Big(\frac{t}{t_f}\Big)^2-2\Big(\frac{t}{t_f}\Big)^3\right].$$

这正是教材 §4.2.1 脚注②说的「可以把系数的计算规范化，使其独立于结束时间 $t_f$ 及路径长度 $|q_f-q_i|$」。对照 例 4.2.B（$q_i=0,q_f=\pi,t_f=1$）得 $a_2=3\pi,\ a_3=-2\pi$ 是。

思路：衔接条件 (4.3) 是 $\ddot q_ct_c=\dfrac{q_m-q_c}{t_m-t_c}$，其中 $t_m=t_f/2$、$q_m=(q_f+q_i)/2$；抛物线段的位移由 (4.4) 给出 $q_c=q_i+\tfrac12\ddot q_ct_c^2$。

关键步：把 (4.4) 代入 (4.3)：

$$\ddot q_ct_c\left(\frac{t_f}{2}-t_c\right)=\frac{q_f-q_i}{2}-\frac12\ddot q_ct_c^2 \;\Longrightarrow\; \ddot q_ct_c^2-\ddot q_ct_ft_c+(q_f-q_i)=0 .$$

这是关于 $t_c$ 的二次方程；取 $t_c\le t_f/2$ 的根即 (4.6)。根号内非负给出

$$t_f^2\ddot q_c-4(q_f-q_i)\ge0\ \Longrightarrow\ |\ddot q_c|\ge\frac{4|q_f-q_i|}{t_f^2} .$$

结论：等号时 $t_c=t_f/2$，巡航段长度为零，速度曲线退化为三角形（定理 4.2.C）。

思路：数未知数与方程数。

关键步：未知数 = $4(N-1)$（$N-1$ 段 × 4 个系数）。方程：中间 $N-2$ 个点各 4 个（位置、位置连续、速度连续、加速度连续）⇒ $4(N-2)$；首末两点各 3 个（位置、速度、加速度）⇒ 6。合计 $4N-2$。

结论：$4N-2-4(N-1)=2$——多出的正是初始与最终加速度这两个任意指定的条件。三种出路：① 首末段改用四阶多项式；② 只在部分时间保持轨迹一致；③ 引入两个虚拟点（只施加连续性约束、不指定位置），把未知数与方程数同时提到 $4(N+1)$ 而配平（定理 4.2.G）。

思路：看 (4.28) 的每个方程牵涉哪些未知量。

关键步：由 (4.26) 二阶导数的线性插值与两次积分，$\Pi_k$ 只依赖 $\ddot\Pi_k(t_k),\ddot\Pi_k(t_{k+1})$ 与两端位置；再用位置连续性与加速度连续性消元后，第 $k$ 个速度连续性方程只涉及 $\ddot\Pi_k(t_k),\ddot\Pi_{k+1}(t_{k+1}),\ddot\Pi_{k+2}(t_{k+2})$ 三个相邻未知量——于是每行只有三个非零元，构成三对角带。

结论：$A$ 的元素（官方解答给出显式表达式）只依赖时间间隔 $\Delta t_k$；所有关节共享同一时间轴、同一 $\Delta t_k$，故 $A$ 与关节无关，只有右端项 $b$ 随关节角度变化（1:46:35）。这带来巨大的算力红利：$A$ 只需分解一次，换关节只换 $b$，且三对角系统可用 $O(N)$ 的前向/后向代入求解。

思路：直接计算 $R^{\mathrm T}R$，看交叉项。

关键步：记 $R=R_i+\lambda D$，$D=R_f-R_i$。则

$$R^{\mathrm T}R=R_i^{\mathrm T}R_i+\lambda(R_i^{\mathrm T}D+D^{\mathrm T}R_i)+\lambda^2D^{\mathrm T}D =I+\lambda(R_i^{\mathrm T}R_f+R_f^{\mathrm T}R_i-2I)+\lambda^2D^{\mathrm T}D .$$

结论：一般地，$R_i^{\mathrm T}R_f+R_f^{\mathrm T}R_i\ne2I$ 且 $D^{\mathrm T}D\ne0$，故 $\lambda\in(0,1)$ 时 $R^{\mathrm T}R\ne I$——插值出的中间矩阵不是旋转矩阵（列不再正交、模不再为 1）。这正是 定理 4.3.E 的内容，也是姿态规划必须换表示（欧拉角 / 角和轴）的原因。

解 4 个约束（定义 4.2.1）：$a_0=0$、$a_1=0$、$a_3+a_2=\pi$、$3a_3+2a_2=0$ ⇒ $a_2=3\pi,\ a_3=-2\pi$。

$$q(t)=-2\pi t^3+3\pi t^2,\qquad \dot q(t)=-6\pi t^2+6\pi t,\qquad \ddot q(t)=-12\pi t+6\pi .$$

速度是抛物线、加速度是直线；加速度在 $t=0$（从 0 跳到 $6\pi$）与 $t=1$（从 $-6\pi$ 跳回 0）两处跳跃。峰值加速度 $6\pi$，峰值速度 $1.5\pi$（$t=0.5$）。

由 (4.6)：$t_c=\dfrac12-\dfrac12\sqrt{\dfrac{6\pi-4\pi}{6\pi}}=\dfrac12-\dfrac{1}{2\sqrt3}\approx0.2113$ s。

巡航段时长 $t_f-2t_c\approx0.5774$ s；巡航速度 $\dot q_c=\ddot q_c t_c=6\pi\times0.2113\approx3.983$ rad/s（> 平均速度 $\pi\approx3.142$，符合 (4.9)）。

$$q(t)=\begin{cases} \tfrac12(6\pi)t^2, & 0\le t\le0.2113\\ 6\pi(0.2113)\,(t-0.1057), & 0.2113<t\le0.7887\\ \pi-\tfrac12(6\pi)(1-t)^2, & 0.7887<t\le1 \end{cases}$$

验收标准：$q(1)=\pi$；$\dot q$ 在三段交界处连续；$\ddot q$ 分段常值 $\pm6\pi$；把 $|\ddot q_c|$ 降到 $4\pi$ 时 $t_c=t_f/2$、巡航段消失（三角形）。

分段斜率：$v_2=\pi$、$v_3=( \pi/2-2\pi)/1=-3\pi/2$、$v_4=(\pi-\pi/2)/2=\pi/4$。

• 点 2：$\operatorname{sgn}(\pi)\ne\operatorname{sgn}(-3\pi/2)$ ⇒ $\dot q_2=0$；
• 点 3：$\operatorname{sgn}(-3\pi/2)\ne\operatorname{sgn}(\pi/4)$ ⇒ $\dot q_3=0$；
• 端点：$\dot q_1=\dot q_4=0$。

结论：所有中间点速度为零——轨迹在每个路径点都「顿一下」再出发，与 例 4.2.F 的图 4.6 一致。若某点前后斜率同号，则该点速度取两斜率平均（verify_ch04_traj.py 第 3 组）。

一般规范化形式（例 4.2.B 的推广）：

$$q(t)=q_i+(q_f-q_i)\Big[10\eta^3-15\eta^4+6\eta^5\Big],\qquad \eta=\frac{t}{t_f}.$$

代入 $q_i=1,q_f=4,t_f=2$：

$$q(t)=1+\frac{15}{4}t^3-\frac{45}{16}t^4+\frac{9}{16}t^5 .$$

验收标准：$q(0)=1$、$q(2)=4$、$\dot q(0)=\dot q(2)=0$、$\ddot q(0)=\ddot q(2)=0$（6 条全过）；与官方解答系数一致。完整版见作业四。

$\Delta t_k\equiv1$，$N=3$，未知量 $[\ddot\Pi_2(t_2),\ddot\Pi_3(t_3),\ddot\Pi_4(t_4)]^{\mathrm T}$：

$$A=\begin{bmatrix}1&1/6&0\\0&2/3&1/6\\0&1/6&1\end{bmatrix},\qquad b=\begin{bmatrix}2\\-1\\-1\end{bmatrix} \;\Longrightarrow\; \ddot\Pi_2=\frac{51}{23},\ \ddot\Pi_3=-\frac{30}{23},\ \ddot\Pi_4=-\frac{18}{23}.$$

四段多项式见 例 4.2.I 与作业四。验收标准：$q(0)=0,q(2)=2,q(4)=3$；端点速度、加速度为零；$t=1,2,3$ 处 $C^0/C^1/C^2$ 连续；虚拟点值 $q(1)=3/8$、$q(3)=23/8$（未被指定）。

线性段：$a_{10}=0,a_{11}=1$；$a_{20}=2,a_{21}=0.5$。

抛物线：$b_{10}=0.025,b_{11}=0.5,b_{12}=2.5$；$b_{20}=1.9875,b_{21}=0.75,b_{22}=-1.25$；$b_{30}=2.9875,b_{31}=0.25,b_{32}=-1.25$。

经由点偏差（与 例 4.2.J 同源）：第 2、3 点处 $|b_{k0}-q_k|=0.0125$——轨迹从路径点附近经过（教材图 4.8）。验收标准：抛物线在 $\pm\Delta t'_k/2$ 处与线性段的位置、速度均连续；把 $\Delta t'_k$ 增大到 0.6 时偏差增大、加速度减小。

• 可验证性：梯形速度曲线的加速度是分段常值，峰值就是 $\ddot q_c$，可以直接与电机力矩上限、结构许用加速度比对——「允许直接验证机械手是否在物理上支持所得的速度和加速度」（0:55:28）。三次/五次的峰值要另算。
• 算力：梯形只需解一个二次方程 (4.5) 或两次除法 (4.10)(4.11)，且首末段对称；五次要解 6 元系统、样条要解三对角系统。工业上这种计算要做千万次，累积差异很大（0:59:48）。
• 可控性：用户界面只需暴露「最大速度的百分比」和「最大加速度的百分比」，天然不会超出物理极限（0:57:49）；指定总时间反而容易让用户给出不可实现的要求。

代价是性能指标 $\int\tau^2\mathrm dt$ 比最优（三次）情形增加 12.5%（教材 §4.2.1）——工程上认为可接受。

受限原因：操作空间的轨迹生成要实时配合逆运动学求解（每一点都要解逆解），计算复杂，采样率过高算不过来（2:00:02）。

做法：规划时用较低采样率得到一组稀疏的设定点；送给底层伺服前，在相邻两点之间做线性微小内插，把输入频率提上去。

收益：参考输入的更新频率提高 ⇒ 伺服跟踪的滞后与误差减小，增强系统的动态性能（教材 §4.3）。注意这只是「加密」，不改变规划轨迹本身；精度由规划点的密度决定。

停顿原因：两条不共线分段在路径点处一阶导数不连续——若要求轨迹经过该点，速度必须归零再重新加速（定义 4.3.5）。这正是关节空间中 例 4.2.K 的现象在操作空间的翻版。

时间超前：在前一分段的梯形速度曲线尚未完成时，就启动下一分段——用 (4.50) 把第 $j$ 段的 $s_j(t)$ 提前 $\Delta t_j$ 开始，$\Delta t_j$ 由递归 $\Delta t_j=\Delta t_{j-1}+\delta t_j$ 估计（定理 4.3.D）。

代价：① 不再保证在指定时刻经过路径点（路径点降格为经由点）；② 该规划没有考虑速度连续性，实际中还要加抛物线过渡或提前预判（2:20:27）。收益是全局持续时间更短、效率更高。

	欧拉角法	角和轴法
表示	三个角 $(\varphi,\vartheta,\psi)$ 分别插值 (4.51)	等效轴 $r'$ 固定，只规划转角 $\theta$ (4.52)–(4.55)
优点	与 2.4 的表示直接衔接；实现简单	几何直观（绕一根固定轴转）；降为一维，无中间表示冗余
坑	① 插值完必须经 $T(\varphi_e)$ 换算回基系（定理 3.6.A）；② $T$ 在 $\sin\vartheta=0$ 处奇异（ZYZ 歧义，3.6）；③ 三个角同时插值可能绕过中间姿态	① $\sin\theta_f=0$ 时轴不唯一，需特殊处理；② $r'$ 是在 $R_i^f=R_i^{\mathrm T}R_f$ 下算的，$\omega'$ 还要左乘 $R_i$

结论：两者都把姿态规划化为一维标量时间律；老师明确本节「不详细讲」但原理要清楚（2:26:03）。工程上另有单位四元数插值（2.6）可避免奇异，教材未展开。