2.12 逆运动学问题

正运动学是函数计算，逆运动学则是解方程：给定 $(\boldsymbol p_e,R_e)$ 求 $\boldsymbol q$。解方程比算函数难得多，难处集中在四条——非线性、多解、可能无解 / 无穷多解、无通用解析法。本节把课程覆盖的解法全部推导一遍：先讲平面臂（代数 + 几何双法），再讲球形腕解耦框架（含球形臂、拟人臂、球形腕三个子问题）。录播 1:55:29–2:13:16。

2.12.1 问题有多难：四难点与存在性

存在性判据很直接：位姿落在灵活工作空间内，解就必然存在（原书）。难的是怎么求。录播 1:57:29 老师把解法版图讲得直白：「正运动学不管什么机械臂都能算；逆运动学没有通用（解析）方法，只能针对特殊构型」——数值法虽然通用，却得不到全部解（见第 3 章）。既然通用解析法走不通，闭合解就只能在特殊构型上做文章，靠的是两种直觉（原书）：代数直觉（找含未知量的关键方程）与几何直觉（找化简未知量的结构关键点）。

2.12.2 三连杆平面臂的求解

要看清逆解怎么做，先从最简单的三连杆平面臂入手。任务很明确：给定 $(p_x,p_y,\phi)$（末端位置与方向角），求 $(\vartheta_1,\vartheta_2,\vartheta_3)$（原书 §2.12.1）。这里有个前提值得注意：若不给方向角 $\phi$，臂就变成冗余的，逆解有无穷多（原书脚注）。

代数解法的思路是「先分离腕心，再平方相加消元」，一共五步（录播 1:58:31：「没有什么技巧……就是简单的三角关系」）：

1. 方向方程 $\phi=\vartheta_1+\vartheta_2+\vartheta_3$；
2. 腕心分离：腕心 $W$（末梢沿末杆回退 $a_3$ 处，即坐标系 2 原点）只依赖前两角： $$p_{wx}=p_x-a_3c_\varphi=a_1c_1+a_2c_{12},\qquad p_{wy}=p_y-a_3s_\varphi=a_1s_1+a_2s_{12};$$
3. 平方相加消 $\vartheta_1$（存在性要求 $-1\le c_2\le 1$，否则点在工作空间外）： $$c_2=\frac{p_{wx}^2+p_{wy}^2-a_1^2-a_2^2}{2a_1a_2},\qquad s_2=\pm\sqrt{1-c_2^2},\qquad \vartheta_2=\operatorname{Atan2}(s_2,c_2);$$ （$s_2$ 正号肘朝下、负号肘朝上）
4. 把 $c_2,s_2$ 代回腕心方程，解这个线性组： $$s_1=\frac{(a_1+a_2c_2)p_{wy}-a_2s_2,p_{wx}}{p_{wx}^2+p_{wy}^2},\qquad c_1=\frac{(a_1+a_2c_2)p_{wx}+a_2s_2,p_{wy}}{p_{wx}^2+p_{wy}^2},\qquad \vartheta_1=\operatorname{Atan2}(s_1,c_1);$$
5. 回代 $\vartheta_3=\phi-\vartheta_1-\vartheta_2$。

还要注意 $s_2=0$（两杆共线、臂伸直）的情形：此时机械臂处于运动学奇异（第 3 章），两支解合一，但 $\vartheta_1$ 仍然唯一——除非同时满足 $a_1=a_2$ 且 $\boldsymbol p_w=\boldsymbol 0$（原书）。

几何解法走的是另一条路：先看图形，再列式子（录播 2:01:10：「这是个三角形……用余弦公式」）。$a_1,a_2$ 与线段 $OW$ 构成一个三角形，对边用余弦定理，得到与代数法相同的 $c_2$（$\vartheta_2=\pm\arccos c_2$，负肘上 / 正肘下）。设 $\alpha=\operatorname{Atan2}(p_{wy},p_{wx})$，则 $$\beta=\arccos\frac{p_{wx}^2+p_{wy}^2+a_1^2-a_2^2}{2a_1\sqrt{p_{wx}^2+p_{wy}^2}}\ \big(\beta\in(0,\pi)\big),\qquad \vartheta_1=\alpha\pm\beta\quad(\vartheta_2<0\text{ 取}+,\ \vartheta_2>0\text{ 取}-);$$ 最后同样 $\vartheta_3=\phi-\vartheta_1-\vartheta_2$。两种解法给出的解数一致，但几何法多给出一层图像：「双支 = 三角形镜像」（原书图 2.31）；同样的思路也适用于运动学等价的平行四边形臂（原书 §2.9.2）。

