3、坐标系转换基础：传感器坐标系定义、欧拉角与四元数、刚体变换矩阵、坐标转换库（tf2/Eigen）
做感知融合，说白了就是让不同传感器「说同一种语言」。激光雷达说「我在前方5米」，摄像头说「我在像素(320,240)」，毫米波雷达说「目标距离7.3米，角度15度」——这些数据怎么对齐？坐标系转换就是那个翻译官。
我个人习惯，拿到一个融合项目，第一件事不是调算法，而是先把所有传感器的坐标系画出来。这一步做不好，后面全是垃圾数据进、垃圾数据出。
3.1 传感器坐标系定义
每个传感器都有自己的「小世界」。常见的坐标系有这么几种：
- 世界坐标系：全局统一的参考系，通常用W表示。比如GPS的经纬高，或者SLAM建图时的原点。
- 车体坐标系：以车辆中心为原点，x轴朝前，y轴朝左，z轴朝上。这是融合的「主坐标系」。
- 传感器坐标系：每个传感器自己的局部坐标系。比如激光雷达的x轴可能朝右，摄像头的z轴朝前。
- 像素坐标系：图像上的(u,v)坐标，原点在左上角。
我记得有一次做项目，摄像头安装时偏了5度，没标定出来。结果融合出来的目标位置总是偏左半米。排查了两天才发现——坐标系定义没对齐。嗯，这里要注意：每个传感器的坐标系定义必须文档化，不能只靠口头传。
核心原则：所有传感器数据最终都要转换到车体坐标系下进行融合。这是感知融合的「公约数」。
3.2 欧拉角与四元数
坐标系转换绕不开旋转的表达。最直观的是欧拉角，但坑也最多。
3.2.1 欧拉角
欧拉角用三个角度描述旋转：roll（横滚）、pitch（俯仰）、yaw（偏航）。你想想看，就像飞机在空中转——先转机头方向（yaw），再抬头低头（pitch），最后侧倾（roll）。
但欧拉角有个致命问题：万向锁。当pitch接近±90度时，roll和yaw会耦合，丢失一个自由度。我在项目中遇到过，做IMU融合时，车辆爬陡坡，pitch接近90度，姿态解算直接炸了。从那以后，我内部计算一律用四元数。
| 表达方式 | 优点 | 缺点 |
|---|---|---|
| 欧拉角 | 直观，容易理解 | 万向锁，插值困难 |
| 四元数 | 无奇异性，可平滑插值 | 不够直观，有单位约束 |
| 旋转矩阵 | 计算方便，可组合 | 9个参数，冗余大 |
3.2.2 四元数
四元数用四个数表示旋转：q = [w, x, y, z]，其中w是实部，x,y,z是虚部。它本质上是一个单位四元数，模长为1。
为什么用四元数？说白了，它没有欧拉角的奇异性，而且可以平滑插值（SLERP）。做轨迹预测时，两个姿态之间的插值，用四元数就非常自然。
小技巧：我习惯把四元数归一化写在每个旋转计算的前面。曾经有一次忘记归一化，累积误差越来越大，最后姿态漂移了10度。血的教训。
3.3 刚体变换矩阵
一个刚体在三维空间中的变换，包含旋转和平移。我们用4x4的齐次变换矩阵来表示：
T = [ R t ]
[ 0 1 ]其中R是3x3旋转矩阵，t是3x1平移向量。这个矩阵可以把一个点从传感器坐标系变换到车体坐标系：
P_body = T_sensor_to_body * P_sensor注意，这里用的是齐次坐标，点要写成[x, y, z, 1]^T的形式。
我个人习惯，把所有传感器的外参（即传感器到车体的变换矩阵）存成一个配置文件。每次启动时加载，统一管理。这样换传感器或者调整安装位置，只需要改配置文件，不用改代码。
避坑指南：我曾经遇到过一个问题——激光雷达和摄像头的时间戳没对齐。坐标系转换算得再准，时间对不上也是白搭。所以做变换之前，先确认时间同步。
3.4 坐标转换库（tf2/Eigen）
手写坐标转换？没必要。ROS的tf2和Eigen库已经帮我们封装好了。
3.4.1 tf2
tf2是ROS的坐标变换库。它维护了一棵「坐标树」，所有坐标系之间的关系都在这棵树上。你只需要知道两个坐标系的名字，就能拿到它们之间的变换。
// 获取激光雷达到车体的变换
geometry_msgs::TransformStamped transform;
try {
transform = tfBuffer->lookupTransform(
"base_link", "laser_frame", ros::Time(0));
} catch (tf2::TransformException &ex) {
ROS_WARN("%s", ex.what());
}tf2的好处是自动管理坐标树，支持时间戳查询。你想想看，如果传感器数据有延迟，tf2可以帮你找到对应时刻的变换关系。
3.4.2 Eigen
Eigen是C++的线性代数库，在自动驾驶领域几乎是标配。它提供了Matrix、Quaternion、Isometry3d等类型，做坐标转换非常方便。
#include <Eigen/Dense>
#include <Eigen/Geometry>
// 定义旋转（四元数）
Eigen::Quaterniond q(0.707, 0.0, 0.707, 0.0); // w,x,y,z
// 定义平移
Eigen::Vector3d t(1.0, 0.0, 0.5);
// 构建变换矩阵
Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
T.rotate(q);
T.pretranslate(t);
// 变换一个点
Eigen::Vector3d p_sensor(2.0, 3.0, 1.0);
Eigen::Vector3d p_body = T * p_sensor;我个人更倾向于用Eigen做内部计算，因为它的API更干净，而且不依赖ROS。在嵌入式平台上，Eigen也能跑得很流畅。
我的建议：tf2适合做「查询」——我需要知道两个坐标系当前的关系。Eigen适合做「计算」——我要批量变换点云或做矩阵运算。两者配合使用，效率最高。
3.5 知识体系总览
下面这张图总结了坐标系转换的核心知识结构。你可以把它当作一个「速查地图」：
坐标系转换是感知融合的「地基」。地基不稳，上层算法再牛也白搭。我个人建议，花时间把坐标系定义和转换逻辑理清楚，后面会省很多麻烦。
实战建议：写一个坐标系转换的单元测试。每次改外参或加新传感器，先跑测试。我团队现在有30多个测试用例，覆盖了所有传感器的正反变换。上线两年，没出过坐标系相关的bug。
可可精品资料库