要让机器人"走到某个点、把手摆成某个角度",首先得能用数学把位置(在哪)和姿态(朝向哪)写出来,合起来叫位姿(Pose)。位置相对简单,姿态(三维旋转)才是难点。
在某个参考坐标系(比如固定在机器人骨盆的坐标系)里,一个点的位置就是一个三维列向量:
p = [ px ] px:沿 X 轴的距离
[ py ] py:沿 Y 轴的距离
[ pz ] pz:沿 Z 轴的距离这只能回答"点在哪儿",回答不了"物体朝向哪"。要描述朝向,需要旋转矩阵。
旋转矩阵 R 是一个 3×3 的正交矩阵,它把坐标系 B 里的向量转到坐标系 A: p_A = R · p_B。它满足两个约束:RᵀR = I(列向量两两垂直且长度为 1)且 det(R) = +1(右手系,不出现镜像)。所有合法的旋转矩阵构成一个集合,记作 SO(3)。
绕 Z 轴旋转 θ 角的旋转矩阵最常用,记下来:
Rz(θ) = [ cosθ -sinθ 0 ]
[ sinθ cosθ 0 ]
[ 0 0 1 ]绕 X、Y 轴同理。旋转矩阵的优点是无歧义、可直接连乘;缺点是9 个数只表达了 3 个自由度,信息冗余,数值漂移后还可能不再"正交"。
欧拉角把任意旋转拆成按固定顺序绕三个轴的旋转,比如常见的 ZYX 顺序(先绕 Z 转偏航角 yaw,再绕 Y 转俯仰角 pitch,最后绕 X 转滚转角 roll):
R = Rz(ψ) · Ry(θ) · Rx(φ)
ψ = yaw 偏航 θ = pitch 俯仰 φ = roll 滚转欧拉角只有 3 个数,符合直觉(飞行员说"滚转 30 度、俯仰 10 度")。但它有两个致命问题:旋转顺序必须约定(ZYX 和 XYZ 结果不同),以及万向锁(Gimbal Lock)——当中间那个角(这里是 pitch)转到 ±90° 时,第一条轴和第三条轴重合,系统突然少了一个自由度,无法唯一表示姿态。
四元数 q = w + xi + yj + zk 是"1 个实部 + 3 个虚部"的超复数。表示旋转时用单位四元数(满足 w² + x² + y² + z² = 1)。把向量 v 旋转后得到 v′ 的公式是 v′ = q · v · q⁻¹,旋转的连续合成用四元数乘法(哈密顿积),插值用球面线性插值 slerp。
为什么四元数能避免万向锁?因为它不是"按顺序转三次"这种参数化,而是把旋转当作 4 维空间里的一个点来整体描述;任何姿态都有连续、唯一的表示(最多差一个正负号),不存在"两根轴重合"的退化点。代价是不直观——人很难从 (w, x, y, z) 四个数直接看出朝哪。
| 表示方式 | 参数个数 | 优点 | 缺点 | 典型用途 |
|---|---|---|---|---|
| 旋转矩阵 R | 9(冗余) | 无歧义、直接连乘、可直接作用于向量 | 冗余、数值需正交化、难读 | 齐次变换、FK 推导、渲染 |
| 欧拉角 | 3 | 直观、好读、适合人机界面 | 万向锁、顺序依赖、插值不自然 | 遥控、航向姿态显示 |
| 轴角 (axis-angle) | 4(轴3+角1) | 几何意义清晰 | 合成/插值复杂 | 误差表示、规划 |
| 四元数 | 4(带单位化约束) | 无万向锁、平滑插值 slerp、合成快 | 不直观 | IMU 姿态解算、动作插值、SLAM |
真实的机器人不是原地转,而是"先转个方向,再挪个位置"。位置(3 个数)+ 姿态(旋转矩阵 3×3)可以合并成一个齐次变换矩阵 T,它是 4×4 的分块矩阵:
T = [ R | p ] = [ r11 r12 r13 | px ]
[---+---] [ r21 r22 r23 | py ]
[ 0 | 1 ] [ r31 r32 r33 | pz ]
[ 0 0 0 | 1 ]
R:3×3 旋转矩阵 p:3×1 平移向量它的好处是一个矩阵乘法就完成"旋转+平移"两步。点用齐次坐标 [x, y, z, 1]ᵀ 表示,变换就是一次乘法 p′ = T · p。
多个变换连乘时,顺序非常重要,且矩阵乘法不满足交换律。约定:想"先绕局部坐标旋转、再把坐标系原点平移到 p",写成:
T = Trans(p) · Rot(θ) ← 先算 Rot,再算 Trans(写在左边,后作用)
即:p_world = R · p_local + t
(先旋转局部点,再叠加平移)反过来 Rot(θ) · Trans(p) 则意味着"先平移、再绕全局原点旋转",结果完全不同。记住口诀:写在最右边的矩阵最先作用在点上。
平面里点 (1, 0) 先绕原点逆时针转 90°,再沿 X 平移 3、沿 Y 平移 1:
T = Trans(3,1) · Rz(90°)
= [ 1 0 3 ] [ 0 -1 0 ] [ 0 -1 3 ]
[ 0 1 1 ] · [ 1 0 0 ] = [ 1 0 1 ]
[ 0 0 1 ] [ 0 0 1 ] [ 0 0 1 ]
作用到 (1,0,1):
[ 0 -1 3 ] [ 1 ] [ 0·1 + (-1)·0 + 3 ] [ 3 ]
[ 1 0 1 ] · [ 0 ] = [ 1·1 + 0·0 + 1 ] = [ 2 ]
[ 0 0 1 ] [ 1 ] [ 0 + 0 + 1 ] [ 1 ]
结果 (3, 2):确实先转到了 (0,1),再平移到 (3,2) ✓一台人形机器人有几十个关节,如果每个都手动画旋转矩阵、来回换算,工程上会疯掉。1955 年 Denavit 和 Hartenberg 提出一套标准化约定(DH 参数):只要给每个关节标 4 个数,就能机械地写出相邻连杆之间的齐次变换,进而求出全臂正运动学。这是工业机器人和 URDF 之外最常见的运动学建模语言。
在每个关节上建立坐标系,从连杆 i-1 的坐标系到连杆 i 的坐标系,经过"两次旋转 + 两次平移"共 4 步,对应 4 个参数:
| 参数 | 名称 | 绕/沿哪根轴 | 含义 | 转动关节里 |
|---|---|---|---|---|
| θ (theta) | 关节角 | 绕 zi-1 轴旋转 | 让 xi-1 转到与 xi 平行 | ★ 关节变量(随电机转动变化) |
| d | 连杆偏距 | 沿 zi-1 轴平移 | 两坐标系沿公轴线的距离 | 常数(通常 0 或连杆厚度) |
| a | 连杆长度 | 沿 xi 轴平移 | 相邻两关节轴线间公垂线长度 | 常数(就是连杆长 L) |
| α (alpha) | 连杆扭转角 | 绕 xi 轴旋转 | 让 zi-1 转到与 zi 平行 | 常数(通常 0 或 ±90°) |
四个参数合成一个变换(注意相乘顺序,正好是"旋转-平移-平移-旋转"):
Ai = Rot_z(θi) · Trans_z(di) · Trans_x(ai) · Rot_x(αi)
展开成 4×4(标准 DH):
[ cosθi -sinθi·cosαi sinθi·sinαi ai·cosθi ]
Ai = [ sinθi cosθi·cosαi -cosθi·sinαi ai·sinθi ]
[ 0 sinαi cosαi di ]
[ 0 0 0 1 ]假设一个平面 2 连杆臂:两根连杆长 L1、L2,两个关节都在同一个平面内转(所有 z 轴平行、指向纸外),没有连杆扭转、没有偏距。它的 DH 表:
| 关节 i | θi | di | ai | αi |
|---|---|---|---|---|
| 1 | θ1(变量) | 0 | L1 | 0 |
| 2 | θ2(变量) | 0 | L2 | 0 |
因为 α = 0、d = 0,每段变换简化为:
[ cosθi -sinθi 0 ai·cosθi ]
Ai = [ sinθi cosθi 0 ai·sinθi ]
[ 0 0 1 0 ]
[ 0 0 0 1 ]正运动学(Forward Kinematics, FK)回答:"我知道每个关节当前的角度,请问末端(手/脚)现在在空间什么位置、什么朝向?"答案是唯一确定的——给定一组关节角,末端位姿就唯一确定。做法就是把 DH 表里每段变换按顺序连乘:
T_end = A1 · A2 · ... · An
输入:q = (θ1, θ2, ..., θn) 关节角向量
输出:末端 4×4 齐次变换 T(位置 p + 姿态 R)把 3.2 的两段变换连乘,A1 · A2 的旋转部分等于 Rz(θ1+θ2)(绕 z 轴的两次旋转直接相加),平移部分展开得到著名的结果:
验证:θ1=0、θ2=0 时,臂完全伸直指向 +X, x = L1 + L2, y = 0 ✓ 与直觉一致
function FK(theta):
T = identity(4)
for i = 1 to n:
# 由 DH 参数 (theta[i], d[i], a[i], alpha[i]) 构 Ai
Ai = dh_transform(theta[i], d[i], a[i], alpha[i])
T = T * Ai # 连乘
p = T[0:3, 3] # 末端位置
R = T[0:3, 0:3] # 末端姿态
return (p, R)正运动学在工程上的三个用途:①仿真校验——把规划好的关节角代进去看末端会不会撞墙;②标定——比对模型算出的位置和实测位置,反推真实的连杆长度误差;③作为逆运动学数值法的"打分器"(见下一节)。
逆运动学(Inverse Kinematics, IK)是正运动学的反问题:"我想让手放到 (0.4, 0.3, 0.5) 这个点、朝某个方向,请问各关节要转多少度?"这才是实际控制里最常用的问题,因为人给的指令通常是"末端去哪",而不是"每个关节转几度"。IK 比 FK 难得多:可能有多个解、可能无解、还可能有奇异点。
| 解析法(闭式解) | 数值法(迭代解) | |
|---|---|---|
| 思路 | 用几何/三角关系直接推出公式 | 从初始猜测出发,一步步逼近目标 |
| 速度 | 极快(微秒级) | 较慢,依赖初值与迭代次数 |
| 能否求所有解 | 能,能看清解的个数与结构 | 一般只收敛到离初值最近的解 |
| 适用性 | 只对结构简单、有闭式解的臂有效 | 几乎任意结构都能用(通用) |
| 典型方法 | 三角几何、Pieper 条件(6 轴臂后 3 轴交于一点) | 雅可比迭代、阻尼最小二乘、LM 法 |
给末端目标 (x, y),两根连杆长 L1、L2。由余弦定理先求第二个关节角:
再由几何关系求 θ1:
末端同一点 → 两种手臂姿态:
θ2>0(肘在上) θ2<0(肘在下)
▲ ▲ ▲
/ \ /
L1/ \ L2 L1/
/ ▼ / \
肩└─────────┘终点 肩└────▼终点
(肘朝上) (肘朝下)当没有闭式解时,用迭代:假设当前关节角 q,末端误差 Δx = x_target − FK(q),每步解一个线性方程让末端往目标挪一点:
Δx = J(q) · Δq
Δq = J⁺ · Δx J⁺ 是 J 的伪逆(最小二乘解)
q ← q + Δq 更新后重算 FK,直到 ‖Δx‖ 足够小为了让迭代稳定(尤其在奇异点附近),常用阻尼最小二乘(DLS)加一个阻尼项 λ:
Δq = Jᵀ (J·Jᵀ + λ²I)⁻¹ · Δxλ 越大越稳但收敛越慢。数值法的坑是:初值选得差可能不收敛、或收敛到不是你想要的那个解。
把 5.2 的解析公式做成可动手的实验:IK 模式直接拖红色目标点,页面实时用余弦定理反解 θ1/θ2,"肘朝上/肘朝下"两组解同时画出(实线为当前解,虚线为另一组解);FK 模式改为拖关节角滑杆,看末端 (x, y) 落到哪。把目标拖出蓝色可达域,就能亲眼看到"c2 超界→无解";把手臂拖到接近伸直,观察 det(J)=L1·L2·sinθ2 趋近 0 的奇异位形。
正/逆运动学回答"位置对不对",雅可比矩阵 J回答"快不快、用多大力",它把关节速度和末端速度线性地联系起来,是机器人动力学的第一块基石。
v = J(q) · q̇
v = [vx, vy, vz, ωx, ωy, ωz]ᵀ 末端线速度 + 角速度(6×1)
q̇ = 关节角速度向量(n×1)
J = 6×n 矩阵,元素是 FK 对每个关节角的偏导 Jij = ∂fi/∂qj对 2 连杆平面臂(只关心末端 (x, y)),雅可比是 2×2:
J = [ -L1·s1 - L2·s12 -L2·s12 ]
[ L1·c1 + L2·c12 L2·c12 ]
(s1=sinθ1, s12=sin(θ1+θ2), c1=cosθ1, c12=cos(θ1+θ2))
行列式:det(J) = L1 · L2 · sin(θ2)当 det(J) = 0 时,矩阵不可逆,称为奇异位形(Singularity)。对 2 连杆臂,奇异发生在 θ2 = 0(臂完全伸直)或 θ2 = π(完全折回)时。此时:
雅可比还反向传递力。若末端要顶住外力/负载 F,各关节需要输出多大的力矩 τ?
τ = Jᵀ · F
末端 6 维力/力矩 F = [Fx, Fy, Fz, Mx, My, Mz]ᵀ
关节力矩 τ = [τ1, τ2, ..., τn]ᵀ直觉:末端挂重物时,力臂越长(离关节越远),关节要出的力矩越大——这正是 Jᵀ 里那堆 sin/cos 系数在做的事。同一位形下,位置映射用 J,力映射用 Jᵀ,这是一对"对偶"关系。选关节电机扭矩时,就要拿最恶劣位形下的 τ 峰值来选型。
走路可以抽象成:规划脚底落点(末端位姿)→ 用 IK 反算髋、膝、踝三个关节的角度。人腿的髋+膝+踝在同一平面内的部分,正好退化成2~3 连杆平面 IK,和本页 5.2 的公式几乎一样。工程上会先解出"脚相对于髋的位置",再套解析 IK 得到膝盖角;膝盖的弯曲方向(向前弯,不像鸟腿向后)就是"肘朝上/朝下"那个多解选择——选错了解,机器人会做出反关节的怪姿势。
人形机器人手臂(肩 3 + 肘 1 + 腕 3)是 7 自由度冗余机械臂——比到达一个位姿所需的 6 个自由度还多 1 个,所以同一末端位姿对应无穷多组关节角。这时 IK 不是"求唯一解",而是"在无数解里挑一个最好看的"(肘不夹身体、避开胸口、像人)。
| 工具 | 是什么 | 典型 API |
|---|---|---|
| Pinocchio | C++ 刚体动力学库,带高效 Python 绑定,人形/四足仿真底层最爱 | buildModelFromUrdf / forwardKinematics / computeJacobians |
| Robotics Toolbox for Python | Peter Corke 的教学向 Python 库,和经典 MATLAB 版一脉相承 | DHRobot / fkine / ikine_LM / jacob0 |
| URDF 解析 | 把机器人模型文件读成运动学树 | pinocchio.buildModelFromURDF / urdf_parser |
| MATLAB | Robotics System Toolbox,适合快速验证与论文复现 | rigidBodyTree / getTransform / inverseKinematics |
# 方式一:Robotics Toolbox for Python(pip install roboticstoolbox-python)
import roboticstoolbox as rtb
import numpy as np
arm = rtb.DHRobot([
rtb.RevoluteDH(a=0.5), # L1 = 0.5 m
rtb.RevoluteDH(a=0.3) # L2 = 0.3 m
], name="2link")
q = np.array([0.3, 0.9])
T = arm.fkine(q) # 正运动学 → 4×4 齐次变换
sol = arm.ikine_LM(T) # 逆运动学 → 数值解
J = arm.jacob0(q) # 雅可比
# 方式二:Pinocchio(conda-forge 的 pinocchio,或 PyPI 的 pin 包)
import pinocchio as pin
model = pin.buildModelFromUrdf("robot.urdf")
data = model.createData()
pin.forwardKinematics(model, data, q)
pin.computeJointJacobians(model, data, q)