📚 课程目录第3章机械臂数学基础

机械臂数学基础：空间描述与变换

机械臂运动学研究的核心问题是：如何描述机械臂各连杆在空间中的位置与姿态，以及如何在不同坐标系之间进行变换。无论是正运动学求解、逆运动学迭代，还是轨迹规划中的插补计算，都离不开一套严谨的数学描述工具。本章将系统梳理位置矢量、姿态矩阵、欧拉角、四元数与齐次变换矩阵的核心概念、相互转换关系及工程应用要点，为后续MATLAB/Simulink仿真建模打下坚实的数学地基。

3.1 位置矢量与坐标系的建立

在三维空间中，任意一点 P 相对于某个坐标系 {A} 的位置可以用一个 3×1 的列矢量 ^A P = [p_x, p_y, p_z]^T 来表示。这个矢量是后续所有变换的基础。在机械臂建模中，我们通常在每个关节处固连一个坐标系，用 D-H 参数法（Denavit-Hartenberg）或旋量理论来定义相邻坐标系之间的相对关系。位置矢量的物理意义非常直观，但工程中容易忽略的是参考坐标系的选择——同一个点在不同坐标系下的坐标值完全不同，因此所有矢量运算必须明确标注参考系。

  核心要点：位置矢量描述的是“点在哪里”，而姿态描述的是“坐标系如何摆放”。两者合在一起，才能完整描述机械臂末端执行器在空间中的位姿（位置+姿态）。

3.2 姿态描述：旋转矩阵

旋转矩阵 R 是一个 3×3 的正交矩阵，满足 R^T R = I 且 det(R) = +1。它的列向量表示被旋转坐标系 {B} 的三个主轴在参考坐标系 {A} 中的方向余弦。例如，绕 Z 轴旋转 θ 角的旋转矩阵为：

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

旋转矩阵的优点是无奇异问题，且可以直接用于矢量旋转运算 ^A P = R * ^B P。但其缺点是参数冗余（9个元素只有3个自由度），且不直观，人眼很难从9个数字中想象出姿态。

描述方式	参数个数	优点	缺点
旋转矩阵	9	无奇异、运算稳定	冗余、不直观
欧拉角	3	直观、易理解	有万向锁问题
四元数	4	无奇异、插值平滑	不直观、需转换

3.3 欧拉角与万向锁

欧拉角通过三次绕坐标轴的旋转来描述姿态，常用的有 ZYX 顺序（即 RPY 角：Roll-横滚、Pitch-俯仰、Yaw-偏航）。其旋转矩阵为 R = Rz(φ) * Ry(θ) * Rx(ψ)。欧拉角的优势在于参数少、物理意义明确，非常适合人机交互界面中的姿态输入。然而，当中间旋转角 θ = ±90° 时，第一次和第三次旋转会绕同一轴，导致自由度丢失，即万向锁（Gimbal Lock）问题。此时系统无法唯一确定姿态，在轨迹规划中会引起奇异。

工程风险：在机械臂控制中，如果使用欧拉角进行实时插补，当机械臂经过 pitch = ±90° 附近时，角速度会急剧增大，可能导致电机过流或轨迹偏差。务必在仿真阶段检测并规避此类姿态。

3.4 四元数：鲁棒的姿态表示

四元数 q = [x, y, z, w] 是一种扩展复数形式，其中 w 为标量部分，(x,y,z) 为矢量部分。单位四元数满足 x² + y² + z² + w² = 1，可以表示三维空间中的任意旋转。相比欧拉角，四元数不存在奇异问题，且支持球面线性插值（Slerp），非常适合轨迹规划中的姿态平滑过渡。在 MATLAB 中，可以使用 quat2rotm 和 rotm2quat 进行转换。

经验建议：在 Simulink 中做轨迹规划时，推荐使用四元数作为内部姿态表示，仅在用户输入/输出界面转换为欧拉角。这样可以避免万向锁问题，同时保证插值平滑。

3.5 齐次变换矩阵：统一位姿描述

齐次变换矩阵 T 是一个 4×4 矩阵，将旋转和位移统一在一个矩阵中：

T = [R   p;
     0   1]

其中 R 为 3×3 旋转矩阵，p 为 3×1 位置矢量。齐次变换矩阵的乘法可以串联多个坐标系变换，例如 ^A T_C = ^A T_B * ^B T_C，这构成了机械臂正运动学的基础。在 MATLAB 中，可以使用 Robotics System Toolbox 中的 tform 或 se3 对象来管理齐次变换。

3.6 实战案例：六轴机械臂末端位姿计算

假设某六轴机械臂的 D-H 参数已给定，我们需要计算末端执行器相对于基座标系的位姿。已知各关节角度为 θ = [0°, 30°, -45°, 60°, 90°, 0°]，连杆参数如下表：

关节 i	a_{i-1} (mm)	α_{i-1} (°)	d_i (mm)	θ_i (°)
1	0	0	150	0
2	50	-90	0	30
3	200	0	0	-45
4	50	-90	180	60
5	0	90	0	90
6	0	-90	50	0

在 MATLAB 中，我们可以逐级计算齐次变换矩阵：

% 定义D-H参数
a = [0, 50, 200, 50, 0, 0];
alpha = [0, -90, 0, -90, 90, -90];
d = [150, 0, 0, 180, 0, 50];
theta = [0, 30, -45, 60, 90, 0];

T = eye(4);
for i = 1:6
    % 计算相邻坐标系变换
    Ti = [cosd(theta(i)), -sind(theta(i))*cosd(alpha(i)), sind(theta(i))*sind(alpha(i)), a(i)*cosd(theta(i));
          sind(theta(i)), cosd(theta(i))*cosd(alpha(i)), -cosd(theta(i))*sind(alpha(i)), a(i)*sind(theta(i));
          0, sind(alpha(i)), cosd(alpha(i)), d(i);
          0, 0, 0, 1];
    T = T * Ti;
end
disp('末端位姿矩阵 T =');
disp(T);

运行上述代码后，我们得到末端位姿矩阵。假设计算结果为：

T = [0.8660  -0.2500   0.4330  120.5;
     0.5000   0.4330  -0.7500   85.3;
     0.0000   0.8660   0.5000  210.0;
     0        0        0        1.0000]

从矩阵中我们可以提取末端位置 p = [120.5, 85.3, 210.0] mm，以及旋转矩阵 R。进一步，我们可以使用 rotm2eul 将旋转矩阵转换为 ZYX 欧拉角，得到 RPY 角约为 (30°, 60°, 90°)。这个案例展示了从 D-H 参数到末端位姿的完整计算流程，也是后续逆运动学求解的验证基准。

  案例复盘：在实际工程中，建议将上述计算封装为函数 fkine(theta)，便于在轨迹规划中反复调用。同时，注意检查旋转矩阵的正交性（R*R' ≈ I），避免数值误差累积。

3.7 常见误区与总结

误区一：混淆参考坐标系。所有位置矢量和旋转矩阵都必须明确标注参考系，否则计算结果毫无意义。
误区二：忽视万向锁。在轨迹规划中，如果使用欧拉角插值，必须检查路径是否经过奇异姿态。
误区三：四元数未归一化。非单位四元数会导致旋转缩放，必须在使用前归一化。
误区四：齐次变换矩阵顺序错误。矩阵乘法不满足交换律，T1*T2 ≠ T2*T1，必须严格按照运动链顺序相乘。