第三节：欧拉角与旋转矩阵

各位同学，今天我们来聊聊三维空间里最基础、也最容易踩坑的一个话题——欧拉角与旋转矩阵。

说实话，我刚入行那会儿，觉得欧拉角特别直观。你想啊，一个物体在空间里怎么转？不就是绕X轴转一下、绕Y轴转一下、再绕Z轴转一下嘛。但后来在项目里被“万向锁”坑过一次之后，我才真正理解了这东西的脾气。

3.1 欧拉角的定义

欧拉角，说白了就是用三个角度来描述一个刚体在三维空间中的朝向。这三个角度分别对应绕三个坐标轴的旋转。

常见的定义方式有这么几种：

• ZYX顺序：先绕Z轴转（偏航Yaw），再绕Y轴转（俯仰Pitch），最后绕X轴转（翻滚Roll）。这是自动驾驶里最常用的，因为符合车辆的运动直觉。
• ZYZ顺序：先绕Z轴，再绕Y轴，再绕Z轴。这种在机器人臂的逆运动学里常见。
• XYZ顺序：按X、Y、Z的顺序依次旋转。

我个人习惯用ZYX顺序，因为做感知融合时，激光雷达和相机的外参标定经常用这个顺序。你想想看，一辆车在路上跑，偏航角决定了它朝哪开，俯仰角决定了它是不是在爬坡，翻滚角决定了它有没有侧倾——多直观。

3.2 万向锁问题

好，接下来我要讲一个让我当年在项目里熬夜调bug的罪魁祸首——万向锁。

什么是万向锁？简单说，就是当物体旋转到某个特定姿态时，会丢失一个旋转自由度。比如你用ZYX顺序，当俯仰角Pitch达到±90度时，偏航Yaw和翻滚Roll就“锁”在一起了，你没法单独控制其中一个。

为什么会这样？我给你画个图就明白了。

看到没？当Pitch=90°时，原本的X轴和Z轴在空间上重合了。这时候你再绕X轴转和绕Z轴转，效果是一样的。这就是万向锁。

3.3 旋转矩阵的推导

好，现在我们来看看旋转矩阵是怎么来的。其实推导过程不复杂，就是绕每个轴的基本旋转矩阵做乘法。

先看绕单个轴的旋转矩阵：

绕X轴旋转角度θ：

R_x(θ) = [1       0        0    ]
         [0   cos(θ)  -sin(θ) ]
         [0   sin(θ)   cos(θ) ]

绕Y轴旋转角度θ：

R_y(θ) = [ cos(θ)   0   sin(θ) ]
         [ 0        1   0      ]
         [-sin(θ)   0   cos(θ) ]

绕Z轴旋转角度θ：

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

如果你用ZYX顺序（先绕Z，再绕Y，再绕X），那么总的旋转矩阵就是：

R = R_x(roll) * R_y(pitch) * R_z(yaw)

注意乘法的顺序！矩阵乘法不满足交换律，顺序错了结果就完全不一样。我刚开始学的时候经常搞混，后来记住一个口诀：从右往左读，就是旋转的顺序。最右边的矩阵对应第一次旋转。

import numpy as np

def rotation_matrix_z(yaw):
    c, s = np.cos(yaw), np.sin(yaw)
    return np.array([[c, -s, 0],
                     [s,  c, 0],
                     [0,  0, 1]])

def rotation_matrix_y(pitch):
    c, s = np.cos(pitch), np.sin(pitch)
    return np.array([[ c, 0, s],
                     [ 0, 1, 0],
                     [-s, 0, c]])

def rotation_matrix_x(roll):
    c, s = np.cos(roll), np.sin(roll)
    return np.array([[1, 0, 0],
                     [0, c, -s],
                     [0, s,  c]])

# ZYX顺序
R = rotation_matrix_x(roll) @ rotation_matrix_y(pitch) @ rotation_matrix_z(yaw)

3.4 从旋转矩阵到欧拉角

反过来，如果你有一个旋转矩阵，怎么提取出欧拉角？这也是实际项目中经常遇到的需求。

对于ZYX顺序，提取公式如下：

pitch = arcsin(-R[2][0])
yaw   = atan2(R[1][0], R[0][0])
roll  = atan2(R[2][1], R[2][2])

注意，这里有个坑：当pitch接近±90°时，R[0][0]和R[2][2]会趋近于0，导致atan2的参数不稳定。这就是万向锁在数值上的体现。

if abs(pitch) > 89.5 * pi/180:
    # 进入万向锁区域，需要特殊处理
    # 通常的做法是：将yaw设为0，roll = atan2(R[0][1], R[1][1])
    yaw = 0
    roll = atan2(R[0][1], R[1][1])

3.5 总结一下

欧拉角直观但有限制，旋转矩阵通用但参数多。在实际的感知融合项目中，我一般这样用：

• 可视化调试：用欧拉角，因为人眼看得懂
• 内部计算：用旋转矩阵或四元数，避免万向锁
• 数据存储：用四元数，因为紧凑且无奇点

嗯，这一节的内容就到这里。记住，坐标系变换是感知融合的基石，搞不清楚旋转矩阵，后面的外参标定、点云投影都会出问题。