作业二：教材习题 2-4 / 2-12 / 2-19（附详细解答与验算）

三题正好串成本章主线的一条链：2-4 验证姿态表示的核心公式 → 2-12 用 DH 法建立正运动学 → 2-19 反过来求逆解。下文每题给「题目 → 思路 → 详细推导 → 结果 → 验算 → 易错点」，全部结论已与官方解答手册（Solution Manual, Springer 2009，Problem 2.4 / 2.12 / 2.19）逐项核对，并通过 2 万组随机数值实验。

2-4 · 验证绕任意轴旋转的旋转矩阵由式（2.25）给出

题目原文（教材 p.75 第 2.4 题，逐字）：

2.4 验证绕任意轴旋转一定角度所对应的旋转矩阵由式（2.25）给出。

思路：式（2.25）是结论，式（2.24）的五步合成是已知。所以只需把五步合成算到底，看是否等于式（2.25）。关键是三步：① 把五步写成「相似变换」$MR_z(\vartheta)M^T$；② 认出 $M$ 的第三列恰是旋转轴 $\boldsymbol r$；③ 证明反对称部分等于叉积矩阵 $[\boldsymbol r]_\times$。

详细推导

第 0 步：把 α、β 用 r 的分量写出。 由式（2.24）的几何（图 2.10），$\boldsymbol r$ 的球坐标分解给出

$$\sin\alpha=\frac{r_y}{\sqrt{r_x^2+r_y^2}},\quad \cos\alpha=\frac{r_x}{\sqrt{r_x^2+r_y^2}},\qquad \sin\beta=\sqrt{r_x^2+r_y^2},\quad \cos\beta=r_z .\tag{1}$$

第 1 步：合并前两个因子。 记

$$M=R_z(\alpha)R_y(\beta)=\begin{bmatrix}c_\alpha c_\beta & -s_\alpha & c_\alpha s_\beta\ s_\alpha c_\beta & c_\alpha & s_\alpha s_\beta\ -s_\beta & 0 & c_\beta\end{bmatrix},$$

则式（2.24）可写成相似变换（因为 $R_y(-\beta)R_z(-\alpha)=M^T$）：

$$R(\vartheta,\boldsymbol r)=M,R_z(\vartheta),M^T .\tag{2}$$

第 2 步：认出 $M$ 的第三列就是 $\boldsymbol r$。 把式（1）代入 $M$ 的第三列：

$$M_{:,3}=\begin{bmatrix}c_\alpha s_\beta\ s_\alpha s_\beta\ c_\beta\end{bmatrix} =\begin{bmatrix}\dfrac{r_x}{\sqrt{r_x^2+r_y^2}}\cdot\sqrt{r_x^2+r_y^2}\[2mm] \dfrac{r_y}{\sqrt{r_x^2+r_y^2}}\cdot\sqrt{r_x^2+r_y^2}\[2mm] r_z\end{bmatrix} =\begin{bmatrix}r_x\ r_y\ r_z\end{bmatrix}=\boldsymbol r .$$

这就是「绕 $\boldsymbol r$ 转」被编码进 $M$ 的地方——$M$ 正是把 $\boldsymbol z$ 轴转到 $\boldsymbol r$ 的那次旋转。

第 3 步：展开相似变换。 记 $M=[\boldsymbol m_1\ \boldsymbol m_2\ \boldsymbol m_3]$（按列分块）。由 (2)：

$$R = M R_z(\vartheta) M^T = \big[c_\vartheta\boldsymbol m_1+s_\vartheta\boldsymbol m_2,\ -s_\vartheta\boldsymbol m_1+c_\vartheta\boldsymbol m_2,\ \boldsymbol m_3\big]M^T = c_\vartheta(\boldsymbol m_1\boldsymbol m_1^T+\boldsymbol m_2\boldsymbol m_2^T)+s_\vartheta(\boldsymbol m_2\boldsymbol m_1^T-\boldsymbol m_1\boldsymbol m_2^T)+\boldsymbol m_3\boldsymbol m_3^T .$$

因 $M$ 正交，$\boldsymbol m_1\boldsymbol m_1^T+\boldsymbol m_2\boldsymbol m_2^T=I-\boldsymbol m_3\boldsymbol m_3^T=I-\boldsymbol r\boldsymbol r^T$，于是

$$R(\vartheta,\boldsymbol r)=c_\vartheta I+(1-c_\vartheta),\boldsymbol r\boldsymbol r^T+s_\vartheta\underbrace{(\boldsymbol m_2\boldsymbol m_1^T-\boldsymbol m_1\boldsymbol m_2^T)}_{=:,S}.\tag{3}$$

第 4 步：证明 $S=[\boldsymbol r]_\times$。 由 $\boldsymbol m_1=[c_\alpha c_\beta,\ s_\alpha c_\beta,\ -s_\beta]^T$、$\boldsymbol m_2=[-s_\alpha,\ c_\alpha,\ 0]^T$ 逐元素计算：

即 $S=[\boldsymbol r]_\times=\begin{bmatrix}0&-r_z&r_y\ r_z&0&-r_x\ -r_y&r_x&0\end{bmatrix}$，其余对角元与 (2,2)、(3,3) 同理为零。

