逆运动学：从目标位姿回到关节空间

前面的文章里，我们已经用 DH、旋量与 PoE 描述过从关节空间到任务空间的道路，也用雅可比研究过它在局部怎样传递速度。

但机器人真正执行任务时，命令往往来自另一端：

• “把夹爪移动到这个位姿”；
• “让焊枪沿这条轨迹前进”；
• “把足端放到这里”；
• “让并联平台到达这个位置”。

已知目标位姿 $T_d$，反过来寻找关节变量 $q$，就是逆运动学（Inverse Kinematics, IK）：

$$f(q)=T_d$$

它看起来只是把正运动学的等号左右交换了一下，性质却完全不同。正运动学通常是一条明确的计算链；逆运动学面对的却可能是一片岔路：目标可能不可达，也可能对应多个构型；冗余机器人甚至会有无穷多组解，而奇异性又会让一些原本分开的解突然汇合。

本文沿着一条尽量直观的路线展开：

1. 先用平面 2R 机械臂看清多解、不可达和奇异性；
2. 再从局部线性化自然得到 Newton 迭代、伪逆与阻尼最小二乘；
3. 接着讨论冗余、关节限位和解的连续性；
4. 最后进入五杆并联机构，看看闭环机器人为什么常常“逆解容易，正解反而更难”。

逆运动学不是某一条万能公式。它更像一套分层决策：先理解几何结构，再选择求解方法，最后用约束、连续性和正运动学校验把数学解变成可执行的机器人构型。

1. 逆运动学为什么不是简单的“求逆”

设机器人的正运动学为：

$$x=f(q)$$

其中 $q\in\mathbb R^n$ 是关节变量，$x$ 是任务变量。若任务只关心平面位置，$x$ 可以是 $(x,y)$；若要控制空间刚体位姿，$x$ 则属于 $SE(3)$。

逆运动学要求解：

$$f(q)-x_d=0$$

这里不能想当然地写成 $q=f^{-1}(x_d)$，因为 $f$ 通常并不是一一映射。

对同一个目标，逆运动学可能出现四种情况：

• 无解：目标落在工作空间之外，或姿态、关节限位与碰撞约束无法同时满足；
• 唯一解：在给定构型分支和约束下，只剩一组可行关节变量；
• 有限多个解：例如平面 2R 的“肘上”和“肘下”，工业六轴机器人的肩、肘、腕组合；
• 无穷多解：任务维数低于有效关节自由度，机器人可以在保持末端任务不变时继续改变内部姿态。

因此，IK 求解器不能只回答“算出了什么”，还应回答：

• 目标是否真的可达？
• 找到了哪个解分支？
• 是否满足关节限位和碰撞约束？
• 与上一个控制周期的解是否连续？
• 把结果代回正运动学后，误差究竟有多大？

这些问题在最简单的 2R 机械臂里已经全部出现。

2. 平面 2R：一张图看懂解析逆解

考虑一台平面两转动关节机械臂：两根连杆长度为 $L_1,L_2$，关节角为 $q_1,q_2$，目标点为 $p_d=(x_d,y_d)^{\scriptscriptstyle\mathsf T}$。

正运动学是：

$$x_d=L_1\cos q_1+L_2\cos(q_1+q_2)$$ $$y_d=L_1\sin q_1+L_2\sin(q_1+q_2)$$

直接从这两条三角方程消元当然可行，但几何能让答案更快浮现出来。

连接基座、肘部和目标点，会得到一个三角形。目标到基座的距离为：

$$r=\sqrt{x_d^2+y_d^2}$$

由余弦定理：

$$c_2=\cos q_2 =\frac{x_d^2+y_d^2-L_1^2-L_2^2}{2L_1L_2}$$

2.1 先判断目标能不能到

真实角度必须满足 $|c_2|\le 1$。等价地，目标距离应满足：

$$|L_1-L_2|\le r\le L_1+L_2$$

这两条边界很直观：

• $r>L_1+L_2$：两根杆完全伸直仍然够不到；
• $r<|L_1-L_2|$：长杆减去短杆后仍然太长，末端无法缩进内孔。

