1、坐标变换基础：为什么精度重要？从欧拉角到四元数的演进

做SLAM这些年，我踩过最大的坑，就是坐标变换。

你可能觉得，不就是把点从一个坐标系挪到另一个坐标系吗？加减乘除的事，能出什么乱子？

嗯，我以前也这么想。直到有一次，我调试一个视觉惯导融合系统，明明算法流程都对，但轨迹就是飘。折腾了三天，最后发现——欧拉角万向锁，让整个姿态估计崩了。

从那天起，我养成了一个习惯：任何涉及旋转的地方，先问自己一句——这里该用欧拉角，还是四元数？

1.1 坐标变换到底在变什么？

说白了，坐标变换就两件事：旋转和平移。

平移好理解，就是位置加减。旋转呢？你得描述一个物体「朝哪看」。

比如一个机器人，它的位姿由两部分组成：

• 位置：在空间中的坐标 (x, y, z)
• 姿态：它朝向哪个方向 (roll, pitch, yaw 或四元数)

你想想看，如果姿态算错了1度，在10米外的目标，位置误差就是：

10 * tan(1°) ≈ 0.175 米 = 17.5 厘米

17.5厘米，对于抓取一个杯子来说，已经够你抓空了。

1.2 欧拉角：直观但危险

欧拉角用三个角度描述旋转：绕X轴转（roll）、绕Y轴转（pitch）、绕Z轴转（yaw）。

听起来很直观对吧？我刚开始学的时候也觉得，这不就是飞机上的俯仰、横滚、偏航吗？

但问题来了——欧拉角的旋转顺序是固定的。你先转哪个轴，后转哪个轴，结果完全不同。

更致命的是：万向锁。

当pitch角接近±90°时，roll和yaw会变成同一个旋转轴。你丢失了一个自由度。这时候，你无论怎么调roll和yaw，都只能在一个平面内转。

我曾经在一个无人机项目中遇到过这个问题。飞控里用欧拉角做姿态解算，飞机做大角度俯冲时，航向角突然跳变。我当时还以为传感器坏了，查了半天才发现是万向锁。

1.3 四元数：优雅的解决方案

四元数是什么？你可以把它理解成「带约束的复数在三维空间的推广」。

一个四元数 q = w + xi + yj + zk，其中 w 是实部，x、y、z 是虚部，且满足 i² = j² = k² = ijk = -1。

听起来有点抽象？我换个说法：

• 没有万向锁：四元数用四个参数描述旋转，避免了奇异性
• 插值平滑：可以用Slerp做平滑旋转过渡
• 计算高效：组合旋转只需要乘法，比矩阵快

在SLAM里，我们几乎只用四元数表示姿态。比如ORB-SLAM、VINS-Mono，核心状态量都是四元数。

1.4 从欧拉角到四元数的转换

实际工作中，你经常需要把欧拉角转成四元数。比如从飞控拿到的是欧拉角，但SLAM系统需要四元数。

转换公式（以ZYX顺序为例）：

q_w = cos(yaw/2)*cos(pitch/2)*cos(roll/2) + sin(yaw/2)*sin(pitch/2)*sin(roll/2)
q_x = cos(yaw/2)*cos(pitch/2)*sin(roll/2) - sin(yaw/2)*sin(pitch/2)*cos(roll/2)
q_y = cos(yaw/2)*sin(pitch/2)*cos(roll/2) + sin(yaw/2)*cos(pitch/2)*sin(roll/2)
q_z = sin(yaw/2)*cos(pitch/2)*cos(roll/2) - cos(yaw/2)*sin(pitch/2)*sin(roll/2)

我习惯用Eigen库来做这件事，代码简洁且不容易出错：

#include <Eigen/Dense>
#include <Eigen/Geometry>

// 欧拉角 (ZYX顺序) 转四元数
Eigen::Vector3d euler(roll, pitch, yaw);
Eigen::Quaterniond q;
q = Eigen::AngleAxisd(euler[2], Eigen::Vector3d::UnitZ())
  * Eigen::AngleAxisd(euler[1], Eigen::Vector3d::UnitY())
  * Eigen::AngleAxisd(euler[0], Eigen::Vector3d::UnitX());

1.5 本章知识体系

下面这张图，是我自己总结的坐标变换知识脉络。每次带新人时，我都会先让他们看这张图：

1.6 实战建议

说了这么多，给你几条我自己的经验：

1. 存储用四元数，显示用欧拉角：内部计算一律用四元数，只在可视化或日志输出时转成欧拉角
2. 注意旋转顺序：不同传感器、不同库的旋转顺序可能不同。ROS里是ZYX，有些飞控是XYZ。一定要确认清楚
3. 归一化是基本功：四元数必须保持单位模长。每次更新后记得归一化，否则误差会累积
4. 测试你的转换：写一个简单的单元测试，把欧拉角转成四元数再转回来，看是否一致