第 5 步：展开得式（2.25）。 把 (3) 写成分量：

$$R(\vartheta,\boldsymbol r)=\begin{bmatrix} r_x^2(1-c_\vartheta)+c_\vartheta & r_xr_y(1-c_\vartheta)-r_zs_\vartheta & r_xr_z(1-c_\vartheta)+r_ys_\vartheta\ r_xr_y(1-c_\vartheta)+r_zs_\vartheta & r_y^2(1-c_\vartheta)+c_\vartheta & r_yr_z(1-c_\vartheta)-r_xs_\vartheta\ r_xr_z(1-c_\vartheta)-r_ys_\vartheta & r_yr_z(1-c_\vartheta)+r_xs_\vartheta & r_z^2(1-c_\vartheta)+c_\vartheta \end{bmatrix}$$

正是式（2.25）。□

验算

2 万组随机 $(\vartheta,\boldsymbol r)$（$\boldsymbol r$ 随机方向、$\vartheta\in(-\pi,\pi)$），比对式（2.25）与式（2.24）五步合成、以及手册的 $(s_\alpha,c_\alpha,s_\beta,c_\beta)$ 形式：

2-12 · 圆柱形臂的正运动学

题目原文（教材 p.75 第 2.12 题，逐字）：

2.12 列出如图 2.35 中的圆柱形臂的正运动学方程。

原题图 2.35（教材扫描，p.75）：

结构：关节 1 绕竖直轴 $z_0$ 转动（$\vartheta_1$）；关节 2 沿竖直方向移动（$d_2$）；关节 3 沿水平径向移动（$d_3$）。末端位置由柱坐标 $(d_3,\vartheta_1,d_2)$ 决定——这正是「圆柱形」得名的原因。

详细推导

第 1 步：建系。 按 DH 建系四规则 的四规则建连杆坐标系：$z_0$ 沿关节 1 转轴（竖直）；$z_1$ 沿关节 2 的移动方向（竖直，与 $z_0$ 同向）；因关节 3 沿水平径向移动，取 $z_2$ 沿该径向、$x_2$ 与之配合（故 $\alpha_2=-\pi/2$）。

对照官方解答手册的建系结果（Fig. S2.2，可见各系的 $x_i,y_i,z_i$ 与 $\vartheta_1,d_2,d_3$ 标注）：

第 2 步：填 DH 参数表。

读法：$a_i$ 全为 0（相邻 $z$ 轴皆相交或平行、公垂线长度为零——总原则：让参数尽量为零 的「能零则零」）；$\alpha_1=0$（$z_0\parallel z_1$）、$\alpha_2=-\pi/2$（$z_1\to z_2$ 由竖直转为水平）、$\alpha_3=0$（$z_2\parallel z_3$）；关节 1 是转动型故 $\vartheta_1$ 是变量，关节 2、3 是移动型故 $d_2,d_3$ 是变量、对应角列为常数 0。

第 3 步：写各关节变换矩阵（代入 $A_i^{i-1}$：从建系几何直接读出两步分解 的通式）:

$$A_1^0(\vartheta_1)=\begin{bmatrix}c_1&-s_1&0&0\ s_1&c_1&0&0\ 0&0&1&0\ 0&0&0&1\end{bmatrix},\quad A_2^1(d_2)=\begin{bmatrix}1&0&0&0\ 0&0&1&0\ 0&-1&0&d_2\ 0&0&0&1\end{bmatrix},\quad A_3^2(d_3)=\begin{bmatrix}1&0&0&0\ 0&1&0&0\ 0&0&1&d_3\ 0&0&0&1\end{bmatrix}.$$

（$A_2^1$ 是 $\vartheta_2=0$、$\alpha_2=-\pi/2$ 代入通式的结果：$c_2=1,s_2=0,c_{\alpha_2}=0,s_{\alpha_2}=-1$。）

第 4 步：连乘（原理 2.8.A）。先算后半段：

$$A_2^1A_3^2=\begin{bmatrix}1&0&0&0\ 0&0&1&d_3\ 0&-1&0&d_2\ 0&0&0&1\end{bmatrix},$$

再左乘 $A_1^0$：

$$\boxed{;T_3^0(\boldsymbol q)=A_1^0A_2^1A_3^2=\begin{bmatrix} c_1 & 0 & -s_1 & -d_3s_1\ s_1 & 0 & c_1 & d_3c_1\ 0 & -1 & 0 & d_2\ 0&0&0&1 \end{bmatrix},\qquad \boldsymbol q=[\vartheta_1\ d_2\ d_3]^T.;}$$

结果：末端执行器的位置与方向（$\boldsymbol n,\boldsymbol s,\boldsymbol a$ 为前三列）分别为

$$\boldsymbol p=\begin{bmatrix}-d_3s_1\ d_3c_1\ d_2\end{bmatrix},\qquad \boldsymbol n=\begin{bmatrix}c_1\ s_1\ 0\end{bmatrix},\quad \boldsymbol s=\begin{bmatrix}0\ 0\ -1\end{bmatrix},\quad \boldsymbol a=\begin{bmatrix}-s_1\ c_1\ 0\end{bmatrix}.$$

