从水下机器人到Cartographer：LLA、ECEF与ENU坐标系转换实战解析

1. 为什么水下机器人需要坐标系转换？

我第一次接触水下机器人项目时，遇到了一个看似简单却让人头疼的问题：RTK设备输出的经纬度数据，为什么不能直接用来定位？后来在调试Cartographer的GPS模块时，又遇到了同样的困惑。经过多次实践才明白，这背后涉及到三种坐标系的转换问题。

想象一下你在一个陌生的城市迷路了，手机地图告诉你"当前位置：东经116.4度，北纬39.9度"。这个经纬度信息就像LLA坐标系（Latitude/Longitude/Altitude）的描述，对人类很友好，但对机器人来说就像天书。机器人需要的是"向前走10米，左转5米"这样的ENU（东北天）坐标系指令。而中间还需要经过ECEF（地心地固）坐标系这个"翻译官"。

在Cartographer这样的SLAM系统中，GPS数据需要与激光雷达、IMU等传感器数据融合。但GPS的LLA坐标与其他传感器本地的ENU坐标就像说着不同语言的人，必须通过ECEF坐标系这个"通用语"才能互相理解。这就是为什么我们需要掌握这三种坐标系的转换方法。

2. LLA到ECEF：从地球表面到地心坐标

2.1 理解LLA坐标系

LLA坐标系是我们最熟悉的GPS坐标表示法：

• 经度(Longitude)：-180°到+180°，本初子午线为0°
• 纬度(Latitude)：-90°到+90°，赤道为0°
• 高度(Altitude)：椭球体表面以上的高度

但这里有个坑：不同国家使用的椭球体模型可能不同。国内常用的是CGCS2000坐标系，而GPS默认使用WGS84。两者参数差异虽然微小，但在高精度定位时不容忽视。

2.2 转换公式详解

LLA转ECEF的公式看似复杂，其实可以拆解理解：

import math def lla_to_ecef(lat, lon, alt): # WGS84椭球参数 a = 6378137.0 # 长半轴 f = 1/298.257223563 # 扁率 b = a*(1-f) # 短半轴 e_squared = 2*f - f*f # 第一偏心率的平方 N = a / math.sqrt(1 - e_squared*math.sin(lat)**2) # 曲率半径 x = (N + alt) * math.cos(lat) * math.cos(lon) y = (N + alt) * math.cos(lat) * math.sin(lon) z = (N*(1-e_squared) + alt) * math.sin(lat) return x, y, z

这个Python实现揭示了几个关键点：

1. 需要先计算基准椭球体的曲率半径N
2. 高度alt是叠加在N上的修正项
3. 经度lon直接影响x/y分量，纬度lat影响所有分量

2.3 Cartographer中的实现解析

Cartographer的代码更加工程化，使用了Eigen库进行矩阵运算：

constexpr double a = 6378137.; // 赤道半径 constexpr double f = 1./298.257223563; // 扁率 const double sin_phi = std::sin(DegToRad(latitude)); const double N = a / std::sqrt(1 - e_squared * sin_phi * sin_phi); const double x = (N + altitude) * cos_phi * cos_lambda;

这段代码有几个优化点：

1. 使用constexpr编译期常量提升性能
2. 角度转换为弧度后再计算三角函数
3. 复用中间计算结果避免重复运算

3. ECEF到ENU：从地心到本地导航

3.1 ECEF坐标系的特点

ECEF坐标系固定在地球上随地球旋转：

• 原点：地球质心
• X轴：本初子午线与赤道交点
• Z轴：指向北极
• Y轴：完成右手坐标系

这种坐标系对描述卫星轨道很方便，但对地面导航就像用地球仪指路——不够直观。

3.2 建立本地ENU坐标系

ENU（East-North-Up）坐标系是站在地面观察者的视角：

• 东(East)：本地水平面东向
• 北(North)：本地水平面北向
• 天(Up)：垂直于水平面向上

转换的关键是找到旋转矩阵S：

