数学基础回顾（一）：向量与坐标系、旋转矩阵、欧拉角与四元数基础概念

做机器人运动控制，说白了就是跟空间位置和姿态打交道。我见过不少新手，一上来就猛调PID，结果机械臂末端就是走不准——问题往往出在数学基础上。这一章咱们把向量、坐标系、旋转矩阵、欧拉角和四元数这几个硬骨头啃下来。别怕，我会结合项目里的实际场景来讲。

1. 向量与坐标系——描述位置的起点

向量就是个有方向、有大小的量。在三维空间里，我们用 [x, y, z]ᵀ 表示一个点相对于原点的位置。坐标系呢，就是给这个向量一个参考基准。

我习惯把坐标系想象成一把尺子。你拿这把尺子量出来的数据，只有在尺子自己的参考系里才有意义。比如机械臂底座有一个坐标系，末端执行器也有一个坐标系。同一个空间点，在不同坐标系下的坐标值是完全不同的。

概念	说明	工程意义
世界坐标系	固定在地面的全局参考系	所有运动规划的最终基准
关节坐标系	附着在每个关节上的局部系	描述相邻连杆的相对运动
工具坐标系	附着在末端执行器上的局部系	直接控制焊枪、夹爪等工具

嗯，这里要注意：坐标系变换是逆运动学里最频繁的操作。你从传感器读到的数据，往往需要先变换到世界坐标系，才能参与运动学计算。

2. 旋转矩阵——描述姿态的硬核工具

旋转矩阵是一个 3×3 的正交矩阵，行列式为 +1。它描述了一个坐标系相对于另一个坐标系的旋转关系。说白了，就是告诉你「怎么转才能让两个坐标系对齐」。

我记得第一次在STM32上实现旋转矩阵乘法时，踩过一个坑：浮点数精度累积导致矩阵不再正交。后来我每次更新旋转矩阵后，都会做一次正交化修正，代码大概长这样：

// 旋转矩阵正交化（Gram-Schmidt 方法）
void normalize_rotation_matrix(float R[3][3]) {
    float dot;
    // 归一化 X 轴
    float norm_x = sqrtf(R[0][0]*R[0][0] + R[1][0]*R[1][0] + R[2][0]*R[2][0]);
    for (int i = 0; i < 3; i++) R[i][0] /= norm_x;
    // 用 X 轴修正 Y 轴
    dot = R[0][0]*R[0][1] + R[1][0]*R[1][1] + R[2][0]*R[2][1];
    for (int i = 0; i < 3; i++) R[i][1] -= dot * R[i][0];
    float norm_y = sqrtf(R[0][1]*R[0][1] + R[1][1]*R[1][1] + R[2][1]*R[2][1]);
    for (int i = 0; i < 3; i++) R[i][1] /= norm_y;
    // Z 轴由叉积得到
    R[0][2] = R[1][0]*R[2][1] - R[2][0]*R[1][1];
    R[1][2] = R[2][0]*R[0][1] - R[0][0]*R[2][1];
    R[2][2] = R[0][0]*R[1][1] - R[1][0]*R[0][1];
}

3. 欧拉角——直观但暗藏杀机

欧拉角用三个角度（比如偏航-俯仰-横滚，即 Yaw-Pitch-Roll）来描述旋转。优点是直观，看一眼数值就知道大概朝向。但缺点也很致命：万向锁。

你想想看，当俯仰角接近 ±90° 时，偏航和横滚的旋转轴会重合，丢失一个自由度。我在做六轴机械臂的示教器时，就遇到过这个问题——操作员在某个姿态下拖动机械臂，末端突然「卡住」了，怎么拧都不动。其实就是欧拉角表示进入了奇异区。

4. 四元数——稳定可靠的旋转利器

四元数可以理解为「带约束的复数在三维空间的推广」。一个四元数 q = w + xi + yj + zk，其中 w 是实部，(x, y, z) 是虚部，且满足 w² + x² + y² + z² = 1。

它的最大优势是：无奇异、可平滑插值。SLERP（球面线性插值）就是基于四元数的，能让机械臂末端从姿态 A 平滑过渡到姿态 B，中间不会出现抖动或突变。

我个人习惯在 STM32 上维护一个四元数结构体，所有姿态运算都用它：

typedef struct {
    float w;
    float x;
    float y;
    float z;
} quaternion_t;

// 四元数乘法（组合旋转）
quaternion_t quat_mult(quaternion_t a, quaternion_t b) {
    quaternion_t res;
    res.w = a.w*b.w - a.x*b.x - a.y*b.y - a.z*b.z;
    res.x = a.w*b.x + a.x*b.w + a.y*b.z - a.z*b.y;
    res.y = a.w*b.y - a.x*b.z + a.y*b.w + a.z*b.x;
    res.z = a.w*b.z + a.x*b.y - a.y*b.x + a.z*b.w;
    return res;
}

5. 三种表示方法的对比与选择

特性	旋转矩阵	欧拉角	四元数
存储空间	9 个浮点数	3 个浮点数	4 个浮点数
直观性	差	好	差
奇异问题	无	有（万向锁）	无
插值平滑性	差	差	好（SLERP）
计算效率	中等	高	高
工程推荐场景	坐标变换链	人机交互界面	运动规划与控制

说白了，没有银弹。我在实际项目中通常是：内部运算全用四元数，人机界面显示用欧拉角，坐标变换链用旋转矩阵。三者之间需要频繁互转，所以转换函数一定要写好、测透。

这一章的内容是后续所有运动学计算的基石。向量和坐标系帮你定位，旋转矩阵、欧拉角、四元数帮你定姿。把这几个概念吃透，后面讲 DH 参数、正逆运动学推导时，你才不会觉得云里雾里。