逆运动学：解析与数值

你对机械臂下的命令是"手爪去那个杯子的把手，朝向这样"，电机能执行的却是"每个关节转到多少度"。7.3 节的正运动学 \(T(\mathbf q)\) 把关节角映成手爪位姿，是一个函数；逆运动学要反过来：给定想要的位姿 \(T_d\in\SE\)，求 \(\mathbf q\) 使 \(T(\mathbf q)=T_d\)。反方向的问题却不再是函数了：

• 可能无解：目标太远，或者落在手臂折不进去的"洞"里。
• 可能有有限多个解：同一个手爪位姿，手臂可以"肘上"也可以"肘下"，腕可以翻也可以不翻。
• 可能有无穷多个解：冗余臂能在手爪不动时改变形状（3.4 节的零空间），一些奇异位形下非冗余臂也会如此。

每个笛卡尔速度指令、每个抓取候选（一次要试成百上千个）、遥操作里的每一帧、规划器采样出的每个目标，都要先过 IK 这一关，所以它既要快，又要可靠：知道有没有解，知道有几个，知道现在该选哪一个。

两大流派各有所长。解析（闭式）IK 利用机构的几何，把方程解成公式，几微秒内给出全部解，但每种构型都要单独推导，只对满足条件的机构存在（本节的 Pieper 准则）。数值 IK 把 3.4 节的"线性化、解一步、重复"搬到 SE(3) 上，对任何机构都适用，但一次只给出一个解，依赖初值，并且可能失败。3.4 节已经在平面上用伪逆和阻尼最小二乘做过数值 IK，这里不再重复，而是：

• 先看为什么平面 2R 有两个解、解在哪里消失（FIG 1），以及位置与位姿约束如何决定解的"个数"与"维数"（FIG 2）；
• 再推导一台真实 6R 臂的闭式解，得到 8 个分支（FIG 3）；
• 然后在 SE(3) 上做数值 IK，看它怎样收敛、怎样失败，以及"初值决定去哪"（FIG 4、FIG 5）；
• 最后沿一条路径跟踪，看选错分支会让关节突然翻转（FIG 6）。

最小的例子：平面两连杆臂，连杆长 \(l_1,l_2\)，手爪要到点 \(\mathbf p=(x,y)\)。关节 1 在原点，所以肘部一定在以原点为圆心、半径 \(l_1\) 的圆上；手爪必须落在目标，所以肘部离目标的距离恰是 \(l_2\)，即肘部又在以目标为圆心、半径 \(l_2\) 的圆上。肘部同时在两个圆上，就是两个圆的交点。

两个圆交于几点，立刻读出解的个数：两点（"肘上"和"肘下"，关于原点到目标的连线互为镜像）、一点（两圆相切，手臂完全伸直或完全折回）、零点（太远，或落在内圈的"洞"里）。可达区域因此是一个圆环 \(|l_1-l_2|\le r\le l_1+l_2\)，\(r=\|\mathbf p\|\)。拖动目标试试；再用滑块改变 \(l_2\)，看圆环的"洞"怎样随两根连杆的长短变化。

"到那个点"与"到那个点，并且手爪朝这个方向"不是一回事。数一数未知数（关节数 \(n\)）和约束数（\(m\)：平面里位置 2 个，位姿 3 个）：\(n-m>0\) 时解是一个 \((n-m)\) 维的连续族（可以在手爪不动时"自运动"），\(n=m\) 时通常是有限个解，\(n<m\) 时一般无解。这正是 7.1 节的自由度计数在逆运动学里的样子，也是 3.4 节零空间的非线性版本：零空间是解集在一点处的切空间。

下图用 3R 或 4R 平面臂演示。右图把关节角画成自由参数的函数：3R 只要位置时，自由参数是手爪朝向 \(\varphi\)，每条曲线是一族连续解（实线肘上、虚线肘下）；要求位姿就是在 \(\varphi\) 处竖直切一刀，只剩下有限个点。切刀落在曲线的空隙里，就是"位置够得着、这个朝向却做不到"。

这里用的是 7.3、7.4 节同一台 Robot.arm6：J1 绕竖直轴偏航，J2、J3 是肩与肘的俯仰（正俯仰让 \(+x\) 方向向下倾，右手定则），J4–J6 是"滚转–俯仰–滚转"的腕，三根腕轴交于腕心 \(\mathbf p_w\)，工具原点在腕心前方 \(d_6\) 处。关键观察：腕心的位置只由前三个关节决定，不管手腕怎么转。于是六个未知数拆成两个小问题：

1. 位置：由目标位姿算出腕心 \(\hl{pw}{\mathbf p_w}=\mathbf p-d_6R\,\mathbf e_x\)，用它解 \(q_1,q_2,q_3\)。\(q_1\) 是腕心的方位角（有两个分支：\(q_1\) 与 \(q_1+\pi\)，手臂"从前面"够，或者 J1 转 180° 后从"身后"翻过来够）；然后在过肩部的竖直平面里，就是 FIG 1 的 2R 问题（肘上、肘下）。
2. 姿态：前三个关节已知后，剩下的旋转 \(R_3^6=(R_0^3)^\top R\) 必须由腕补上。腕是"滚转–俯仰–滚转"，把一个旋转矩阵分解成这三个角，有两个分支（\(q_5\) 的符号，即腕翻转）。

