第二章：坐标系统与空间变换

说实话，做多传感器融合这些年，我见过太多项目栽在坐标变换这个坑里。明明算法模型都调好了，一上路就飘，最后发现是某个传感器的坐标系定义反了。嗯，咱们今天就把这块彻底讲透。

2.1 三大坐标系统：世界、车身、传感器

先理清一个概念。我们做融合，本质上是在回答一个问题：不同传感器看到的同一个物体，到底在哪儿？

这就要靠坐标系统来统一语言了。

2.1.1 世界坐标系

世界坐标系是绝对的参考系。你可以把它想象成地球上的经纬度加海拔。我习惯用东北天坐标系（ENU），也就是X轴指向东，Y轴指向北，Z轴指向天顶。为什么？因为GPS输出的经纬高数据，转成ENU最直观。

2.1.2 车身坐标系

车身坐标系跟着车走。标准定义是：X轴朝前（车头方向），Y轴朝左（驾驶员左侧），Z轴朝上。原点一般在车辆后轴中心。

为什么是后轴中心？因为车辆运动学模型里，后轴中心是转向的基准点。我之前有个项目把原点放在前保险杠，结果做路径规划时，转弯半径怎么算都不对。后来改成后轴中心，问题迎刃而解。

2.1.3 传感器坐标系

每个传感器都有自己的小世界。比如激光雷达，通常X轴朝前，Y轴朝左，Z轴朝上。但摄像头就不一样了——很多摄像头是Z轴朝前，X轴朝右，Y轴朝下（图像坐标系）。

2.2 旋转的三种表达方式

旋转是坐标变换里最绕的部分。说白了，就是怎么描述一个物体转了多少。有三种主流方法：欧拉角、旋转矩阵、四元数。

2.2.1 欧拉角

欧拉角最直观。就是绕三个轴依次旋转：偏航角（Yaw）绕Z轴，俯仰角（Pitch）绕Y轴，横滚角（Roll）绕X轴。

但欧拉角有个致命问题——万向锁。当俯仰角接近±90°时，偏航和横滚会耦合，丢失一个自由度。我在做无人机姿态解算时就吃过这个亏，飞机一抬头，姿态就乱跳。

2.2.2 旋转矩阵

旋转矩阵是个3×3的矩阵，用9个数描述旋转。好处是数学性质好，可以连续相乘。坏处是——冗余，而且有约束条件（必须是正交矩阵，行列式为1）。

我一般在做传感器标定时用旋转矩阵。因为标定过程需要解算多个姿态，矩阵形式方便做最小二乘优化。

// 绕Z轴旋转θ角的旋转矩阵
R_z(θ) = [cosθ  -sinθ  0]
         [sinθ   cosθ  0]
         [0      0     1]

// 绕Y轴旋转φ角的旋转矩阵
R_y(φ) = [cosφ   0   sinφ]
         [0      1   0   ]
         [-sinφ  0   cosφ]

// 绕X轴旋转ψ角的旋转矩阵
R_x(ψ) = [1   0      0   ]
         [0   cosψ  -sinψ]
         [0   sinψ   cosψ]

2.2.3 四元数

四元数是我个人最推荐的方式。它用4个数（w, x, y, z）表示旋转，没有万向锁，插值平滑，计算效率高。

你可能会问：为什么是4个数？其实四元数可以理解为「绕一个单位向量旋转θ角」。w = cos(θ/2)，(x,y,z) = sin(θ/2) * 单位向量。

// 四元数乘法（两个旋转叠加）
q_result = q1 * q2

// 四元数旋转向量
v_rotated = q * v * q_conjugate

// 四元数转旋转矩阵（常用公式）
R = [1-2(y²+z²)  2(xy-wz)    2(xz+wy)  ]
    [2(xy+wz)    1-2(x²+z²)  2(yz-wx)  ]
    [2(xz-wy)    2(yz+wx)    1-2(x²+y²)]

