3、坐标系与标定：车辆坐标系、传感器坐标系、世界坐标系，以及传感器联合标定方法
各位同学，咱们今天聊一个绕不开的话题——坐标系与标定。
说实话，我在刚入行那会儿，觉得坐标系这东西不就是个数学概念嘛，有啥好纠结的？直到我第一次在实车上做BEV融合，发现激光雷达点云和摄像头图像怎么都对不上，才意识到：坐标系搞不清楚，后面全是白搭。
你想想看，每个传感器都有自己的“小脾气”，它们各自定义了自己的坐标系。你要让它们协同工作，就得先给它们“统一语言”。这就是我们今天要讲的核心。
3.1 三大坐标系：从哪儿来到哪儿去
在BEV模型中，我们通常要面对三个坐标系。我个人习惯把它们比作三个“世界”：
- 传感器坐标系：传感器自己的“小世界”。比如激光雷达，原点在它的旋转中心；摄像头，原点在它的光心。
- 车辆坐标系：整车的“基准世界”。通常原点在后轴中心，或者车辆几何中心。X轴朝前，Y轴朝左，Z轴朝上。
- 世界坐标系：全局的“大世界”。比如GPS定义的经纬高，或者高精地图里的UTM坐标。
为什么要分这么细？我在项目中遇到过一个问题：一辆车的IMU装在车尾，而激光雷达装在车顶。如果不做坐标系转换，IMU测到的加速度和激光雷达看到的障碍物位置，根本对不上。说白了，你得先知道“我在哪儿”，才能知道“它在那儿”。
核心原则：所有传感器数据，最终都要统一到车辆坐标系下，才能做BEV融合。
3.2 传感器坐标系：每个传感器的“小脾气”
咱们一个一个来看。
3.2.1 激光雷达坐标系
激光雷达的坐标系，通常以它的旋转中心为原点。X轴指向正前方，Y轴指向左侧，Z轴指向上方。嗯，这里要注意：不同厂家的定义可能不一样。比如Velodyne和Hesai，它们的坐标系定义就有细微差别。
我曾经踩过一个坑：用某款国产激光雷达，它的Y轴是朝右的，而我默认是朝左。结果点云投影到图像上，左右完全反了。排查了半天才发现是坐标系定义的问题。所以，拿到一个新传感器，第一件事就是看它的数据手册，确认坐标系定义。
3.2.2 摄像头坐标系
摄像头坐标系，原点在光心。Z轴指向镜头前方（也就是拍摄方向），X轴朝右，Y轴朝下。注意，这个Y轴朝下是跟车辆坐标系相反的。所以做投影时，需要额外处理。
我记得有一次做摄像头和激光雷达的融合，发现投影后的点云总是偏上。后来发现，是摄像头坐标系里的Y轴方向搞反了。你想想看，一个简单的符号错误，就能让整个融合结果崩掉。
3.2.3 IMU坐标系
IMU的坐标系，通常以它的芯片中心为原点。X轴朝前，Y轴朝左，Z轴朝上。但IMU的安装位置往往不在车辆中心，所以需要做平移和旋转。
小技巧：IMU的标定，我建议用“六面法”。把IMU放在六个不同的姿态下，采集数据，然后解算出它的零偏和标度因数。这个方法虽然老，但很可靠。
3.3 车辆坐标系：统一战场
车辆坐标系，是所有传感器数据的“归宿”。通常定义如下：
- 原点：后轴中心（或者车辆几何中心）
- X轴：车辆前进方向
- Y轴：车辆左侧方向
- Z轴：车辆上方方向
为什么选后轴中心？因为车辆的运动学模型，以后轴中心为原点最方便。你想想看，阿克曼转向模型里，后轴中心是不发生侧偏的。所以用它做基准，计算轨迹和位置最准确。
在实际项目中，我习惯把车辆坐标系的原点放在后轴中心的地面投影点上。这样Z轴零点就是地面，方便做高度相关的计算。
3.4 世界坐标系：全局视野
世界坐标系，通常用于多车协同或者高精地图。常见的有：
- UTM坐标系：以米为单位，适合局部区域
- 经纬高坐标系：WGS84，适合全球范围
- 自定义坐标系：比如以停车场入口为原点
世界坐标系和车辆坐标系之间的转换，通常通过GPS/IMU组合导航来实现。说白了，就是知道车在世界坐标系下的位姿，然后就能把传感器数据投影到世界坐标系里。
警告：世界坐标系的精度，取决于GPS和IMU的精度。如果你用消费级的GPS，误差可能达到几米。这种情况下，做BEV融合时，建议以车辆坐标系为主，世界坐标系只做参考。
3.5 传感器联合标定：让它们“说同一种语言”
好了，坐标系定义清楚了，接下来就是标定。标定的目的，就是求出传感器坐标系到车辆坐标系的变换矩阵（旋转矩阵R和平移向量t）。
联合标定，说白了就是让不同传感器“对齐”。我常用的方法有两种：
3.5.1 目标法标定
用标定板或者标定物，让所有传感器同时观测同一个目标。然后通过匹配特征点，解算出变换矩阵。
举个例子，激光雷达和摄像头的联合标定：
- 放置一个棋盘格标定板
- 激光雷达扫描到标定板的点云
- 摄像头拍到标定板的图像
- 提取点云中的角点和图像中的角点
- 通过PnP算法解算变换矩阵
我曾经在项目里用这种方法，标定完以后，点云投影到图像上的误差在2个像素以内。效果还不错。
3.5.2 无目标法标定
如果环境不允许放标定板，也可以用无目标法。比如：
- 利用自然特征点（建筑物角点、道路标线等）
- 利用运动信息（比如车辆直线行驶时，IMU和激光雷达的轨迹应该一致）
无目标法的精度通常不如目标法，但在某些场景下很实用。我记得有一次在野外做测试，没带标定板，就是用路边的电线杆做特征点，硬是把标定做完了。
3.6 标定流程：一步一步来
下面我给出一个典型的联合标定流程，供大家参考：
- 准备阶段：确认所有传感器的坐标系定义，准备标定工具（标定板、测量工具等）
- 数据采集：让车辆停在平坦地面，放置标定板，同时采集所有传感器数据
- 特征提取：从各传感器数据中提取特征点（角点、边缘等）
- 匹配求解：用最小二乘法或优化算法，求解变换矩阵
- 验证：用新的数据验证标定结果，检查投影误差
关键点：标定不是一次性的。传感器可能会因为振动、温度变化而发生微小的位移。我建议每隔一段时间重新标定一次，或者在每次车辆启动时做一次快速校验。
3.7 代码示例：激光雷达到车辆坐标系的转换
下面给一个简单的Python代码示例，展示如何把激光雷达的点云转换到车辆坐标系下：
import numpy as np
# 假设激光雷达到车辆坐标系的变换矩阵
# R: 旋转矩阵, t: 平移向量
R = np.array([[0.999, -0.017, 0.017],
[0.017, 0.999, 0.017],
[-0.017, -0.017, 0.999]])
t = np.array([1.5, 0.0, 0.5]) # 单位：米
# 激光雷达坐标系下的点云（N x 3）
lidar_points = np.array([[10.0, 2.0, -1.0],
[20.0, -3.0, 0.5]])
# 转换到车辆坐标系
vehicle_points = (R @ lidar_points.T).T + t
print("车辆坐标系下的点云：")
print(vehicle_points)
这段代码很简单，但实际项目中，你还需要考虑时间同步、畸变校正等问题。嗯，这些我们后面章节会详细讲。
3.8 避坑指南
最后，分享几个我踩过的坑：
- 坐标系方向搞反：尤其是Y轴，不同传感器定义可能不同。一定要看数据手册。
- 平移向量单位搞错：有的传感器用毫米，有的用米。统一成米再计算。
- 旋转矩阵顺序搞错：欧拉角的旋转顺序（比如XYZ还是ZYX）会影响结果。我建议直接用四元数，避免歧义。
- 忽略时间同步：标定时的数据必须是在同一时刻采集的。如果时间不同步，标定结果会不准。
我的习惯：每次标定完，我都会做一个“投影验证”。把激光雷达点云投影到图像上，看看是否对齐。如果偏差超过3个像素，我就会重新标定。
好了，坐标系与标定这部分就讲到这里。下一章我们会聊传感器的时间同步问题，这也是个容易踩坑的地方。大家有什么问题，欢迎随时交流。