几何解读：长度 $d_3$ 的臂绕 $z_0$ 转 $\vartheta_1$ 后指到径向；注意零位（$\vartheta_1=0$）时 $p_x=0,p_y=d_3$——臂指向 $+y_0$，这是本建系约定的产物（$\boldsymbol p=[0,d_3,0]^T$ 是把 $[0,d_3]^T$ 逆时针转 $\vartheta_1$ 的结果）；$z$ 坐标只由 $d_2$ 决定，与两个水平变量解耦；$\boldsymbol s=-\boldsymbol z_0$ 说明末端坐标系的 $y$ 轴朝下。

验算

2-19 · 圆柱形臂的逆运动学

题目原文（教材 p.76 第 2.19 题，逐字）：

2.19 求解如图 2.35 中的圆柱形臂的逆运动学方程。

（原题图同 2-12 的图 2.35，见上。）

思路：正解位置式 $\boldsymbol p=[-d_3s_1,\ d_3c_1,\ d_2]^T$ 已在上题得到，逆解就是对它做「平方求和 + 取比 + 直读」三步——注意 $d_3$ 是移动关节的实际长度，有明确物理约束 $d_3\ge0$，这一条恰好把解定成唯一。

详细推导

第 1 步：分离 $d_2$。 第三式只含 $d_2$：

$$d_2=p_z .$$

第 2 步：平方求和消去 $\vartheta_1$。

$$p_x^2+p_y^2=d_3^2(s_1^2+c_1^2)=d_3^2 ;\Longrightarrow; d_3=\sqrt{p_x^2+p_y^2}\quad(\text{取 }d_3\ge0\ \text{支}).$$

第 3 步：作比得 $\vartheta_1$。 由 $\sin\vartheta_1=-p_x/d_3$、$\cos\vartheta_1=p_y/d_3$（$d_3>0$ 时两式可同除以 $d_3$）：

$$\vartheta_1=\operatorname{Atan2}!\left(\frac{-p_x}{d_3},\ \frac{p_y}{d_3}\right)=\operatorname{Atan2}(-p_x,\ p_y).$$

用 Atan2 而非 $\arccos$ 的理由见 原理 2.4.A（唯一化象限 + 精度均匀）。

验算

附：验算脚本

以下脚本可独立运行（需 NumPy），复现上表全部结论：

import numpy as np
rng = np.random.default_rng(20260916)

Rz = lambda t: np.array([[np.cos(t),-np.sin(t),0],[np.sin(t),np.cos(t),0],[0,0,1.]])
Ry = lambda t: np.array([[np.cos(t),0,np.sin(t)],[0,1.,0],[-np.sin(t),0,np.cos(t)]])

# —— 2-4：式(2.25) vs 五步合成   ——
def rodrigues(th, r):
    r = np.asarray(r,float)/np.linalg.norm(r)
    K = np.array([[0,-r[2],r[1]],[r[2],0,-r[0]],[-r[1],r[0],0]])
    return np.cos(th)*np.eye(3) + (1-np.cos(th))*np.outer(r,r) + np.sin(th)*K
def five(th, r):
    r = np.asarray(r,float)/np.linalg.norm(r)
    a = np.arctan2(r[1], r[0]); b = np.arctan2(np.hypot(r[0],r[1]), r[2])
    return Rz(a) @ Ry(b) @ Rz(th) @ Ry(-b) @ Rz(-a)
print(max(np.abs(rodrigues(t,r) - five(t,r)).max()
          for t,r in zip(rng.uniform(-np.pi,np.pi,20000), rng.normal(size=(20000,3)))))

# —— 2-12 / 2-19：圆柱形臂 ——
def A(a, al, d, th):
    c,s,ca,sa = np.cos(th),np.sin(th),np.cos(al),np.sin(al)
    return np.array([[c,-s*ca,s*sa,a*c],[s,c*ca,-c*sa,a*s],[0,sa,ca,d],[0,0,0,1.]])
def fk(q):
    t1,d2,d3 = q
    return (A(0,0,0,t1) @ A(0,-np.pi/2,d2,0) @ A(0,0,d3,0))
def fk_p(q):
    t1,d2,d3 = q; return np.array([-d3*np.sin(t1), d3*np.cos(t1), d2])
def ik(p):
    px,py,pz = p; d3 = np.hypot(px,py)
    return np.array([np.arctan2(-px,py) if d3>0 else np.nan, pz, d3])

q = [0.9, 0.35, 1.25]; print(fk(q)[:3,3], fk_p(q))          # 正解
q = [0.83, 0.2, 1.4];   print(ik(fk_p(q)))                  # 逆解回代
p_test = np.array([0.7, -1.1, 0.4])
print(np.abs(fk_p(ik(p_test)) - p_test).max())               # 逆→正 复现 p（≈0）

依据 Siciliano 等《机器人学：建模、规划与控制》第 2 章习题 2-4/2-12/2-19 · 与官方解答手册（Springer, 2009）逐项核对 · 数值验算通过 · 整理日期 2026-09-16