2、坐标系定义：左手系与右手系、欧拉角、四元数、旋转矩阵的基本概念

做数据回放和坐标变换校验，说白了，第一件事就是把坐标系搞明白。我见过太多项目，算法模型跑得飞起，结果一上实车数据就崩，追根溯源——坐标系定义错了。嗯，这坑我踩过，所以今天咱们把这几个基础概念彻底捋清楚。

2.1 左手系 vs 右手系：你习惯用哪只手？

先问个问题：你平时用左手还是右手？别笑，这真的决定了你整个坐标系的生死。

右手系，就是伸出右手，拇指指向X轴，食指指向Y轴，中指指向Z轴。三个手指互相垂直。OpenGL、ROS、Unity 这些主流框架，默认都是右手系。我个人习惯用右手系，因为大部分传感器（比如相机、IMU）的输出都是右手系。

左手系呢？左手拇指X，食指Y，中指Z。DirectX、某些游戏引擎喜欢用左手系。为什么会有这种差异？说白了，历史原因和数学习惯不同。

我在项目中遇到过最头疼的事：一个传感器输出的是右手系点云，另一个算法库默认左手系。你想想看，如果不做转换，直接拿过来用，结果就是点云全反了。避坑指南：拿到任何数据，第一件事确认坐标系手性。

2.2 欧拉角：直观但容易翻车

欧拉角，就是用三个角度来描述旋转：绕X轴转（滚转Roll）、绕Y轴转（俯仰Pitch）、绕Z轴转（偏航Yaw）。听起来很直观对吧？

但这里有个大坑——万向锁。当俯仰角接近±90°时，滚转和偏航会变得无法区分。我记得第一次做无人机姿态解算时，飞控突然抽风，查了半天，就是万向锁导致的。嗯，从那以后，我对欧拉角就多了一份敬畏。

旋转顺序	常见应用	注意点
ZYX（偏航→俯仰→滚转）	航空、车辆	最常用，符合直觉
XYZ（滚转→俯仰→偏航）	机器人	注意顺序不同结果不同
ZYZ	某些数学推导	避免万向锁的一种方式

2.3 四元数：数学家的优雅方案

四元数，说白了就是用一个实部和三个虚部来表示旋转。形式是 q = w + xi + yj + zk，其中 w 是实部，x、y、z 是虚部。为什么用四元数？因为它完美避开了万向锁，而且插值平滑。

我建议你在做姿态估计、SLAM、或者任何需要连续旋转的场景时，优先用四元数。虽然它不如欧拉角直观，但稳定性和精度都好得多。

# 四元数基本运算示例
import numpy as np

def quaternion_multiply(q1, q2):
    """四元数乘法"""
    w1, x1, y1, z1 = q1
    w2, x2, y2, z2 = q2
    return np.array([
        w1*w2 - x1*x2 - y1*y2 - z1*z2,
        w1*x2 + x1*w2 + y1*z2 - z1*y2,
        w1*y2 - x1*z2 + y1*w2 + z1*x2,
        w1*z2 + x1*y2 - y1*x2 + z1*w2
    ])

# 单位四元数（无旋转）
q_identity = np.array([1, 0, 0, 0])

2.4 旋转矩阵：最通用的表达

旋转矩阵，就是一个3×3的正交矩阵，行列式为+1。它可以把一个向量从一个坐标系变换到另一个坐标系。说白了，就是线性代数里的标准做法。

旋转矩阵的好处是：可以任意组合、求逆方便、数学性质好。坏处是：9个参数，冗余度高，而且容易受数值误差影响变得不正交。

# 绕Z轴旋转θ角的旋转矩阵
import numpy as np

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

# 使用示例
R = rotation_matrix_z(np.pi/4)  # 绕Z轴转45°
print(R)

2.5 三者之间的转换：打通任督二脉

在实际项目中，你不可能只用一种表达。传感器输出可能是欧拉角，算法内部用四元数，最终输出又要转成旋转矩阵。所以，转换函数必须烂熟于心。

转换方向	方法	注意事项
欧拉角 → 四元数	按旋转顺序连乘	注意旋转顺序和轴定义
四元数 → 旋转矩阵	用四元数元素构造矩阵	确保四元数已归一化
旋转矩阵 → 欧拉角	从矩阵元素反解角度	注意万向锁时的处理

2.6 知识体系总览

下面这张图，是我自己总结的坐标系与旋转表达的知识结构。你把它存下来，以后遇到相关问题时，先看这张图定位问题。

这张图把整个知识体系串起来了。你从坐标系定义出发，选择一种旋转表达方式，然后应用到具体场景中。每个环节都有坑，但只要你理解了手性、万向锁、归一化这几个关键点，基本就能避开90%的问题。