4、运动学基础（三）：逆运动学求解（几何法）、雅可比矩阵。

好，我们继续往下走。

上一章我们聊了正运动学，知道了怎么从关节角度算出足端位置。那反过来呢？给定一个目标位置，让机器人把脚踩到那儿去，关节应该转多少度？这就是逆运动学要干的事。

说实话，逆运动学比正运动学麻烦不少。正运动学是唯一的，你给一组角度，位置就定了。但逆运动学不一样——同一个位置，可能有好几组关节角度都能达到，也可能压根儿就没有解。我在做四足机器人步态规划时，就经常遇到这种“理论上能到，但关节限位卡死”的情况。

4.1 几何法求解逆运动学

求解逆运动学的方法有很多，数值迭代、解析法、几何法。我个人最推荐从几何法入手。为什么？因为直观。你看着机器人的腿，画几条辅助线，用初中三角函数就能算出来。

拿我们那条简化的二连杆腿来说吧。大腿长 L1，小腿长 L2，髋关节在原点，膝关节在中间，足端在 (x, z)。

第一步，看大腿和小腿组成的三角形。已知三边：L1、L2、还有足端到髋关节的距离 r = sqrt(x² + z²)。用余弦定理就能求出膝关节角度 q2：

cos(q2) = (L1² + L2² - r²) / (2 * L1 * L2)
q2 = arccos(cos(q2))   # 注意，这里有两个可能解：正腿和反腿

嗯，这里要注意。arccos 出来的角度在 0 到 π 之间。如果 q2 是正的，腿是向外弯的（我们叫它“正腿”）；如果取负值，腿是向内弯的（“反腿”）。四足机器人一般用正腿，因为结构上更稳定。

第二步，算髋关节角度 q1。这需要用到几何关系：

# 先算从髋到足的直线与竖直方向的夹角
alpha = atan2(x, z)

# 再算大腿与这条直线的夹角（用正弦定理或余弦定理）
beta = arccos((L1² + r² - L2²) / (2 * L1 * r))

# 最终髋关节角度
q1 = alpha - beta   # 注意符号方向取决于你的坐标系定义

我曾经在调试一个 trot 步态时，发现机器人总是往一边偏。查了半天，原来是 atan2 的参数顺序写反了。这种低级错误，排查起来真的很花时间。

4.2 多解与奇异点

逆运动学有几个绕不开的问题，我简单列一下：

• 多解性：同一个足端位置，膝关节可以向上弯也可以向下弯。你需要根据机器人的实际构型选一个。
• 无解：目标点离髋关节太远，超过了 L1+L2。这时候机器人就算把腿伸直了也够不着。
• 奇异点：当腿完全伸直（q2=0）或完全折叠（q2=π）时，关节速度会变得非常大。你想想看，这时候稍微动一下足端，关节就得猛转。

情况	原因	处理方法
多解	几何构型对称	根据关节限位和能耗选最优解
无解	目标超出工作空间	限制目标位置在可达范围内
奇异	关节共线	避开奇异区域，或使用阻尼最小二乘法

4.3 雅可比矩阵

好，逆运动学算完了，但问题还没结束。我们不光要知道关节角度，还要知道关节速度和足端速度之间的关系。这就引出了雅可比矩阵。

说白了，雅可比矩阵就是一个线性变换。它把关节空间的速度映射到操作空间的速度：

v = J(q) * dq/dt

其中 v 是足端速度（3维向量），dq/dt 是关节速度（2维向量），J 是 3×2 的雅可比矩阵。

怎么求雅可比？有两种方法：

1. 解析法：对正运动学方程直接求偏导。数学上严谨，但推导起来麻烦。
2. 几何法：利用每个关节对末端速度的贡献来构造。更直观，我一般用这个。

对于我们的二连杆腿，几何法构造雅可比是这样的：

# 每个旋转关节对足端速度的贡献 = 关节角速度 × 关节轴方向 × 关节到足端的向量
# 对于平面腿，所有关节轴都垂直于平面（即 y 轴方向）

J = [
    [-L1*sin(q1) - L2*sin(q1+q2),  -L2*sin(q1+q2)],
    [ L1*cos(q1) + L2*cos(q1+q2),   L2*cos(q1+q2)]
]

# 第一列是髋关节的贡献，第二列是膝关节的贡献

有了雅可比矩阵，我们能干很多事：

• 速度控制：给定足端期望速度，反算出关节速度。
• 静力映射：关节力矩和足端力之间的关系是雅可比的转置：τ = Jᵀ * F。
• 可操作性分析：通过雅可比的条件数判断当前位姿是否接近奇异点。

4.4 知识结构总览

下面这张图把本章的核心逻辑串起来了。你可以看到，从目标位置出发，经过几何法逆运动学得到关节角度，再通过雅可比矩阵关联到速度和力。这是一个完整的闭环。

4.5 代码实现要点

最后，我分享一下实际写代码时的几个要点：

• 角度范围检查：算出来的 q1、q2 要跟关节限位做比较，超限了就做饱和处理。
• 数值稳定性：当 r 接近 L1+L2 时，余弦定理的分母接近 0，容易产生数值误差。我一般会加一个小 epsilon 做保护。
• 雅可比求逆：对于非方阵的雅可比，用伪逆 J⁺ = Jᵀ(JJᵀ)⁻¹。如果接近奇异点，伪逆会变得很大，这时候要用阻尼伪逆。