第1章：坐标系与刚体运动基础

各位同学，欢迎来到多传感器融合定位的第一课。

说实话，我刚开始做定位系统那会儿，最头疼的不是算法有多复杂，而是坐标系搞混了。有一次在实车测试，IMU的数据死活对不上GPS，折腾了两天才发现——我把车体坐标系和传感器坐标系的Y轴方向搞反了。嗯，从那以后，我养成了一个习惯：拿到任何传感器，第一件事就是确认它的坐标系定义。

这一章，我们就来把坐标系和刚体运动这些基础打牢。你想想看，如果连“我在哪”都说不清楚，后面谈什么融合定位呢？

1.1 三个核心坐标系

多传感器融合里，我们天天跟三个坐标系打交道：世界坐标系、车体坐标系、传感器坐标系。说白了，就是回答三个问题：

世界坐标系：车在哪儿？—— 全局定位的参考系
车体坐标系：车头朝哪？—— 车辆自身的运动描述
传感器坐标系：传感器装在哪？—— 每个传感器的观测基准

我个人习惯，在项目一开始就画一张坐标系关系图。下面这张图，就是我在课程里反复用的框架：

1.2 世界坐标系（W系）

世界坐标系是全局参考系，它不随车辆移动而改变。在自动驾驶里，我们常用的有：

经纬度坐标系（WGS84）：GPS/RTK直接输出的原始数据
UTM坐标系：把经纬度投影到平面，方便计算距离和角度
地心地固坐标系（ECEF）：卫星定位的底层参考

我在项目里一般这样处理：GPS原始数据是WGS84经纬度，先转成UTM坐标，再作为世界坐标系使用。为什么？因为UTM是平面直角坐标系，算欧氏距离非常方便。

💡 我的习惯： 在代码里用枚举类型定义坐标系，避免硬编码。比如 enum CoordFrame { WGS84, UTM, ECEF, BODY };，每次数据流转都显式标注坐标系。

1.3 车体坐标系（B系）

车体坐标系固定在车辆上。常见的定义方式有两种：

标准	X轴	Y轴	Z轴
ISO 8855	车辆前进方向	车辆左侧	垂直向上
SAE J670	车辆前进方向	车辆右侧	垂直向下

嗯，这里要注意：ISO和SAE的Y轴方向是反的！我见过不少新手在这上面栽跟头。我个人习惯用ISO标准（前-左-上），因为和大多数IMU的坐标系定义一致。

⚠️ 避坑指南： 我曾经在一个项目里，IMU用的是SAE标准，但轮速计用的是ISO标准。融合的时候没注意，结果车辆明明在直行，定位结果却显示在转圈。后来花了整整一天排查，才发现是Y轴方向反了。所以，拿到任何传感器，第一件事：确认坐标系定义！

1.4 传感器坐标系（S系）

每个传感器都有自己的坐标系。常见的传感器坐标系定义：

IMU：通常遵循右-前-上（或前-左-上），加速度和角速度都是在这个坐标系下测量的
相机：Z轴指向镜头前方，X轴向右，Y轴向下（图像坐标系）
LiDAR：通常X轴指向车辆前方，Y轴向左，Z轴向上
轮速计：一般直接定义在车体坐标系下，或者有固定的安装角度

传感器坐标系到车体坐标系的变换，就是我们常说的外参。外参标定的精度，直接决定了融合定位的效果。

1.5 刚体运动的数学表达

坐标系之间的变换，本质上是刚体运动。刚体运动包含两部分：旋转和平移。

旋转的表达方式有三种：旋转矩阵、四元数、欧拉角。我分别说一下：

1.5.1 旋转矩阵

旋转矩阵是一个3x3的正交矩阵，行列式为+1。它最直观，但参数冗余（9个参数，实际只有3个自由度）。

// C++ 示例：旋转矩阵定义
Eigen::Matrix3d R;
R = Eigen::AngleAxisd(M_PI/4, Eigen::Vector3d::UnitZ())  // 绕Z轴旋转45度
  * Eigen::AngleAxisd(M_PI/6, Eigen::Vector3d::UnitX()); // 再绕X轴旋转30度

    📌 关键点： 旋转矩阵的逆等于它的转置（R⁻¹ = Rᵀ），这个性质在代码里经常用到，可以避免求逆运算。

