逆向运动学

3 most important points

1. 逆向运动学的挑战包括无解、多重解，需要格外注意奇异点
2. 解法主要包括解析法和数值迭代法，分别：效率高、解精确 / 灵活、通用
3. ROS 内使用的库主要是 Track-IK（数值迭代）和 IK（解析）

5 thoughts

1. 牛顿-拉夫森法虽然和优化法很像，但是本质不同，前者是求根，后者是优化；具体体现就是即使没有解，后者也能找到一个点（最优妥协解），而前者不行

Notes

简介

逆向运动学是机器人执行几乎所有任务的基础：

• 轨迹规划： 要让机器人的末端沿着一条预定路径（如直线、圆弧）运动，需要将路径上的每一个点都通过逆向运动学转换成对应的关节角度序列。
• 抓取与操作： 要让手爪以特定的姿态到达物体并抓取，必须计算出手臂各个关节的角度。
• 避障： 当存在多个解时，可以选择一个不会与环境中障碍物发生碰撞的关节配置。
• 动画： 在制作 3D 角色动画时，动画师设定手或脚的目标位置，软件通过逆向运动学自动计算出膝盖、臀部等关节的合理弯曲角度，使动作看起来自然。

核心挑战：

• 解的存在性：
  • 目标位姿可能完全在机器人的工作空间之外，此时无解
  • 目标位姿可能在奇异点附近，导致无法实现某些方向的运动
• 多重解
  • 对于一个给定的末端位姿，机器人通常有多种不同的关节配置可以达到它
  • 例如，一个 6 自由度的机械臂对于同一个目标点，可能有“左撇子”和“右撇子”两种构型，以及肘部“向上”和“向下”两种构型。理论上最多可有 16 个实数解
• 奇异位形：
  • 在奇异位形下，机器人会失去一个或多个方向的运动自由度（就像完全伸直的手臂，你无法再沿着手臂的方向移动手掌）
  • 此时，逆向运动学的雅可比矩阵奇异，导致无解或关节速度趋于无穷大，是控制中需要极力避免的情况

解析法

• 分类
  • 代数法： 
    • 从正向运动学出发，得到一个描述末端执行器位姿的齐次变换矩阵（连乘形式）
    • 消元和求解：通过左乘或右乘逆矩阵来隔离变量，逐个求解关节角度
  • 几何法：
    • 将机器人连杆结构分解为多个平面三角形，利用余弦定理等几何关系直接计算角度
• 优点
  • 计算速度快
  • 精确解
• 缺点
  • 存在多个解或无解的情况

解的情况

解析法（无论是代数还是几何）要求系统方程是“可解”的，这通常意味着机器人的结构必须满足一定的条件。

最著名和最重要的条件是 Pieper 准则：对于一个 6 自由度的机械臂，如果其最后三个关节轴（腕关节）相交于一点（即构成一个球型腕），那么该机器人的逆向运动学问题具有封闭解。

数值迭代法

• 分类
  • 牛顿-拉夫森法：
    • 利用微分运动关系，将末端位姿的微小变化与关节角度的微小变化通过 Jacobian 矩阵关联：$\Delta x=J(\theta )\Delta \theta$
    • 公式：${\theta }_{k+1}={\theta }_k+\alpha J^{-1}({\theta }_k)\ast (x_d-x_k)$
      • $x_d$：目标位姿
      • $x_k$：当前末端位姿
      • $\alpha$：步长因子
    • 特点：
      • 要求 Jacobian 矩阵可逆
      • 对奇异点不鲁棒
    • 总结：方程求根
  • 优化法：
    • 将逆向运动学问题转化为一个优化问题：寻找一组关节角度 $\theta$，使得末端位姿 $f(\theta )$ 与目标位姿 $x_d$ 之间的误差最小
    • 代价函数：$minL(\theta )={\Vert f(\theta )-x_d\Vert }^2$
    • 使用梯度下降、Levenberg-Marquardt（结合牛顿法和梯度下降）等优化算法来求解，其中 Levenberg-Marquardt 如下：${\theta }_{k+1}={\theta }_k+(J_k^T{J}_k+\lambda I{)}^{-1}{J}_k^T{e}_k$
      • 当 $\lambda \to 0$：算法退化为高斯-牛顿法。收敛速度快（二阶收敛），但在奇异点附近不稳定
      • 当 $\lambda \to \infty$：算法行为类似于梯度下降法（步长为 1/λ），收敛稳定但速度慢（一阶收敛）
    • 特点：
      • 适用于冗余机器人
      • 总是可解
      • 通过 $\lambda$ 平衡速度与稳定性
    • 总结：函数优化

