2、坐标系与变换：WGS84、ENU、车辆坐标系、刚体变换与四元数

各位同学好，欢迎来到第二章。

上一章我们聊了定位的整体框架，说白了就是回答「车在哪」这个问题。但这个问题有个前提——你得先定义清楚「哪」是哪儿。这就引出了坐标系。

我刚开始做自动驾驶定位时，犯过一个低级错误：把GPS给的经纬度直接当平面坐标用，结果车辆定位在几百公里外。嗯，从那以后我再也不敢轻视坐标系变换了。

2.1 全球坐标系：WGS84

WGS84，全称World Geodetic System 1984。它是目前GPS使用的全球大地坐标系。

你手机里收到的经纬度，就是WGS84坐标。它用三个参数描述一个点：经度(longitude)、纬度(latitude)、高度(altitude)。

但这里有个坑——WGS84的高度是椭球高，不是海拔高。椭球高是相对于WGS84参考椭球面的高度，而海拔高是相对于大地水准面的高度。两者差多少？

我曾在某山区项目里遇到过，椭球高和海拔高差了将近50米。如果你直接用椭球高做高程定位，车可能就「飞」到山腰上了。

注意：WGS84高度是椭球高，不是海拔高。两者差异可达几十米，需要根据当地大地水准面模型进行修正。

2.2 局部坐标系：ENU

WGS84坐标虽然全球通用，但做局部定位计算时很不方便。你想想看，经纬度是角度单位，算距离还得用球面三角公式，太麻烦。

所以实际工程中，我们通常把WGS84转换到ENU坐标系。ENU是东-北-天(East-North-Up)坐标系，原点选在某个参考点，三个轴分别指向东、北、天顶。

转换过程分两步：

  1. WGS84 → ECEF：地心地固坐标系，原点在地心，Z轴指向北极，X轴指向本初子午线与赤道交点。
  2. ECEF → ENU：通过旋转和平移，把原点移到参考点。

代码实现其实不复杂，我贴一段核心转换函数：

// WGS84转ECEF
void wgs84_to_ecef(double lat, double lon, double alt,
                   double &x, double &y, double &z) {
    const double a = 6378137.0;          // 长半轴
    const double f = 1.0 / 298.257223563; // 扁率
    const double e2 = 2*f - f*f;          // 第一偏心率平方

    double sin_lat = sin(lat * M_PI / 180.0);
    double cos_lat = cos(lat * M_PI / 180.0);
    double sin_lon = sin(lon * M_PI / 180.0);
    double cos_lon = cos(lon * M_PI / 180.0);

    double N = a / sqrt(1 - e2 * sin_lat * sin_lat);

    x = (N + alt) * cos_lat * cos_lon;
    y = (N + alt) * cos_lat * sin_lon;
    z = (N * (1 - e2) + alt) * sin_lat;
}
个人习惯：我一般把参考点选在车辆启动位置，这样ENU坐标在初始时刻就是(0,0,0)，后续定位结果一目了然。

2.3 车辆坐标系

车辆坐标系是跟着车走的。标准定义是：

  • X轴：车头方向（前进方向）
  • Y轴：车身左侧方向
  • Z轴：车顶方向（符合右手定则）

这个坐标系有什么用？传感器标定、控制指令、碰撞检测，全都要用到它。

举个例子：激光雷达装在车顶，它测得的一个点坐标是(1.0, 0.5, 0.2)，这个值是在雷达坐标系下的。要把它转换到车辆坐标系下，就需要知道雷达相对于车辆的安装位置和姿态。

我见过一个案例：某团队把激光雷达的安装角度算反了，结果车辆一直往右偏，以为是控制问题，查了三天才发现是坐标系变换错了。

2.4 刚体变换

刚体变换，说白了就是「不改变物体形状的移动和旋转」。在定位中，我们用它来描述不同坐标系之间的关系。

一个刚体变换包含两部分：

  • 旋转：用旋转矩阵R表示（3×3正交矩阵）
  • 平移：用平移向量t表示（3×1）

变换公式很简单：p' = R * p + t

其中p是原坐标系下的点，p'是目标坐标系下的点。

但这里有个细节——旋转矩阵有9个元素，但自由度只有3个。所以实际工程中，我们很少直接用旋转矩阵做优化，而是用四元数或欧拉角。

2.5 四元数

四元数，很多人一听就头大。我当年学的时候也觉得这东西很玄乎。

其实你可以把它理解成「带约束的复数」。一个四元数q = [w, x, y, z]，满足w² + x² + y² + z² = 1。

为什么用四元数？三个原因：

  1. 无万向锁：欧拉角在俯仰角±90°时会丢失一个自由度，四元数不会。
  2. 插值平滑：四元数球面插值(SLERP)可以生成平滑的旋转过渡。
  3. 计算高效：组合旋转只需要四元数乘法，比矩阵乘法快。

四元数表示旋转的公式：

// 四元数旋转点
Eigen::Vector3d rotate_point(const Eigen::Quaterniond &q,
                             const Eigen::Vector3d &p) {
    Eigen::Quaterniond p_quat(0, p.x(), p.y(), p.z());
    Eigen::Quaterniond rotated = q * p_quat * q.inverse();
    return Eigen::Vector3d(rotated.x(), rotated.y(), rotated.z());
}

这段代码看着简单，但背后有个坑——四元数乘法不满足交换律。q1 * q2和q2 * q1结果不同，顺序很重要。

避坑指南：我曾经在IMU融合时把四元数更新顺序写反了，结果姿态估计一直在漂。查了两天，发现是q * delta_q和delta_q * q的区别。记住：四元数左乘表示在全局坐标系下旋转，右乘表示在局部坐标系下旋转。

2.6 坐标系变换链

实际定位系统中，坐标系变换是一个链条：

WGS84 → ECEF → ENU → 车辆坐标系 → 传感器坐标系

每一步都是一个刚体变换。我们通常用变换矩阵T（4×4齐次矩阵）来串联：

// 齐次变换矩阵
Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
T.block<3,3>(0,0) = R;  // 旋转部分
T.block<3,1>(0,3) = t;  // 平移部分

// 串联变换
Eigen::Vector4d p_sensor(1.0, 0.5, 0.2, 1.0);
Eigen::Vector4d p_vehicle = T_sensor_to_vehicle * p_sensor;
Eigen::Vector4d p_enu = T_vehicle_to_enu * p_vehicle;

这里要注意：变换矩阵的乘法顺序是从右往左读。先乘离点最近的变换，再乘远的。

2.7 总结与建议

这一章内容不少，但核心就一句话：坐标系变换是定位的基石，搞错了全盘皆输。

给你几个实操建议：

  • 写代码时，每个坐标系都明确命名，不要用「坐标系1」「坐标系2」这种模糊名字
  • 每次变换都做单元测试：把一个点变换过去再变换回来，看是否回到原点
  • 用可视化工具（如RViz）把坐标系画出来，肉眼确认方向是否正确

下一章我们会讲传感器——IMU、GPS、轮速计，这些传感器输出的数据都是在各自坐标系下的，到时候你会更深刻地理解坐标系变换的重要性。

好，这一章就到这儿。有问题欢迎在评论区交流。

← 上一章 📖 返回目录 下一章 →