3. 逆运动学
对应 CS123 · Lab 3 "Copy Cat / Romeo and Juliet" + Lecture 4 "Inverse Kinematics"
正运动学是单向的：关节角 → 末端。逆运动学是我们真正想要的方向：给一个末端目标，求出关节角。这一章我们用两种方式实现 IK，并把它接到第 1 章的 PD 控制器上，做出第一个"到点控制" demo。
本章目标
- 理解 IK 的多解性、奇异性、可达性
- 能对 3-DoF 平面臂手推解析解
- 能用雅可比伪逆法 / damped least squares 写数值 IK
- 在 MuJoCo 里让机械臂末端追踪一个移动目标
前置阅读
- 第 2 章 正运动学
- ROS · 正逆运动学实现
- 交互页:IK Playground,鼠标拖目标就能看 DLS 的收敛
先建立 FK ↔ IK 直觉
直接看解析式更快:
- FK(正向):给定
,末端位置是 写成关节角的显式函数,一次计算得出。 - IK(反向):给定
,求 。这是一个多解、可能无解的非线性方程组——同一个末端目标经常对应多组关节角("肘上 / 肘下"),也有目标在工作空间外时根本解不出。 - 数值 IK 的通用写法是 Damped Least Squares (DLS):
其中 , 是阻尼系数。奇异点附近用 防止伪逆爆炸,是数值 IK 能稳定收敛的关键。
3.1 IK 是什么问题:约束、解空间、局部最优
FK 是一个干净的前向函数,IK 则是"解一个欠定/过定非线性方程组",至少有三种和 FK 完全不同的麻烦:
- 多解。平面 2-DoF 臂的一个目标点,通常对应"肘朝上"和"肘朝下"两组关节角;6-DoF 机械臂经常有 8 组甚至 16 组。选哪组不是数学问题,而是工程约束(自碰撞、关节限位、能量最小、上一步最近)。
- 无解。目标点落在工作空间外面——这是第 2 章那张"甜甜圈"图之所以重要的原因。严肃的 IK 实现会先做一次"目标是否可达"的保护,再进正式迭代。
- 可达但到不了。目标在工作空间里,但当前构型被关节限位或障碍物挡住,从当前
出发的数值法只能收敛到一个局部最优。一些控制框架(MoveIt2、TRAC-IK)会并行多个 random seed 来缓解这个问题。
所以第 3 章的关注点不是"有没有 IK 算法能跑",而是在 1/2/3 上踩哪些坑、如何绕过。
3.2 解析法:3-DoF 平面臂的几何推导
先退到最干净的情形——3-DoF 平面臂,三节长度
常见的简化做法是把"末端朝向"也作为输入(
令:
由余弦定理:
- 若
:目标不可达,直接返回"不行"。 - 否则
——正负号就是"肘上/肘下",一般按期望构型固定选一个。
翻译成代码:
import numpy as np
def ik_analytic_planar(xy, phi, L=(0.3, 0.25, 0.15), elbow_up=True):
X, Y = xy
L1, L2, L3 = L
xp = X - L3 * np.cos(phi)
yp = Y - L3 * np.sin(phi)
r2 = xp * xp + yp * yp
cos_t2 = (r2 - L1**2 - L2**2) / (2 * L1 * L2)
if abs(cos_t2) > 1.0:
return None # 不可达,调用方要能处理 None
t2 = np.arccos(cos_t2) * (1.0 if elbow_up else -1.0)
t1 = np.arctan2(yp, xp) - np.arctan2(L2 * np.sin(t2), L1 + L2 * np.cos(t2))
t3 = phi - t1 - t2
return np.array([t1, t2, t3])
解析解的价值不是"省 CPU",而是做数值解的参考答案:下一节的数值 IK 跑完后,在平面工况下和解析解比一下,能第一时间抓到"我的雅可比写错了"这类 bug。
3.3 数值法:雅可比伪逆 + Damped Least Squares
一旦到 6-DoF 一般机械臂、人形机器人,解析解要么不存在、要么写出来比数值法还长。业内的通用解法是迭代线性化 + 最小二乘。
从当前构型
最朴素的"雅可比伪逆法"是直接
Damped Least Squares (DLS) 把它改造成一个带正则的最小二乘:
闭式解:
代码:
def ik_dls(fk_fn, jac_fn, theta0, p_target,
lam=0.05, step=0.3, tol=1e-4, max_iter=200):
theta = theta0.copy()
for k in range(max_iter):
p = fk_fn(theta)
e = p_target - p
if np.linalg.norm(e) < tol:
return theta, k
J = jac_fn(theta) # (3, n) 或 (6, n)
JJt = J @ J.T
dtheta = J.T @ np.linalg.solve(
JJt + (lam ** 2) * np.eye(JJt.shape[0]), e)
theta = theta + step * dtheta
return theta, max_iter # 没收敛也要返回,调用方决定怎么办
几个调试时会反复遇到的现象:
step取 1.0 很容易震荡、越走越远:减到 0.2~0.3 往往就稳了。lam = 0效果最"贪心",但一进奇异就蹦;lam太大则慢到收敛不动——经验值 0.01~0.1。- 迭代 500 次还没收敛,往往不是数值问题,而是目标本来就不可达。
3.4 奇异点处理:为什么纯伪逆会爆
奇异点(singular configuration)在几何上对应"机械臂瞬间少了一个自由度"——最经典的例子是6-DoF 机械臂完全伸直:第 2、3 关节共线,腕关节的某些方向再怎么转,末端也出不去。
数学上
三种处理思路,从软到硬:
- DLS 阻尼(上一节的默认做法)。
- 奇异值截断(Truncated SVD):把小于阈值的奇异值强行置 0,等价于放弃那个方向的修正。
- 路径规划层面提前绕开:把奇异区在工作空间里框出来,IK 之前就不让目标落进去。
实战上,DLS + 关节限位 + 起步 seed 多样化是性价比最高的组合。到第 4、5 章的四足 / 末端执行器时还会再碰到它。
3.5 把 IK 结果喂给 PD:串行控制回路
IK 给出的是关节目标角,不是力矩。把它喂进第 1 章那条 PD 回路,就构成了机器人学里最基础的Task-space → Joint-space → Torque 三层串联:
p_target ──▶ [IK] ──▶ θ_target ──▶ [PD] ──▶ τ ──▶ MuJoCo 物理
▲
│ θ, θ̇
└────── data.qpos / qvel
一个能跑起来的最小控制循环:
import mujoco
model = mujoco.MjModel.from_xml_path('planar_3dof.xml')
data = mujoco.MjData(model)
Kp = np.array([40.0, 40.0, 20.0])
Kd = np.array([ 4.0, 4.0, 2.0])
theta = np.zeros(3) # IK 的热启动值
for _ in range(5000):
p_target = np.array([0.4, 0.1]) # 或者第 3.6 节里让它画圆
sol = ik_dls(fk_fn, jac_fn, theta, p_target)
if sol is not None:
theta, _ = sol # IK 结果作为 PD 的目标
q, dq = data.qpos[:3], data.qvel[:3]
data.ctrl[:3] = Kp * (theta - q) + Kd * (-dq)
mujoco.mj_step(model, data)
两条别忽略的工程点:
- 把上一步的
theta作为下一步 IK 的热启动——DLS 是局部方法,起点离得越近迭代越少、越不容易跳到"肘上/肘下"的另一解。 - IK 的频率可以低于 PD。典型值 IK 200 Hz、PD 1 kHz,这也是真机上大多数控制栈的默认配置。
:::tip 想立刻看效果?
不想装 MuJoCo 可以直接打开 IK Playground,鼠标拖目标就能看 DLS 的收敛过程——尤其是拖到机械臂"伸直"那条奇异线附近,能直观感受 ik_dls,只是搬到浏览器里、不挂物理引擎。
:::
3.6 实验:末端画一个圆
用一个参数化目标轨迹,就能看到 IK + PD 是不是真的"连成一条线":
import numpy as np
center = np.array([0.45, 0.0])
R, T = 0.08, 4.0 # 半径 8 cm,周期 4 s
while data.time < 20.0:
t = data.time
p_target = center + R * np.array([np.cos(2*np.pi*t/T),
np.sin(2*np.pi*t/T)])
sol = ik_dls(fk_fn, jac_fn, theta, p_target)
if sol is not None:
theta, _ = sol
q, dq = data.qpos[:3], data.qvel[:3]
data.ctrl[:3] = Kp * (theta - q) + Kd * (-dq)
mujoco.mj_step(model, data)
观察重点:
- 圆画得圆不圆:不圆往往是 PD 跟不上,加大
或降低圆速度。 - 半径
加大到工作空间边缘时,圆会突然变成一段弧——那是 IK 开始返回 None,末端走不出去。 - 圆心
center设到这种绝对不可达的位置,可以看到 IK 直接退化成"朝目标方向伸直"。DLS 在这里是优雅失败,不崩不炸,而是尽力靠近。
这段脚本录一段 15 秒的视频,配上
组队学习任务
-
ik_analytic.py：3-DoF 平面臂解析解 -
ik_numeric.py：DLS 数值解，支持 6-DoF - 录一段 15 秒的"末端画圆" MuJoCo 视频
参考资料
- CS123 Lab 3 & Lecture 4（IK 章节）
- Buss, "Introduction to Inverse Kinematics with Jacobian Transpose, Pseudoinverse and Damped Least Squares methods"