def ecef_to_enu(ref_lat, ref_lon, ref_alt, x, y, z): # 参考点LLA转ECEF x0, y0, z0 = lla_to_ecef(ref_lat, ref_lon, ref_alt) # 计算相对坐标 dx = x - x0 dy = y - y0 dz = z - z0 # 构建旋转矩阵 sin_lon = math.sin(ref_lon) cos_lon = math.cos(ref_lon) sin_lat = math.sin(ref_lat) cos_lat = math.cos(ref_lat) e = -sin_lon*dx + cos_lon*dy n = -sin_lat*cos_lon*dx - sin_lat*sin_lon*dy + cos_lat*dz u = cos_lat*cos_lon*dx + cos_lat*sin_lon*dy + sin_lat*dz return e, n, u

3.3 工程实现中的注意事项

1. 参考点选择：通常取第一帧GPS数据作为ENU坐标系原点
2. 数值稳定性：当参考点靠近极点时，需特殊处理
3. 矩阵求逆：ENU转ECEF就是S矩阵的转置（正交矩阵特性）

Cartographer中通过Rigid3d同时保存旋转和平移：

Eigen::Quaterniond rotation = Eigen::AngleAxisd(latitude-90, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(-longitude, Eigen::Vector3d::UnitZ()); return transform::Rigid3d(rotation * -translation, rotation);

这种表示法既节省存储空间，又便于进行坐标变换的复合运算。

4. 实战中的坑与解决方案

4.1 度与弧度的千年虫问题

我曾在项目中浪费一整天调试一个诡异的定位漂移问题，最终发现是有的函数预期弧度输入，有的却要求角度。建议：

• 所有内部计算统一使用弧度
• 在接口处显式转换：

double DegToRad(double deg) { return deg * M_PI / 180.0; }

4.2 高度基准面的选择

高度值可能基于：

• 椭球高（GPS原始输出）
• 大地高（EGM96模型修正）
• 海拔高（当地水准面）

水下机器人需要特别注意：声学定位设备可能使用不同的高程基准，必须统一到同一基准面。

4.3 实时性优化技巧

对于需要高频更新的系统，可以：

1. 预计算旋转矩阵
2. 使用查表法替代实时三角函数计算
3. 采用定点数运算替代浮点数

// 预计算旋转矩阵 Eigen::Matrix3d S; S << -sin_lon, cos_lon, 0, -sin_lat*cos_lon, -sin_lat*sin_lon, cos_lat, cos_lat*cos_lon, cos_lat*sin_lon, sin_lat;

5. 更复杂的应用场景

5.1 多传感器数据融合

在SLAM系统中，需要将GPS、IMU、激光雷达等数据统一到同一坐标系：

1. GPS提供全局LLA坐标
2. 转换为ENU坐标系后与激光雷达的局部地图匹配
3. IMU提供两帧间的相对运动估计

sensor::RangefinderPoint point_in_enu = transform * point_in_lidar_frame;

5.2 动态参考系处理

对于移动的水下机器人母船，参考系也在移动。这时需要：

1. 建立母船为中心的ENU坐标系
2. 子机器人相对母船的局部坐标
3. 定期更新参考系原点

5.3 坐标系转换的验证方法

我常用的验证三板斧：

1. 静态点测试：固定位置的坐标转换应保持一致
2. 往返测试：LLA→ECEF→LLA应恢复原始值
3. 相对距离验证：两点的ENU坐标差应与实际距离匹配

# 往返测试示例 lat, lon, alt = 39.9, 116.4, 50 x, y, z = lla_to_ecef(lat, lon, alt) new_lat, new_lon, new_alt = ecef_to_lla(x, y, z) assert abs(lat-new_lat) < 1e-9

经过多个水下机器人项目的锤炼，我发现坐标系转换就像机器人感知世界的"普通话"，虽然底层数学复杂，但掌握后就能让各种传感器顺畅交流。特别是在处理RTK和Cartographer组合导航时，精确的坐标转换是避免"鸡同鸭讲"的关键。