坐标系基础：地理坐标系（WGS84）、投影坐标系（UTM）、局部坐标系（ENU）、坐标系转换原理
做定位漂移纠正，说白了就是跟坐标系打交道。我刚开始接触这行时，觉得坐标系不就是经纬度嘛，有啥好研究的？结果第一次做多传感器融合，GPS数据和雷达数据怎么都对不上，折腾了两天才发现——原来是坐标系没统一。嗯，从那以后我再也不敢小看坐标系了。
1. 地理坐标系：WGS84
WGS84，全称World Geodetic System 1984。这是目前全球最通用的地理坐标系。GPS卫星发下来的原始数据，就是WGS84坐标系下的经纬度。
它的核心思想很简单：用经度、纬度、高度三个值，描述地球上的一个点。地球被近似为一个椭球体，WGS84定义了椭球的长半轴、短半轴、扁率等参数。
WGS84关键参数：
- 长半轴 a = 6378137.0 m
- 短半轴 b ≈ 6356752.3142 m
- 扁率 f = 1/298.257223563
我在项目中遇到过一个问题：用手机GPS采集的数据，直接拿去做高精度地图匹配，结果偏差了十几米。为什么？因为手机GPS的WGS84坐标本身就有误差，再加上地球椭球模型和真实地形的差异，累积起来就大了。
我的经验：WGS84坐标适合做全球定位，但不适合直接做局部计算。因为经纬度不是平面单位，算距离要用球面几何，很麻烦。
2. 投影坐标系：UTM
WGS84是球面上的坐标，但我们的地图、屏幕、传感器数据，大多是在平面上的。这就需要一个桥梁——投影坐标系。
UTM（Universal Transverse Mercator）是最常用的投影方式。它将地球表面划分为60个带，每个带宽度6度经度。每个带内，用横轴墨卡托投影把球面映射到平面。
UTM坐标用东向（Easting）和北向（Northing）表示，单位是米。这样算距离、算角度就方便多了。
UTM分带规则：
- 带号 = floor((经度 + 180) / 6) + 1
- 中国主要位于UTM 49-53带
- 中央经线 = (带号 - 1) * 6 - 180 + 3
我记得有一次做车辆定位，GPS数据是WGS84的，但高精地图用的是UTM坐标。我写了个转换函数，结果发现车辆位置总是偏了几米。排查了半天，原来是UTM带号选错了——车辆跨了两个带，我没做带间切换处理。
避坑指南：我曾经在跨带区域吃过亏。如果你的定位区域跨越两个UTM带，建议统一用一个带，或者改用局部坐标系。否则转换误差会很大。
3. 局部坐标系：ENU
ENU（East-North-Up）是我个人最喜欢的坐标系。它是个局部直角坐标系，原点选在某个参考点，三个轴分别指向东、北、天顶方向。
为什么喜欢它？因为直观。东向就是x轴，北向就是y轴，天顶就是z轴。所有计算都在直角坐标系下进行，加减乘除就行，不用考虑球面几何。
在自动驾驶、机器人定位中，ENU坐标系非常常用。比如IMU输出的加速度、角速度，就是在载体坐标系下的；但做融合时，通常要转换到ENU坐标系下。
ENU与WGS84的关系：
- 选择一个参考点（比如起点），记录其WGS84坐标 (lat0, lon0, h0)
- 将WGS84坐标转换为地心地固坐标系（ECEF）
- 通过旋转矩阵，将ECEF坐标转换到以参考点为原点的ENU坐标
你想想看，如果直接用WGS84做局部路径规划，每次算两点距离都要调用球面余弦定理，多麻烦。换成ENU后，直接勾股定理就搞定了。
4. 坐标系转换原理
坐标系转换，说白了就是三步：平移、旋转、缩放。在定位领域，我们通常不考虑缩放（因为都是刚性变换），所以核心就是平移和旋转。
我画了一张图，帮你理清这几个坐标系之间的关系：
从图上你能看到，WGS84到ECEF是第一步，ECEF到UTM或ENU是第二步。实际项目中，我通常走这条路径：WGS84 → ECEF → ENU。因为ENU做局部计算最方便。
5. 核心转换公式
这里给出最常用的WGS84转ENU的代码。我习惯用Python写，因为调试方便。
import math
def wgs84_to_enu(lat, lon, h, lat0, lon0, h0):
"""
WGS84转ENU坐标系
lat, lon, h: 目标点经纬度(度)和高度(米)
lat0, lon0, h0: 参考点经纬度(度)和高度(米)
返回: (east, north, up) 单位米
"""
# WGS84椭球参数
a = 6378137.0
f = 1 / 298.257223563
e2 = 2*f - f*f
# 转弧度
lat = math.radians(lat)
lon = math.radians(lon)
lat0 = math.radians(lat0)
lon0 = math.radians(lon0)
# 计算目标点的ECEF坐标
sin_lat = math.sin(lat)
cos_lat = math.cos(lat)
sin_lon = math.sin(lon)
cos_lon = math.cos(lon)
N = a / math.sqrt(1 - e2 * sin_lat * sin_lat)
x = (N + h) * cos_lat * cos_lon
y = (N + h) * cos_lat * sin_lon
z = (N * (1 - e2) + h) * sin_lat
# 计算参考点的ECEF坐标
sin_lat0 = math.sin(lat0)
cos_lat0 = math.cos(lat0)
sin_lon0 = math.sin(lon0)
cos_lon0 = math.cos(lon0)
N0 = a / math.sqrt(1 - e2 * sin_lat0 * sin_lat0)
x0 = (N0 + h0) * cos_lat0 * cos_lon0
y0 = (N0 + h0) * cos_lat0 * sin_lon0
z0 = (N0 * (1 - e2) + h0) * sin_lat0
# ECEF差值
dx = x - x0
dy = y - y0
dz = z - z0
# 旋转矩阵：ECEF -> ENU
east = -sin_lon0 * dx + cos_lon0 * dy
north = (-sin_lat0 * cos_lon0 * dx
- sin_lat0 * sin_lon0 * dy
+ cos_lat0 * dz)
up = (cos_lat0 * cos_lon0 * dx
+ cos_lat0 * sin_lon0 * dy
+ sin_lat0 * dz)
return east, north, up
使用建议：这段代码我用了好几年，精度在厘米级。注意参考点要选在定位区域中心附近，离得越远，ENU的误差越大。一般建议参考点距离目标不超过50公里。
6. 坐标系选择的经验法则
说了这么多，到底什么时候用哪个坐标系？我总结了一个表格：
| 应用场景 | 推荐坐标系 | 原因 |
|---|---|---|
| 全球定位、导航 | WGS84 | GPS原始输出，通用性强 |
| 地图显示、路径规划 | UTM | 平面坐标，计算方便 |
| 多传感器融合（IMU+GPS） | ENU | 局部直角坐标系，融合算法简单 |
| 高精度定位（RTK） | ENU | 厘米级精度，适合局部差分 |
| 跨区域长距离定位 | WGS84 + UTM分段 | 避免UTM带间误差 |
我个人习惯是：能不用WGS84做计算就不用。球面上的三角函数运算，不仅慢，还容易引入数值误差。换成ENU后，加减乘除就搞定，代码也清爽很多。
重要提醒：我曾经在项目中直接用WGS84坐标做卡尔曼滤波，结果状态协方差矩阵很快就奇异了。为什么？因为经纬度的数值量级和位置变化量级不匹配。后来换成ENU，一切正常。所以，做融合算法前，一定先把坐标统一到ENU。
好了，坐标系基础就讲到这里。记住一句话：定位漂移的根源，很多时候不是传感器坏了，而是坐标系没搞对。把坐标系理清楚，漂移纠正就成功了一半。