📚 课程目录第4章逆运动学

4、逆运动学：解析法、几何法、数值迭代法（如牛顿-拉夫森法），处理多解、奇异位形和冗余度问题

逆运动学（Inverse Kinematics, IK）是机器人运动控制中的核心问题：给定末端执行器（End-Effector）的目标位姿（位置与姿态），求解满足该位姿的关节变量组合。与正运动学（Forward Kinematics, FK）的确定性映射不同，逆运动学往往面临多解、奇异位形（Singularity）和冗余度（Redundancy）三大挑战。本章将系统梳理解析法、几何法与数值迭代法的数学原理、适用场景与工程取舍，并给出处理多解选择、奇异规避和冗余优化的完整方法论。

4.1 解析法：闭式求解的数学基础与适用条件

核心原理：解析法通过代数消元、三角恒等变换或矩阵方程分解，直接推导出关节变量的闭式表达式。其数学本质是将末端位姿矩阵 T₆ 与各关节齐次变换矩阵 A₁…A₆ 的连乘等式逐项展开，利用矩阵元素之间的约束关系分离出未知关节角。最经典的判据是 Pieper准则：当六自由度机器人满足三个相邻关节轴交于一点（如球腕结构）或三个相邻关节轴平行时，逆运动学可分解为位置求解与姿态求解两个独立子问题，从而获得闭式解。

操作流程：

构型识别：检查机器人是否满足Pieper准则（球腕、肩部偏置等）；
位置解耦：将腕部中心位置从目标位姿中分离，先求前三个关节角（θ₁, θ₂, θ₃）；
姿态回代：利用腕部旋转矩阵 R₆ 与 R₃ 的关系，求解腕部三个关节角（θ₄, θ₅, θ₆）；
多解枚举：根据 atan2 的象限特性，列出所有可能的关节角组合（通常为 8 组或 16 组）。

方法	数学工具	典型适用构型	计算耗时	多解处理
解析法	代数消元、atan2、矩阵分解	6R球腕、PUMA、SCARA	μs级	显式枚举
几何法	余弦定理、平面几何	平面2R、3R、球腕	μs级	几何分支判断
数值迭代法	雅可比矩阵、牛顿-拉夫森	任意构型、冗余机器人	ms级	依赖初值

  案例拆解：以典型6R球腕机器人（如Motoman HP6）为例，目标位姿为 T₆ = [R|p]。首先从 p 中减去 d₆·R·[0,0,1]ᵀ 得到腕部中心坐标 p_w。然后通过几何关系求解 θ₁ = atan2(p_w.y, p_w.x)，θ₂ 和 θ₃ 通过余弦定理在肩-肘-腕三角形中求解。最后利用 R₃ᵀ·R₆ 提取欧拉角得到 θ₄, θ₅, θ₆。整个过程在嵌入式控制器上耗时约 20μs，且能枚举全部 8 组解。

常见误区：解析法并非万能。当机器人存在肩部偏置（如KUKA KR系列）或腕部非球型时，Pieper准则不满足，闭式解不存在或极其复杂。此时强行推导解析式会导致数值不稳定和计算冗余。

4.2 几何法：直观建模与快速求解

核心原理：几何法将机器人连杆投影到二维平面，利用三角形边长关系和角度约束直接求解关节角。其优势在于避免了矩阵连乘的抽象性，特别适合平面机器人（SCARA、2R/3R平面臂）或具有明确几何特征的球腕结构。几何法本质上是解析法的一种特殊形式，但更强调空间几何直觉。

判断方法：

若机器人所有关节轴平行（平面机器人），可直接在平面内建立几何模型；
若存在球腕，可将位置与姿态解耦，位置部分用几何法求解；
几何法求解时需注意三角形解的存在性条件（如两边之和大于第三边）。

实战场景：在SCARA机器人中，θ₁ 和 θ₂ 决定末端在XY平面的位置，θ₄ 决定姿态。通过余弦定理：

cos(θ₂) = (x² + y² - l₁² - l₂²) / (2·l₁·l₂)
θ₂ = ±acos(cos(θ₂))   // 正负对应肘部上下
θ₁ = atan2(y, x) - atan2(l₂·sin(θ₂), l₁ + l₂·cos(θ₂))

几何法的多解体现在肘部上下（±θ₂）和腕部翻转（θ₄ 与 θ₄+π）的组合上，工程中通常根据关节限位和避障需求选择。

经验提醒：几何法求解时，建议先绘制连杆平面图并标注所有已知角度和边长，再逐步推导。对于3R平面臂，可先求腕部位置，再反解肩肘角，避免直接求解高次方程。

4.3 数值迭代法：牛顿-拉夫森与雅可比矩阵

核心原理：数值迭代法将逆运动学转化为非线性方程组的求根问题。定义误差函数 e(θ) = T_target - T_current(θ)，通过迭代更新关节角使误差趋近于零。牛顿-拉夫森法利用雅可比矩阵 J 的逆（或伪逆）进行一阶线性化：

Δθ = J⁻¹ · Δx   // 其中 Δx 为末端位姿误差（位置+姿态）
θ_new = θ_old + Δθ