三个二选一，共 \(2\times2\times2=8\) 个解。下图把它们全部画出来：灰度淡的是越出关节限位的解，实心的是当前选中的解，表格给出每个解的关节角、FK 往返误差（把解代回正运动学，再与目标位姿求 SE(3) 误差）与限位检查。拖滑块改变目标位姿，或用"随机可达位姿"生成一个按构造一定可达的目标。

没有闭式解（或懒得推导）时，就用 3.4 节的老办法，只是把"位置误差"换成位姿误差。当前位姿 \(T(\mathbf q)\) 与目标 \(T_d\) 之间的误差，用 5.6 节的对数映射表示成机体系下的一个 twist：\(\mathbf e=\Log\big(T(\mathbf q)^{-1}T_d\big)\in\R^6\)（前三个分量是转角向量，后三个是平移）。它正好是 7.4 节机体雅可比 \(J_b\) 的"自然输入"：\(J_b\Delta\mathbf q\approx\mathbf e\)，解出 \(\Delta\mathbf q\)（伪逆或阻尼最小二乘），更新 \(\mathbf q\leftarrow\mathbf q+\Delta\mathbf q\)，重复。这就是 6.6 节的牛顿 / 高斯–牛顿法，只不过残差活在 SE(3) 上。

下图从 4 个不同的初值同时出发（编号 1–4，四种颜色），对同一个目标位姿迭代。拖动"迭代 k"或按播放，看手臂怎样走向目标，右侧是每次迭代的误差 \(\|\mathbf e\|\)（对数）。每一步的关节增量 \(\|\Delta\mathbf q\|\) 被限幅到 0.8 rad（相当于速度限制）。四种目标位姿分别演示：正常收敛、手臂伸直的奇异位形、刚好够不着、远超范围。

FIG 4 里同一个目标，从不同初值走向不同的解，有些初值根本不收敛。把这一点画清楚最容易用平面 2R：构型空间只有 \((\theta_1,\theta_2)\) 两维，可以给每一个初值着色，颜色是牛顿迭代最终落到的解。下图右侧就是这张"吸引域地图"：青色 = 肘上，紫色 = 肘下，红色 = 没有收敛。两个解是地图上带描边的圆点。点击地图中任一点，作为初值，左边的手臂就沿着迭代路径走过去。虚线是 \(\theta_2=0,\pm\pi\)（雅可比奇异的位置），吸引域的边界总是沿着它们"撕开"。

一般的 6R 臂，逆运动学是 6 个耦合的三角方程。Pieper（1968）发现：若三个相邻关节的轴交于一点（典型情形是球腕），位置与姿态解耦，方程组分裂成两个小问题，闭式解存在，且至多 8 个解。原因很直接：这个交点（腕心）的位置只由它之前的关节决定，不受腕转动的影响，所以

用 PoE（5.10 节）读出这台臂的结构：轴依次是 \(\hat z,\hat y,\hat y,\hat x,\hat y,\hat x\)，所以旋转部分就是

第 1 步，肩。 \(x_w^2+y_w^2=\rho^2\)，所以 \(q_1=\operatorname{atan2}(y_w,x_w)\)（此时 \(\rho=+\sqrt{x_w^2+y_w^2}\)），或 \(q_1=\operatorname{atan2}(y_w,x_w)+\pi\)（\(\rho=-\sqrt{x_w^2+y_w^2}\)）。

第 2 步，肘与肩的俯仰。 记 \(h=z_w-d_1\)。\((\rho,h)\) 是一个平面 2R 问题（FIG 1），余弦定理给出

第 3 步，腕。 前三个关节已知，\(W=R_3^6=\big(R_z(q_1)R_y(q_2+q_3)\big)^\top R\)。把 \(R_x(q_4)R_y(q_5)R_x(q_6)\) 乘开，记 \(a,b,c=q_4,q_5,q_6\)：

第一行和第一列只含 \(\sin b\) 的乘积，所以直接读出。第二个分支取 \(\sin q_5<0\)：\(q_5\to-q_5\)，\(q_4\to\operatorname{atan2}(-W_{21},W_{31})\)，\(q_6\to\operatorname{atan2}(-W_{12},-W_{13})\)。这就是 FIG 3 中的腕翻转：同一个旋转矩阵的两种欧拉角分解。

奇异情形必须单独处理。 (i) 肩奇异：\(x_w=y_w=0\)，腕心在 J1 轴上，\(q_1\) 任意（取上一时刻的值保持连续）；(ii) 肘奇异：\(\cos q_3=\pm1\)，手臂完全伸直或折回，两个肘分支合并，且 \(r\) 的小变化需要 \(q_3\) 大变化；(iii) 腕奇异：\(\sin q_5=0\)，J4 与 J6 的轴共线，只有 \(q_4+q_6\)（\(q_5=0\)）或 \(q_6-q_4\)（\(q_5=\pi\)）确定，其余是一个自由度。代码里的每个分支都对应这三种情形之一。

