旋转矩阵与欧拉角：三维空间中的旋转表示，万向锁问题

说到三维空间里的旋转，这可能是我们感知工程师每天都要打交道的东西。你想想看，一辆车在路上跑，摄像头装在车身上，车身在动，摄像头跟着动，那图像里的世界自然也在动。怎么描述这个「动」？说白了，就是旋转。

我个人习惯把旋转理解成「坐标系之间的相对姿态」。今天咱们就聊聊两种最常见的表示方式：旋转矩阵和欧拉角。嗯，还有一个绕不开的坑——万向锁。

旋转矩阵：最老实但最可靠的方式

旋转矩阵，说白了就是一个 3x3 的矩阵。它能把一个向量从一个坐标系转到另一个坐标系。比如你有一个点 P 在相机坐标系下的坐标，乘以旋转矩阵 R，就能得到它在车体坐标系下的坐标。

数学上长这样：

R = [[r11, r12, r13],
     [r21, r22, r23],
     [r31, r32, r33]]

这个矩阵有个特点：它是正交矩阵，行列式为 +1。什么意思呢？就是它不会改变向量的长度，只会改变方向。我在项目中遇到过有人直接把任意 3x3 矩阵当旋转矩阵用，结果算出来的坐标越偏越远——嗯，那肯定不对。

旋转矩阵的好处是稳定、无歧义。坏处呢？9 个数字，冗余。而且你很难直观地看出它到底转了多大角度、绕哪个轴转的。

欧拉角：直观但暗藏杀机

欧拉角就直观多了。它用三个角度来描述旋转：绕 X 轴转多少（roll）、绕 Y 轴转多少（pitch）、绕 Z 轴转多少（yaw）。

在自动驾驶里，我们常用的是 ZYX 顺序：先绕 Z 轴转 yaw，再绕 Y 轴转 pitch，最后绕 X 轴转 roll。这个顺序不是随便定的，它对应了车辆的运动习惯——先看车头朝向（yaw），再看俯仰（pitch），最后看侧倾（roll）。

从欧拉角到旋转矩阵的转换，代码实现是这样的：

import numpy as np

def euler_to_rotation_matrix(yaw, pitch, roll):
    """
    将 ZYX 顺序的欧拉角转换为旋转矩阵
    yaw: 绕 Z 轴 (rad)
    pitch: 绕 Y 轴 (rad)
    roll: 绕 X 轴 (rad)
    """
    cy, sy = np.cos(yaw), np.sin(yaw)
    cp, sp = np.cos(pitch), np.sin(pitch)
    cr, sr = np.cos(roll), np.sin(roll)

    R = np.array([
        [cy*cp, cy*sp*sr - sy*cr, cy*sp*cr + sy*sr],
        [sy*cp, sy*sp*sr + cy*cr, sy*sp*cr - cy*sr],
        [-sp,   cp*sr,            cp*cr]
    ])
    return R

反过来，从旋转矩阵提取欧拉角：

def rotation_matrix_to_euler(R):
    """
    从旋转矩阵提取 ZYX 欧拉角
    注意：当 pitch 接近 ±90° 时，会出现万向锁
    """
    pitch = np.arcsin(-R[2, 0])
    
    if np.abs(np.cos(pitch)) > 1e-6:
        yaw = np.arctan2(R[1, 0], R[0, 0])
        roll = np.arctan2(R[2, 1], R[2, 2])
    else:
        # 万向锁情况
        yaw = 0.0
        roll = np.arctan2(R[0, 1], R[0, 2])
    
    return yaw, pitch, roll

万向锁：一个让新手崩溃的问题

为什么会这样？

万向锁发生在第二个旋转角（pitch）接近 ±90° 的时候。这时候，第一个和第三个旋转轴会变得共线，导致你丢失了一个自由度。

举个例子：假设你先把车头朝北（yaw=0°），然后 pitch 转到 90°（车头朝天），这时候你再转 roll，你会发现车头还是在绕垂直轴转——跟 yaw 的效果一样。也就是说，yaw 和 roll 变得无法区分了。

我曾经在做一个多传感器标定项目时，用欧拉角做优化变量。结果发现优化器怎么都不收敛，最后定位到是万向锁的问题。从那以后，我只要涉及优化，一律用旋转矩阵或四元数。

三种表示方式的对比

特性	旋转矩阵	欧拉角	四元数
参数数量	9	3	4
直观性	差	好	中
万向锁	无	有	无
插值	困难	有奇异性	容易（球面插值）
计算效率	中	高	高
适合场景	内部计算、优化	可视化、人机交互	插值、融合

知识体系结构图

下面这张图展示了三维旋转表示的核心逻辑和它们之间的关系：

实战中的选择建议

说了这么多，到底该用哪个？我的建议很简单：

• 做可视化、写日志、调参数：用欧拉角。一眼就能看出车是不是翻了、头是不是抬起来了。
• 做优化、做融合、做标定：用旋转矩阵或四元数。别给自己找麻烦。
• 做插值、做平滑：用四元数。球面线性插值（SLERP）效果非常好。

嗯，旋转这件事，说难不难，说简单也不简单。关键是理解每种表示方式的优缺点，知道什么时候该用哪个。下次咱们聊聊四元数，那个东西虽然看着抽象，但用起来是真的香。