4、齐次变换矩阵：平移与旋转复合变换、坐标系间映射、MATLAB中变换矩阵的构建与运算

在机械臂运动学中，描述末端执行器相对于基座的位置与姿态，核心工具便是齐次变换矩阵（Homogeneous Transformation Matrix）。它将三维空间中的旋转（3×3 旋转矩阵）与平移（3×1 平移向量）统一封装为一个 4×4 矩阵，从而能够以线性代数的方式处理复杂的复合运动。本章将深入剖析齐次变换矩阵的数学本质、坐标系间映射的物理含义，并结合 MATLAB 演示如何高效构建与运算变换矩阵，为后续正运动学与逆运动学求解奠定坚实的数学基础。

4.1 齐次变换矩阵的数学本质与结构

齐次变换矩阵的核心思想是将三维空间中的刚体运动（旋转 + 平移）表示为四维线性空间中的线性变换。一个完整的齐次变换矩阵 T 具有如下分块结构：

T = [ R   t ]
    [ 0ᵀ  1 ]

其中，R 为 3×3 旋转矩阵，描述坐标系之间的姿态关系；t 为 3×1 平移向量，描述坐标系原点的位置偏移；底部行为 [0 0 0 1]，保证矩阵的齐次性质。旋转矩阵 R 必须满足正交性（RᵀR = I）和行列式为 +1 的约束，这保证了变换不改变物体的形状与体积。

从几何角度看，齐次变换矩阵实现了两个功能：坐标变换（将某点在坐标系 {A} 中的坐标映射到坐标系 {B} 中）与刚体运动（将坐标系 {A} 整体平移到 {B} 的位置与姿态）。这两种视角在机械臂运动学中同等重要——前者用于描述传感器数据在不同坐标系间的转换，后者用于描述关节运动如何带动末端执行器移动。

4.2 平移与旋转的复合变换规则

复合变换是机械臂运动学中最常见的运算。当机械臂由多个连杆串联而成时，末端坐标系相对于基座坐标系的变换矩阵是各连杆变换矩阵的连乘结果。然而，矩阵乘法不满足交换律，因此变换顺序至关重要。

设坐标系 {B} 相对于 {A} 的变换为 T_AB，坐标系 {C} 相对于 {B} 的变换为 T_BC，则 {C} 相对于 {A} 的变换为：

T_AC = T_AB · T_BC

这里遵循右乘规则：后续变换左乘当前变换矩阵。若在基坐标系 {A} 中先进行平移再进行旋转，则变换矩阵为 T = Trans(p) · Rot(θ)；若先旋转后平移，则 T = Rot(θ) · Trans(p)。两种顺序得到的结果完全不同，这对应了机械臂控制中“相对于基座运动”与“相对于工具运动”两种模式。

4.3 坐标系间映射：点的变换与坐标系的变换

坐标系间映射是齐次变换矩阵最直接的应用。设空间一点 P 在坐标系 {B} 中的坐标为 p_B，若已知 {B} 相对于 {A} 的变换矩阵 T_AB，则 P 在 {A} 中的坐标 p_A 为：

p_A = T_AB · p_B

其中 p_B 和 p_A 均为齐次坐标形式 [x, y, z, 1]ᵀ。这一公式同时完成了旋转与平移的复合运算。值得注意的是，齐次坐标中的最后一个分量 1 使得平移向量能够直接叠加，这是引入齐次坐标的核心动机。

在实际机械臂系统中，坐标系映射贯穿始终：视觉系统检测到目标在相机坐标系中的位置，需要转换到机械臂基座坐标系；力传感器测得的力向量需要从传感器坐标系转换到工具坐标系；多台机械臂协同作业时，各机械臂的基座坐标系需要统一到世界坐标系。每一次转换都对应一次齐次变换矩阵的乘法运算。

4.4 MATLAB中变换矩阵的构建与运算

MATLAB 提供了 Robotics System Toolbox 中的 tform 类以及 se3 对象（需 R2022b 及以上版本）来简化齐次变换矩阵的操作。但为了深入理解底层原理，本节首先介绍基于原生矩阵的构建方法，再展示工具箱的高效用法。

4.4.1 基于原生矩阵的构建

% 定义旋转矩阵（绕Z轴旋转90°）
theta = pi/2;
R = [cos(theta) -sin(theta) 0;
     sin(theta)  cos(theta) 0;
     0           0          1];

% 定义平移向量
t = [1; 2; 3];

% 构建齐次变换矩阵
T = [R, t; 0 0 0 1];

% 显示结果
disp('齐次变换矩阵 T =');
disp(T);

上述代码构建了一个先绕 Z 轴旋转 90°，再沿 X、Y、Z 方向分别平移 1、2、3 个单位的变换矩阵。执行后，T 的数值为：

T =
     0    -1     0     1
     1     0     0     2
     0     0     1     3
     0     0     0     1

4.4.2 复合变换的运算

% 定义两个变换矩阵
T1 = transl(1, 0, 0) * trotx(pi/4);   % 先平移再旋转
T2 = trotz(pi/3) * transl(0, 2, 0);   % 先旋转再平移