所以 2R 机械臂的位置工作空间是一个圆环；当 $L_1=L_2$ 时，内圆半径退化为零。

2.2 一次得到“肘上”和“肘下”

当 $|c_2|<1$ 时：

$$s_2=\sin q_2=\pm\sqrt{1-c_2^2}$$

于是第二关节有两个分支：

$$q_2=\operatorname{atan2}(s_2,c_2)$$

再定义目标方向：

$$\phi=\operatorname{atan2}(y_d,x_d)$$

第一关节角为：

$$q_1 =\phi- \operatorname{atan2} \left(L_2s_2,\,L_1+L_2c_2\right)$$

取 $s_2>0$ 和 $s_2<0$，就得到两组镜像构型。它们把末端放在同一个点，却让肘部位于目标连线的两侧。

这里使用 atan2 而不是普通的 $\arctan(y/x)$ 很重要。atan2 同时保留分子和分母的符号，能够正确区分象限，也能避免在分母接近零时丢失方向信息。

2.3 奇异性就是两个分支汇合的地方

平面 2R 位置雅可比的行列式为：

$$\det J=L_1L_2\sin q_2$$

当 $q_2=0$ 或 $q_2=\pi$ 时，机械臂完全伸直或完全折叠，$\det J=0$。恰好在这里：

$$s_2=0$$

“肘上”和“肘下”不再是两组不同构型，而是在工作空间边界汇合成同一个姿态。

这揭示了逆解与奇异性之间很深的联系：奇异性不仅让速度映射丢失方向，也常常是逆解分支诞生、合并或消失的边界。

2.4 数学上的两解，不等于工程上的两解

解析式返回两组角度后，仍要继续筛选：

• 关节角是否落在机械限位内；
• 连杆是否与基座、环境或自身碰撞；
• 电缆、软管和工具姿态是否允许该构型；
• 哪个解离上一时刻的关节位置更近；
• 哪个解离奇异性更远、传动性能更好。

因此，解析逆解通常分为两层：先枚举几何分支，再根据工程约束选择可执行解。

3. 从位置走向位姿：先找“腕点”

2R 机械臂只能独立控制平面中的两个位置分量。若希望同时控制末端方向，可以再增加一个转动关节，得到平面 3R 机械臂。

设第三根连杆长度为 $L_3$，目标位姿为 $(x_d,y_d,\varphi_d)$。末端方向满足：

$$q_1+q_2+q_3=\varphi_d$$

先从目标点沿工具方向退回 $L_3$，得到腕点：

$$p_w= \begin{bmatrix} x_d-L_3\cos\varphi_d\\ y_d-L_3\sin\varphi_d \end{bmatrix}$$

接下来只需：

1. 用上一节的 2R 解析式把前两根连杆送到 $p_w$；
2. 再令：

$$q_3=\varphi_d-q_1-q_2$$

这是一种非常重要的思想：利用机构结构，把位置和姿态拆开。

许多经典六轴工业机器人把最后三个腕关节的轴线设计为交于一点。这样可以先求腕心位置，再求腕部姿态，把一个复杂的空间逆解拆成两个更简单的子问题。Pieper 在 1968 年的工作系统研究了这类具有特殊轴线结构的解析逆解。

但并非所有机器人都具备这种结构。轴线偏置、一般 6R 构型、冗余机械臂、柔性关节和复杂闭环机构，往往难以得到简洁的闭式解。这时，更通用的数值方法就会登场。

4. 数值逆运动学：把“到目标”变成一连串小步

设当前关节变量为 $q_k$，末端位置为 $f(q_k)$，目标误差为：

$$e_k=x_d-f(q_k)$$

在当前构型附近，对正运动学做一阶展开：

$$f(q_k+\Delta q) \approx f(q_k)+J(q_k)\Delta q$$

若希望下一步消除误差，就要解：

$$J(q_k)\Delta q\approx e_k$$

然后更新：

$$q_{k+1}=q_k+\alpha\Delta q, \qquad 0<\alpha\le 1$$

