机器人运动学中的刚体变换与齐次矩阵

可视化演示

刚体变换本质

舞者在舞台上，无论怎么移动和转身，她的身体始终保持原样——不会拉伸、不会扭曲。这种“不变形”的物体，在机器人学中被称为刚体。机械臂的每一段金属连杆就是刚体，它们通过关节连接，每个关节只能让连杆旋转或滑动，但连杆本身绝不会弯曲变形。

要完整描述一个刚体在空间中的状态，我们需要两个信息：它在哪儿（位置），它面朝哪个方向（姿态）。位置好理解，就是坐标；姿态则描述刚体坐标系相对于参考坐标系的旋转关系。在机器人学中，这两个信息合在一起称为位姿（Pose）。

刚体变换的本质，就是从一个位姿到另一个位姿的映射，它可以分解为两步：先旋转（改变姿态），再平移（改变位置）。旋转用旋转矩阵 R 表示，平移用平移向量 t 表示。这样，空间中任意一点在新坐标系下的坐标，就是老坐标先旋转、再平移的结果。

这个公式虽然优雅，但旋转和平移分开写，在连续变换时非常繁琐。正如舞者连续完成多个动作时，导演需要一个更简洁的记录方式——这就引出了齐次矩阵的思想。

旋转矩阵推导

舞者转身的角度记录在旋转矩阵中。先从最简单的二维情况开始：舞者在舞台上转身 θ 角度。我们可以用极坐标来推导。设舞者手中捧着一盏灯，灯在原坐标系中的坐标为 $(x, y)$。用极坐标表示，令 $x = r\cos\phi$，$y = r\sin\phi$，其中 $r$ 是到原点的距离，$\phi$ 是初始角度。旋转 θ 角后，灯的新坐标为 $(x', y')$，对应的极角变为 $\phi + \theta$。展开三角公式，就能得到旋转矩阵的表达式。

二维旋转矩阵的两个列向量是正交的，且模长为 1，这就是正交矩阵的性质。推广到三维，绕 $z$ 轴旋转的矩阵保持不变，绕 $x$、$y$ 轴旋转的矩阵也类似。正交矩阵有一个重要性质：它的逆矩阵等于转置矩阵，即 $\mathbf{R}^{-1} = \mathbf{R}^{T}$。这意味着“转回去”的矩阵就是旋转矩阵的转置——舞者逆时针转了 θ，要回到原姿态只需顺时针转 θ 即可。

齐次坐标引入

问题来了：旋转可以用矩阵乘法表示，但平移呢？如果用 $\mathbf{p}' = \mathbf{R}\mathbf{p} + \mathbf{t}$，每次变换都要做一次矩阵乘法和一次加法。更麻烦的是，多次变换嵌套时，表达式会变得越来越长——就像导演的场记本上写满了一堆“先走再转再走”的批注，而不是一张干净的表格。

齐次坐标就是解决这个问题的关键技巧。它做的是一件看似“多此一举”的事：给二维点加一个维度。原本 $(x, y)$ 变成 $(x, y, 1)$，原本 $(x, y, z)$ 变成 $(x, y, z, 1)$。这样，平移向量 $t$ 就能“塞进”矩阵的最后一列，平移操作也变成了矩阵乘法。所有的变换都可以统一为：

齐次坐标的“升维”看似绕路，实则大道至简——所有变换统一为矩阵乘法，这让计算机的批量运算变得极其高效。正如导演将每个舞步写成一行标准格式的记录，所有变换都能用同一套规则计算了。

变换矩阵构造

现在我们要把齐次变换矩阵“拼装”起来——就像导演将“转身”和“走位”两个指令写进同一行场记。一个 4×4 的齐次变换矩阵 $\mathbf{T}$ 由四个子块构成：

构造变换矩阵时，要特别注意“先旋转后平移”的顺序。如果先平移再旋转，结果会完全不同。这就像导演对舞者下指令：“先向右走两步，再转身九十度”和“先转身九十度，再向右走两步”，最终的位置截然不同。