初始化：θ = θ_initial, λ = λ_initial (e.g., 0.01), max_iterations, tolerance
计算初始误差：e = x_d - f(θ)
 
for k = 1 to max_iterations do
    计算雅可比矩阵 J 在当前 θ 处的值
    尝试步：Δθ = (J^T J + λ I)^(-1) J^T e
    θ_new = θ + Δθ
    计算新误差：e_new = x_d - f(θ_new)
    
    if (||e_new||^2 < ||e||^2) then
        // 成功，接受更新
        λ = λ / 10       // 减小阻尼，更接近高斯-牛顿
        θ = θ_new
        e = e_new
        if (||e|| < tolerance) then
            break         // 收敛
        end if
    else
        // 失败，拒绝更新
        λ = λ * 10       // 增大阻尼，更接近梯度下降
    end if
end for
 
return θ

• 优点
  • 灵活性高（优化法可以轻松地加入关节限位、避障等约束条件）
  • 适用于结构复杂或自由度较高的机器人
• 缺点
  • 收敛速度和计算时间慢
  • 可能陷入局部最优解

解的个数

• 判断前提： 我们只讨论机器人自由度（DOF）等于任务空间维度的情况。例如：
  • 在一条直线上运动，任务维度是 1。
  • 在一个平面上运动，任务维度是 2 或 3（位置 x, y + 朝向θ）。
  • 在三维空间中运动，任务维度是 6（位置 x, y, z + 姿态 Rx, Ry, Rz）。
• 如果自由度少于任务维度，则无解（欠驱动）。
• 如果自由度多于任务维度，则有无穷多解（冗余机器人），此时我们关心的是“最优解”。
• 关节：
  • 旋转关节：可能有多解
  • 平移关节：解通常唯一
• 案例：
  • 2个关节的机器人（RR平面臂）
    • 双解：对于同一个目标点，机械臂通常可以有两种弯曲方式——肘部向上/向下
    • 单解：目标点为奇异点，比如当手臂完全伸直或完全收缩时（工作空间边界）
    • 无解： 目标点位于工作空间外

如何判断奇异点？

• 解析法：计算 Jacobian 矩阵行列式，求解行列式为 0 的情况
• 几何法：对于常见的串联机器人，奇异点通常对应几种特定的、易于识别的物理构型
• 数值法：实时计算 Jacobian 条件数或可操作度
  • 条件数： 雅可比矩阵最大奇异值与最小奇异值的比值。条件数越大，矩阵越接近奇异（越“病态”）。当条件数趋于无穷时，完全奇异。
  • 可操作度：$w=\sqrt{det(J{J}^T)}$，接近 0 时，表示机器人接近奇异点。

库

• ROS：MoveIt! 运动规划框架
  • KDL
    • ROS 默认，基于数值迭代法
    • 通用性强但计算效率较低
    • 在奇异点附近容易求解失败
  • Track-IK
    • KDL 的上位替代
    • 结合两种数值优化算法并引入信任域
    • 显著提升速度和成功率，对奇异点的鲁棒性更好
    • 不能保证找到所有解
  • IKFast
    • 解析法，通过符号运算生成极其高效的的 C++逆向运动学求解代码
    • 求解速度极快（微秒级），稳定可靠，能求出所有可能解
    •  配置复杂，生成的代码量大

Related

Reference

• 机器人基础