这就是 Jacobian 迭代类 IK 的共同骨架：在当前位置重新计算雅可比，用一小步关节运动修正任务误差，再不断重复。

4.1 方阵且非奇异：Newton–Raphson

若 $J$ 是可逆方阵，可以取：

$$\Delta q=J^{-1}e_k$$

于是：

$$q_{k+1}=q_k+\alpha J(q_k)^{-1}e_k$$

在初值足够接近、雅可比条件良好且模型光滑时，Newton 方法可以收敛得很快。但机器人问题中，$J$ 常常不是方阵，或者会在奇异附近失去可逆性，因此直接求逆并不是通用方案。

4.2 伪逆：给非方阵一个最自然的答案

更一般地，可以使用 Moore–Penrose 伪逆：

$$\Delta q=J^\dagger e_k$$

它在不同维数关系下有清晰的含义：

• 若任务方程无法被完全满足，它给出最小化 $\|J\Delta q-e_k\|_2$ 的最小二乘解；
• 若存在无穷多组精确关节增量，它选择欧氏范数最小的那一组。

“最小范数”并不自动等于“最安全”或“最自然”。它只说明在当前坐标尺度下，这一步的关节增量最小。关节限位、碰撞、能耗与姿态偏好仍需额外建模。

4.3 空间位姿误差不能直接相减

对位置向量，$e=p_d-p$ 很自然；但旋转矩阵不能简单逐元素相减后当作角速度误差。

若使用末端坐标系表达误差，可以定义：

$$T_{err}=T(q_k)^{-1}T_d$$

再通过李群对数得到体坐标误差旋量：

$$\mathcal V_b =\operatorname{Log}(T_{err})^\vee$$

并与体雅可比 $J_b$ 配对：

$$\Delta q=J_b^\dagger\mathcal V_b$$

也可以把误差和雅可比都放在空间坐标系表达。关键不在于选哪一种，而在于：误差旋量和雅可比必须处在同一个坐标系、采用同一种分量顺序。

4.4 何时停止迭代

一个可靠的数值 IK 至少需要三类退出条件：

$$\|e_p\|<\varepsilon_p, \qquad \|e_R\|<\varepsilon_R$$

以及：

• 达到最大迭代次数；
• 误差长时间不再下降；
• 关节步长已经很小，却仍无法满足任务。

位置误差和姿态误差具有不同单位，应分别设置容差。若把米和弧度直接混成一个范数，隐含的权重可能与真实任务完全不符。

5. 奇异附近：为什么伪逆会突然变得激进

对雅可比做奇异值分解：

$$J=U\Sigma V^{\scriptscriptstyle\mathsf T}$$

伪逆为：

$$J^\dagger =V\Sigma^\dagger U^{\scriptscriptstyle\mathsf T}$$

对每个非零奇异值 $\sigma_i$，伪逆使用的增益是：

$$g_i^{pinv}=\frac{1}{\sigma_i}$$

当某个 $\sigma_i$ 接近零时，一个很小的任务误差就会被放大成很大的关节增量。这正是机器人接近奇异位形时，数值 IK 容易抖动、跳解或撞上速度限制的原因。

5.1 阻尼最小二乘：允许“不要一步到位”

阻尼最小二乘（Damped Least Squares, DLS）不再要求这一小步把任务误差压到最低，而是在任务修正与关节步长之间做权衡：

$$\Delta q =\arg\min_{\Delta q} \left( \|J\Delta q-e_k\|_2^2 +\lambda^2\|\Delta q\|_2^2 \right)$$

其解可以写成：

$$\Delta q =J^{\scriptscriptstyle\mathsf T} \left(JJ^{\scriptscriptstyle\mathsf T}+\lambda^2I\right)^{-1}e_k$$

也可以写成：

$$\Delta q =\left(J^{\scriptscriptstyle\mathsf T}J+\lambda^2I\right)^{-1} J^{\scriptscriptstyle\mathsf T}e_k$$

在奇异值方向上，DLS 的增益变成：