怎么知道它是对的？ 用 FK 往返：任取 \(\mathbf q\)，算 \(T=\operatorname{FK}(\mathbf q)\)，求 IK，检查（1）返回的每个解代回 FK 都等于 \(T\)；（2）原来的 \(\mathbf q\) 在解集中。作者在 Node 里对 20000 个关节限位内的随机 \(\mathbf q\) 做了这个检验：每次都恰好返回 8 个解，全部满足 FK = 目标，误差不超过 \(6\times10^{-13}\)，\(\mathbf q\) 无一遗漏；奇异位形（上面三种）同样通过，只是解的个数减少。FIG 3 的表格每次都在实时重复这个检验。

球腕 6R 臂：肩 2 × 肘 2 × 腕 2 = 8（FIG 3）。一般的 6R 臂（没有交点）最多 16 个解：Raghavan–Roth（1993）把问题消元成关于某个关节半角正切的一个 16 次多项式，其中的实根就是实解。冗余臂（\(n>6\)）有连续的解族，维数 \(n-6\)；7R 臂对完整位姿的解是 1 维的曲线（肘部绕"肩–腕"连线转动的"摆动角"），常见做法是把这个自由参数固定或优化，再套闭式解。

这是对 \(\tfrac12\|\mathbf e(\mathbf q)\|^2\) 的高斯–牛顿法（6.6、6.9 节）。机体系下的误差 \(\mathbf e\) 与机体雅可比 \(J_b\)（7.4 节）配对；若误差写在空间系里，就改用 \(J_s\)。\(\mathbf e\) 对 \(\mathbf q\) 的精确导数是 \(-J_l^{-1}(\mathbf e)\,J_b\)，其中 \(J_l\) 是 SE(3) 的左雅可比，它与单位阵相差 \(O(\|\mathbf e\|)\)；所以用 \(-J_b\) 代替它，迭代在解附近仍是二次收敛（FIG 4 里的 \(10^{-2}\to10^{-4}\to10^{-8}\)），前提是 \(J_b\) 满秩。远离解时不保证任何事：这就是 FIG 5 的吸引域，需要限幅、回溯线搜索（6.5 节）或阻尼（Levenberg–Marquardt）来保护。阻尼只改变路径，不改变不动点（\(\mathbf e=\mathbf 0\)），但在 \(J_b\) 降秩时会停在误差平台上（FIG 4 的"伸直"目标）。

量纲与权重。 \(\mathbf e\) 的前三个分量是弧度，后三个是米。"1 弧度"与"1 米"并不等价，所以把行加权：\(\min_{\Delta\mathbf q}\|W(J_b\Delta\mathbf q-\mathbf e)\|^2+\lambda^2\|\Delta\mathbf q\|^2\)，\(W=\operatorname{diag}(w_\omega I_3,\,w_vI_3)\)。典型做法是取 \(w\) 为各自的容差的倒数（如 1 mm 对应 \(w_v=10^3\)，0.1° 对应 \(w_\omega\approx570\)）。位置优先的任务则把 \(w_\omega\) 取得更小。

闭式解先求全部解，再按关节限位、碰撞、离当前构型的距离筛选；数值 IK 则要把限位放进迭代：最粗糙的是把 \(\mathbf q\) 截断到限位内（容易卡住），更好的是把每一步写成带不等式约束的 QP：\(\min\|J_b\Delta\mathbf q-\mathbf e\|^2+\lambda^2\|\Delta\mathbf q\|^2\)，\(\mathbf q_{\min}\le\mathbf q+\Delta\mathbf q\le\mathbf q_{\max}\)（6.8 节）。冗余臂可以像 3.4 节那样把次要目标（远离限位、保持肘部朝向）投影到零空间 \((I-J^+J)\mathbf z\)。再往前一步是把整个 IK 当作一个非线性优化问题（6.9 节），代价和约束写成什么都行（可达、碰撞距离、姿态、平滑），用 SQP/内点法求解；Part VI 还会看到用神经网络直接给出 IK 的初值或解。

IK 在机器人里最常见的用法不是"解一次"，而是每个控制周期解一次：笛卡尔直线运动、遥操作、视觉伺服都是这样。这时候最重要的不是解得对不对，而是前后两次选的是不是同一个分支。下图让工具沿一条路径运动，比较三种策略：

• 固定分支：永远取起点所在的那个（肩、肘、腕）分支；
• 最近的解：闭式解的 8 个解里，取离上一时刻关节角最近的（不管限位）；
• 数值 IK 热启动：以上一时刻的 \(\mathbf q\) 为初值迭代，天然连续。

两条路径都经过奇异位形。"手爪转过腕奇异"：腕心不动，工具朝向转过 \(q_5=0\)；"手腕越过底座上方"：腕心在水平面里从 J1 轴一侧移到另一侧，经过肩奇异。红色竖线标出关节突变（单步超过 0.5 rad）的位置。