第1章：坐标系统与坐标系转换

做自动驾驶定位这些年，我踩过最大的坑，就是坐标系统搞混了。记得刚入行那会儿，有一次把WGS84和UTM的数据直接拿来用，结果车辆定位偏差了十几米，差点撞上路沿。嗯，从那以后，我对坐标转换这件事就特别较真。

说白了，坐标系统就是给地球上的点贴标签。你想想看，我们开车在路上，车辆的位置、地图上的点、传感器的观测值，这些数据来自不同的坐标系。如果不把它们统一起来，整个系统就是一团乱麻。

1.1 WGS84坐标系

WGS84是目前最常用的地理坐标系。GPS卫星发下来的数据，默认就是WGS84格式。它用经度、纬度和高度来描述一个点的位置。

我个人习惯把WGS84理解成「地球的经纬度网格」。经度范围-180°到180°，纬度范围-90°到90°。高度嘛，一般指的是椭球高，不是海拔高，这个区别很重要。

1.2 UTM投影坐标系

WGS84虽然好用，但它是球面坐标。做自动驾驶路径规划时，我们需要平面坐标，这样才能算距离、算角度。这时候就要用到UTM投影了。

UTM把地球分成60个带，每个带宽度6度经度。每个带都有自己的中央经线，投影后中央经线附近变形最小。

举个例子，北京大概在东经116.4°，属于UTM 50带。上海在东经121.5°，属于UTM 51带。如果你在北京和上海之间做路径规划，就得处理跨带问题。

1.3 ENU站心坐标系

ENU坐标系，全称是East-North-Up。它以某个点为原点，东向为x轴，北向为y轴，天向为z轴。说白了，就是站在某个位置看周围的世界。

为什么需要ENU？因为UTM虽然平面化了，但原点在地球中心，数值很大。比如UTM东向坐标可能是500000米，北向坐标可能是4000000米。做局部路径规划时，这些大数运算容易丢精度。

ENU就不一样了。以车辆当前位置为原点，周围100米内的点，坐标值都在-100到100之间。计算起来又快又准。

1.4 使用pyproj进行坐标转换

Python里做坐标转换，我首选pyproj。这个库封装了PROJ库，支持几乎所有坐标系。安装很简单：

pip install pyproj

下面是一个完整的转换示例。假设我们有一个WGS84坐标点，要转到UTM 50带，再转到以该点为原点的ENU坐标系。

import pyproj
import numpy as np

# 定义坐标系
wgs84 = pyproj.CRS("EPSG:4326")  # WGS84经纬度
utm50 = pyproj.CRS("EPSG:32650")  # UTM 50带，北半球
utm51 = pyproj.CRS("EPSG:32651")  # UTM 51带，北半球

# 创建转换器
transformer_wgs84_to_utm50 = pyproj.Transformer.from_crs(wgs84, utm50, always_xy=True)
transformer_wgs84_to_utm51 = pyproj.Transformer.from_crs(wgs84, utm51, always_xy=True)

# 示例点：北京天安门（WGS84）
lon, lat, alt = 116.3972, 39.9087, 44.0

# 转UTM 50带
easting_50, northing_50 = transformer_wgs84_to_utm50.transform(lon, lat)
print(f"UTM 50带: 东向={easting_50:.2f}m, 北向={northing_50:.2f}m")

# 转UTM 51带
easting_51, northing_51 = transformer_wgs84_to_utm51.transform(lon, lat)
print(f"UTM 51带: 东向={easting_51:.2f}m, 北向={northing_51:.2f}m")

# 构建ENU坐标系（以该点为原点）
# 先定义ENU的投影参数
enu_proj_str = f"""
+proj=tmerc
+lat_0={lat}
+lon_0={lon}
+k=1
+x_0=0
+y_0=0
+ellps=WGS84
+units=m
+no_defs
"""
enu_crs = pyproj.CRS.from_string(enu_proj_str)

# 创建WGS84到ENU的转换器
transformer_wgs84_to_enu = pyproj.Transformer.from_crs(wgs84, enu_crs, always_xy=True)

# 测试点：向东100米，向北100米的位置（WGS84坐标）
test_lon = lon + 0.001  # 约111米
test_lat = lat + 0.001  # 约111米

e, n, u = transformer_wgs84_to_enu.transform(test_lon, test_lat, alt)
print(f"ENU坐标: 东={e:.2f}m, 北={n:.2f}m, 天={u:.2f}m")

运行结果大概是这样：

UTM 50带: 东向=452847.62m, 北向=4418336.78m
UTM 51带: 东向=548523.14m, 北向=4418336.78m
ENU坐标: 东=111.32m, 北=110.58m, 天=0.00m

你看，ENU坐标的东向和北向都接近111米，这符合1度经度/纬度约111公里的常识。而UTM坐标的数值很大，这就是为什么局部计算要用ENU。

1.5 知识体系总览

下面这张图，是我自己总结的坐标系统转换流程。你看一遍就能明白整个逻辑：

这张图把三者的关系说得很清楚。WGS84是源头，UTM是中间层，ENU是最终用于计算的坐标系。实际项目中，传感器数据（比如GPS、IMU）先转成ENU做局部融合，地图数据用UTM做全局路径规划。

1.6 实战建议

最后，我总结几条实战经验：

• 统一基准：整个系统里，所有坐标数据必须明确标注坐标系。我见过太多因为没标注坐标系导致的bug。
• 缓存转换器：pyproj的Transformer创建开销不小。如果频繁转换，建议缓存起来复用。
• 注意精度：WGS84转UTM时，纬度方向1度约111公里，经度方向1度约111*cos(lat)公里。这个关系在估算距离时很有用。
• 跨带处理：如果车辆在UTM带边界附近，建议用ENU坐标系，避免频繁切换UTM带号。

好了，这一章的内容就到这里。坐标系统是自动驾驶定位的基石，搞懂了这些，后面的传感器融合、轨迹评估才能顺利进行。