$$g_i^{DLS}=\frac{\sigma_i}{\sigma_i^2+\lambda^2}$$

当 $\sigma_i\to 0$ 时，这个增益也趋于零，不再把微小误差无限放大。

阻尼不是免费午餐：$\lambda$ 越大，关节更新越平稳，但任务收敛会更慢，并且会留下更明显的瞬时误差。工程上常根据最小奇异值、可操作度或误差大小自适应调节阻尼，而不是在所有姿态使用同一个很大的 $\lambda$。

6. 冗余机器人：多出来的自由度去哪里

当关节自由度多于任务维数时，即使末端任务完全确定，内部姿态仍然可以变化。以七自由度机械臂为例，手部位姿固定后，肘部通常还可以沿一条自运动流形移动。

局部速度层面的通解可以写成：

$$\Delta q =J^\dagger e +\left(I-J^\dagger J\right)z$$

第一项完成主任务；第二项位于 $J$ 的零空间，因为：

$$J\left(I-J^\dagger J\right)=0$$

向量 $z$ 可以用来表达次要目标，例如：

• 远离关节限位；
• 保持肘部朝向某一侧；
• 增大可操作度；
• 避开障碍物；
• 减少关节运动或能耗。

若定义一个代价函数 $h(q)$，常见选择是：

$$z=-k\nabla h(q)$$

但要注意：使用阻尼伪逆后，$I-J_\lambda^\dagger J$ 一般不再是严格的正交零空间投影。若主任务必须高精度保持，或者同时存在关节限位、速度限幅和碰撞不等式，分层优化或二次规划通常比简单叠加更可靠。

7. 并联机器人：为什么逆运动学反而更直接

串联机器人沿一条开链从基座走到末端。已知关节角时，逐节相乘即可得到末端位姿；反过来寻找关节变量，却要面对整条链耦合起来的非线性方程。

并联机器人恰好相反：末端平台同时被多条支链连接。给定平台位姿后，每条支链常常可以独立求解；但给定驱动关节后，多条支链必须在闭环中相交到同一个平台位姿，正运动学可能出现多个装配解。

最清楚的例子，是平面两自由度五杆机构。

7.1 把五杆机构拆成两条 2R 支链

设左右基座转轴为 $A,B$，目标点为 $P$。左支链的主动杆和从动杆长度为 $a_L,b_L$，右支链为 $a_R,b_R$。

定义：

$$r_L=\|P-A\|, \qquad \phi_L=\operatorname{atan2}(P_y-A_y,P_x-A_x)$$

左侧三角形由余弦定理给出：

$$\alpha_L =\arccos \left( \frac{a_L^2+r_L^2-b_L^2}{2a_Lr_L} \right)$$

因此左侧主动关节角有两个分支：

$$\theta_L=\phi_L\pm\alpha_L$$

右侧完全相同：

$$r_R=\|P-B\|, \qquad \phi_R=\operatorname{atan2}(P_y-B_y,P_x-B_x)$$ $$\alpha_R =\arccos \left( \frac{a_R^2+r_R^2-b_R^2}{2a_Rr_R} \right)$$ $$\theta_R=\phi_R\pm\alpha_R$$

目标点一旦给定，左右支链的主动角就能分别由两个三角形直接求出。两侧符号独立组合，理论上最多形成四种逆解分支。

7.2 工作空间是两个圆环的交集

目标必须同时被左右支链够到：

$$|a_L-b_L|\le r_L\le a_L+b_L$$ $$|a_R-b_R|\le r_R\le a_R+b_R$$

所以五杆机构的几何工作空间，是以 $A$、$B$ 为圆心的两个支链圆环的交集。再考虑关节限位、连杆碰撞和奇异曲线后，真正可用的工作空间会进一步缩小。

7.3 工作模式与装配模式不是一回事

并联机器人中有两个容易混淆的概念：

• 工作模式（working mode）：给定末端位姿时，各支链选择哪一个逆解分支；在五杆机构中，对应左右“肘”的符号组合；
• 装配模式（assembly mode）：给定主动关节变量时，闭环机构可能以哪一种整体形状装配起来；在五杆机构中，两个远端圆通常有两个交点，对应两个正运动学解。

