4、刚体变换基础：旋转矩阵、欧拉角、四元数、齐次变换矩阵

机械臂运动学的核心是描述刚体在三维空间中的位置与姿态。无论是正运动学求解、逆运动学迭代，还是轨迹规划中的插补计算，都离不开刚体变换的数学工具。本章围绕旋转矩阵、欧拉角、四元数和齐次变换矩阵四种表达方式，厘清它们之间的数学关系、适用场景与转换方法，并结合MATLAB的Robotics System Toolbox与Navigation Toolbox，演示旋转矩阵与四元数之间的工程化转换函数。理解本章内容，是后续建立DH参数表、求解雅可比矩阵、进行轨迹仿真的前提。

4.1 旋转矩阵：刚体姿态的数学基石

4.1.1 核心原理

旋转矩阵 R 是一个 3×3 的正交矩阵，满足 R^TR = I 且 det(R) = +1。其列向量表示旋转后坐标系各轴在原坐标系中的方向余弦。例如，绕 Z 轴旋转 θ 角的旋转矩阵为：

Rz(θ) = [cosθ  -sinθ  0;
         sinθ   cosθ  0;
         0      0     1]

旋转矩阵的优点是数学性质优良，可直接用于向量旋转（v' = R·v）和坐标系复合（R = R1·R2）。但缺点是参数冗余——9 个元素只有 3 个独立自由度，且不直观，人无法直接从矩阵数值看出旋转角度。

4.1.2 判断方法与操作流程

在MATLAB中判断一个矩阵是否为合法旋转矩阵，可使用以下流程：

1. 检查矩阵维度是否为 3×3；
2. 计算 R'*R - eye(3)，若范数小于 1e-6 则满足正交性；
3. 计算 det(R)，应接近 +1；
4. 若上述条件均满足，则确认其为旋转矩阵。

实际工程中，由于浮点误差，常使用 R'*R ≈ I 的容差判断，而非严格相等。

4.1.3 案例拆解：从旋转矩阵提取旋转轴与角度

假设某机械臂末端执行器的旋转矩阵为：

R = [0.8660  -0.5000  0;
     0.5000   0.8660  0;
     0        0       1]

该矩阵对应绕 Z 轴旋转 30°。通过 rotm2axang(R) 可得到轴角表示：轴为 [0,0,1]，角度为 0.5236 rad（即 30°）。在轨迹规划中，这种提取方式常用于将旋转矩阵转换为更紧凑的插值参数。

4.2 欧拉角：直观但存在奇异性

4.2.1 核心原理

欧拉角将任意旋转分解为三个依次绕坐标轴的旋转。常见的 ZYX 顺序（偏航-俯仰-滚转）定义为：先绕 Z 轴转偏航角 ψ，再绕新 Y 轴转俯仰角 θ，最后绕新 X 轴转滚转角 φ。其复合旋转矩阵为：

R = Rz(ψ) · Ry(θ) · Rx(φ)

欧拉角的优势是直观，适合人机交互和姿态显示。但存在万向锁问题：当 θ = ±90° 时，ψ 和 φ 的旋转轴重合，丢失一个自由度，导致姿态奇异。

4.2.2 判断方法与操作流程

在MATLAB中，使用 eul2rotm(eul, 'ZYX') 将欧拉角转换为旋转矩阵，使用 rotm2eul(R, 'ZYX') 进行逆转换。需注意：

• 欧拉角单位默认为弧度，若使用角度需先转换；
• 旋转顺序参数必须显式指定，不同顺序（如 'ZYZ'、'XYZ'）结果完全不同；
• 当俯仰角接近 ±90° 时，输出偏航角和滚转角可能跳变，需进行角度归一化处理。

4.2.3 案例拆解：机械臂腕部姿态的欧拉角表示

某六轴机械臂的腕部姿态要求为：偏航 45°、俯仰 30°、滚转 60°（ZYX顺序）。在MATLAB中：

eul = [deg2rad(45), deg2rad(30), deg2rad(60)];
R = eul2rotm(eul, 'ZYX');
% 验证
eul_back = rotm2eul(R, 'ZYX');
assert(all(abs(eul - eul_back) < 1e-10));