1.5.2 四元数

四元数是我个人最喜欢的旋转表达方式。它没有万向锁问题，插值也方便。四元数 q = [w, x, y, z]，其中 w 是实部，[x, y, z] 是虚部。

// C++ 示例：四元数使用
Eigen::Quaterniond q;
q = Eigen::AngleAxisd(M_PI/3, Eigen::Vector3d::UnitY()); // 绕Y轴旋转60度

// 四元数乘法（组合旋转）
Eigen::Quaterniond q1(0.707, 0.707, 0, 0); // 绕X轴90度
Eigen::Quaterniond q2(0.866, 0, 0.5, 0);   // 绕Y轴60度
Eigen::Quaterniond q_combined = q1 * q2;   // 先q2再q1

💡 我的经验： 四元数乘法不满足交换律！q1 * q2 和 q2 * q1 结果不同。我在做IMU姿态解算时，经常搞混旋转顺序。后来我养成了一个习惯：在代码注释里写明“当前四元数表示从A系到B系的旋转”。

1.5.3 欧拉角

欧拉角最直观，但有两个问题：万向锁和旋转顺序依赖。常用的顺序有ZYX（偏航-俯仰-横滚）和ZXY等。

// C++ 示例：欧拉角转四元数（ZYX顺序）
double yaw = 0.5;   // 偏航
double pitch = 0.2; // 俯仰
double roll = 0.1;  // 横滚

Eigen::Quaterniond q;
q = Eigen::AngleAxisd(yaw,   Eigen::Vector3d::UnitZ())
  * Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY())
  * Eigen::AngleAxisd(roll,  Eigen::Vector3d::UnitX());

⚠️ 避坑指南： 我曾经在调试一个无人机项目时，用欧拉角做姿态控制。当俯仰角接近90度时，偏航和横滚突然耦合，无人机直接失控。这就是万向锁！从那以后，我在所有需要连续旋转的场景里，都改用四元数。

1.6 三种表达方式的对比

表达方式	优点	缺点	适用场景
旋转矩阵	直观，容易理解	参数冗余，计算量大	坐标变换、可视化
四元数	无万向锁，插值平滑	不够直观	IMU解算、姿态估计
欧拉角	物理意义明确	万向锁，顺序依赖	人机交互、简单控制

1.7 刚体变换的完整表达

一个完整的刚体变换，包含旋转和平移。我们用变换矩阵 T 来表示：

// 变换矩阵 T = [R  t]
//             [0  1]
Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
T.block<3,3>(0,0) = R;  // 旋转矩阵
T.block<3,1>(0,3) = t;  // 平移向量

// 应用变换：点 p 从传感器坐标系变换到车体坐标系
Eigen::Vector3d p_sensor(1.0, 2.0, 3.0);
Eigen::Vector3d p_body = T.block<3,3>(0,0) * p_sensor + T.block<3,1>(0,3);

说白了，整个多传感器融合定位，就是在不断地做一件事：把不同坐标系下的观测数据，通过刚体变换，统一到同一个坐标系下。你想想看，GPS给的是经纬度（世界系），IMU给的是加速度和角速度（传感器系），轮速计给的是速度（车体系）——不把它们统一起来，怎么融合？

    📌 本章核心要点：
    三个坐标系：世界系（W）、车体系（B）、传感器系（S）
坐标系变换链：W ← B ← S，每个箭头对应一个刚体变换
旋转表达：旋转矩阵（直观）、四元数（实用）、欧拉角（易理解）
刚体变换 = 旋转 + 平移，用变换矩阵 T 统一表达
外参标定：求传感器到车体的变换矩阵 T_BS

好了，这一章的内容就到这里。坐标系和刚体运动是后面所有章节的基础，建议你动手写代码验证一下。下一章我们会讲IMU的测量模型和预积分，到时候这些坐标系变换的知识会反复用到。

可可精品资料库

📚 课程目录

第1章：坐标系与刚体运动基础

1.1 三个核心坐标系

1.2 世界坐标系（W系）

1.3 车体坐标系（B系）

1.4 传感器坐标系（S系）

1.5 刚体运动的数学表达

1.5.1 旋转矩阵

1.5.2 四元数

1.5.3 欧拉角

1.6 三种表达方式的对比

1.7 刚体变换的完整表达