对于非方阵（冗余机器人），使用伪逆 J⁺ = Jᵀ(JJᵀ)⁻¹ 或阻尼最小二乘（DLS）J⁺ = Jᵀ(JJᵀ + λ²I)⁻¹ 来避免奇异。

操作流程：

初始化：给定关节角初值 θ₀（通常取当前关节角或中位值）；
正运动学计算：求当前末端位姿 T(θₖ)；
误差计算：计算位置误差 Δp 和姿态误差 ΔR（用轴角表示）；
雅可比更新：计算 J(θₖ)，求解 Δθ = J⁺·Δx；
迭代收敛：若 ||Δx|| < ε（如 1e-6），则停止；否则更新 θₖ₊₁ = θₖ + Δθ 并返回步骤2。

迭代方法	收敛速度	奇异鲁棒性	计算复杂度	适用场景
牛顿-拉夫森	二阶收敛	差（J⁻¹ 在奇异处爆炸）	O(n³)	非奇异、6DOF
阻尼最小二乘	线性收敛	好（λ 正则化）	O(n³)	奇异附近、冗余
雅可比转置	线性收敛	较好	O(n²)	实时性要求高

  案例闭环：某7自由度冗余机械臂（如KUKA iiwa）需要跟踪一条复杂空间曲线，且要求避开关节限位。采用DLS方法，λ 自适应调节：当雅可比条件数 > 1000 时，λ 从 0.01 增至 0.1。初值选取当前关节角，迭代步长限制为 0.05rad。实测平均迭代 8 次收敛，位置误差 < 0.1mm，姿态误差 < 0.01rad。相比纯牛顿法，DLS在奇异位形附近避免了关节速度突变，但收敛速度下降约 30%。通过引入零空间投影 Δθ_null = (I - J⁺J)·∇H(θ)，成功将关节角保持在限位范围内，同时优化了能耗。

风险说明：数值迭代法对初值极其敏感。若初值远离真实解，可能收敛到局部极小值或发散。工程中应结合上一时刻的解作为初值（连续性假设），或使用多组随机初值并行求解后选择最优解。

4.4 多解选择、奇异位形与冗余度处理

多解选择策略：解析法和几何法会枚举出多组解，工程中需根据以下优先级筛选：

关节限位约束：剔除超出关节运动范围的解；
避障约束：检查连杆是否与障碍物碰撞；
连续性约束：选择与当前关节角最接近的解（最小关节位移）；
运动学性能：优先选择远离奇异位形、关节速度较小的解。

奇异位形分类与规避：

奇异类型	数学特征	物理表现	规避方法
边界奇异	det(J)=0，工作空间边界	末端无法沿某方向运动	路径规划时预留边界余量
内部奇异	det(J)=0，工作空间内部	关节速度突变	奇异分离+阻尼最小二乘
腕部奇异	θ₅=0，腕部轴对齐	姿态突变	调整路径或增加冗余自由度

冗余度处理：当自由度 n > 6 时，逆运动学有无穷多解。利用零空间投影实现任务优先级控制：主任务（末端跟踪）优先，次任务（避障、限位、能耗优化）在零空间内执行。数学表达为：

θ̇ = J⁺·ẋ + (I - J⁺J)·∇H(θ)

其中 H(θ) 为优化目标函数（如关节限位惩罚函数），∇H 为其梯度。通过调节梯度方向，可在不影响主任务的前提下优化次任务。

工程实践建议：对于实时控制系统，推荐混合策略——在正常区域使用解析法（若构型满足Pieper准则），在奇异附近切换至DLS数值法。同时，在轨迹规划阶段就应检测奇异位形，通过调整路径点或速度分布来主动规避，而非仅依赖实时IK的鲁棒性。

  综合案例复盘：某焊接机器人（6R球腕）在焊接圆角时出现末端抖动。分析发现，当焊枪姿态接近垂直向下时，θ₅ 接近 0°，触发腕部奇异。解决方案：① 在路径规划阶段，将奇异区域的姿态路径微调 5°，使 θ₅ 最小值为 15°；② 在IK求解中，对 θ₅ 施加软约束，当 |θ₅| < 10° 时，将 θ₅ 钳位至 10° 并重新计算 θ₄、θ₆；③ 采用DLS方法替代纯解析法，λ=0.05。改造后，焊接速度从 0.8m/min 提升至 1.2m/min，抖动幅度降低 90%，焊缝质量合格率从 92% 提升至 99.5%。该案例表明，奇异规避需要路径规划与IK求解协同设计，单一层面的优化难以彻底解决问题。

总结复盘：逆运动学求解没有银弹。解析法适合结构化构型，提供闭式解和全部多解；几何法适合平面或球腕构型，直观高效；数值迭代法通用性强，但需精心设计初值、阻尼参数和收敛判据。工程实践中，应首先分析机器人构型是否满足Pieper准则，再结合实时性要求、奇异风险和工作环境选择合适方法。多解选择需综合考虑限位、避障和连续性；奇异规避需在路径规划与IK求解两个层面协同处理；冗余度则通过零空间投影实现多任务优化。掌握这三种方法及其组合策略，是机器人运动控制工程师的核心能力之一。