第2章：坐标变换数学基础

各位同学好，我是老张。今天咱们聊聊坐标变换的数学基础。说实话，我刚入行那会儿，觉得旋转矩阵、欧拉角、四元数这些东西就是纯数学游戏，跟实际工程没啥关系。直到有一次做无人机飞控，因为欧拉角万向锁问题导致姿态解算出错，飞机差点炸了……嗯，从那以后我再也不敢小看这些基础了。

2.1 旋转矩阵：最直观的旋转表达

旋转矩阵，说白了就是一个3x3的方阵。它能把一个三维向量从一个坐标系映射到另一个坐标系。我个人习惯把旋转矩阵想象成「坐标系的旋转说明书」——你给它一个向量，它告诉你这个向量在新坐标系里长什么样。

举个例子，绕Z轴旋转θ角度的旋转矩阵长这样：

R_z(θ) = [
    [cosθ, -sinθ, 0],
    [sinθ,  cosθ, 0],
    [0,      0,    1]
]

绕X轴和Y轴的类似，我就不重复写了。你想想看，这三个基本旋转矩阵组合起来，就能表达任意姿态。

我在做惯性导航系统时，经常用旋转矩阵来转换载体坐标系和导航坐标系。比如加速度计测的是载体坐标系的加速度，要转换到导航坐标系才能积分算位置。这一步要是搞反了，位置误差会像滚雪球一样越滚越大。

2.2 欧拉角：最符合直觉的表达

欧拉角用三个角度来描述旋转：偏航角(yaw)、俯仰角(pitch)、横滚角(roll)。说白了就是「先转哪个轴，再转哪个轴，最后转哪个轴」。我刚开始学的时候觉得这玩意儿太直观了，比旋转矩阵好理解多了。

但这里有个大坑——万向锁(Gimbal Lock)。

常见的欧拉角旋转顺序有ZYX、ZYZ等。不同顺序对应的旋转矩阵不同，千万别搞混。我建议你在代码里明确注释旋转顺序，不然三个月后你自己都看不懂。

2.3 四元数：最优雅的旋转表达

四元数是个神奇的东西。它用四个数 q = (w, x, y, z) 来表示旋转，其中 w 是实部，x、y、z 是虚部。为什么说它优雅？因为它避免了万向锁，而且插值平滑，计算效率高。

四元数的核心公式：

q = cos(θ/2) + (u_x * i + u_y * j + u_z * k) * sin(θ/2)

其中 u 是旋转轴的单位向量，θ 是旋转角度。你看，一个四元数同时包含了旋转轴和旋转角的信息。

2.4 三种表达的转换关系

实际工程中，这三种表达经常需要互相转换。我整理了一个对照表：

转换方向	公式/方法	注意事项
旋转矩阵 → 欧拉角	从矩阵元素反解角度	注意万向锁时的奇异性
欧拉角 → 旋转矩阵	按顺序乘基本旋转矩阵	明确旋转顺序
四元数 → 旋转矩阵	用四元数元素构造矩阵	确保四元数已归一化
旋转矩阵 → 四元数	从矩阵迹和元素求解	注意数值稳定性

这里我贴一段Python代码，展示四元数转旋转矩阵的实现：

def quaternion_to_rotation_matrix(q):
    """
    四元数转旋转矩阵
    q: [w, x, y, z]
    """
    w, x, y, z = q
    # 归一化
    norm = np.sqrt(w**2 + x**2 + y**2 + z**2)
    w, x, y, z = w/norm, x/norm, y/norm, z/norm
    
    R = np.array([
        [1 - 2*y**2 - 2*z**2, 2*x*y - 2*w*z,     2*x*z + 2*w*y],
        [2*x*y + 2*w*z,       1 - 2*x**2 - 2*z**2, 2*y*z - 2*w*x],
        [2*x*z - 2*w*y,       2*y*z + 2*w*x,     1 - 2*x**2 - 2*y**2]
    ])
    return R

2.5 知识体系总览

下面这张SVG图，是我梳理的本章知识结构。你看一眼就能明白三种表达之间的关系和应用场景。

2.6 工程实践中的选择建议

说了这么多，到底该用哪种？我个人的经验是：

• 人机交互界面：用欧拉角。操作员需要直观理解姿态，偏航、俯仰、横滚一目了然。
• 姿态解算与滤波：用四元数。卡尔曼滤波里用四元数做状态量，没有奇异性问题。
• 坐标变换计算：用旋转矩阵。矩阵乘法在GPU上可以并行加速，适合批量处理。

好了，这一章的内容就到这里。记住，数学工具没有好坏之分，只有合不合适。选对了工具，你的工程实现会事半功倍。

---