3、坐标系统与空间变换：世界坐标系、相机坐标系、车身坐标系、刚体变换、欧拉角与四元数
多模态融合，说白了就是让不同传感器「说同一种语言」。摄像头看到的是像素，激光雷达看到的是点云，IMU 测的是加速度和角速度——它们各自活在自己的坐标系里。如果不把这些坐标系统一到同一个空间框架下，融合就是一句空话。
我刚开始做多模态项目时，就吃过这个亏。当时把激光雷达的点云直接投影到图像上，结果偏差了十几个像素。查了两天才发现，是雷达坐标系和相机坐标系之间的外参标定出了问题。嗯，从那以后，我每次做融合项目，第一件事就是先把坐标系理清楚。
3.1 四大坐标系统
多模态融合里，最常见的坐标系有四个。我习惯把它们分成「全局」和「局部」两类来看。
| 坐标系 | 原点位置 | 轴定义 | 典型用途 |
|---|---|---|---|
| 世界坐标系 | 人为定义（如 GPS 原点、场地角点） | X-东，Y-北，Z-天（右手系） | 全局定位、地图构建、多车协同 |
| 车身坐标系 | 车辆后轴中心或质心 | X-前，Y-左，Z-上 | 车辆控制、路径规划 |
| 相机坐标系 | 相机光心 | Z-前（光轴），X-右，Y-下 | 图像投影、视觉测距 |
| 传感器坐标系 | 各传感器物理中心 | 由传感器手册定义 | 原始数据采集 |
关键认知：所有传感器数据最终都要变换到同一个坐标系下才能融合。通常以车身坐标系或世界坐标系作为「融合枢纽」。
你想想看，如果摄像头检测到前方 10 米有行人，这个「前方 10 米」是在相机坐标系下算的。但车辆要刹车，需要知道这个行人在车身坐标系下的位置。这就涉及坐标变换了。
3.2 刚体变换：旋转 + 平移
两个坐标系之间的变换，本质上就是一个刚体变换。刚体变换包含两部分：旋转和平移。数学上写成：
P_w = R * P_c + t其中 P_w 是世界坐标系下的点，P_c 是相机坐标系下的点，R 是旋转矩阵，t 是平移向量。
我在项目中遇到过一个问题：为什么不用 4×4 矩阵？其实用齐次坐标更方便：
| P_w | = | R t | * | P_c |
| 1 | | 0 1 | | 1 |这样旋转和平移就统一成一个矩阵乘法了。代码里实现起来也干净：
import numpy as np
def transform_point(p_c, R, t):
# p_c: [x, y, z] 在相机坐标系下
# R: 3x3 旋转矩阵
# t: 3x1 平移向量
p_w = R @ p_c + t
return p_w
# 齐次版本
def transform_point_homogeneous(p_c, T):
# T: 4x4 变换矩阵
p_c_h = np.append(p_c, 1)
p_w_h = T @ p_c_h
return p_w_h[:3]我的习惯：代码里统一用齐次坐标。虽然多了一维，但避免了手动拼接旋转和平移的麻烦，也更容易做链式变换。
3.3 旋转的三种表示方式
旋转矩阵有 9 个参数，但自由度只有 3。实际工程中，我们很少直接用旋转矩阵做优化或插值。常用的三种表示是：
3.3.1 欧拉角
欧拉角用三个角度表示旋转：绕 X 轴（滚转 Roll）、绕 Y 轴（俯仰 Pitch）、绕 Z 轴（偏航 Yaw）。直观，但有个大坑——万向锁。
我曾经在调试一个无人机姿态估计系统时，发现当俯仰角接近 90° 时，滚转和偏航突然耦合了，控制完全乱掉。查了半天，就是欧拉角导致的万向锁。
避坑指南：欧拉角不要用于插值和优化。如果你必须用（比如和硬件接口对齐），一定要明确旋转顺序（如 ZYX 还是 XYZ），不同顺序结果完全不同。
3.3.2 旋转矩阵
旋转矩阵是 3×3 的正交矩阵，行列式为 +1。优点是数学性质好，可以直接用于向量旋转。缺点是参数冗余，且数值误差会导致矩阵不再正交。
我建议：旋转矩阵只作为中间表示，不要用它做长期迭代。
3.3.3 四元数
四元数是我个人最推荐的旋转表示方式。它用四个参数（w, x, y, z）表示旋转，没有万向锁，插值平滑，计算效率高。
四元数的核心公式：
q = w + x*i + y*j + z*k
其中 w² + x² + y² + z² = 1用四元数旋转一个向量：
def rotate_vector_by_quaternion(v, q):
# v: 3D 向量
# q: 单位四元数 [w, x, y, z]
v_quat = np.array([0, v[0], v[1], v[2]])
q_conj = np.array([q[0], -q[1], -q[2], -q[3]])
v_rot = quaternion_multiply(quaternion_multiply(q, v_quat), q_conj)
return v_rot[1:]我的建议：所有涉及旋转的优化、滤波、插值，一律用四元数。只在需要可视化或与硬件交互时，才转成欧拉角。
3.4 坐标变换链：从传感器到世界
实际系统中，数据要经过多次变换才能到目标坐标系。比如：
- 激光雷达点云从雷达坐标系变换到车身坐标系（外参标定）
- 车身坐标系下的点变换到世界坐标系（车辆位姿）
- 世界坐标系下的点投影到相机图像（相机外参 + 内参）
这就是一个变换链：
T_world_camera = T_world_vehicle * T_vehicle_lidar * T_lidar_camera注意顺序：从右往左读。我刚开始做时总搞反，后来养成一个习惯——在代码里用「链式乘法」并加注释：
# 从相机坐标系到世界坐标系的完整变换
T_cam_to_world = T_vehicle_to_world @ T_lidar_to_vehicle @ T_cam_to_lidar小技巧：每次写变换代码时，先画一个坐标系关系图，标清楚每个变换的方向。画图 5 分钟，debug 省 2 小时。
3.5 知识体系总览
下面这张图总结了本章的核心逻辑：
3.6 工程实践要点
- 外参标定是第一步：所有传感器之间的相对位姿（旋转 + 平移）必须精确标定。我见过太多项目因为外参不准，融合效果直接崩掉。
- 时间同步不能忘：坐标变换需要同一时刻的数据。如果相机和激光雷达的时间戳没对齐，变换后的位置会有动态误差。
- 数值稳定性：四元数要定期归一化，旋转矩阵要正交化。这些小细节在长时间运行中会累积成大问题。
我曾经踩过的坑：在嵌入式平台上用 float 做四元数乘法，跑了几个小时后四元数范数漂到了 1.05，旋转完全失真。后来改成 double 并每帧归一化，问题解决。
坐标系统与空间变换，是多模态融合的「地基」。地基不稳，上层建筑再漂亮也没用。把这一章的内容吃透，后面的融合算法才能跑得稳、跑得准。