「双支 = 三角形镜像」这句话可以直接画出来——就长下图这样：同一个腕心 $W$、同一个末端方向 $\varphi$，两组两连杆三角形一上一下互为镜像，而 $\vartheta_1=\alpha\pm\beta$ 的两个符号选择，就发生在这里。

看静态图不过瘾？下面的操作台可以直接拖：改变两个关节角，末端、肘部、可达工作空间与「同一 P 的另一支解」会全部实时联动。试试把 $c_2$ 拖到 $\pm1$ 附近，看两支解如何合并成奇异。

2.12.3 解耦：带球形腕机械手的求解

解耦的关键，是把一个 6 元问题拆成两个 3 元子问题，让位置与方向分离（「位置解和方向解被解耦」，原书）。操作上分五步：

这个框架并不限定臂的具体形态：只要结构满足上述条件，位置与方向就能分开处理——下面两小节分别给出球形臂与拟人臂两个实例。

2.12.4 球形臂的求解

先看球形臂。给定腕心位置 $\boldsymbol p_w$，求 $(\vartheta_1,\vartheta_2,d_3)$（原书 §2.12.3，正运动学见 2.9.3 式 2.65）。为了把变量分离开，先把 $\boldsymbol p_w$ 移到系 1 中表达（左乘 $A_0^1$），再令它与 $A_2^1A_3^2$ 的第 4 列相等：

$$\begin{bmatrix}p_{wx}c_1+p_{wy}s_1\ -p_{wz}\ -p_{wx}s_1+p_{wy}c_1\end{bmatrix}=\begin{bmatrix}d_3s_2\ -d_3c_2\ d_2\end{bmatrix}\qquad(\text{原书式 }2.94)$$

求解分三步，逐一拿下 $\vartheta_1$、$d_3$、$\vartheta_2$：

1. 第三行只含 $\vartheta_1$：$-p_{wx}s_1+p_{wy}c_1=d_2$。作半角代换 $t=\tan(\vartheta_1/2)$（$c_1=\frac{1-t^2}{1+t^2},\ s_1=\frac{2t}{1+t^2}$），化为二次方程

$$(d_2+p_{wy}),t^2+2p_{wx},t+(d_2-p_{wy})=0 \ \Longrightarrow\ t=\frac{-p_{wx}\pm\sqrt{p_{wx}^2+p_{wy}^2-d_2^2}}{d_2+p_{wy}},$$

即两支解（对应两种肩姿态）

$$\vartheta_1=2\operatorname{Atan2}!\big(-p_{wx}\pm\sqrt{p_{wx}^2+p_{wy}^2-d_2^2},\ p_{wy}+d_2\big).$$

判别式 $p_{wx}^2+p_{wy}^2\ge d_2^2$ 是解存在的几何条件——腕心到 $z_0$ 轴的距离不得小于 $|d_2|$，否则判别式为负、解不存在。

2. 求 $d_3$：$\vartheta_1$ 已知后，对前两行的平方求和（右端 $s_2^2+c_2^2=1$，左端第二行恰为 $-p_{wz}$）：

$$d_3=\sqrt{(p_{wx}c_1+p_{wy}s_1)^2+p_{wz}^2}\qquad(\text{只取 } d_3>0\ \text{支});$$

$\vartheta_1$ 的两支给出同一个 $d_3$。

3. 求 $\vartheta_2$（$d_3\neq0$）：$s_2=(p_{wx}c_1+p_{wy}s_1)/d_3$、$c_2=p_{wz}/d_3$，故

$$\vartheta_2=\operatorname{Atan2}\big(p_{wx}c_1+p_{wy}s_1,\ p_{wz}\big).$$

若 $d_3=0$，$\vartheta_2$ 不能唯一确定（伸缩归零后第二关节的指向不再影响腕心位置）。

2.12.5 拟人臂的求解

再看拟人臂。给定腕心 $\boldsymbol p_w$，求 $(\vartheta_1,\vartheta_2,\vartheta_3)$（正运动学见 2.9.4 式 2.66）。腕心方程的三个分量如下，后面按 $\vartheta_3\to\vartheta_2\to\vartheta_1$ 的顺序逐个解出：

$$p_{wx}=c_1(a_2c_2+a_3c_{23}),\qquad p_{wy}=s_1(a_2c_2+a_3c_{23}),\qquad p_{wz}=a_2s_2+a_3s_{23}.$$

