使用说明

本卷题目绝大多数为新增。第 17 题与习题 C2 同族（球形臂逆解结构）、第 18 题与习题 B2 同族（雅可比与奇点）——这两处有意保留：它们是本章「会背公式但不会用」的两个高风险区，隔一段时间再做一次正好检验是否真的会用。第 16 题（Rodrigues 数值）与习题 C1 思路相同但换了轴、换了数值，请独立重算。解答默认折叠，便于自测。若某题完全无从下手，先做后面的题；全部做完后再回看。

第一部分答案（每题 3 分：判断 1 分 + 理由 2 分）

（1） 错。 前半句对（6 个正交性约束压掉 6 个自由度 ⇒ 独立参数 3 个），但 $\det R$ 的符号不是无关紧要：$\det R=-1$ 对应把某根轴镜像翻转的左手系，它不属于 $SO(3)$，用它算出来的是「照镜子」的假姿态。理由：正交性只给出 $(\det R)^2=1$；而右手系的固连坐标系总可以连续转回与参考系重合，$\det$ 是元素的连续函数、途中不可能从 $+1$ 跳到 $-1$，故右手系必为 $\det R=+1$。（2.2.1）

（2） 错——两条规则互为镜像。 绕固定轴按序 $(\vartheta_1,\vartheta_2)$ 与绕当前轴按序 $(\vartheta_2,\vartheta_1)$ 给出同一个终态，而且乘积写出来完全相同（先转的落在右端）；固定轴的时序与乘积排列互为逆序。所以它们不是两条独立规则，而是同一条事实的两种读法——「站在地上看」与「站在刚体上看」。（原理 2.3.A、原理 2.3.B、例 2.3.C）

（3） 对。 两处的根源都是两根转轴共线导致独立方向减少：ZYZ 在 $\vartheta=0$ 时 $z$ 与 $z''$ 共线，$\varphi$ 与 $\psi$ 的贡献合并成一次转动，故只能确定和或差；角和轴在 $\vartheta=0$ 时 $R=I_3$，而 $\exp(\boldsymbol 0)=I$ 对任意 $\boldsymbol r$ 成立——轴失去定义（对数不唯一）。同一数学事实：前者表现为「两个角的组合被合并」，后者表现为「轴不被约束」。（定理 2.4.B、定理 2.5.B）

（4） 错。 四元数是 4 参数 − 1 个单位约束 = 3 个独立参数，与最简表示的要求不矛盾；它换来的是无奇点（逆解对任何 $R$ 都有效）与插值稳定（姿态演化是单位球面上的一条连续轨迹）。代价是 4 个参数不再能直观读成角度、乘法含叉积项。正确说法：「欧拉角给人看，四元数给机器算」——判别看用途，不看参数个数。（原理 2.6.A、2.6）

（5） 错。 正交性只在旋转块内成立；右上角的平移块使整个 $4\times4$ 矩阵不再正交（原书式 2.46 明确 $A^{-1}\neq A^{\mathrm T}$）。正确做法：按分块公式装配 $A^{-1}=\begin{bmatrix}R^{\mathrm T}&-R^{\mathrm T}\boldsymbol o\\ \boldsymbol 0^{\mathrm T}&1\end{bmatrix}$——旋转块取转置、平移块先反向再左乘转置。（2.7.3）