因此，五杆机构常呈现出一种与串联臂相反的计算性格：

• 逆运动学：给定 $P$，左右两条腿分别解三角形，较直接；
• 正运动学：给定 $\theta_L,\theta_R$，要寻找两条远端杆圆的公共交点，存在装配模式的多值性。

对 Delta、Stewart 平台等更复杂并联机构，这种“逆解便于按支链拆分、正解需要处理闭环耦合”的特征仍然常见，虽然具体方程会更复杂。

7.4 并联机构的两类奇异性

五杆机构也能直观地区分两类奇异性：

• 当某条支链的两根杆共线时，该支链的逆雅可比退化，驱动关节难以产生某个末端速度方向；
• 当左右远端杆的约束方向共线时，两条支链对末端的约束失去独立性，平台可能出现驱动器无法控制的瞬时运动。

前者常与工作空间边界、工作模式切换相关；后者可能出现在工作空间内部，是并联机器人规划时尤其需要避开的区域。

8. 从“有一个解”到“有一个好解”

真实机器人中的 IK 通常不是孤立求一个点，而是连续求解一条轨迹。一个实用流程可以分成六步。

8.1 明确任务到底控制什么

先定义任务变量：只控位置，还是同时控制姿态？某个姿态轴是否可以自由？位置和姿态误差如何加权？

不必要的任务约束会把原本可达的问题变成不可达。比如轴对称工具绕自身轴的转角并不重要，就不必强迫求解器精确控制它。

8.2 优先利用机构结构

若机器人具有稳定、可维护的解析解，应先枚举解析分支。它速度快、可解释，也能明确告诉我们目标为何不可达。

若结构一般、约束复杂或机器人冗余，则使用数值 IK；解析解也可以作为数值法的高质量初值。

8.3 用上一时刻的解作为初值

轨迹连续时，上一控制周期的 $q_{k-1}$ 往往是当前目标最好的种子。它能让迭代留在同一个解分支，减少肘部或腕部突然翻转。

比较关节距离时还应处理角度周期：$179^\circ$ 与 $-179^\circ$ 在数值上相差 $358^\circ$，物理上却只差 $2^\circ$。

8.4 在求解过程中处理约束

简单地在每次更新后裁剪关节角，可能破坏收敛方向，并让求解器卡在限位边界。更稳健的做法包括：

• 加权伪逆；
• 零空间关节居中；
• 带边界的非线性最小二乘；
• 将速度、关节限位和碰撞距离写进二次规划。

8.5 对所有候选解排序

可用一个明确的代价函数选择解析分支或多初值结果：

$$C(q)= w_c\,d(q,q_{prev})^2 +w_l\,C_{limit}(q) +w_s\,C_{sing}(q) +w_o\,C_{collision}(q)$$

这里的权重应反映真实任务优先级，而不是为了让某次演示看起来顺滑而随意调整。

8.6 永远用正运动学做最后验算

无论解析法还是数值法，最终都应计算：

$$T_{check}=f(q^*)$$

并检查：

• 位置与姿态残差；
• 关节限位和速度限制；
• 碰撞状态；
• 雅可比条件数或最小奇异值；
• 与上一时刻解的连续性。

IK 返回“成功”只是求解器内部的判断；正运动学残差和约束检查，才是机器人真正可以执行这组关节命令的证据。

9. 解析法与数值法，应该怎样选择

维度	解析逆解	数值逆解
适用结构	具有可利用几何结构的特定机器人	几乎任意可微运动学模型
多解处理	可以显式枚举分支	通常由初值决定落入哪个局部解
不可达判断	常能直接由几何条件给出	通常表现为残差无法下降或约束不可满足
计算速度	通常快且确定	需要迭代，耗时与初值和容差有关
约束扩展	后处理较直观，复杂约束较麻烦	易与优化、限位和碰撞约束结合
奇异附近	分支会合，公式需谨慎处理	需要阻尼、步长控制或奇异鲁棒方法
可解释性	高	依赖误差定义、初值和求解器设置

