坐标系基础：WGS84、ENU、载体坐标系之间的转换关系，四元数与旋转矩阵

各位同学，欢迎来到《定位漂移后的坐标变换与重对齐实战》的第一章。

说实话，做SLAM这么多年，我见过太多人栽在坐标系上。明明算法跑得挺好，结果一上真机，定位直接飞了。查来查去，最后发现是坐标系搞反了。嗯，这种坑我自己也踩过不止一次。

所以这一章，咱们先把地基打牢。坐标系这东西，说白了就是给空间里的点一个「身份证」。你搞不清楚它从哪来、往哪去，后面所有变换都是空中楼阁。

1.1 三个核心坐标系：WGS84、ENU、载体坐标系

做定位，你绕不开这三个坐标系。我个人的习惯是，先把它们各自的特点记清楚，再谈转换。

1.4.1 WGS84（全球大地坐标系）

这是GPS直接给你的数据。它用经度、纬度、高度来描述一个点。你手机上的定位芯片，输出的就是WGS84坐标。

但注意，WGS84是个椭球模型，不是平面。你想想看，地球是个球，你拿经纬度算距离，直接做加减法？那误差会大到离谱。我在项目中遇到过，有人直接用经纬度差算位移，结果跑了几百米就偏了十几米。

• 特点：全球统一，但计算复杂，不适合直接做局部运算。
• 单位：度、度、米（高度）。
• 典型应用：GPS原始输出、地图服务（如Google Maps）。

1.4.2 ENU（站心坐标系）

ENU就是「东-北-天」坐标系。它把地球表面某一点当作原点，然后向东是X轴，向北是Y轴，向天是Z轴。

为什么需要它？因为WGS84算距离太麻烦。ENU是平面直角坐标系，加减乘除直接来。你想想看，做SLAM的时候，你关心的是「机器人往东走了5米，往北走了3米」，而不是「经度变了0.000045度」。

我建议，所有局部定位计算，都先在ENU下做。等算完了，再转回WGS84去上报。

• 特点：局部平面，计算简单，适合做相对位移。
• 单位：米。
• 典型应用：无人机起飞点附近的导航、车载局部路径规划。

1.4.3 载体坐标系（Body Frame）

这个坐标系是「长在」机器人身上的。一般定义：X轴朝前，Y轴朝左（或朝右，看习惯），Z轴朝上。

IMU（惯性测量单元）输出的加速度和角速度，就是在载体坐标系下的。你想想看，IMU装在机器人身上，它测到的「向前加速」，当然是相对于机器人自己的朝向，而不是相对于地球。

我记得有一次调试，同事把IMU的加速度直接当成ENU下的加速度去积分，结果机器人明明在直行，算出来的轨迹却拐了个弯。原因就是坐标系没对齐。

• 特点：随机器人运动而旋转，是传感器数据的「母语」。
• 单位：米/秒²（加速度）、弧度/秒（角速度）。
• 典型应用：IMU数据、轮式里程计、视觉SLAM中的相机位姿。

1.2 坐标系之间的转换关系

搞清楚了三个坐标系长什么样，接下来就是怎么在它们之间「串门」。说白了，就是做旋转和平移。

1.2.1 WGS84 → ENU

这个转换分两步：

1. 先把WGS84（经纬高）转成地心地固坐标系（ECEF）。ECEF的原点在地球中心，X轴指向赤道与本初子午线交点，Z轴指向北极。
2. 再把ECEF转到以你设定的原点（比如起飞点）为中心的ENU。

公式有点长，但别怕。实际工程中，我们直接用现成的库，比如GeographicLib或者ROS里的geodetic_utils。我个人的习惯是，把转换函数封装好，永远不要手写经纬度转ENU的公式——太容易出错了。

1.2.2 ENU ↔ 载体坐标系

这个转换的核心是「旋转」。载体坐标系相对于ENU的朝向，由机器人的姿态决定——也就是航向角（偏航）、俯仰角、横滚角。

假设机器人当前朝向是北偏东30度，那么一个在载体坐标系下「向前1米」的点，在ENU下就是「向北0.866米，向东0.5米」。这就是一个简单的二维旋转。

三维情况下，我们需要用旋转矩阵或者四元数。我建议你直接用四元数，原因下面会讲。

1.3 四元数与旋转矩阵

终于到了这一节的重头戏。旋转矩阵和四元数，都是描述旋转的数学工具。但为什么我强烈推荐四元数？

1.3.1 旋转矩阵

旋转矩阵是一个3x3的正交矩阵。它的好处是直观——你拿它去乘一个向量，向量就转了。但坏处也很明显：

• 冗余：9个元素描述3个自由度，浪费。
• 万向锁：用欧拉角（偏航、俯仰、横滚）构造旋转矩阵时，俯仰角接近±90度会丢失一个自由度。我在做无人机仿真时遇到过，飞机一抬头，航向角就乱跳，就是因为万向锁。
• 插值困难：你想让机器人从姿态A平滑转到姿态B，用旋转矩阵插值？算出来的中间姿态可能完全不对。

1.3.2 四元数

四元数用4个元素（w, x, y, z）表示旋转。它没有万向锁，插值也简单（用球面线性插值，即Slerp）。

你想想看，在SLAM里，我们要频繁地做姿态预测、更新、融合。用四元数，计算量小，还稳定。我建议所有新项目，一律用四元数表示姿态。

四元数转旋转矩阵的公式如下（假设四元数已归一化）：

R = | 1-2y²-2z²   2xy-2wz     2xz+2wy   |
    | 2xy+2wz      1-2x²-2z²   2yz-2wx   |
    | 2xz-2wy      2yz+2wx     1-2x²-2y² |

看着复杂？别怕。实际工程中，Eigen、Sophus这些库都帮你写好了。你只需要调用quaternion.toRotationMatrix()就行。

1.4 本章知识体系总览

为了让你更直观地理解这三个坐标系和两种旋转表示方法之间的关系，我画了一张图。你可以看到，WGS84是源头，ENU是桥梁，载体坐标系是终点。四元数和旋转矩阵则是连接它们的「翻译官」。

这张图你看懂了吗？从上往下看，WGS84通过ECEF中转变成ENU，ENU再通过旋转和平移变成载体坐标系。而旋转矩阵和四元数，就是用来描述「旋转」这个动作的。

1.5 避坑指南与个人经验

最后，分享几个我这些年踩过的坑，希望能帮你省点时间。

• 我曾经在融合GPS和IMU时，忘记把GPS的WGS84转成ENU，直接拿经纬度去和IMU的加速度做融合。结果滤波器直接发散。记住：所有传感器数据，必须先统一到同一个坐标系下。
• 我建议，在代码里定义一个全局的坐标系管理器。所有数据进来，先调用一个函数转成ENU。这样后续所有算法都在ENU下跑，清爽很多。
• 嗯，这里要注意：四元数一定要归一化。不归一化的四元数，表示的旋转是错的。我见过有人从IMU读四元数直接拿来用，结果姿态越偏越大。归一化只需要一行代码：q.normalize()。

好了，这一章的内容就到这里。坐标系是定位的基石，搞懂了它，后面讲漂移检测、重对齐才会顺手。下一章我们会深入「定位漂移的数学模型」，看看漂移到底是怎么发生的。