（6） 对。 开链每个单自由度关节贡献一个独立变量，可直接计数；闭式链存在回路约束，必须先扣除独立约束数目（1.2.1 的定义 1.2.2）。典型反例：平行四边形臂的关节数多于独立变量数——约束 $\vartheta_2'=\vartheta_1''-\vartheta_1'$、$\vartheta_3'=\pi-\vartheta_1''+\vartheta_1'$ 把自由度压到 $\boldsymbol q=[\vartheta_1'\ \vartheta_1'']^{\mathrm T}$ 两个。（定义 1.2.2、2.9.2）

（7） 错。 本教程与原书用标准 DH：$z_{i-1}$ 沿关节 $i$ 的轴（看守模型），或等价地说「杆 $i$ 上固连坐标系 $i$，其 $z$ 轴放在关节 $i+1$ 的轴线上」。而 Craig《机器人学导论》用改进 DH：$z_i$ 就沿关节 $i$ 的轴。两套各自自洽、混用必错；同一机械手在两套约定下参数表整列不同，但 $T_n^0$ 相同。（先把「系到底挂在哪」说透：看守模型、勘误 E2.4）

（8） 对。 这就是 定理 2.12.B（文献源头为 Pieper 1968）：6 自由度运动学结构具有闭合形式逆解的充分条件，满足其一即可——三个连续转动关节的轴交于一点（球形腕），或三个连续转动关节的轴平行。这也解释了工程界为什么把主流机械手设计成「臂 + 球形腕」。（定理 2.12.B、勘误 E2.6）

（9） 错。 线性化方程本身是线性的，但它来自于对非线性函数 $\boldsymbol x_e=k(\boldsymbol\xi)$ 在标称值附近的一阶近似——只在小偏差假定下成立。每轮迭代都要用新参数同时刷新三样：参数 $\boldsymbol\xi'=\boldsymbol\xi+\Delta\boldsymbol\xi$、线性化矩阵 $\Phi$、以及新的偏差 $\Delta\boldsymbol x$（新的量测值与新的正运动学计算值之差），直到修正量收敛。（标定三步法）

（10） 对。 判据是 $n>r$，其中 $r$ 是任务实际用到的操作空间分量数（而不是 $m$）：三连杆平面臂 $n=3$、平面操作空间 $m=3$，若任务只做平面内 $xy$ 定位、不关心末端姿态，则 $r=2<3$ ⇒ 功能冗余（肘部姿态可以自由摆动而笔尖不动）；若任务要求位置 + 姿态（$r=3$），同一台臂立刻非冗余。（定义 2.10.3、例 3.4.A）

第二部分答案（每题 4 分）

11. 起点：正交性 $RR^{\mathrm T}=I_3$，三列构成标准正交组。第一步：$(R^{\mathrm T}R)_{ij}=\boldsymbol c_i^{\mathrm T}\boldsymbol c_j$（$\boldsymbol c_i$ 为 $R$ 的列），单位模长给对角线的 $1$、两两垂直给非对角的 $0$ ⇒ $R^{\mathrm T}R=I_3$。第二步：由 $R^{\mathrm T}R=I_3$ 得 $R^{-1}=R^{\mathrm T}$。自由度账：三列模长为 1 给 3 个约束、三列两两垂直再给 3 个约束 ⇒ 共 6 个，$9-6=3$。评分：两步结论各 1 分；自由度账 2 分（须说出 3 + 3 的来历）。失分点：只说「9 减 6 得 3」而不说 6 从哪来。（9 个量、6 个约束、3 个独立参数）

12. ① 不唯一：$\arccos$ 只在 $[0,\pi]$ 取值，无法覆盖 $(-\pi,\pi]$ 内的完整角度。② 精度不均匀：$(\arccos x)'=-\dfrac{1}{\sqrt{1-x^2}}$ 在 $x\to\pm1$（角度接近 $0$ 或 $\pi$）处无界，微小输入误差被无限放大；而 $(\arctan x)'=\dfrac{1}{1+x^2}\in(0,1]$ 处处有界，输入误差不会被放大。结论：改用双变量 $\operatorname{Atan2}(y,x)$（第一参数是分子），用两参数符号定象限、在 $(-\pi,\pi]$ 内唯一。评分：两条理由各 1.5 分（须给公式）；Atan2 的用法 1 分。（原理 2.4.A）

13. ① $z_i$ 放哪：沿轴 $i+1$（即关节 $i+1$ 的轴线）→ 为变量 $\vartheta_i,d_i$ 服务（关节运动在该系下恰好表现为绕 $z_{i-1}$ 转 $\vartheta_i$ 或沿 $z_{i-1}$ 移 $d_i$）。② 原点 $o_i$ 放哪：取 $z_{i-1}$ 与 $z_i$ 的公垂线和 $z_i$ 的交点 → 为常数 $a_i$ 服务。③ $x_i$ 指哪：沿公垂线，由关节 $i$ 指向关节 $i+1$ → 与 ② 共同确定 $a_i$ 的方向（第 4 列的 $(a_ic_\vartheta,a_is_\vartheta)$）。④ $y_i$：右手系补齐 → 不引入参数（只保证 $R\in SO(3)$ 且唯一）。一句话总结：$z$ 轴规则管变量，公垂线规则管常数；两 $z$ 轴的方向关系给 $\alpha_i$。评分：四条规则 2 分；四条「为谁服务」1.5 分；总结句 0.5 分。（DH 建系四规则、规则 → 参数：每条建系规则「为谁服务」）

14. 把参考点从末端挪到腕心 $W$，微分运动学分块化为下三角：

$$\begin{bmatrix}\dot{\boldsymbol p}_w\\ \boldsymbol\omega_e\end{bmatrix}=\begin{bmatrix}J_{11}&0\\ J_{21}&J_{22}\end{bmatrix}\begin{bmatrix}\dot{\boldsymbol q}_a\\ \dot{\boldsymbol q}_w\end{bmatrix},\qquad \det J=\det J_{11}\cdot\det J_{22}.$$

能做什么：奇点分析——判定拆成两个三阶行列式（腕奇点 $\det J_{22}\propto s_5$；臂奇点 $\det J_{11}=0$，含肘奇点 $s_3=0$ 与肩奇点 $a_2c_2+a_3c_{23}=0$）。不能做什么：做逆解 / 速度控制——参考点已不是末端，矩阵不再提供「关节速度 → 末端执行器速度」的映射。合法性来源：运动学奇点是机械结构固有特性、与坐标系建在哪里无关，故可为简化计算而重选参考系；做完判断必须换回末端参考。（定理 3.3.A、3.3.3）

15. 线性化：$\Delta\boldsymbol x=\Phi\Delta\boldsymbol\xi$（$\Phi$ 为 $m\times4n$ 的运动学标定矩阵，即参数空间到操作空间的雅可比）。最小二乘：$\Delta\boldsymbol\xi=(\Phi^{*\mathrm T}\Phi^*)^{-1}\Phi^{*\mathrm T}\Delta\boldsymbol x^*$，量测条件 $tm\ge4n$、建议 $tm\gg4n$。为什么不能太集中：要让 $(\Phi^{*\mathrm T}\Phi^*)^{-1}$ 存在且数值稳定，$\Phi^*$ 必须列满秩——量测位姿挤在工作空间一隅时，各列近乎线性相关，$\Phi^{*\mathrm T}\Phi^*$ 接近奇异（病态），解出的 $\Delta\boldsymbol\xi$ 会被噪声放大。评分：两式各 1.5 分；量测条件与病态解释 1 分。（标定三步法）

第三部分答案与评分要点

16.（1）（3 分）

$$S(\boldsymbol r)=\begin{bmatrix}0&-r_z&r_y\\ r_z&0&-r_x\\ -r_y&r_x&0\end{bmatrix}=\frac13\begin{bmatrix}0&-1&2\\ 1&0&-2\\ -2&2&0\end{bmatrix}.$$

$\boldsymbol r\boldsymbol r^{\mathrm T}=\dfrac19\begin{bmatrix}4&4&2\\ 4&4&2\\ 2&2&1\end{bmatrix}$，故 $S^2=\boldsymbol r\boldsymbol r^{\mathrm T}-I=\dfrac19\begin{bmatrix}4&4&2\\4&4&2\\2&2&1\end{bmatrix}-\begin{bmatrix}1&0&0\\0&1&0\\0&0&1\end{bmatrix}=\dfrac19\begin{bmatrix}-5&4&2\\4&-5&2\\2&2&-8\end{bmatrix}$。评分：$S$ 2 分、$S^2$ 1 分。自查：$S^2$ 必须对称且迹为 $-1$（因为 $\operatorname{tr}(\boldsymbol r\boldsymbol r^{\mathrm T})=1$）。

（2）（4 分）$\vartheta=90^{\circ}\Rightarrow c_\vartheta=0,\ s_\vartheta=1$，故 $R=0\cdot I+1\cdot\boldsymbol r\boldsymbol r^{\mathrm T}+1\cdot S(\boldsymbol r)=\boldsymbol r\boldsymbol r^{\mathrm T}+S(\boldsymbol r)$：

$$R=\frac19\begin{bmatrix}4&4&2\\4&4&2\\2&2&1\end{bmatrix}+\frac13\begin{bmatrix}0&-1&2\\1&0&-2\\-2&2&0\end{bmatrix} =\frac19\begin{bmatrix}4&1&8\\7&4&-4\\-4&8&1\end{bmatrix}.$$

评分：项数选择正确（$c_\vartheta I$ 项为零、$s_\vartheta=1$）2 分；九个元素全对 2 分。失分点：$S(\boldsymbol r)$ 符号写反（导致 $R$ 转反）；忘记 $c_\vartheta=0$ 仍要写 $0\cdot I$ 与 $S^2$ 的取舍关系。

（3）（3 分）

$$R\boldsymbol p=\frac19\begin{bmatrix}4\cdot1+1\cdot2+8\cdot2\\ 7\cdot1+4\cdot2+(-4)\cdot2\\ -4\cdot1+8\cdot2+1\cdot2\end{bmatrix}=\frac19\begin{bmatrix}22\\7\\14\end{bmatrix}.$$

验算：$\operatorname{tr}R=\frac19(4+4+1)=1$，而 $1+2\cos90^{\circ}=1+0=1$ ✓——与 定理 2.5.E 的取迹公式一致。顺带的免费检查：$\|R\boldsymbol p\|=\frac{\sqrt{484+49+196}}{9}=\frac{\sqrt{729}}{9}=\frac{27}{9}=3$，而 $\|\boldsymbol p\|=\sqrt{1+4+4}=3$ ✓（旋转保持模长）。评分：结果 2 分；迹验算 1 分。加分不改分：能顺带用模长守恒自查。

17.（1）（2 分）腕心 = 从末端原点沿末杆回退 $a_3$（方向角 $\phi=30^{\circ}$）：

$$p_{wx}=p_x-a_3\cos\phi=0.60-0.20\times0.8660=0.4268\ \mathrm{m},\qquad p_{wy}=p_y-a_3\sin\phi=0.30-0.20\times0.5000=0.2000\ \mathrm{m}.$$

评分：两式各 1 分。失分点：忘记扣 $a_3$（直接对 $(p_x,p_y)$ 用两连杆公式）——这是 解耦五步 腕心公式在平面情形的最小实例。

（2）（4 分）

$$c_2=\frac{p_{wx}^2+p_{wy}^2-a_1^2-a_2^2}{2a_1a_2} =\frac{0.18215+0.04000-0.2500-0.1600}{2\times0.50\times0.40} =\frac{-0.18785}{0.40}=-0.46963,$$

$$s_2=\pm\sqrt{1-c_2^2}=\pm\sqrt{1-0.22055}=\pm\sqrt{0.77945}=\pm0.88287.$$

因为 $|c_2|=0.470<1$，该点在工作空间内（存在性判据 $-1\le c_2\le1$ 满足）。为什么两支：把腕心方程取平方相加时消去了 $\vartheta_1$，回代时 $s_2$ 的符号未被方程约束——正号对应肘朝下、负号对应肘朝上，两组解给出同一个腕心、两组镜像的两连杆三角形（几何法里的 $\vartheta_1=\alpha\pm\beta$ 与此等价）。取哪一支要由关节限位、避障或路径连续性决定。评分：$c_2$ 数值 1.5 分；$s_2$ 双符号 1 分；工作空间内判断 0.5 分；双支的几何解释 1 分。

（3）（4 分）取 $s_2=+0.88287$（肘朝下）：

$$\vartheta_2=\operatorname{Atan2}(0.88287,\ -0.46963)=118.03^{\circ}\quad(\text{约}\ 118.03^{\circ}).$$

（注意 $\vartheta_2$ 落在第二象限：$c_2<0$、$s_2>0$。）

再由腕心方程解 $\vartheta_1$（$k_1=a_1+a_2c_2=0.50+0.40\times(-0.46963)=0.31215$，$k_2=a_2s_2=0.40\times0.88287=0.35315$）：

$$s_1=\frac{k_1p_{wy}-k_2p_{wx}}{p_{wx}^2+p_{wy}^2}=\frac{0.31215\times0.2000-0.35315\times0.4268}{0.22215} =\frac{0.06243-0.15072}{0.22215}=-0.39748,$$

$$c_1=\frac{k_1p_{wx}+k_2p_{wy}}{p_{wx}^2+p_{wy}^2}=\frac{0.31215\times0.4268+0.35315\times0.2000}{0.22215} =\frac{0.13323+0.07063}{0.22215}=0.91763,$$

$$\vartheta_1=\operatorname{Atan2}(-0.39748,\ 0.91763)=-23.42^{\circ},\qquad \vartheta_3=\phi-\vartheta_1-\vartheta_2=30^{\circ}-(-23.42^{\circ})-118.03^{\circ}=-64.61^{\circ}.$$

回代验算：

$$a_1c_1+a_2c_{12}=0.50\times0.91763+0.40\cos(94.61^{\circ})=0.45882+0.40\times(-0.08042)=0.4267\ \checkmark$$

$$a_1s_1+a_2s_{12}=0.50\times(-0.39748)+0.40\sin(94.61^{\circ})=-0.19874+0.40\times0.99677=0.2000\ \checkmark$$

与 (1) 的腕心 $(0.4268,0.2000)$ 一致。评分：$\vartheta_2$ 1 分；$\vartheta_1$ 1.5 分；$\vartheta_3$ 0.5 分；回代验算 1 分（未验算扣 1 分）。加分不改分：能指出同一腕心的另一支解 $\vartheta_2=-118.03^{\circ}$（肘朝上）并说明它给出一组不同的 $(\vartheta_1,\vartheta_3)$。

18.（1）（4 分）$\vartheta_1=30^{\circ}\Rightarrow c_1=0.8660,\ s_1=0.5000$；$\vartheta_{12}=30^{\circ}+60^{\circ}=90^{\circ}\Rightarrow c_{12}=0,\ s_{12}=1$。于是

$$\bar J=\begin{bmatrix}-1.00\times0.5000-0.80\times1 & -0.80\times1\\ 1.00\times0.8660+0.80\times0 & 0.80\times0\end{bmatrix} =\begin{bmatrix}-1.300&-0.800\\ 0.866&0.000\end{bmatrix},$$

$$\det\bar J=(-1.300)(0.000)-(-0.800)(0.866)=+0.6928.$$

与 $a_1a_2s_2$ 一致：$a_1a_2s_2=1.00\times0.80\times\sin60^{\circ}=0.6928$ ✓（这正是 例 3.2 的一般形式）。评分：四个元素 2 分（每个 0.5 分）；$\det$ 1 分；一致性检查 1 分。失分点：把 $s_{12}$ 当成 $s_1s_2$。

（2）（3 分）$\det\bar J=0.6928\ne0$ ⇒ 不是奇点。奇异位形由 $\det\bar J=a_1a_2s_2=0$ 给出：$\vartheta_2=0$（臂伸直）或 $\vartheta_2=\pi$（臂折回），与 $\vartheta_1$ 无关（转肩不改变奇异性）。两处都是边界奇点——它们出现在可达工作空间的边界（外轮廓 $R=a_1+a_2$ 与内轮廓 $r=|a_1-a_2|$），「不表示真正的缺陷」，使用时避开边界即可；与之相对的内部奇点出现在可达工作空间内部、由两个或以上运动轴线共线引起，轨迹规划时一定要避开。（例 3.2、3.3.1）

评分：判断 + 条件 1.5 分（须写出 $s_2=0$ 且说明与 $\vartheta_1$ 无关）；边界奇点归类 1.5 分（须说明它与内部奇点在处置上的区别）。

（3）（3 分）

$$\bar J^{-1}=\frac{1}{\det\bar J}\begin{bmatrix}0.000&0.800\\ -0.866&-1.300\end{bmatrix} =\frac{1}{0.6928}\begin{bmatrix}0.000&0.800\\ -0.866&-1.300\end{bmatrix},$$

$$\dot{\boldsymbol q}=\bar J^{-1}\dot{\boldsymbol p}_e=\frac{1}{0.6928}\begin{bmatrix}0.000\times0.10+0.800\times(-0.05)\\ -0.866\times0.10+(-1.300)\times(-0.05)\end{bmatrix} =\frac{1}{0.6928}\begin{bmatrix}-0.0400\\ -0.0216\end{bmatrix} =\begin{bmatrix}-0.0577\\ -0.0312\end{bmatrix}\ \mathrm{rad/s}.$$

两个关节速度的绝对值都远小于 $2.0\ \mathrm{rad/s}$ ⇒ 满足限速要求（且离奇异位形 $s_2=0$ 很远，$\bar J$ 的条件数良好）。评分：$\bar J^{-1}$ 正确 1.5 分（伴随矩阵的符号错则整题不得分）；$\dot{\boldsymbol q}$ 数值 1 分；结论 0.5 分。顺带判据：关节速度大不大，除了看任务速度，还要看 $\det\bar J$（奇异邻域内 $\bar J^{-1}=\operatorname{adj}\bar J/\det\bar J$ 的元素整体趋于无穷）——这就是「奇点邻域关节速度趋大」的定量说法。

第四部分答案与评分要点

19.（10 分）

(a) 多支性与工作空间（3.5 分）。 同一末端位姿可对应多组关节变量：六轴臂的拟人臂部分给 4 组解（肘上 / 肘下 × 肩左 / 肩右，定理 2.12.C），球形腕再给 2 支（$\vartheta_5\in(0,\pi)$ 与 $(-\pi,\pi)$）——合起来可达 8 组，加上腕的翻转分支最多 16 组。做弧焊：末端须贴近工件沿曲线走，通常取「肘上 + 腕不过度翻转」那一支以避开夹具；做高速分拣：追求节拍，要取离奇异最远、关节行程最短的那一支。所以「同一台臂能两用」的第一层条件是：两组任务各有一支可行解，且都在各自的工作空间内。若分拣点分布在工作空间的外层（近边界），肘奇点 $s_3=0$ 会把可用解压到一支甚至合一，多支红利消失。

(b) 奇异位形与关节限速（3.5 分）。 三处奇点都要核：腕奇点 $s_5=0$（$\vartheta_5=0,\pi$，腕 4、6 轴共线，绕垂直于该轴方向的角速度自由度丢失，属内部奇点、轨迹规划必须避开）、肘奇点 $s_3=0$（边界奇点）、肩奇点 $a_2c_2+a_3c_{23}=0$（腕心落到第一关节轴上，$J$ 第一列整列为零、逆解无穷多）。判据落地：分拣轨迹要检查整条路径上 $\det J=\det J_{11}\det J_{22}$ 是否远离零；弧焊轨迹要在焊缝两侧比对任务速度所需的最坏关节速度（由 $\bar J^{-1}$ 放大后的量）是否仍在电机与限速内。注意：$\det J\neq0$ 但很小时，$\bar J^{-1}=\operatorname{adj}\bar J/\det\bar J$ 的元素已经巨大——边界奇点附近的邻域同样要规划规避。

(c) 「结构先于算法」的落点（3 分）。 分拣要的是「单位时间走得远、反复起停」，这要求末端速度在任意方向都能生成——于是需要远离奇点且关节行程短；弧焊要的是「末端沿指定几何曲线匀速」，这要求执行空间有直线型路径基元且姿态连续可规划。两者的可行性边界由构形（球形腕给的解耦、拟人臂给的工作空间形状与精度分布）划定：算法只能在结构给定的可达 / 灵活工作空间与精度分布内优化，改不了分布本身。所以「同一台臂两用」成立的范围是「两组任务的点集都落在同一副结构的可用区间内」；一旦有一组要求落在灵活工作空间之外、或必须经过内部奇点，答案就是换构型（如并联、SCARA 或加大规格），而不是换算法。

评分建议：多支性与工作空间 3.5 分（四组解 + 腕两支 + 工作空间这一层）；奇点与限速 3.5 分（三处奇点至少写两处，并说明内部 / 边界奇点的处置差别）；「结构先于算法」的落点 3 分（须给出一个可判定的分界线）。只列名词不作论证的，最多 4 分。

20.（10 分）

五步提纲（5 分）

1. 算腕心——输入：末端位置 $\boldsymbol p_e$ 与姿态 $R_e=[\boldsymbol n_e\ \boldsymbol s_e\ \boldsymbol a_e]$；运算 $\boldsymbol p_w=\boldsymbol p_e-d_6\boldsymbol a_e$；输出：腕心位置。（3 分之一的得分点在这一步：必须扣 $d_6$。）
2. 解臂——输入 $\boldsymbol p_w$；对球形臂用半角代换 $t=\tan(\vartheta_1/2)$ 化为二次方程，判别式 $p_{wx}^2+p_{wy}^2\ge d_2^2$ 是解存在的几何条件；输出 $(\vartheta_1,\vartheta_2,d_3)$（两支）。
3. 算中间旋转矩阵——输入 $(\vartheta_1,\vartheta_2,\vartheta_3)$ 或 $(\vartheta_1,\vartheta_2,d_3)$；运算 $R_3^0=R_1^0R_2^1R_3^2$；输出 $R_3^0$。
4. 方向分离——运算 $R_6^3=(R_3^0)^{\mathrm T}R_e$；输出腕的相对姿态矩阵（这一步是全部分工的枢纽）。
5. 解腕——把 $R_6^3=[\boldsymbol n'\ \boldsymbol s'\ \boldsymbol a']$ 的三个列元素代入 ZYZ 反解，输出 $(\vartheta_4,\vartheta_5,\vartheta_6)$（两支）。

为什么第 5 步是 ZYZ 反解（2 分）：球形腕的三个关节依次绕 $z$、$y'$、$z''$（当前轴）转动，其旋转块与 ZYZ 合成矩阵 $R_z(\vartheta_4)R_{y'}(\vartheta_5)R_{z''}(\vartheta_6)$ 逐元素相同——这是 2.9.5 从正运动学结果里直接读出来的结构性发现，于是腕的逆解就是套用 定理 2.4.B 的现成公式：$\vartheta_4=\operatorname{Atan2}(a_2',a_1')$、$\vartheta_5=\operatorname{Atan2}(\sqrt{a_1'^2+a_2'^2},a_3')$、$\vartheta_6=\operatorname{Atan2}(s_3',-n_3')$（第一支）。没有这条发现，第 5 步就得从头解一个三元非线性方程组。

两处工程细节（3 分）

① 多支解整理：臂给两支（$\vartheta_1$ 的双支）、腕给两支（$\vartheta_5$ 的正负）⇒ 至少 4 组可行解。处置理由：关节限位与路径连续性——把上一采样周期的解作为参考，选「与上一次最接近」的那一支，避免关节角在一支与另一支之间跳变（表现为轨迹抖动甚至撞机）。 ② 奇点处置：$s_5=0$ 时腕 4、6 轴共线，$\varphi$ 与 $\psi$ 不可分离，公式给出「不可分离」而不是数值。处置理由：表示奇点必须靠规划绕开或结构性回避——工程上有两条可用手段：降速通过（在奇点邻域自动降速，容忍较大的跟踪误差）与自动规避（不严格遵循设定轨迹，在邻域内微调路径），本质上都是「承认这里解不唯一，就别在这里要求严格跟随」；在算法层面则可用阻尼最小二乘（3.5.2）换回可解性。

评分建议：五步齐全 3 分；腕心公式与 $d_6$ 的角色 1 分；$R_6^3=(R_3^0)^{\mathrm T}R_e$ 与「ZYZ 反解」的理由 3 分；两处工程细节各 1.5 分。失分点：把第 4 步写成 $R_6^3=R_3^{0}R_e$（左乘），或漏掉「两支解」这一层。