4. 坐标系与坐标变换：世界坐标系、车辆坐标系、传感器坐标系、刚体变换与齐次坐标
做感知融合，说白了就是回答三个问题：「我在哪？」「东西在哪？」「怎么把它们对齐？」。
这三个问题的答案，全藏在坐标系和坐标变换里。我刚开始接触融合时，觉得这玩意儿就是纯数学，枯燥得很。直到有一次，我把激光雷达的点云投影到图像上，结果车跑到树顶上去了……嗯，从那以后，我再也不敢小看坐标系了。
4.1 为什么需要多个坐标系？
你想想看，每个传感器都有自己的「视角」。摄像头看到的是像素，激光雷达看到的是三维点，GPS 给的是经纬度。它们说的不是同一种语言。
所以我们需要一个「翻译官」——坐标变换。把不同传感器的数据，统一到同一个坐标系下，才能做融合。
核心原则：所有传感器数据，最终都要转换到同一个参考坐标系中。这个参考系，通常选车辆坐标系或世界坐标系。
4.2 三大坐标系详解
4.2.1 世界坐标系
世界坐标系是「绝对」的。它固定在地球上，不随车辆移动而改变。常用的有 UTM 坐标系（通用横轴墨卡托）或经纬度坐标系。
我个人习惯用 UTM，因为它是平面直角坐标，计算起来方便。经纬度是球面坐标，做距离计算时还得考虑曲率，麻烦。
- 原点：通常选在地球上的某个固定点（如 UTM 投影带的中央经线与赤道交点）
- X轴：东向
- Y轴：北向
- Z轴：天向（向上）
避坑指南：我曾经在项目中直接用 GPS 的经纬度做距离计算，结果高速场景下误差累积到米级。后来改用 UTM，问题就解决了。记住：球面坐标不适合做局部欧氏距离计算。
4.2.2 车辆坐标系
车辆坐标系是「相对」的。它跟着车走，车头朝哪，坐标系就朝哪。这是感知融合中最常用的坐标系。
为什么？因为决策和控制模块只关心「障碍物相对于我的车在哪里」，不关心它在世界上的绝对位置。
- 原点：通常选在后轴中心（或车辆质心）
- X轴：车辆前进方向
- Y轴：车辆左侧（注意：有些标准用右侧，要统一）
- Z轴：天向
注意：不同车厂对车辆坐标系定义可能不同。有的用前轴中心，有的用后轴中心。做融合前，一定要确认好定义，否则数据全乱套。
4.2.3 传感器坐标系
每个传感器都有自己的小世界。摄像头有相机坐标系，激光雷达有雷达坐标系，毫米波雷达也有自己的坐标系。
举个例子，相机坐标系通常是：
- 原点：相机光心
- Z轴：镜头朝向（向前）
- X轴：向右
- Y轴：向下（注意：图像坐标系 Y 轴向下，和常规相反）
激光雷达坐标系则不同：
- 原点：激光雷达中心
- X轴：向前
- Y轴：向左
- Z轴：向上
你看，同样一个障碍物，在相机坐标系里是 (x, y, z)，在激光雷达坐标系里可能是 (x', y', z')。不转换，根本没法用。
4.3 刚体变换：旋转 + 平移
坐标系之间的转换，本质上就是刚体变换。刚体变换包含两部分：旋转和平移。物体不会变形，只是位置和朝向变了。
4.3.1 旋转矩阵
旋转矩阵 R 是一个 3x3 的正交矩阵，描述了两个坐标系之间的朝向关系。
R = [[cosθ, -sinθ, 0],
[sinθ, cosθ, 0],
[0, 0, 1]] // 绕 Z 轴旋转 θ 角度绕 X 轴、Y 轴旋转也有对应的矩阵。实际应用中，我们通常用欧拉角（roll, pitch, yaw）来表示旋转，然后转换成旋转矩阵。
我的经验：欧拉角有万向锁问题。如果你要做连续旋转或插值，建议用四元数。我在做多传感器标定时，就吃过万向锁的亏，后来全改成四元数了。
4.3.2 平移向量
平移向量 t 是一个 3x1 的向量，描述了两个坐标系原点之间的偏移。
t = [tx, ty, tz]^T比如，激光雷达安装在车顶，相对于车辆坐标系原点的偏移就是 (tx, ty, tz)。
4.3.3 完整的刚体变换公式
假设有一个点 P，在坐标系 A 中的坐标为 P_A，在坐标系 B 中的坐标为 P_B。那么：
P_B = R * P_A + t其中 R 是从 A 到 B 的旋转矩阵，t 是从 A 到 B 的平移向量。
4.4 齐次坐标：让变换更优雅
上面的公式 P_B = R * P_A + t 看起来简单，但有个问题：连续变换时，公式会变得很冗长。
比如，从传感器坐标系到车辆坐标系，再到世界坐标系，需要两次变换：
P_vehicle = R1 * P_sensor + t1
P_world = R2 * P_vehicle + t2写起来麻烦，计算也麻烦。这时候，齐次坐标就派上用场了。
4.4.1 什么是齐次坐标？
齐次坐标就是在三维坐标 (x, y, z) 后面加一个维度 w，变成 (x, y, z, w)。通常 w=1。
这样，旋转和平移就可以合并成一个 4x4 的变换矩阵 T：
T = [[R, t],
[0, 1]] // 4x4 矩阵那么变换就变成了简单的矩阵乘法：
P_B_homo = T * P_A_homo连续变换也只需要连乘：
P_world_homo = T2 * T1 * P_sensor_homo为什么好用？因为矩阵乘法可以串联。你只需要把所有的变换矩阵乘在一起，得到一个总的变换矩阵，然后一次乘法搞定。代码写起来干净，计算效率也高。
4.4.2 代码示例：用齐次坐标做变换
下面是一个 Python 示例，演示如何用齐次坐标将激光雷达点云转换到车辆坐标系：
import numpy as np
# 定义旋转矩阵（绕 Z 轴旋转 30 度）
theta = np.radians(30)
R = np.array([[np.cos(theta), -np.sin(theta), 0],
[np.sin(theta), np.cos(theta), 0],
[0, 0, 1]])
# 定义平移向量（激光雷达安装在车顶前方）
t = np.array([1.5, 0.0, 0.5]) # 单位：米
# 构建齐次变换矩阵
T = np.eye(4)
T[:3, :3] = R
T[:3, 3] = t
# 激光雷达检测到一个点（在雷达坐标系中）
point_lidar = np.array([10.0, 2.0, -1.0, 1.0]) # 齐次坐标
# 转换到车辆坐标系
point_vehicle = T @ point_lidar
print(f"雷达坐标系中的点: {point_lidar[:3]}")
print(f"车辆坐标系中的点: {point_vehicle[:3]}")
输出结果：
雷达坐标系中的点: [10. 2. -1.]
车辆坐标系中的点: [ 8.16025404 6. 0.5 ]小技巧：实际项目中，我会把每个传感器的变换矩阵预先算好，存成配置文件。运行时直接加载，不用每次都重新计算。这样既快又不容易出错。
4.5 知识体系总览
下面这张图，帮你理清本章的核心逻辑：
4.6 实战中的注意事项
讲完了理论，说点实际的。我在项目中踩过不少坑，分享几个给你：
- 标定精度决定一切。旋转矩阵和平移向量都是标定出来的。标定误差 1 度，100 米外的障碍物就偏了 1.7 米。所以标定一定要做精细。
- 时间同步不能忘。坐标系变换是空间上的对齐，但别忘了时间上的对齐。如果传感器数据时间戳对不上，变换再准也没用。
- 注意坐标系定义的一致性。我曾经遇到过，摄像头团队用右手系，激光雷达团队用左手系。结果融合出来的障碍物位置全是镜像的……
- 浮点数精度问题。连续变换多次后，误差会累积。建议定期用世界坐标系中的已知点做校验，修正累积误差。
重要提醒：千万不要在代码里硬编码变换参数。一定要从配置文件或标定文件中读取。否则换一辆车，或者传感器位置动了，你得改代码，容易出事故。
好了，坐标系与坐标变换就讲到这里。这部分是感知融合的基石，理解透了，后面的融合算法才能站得稳。下一章，我们会聊传感器的时间同步——另一个容易踩坑的地方。