逆运动学：解析与数值Inverse Kinematics: Analytic & Numerical

你对机械臂下的命令是"手爪去那个杯子的把手，朝向这样"，电机能执行的却是"每个关节转到多少度"。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）。

The command you give an arm is "put the gripper on that mug's handle, oriented like this", while the motors can only execute "turn each joint to this angle". Forward kinematics \(T(\mathbf q)\) (Lesson 7.3) maps joint angles to a hand pose, and it is a function. Inverse kinematics runs it backwards: given a desired pose \(T_d\in\SE\), find \(\mathbf q\) with \(T(\mathbf q)=T_d\). The backwards problem is no longer a function:

• It may have no solution: the target is too far, or sits in a "hole" the arm cannot fold into.
• It may have finitely many: for one hand pose the arm can bend "elbow up" or "elbow down", and the wrist can flip or not.
• It may have infinitely many: a redundant arm can reshape itself while the hand stays put (the null space of Lesson 3.4), and even non-redundant arms do so at some singular postures.

Every Cartesian velocity command, every grasp candidate (hundreds are tried at once), every frame of teleoperation and every goal a planner samples has to pass through IK first. It therefore has to be fast and reliable: know whether a solution exists, how many there are, and which to choose now.

Two schools exist. Analytic (closed-form) IK exploits the mechanism's geometry, solves the equations as formulas, returns all solutions in microseconds, but must be derived per arm geometry and exists only for mechanisms meeting a condition (Pieper's criterion below). Numerical IK carries Lesson 3.4's "linearize, solve one step, repeat" over to SE(3); it works for any mechanism, but returns one solution at a time, depends on the initial guess, and can fail. Lesson 3.4 already did numerical IK for planar arms with the pseudo-inverse and damped least squares, so we do not repeat that. Instead:

• first see why the planar 2R has two solutions and where they disappear (FIG 1), and how position vs. pose constraints set the number and dimension of the solutions (FIG 2);
• then derive the closed-form solution of a real 6R arm and its 8 branches (FIG 3);
• then run numerical IK on SE(3) and watch it converge, fail, and "let the initial guess decide" (FIG 4, FIG 5);
• finally track a path and see what choosing the wrong branch does to the joints (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\)，看圆环的"洞"怎样随两根连杆的长短变化。

The smallest example: a planar two-link arm with link lengths \(l_1,l_2\) must bring the hand to \(\mathbf p=(x,y)\). Joint 1 sits at the origin, so the elbow lies on the circle of radius \(l_1\) around the origin. The hand must end at the target, so the elbow is exactly \(l_2\) from the target: it also lies on the circle of radius \(l_2\) around the target. The elbow is on both circles at once, so it is one of their intersection points.

The number of intersections reads off the number of solutions: two (elbow up and elbow down, mirror images across the line from the origin to the target), one (the circles touch: the arm is fully stretched or fully folded), or none (too far, or inside the inner "hole"). The reachable region is therefore an annulus \(|l_1-l_2|\le r\le l_1+l_2\), \(r=\|\mathbf p\|\). Drag the target; then change \(l_2\) with the slider and watch the hole change with the relative link lengths.

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

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

"Reach that point" and "reach that point with the hand pointing this way" are different problems. Count the unknowns (the number of joints \(n\)) and the constraints (\(m\): 2 for a planar position, 3 for a planar pose). If \(n-m>0\) the solutions form an \((n-m)\)-dimensional continuous family (the arm can "self-move" while the hand stays), if \(n=m\) there are generically finitely many, and if \(n<m\) there are generically none. This is Lesson 7.1's degrees-of-freedom count in its inverse-kinematics form, and the nonlinear version of Lesson 3.4's null space: the null space is the tangent space of the solution set at a point.

The figure uses a planar 3R or 4R arm. The right panel draws the joint angles as functions of the free parameter: for 3R with a position-only target the free parameter is the hand heading \(\varphi\), and each curve is a continuous family of solutions (solid: elbow up, dashed: elbow down). Asking for a pose means slicing the picture vertically at \(\varphi\), which leaves finitely many points. If the cut falls in a gap between the curves, the position is reachable but not with that heading.

这里用的是 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) 误差）与限位检查。拖滑块改变目标位姿，或用"随机可达位姿"生成一个按构造一定可达的目标。

This is the same Robot.arm6 as in Lessons 7.3 and 7.4: J1 yaws about the vertical axis, J2 and J3 are shoulder and elbow pitch (positive pitch tips the \(+x\) direction down, by the right-hand rule), and J4–J6 form a "roll–pitch–roll" wrist whose three axes meet at the wrist centre \(\mathbf p_w\); the tool origin is \(d_6\) ahead of it. The key observation: the position of the wrist centre depends only on the first three joints, however the wrist turns. The six unknowns therefore split into two small problems:

1. Position: from the target pose compute the wrist centre \(\hl{pw}{\mathbf p_w}=\mathbf p-d_6R\,\mathbf e_x\) and use it to solve \(q_1,q_2,q_3\). \(q_1\) is the azimuth of the wrist centre (two branches: \(q_1\) and \(q_1+\pi\): the arm reaches the target "from the front", or turns J1 by 180° and reaches "over its back"); then in the vertical plane through the shoulder it is FIG 1's 2R problem (elbow up, elbow down).
2. Orientation: with the first three joints known, the remaining rotation \(R_3^6=(R_0^3)^\top R\) must be supplied by the wrist. The wrist is "roll–pitch–roll", so decomposing a rotation matrix into these three angles has two branches (the sign of \(q_5\), the wrist flip).

Three binary choices give \(2\times2\times2=8\) solutions. The figure draws all of them: faint ones violate a joint limit, the solid one is the current selection, and the table lists each solution's joint angles, its FK round-trip error (substitute the solution into forward kinematics and measure the SE(3) error against the target) and the limit check. Move the sliders to change the target pose, or use "Random reachable" to get a target that is reachable by construction.

没有闭式解（或懒得推导）时，就用 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（相当于速度限制）。四种目标位姿分别演示：正常收敛、手臂伸直的奇异位形、刚好够不着、远超范围。

With no closed form (or no patience to derive one), use Lesson 3.4's recipe, only with a pose error in place of a position error. The error between the current pose \(T(\mathbf q)\) and the goal \(T_d\) is expressed with Lesson 5.6's logarithm as a body-frame twist: \(\mathbf e=\Log\big(T(\mathbf q)^{-1}T_d\big)\in\R^6\) (first three components: rotation vector, last three: translation). That is exactly the natural input of Lesson 7.4's body Jacobian \(J_b\): \(J_b\Delta\mathbf q\approx\mathbf e\). Solve for \(\Delta\mathbf q\) (pseudo-inverse or damped least squares), update \(\mathbf q\leftarrow\mathbf q+\Delta\mathbf q\), repeat. This is Lesson 6.6's Newton / Gauss–Newton method with the residual living on SE(3).

The figure starts four different initial guesses at once (numbered 1–4, four colours) and iterates them on the same target pose. Drag "iteration k" or press Play to see the arms walk toward the goal; the right panel shows the error \(\|\mathbf e\|\) (log scale) per iteration. Each step \(\|\Delta\mathbf q\|\) is clamped to 0.8 rad (a speed limit). Four targets demonstrate: normal convergence, the stretched-arm singularity, just out of reach, and far out of reach.

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

In FIG 4 the same target is reached from different starts at different solutions, and some starts do not converge at all. The planar 2R shows this most clearly: configuration space has just two dimensions \((\theta_1,\theta_2)\), so we can colour every initial guess by the solution Newton's iteration ends up at. The right panel is that "basin map": cyan = elbow-up, purple = elbow-down, red = no convergence. The two solutions are the outlined dots. Click anywhere on the map to use that point as the initial guess and watch the left-hand arm follow the iteration path. The dashed lines are \(\theta_2=0,\pm\pi\) (where the Jacobian is singular), and basin boundaries always "tear" along them.

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

For a general 6R arm, inverse kinematics is six coupled trigonometric equations. Pieper (1968) found that if three adjacent joint axes meet at a point (the typical case is a spherical wrist), position and orientation decouple: the system splits into two small problems with a closed-form solution and at most 8 solutions. The reason is direct: the intersection point (the wrist centre) is moved only by the joints before it, not by the wrist rotation, so

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

Read the structure off the PoE form (Lesson 5.10): the axes are \(\hat z,\hat y,\hat y,\hat x,\hat y,\hat x\), so the rotation part is simply

第 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），余弦定理给出

Step 1, shoulder. \(x_w^2+y_w^2=\rho^2\), so \(q_1=\operatorname{atan2}(y_w,x_w)\) (then \(\rho=+\sqrt{x_w^2+y_w^2}\)) or \(q_1=\operatorname{atan2}(y_w,x_w)+\pi\) (then \(\rho=-\sqrt{x_w^2+y_w^2}\)).

Step 2, elbow and shoulder pitch. Write \(h=z_w-d_1\). \((\rho,h)\) is a planar 2R problem (FIG 1), and the law of cosines gives

第 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\)：

Step 3, wrist. With the first three joints known, \(W=R_3^6=\big(R_z(q_1)R_y(q_2+q_3)\big)^\top R\). Multiplying out \(R_x(q_4)R_y(q_5)R_x(q_6)\) with \(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 的表格每次都在实时重复这个检验。

The first row and first column contain only a single product with \(\sin b\), so they can be read off directly. The second branch takes \(\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})\). That is FIG 3's wrist flip: two Euler-angle decompositions of the same rotation matrix.

Singular cases need their own handling. (i) Shoulder singularity: \(x_w=y_w=0\), the wrist centre is on the J1 axis and \(q_1\) is arbitrary (keep the previous value for continuity); (ii) Elbow singularity: \(\cos q_3=\pm1\), the arm is fully stretched or folded, the two elbow branches merge, and a small change of \(r\) needs a large change of \(q_3\); (iii) Wrist singularity: \(\sin q_5=0\), the J4 and J6 axes line up, and only \(q_4+q_6\) (for \(q_5=0\)) or \(q_6-q_4\) (for \(q_5=\pi\)) is determined, leaving one free parameter. Each special branch in the code corresponds to one of these three cases.