2.3 刚体变换与齐次坐标

旋转讲完了，但实际变换还有平移。一个刚体从A坐标系到B坐标系，需要旋转加平移。这就是刚体变换。

齐次坐标就是干这个的。把3D点(x,y,z)变成4D的(x,y,z,1)，然后用一个4×4的变换矩阵一次搞定旋转和平移。

// 齐次变换矩阵
T = [R   t]
    [0   1]

// 其中R是3×3旋转矩阵，t是3×1平移向量

// 应用变换
p_B = T * p_A
// p_B和p_A都是齐次坐标 [x, y, z, 1]^T

为什么用齐次坐标？说白了，就是为了把旋转和平移写成一个矩阵乘法。这样多个变换可以连乘，形成一个变换链。

2.4 坐标转换的工程实现

理论讲完了，咱们看看实际怎么写代码。我以C++为例，展示一个完整的坐标转换流程。

2.4.1 数据结构定义

// 3D点
struct Point3D {
    double x, y, z;
};

// 四元数
struct Quaternion {
    double w, x, y, z;
};

// 齐次变换矩阵
struct Transform {
    Eigen::Matrix3d rotation;    // 旋转矩阵
    Eigen::Vector3d translation; // 平移向量
    
    // 应用变换
    Point3D apply(const Point3D& p) const {
        Eigen::Vector3d v(p.x, p.y, p.z);
        v = rotation * v + translation;
        return {v.x(), v.y(), v.z()};
    }
};

2.4.2 传感器到车身坐标转换

// 假设激光雷达安装在车顶，位置偏移和姿态已知
Transform lidar_to_vehicle;

// 初始化：从标定文件读取
lidar_to_vehicle.rotation = ...;    // 旋转矩阵
lidar_to_vehicle.translation = ...; // 平移向量 [dx, dy, dz]

// 转换激光雷达点云到车身坐标系
Point3D point_in_lidar = {1.0, 0.5, -0.3};
Point3D point_in_vehicle = lidar_to_vehicle.apply(point_in_lidar);

2.4.3 车身到世界坐标转换

// 车身到世界坐标，需要车辆的当前位姿
Transform vehicle_to_world;

// 从GPS/IMU获取车辆位置和姿态
vehicle_to_world.translation = get_gps_position();  // 经纬高转ENU
vehicle_to_world.rotation = get_vehicle_orientation(); // 从IMU四元数转矩阵

// 完整转换链：传感器点 → 世界坐标
Point3D point_in_world = vehicle_to_world.apply(
    lidar_to_vehicle.apply(point_in_lidar)
);

2.4.4 时间同步问题

坐标转换还有一个隐藏的坑——时间同步。不同传感器的数据到达时间不同，车辆本身也在运动。如果你用10ms前的车辆位姿去转换当前时刻的激光雷达数据，结果会有明显的运动畸变。

我的做法是：给每个数据打时间戳，用插值法获取对应时刻的车辆位姿。

// 线性插值车辆位姿
Transform interpolate_pose(
    const Transform& pose_t1, double t1,
    const Transform& pose_t2, double t2,
    double t_target
) {
    double ratio = (t_target - t1) / (t2 - t1);
    // 平移线性插值
    // 旋转用四元数球面线性插值（SLERP）
    // ...
}

2.5 总结与建议

坐标变换这东西，说难不难，说简单也不简单。我给你的建议是：

• 统一坐标系定义：项目一开始就定好所有坐标系的定义，写在文档里
• 用四元数做内部计算：别碰欧拉角，除非你只是显示给人看
• 写单元测试：每个变换矩阵都写测试用例，验证正反变换是否互逆
• 可视化验证：把转换后的点云和图像叠加显示，一眼就能看出问题

下一章我们会讲传感器的时间同步问题。你会发现，时间不同步带来的误差，有时候比坐标转换还大。咱们到时候细聊。