1. 三式平方相加（前两式的平方和 + 第三式的平方）消 $\vartheta_1,\vartheta_2$： $$c_3=\frac{p_{wx}^2+p_{wy}^2+p_{wz}^2-a_2^2-a_3^2}{2a_2a_3}\qquad(\text{原书式 }2.98)$$ 容许性条件 $-1\le c_3\le 1$，等价于 $|a_2-a_3|\le|\boldsymbol p_w|\le a_2+a_3$（超出即腕心在可达工作空间外）。$s_3=\pm\sqrt{1-c_3^2}$，$\vartheta_3^{\mathrm{I}}=\operatorname{Atan2}(s_3,c_3)$，$\vartheta_3^{\mathrm{II}}=-\vartheta_3^{\mathrm{I}}$（原书式 2.99–2.101）——肘上 / 肘下。
2. 前两式平方和给出 $a_2c_2+a_3c_{23}=\pm\sqrt{p_{wx}^2+p_{wy}^2}$（原书式 2.102），与 $p_{wz}$ 式联立，对每支 $\vartheta_3$ 解出（原书式 2.103–2.104） $$c_2=\frac{\pm\sqrt{p_{wx}^2+p_{wy}^2},(a_2+a_3c_3)+p_{wz}a_3s_3}{a_2^2+a_3^2+2a_2a_3c_3},\qquad s_2=\frac{p_{wz}(a_2+a_3c_3)\mp\sqrt{p_{wx}^2+p_{wy}^2},a_3s_3}{a_2^2+a_3^2+2a_2a_3c_3},\qquad \vartheta_2=\operatorname{Atan2}(s_2,c_2);$$
3. 最后由前两式的结构 $p_{wx}=\pm c_1\sqrt{p_{wx}^2+p_{wy}^2},\ p_{wy}=\pm s_1\sqrt{p_{wx}^2+p_{wy}^2}$： $$\vartheta_{1}^{\mathrm{I}}=\operatorname{Atan2}(p_{wy},p_{wx}),\qquad \vartheta_1^{\mathrm{II}}=\operatorname{Atan2}(-p_{wy},-p_{wx})$$ （二者相差 $\pi$；原书式 2.109–2.110）——肩左 / 肩右。$\sqrt{\cdot}=0$（$p_{wx}=p_{wy}=0$）时 $\vartheta_1$ 任意（无穷多解，臂处于肩奇异，原书习题 2.18）。

2.12.6 球形腕的求解

最后是腕。给定方向 $R_6^3$，求 $(\vartheta_4,\vartheta_5,\vartheta_6)$。2.9.5 留下的结构性发现在这里兑现：$R_6^3$ 就是 ZYZ 合成矩阵（原书式 2.18）。把它的三个列向量元素记为 $\boldsymbol n_6^3=[n_1',n_2',n_3']^T$、$\boldsymbol s_6^3=[s_1',s_2',s_3']^T$、$\boldsymbol a_6^3=[a_1',a_2',a_3']^T$，逆解就可直接套用 2.4.4 的式 2.19–2.20（原书式 2.111–2.112），按 $\vartheta_5$ 的正负分成两支：

$$\vartheta_5\in(0,\pi):\qquad \vartheta_4=\operatorname{Atan2}(a_2',a_1'),\qquad \vartheta_5=\operatorname{Atan2}!\big(\sqrt{a_1'^2+a_2'^2},\ a_3'\big),\qquad \vartheta_6=\operatorname{Atan2}(s_3',-n_3');$$

$$\vartheta_5\in(-\pi,0):\qquad \vartheta_4=\operatorname{Atan2}(-a_2',-a_1'),\qquad \vartheta_5=\operatorname{Atan2}!\big({-\sqrt{a_1'^2+a_2'^2}},\ a_3'\big),\qquad \vartheta_6=\operatorname{Atan2}(-s_3',n_3').$$

录播 2:11:14–2:12:36 老师：「整个 $R_6^3$ 的旋转矩阵都是已知的，把它带进去就可以计算出来……就跟前面（ZYZ 逆解）一样」——这正是解耦框架的价值：把「难的 6 关节问题」拆成了「平面臂式三角题 + 查表式 ZYZ 反解」。

2.12 小结与常见错误

• 一张决策树：任务位姿 → 属于灵活工作空间？（无解预警）→ 结构满足解耦条件（定理 2.12.B）？→ 是：算 $\boldsymbol p_w$、拆臂 + 腕；否：试几何直觉、再退到第 3 章数值法。
• 三个「多解来源」：肘（$s_2$ / $s_3$ 符号）、肩（$\vartheta_1$ 加 $\pi$）、腕（$\vartheta_5$ 符号）——选解准则 + 关节限位在工程上裁决。
• 一个习惯：每解出一支，都回代正运动学验证——本章公式都很长，抄错一个下标并不奇怪。