How do we know it is right? By an FK round trip: pick any \(\mathbf q\), compute \(T=\operatorname{FK}(\mathbf q)\), run IK, and check (1) every returned solution maps back to \(T\) under FK, and (2) the original \(\mathbf q\) is among the solutions. The author ran this in Node on 20,000 random \(\mathbf q\) inside the joint limits: every call returned exactly 8 solutions, all with FK = target to within \(6\times10^{-13}\), and the original \(\mathbf q\) was never missing; the singular configurations above pass too, with fewer solutions. The table in FIG 3 repeats the check live.

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

A spherical-wrist 6R arm: shoulder 2 × elbow 2 × wrist 2 = 8 (FIG 3). A general 6R arm (no intersection) has at most 16: Raghavan and Roth (1993) eliminated the unknowns down to a degree-16 polynomial in the tangent of a half-angle of one joint, whose real roots are the real solutions. Redundant arms (\(n>6\)) have a continuous family of dimension \(n-6\); for a 7R arm and a full pose it is a 1-dimensional curve (the elbow's "swivel angle" about the shoulder–wrist line), and a common strategy is to fix or optimize that parameter and then apply a closed form.

这是对 \(\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\) 取得更小。

This is Gauss–Newton on \(\tfrac12\|\mathbf e(\mathbf q)\|^2\) (Lessons 6.6, 6.9). The body-frame error \(\mathbf e\) pairs with the body Jacobian \(J_b\) (Lesson 7.4); an error written in the space frame pairs with \(J_s\) instead. The exact derivative of \(\mathbf e\) with respect to \(\mathbf q\) is \(-J_l^{-1}(\mathbf e)\,J_b\), where \(J_l\) is the left Jacobian of SE(3), which differs from the identity by \(O(\|\mathbf e\|)\). Using \(-J_b\) instead therefore still converges quadratically near a solution (FIG 4's \(10^{-2}\to10^{-4}\to10^{-8}\)), provided \(J_b\) has full rank. Far from a solution nothing is guaranteed: that is the basin picture of FIG 5, and it takes clamping, backtracking line search (Lesson 6.5) or damping (Levenberg–Marquardt) to protect the iteration. Damping changes the path but not the fixed points (\(\mathbf e=\mathbf 0\)); when \(J_b\) drops rank, however, it can stall on an error plateau (FIG 4's "stretched" target).

Units and weights. The first three components of \(\mathbf e\) are radians and the last three are metres, and "1 rad" is not equivalent to "1 m". Weight the rows: \(\min_{\Delta\mathbf q}\|W(J_b\Delta\mathbf q-\mathbf e)\|^2+\lambda^2\|\Delta\mathbf q\|^2\) with \(W=\operatorname{diag}(w_\omega I_3,\,w_vI_3)\). A typical choice makes each weight the inverse of its tolerance (1 mm gives \(w_v=10^3\), 0.1° gives \(w_\omega\approx570\)); a position-first task uses a smaller \(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 的初值或解。

A closed-form solver first returns all solutions and then filters them by joint limits, collisions and distance from the current configuration. A numerical solver has to put the limits into the iteration: the crudest way clamps \(\mathbf q\) into its limits (and can get stuck); better is to write each step as a QP with inequality constraints, \(\min\|J_b\Delta\mathbf q-\mathbf e\|^2+\lambda^2\|\Delta\mathbf q\|^2\) subject to \(\mathbf q_{\min}\le\mathbf q+\Delta\mathbf q\le\mathbf q_{\max}\) (Lesson 6.8). A redundant arm can, as in Lesson 3.4, project secondary goals (stay away from limits, keep the elbow oriented) into the null space with \((I-J^+J)\mathbf z\). One step further treats the whole IK as a nonlinear optimization problem (Lesson 6.9) with any costs and constraints (reachability, collision distance, orientation, smoothness) solved by SQP or interior-point methods; Part VI will also use neural networks to give IK initial guesses or solutions directly.

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

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

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

In robotics, IK is rarely "solve once"; it is solve every control cycle: Cartesian line moves, teleoperation and visual servoing all work like that. What matters then is not only whether each solution is right, but whether consecutive solutions are on the same branch. The figure moves the tool along a path and compares three policies:

• Fixed branch: always take the (shoulder, elbow, wrist) branch of the start;
• Nearest solution: of the 8 closed-form solutions take the one closest to the previous joint angles (ignoring limits);
• Numerical IK, warm start: iterate from the previous \(\mathbf q\), which is continuous by construction.

Both paths pass through a singularity. "Hand turns through the wrist singularity": the wrist centre stays put while the tool heading sweeps through \(q_5=0\). "Wrist passes over the base": the wrist centre crosses the J1 axis in the horizontal plane, through the shoulder singularity. Red vertical lines mark joint jumps (more than 0.5 rad in one step).