坐标系链式法则

机械臂有多个关节，每个关节都有自己的坐标系。要算出末端执行器在基坐标系中的位姿，必须沿着运动链一节一节地“叠”变换矩阵。这就像导演要算出舞者手中的花束在舞台上的最终位置：需要先追踪舞者从舞台原点走到位置 A，再追踪她的手从肩膀位置 A 移动到位置 B，最后追踪花束从手的位置 B 移到花尖位置 C。每一步都是一次刚体变换。

这些变换矩阵的“叠”法是左乘：从基坐标系开始，每经过一个关节，就在前面乘以该关节的变换矩阵。

链式法则的物理含义是：机械臂末端的位姿，是所有关节从基座开始逐级累加的结果。这就好比舞者的每个“转身”和“移步”都记录在案，而最终她在舞台上的位置和面向，是所有这些动作按先后顺序叠加出来的。对机器人工程师来说，这个连乘过程就是正运动学——给定每个关节的角度，算出末端的位置和姿态。

DH参数表

你可能已经意识到，如果每个关节都要手动构造变换矩阵，那将是一场灾难。幸运的是，1955 年 Denavit 和 Hartenberg 提出了一套标准化方法，用四个参数就能描述相邻连杆之间的变换关系。这就是著名的 DH 参数，它像一张身份证一样唯一地刻画了每个连杆的几何特征。

四个参数分别是：连杆长度 $a_i$（沿 $x_i$ 轴从 $z_{i-1}$ 移动到 $z_i$ 的距离）、连杆扭转 $\alpha_i$（绕 $x_i$ 轴从 $z_{i-1}$ 旋转到 $z_i$ 的角度）、连杆偏距 $d_i$（沿 $z_{i-1}$ 轴从 $x_{i-1}$ 移动到 $x_i$ 的距离）、关节角 $\theta_i$（绕 $z_{i-1}$ 轴从 $x_{i-1}$ 旋转到 $x_i$ 的角度）。对于旋转关节，$\theta_i$ 是变量；对于移动关节，$d_i$ 是变量。

有了这四个参数，相邻坐标系的变换矩阵可以标准化地写为：

这四次变换全部是绕标准轴或沿标准轴的简化操作，因此每个变换矩阵都很容易写出来，而且它们相乘的结果也保持简洁。工程师们通常预先算好每个连杆的 DH 参数，放入表格中，运动学求解时直接查表即可，不必从零开始推导。

正运动学求解

现在我们可以完成本节最激动人心的部分：给定机械臂每个关节的角度，算出末端执行器在基坐标系下的位置和姿态。这个过程叫正运动学（Forward Kinematics）。就像导演根据场记本上记录的每个舞步，推算出舞者最终站在舞台的哪个位置。

以常见的六轴机械臂为例，我们通过 DH 表查出六个连杆的参数，然后连乘六个变换矩阵：

其中 $\mathbf{T}_{06}$ 的左上角 3×3 子块给出末端的姿态（朝向），右上角 3×1 子块给出末端的位置（坐标）。这就是完整的正运动学解。

正运动学是一个直接了当的过程：查表、算矩阵、连乘、提取结果。它没有多解性问题——给定一组关节角，末端位姿是唯一的。这就像导演按照场记本一步步执行，舞者最终的站位是确定的。

逆运动学初探

正运动学告诉我们“舞者站在哪儿”，而逆运动学（Inverse Kinematics）则相反：我们知道了末端要到达某个位置，反推每个关节该转多少角度。这在实践中更为关键——工程师设计的每一个动作，都是从末端目标反推关节角度的。

逆运动学的数学形式是：给定目标位姿 $\mathbf{T}_{goal}$，求解满足下式的关节角 $\theta_1, \theta_2, \ldots, \theta_n$：

