4. 坐标系与标定

各位同学，欢迎来到第四讲。

说实话，做传感器融合，最让我头疼的往往不是算法本身，而是标定。你想想看，激光雷达说“我在 (1,0,0) 看到个点”，毫米波雷达说“我在 (0.8, 0.2, 0) 看到个目标”。它们说的到底是不是同一个东西？

嗯，这就引出了我们今天要聊的核心——坐标系与标定。

4.1 传感器坐标系定义

先统一一下语言。每个传感器都有自己的“小世界”，也就是它的坐标系。

• 激光雷达坐标系：通常采用右手系。X轴朝前（车头方向），Y轴朝左，Z轴朝上。原点在激光雷达的旋转中心。我个人习惯把激光雷达的坐标系当作“基准坐标系”，因为它的点云信息最丰富，便于后续处理。
• 毫米波雷达坐标系：同样是右手系。但注意！很多毫米波雷达的X轴是朝右的，Y轴朝前。这跟激光雷达的坐标系定义刚好差了90度。我在项目中遇到过，第一次做融合时，直接把毫米波的点投影到激光雷达坐标系下，结果目标全跑到左边去了。排查了半天，才发现是坐标系定义没对齐。
• 车身坐标系：这是最终的“世界坐标系”。通常以后轴中心为原点，X轴朝前，Y轴朝左，Z轴朝上。所有传感器的数据最终都要统一到这个坐标系下。

4.2 刚体变换：旋转 + 平移

坐标系之间怎么转换？说白了，就是刚体变换。一个传感器坐标系到另一个传感器坐标系，无非就是先转个角度，再挪个位置。

数学上，我们用旋转矩阵 R 和平移向量 t 来表示。一个点 P 在坐标系 A 下的坐标，转换到坐标系 B 下：

P_B = R * P_A + t

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

旋转矩阵看起来很复杂，其实你可以理解为三个轴的旋转组合。绕 X 轴转、绕 Y 轴转、绕 Z 轴转。每个轴旋转一个角度，三个角度一乘，就是最终的旋转矩阵。

4.3 外参标定方法

外参标定，就是求这个 R 和 t。方法分两种：手动和自动。

4.3.1 手动标定

手动标定，说白了就是“人肉对齐”。

• 步骤：找一个特征明显的场景，比如墙角、杆子。激光雷达和毫米波雷达同时采集数据。然后你手动调整 R 和 t，直到两个传感器的数据在可视化界面上重合。
• 优点：简单粗暴，不需要复杂的算法。
• 缺点：精度看手感。我曾经手动标定过一个项目，调了整整一下午，眼睛都快瞎了。最后发现还是差了 0.1 度，导致远距离目标融合时偏差很大。

4.3.2 自动标定

自动标定才是工程界的“正道”。

常见的自动标定方法有：

• 基于特征点匹配：在场景中放置标定物（比如角反射器）。激光雷达和毫米波雷达都能检测到这个标定物。然后通过匹配这些特征点，求解 R 和 t。
• 基于互信息最大化：不需要标定物。利用两个传感器数据的统计相关性，找到使互信息最大的变换参数。这个方法很强大，但计算量大。
• 基于深度学习：最近几年开始流行。用神经网络直接回归出外参。我试过，效果不错，但需要大量标注数据。

我个人最推荐的是基于特征点匹配的方法。稳定、可靠、精度高。你只需要一个角反射器，放在车前不同位置，采集几组数据，就能解算出外参。

4.4 联合标定流程

好了，理论讲完了。我们来看看实际怎么做。下面是我在项目中总结的一套联合标定流程：

1. 准备阶段：找一个空旷场地。放置 3-5 个角反射器，分布在车前 5-30 米范围内。
2. 数据采集：同时启动激光雷达和毫米波雷达。采集 10 秒钟数据。确保每个角反射器在两个传感器中都能被清晰检测到。
3. 特征提取：从激光雷达点云中提取角反射器的中心点坐标。从毫米波雷达点云中提取目标点坐标。
4. 数据关联：将激光雷达检测到的角反射器与毫米波雷达检测到的目标一一对应。这一步很关键。我一般用最近邻匹配，距离阈值设为 0.5 米。
5. 求解外参：使用 SVD 分解或者最小二乘法，求解 R 和 t。
6. 验证：将求解出的外参应用到新采集的数据上。可视化检查两个传感器的点云是否重合。如果偏差超过 0.1 米，重新标定。

下面这张图，是我画的联合标定流程框架图。你可以看到整个流程的脉络：

嗯，这套流程我用了好几年，基本没出过问题。你只要严格按照步骤来，标定精度可以做到 0.05 米以内。

最后，我想说一句：标定是融合的基石。别嫌麻烦，别偷懒。把标定做好了，后面的融合算法才能发挥出真正的威力。

好了，坐标系与标定就讲到这里。记住：坐标系定义要统一，刚体变换要准确，标定方法要选对，联合流程要规范。做到这四点，你的融合系统就成功了一半。