3. 坐标变换与空间同步：刚体变换、欧拉角与四元数、传感器外参标定原理、时间戳对齐方法
各位同学，欢迎来到第三章。
这一章，我们聊聊多传感器融合里最基础、也最绕不开的话题——空间同步。
说白了，就是让不同传感器“看”到的世界，能对上号。
你想想看，激光雷达说“前方3米有障碍物”，摄像头说“前方2.8米有障碍物”。到底信谁的？
嗯，很可能它们说的不是同一个坐标系。这就是我们要解决的问题。
3.1 刚体变换：传感器之间的“翻译官”
刚体变换，听起来挺唬人。其实它就是描述一个物体在空间里怎么移动、怎么旋转。
一个刚体，不会变形。它从位置A到位置B，无非就是两步：旋转 + 平移。
数学上，我们用旋转矩阵 R 和平移向量 t 来表示：
P_b = R * P_a + t这里，P_a 是物体在坐标系a下的坐标，P_b 是它在坐标系b下的坐标。
R 是3x3的旋转矩阵，t 是3x1的平移向量。
核心要点：刚体变换不改变物体的形状和大小，只改变它的位姿（位置+姿态）。
我在做第一个融合项目时，就犯过傻。直接把两个传感器的数据叠加，结果点云和图像完全对不上。后来才发现，我忘了做坐标变换。嗯，从那以后，我每次拿到新传感器，第一件事就是确认它的坐标系定义。
3.2 欧拉角与四元数：旋转的两种“语言”
旋转矩阵有9个参数，但自由度只有3个。用起来有点浪费。于是就有了欧拉角和四元数。
3.2.1 欧拉角
欧拉角用三个角度来描述旋转：滚转角（Roll）、俯仰角（Pitch）、偏航角（Yaw）。
你可以想象成：先绕X轴转一个角度，再绕Y轴转一个角度，最后绕Z轴转一个角度。
但欧拉角有个臭名昭著的问题——万向锁。
当俯仰角达到±90度时，滚转和偏航会变得无法区分。说白了，就是丢失了一个自由度。
避坑指南：我曾经在无人机项目里用欧拉角做姿态控制。结果飞机做大机动时，姿态解算直接崩了。后来换成四元数，问题才解决。所以，如果你做的是全姿态运动（比如无人机、机器人），千万别用欧拉角。
3.2.2 四元数
四元数，说白了就是一个“超复数”。它用四个数来表示旋转：
q = w + x*i + y*j + z*k其中，w是实部，x、y、z是虚部。并且满足 i² = j² = k² = ijk = -1。
四元数没有万向锁问题，而且插值平滑。在传感器融合里，我几乎只用四元数。
举个例子，用四元数旋转一个向量：
P_rotated = q * P * q_conjugate这里，q_conjugate 是 q 的共轭四元数。
我的习惯：在代码里，我通常用Eigen库或ROS的tf库来处理四元数。手写四元数乘法容易出错，相信我，你不想debug那个。
3.3 传感器外参标定原理
外参标定，就是求两个传感器之间的刚体变换矩阵（R, t）。
说白了，就是让激光雷达和摄像头“对齐”。
常用的方法有：
- 目标法：用标定板（比如棋盘格），让两个传感器同时观测。然后通过特征点匹配，求解R和t。
- 自然特征法：利用环境中的自然特征（比如角点、边缘）进行匹配。适合在线标定。
- 手眼标定法：机器人领域常用。通过机械臂的运动，间接求解传感器之间的变换。
我个人最常用的是目标法。简单、可靠。
具体步骤：
- 放置标定板，确保两个传感器都能看到。
- 从激光雷达点云中提取标定板的中心点。
- 从图像中提取标定板的角点。
- 建立对应关系，求解PnP问题。
- 优化：用非线性最小二乘法（比如Levenberg-Marquardt）优化结果。
注意：标定板的材质很重要。反光太强，激光雷达会打偏。我建议用哑光材质的标定板。
下面这张图，是我自己画的标定流程。你看一眼就明白了。
3.4 时间戳对齐方法
空间同步搞定了，还有时间同步。
不同传感器的采样频率不同。激光雷达10Hz，摄像头30Hz，IMU 200Hz。它们的数据到达时间也不一样。
如果不做时间对齐，你融合的就是“过去”和“未来”的数据。结果可想而知。
常用的方法：
- 硬件同步：用同一个时钟源（比如GPS的PPS信号）触发所有传感器。精度最高，但硬件成本高。
- 软件同步：用时间戳插值。比如，摄像头图像的时间戳是t1，激光雷达点云的时间戳是t2。我们找一个最近的匹配。
- 滤波同步：在卡尔曼滤波框架里，用预测-更新机制来处理异步数据。这是最优雅的方式。
我的经验：在量产项目中，我倾向于硬件同步+软件插值。硬件同步保证时间基准一致，软件插值处理不同频率的数据。这样既可靠又灵活。
举个例子，时间戳对齐的伪代码：
def align_timestamps(lidar_data, camera_data):
# lidar_data: [(t_l1, pcl1), (t_l2, pcl2), ...]
# camera_data: [(t_c1, img1), (t_c2, img2), ...]
aligned_pairs = []
for t_c, img in camera_data:
# 找到最接近的激光雷达数据
nearest_lidar = min(lidar_data, key=lambda x: abs(x[0] - t_c))
aligned_pairs.append((img, nearest_lidar[1]))
return aligned_pairs这段代码很简单，但实际项目中要考虑更多。比如，时间差超过阈值时要丢弃数据。我曾经因为时间戳没对齐，导致融合后的障碍物位置跳变。排查了三天才找到原因。
警告：不要依赖系统时间戳。不同传感器的时钟漂移不同。一定要用同一个时钟源，或者做时钟同步。
好了，这一章的内容就到这里。
坐标变换和空间同步，是传感器融合的基石。你想想看，如果连“对齐”都做不好，后面的融合、跟踪、决策，全是空中楼阁。
下一章，我们会聊聊滤波与状态估计。嗯，到时候见。