坐标系与坐标转换：WGS84坐标系、UTM坐标系、车辆坐标系、传感器坐标系、坐标转换矩阵

各位同学好，我是老张。今天咱们聊聊坐标系这个话题。

说实话，我刚入行那会儿，觉得坐标系不就是个参考系嘛，有啥好学的？直到有一次，我把WGS84的经纬度直接当成了平面坐标去算距离，结果定位偏差跑出去好几公里。嗯，从那以后，我再也不敢小看坐标系了。

在高精地图匹配定位中，坐标系是地基。地基不稳，上面盖的房子再漂亮也得塌。今天我就把几个核心坐标系掰开揉碎了讲清楚。

1. WGS84坐标系：地球的“身份证”

WGS84，全称World Geodetic System 1984。说白了，就是给地球上的每一个点发一个唯一的“门牌号”——经度、纬度、高度。

你手机里的GPS，默认输出的就是WGS84坐标。我平时调试定位算法时，最常打交道的就是它。

但有个坑——WGS84是球面坐标，单位是度。你没法直接用经纬度的差值去算距离，因为经度1度对应的地面距离，在赤道和北极是完全不同的。我曾经有个同事，直接用经纬度差算欧氏距离，结果在北极附近跑出来一个“时空扭曲”的效果……

2. UTM坐标系：把球面拍扁

既然球面坐标不好算距离，那怎么办？投影呗。

UTM（Universal Transverse Mercator）就是把地球切成60个带，每个带6度，然后分别投影到平面上。这样一来，坐标单位变成了米，距离计算就简单了。

我个人习惯在局部区域做匹配定位时，先把WGS84转成UTM。原因很简单——平面上的几何运算，比球面上快得多。

3. 车辆坐标系：车就是宇宙中心

车辆坐标系，顾名思义，原点在车辆质心（或者后轴中心），x轴朝前，y轴朝左，z轴朝上。这是做车辆控制、路径规划时最常用的坐标系。

我一般在做匹配定位时，会把高精地图上的点转换到车辆坐标系下。这样，车辆前方50米的路况，直接看x=50处的点就行，非常直观。

4. 传感器坐标系：各说各话

车上装了激光雷达、摄像头、毫米波雷达……每个传感器都有自己的坐标系。

激光雷达的原点通常在设备中心，x轴朝前，y轴朝左，z轴朝上。摄像头坐标系则复杂一些，有的x轴朝右，有的z轴朝前。你想想看，如果不统一坐标系，激光雷达说“前方5米有障碍物”，摄像头说“右方3米有障碍物”，你信谁的？

所以，传感器标定的本质，就是把所有传感器坐标统一到车辆坐标系下。

5. 坐标转换矩阵：数学的魔法

坐标转换的核心，就是旋转和平移。

旋转用旋转矩阵R，平移用平移向量t。一个点从坐标系A转到坐标系B，公式很简单：

P_B = R * P_A + t

其中R是3x3的旋转矩阵，t是3x1的平移向量。

旋转矩阵有个性质——它是正交矩阵，逆等于转置。所以从B转回A也很简单：

P_A = R^T * (P_B - t)

我在项目中遇到过最头疼的事，就是旋转矩阵的符号搞反了。明明应该是顺时针转，我写成了逆时针，结果车辆定位轨迹直接画了个“8”字……

6. 实战：从WGS84到车辆坐标系的完整链路

咱们捋一下完整的转换流程：

1. GPS输出WGS84经纬度
2. 转成UTM平面坐标（米）
3. 根据车辆航向角，旋转到车辆坐标系
4. 减去车辆当前位置，得到相对坐标

这一步一步，环环相扣。哪一步出了错，定位就崩了。

import numpy as np

def wgs84_to_utm(lat, lon):
    # 调用proj库或自定义实现
    # 返回东向、北向坐标（米）
    pass

def utm_to_vehicle(east, north, vehicle_east, vehicle_north, yaw):
    dx = east - vehicle_east
    dy = north - vehicle_north
    # 旋转矩阵（绕z轴）
    R = np.array([[np.cos(yaw), np.sin(yaw)],
                  [-np.sin(yaw), np.cos(yaw)]])
    local = R @ np.array([dx, dy])
    return local[0], local[1]  # x_forward, y_left

这里有个细节——yaw角的方向定义。有的系统定义北偏东为正，有的定义北偏西为正。我建议你一开始就统一好，不然后面全是坑。

知识体系总览

下面这张图，是我自己总结的坐标系转换全链路。你看一眼，心里就有谱了。

你看，从WGS84到UTM，再到车辆坐标系，最后到各个传感器，每一步都有明确的数学关系。搞懂了这些，高精地图匹配定位的坐标系问题，你就拿下了八成。

好了，今天就聊到这儿。坐标系这东西，纸上谈兵没用，我建议你打开代码，拿一组真实数据跑一遍转换流程。踩几个坑，就全明白了。