2. 正运动学
上一章我们只控一个关节。这一章把关节串起来——给出 n 个关节角，求末端执行器在世界坐标系下的 6-DoF 位姿。正运动学是逆运动学、步态规划、视觉-动作对齐的前提。
本章目标
- 能写出齐次变换矩阵
T = [R p; 0 1] - 能从 URDF / MJCF 描述里读出每个关节的父坐标系与旋转轴
- 能用 Python + NumPy 实现一个 3-DoF 机械臂的 FK，并在 MuJoCo 里验证末端位置误差 < 1e-6
- 能画出雅可比矩阵（为第 3 章逆解做准备）
前置阅读
- 第 1 章 执行器与 PD 控制
- ROS · 运动学与坐标变换
- ROS · 正逆运动学实现（可选，讲得更细）
- 想先拖一拖关节再看公式:FK Playground
2.1 位置、姿态与位姿
先对照下图建立一个直觉:位置只回答"末端原点在哪",姿态只回答"末端坐标轴朝哪",位姿则把两者合在同一个变换里。
图里参考坐标系记为 {B},末端坐标系记为 {E},末端原点记为
- 位置(position):只回答"末端原点在哪"。图左侧蓝色虚线向量
表示的就是位置,通常写成 。 - 姿态(orientation):只回答"末端坐标轴朝哪"。它描述的是
{E}相对{B}的朝向,3D 里常用旋转矩阵、四元数或 rpy表示;平面机械臂里它通常退化成一个角度或 。 - 位姿(pose):位置 + 姿态。它既描述末端原点落在什么位置,也描述末端坐标系的朝向,因此是对刚体状态的完整描述。
这三个概念合在一起,就得到本章后面反复出现的齐次变换矩阵:
这里 {E} 中表示的点,变换到参考坐标系 {B} 中。对 2D 平面机械臂,姿态进一步退化成一个角度,于是位姿矩阵也退化成常见的 3×3 形式。
本章后面的 3-DoF 平面例子会先把重点放在位置上,便于把链式乘法和坐标变换先吃透;到了第 3 章做 IK 时,平面臂的姿态/朝向约束会显式写成
2.2 坐标系、刚体变换、齐次矩阵
机器人学里所有"位姿"的问题,最后都落到一个问题:同一个点,在不同坐标系下的坐标怎么换算?
任意刚体变换都可以拆成"先旋转再平移":
写成齐次矩阵更紧凑,这也是后面所有代码里真正用到的形式:
两个关键性质:
- 可以一路乘下去。
个关节串起来的末端位姿就是 ,这一乘法正是 FK 的全部计算内容。 - 可逆且正交。
,这在把世界坐标系下的目标搬到末端坐标系时会反复用到。
一个很常踩的坑:旋转矩阵来源五花八门——MuJoCo 默认给 (w,x,y,z) 四元数,URDF 默认 rpy(roll-pitch-yaw),文献里还经常是绕固定轴 vs 绕活动轴。先把"我这一步的
2.3 DH 参数 vs URDF 父-子链式约定
经典教材(Craig 那本)用 DH 参数描述机械臂:每个连杆四个参数
URDF / MJCF 走的是另一条路——父-子链式约定:
world → base → link1 → link2 → link3 → ... → end_effector
joint1 joint2 joint3
每条 <joint> 定义子连杆相对父连杆的固定偏置 + 轴向:
<joint name="joint2" type="revolute" axis="0 0 1">
<origin xyz="0.3 0 0" rpy="0 0 0"/>
<parent link="link1"/>
<child link="link2"/>
</joint>
- 静止部分
origin(平移+欧拉角)在 URDF 加载时就确定下来,本质是一个常量。 - 运动部分:绕
axis旋转的增量 。
于是第
本章后面的代码都按这个更贴近 URDF 的写法走。好处是你写的 FK 代码可以直接从 URDF / MJCF 里读参数,不用再手动换算一遍 DH。
2.4 一个 3-DoF 平面臂的 FK 手推
作为热身,先考虑最干净的平面 3-DoF 臂——三个连杆长度
如果把末端姿态也一起写出来,这个平面例子其实还有一个很干净的结果:
也就是说,这个 3-DoF 平面臂的完整 FK 输出可以理解成
注意这里关节角是逐级累加的——这是下一章解析 IK 能那么干净收敛的关键。
把它写成齐次矩阵形式,方便后面推广到 3D:
把三个
2.5 用 NumPy 写 FK 并与 mj_forward 交叉校验
先定义两个最基础的齐次变换构造器,后面所有 FK 代码都靠它们拼积木:
import numpy as np
def rot_z(theta):
c, s = np.cos(theta), np.sin(theta)
T = np.eye(4)
T[:3, :3] = [[c, -s, 0],
[s, c, 0],
[0, 0, 1]]
return T
def trans(x, y, z):
T = np.eye(4)
T[:3, 3] = [x, y, z]
return T
3-DoF 平面臂的 FK:
def fk_planar(thetas, L=(0.3, 0.25, 0.15)):
t1, t2, t3 = thetas
T = rot_z(t1) @ trans(L[0], 0, 0) \
@ rot_z(t2) @ trans(L[1], 0, 0) \
@ rot_z(t3) @ trans(L[2], 0, 0)
return T # 末端位置直接读 T[:3, 3]
然后用 MuJoCo 的 mj_forward 作为"标准答案"交叉校验——给 100 组随机关节角,比较末端位置的最大误差:
import mujoco
model = mujoco.MjModel.from_xml_path('planar_3dof.xml')
data = mujoco.MjData(model)
end_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, 'end_site')
max_err = 0.0
for _ in range(100):
q = np.random.uniform(-np.pi, np.pi, size=3)
data.qpos[:3] = q
mujoco.mj_forward(model, data) # 让 MuJoCo 用 kinematic tree 算
p_mj = data.site_xpos[end_id] # 它算的末端位置
p_ours = fk_planar(q)[:3, 3] # 我们自己算的末端位置
max_err = max(max_err, np.linalg.norm(p_mj - p_ours))
print(f'max |p_ours - p_mj| = {max_err:.2e}')
# 期望量级在 1e-14 ~ 1e-12;超过 1e-6 基本是参数/轴向写错了
为什么要花这一步校验? 因为 FK 是后面所有章节的基石——IK 靠它算残差,雅可比靠它的导数,控制器靠它估末端位置。差 0.5 mm 在第 3 章 末端画圆时就是一圈肉眼可见的毛刺。把"和物理引擎完全一致"作为第一次运行的硬指标,后面所有实验才有意义。
2.6 雅可比矩阵:从 FK 到速度映射
FK 把关节角映射到末端位姿。若只关心位置部分,可以写成
对平面 3-DoF 臂,
有两种常用算法:
- 解析求导:把 FK 关于每个
求偏导,适合符号化推一次、运行时直接算——速度快,但和具体机器人强绑定。 - 几何雅可比:对第
列,线速度部分是 ,角速度部分就是 (关节轴在世界坐标系下的方向)。代码写起来通用,也是下一章 IK 用的那一种。
雅可比是运动学和动力学之间的桥:它一出现,速度、力、能量就都能用同一套代数来写——所以第 3 章数值 IK、第 5 章脚端接触力、第 6 章 RL 奖励里都会反复见到它。
2.7 实验 1:画出工作空间散点图
工作空间(reachable workspace)是末端能到达的所有点的集合。用 FK 做一次 Monte Carlo 就能把它画出来:
import matplotlib.pyplot as plt
pts = []
for _ in range(20000):
q = np.random.uniform(-np.pi, np.pi, size=3)
pts.append(fk_planar(q)[:2, 3])
pts = np.array(pts)
plt.figure(figsize=(5, 5))
plt.scatter(pts[:, 0], pts[:, 1], s=1, alpha=0.3)
plt.axis('equal'); plt.title('3-DoF planar arm workspace')
plt.show()
你会看到一个甜甜圈形状的区域——外圈是三节完全伸直能到的最远距离
2.8 实验 2:把末端位置叠加到 MuJoCo 可视化上
MuJoCo 支持在 viewer 里叠加"调试几何",非常适合把 FK 的中间结果可视化出来——比如末端位置、每一节连杆坐标轴、目标点。
import mujoco.viewer
with mujoco.viewer.launch_passive(model, data) as v:
while v.is_running():
# 假设我们在 Python 里用自己的 FK 算末端位置
p = fk_planar(data.qpos[:3])[:3, 3]
# 清空旧的调试几何再画
v.user_scn.ngeom = 0
mujoco.mjv_initGeom(
v.user_scn.geoms[0],
type=mujoco.mjtGeom.mjGEOM_SPHERE,
size=[0.02, 0, 0], pos=p, mat=np.eye(3).flatten(),
rgba=[1, 0.3, 0.3, 1],
)
v.user_scn.ngeom = 1
mujoco.mj_step(model, data)
v.sync()
这一步看似"只是画个红球",但它实际上是把 Python 世界里的数学模型和 MuJoCo 世界里的物理模型对上。如果红球不在机械臂末端上,不是 viewer 的锅——要么坐标系没对齐,要么链式乘法少了一环、多了一环。到第 3 章做 IK 时,目标点和当前末端都是用这种方式实时叠到场景里的。
组队学习任务
- 实现
fk.py，输入关节角列表，输出末端 4×4 齐次矩阵 - 对 100 个随机关节组合，比较自写 FK 和
mj_forward的最大误差 - 画工作空间散点图（matplotlib 3D）
参考资料
- CS123 Lecture 3: Forward Kinematics
- 《机器人学导论》（Craig）第 3 章