3、坐标系与坐标变换：车辆坐标系、全局坐标系、经纬度与UTM坐标转换、齐次变换矩阵
说实话，坐标系这个东西，刚入行时我觉得特别简单。不就是几个轴嘛，谁不会？
直到我第一次在实车上跑路径规划，车死活不走我规划的路线。排查了半天，发现是坐标系搞反了。嗯，从那以后，我再也不敢小看坐标系了。
3.1 车辆坐标系——我们最熟悉的坐标系
车辆坐标系，说白了就是「以车为中心」的坐标系。我个人习惯把它叫做车身坐标系。
定义很简单：
- x轴：指向车辆正前方
- y轴：指向车辆左侧（也有用右侧的，看标准）
- z轴：指向车辆正上方
原点一般放在后轴中心，或者车辆质心。我在项目中遇到过，用后轴中心更常见，因为和运动学模型好对接。
重要提醒：车辆坐标系是随车移动的。车转弯，坐标系也跟着转。这是它和全局坐标系最大的区别。
你想想看，路径规划里经常要判断「障碍物在车的左边还是右边」。这个判断就是在车辆坐标系下做的。如果搞反了左右，那避障就全乱套了。
3.2 全局坐标系——地图的「世界」
全局坐标系是固定不动的。它给整个环境一个统一的参考。
常见的全局坐标系有两种：
- 笛卡尔坐标系：就是(x, y, z)，简单直观
- UTM坐标系：通用横轴墨卡托投影，后面细说
我记得有一次做高精地图融合，地图用的是UTM坐标，但传感器数据是经纬度。两个坐标系没对齐，结果路径规划出来的路线直接穿墙了。你说尴尬不尴尬？
所以，坐标系转换是路径规划里绕不开的坎。
3.3 经纬度与UTM坐标转换
GPS给的是经纬度，但路径规划需要的是平面坐标。这就得做投影转换。
经纬度转UTM的公式挺复杂，我一般直接用现成的库。但原理你得懂：
| 坐标类型 | 表示方式 | 特点 |
|---|---|---|
| 经纬度 | (lat, lon) | 球面坐标，单位是度 |
| UTM | (easting, northing) | 平面坐标，单位是米 |
UTM把地球分成60个带，每个带6度。中国主要覆盖43-53带。我曾经踩过一个坑：用了错误的UTM带号，结果坐标偏移了几十公里。嗯，那次排查花了我整整一天。
我的小技巧：做转换时，先确认你所在的UTM带号。可以用在线工具查一下，别凭感觉猜。
代码示例（Python）：
import utm
# 经纬度转UTM
lat, lon = 39.9042, 116.4074 # 北京天安门
easting, northing, zone_num, zone_letter = utm.from_latlon(lat, lon)
print(f"UTM: {easting:.2f}, {northing:.2f}, Zone {zone_num}{zone_letter}")
# UTM转经纬度
lat_back, lon_back = utm.to_latlon(easting, northing, zone_num, zone_letter)
print(f"经纬度: {lat_back:.4f}, {lon_back:.4f}")3.4 齐次变换矩阵——坐标转换的「瑞士军刀」
齐次变换矩阵，说白了就是把旋转和平移打包成一个4x4的矩阵。
为什么用4x4？因为3x3只能做旋转，做不了平移。加一个维度，就能统一处理了。
形式长这样：
| R t |
| 0 1 |其中R是3x3旋转矩阵，t是3x1平移向量。
举个例子，把车辆坐标系下的点转换到全局坐标系：
P_global = T * P_vehicle这里的T就是齐次变换矩阵。
注意：齐次变换矩阵的乘法顺序很重要！先旋转再平移，还是先平移再旋转，结果完全不同。我见过有人搞反了顺序，路径规划出来的轨迹直接反方向跑。
我个人习惯用欧拉角来表示旋转，因为直观。但要注意万向锁问题。如果你做的是全向运动规划，建议用四元数。
3.5 坐标系转换的实战流程
在路径规划系统里，坐标系转换的流程一般是这样的：
- 传感器数据：先转到车辆坐标系
- 车辆坐标系：通过齐次变换转到全局坐标系
- 全局坐标系：如果是GPS路径，还要转成UTM
- 规划结果：再转回车辆坐标系，发给控制模块
我曾经在一个项目里，因为少做了一步坐标系转换，导致规划出来的路径和实际行驶轨迹差了2米。嗯，从那以后我每次做转换都会画个流程图，标清楚每一步。
核心要点：
- 车辆坐标系：随车移动，用于局部感知和避障
- 全局坐标系：固定不动，用于全局路径规划
- 经纬度转UTM：注意带号，别搞错
- 齐次变换矩阵：旋转+平移，顺序不能乱
最后说一句，坐标系转换看起来是基础，但它是整个路径规划系统的地基。地基不稳，上面盖的楼再漂亮也没用。我见过太多因为坐标系搞错而翻车的案例了。
你想想看，如果连「车在哪」「目标在哪」都搞不清楚，路径规划还怎么玩？