3. 运动学基础（二）：逆运动学求解与雅可比矩阵

上一讲我们把正运动学搞清楚了——给定关节角度，算腿末端位置。但实际控制中，我们面临的是反过来的问题：想让脚踩到某个点，关节应该转多少度？这就是逆运动学（IK）。

说实话，IK 才是真正让人头疼的部分。我早年做四足机器人时，花在 IK 调试上的时间比写步态逻辑还多。你想想看，一条腿三个关节，要解一组非线性方程，稍不留神就跑到奇异点去了。

3.1 逆运动学问题的数学本质

先回顾一下正运动学公式：

T = f(θ)    （3-1）

其中 T 是末端位姿，θ 是关节角度向量。IK 就是求它的反函数：

θ = f⁻¹(T)    （3-2）

但问题在于——f 是非线性的，反函数不一定存在，也不一定唯一。说白了，同一个脚的位置，可能对应好几组关节角度（比如肘关节朝上还是朝下）。

我个人习惯：能解析就解析，解析不了再用数值法。四足机器人腿部结构通常比较规整，大部分情况都能写出解析解。

3.2 三关节腿的解析逆运动学

以最常见的三关节腿为例（髋关节横滚、髋关节俯仰、膝关节俯仰）。我们已知足端位置 (x, y, z)，要求三个关节角 θ₁, θ₂, θ₃。

推导思路分三步走：

1. 先解 θ₁：从俯视图看，θ₁ 只影响 x 和 y 的投影。
2. 再解 θ₃：利用余弦定理，从大腿和小腿长度反推膝关节角。
3. 最后解 θ₂：通过几何关系，用 θ₃ 和足端位置反算髋关节俯仰角。

具体公式如下（大腿长 L₁，小腿长 L₂）：

θ₁ = atan2(y, x)

r = sqrt(x² + y²) - L₀   // L₀ 是髋关节偏置
z' = -z

D = (r² + z'² - L₁² - L₂²) / (2 * L₁ * L₂)
θ₃ = -acos(clamp(D, -1, 1))

θ₂ = atan2(z', r) - atan2(L₂ * sin(θ₃), L₁ + L₂ * cos(θ₃))

3.3 数值逆运动学：当解析解失效时

有些腿的构型比较复杂（比如有并联结构、或者关节轴线不垂直），解析解很难写。这时候就得用数值法了。

最经典的方法是牛顿-拉夫森迭代法：

给定目标位姿 T_des
初始化 θ₀
while ||T_cur - T_des|| > ε:
    J = 计算雅可比矩阵(θ_cur)
    Δθ = J⁺ · (T_des - T_cur)   // J⁺ 是伪逆
    θ_cur += Δθ
return θ_cur

嗯，这里有个关键点——雅可比矩阵的伪逆。为什么不用普通逆？因为 J 不一定是方阵（6×n 的矩阵），只有伪逆才能处理这种欠定或超定系统。

Δθ = Jᵀ · (J · Jᵀ + λ²I)⁻¹ · e

3.4 雅可比矩阵：速度层面的映射

雅可比矩阵 J 把关节速度映射到末端速度：

v = J(θ) · θ̇    （3-3）

其中 v 是末端线速度和角速度组成的 6 维向量，θ̇ 是关节速度向量。

对于三关节腿，J 是 6×3 的矩阵。每一列对应一个关节的贡献：

其中 zᵢ 是关节 i 的旋转轴方向，pᵢ 是关节 i 的位置，p 是足端位置。

你可能会问：算这个有什么用？用处大了去了——

• 步态规划：给定足端速度，反算关节速度，做轨迹跟踪。
• 力控制：通过 Jᵀ 把关节力矩映射到足端力（虚功原理）。
• 奇异点检测：当 det(J·Jᵀ) 接近 0 时，说明腿快打直了，需要切换控制策略。

3.5 奇异点分析与避坑

奇异点就是雅可比矩阵秩亏损的位置。在四足机器人里，最常见的奇异点有两种：

1. 边界奇异点：腿完全伸直（θ₃ = 0°）或完全蜷缩（θ₃ = 180°）。此时沿腿长方向的速度会失控。
2. 内部奇异点：两个关节轴共线，比如 θ₂ 和 θ₃ 的轴线重合时，会丢失一个自由度。

3.6 本章知识体系

下面这张图把逆运动学和雅可比矩阵的核心脉络串起来了：

3.7 代码实战：三关节腿 IK 实现

最后给一段可运行的 Python 代码，实现前面讲的解析 IK：

import numpy as np

def leg_ik(x, y, z, L1=0.2, L2=0.2, L0=0.05):
    """
    三关节腿逆运动学求解
    参数：
        x, y, z: 足端位置（基坐标系）
        L1: 大腿长度
        L2: 小腿长度
        L0: 髋关节偏置
    返回：
        [theta1, theta2, theta3] 弧度
    """
    # 1. 解髋关节横滚角
    theta1 = np.arctan2(y, x)
    
    # 2. 投影到 sagittal 平面
    r = np.sqrt(x**2 + y**2) - L0
    z_prime = -z
    
    # 3. 余弦定理求膝关节角
    D = (r**2 + z_prime**2 - L1**2 - L2**2) / (2 * L1 * L2)
    D = np.clip(D, -1.0, 1.0)  # 防止越界
    theta3 = -np.arccos(D)
    
    # 4. 求髋关节俯仰角
    alpha = np.arctan2(z_prime, r)
    beta = np.arctan2(L2 * np.sin(theta3), L1 + L2 * np.cos(theta3))
    theta2 = alpha - beta
    
    return np.array([theta1, theta2, theta3])

# 测试：足端在 (0.2, 0.0, -0.3)
angles = leg_ik(0.2, 0.0, -0.3)
print(f"关节角: {np.degrees(angles)} 度")