最实用的选择往往不是二选一，而是组合：

1. 用几何知识判断工作空间并生成候选初值；
2. 用解析式快速枚举可见分支；
3. 用数值优化吸收标定误差和附加约束；
4. 用连续性、碰撞和奇异性指标选择最终解。

10. 回到这个系列：逆运动学把前面的工具连了起来

逆运动学并没有引入一个孤立的新世界。它恰好把前面几篇文章中的语言重新汇合：

• PoE 与齐次变换给出可靠的正运动学模型 $T=f(q)$；
• 雅可比矩阵把当前位姿误差变成局部关节修正；
• 奇异值与可操作度告诉我们哪些方向正在变得脆弱；
• 阻尼最小二乘让数值迭代在奇异附近不至于失控；
• 并联机构的闭环几何把逆解分成各支链的工作模式，再把正解留给装配模式。

对平面 2R，我们可以沿三角形一次找到所有解；对一般空间机械臂，我们让雅可比带着当前构型一步步靠近目标；对冗余机器人，我们利用零空间安排内部姿态；对五杆并联机构，我们又回到几何，把一个闭环逆解拆成两个简单三角形。

这或许就是逆运动学最值得欣赏的地方：它不是强行把机器人“倒着算”，而是在几何、微分与优化之间寻找一条从目标回到关节的合适道路。

11. 扩展阅读：开源机器人库怎样求解 IK

前面的推导落到工程中，并不会变成某一个统一的 solveIK()。不同开源项目对逆运动学的分工并不相同：有的负责组织规划流程和切换求解器，有的直接实现数值或解析算法，还有的只提供运动学、雅可比和李群运算，让使用者自行搭建求解器。

理解这种分工，能避免一个常见误解：调用同一个机器人框架，并不代表所有机器人都在使用同一种 IK 算法。

11.1 MoveIt：通过插件选择求解路线

MoveIt 本身不是单一的 IK 算法，而是一套运动规划框架。每个规划组都可以在 kinematics.yaml 中选择自己的运动学插件，因此同一个 MoveIt 工程可以根据机构结构和任务需要，使用完全不同的求解器。

常见插件与本文方法的关系如下：

求解器	核心思路	与本文的对应关系	主要特点
KDL	Newton–Raphson 位置迭代，内部用雅可比伪逆求关节修正	第 4 节的 Jacobian 迭代与伪逆	通用、简单，是 MoveIt 常见的默认选择；但依赖初值，在关节限位和奇异附近可能难以收敛
LMA	Levenberg–Marquardt 阻尼最小二乘	第 5 节的 DLS	用阻尼抑制病态方向，比直接伪逆更稳健；仍然是依赖初值的局部迭代法
TRAC-IK	改进的 KDL 路线与 SQP 非线性优化并行竞争	Jacobian 迭代 + 约束优化	一条路线通过随机跳转逃离关节限位附近的局部困境，另一条路线用 SQP 处理带边界优化，通常比标准 KDL 更容易找到可行解
pick_ik	全局进化搜索与局部梯度下降结合	第 8 节的多初值与代价函数思想	可以同时考虑位姿误差、关节位移和自定义目标；局部模式适合连续轨迹，全局模式适合较远初值和复杂解空间
IKFast	针对具体机器人生成解析 C++ 求解器	第 2、3 节的解析逆解	运行时几乎不需要迭代，速度快且结果稳定；但依赖机器人几何结构，模型改变后通常需要重新生成

KDL 的计算骨架，正是前文的局部线性修正：

$$J(q_k)\Delta q\approx e_k, \qquad q_{k+1}=q_k+\Delta q$$

LMA 则在这个问题中加入阻尼，使小奇异值方向不再产生过大的关节步长。TRAC-IK 和 pick_ik 进一步承认了一个现实：IK 不只是解一组等式，还要面对关节边界、局部极小值和“多个可行解中选哪一个”等优化问题。