该案例展示了欧拉角与旋转矩阵的无损转换。但在实际轨迹插补中，若直接对欧拉角进行线性插值，当角度跨越 ±180° 边界时会产生不必要的旋转路径，因此工程上更推荐使用四元数插值。

4.3 四元数：无奇异性的紧凑表示

4.3.1 核心原理

四元数 q = (w, x, y, z) 是复数在四维空间的推广，其中 w 为标量部分，(x, y, z) 为矢量部分。单位四元数（范数为 1）可用于表示旋转，其与轴角的关系为：

q = (cos(θ/2), sin(θ/2)·n)

其中 n 为旋转轴单位向量，θ 为旋转角度。四元数乘法 q1 ⊗ q2 对应旋转的复合，且无万向锁问题。四元数插值（如 SLERP）可生成平滑的旋转路径，是轨迹规划的首选。

4.3.2 判断方法与操作流程

在MATLAB中，四元数对象使用 quaternion(w, x, y, z) 创建，注意构造函数参数顺序为 w,x,y,z。常用转换函数包括：

4.3.3 案例拆解：四元数插值实现末端姿态平滑过渡

设机械臂末端从姿态 A（旋转矩阵 R_A）运动到姿态 B（旋转矩阵 R_B），要求中间插补 10 个点。使用四元数 SLERP 插值：

qA = rotm2quat(R_A);
qB = rotm2quat(R_B);
t = linspace(0, 1, 10);
q_interp = slerp(qA, qB, t);
% 将插值结果转回旋转矩阵用于逆运动学求解
for i = 1:10
    R_interp(:,:,i) = quat2rotm(q_interp(i));
end

对比直接对欧拉角线性插值，四元数插值保证了旋转轴恒定、角速度均匀，避免了中间姿态的“抖动”现象。该案例在轨迹规划模块中会反复使用。

4.4 齐次变换矩阵：位置与姿态的统一表达

4.4.1 核心原理

齐次变换矩阵 T 是 4×4 矩阵，形式为：

T = [R  t;
     0  1]

其中 R 为 3×3 旋转矩阵，t 为 3×1 平移向量。齐次变换矩阵允许将旋转和平移统一为矩阵乘法，从而通过 T = T1 · T2 实现坐标系链式变换。在机械臂中，相邻连杆间的变换矩阵 T_i 由 DH 参数决定，正运动学即为 T_0n = T₁·T₂·…·T_n。

4.4.2 判断方法与操作流程

在MATLAB中，使用 tform = rt2tform(R, t) 构造齐次矩阵，使用 [R, t] = tform2rt(tform) 分解。验证齐次矩阵合法性需检查：

1. 左上 3×3 子块为正交矩阵；
2. 最后一行固定为 [0 0 0 1]；
3. 平移向量 t 为任意实数。

4.4.3 案例拆解：机械臂正运动学中的齐次变换链

以六轴机械臂为例，已知各关节的 DH 参数，计算末端位姿：

% 假设已计算各连杆变换矩阵 T1, T2, ..., T6
T_0n = T1 * T2 * T3 * T4 * T5 * T6;
% 提取末端位置与姿态
pos = tform2trvec(T_0n);   % 位置向量
R_end = tform2rotm(T_0n);  % 旋转矩阵
% 转换为四元数用于后续轨迹规划
q_end = rotm2quat(R_end);

该流程展示了齐次矩阵作为“容器”的作用——它将旋转矩阵和平移向量打包，使得多坐标系变换只需简单的矩阵乘法。在逆运动学中，给定目标 T 后，通过数值迭代求解关节角。

4.5 四种表达方式的对比与工程选型

工程选型建议：在运动学求解和坐标系变换中使用齐次矩阵；在轨迹规划中使用四元数进行姿态插值；在用户界面显示中使用欧拉角；在数学推导和优化问题中使用旋转矩阵。MATLAB 的 rigidtform3d 对象可同时封装旋转与平移，并提供 tform2quat、tform2eul 等便捷转换方法，建议优先使用。