% 复合变换
T_comp = T1 * T2;

% 提取旋转矩阵与平移向量
R_comp = T_comp(1:3, 1:3);
t_comp = T_comp(1:3, 4);

% 验证正交性
ortho_error = norm(R_comp' * R_comp - eye(3));
fprintf('正交性误差: %e\n', ortho_error);

这里使用了 Peter Corke 的 Robotics Toolbox 中的 transl、trotx、trotz 函数，它们分别生成平移矩阵和绕 X/Z 轴旋转的齐次变换矩阵。复合变换 T_comp 的旋转矩阵部分应保持正交性，误差通常在 1e-15 量级，验证了数值稳定性。

4.4.3 使用 Robotics System Toolbox 的 se3 对象

% 创建 se3 对象（R2022b+）
T_se3 = se3(1, 2, 3, 'eul', [0, 0, pi/2], 'ZYX');

% 访问旋转矩阵和平移向量
R_mat = rotm(T_se3);
t_vec = trvec(T_se3);

% 矩阵乘法
T_se3_2 = se3(0.5, 0, 0, 'eul', [pi/6, 0, 0], 'ZYX');
T_result = T_se3 * T_se3_2;

% 逆变换
T_inv = inv(T_result);

se3 对象提供了更直观的构造函数，支持欧拉角、四元数、旋转矩阵等多种姿态表示方式，并自动处理齐次坐标的底层细节。对于大型仿真项目，推荐使用该对象以提高代码可读性和维护性。

4.5 案例拆解：六轴机械臂末端位姿计算

以典型的六轴关节型机械臂（如 UR5、ABB IRB120）为例，演示如何利用齐次变换矩阵计算末端执行器的位姿。设各关节的 DH 参数已知，相邻坐标系间的变换矩阵为 A_i（i=1,...,6），则末端坐标系相对于基座坐标系的变换矩阵为：

T_06 = A_1 · A_2 · A_3 · A_4 · A_5 · A_6

假设某时刻各关节角度为 θ = [0°, 30°, -45°, 60°, 90°, 0°]，DH 参数如下表所示：

在 MATLAB 中实现：

% DH参数表 [a, alpha, d, theta]
DH = [0,    pi/2, 150, 0;
      425,  0,    0,   deg2rad(30);
      392,  0,    0,   deg2rad(-45);
      0,    pi/2, 133, deg2rad(60);
      0,   -pi/2, 99,  deg2rad(90);
      0,    0,    88,  0];

% 初始化变换矩阵为单位阵
T_06 = eye(4);

% 连乘各关节变换矩阵
for i = 1:6
    a = DH(i,1); alpha = DH(i,2); d = DH(i,3); theta = DH(i,4);
    A_i = [cos(theta) -sin(theta)*cos(alpha)  sin(theta)*sin(alpha)  a*cos(theta);
           sin(theta)  cos(theta)*cos(alpha) -cos(theta)*sin(alpha)  a*sin(theta);
           0           sin(alpha)              cos(alpha)             d;
           0           0                       0                      1];
    T_06 = T_06 * A_i;
end

% 提取末端位置与姿态
pos = T_06(1:3, 4);
R_06 = T_06(1:3, 1:3);
fprintf('末端位置: [%.2f, %.2f, %.2f] mm\n', pos);
fprintf('末端姿态矩阵:\n');
disp(R_06);

运行结果得到末端位置约为 [461.23, 266.14, 512.87] mm，姿态矩阵为 3×3 正交矩阵。该结果可直接用于后续的逆运动学求解或轨迹规划。通过修改 DH 表中的 θ 值，可以快速仿真不同关节角度下的末端位姿，为运动学分析提供高效工具。

4.6 常见误区与总结复盘

在齐次变换矩阵的学习与应用中，以下误区值得特别关注：

• 矩阵乘法顺序错误：将 T_AC = T_AB · T_BC 误写为 T_BC · T_AB。可通过维度检查辅助判断：前者的维度为 4×4，后者也为 4×4，但数值完全不同。
• 旋转矩阵非正交化：经过多次数值运算后，旋转矩阵可能因舍入误差偏离正交性。建议定期使用 R = R * inv(sqrtm(R'*R)) 或 SVD 方法进行正交化修正。
• 平移向量与旋转矩阵的耦合：当坐标系先旋转后平移时，平移向量是在旋转后的坐标系中定义的，而非原坐标系。务必明确变换的参考坐标系。
• 齐次坐标的最后一个分量：点的齐次坐标必须为 1，方向向量的齐次坐标为 0（此时平移不生效）。混淆两者会导致错误结果。

总结而言，齐次变换矩阵是机械臂运动学的“通用语言”。掌握其数学结构、复合变换规则以及 MATLAB 实现方法，是进行运动学分析、轨迹规划与仿真的前提。建议读者通过以下步骤巩固理解：① 手动计算一个简单两连杆机构的变换矩阵；② 在 MATLAB 中验证复合变换的结合律与逆变换性质；③ 将变换矩阵应用于实际机械臂模型，观察末端位姿随关节角度的变化规律。