MoveIt 还可以在求解器外层检查碰撞、调用有效性回调并尝试不同种子。因而“MoveIt 找到了一组无碰撞 IK”不等于底层 KDL 自己优化了碰撞距离，而可能是框架对候选解进行了筛选。类似地，Cached IK 会用过去的相近结果提供更好的初值，它加速了搜索，却没有改变底层 IK 的数学原理。

11.2 Pinocchio：提供搭建求解器的数学积木

Pinocchio 的定位与 MoveIt 不同。它是一套高性能刚体运动学与动力学库，擅长计算正运动学、关节雅可比、$SE(3)$ 对数映射和构型流形上的积分，但并不规定用户必须采用哪一种 IK 求解器。

Pinocchio 官方的闭环逆运动学示例采用了很典型的李群迭代路线。先在末端坐标系中定义位姿误差：

$$e_k= \operatorname{Log} \left(T(q_k)^{-1}T_d\right)^\vee$$

再计算与该误差表达一致的雅可比，并使用阻尼伪逆得到关节速度：

$$v_k= -J^{\scriptscriptstyle\mathsf T} \left( JJ^{\scriptscriptstyle\mathsf T}+\lambda I \right)^{-1}e_k$$

最后不是简单地写成 $q_{k+1}=q_k+v_k\Delta t$，而是通过构型流形上的积分更新：

$$q_{k+1}=\operatorname{integrate} \left(q_k,v_k\Delta t\right)$$

对普通转动关节，这看起来与角度相加差别不大；对浮动基座、四元数关节等非欧氏构型，integrate 才能保证更新仍然落在正确的流形上。

因此，Pinocchio 更像一个精确而高效的“数学发动机”。使用者可以在它之上搭建 DLS、非线性最小二乘、二次规划或分层任务求解器，并自行加入关节限位、碰撞约束、零空间任务和多初值策略。

11.3 实际工程中怎样选择

没有一种求解器在所有机器人和任务上都占优，可以先从问题结构出发：

• 具有稳定解析结构、模型长期不变，并且追求极低延迟：优先考虑解析解或 IKFast；
• 一般串联机械臂，目标连续、初值较好：KDL、LMA 或 Pinocchio 上的 DLS 往往已经足够；
• 关节限位频繁影响收敛，希望提高单次求解成功率：可以尝试 TRAC-IK；
• 需要关节偏好、自定义代价或更大范围搜索：pick_ik 或专门的非线性优化更合适；
• 浮动基座、复合流形、分层任务或自定义控制器：Pinocchio 提供的李群与雅可比工具更容易继续扩展。

无论选哪一种库，求解器名称都不能替代验证。真正决定 IK 是否可靠的，仍然是目标定义、坐标系一致性、初值策略、关节与碰撞约束、终止条件，以及最后那次正运动学校验。

进一步阅读：

• MoveIt：运动学插件概念
• MoveIt：运动学求解器配置
• MoveIt：TRAC-IK 教程
• MoveIt：pick_ik 教程
• MoveIt：IKFast 教程
• Pinocchio：闭环逆运动学示例

参考资料

1. Kevin M. Lynch, Frank C. Park, Modern Robotics: Mechanics, Planning, and Control, Chapter 6: Inverse Kinematics, Cambridge University Press, 2017. 官方课程资源
2. Kevin M. Lynch, Frank C. Park, “Numerical Inverse Kinematics,” Modern Robotics, Section 6.2. 官方讲解
3. Donald L. Pieper, The Kinematics of Manipulators Under Computer Control, Ph.D. thesis, Stanford University, 1968.
4. Charles W. Wampler II, “Manipulator Inverse Kinematic Solutions Based on Vector Formulations and Damped Least-Squares Methods,” IEEE Transactions on Systems, Man, and Cybernetics, 16(1), 1986. DOI: 10.1109/TSMC.1986.289285
5. Xin-Jun Liu, Jinsong Wang, G. Pritschow, “Kinematics, Singularity and Workspace of Planar 5R Symmetrical Parallel Mechanisms,” Mechanism and Machine Theory, 41(2), 2006. DOI: 10.1016/j.mechmachtheory.2005.05.004