坐标系与变换：世界坐标系、相机坐标系、激光雷达坐标系、IMU坐标系定义与转换关系

做传感器融合，说白了就是让不同传感器“说同一种语言”。

每个传感器都有自己的小世界——自己的坐标系。相机有相机的视角，激光雷达有激光雷达的扫描范围，IMU有IMU的加速度方向。如果不把这些坐标系对齐，你融合出来的数据就是一团乱麻。

我刚开始做多传感器融合时，就吃过这个亏。当时把激光雷达的点云投影到图像上，结果投影出来的点全偏到画面外面去了。查了半天，发现是激光雷达坐标系和相机坐标系的旋转方向定义反了。嗯，从那以后，我再也不敢轻视坐标系定义了。

1. 四种坐标系的基本定义

先搞清楚每个坐标系长什么样。我习惯用右手定则来记忆，这样不容易搞混。

坐标系	原点位置	X轴方向	Y轴方向	Z轴方向	典型应用
世界坐标系	自定义（如GPS原点）	东向或北向	北向或东向	垂直向上	全局定位、地图构建
相机坐标系	相机光心	图像右方向	图像下方向	相机前方	视觉SLAM、目标检测
激光雷达坐标系	激光雷达中心	前方或右侧	左侧或前方	垂直向上	点云建图、障碍物检测
IMU坐标系	IMU芯片中心	载体前进方向	载体左侧	载体垂直向上	姿态估计、惯性导航

2. 坐标系之间的转换关系

坐标系转换，说白了就是“把一个点从一个坐标系搬到另一个坐标系”。

数学上，这需要两个东西：旋转矩阵R和平移向量t。

假设有一个点P，在世界坐标系下的坐标为P_w，在相机坐标系下的坐标为P_c。那么：

P_c = R * P_w + t

反过来，从相机坐标系到世界坐标系：

P_w = R^(-1) * (P_c - t)

你想想看，这个公式其实很简单。R负责“转方向”，t负责“挪位置”。

3. 常见坐标系转换场景

3.1 激光雷达到相机

这是我最常做的转换。把激光雷达的3D点云投影到2D图像上。

流程是这样的：

1. 激光雷达点 P_lidar → 通过外参变换到相机坐标系 P_cam
2. 相机坐标系 P_cam → 通过内参投影到图像平面 (u, v)

我曾经在项目里遇到一个问题：投影出来的点总是偏左几个像素。排查了很久，发现是激光雷达和相机的安装位置测量有2厘米的误差。嗯，有时候硬件安装的精度，比算法本身更关键。

3.2 IMU到世界坐标系

IMU测量的是载体坐标系下的加速度和角速度。要得到世界坐标系下的姿态，需要做积分。

我一般用四元数来表示旋转，避免欧拉角的万向锁问题。

// 伪代码示例：IMU数据转换到世界坐标系
// 输入：IMU原始数据 (acc_x, acc_y, acc_z, gyro_x, gyro_y, gyro_z)
// 输出：世界坐标系下的加速度

// 1. 用陀螺仪数据更新姿态四元数
q = updateQuaternion(q, gyro, dt);

// 2. 将加速度从IMU坐标系旋转到世界坐标系
acc_world = q.rotate(acc_imu);

// 3. 减去重力分量
acc_world.z -= 9.81;

4. 外参标定的核心逻辑

外参标定，就是求两个传感器之间的R和t。

说白了，就是找一个“对齐”的关系。让同一个物体在不同传感器下的观测值，在空间上重合。

常用的方法有两种：

• 目标法：用标定板、棋盘格等已知物体，提取特征点，然后求解R和t。精度高，但需要专用工具。
• 自然特征法：用环境中的角点、边缘等自然特征，通过匹配来求解。灵活，但鲁棒性要求高。

我个人更推荐目标法做初标定，然后用自然特征法做在线微调。这样既有精度，又能适应环境变化。

5. 知识体系结构图

下面这张图，是我梳理的坐标系与变换的核心逻辑。你可以把它当作一个“地图”，随时回来对照。

6. 避坑指南

做坐标系转换，有几个坑我踩过，分享给你：

• 旋转方向搞反：我曾经把顺时针旋转写成了逆时针，结果点云投影全反了。建议每次写完转换代码，先用一个已知点做验证。
• 单位不一致：激光雷达用毫米，相机用米，IMU用厘米。这种单位混用的问题，我至少遇到过三次。统一单位，用米。
• 忘记重力补偿：IMU的加速度包含重力分量。如果不减去，积分出来的位置会一直往下掉。
• 时间戳对齐：不同传感器的数据到达时间不同。做转换前，先确保时间戳对齐，否则R和t再准也没用。

好了，这一章的内容就到这里。坐标系定义和转换关系，是后续所有标定工作的基础。你可以在实际项目中多练练，慢慢就会形成肌肉记忆。