逆运动学可能有多解、无解或无穷多解的情况。比如说，一个动作可能有多种关节组合都能实现，舞者可以“摆手到腰”也可以“转肩到腰”，结果相同但过程不同。这些解的取舍涉及避障、能耗和关节限位等工程约束。不过，逆运动学问题最常用的数值解法——雅可比矩阵迭代法，其中的核心思想依然是基于正运动学的链式法则和刚体变换。因此，掌握好刚体变换与齐次矩阵，是你攻克逆运动学、乃至整个机器人控制理论的基石。

AI 应用场景

Python 演示

import numpy as np def rot_z(theta):    """绕z轴旋转矩阵，theta为弧度"""    c, s = np.cos(theta), np.sin(theta)    return np.array([        [c, -s, 0],        [s, c, 0],        [0, 0, 1]    ]) def rot_x(alpha):    """绕x轴旋转矩阵，alpha为弧度"""    c, s = np.cos(alpha), np.sin(alpha)    return np.array([        [1, 0, 0],        [0, c, -s],        [0, s, c]    ]) def dh_transform(a, alpha, d, theta):    """根据DH参数构造相邻关节的齐次变换矩阵"""    rz = np.eye(4)    rz[:3, :3] = rot_z(theta)    tz = np.eye(4)    tz[2, 3] = d    tx = np.eye(4)    tx[0, 3] = a    rx = np.eye(4)    rx[:3, :3] = rot_x(alpha)    return rz @ tz @ tx @ rx def forward_kinematics(dh_params):    """正运动学：连乘所有DH变换矩阵"""    T = np.eye(4)    for a, alpha, d, theta in dh_params:        T = T @ dh_transform(a, alpha, d, theta)    return T # 以2连杆平面机械臂为例 (单位: 米, 弧度)# 连杆1: a=1.0, alpha=0, d=0, theta=theta1# 连杆2: a=0.8, alpha=0, d=0, theta=theta2theta1 = np.radians(30)theta2 = np.radians(45)dh_params = [    (1.0, 0, 0, theta1),    (0.8, 0, 0, theta2),] T_end = forward_kinematics(dh_params)print("末端位置:", T_end[:3, 3])print("末端姿态矩阵:\n", T_end[:3, :3]) # 验证: 解析解 x = l1*cos(t1) + l2*cos(t1+t2)x = 1.0*np.cos(theta1) + 0.8*np.cos(theta1+theta2)y = 1.0*np.sin(theta1) + 0.8*np.sin(theta1+theta2)print("解析解位置: [{:.4f}, {:.4f}]".format(x, y)) # 可视化末端轨迹: 扫过theta1, 固定theta2print("\n轨迹采样:")for t1_deg in range(0, 91, 30):    t1 = np.radians(t1_deg)    T = forward_kinematics([(1.0, 0, 0, t1), (0.8, 0, 0, theta2)])    print("theta1={:3d}deg -> ({:.3f}, {:.3f})".format(t1_deg, T[0, 3], T[1, 3]))

1. def dh_transform(a, alpha, d, theta):这是 DH 参数法的核心函数：接收四个 DH 参数，返回一个 4×4 的齐次变换矩阵。每次调用，机械臂每根连杆的“身份证”都被转换为一座连接相邻坐标系的桥梁。
2. return rz @ tz @ tx @ rx严格按照“先绕 z 旋转、再沿 z 平移、再沿 x 平移、最后绕 x 旋转”的顺序相乘，顺序错了，矩阵就错了——这和“先转身再走”还是“先走再转身”的区别一样。
3. def forward_kinematics(dh_params):正运动学函数：用一个单位矩阵 I 作为起点（基坐标系），每经过一个连杆就左乘一个变换矩阵。
4. T = T @ dh_transform(a, alpha, d, theta)链式法则的代码实现：矩阵连乘。注意是 T @ 新矩阵 而不是反过来，因为左乘顺序反映了从基座到末端的传递方向。
5. T_end[:3, 3]从变换矩阵中提取末端位置向量（第 4 列的前三个元素），这正是末端执行器在基坐标系中的坐标。
6. T_end[:3, :3]提取末端姿态矩阵（左上 3×3），它告诉你末端坐标系各轴在基坐标系中的指向。

常见误区

面试题