2. 坐标系与空间变换
做感知融合，说白了就是跟坐标系打交道。我刚开始接触这个领域时，觉得坐标变换不就是乘个矩阵嘛，有啥难的？结果第一次实车测试，点云和图像怎么都对不上，排查了一整天——原来是某个传感器的安装角度写反了。嗯，从那以后，我对坐标系这件事再也不敢马虎了。
1. 为什么需要这么多坐标系？
你想想看，一辆自动驾驶车上，装了激光雷达、摄像头、毫米波雷达、GPS/IMU。每个传感器都有自己的“视角”。摄像头看到的是像素坐标，激光雷达看到的是三维点云，GPS给的是经纬度。它们说的都不是同一种“语言”。
所以我们需要一个统一的“翻译官”——世界坐标系。所有传感器的数据，最终都要转换到同一个坐标系下，才能做融合。
核心原则：每个传感器在自己的坐标系里感知世界，融合中心在世界坐标系里理解世界。
2. 四大坐标系详解
2.1 世界坐标系
这是全局的、固定的坐标系。通常用GPS/IMU定义，原点在地球上的某个点，X轴指向东，Y轴指向北，Z轴指向天（ENU坐标系）。我习惯用UTM坐标系做局部世界坐标，因为它是平面直角坐标，计算起来比经纬度方便得多。
2.2 车辆坐标系
车辆坐标系跟着车走。ISO标准定义：X轴指向车头前方，Y轴指向驾驶员左侧，Z轴指向车顶。原点一般在车辆后轴中心或质心。我在项目中遇到过一个问题——不同供应商对车辆原点的定义不一样，有的在保险杠中心，有的在后轴。不统一的话，融合结果直接偏掉。
2.3 传感器坐标系
每个传感器都有自己的坐标系。比如激光雷达，通常X轴指向雷达正面，Z轴向上。摄像头坐标系则是Z轴指向镜头前方，Y轴向下（图像坐标系）。
我的习惯：拿到一个新传感器，第一件事就是画它的坐标系示意图，贴在墙上。别偷懒，这一步能省掉后面80%的调试时间。
3. 刚体变换：旋转与平移
坐标系之间的变换，本质上是刚体变换。刚体变换包含两部分：旋转 + 平移。用数学表达就是：
P_world = R * P_sensor + T其中R是旋转矩阵，T是平移向量。R是3x3的正交矩阵，T是3x1的向量。
3.1 旋转矩阵
旋转矩阵有9个元素，但自由度只有3。说白了，就是绕X、Y、Z三个轴分别转一个角度。绕Z轴旋转θ角的矩阵长这样：
R_z(θ) = [[cosθ, -sinθ, 0],
[sinθ, cosθ, 0],
[0, 0, 1]]绕X轴和Y轴的类似。多个旋转可以组合，但顺序很重要。我记得有一次，我把绕X轴和绕Y轴的顺序搞反了，结果点云直接翻了个跟头。
3.2 欧拉角
欧拉角用三个角度表示旋转：横滚角(Roll)、俯仰角(Pitch)、偏航角(Yaw)。直观易懂，但有个大坑——万向锁。当Pitch接近±90°时，Roll和Yaw会耦合，丢失一个自由度。
避坑指南：我曾经在IMU数据融合时用了欧拉角，结果车辆经过一个陡坡时，Pitch接近90°，姿态估计直接炸了。从那以后，内部计算我全部改用四元数。
3.3 四元数
四元数用四个数表示旋转：q = [w, x, y, z]，其中w是实部，x、y、z是虚部。它没有万向锁问题，插值也平滑。四元数到旋转矩阵的转换公式：
R = [[1-2(y²+z²), 2(xy-wz), 2(xz+wy)],
[2(xy+wz), 1-2(x²+z²), 2(yz-wx)],
[2(xz-wy), 2(yz+wx), 1-2(x²+y²)]]看着复杂，但代码里直接用库函数就好。我个人推荐Eigen或Sophus库，C++里用起来很顺手。
4. 坐标变换实战
理论说完了，来点实际的。假设我们有一个激光雷达点，需要从激光雷达坐标系变换到世界坐标系。
4.1 标定参数
首先，我们需要知道激光雷达相对于车辆的安装位置和姿态。这就是外参标定。通常标定结果是一个4x4的齐次变换矩阵：
T_lidar_to_vehicle = [[R, T],
[0, 1]]然后，车辆相对于世界坐标系的位姿由GPS/IMU提供，也是一个4x4矩阵T_vehicle_to_world。
4.2 变换流程
整个变换链就是：
P_world = T_vehicle_to_world * T_lidar_to_vehicle * P_lidar用代码实现：
// C++ 示例：使用Eigen库
#include <Eigen/Dense>
Eigen::Vector4d transform_point(
const Eigen::Vector4d& pt_lidar,
const Eigen::Matrix4d& T_lidar_to_vehicle,
const Eigen::Matrix4d& T_vehicle_to_world) {
Eigen::Vector4d pt_vehicle = T_lidar_to_vehicle * pt_lidar;
Eigen::Vector4d pt_world = T_vehicle_to_world * pt_vehicle;
return pt_world;
}注意：齐次坐标的最后一个分量是1，表示点。如果是向量（方向），最后一个分量是0。这个细节我见过好多人搞错。
5. 知识体系总览
下面这张图总结了本章的核心内容，我画了很久，希望能帮你理清思路：
6. 常见问题与避坑
| 问题 | 原因 | 解决方案 |
|---|---|---|
| 点云和图像对不上 | 外参标定不准或坐标系定义不一致 | 重新标定，检查坐标系定义文档 |
| 融合轨迹跳变 | 时间戳不同步或变换顺序错误 | 统一时间基准，检查变换链顺序 |
| 姿态估计发散 | 使用了欧拉角且接近万向锁 | 改用四元数进行内部计算 |
我曾经踩过的坑：有一次做多传感器融合，激光雷达和毫米波雷达的数据始终对不上。查了两天，最后发现是激光雷达的坐标系定义是“Z轴向上”，而毫米波雷达的坐标系是“Z轴向前”。两个供应商的文档都没写清楚。所以，拿到传感器第一件事——确认坐标系定义！
嗯，坐标系与空间变换就讲到这里。这部分内容虽然基础，但真的是感知融合的基石。你想想看，如果坐标系都搞不对，后面再牛的算法也是白搭。我个人习惯在项目初期就花时间把坐标系关系理清楚，画成图贴在工位上，后面能省下大把调试时间。
可可精品资料库