第 5 章逆运动学¶

逆运动学（inverse kinematics, IK）问"要让末端到指定位姿，关节角该是多少"，即求 \(T(\theta)=T_d\) 的 \(\theta\)，或位姿误差下的 \(\dot q=J^{+}\dot x\)。它比正运动学难得多：解可能不存在、不唯一、且高度非线性。下面的路线按"能解析就解析、不能解析就迭代、冗余就投影"排：解析解与 Pieper 可分性、解的多重性、伪逆迭代与阻尼最小二乘、冗余臂的零空间投影，最后是关节限位与失败处理。

1. 解析解与可分性¶

平面二连杆有闭式解。给定目标 \((x,y)\)、\(L_1=L_2=1\)：

\[ c_2=\frac{x^2+y^2-L_1^2-L_2^2}{2L_1L_2},\quad q_2=\operatorname{atan2}(\pm\sqrt{1-c_2^2},\,c_2),\quad q_1=\operatorname{atan2}(y,x)-\operatorname{atan2}(L_2\sin q_2,\,L_1+L_2\cos q_2). \]

tgt = fk2([np.radians(30), np.radians(90)])   # (0.366, 1.366)
## elbow_up   -> [30., 90.]   回代 FK 误差 0.0
## elbow_down -> [120., -90.] 回代 FK 误差 0.0

对同一目标点，肘上解 \((30^\circ,90^\circ)\) 与肘下解 \((120^\circ,-90^\circ)\) 都能精确回代（FK 误差 \(0.0\)）。空间 6 自由度臂何时也有闭式解，由 Pieper 可分性定理（Pieper's separability theorem）刻画：当三根相邻关节轴交于一点或互相平行时，位姿逆问题可运动学解耦（kinematic decoupling）为先由位置定前三轴、再由姿态定后三轴，从而逐段手推闭式解——多数工业臂的球形腕正满足此条件。

2. 多解与解空间离散性¶

非冗余 \(n\) 自由度臂解 \(n\) 个任务约束时，解通常是有限离散的一组：平面二连杆 2 个，满足可分性的空间 6R 臂最多可达 16 组（肩部、肘部、腕部的二值组合）。冗余臂（关节数多于任务维）则解成一个连续流形——同一末端位姿有无穷多关节组合。选择原则是工程性的：在限位内、离当前构型最近、可操作度最高。解的多重性不是缺陷，而是冗余臂躲避障碍、避奇异的自由度来源。

分支按几何枚举，不靠随机初值碰：肩部左右、肘部上下、腕部翻转各一位二值，组合数就是解的上界；每条分支在关节限位下的可行区间可以预先算出来，运行时先按当前位姿判定处在哪一支，再在本支内求解。跨支切换意味着关节角大跳变，只能安排在轨迹规划阶段（且落在无障碍的换臂姿态处），不能发生在跟踪过程中。

数值 IK 的迭代闭环（解析可解时优先解析）：

flowchart TD
    e["位姿误差 e"] --> jac["雅可比 J(θ)"]
    jac --> step["Δθ = J⁺e（伪逆）<br/>或 Jᵀ(JJᵀ+λ²I)⁻¹e（DLS）"]
    step --> lim["关节限位裁剪"]
    lim --> upd["θ ← θ + Δθ"]
    upd --> chk{"‖e‖ 收敛？"}
    chk -->|"否"| e
    chk -->|"是"| out["输出 θ"]

解析解的多支与数值法的补位：

flowchart LR
    goal["目标位姿 T_d"] --> analytic{"解析可分？"}
    analytic -->|"是（Pieper）"| multi["多解：elbow up / down<br/>按限位与代价挑"]
    analytic -->|"否"| num["数值迭代"]
    num --> pinv["伪逆 J⁺"]
    num --> dls["阻尼最小二乘<br/>奇异附近稳定"]
    redundant["冗余臂"] --> null["零空间投影<br/>不动末端、只优化次要目标"]

3. 数值法：伪逆与阻尼最小二乘¶

无闭式或需在线跟踪时，用雅可比做迭代。牛顿型更新 \( \Delta q = J^{+}\,\Delta x\)，其中 \(J^{+}=J^\top(JJ^\top)^{-1}\)（方阵满秩时即 \(J^{-1}\)）。问题出在奇异附近：\(JJ^\top\) 接近奇异，伪逆元素爆炸。

qs = [np.radians(45), np.radians(1.0)]      # 接近伸直（近奇异）
Js = jac2(qs)
Jpinv = Js.T @ np.linalg.inv(Js @ Js.T)              # 伪逆
lam = 0.1
Jdls  = Js.T @ np.linalg.inv(Js @ Js.T + lam**2*np.eye(2))   # 阻尼最小二乘
## ||J^+|| = 128.12   ||J_dls|| = 0.895

近奇异 \(q_2=1^\circ\) 处，伪逆范数冲到 \(128.1\)，而阻尼最小二乘（damped least squares, DLS，亦即 Levenberg–Marquardt）\(J^{+}_{\text{dls}}=J^\top(JJ^\top+\lambda^2 I)^{-1}\) 把范数压回 \(0.895\)。DLS 用阻尼因子 \(\lambda\) 换取解的有界性：离奇异远时可忽略、近奇异时以牺牲一点精度避免关节速度暴涨，是在奇异区安全迭代的默认手段。

