作业四：教材习题 4.1 / 4.5 / 4.6（附详细解答与验算）

三道题是同一条主线上的三级台阶：4.1 是点对点（4.2.1），用五次多项式满足 6 个边界条件；4.5 与 4.6 用同一组路径点，分别演示「系列点」的两条路线——三次样条（4.2.2.3，加速度连续）与线性 + 抛物线混合（4.2.2.4，经由点）。三题的全部数值已由 tools/verify_ch04_traj.py（make verify）与官方《课后习题参考答案》逐项核对。

4.1 五次多项式：6 个约束定 6 个系数

题目：$q(0)=1\to q(2)=4$，初始与最终的速度、加速度均为零。

为什么是五次（4.2.1.2）：位置、速度、加速度的初终值共 6 个条件，而三次只有 4 个系数——必须升到五次：

$$q(t)=a_5t^5+a_4t^4+a_3t^3+a_2t^2+a_1t+a_0 .$$

列方程（$q_i=1,\ q_f=4,\ t_f=2$，初终速度、加速度全零）：

$$\begin{cases} a_0=q_i=1\ a_1=\dot q_i=0\ 2a_2=\ddot q_i=0\ a_5t_f^5+a_4t_f^4+a_3t_f^3+a_2t_f^2+a_1t_f+a_0=q_f\ 5a_5t_f^4+4a_4t_f^3+3a_3t_f^2+2a_2t_f+a_1=\dot q_f=0\ 20a_5t_f^3+12a_4t_f^2+6a_3t_f+2a_2=\ddot q_f=0 \end{cases}$$

一般解（消元后与 $t_f$、$|q_f-q_i|$ 无关的规范化形式，教材 §4.2.1 脚注）：

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

（检验 $t=t_f$：$10-15+6=1$ 是，恰到达 $q_f$；一、二阶导数在 $t_f$ 处均为零。）

本题系数（与官方解答完全一致）：

$$a_0=1,\quad a_1=0,\quad a_2=0,\quad a_3=\frac{15}{4},\quad a_4=-\frac{45}{16},\quad a_5=\frac{9}{16}$$

即

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

验算：$q(2)=1+30-45+18=4$ 是；$q(t_f)$ 处 $q'=q''=0$ 是（verify_ch04_traj.py 第 4 组）。注意：选择五次就放弃了 定理 4.2.A 的最小能量指标——能量最优解只能是三次（4.2.1.2）。

4.5 三次样条：两个虚拟点与三对角系统

题目：$q(0)=0,\ q(2)=2,\ q(4)=3$，初始与最终的速度、加速度均为零。

为什么需要虚拟点（定理 4.2.G）：$N=3$ 个路径点、初终加速度也指定时，方程数比未知数多 2，系统不能直接求解。教材的做法是引入两个虚拟点 $t_2=1$、$t_4=3$（官方解答即取此值），得 $N+1=4$ 个三次多项式、$4(N+1)=16$ 个方程对 16 个未知数。

步骤一：确定时间间隔。 五个时刻 $t_1=0,\ t_2=1,\ t_3=2,\ t_4=3,\ t_5=4$（$t_2,t_4$ 为虚拟点），故

$$\Delta t_1=\Delta t_2=\Delta t_3=\Delta t_4=1 .$$

步骤二：写出三对角系统 (4.28)。 未知量取 $\big[\ddot\Pi_2(t_2),\ \ddot\Pi_3(t_3),\ \ddot\Pi_4(t_4)\big]^{\mathrm T}$，代入 定理 4.2.H 给出的 $A$、$b$：

$$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}$$

（与官方解答一致；$A$ 的元素只依赖 $\Delta t_k$，故对所有关节相同。）

步骤三：解出加速度。 前向 / 后向代入得

$$\ddot\Pi_2(t_2)=\frac{51}{23}\approx2.2174,\qquad \ddot\Pi_3(t_3)=-\frac{30}{23}\approx-1.3043,\qquad \ddot\Pi_4(t_4)=-\frac{18}{23}\approx-0.7826 .$$

步骤四：写出四段多项式（每段以该段起点为基点，$\tau=t-t_k$）：

验算（全部通过，见 verify_ch04_traj.py 第 5 组）：

• 插值：$q(0)=0$、$q(2)=2$、$q(4)=3$ 是；
• 端点：$\dot q(0)=\dot q(4)=0$、$\ddot q(0)=\ddot q(4)=0$ 是；
• 内部连续性：在 $t=1,2,3$ 处位置、速度、加速度三重连续 是（这正是样条区别于 4.2.2.1/4.2.2.2 两路线的定义性特征）；
• 虚拟点值（未被指定，由连续性解出）：$q(t_2)=3/8$、$q(t_4)=23/8$——它们不是路径点，只是连续性约束的产物。

4.6 线性分段 + 抛物线混合：经由点

题目：$q(0)=0,\ q(2)=2,\ q(4)=3$，初始与最终速度为零。

结构与记号（定义 4.2.7）：三段之间用线性段连接，在三个路径点处各用一段持续 $2\times(\Delta t'_k/2)$ 的抛物线过渡。取官方解答的 $\Delta t'_k=0.2$（$k=1,2,3$），$\Delta t_1=\Delta t_2=2$。

步骤一：线性段速度（相邻路径点的斜率；端点速度由题设为零）：

$$\dot q_i=0,\qquad \dot q_{1,2}=\frac{2-0}{2}=1,\qquad \dot q_{2,3}=\frac{3-2}{2}=\frac{1}{2},\qquad \dot q_f=0 .$$

步骤二：线性段系数（$a_{k0}=q_k$，$a_{k1}=\dot q_{k,k+1}$）：

$$a_{10}=0,\ a_{11}=1;\qquad a_{20}=2,\ a_{21}=\frac{1}{2}.$$

步骤三：抛物线系数（由 定义 4.2.7 的 (4.29)(4.30) 与速度连续性导出）：

$$b_{k1}=\frac{\dot q_{k,k+1}+\dot q_{k-1,k}}{2},\qquad b_{k2}=\frac{\dot q_{k,k+1}-\dot q_{k-1,k}}{2\Delta t'_k},\qquad b_{k0}=q_k+\frac{(\dot q_{k,k+1}-\dot q_{k-1,k}),\Delta t'_k}{8}$$

代入得（与官方解答逐项一致）：

验算（verify_ch04_traj.py 第 6 组）：

• 抛物线在 $\tau=\pm\Delta t'_k/2$ 处与相邻线性段的位置、速度都连续 是；
• 经由点不是路径点：抛物线顶点值为 $b_{k0}$，与 $q_k$ 的偏差恰为 $\dfrac{(\dot q_{k,k+1}-\dot q_{k-1,k})\Delta t'_k}{8}$——第 2 点处 $2-1.9875=0.0125$，第 3 点处 $3-2.9875=0.0125$。轨迹从路径点附近经过而非到达（定义 4.2.7），这正是「经由点」一词的由来；
• 若把 $\Delta t'_k$ 调大（如 $0.6$），削角更多、加速度更小；调小则更贴近路径点、加速度更大（教材图 4.9 的实线 / 虚线对比）。

三题对照

一句话：4.1 考「数约束、解系数」；4.5 考「方程比未知数多 2 时用虚拟点、并降到三对角」；4.6 考「梯形速度曲线推广到多点时会削角」。三题合起来正好覆盖本章「关节空间」的全部要点。