迭代的三件事决定能不能收敛。误差表示用旋转矢量（\(\log\) 映射）而非欧拉角之差，后者在大角度与万向锁附近非线性严重；阻尼因子随最小奇异值自适应，离奇异远时取小保精度、靠近时放大保稳定，固定 \(\lambda\) 要么在正常区拖慢收敛、要么在奇异区仍发散；终止条件同时看误差范数与步长，任一项低于阈值即停，并限最大迭代次数防死循环。

数值 IK 只有局部收敛性：初值落在另一分支的吸引域里，会稳稳收敛到另一个有效解。所以工程上用上一时刻的关节角作初值，既保连续性又天然落在正确分支；冷启动时用多个种子各自迭代再挑，不指望单次迭代跨分支。

工程里这条路线不必自写：Pinocchio 给 \(J\) 与 \(M\)，QP 层用 pink（Pinocchio 生态）——每拍解一个带约束的 QP 得增量速度，限位、速度上限与碰撞线性化约束显式进问题，解不出时给的是“冲突方向”而不是空值。自写 DLS 的意义在于看清阻尼与限位处理，上产品前再换库（MuJoCo 生态的 mink 同思路，见 manipulation 笔记）。

对第 3 章那台 6R 球腕臂，同一迭代法可解出完整 6 维位姿逆：以 \(\theta_{\text{tgt}}=(35,-20,45,10,30,-25)^\circ\) 的 FK 位姿为目标，从远离解的初值 \(\theta_0=(5,\dots,5)^\circ\) 出发，每步 \(\Delta\theta=J^{+}_{\text{dls}}\,e\)（\(e\) 为位置误差拼接姿态旋转矢量误差），\(\lambda=0.05\) 下 10 步内位姿误差范数降到 \(6.6\times10^{-10}\)、收敛回 \(\theta_{\text{tgt}}\)：

## 6 R DLS IK：从 θ0=(5,...,5)° 迭代 10 步，末位姿误差 = 6.6e-10
## 解 θ(deg) = [35,-20,45,10,30,-25]（= 生成目标的 θ_tgt）

目标位姿由 FK 生成、再被 DLS 迭代解回，正逆互为检验；收敛到哪一组解由初值决定，这也正是运行时拿上一时刻关节角作种子的理由。这套迭代是空间 6-DOF 臂无闭式逆时的在线标准做法，与解析法互补。

4. 冗余与零空间投影¶

冗余臂的一般速度解写成

\[ \dot q = J^{+}\dot x + (I - J^{+}J)\,k, \]

第二项落在 \(J\) 的零空间（null space），\(k\) 任意时都不改变末端速度——这是"自运动"（self-motion）。把次级任务（避障、关节能量最优、远离限位）写成代价梯度 \(\nabla H\)，取 \(k=-\alpha\nabla H\)，即在不扰动主任务的前提下同时优化次级目标，这就是零空间投影的任务优先级做法，也是冗余机械臂控制的标准结构（闭环优先级调度归 control-note）。

5. 关节限位与失败处理¶

IK 不是总能给可用解，落地时须显式处理失败：

无解 / 超出工作空间：\((x,y)\) 落在可达环带外时 \(c_2\) 越界，解析法须先判 \(|c_2|\le1\)；数值法看迭代是否收敛、残差是否低于阈值，不收敛即置失败标志，绝不返回半路解。
关节限位：解含 \(360^\circ\) 整数倍等价、或多组候选时，先按各轴 [qmin,qmax] 过滤，再在合法子集里选最近构型；数值迭代中把限位做成软约束（越界回弹）或硬截断（clamp）都可能引入残差，需回代 FK 校验。
奇异 / 病态：用 DLS 的 \(\lambda\) 正则化，或检测到可操作度（第 4 章）过低时降步长、换构型。
连续性：多解之间若每步自由重选，关节角会突跳；应选离上一时刻解最近的一组，保证轨迹平滑可执行。

所有返回解都要用第 3 章的 FK 回代验证末端误差，这是判定 IK 成功与否的唯一硬标准。

失败要按处理代价分档：目标确实在工作空间之外——只能改任务或改基座，重试没有意义；初值落在坏分支或奇异区——换种子、加阻尼后重试；撞上限位——把目标沿路径向可行域内缩，或插入换构型动作。三档分开，上层才能决定是重规划还是报错，而不是把所有失败都当成"算不出来"。

6. 要点¶

平面二连杆有闭式逆解，肘上/肘下两支回代误差均为 \(0.0\)；空间臂可解析求逆的充分条件是 Pieper 可分性（三轴共点或平行），据此运动学解耦。
非冗余臂解离散（6R 最多 16 组），冗余臂解成连续流形；多解按限位、最近构型、可操作度择优。
数值法用伪逆迭代 \(\Delta q=J^{+}\Delta x\)，近奇异处伪逆发散（范数 128），阻尼最小二乘（\(\lambda\) 正则）压回有界（0.895）。
冗余解 \(\dot q=J^{+}\dot x+(I-J^{+}J)k\)，零空间项承载次级任务而不影响末端。
失败须显式处理：工作空间外无解、限位过滤、奇异正则、解的连续性，并一律回代 FK 校验。

参考文献与延伸阅读¶

Siciliano, B., Sciavicco, L., Villani, L., & Oriolo, G. Robotics: Modelling, Planning and Control. Springer, 2009. DOI
Craig, J. J. Introduction to Robotics: Mechanics and Control (4th ed.). Pearson, 2017.
Lynch, K. M., & Park, F. C. Modern Robotics: Mechanics, Planning, and Control. Cambridge University Press, 2017. 书籍主页

第 5 章 逆运动学¶