2. 坐标系基础：世界坐标系、机器人坐标系、传感器坐标系、坐标变换与齐次矩阵
说实话，坐标系这个东西，刚入行时我觉得太简单了。不就是三个轴嘛，谁不会？
直到我第一次做多传感器融合的项目——激光雷达和IMU对不上，GPS数据飘得离谱，机器人明明往前走，地图上却在后退。折腾了三天，最后发现是某个坐标系的旋转方向搞反了。
嗯，从那以后，我再也不敢小看坐标系了。
2.1 为什么需要多个坐标系？
你想想看，一个机器人要完成定位和建图，它得同时回答三个问题：
- 我在哪？ —— 这需要世界坐标系
- 我的身体怎么动的？ —— 这需要机器人坐标系
- 传感器看到了什么？ —— 这需要传感器坐标系
这三个问题，没法用同一个坐标系回答。因为每个坐标系都有自己的“原点”和“朝向”。
核心认知：SLAM的本质，就是不断在这三个坐标系之间来回切换，找到它们之间的变换关系。
2.2 世界坐标系（World Frame）
世界坐标系是绝对的、固定的。它不会动。
我习惯把世界坐标系的原点设在机器人启动的位置，或者地图的某个角落。比如，GPS的经纬度就是一种世界坐标系，但我们在室内SLAM中通常用笛卡尔坐标系。
- 符号：通常记为 \( W \) 或 \( G \)
- 特点：静止不动，是所有其他坐标系的参考基准
- 用途：存储地图、规划全局路径、回环检测
2.3 机器人坐标系（Robot/Body Frame）
机器人坐标系是跟着机器人走的。它的原点在机器人的中心（通常是质心或底盘中心），x轴朝前，y轴朝左，z轴朝上。
我在项目中遇到过一个问题：用里程计推算位置时，如果直接用机器人坐标系下的速度积分，结果会越偏越远。为什么？因为机器人坐标系本身在旋转，速度方向一直在变。
- 符号：通常记为 \( B \) 或 \( R \)
- 特点：随机器人运动而运动，是控制指令的参考系
- 用途：发布速度指令、处理IMU数据、局部避障
2.4 传感器坐标系（Sensor Frame）
每个传感器都有自己的小世界。激光雷达的坐标系原点在旋转中心，相机的坐标系原点在光心，IMU的坐标系在芯片内部。
我建议你拿到一个新传感器时，第一件事就是查它的坐标系定义。有些厂家把x轴朝前，有些朝右，搞混了就是灾难。
- 符号：通常记为 \( S \) 或 \( C \)（Camera）、\( L \)（Lidar）
- 特点：每个传感器独立，需要标定到机器人坐标系
- 用途：原始数据采集、特征提取
个人习惯：我会在代码里用枚举类型明确定义每个坐标系，比如 Frame::WORLD、Frame::ROBOT、Frame::LIDAR。这样即使项目做大了，也不会搞混。
2.5 坐标变换：从传感器到世界
说白了，坐标变换就是“搬家”。把传感器看到的一个点，搬到世界坐标系里去。
举个例子：激光雷达检测到前方2米处有一个障碍物。这个“前方2米”是在激光雷达坐标系下的。但机器人是倾斜的，激光雷达是装在侧面的，所以这个点在世界坐标系下的位置完全不一样。
变换分为两种：
- 平移：坐标系原点之间的位移，用向量 \( t \) 表示
- 旋转：坐标系轴之间的朝向差异，用旋转矩阵 \( R \) 表示
完整的变换公式：
P_w = R * P_s + t其中 \( P_w \) 是世界坐标，\( P_s \) 是传感器坐标。
2.6 齐次矩阵：把变换写成乘法
你发现没有？上面的公式里既有乘法又有加法，写起来麻烦，组合多个变换时更麻烦。
齐次矩阵就是来解决这个问题的。它把旋转和平并到一起，变成一个4x4的矩阵：
| R t |
| 0 1 |这样，变换就变成了纯粹的矩阵乘法：
P_w = T * P_s其中 \( P_w \) 和 \( P_s \) 都是齐次坐标（4维向量，最后一维是1）。
为什么重要？ 因为你可以把多个变换连乘起来：
T_world_to_sensor = T_world_to_robot * T_robot_to_sensor这样，从传感器到世界的变换，就变成了两个矩阵的乘积。简洁、高效、不易出错。
2.7 知识体系图
下面这张图展示了本章的核心逻辑：
2.8 避坑指南
我曾经犯过的错：
- 把旋转矩阵的转置和逆搞混了。记住：旋转矩阵是正交矩阵，逆等于转置，但平移部分不能这么干。
- 齐次坐标最后一维忘了写1。结果矩阵乘法出来全是错的，查了半天。
- 多个传感器标定时，变换顺序搞反了。先旋转后平移，和先平移后旋转，结果完全不同。
调试小技巧：写一个简单的单元测试，把一个已知点从传感器坐标系变换到世界坐标系，再变换回来。如果结果和原点一致，说明变换写对了。
2.9 代码示例：C++中的坐标变换
#include <Eigen/Dense>
// 定义齐次变换矩阵
Eigen::Matrix4d T_world_to_robot;
T_world_to_robot <<
1, 0, 0, 1.0, // x方向平移1米
0, 1, 0, 0.5, // y方向平移0.5米
0, 0, 1, 0.0, // z方向不变
0, 0, 0, 1;
// 传感器坐标系下的点
Eigen::Vector4d p_sensor(0.5, 0.0, 0.0, 1.0);
// 变换到世界坐标系
Eigen::Vector4d p_world = T_world_to_robot * p_sensor;
std::cout << "世界坐标: " << p_world.transpose() << std::endl;
// 输出: 1.5, 0.5, 0.0, 1嗯，代码很简单，但背后是整套坐标系理论在支撑。没有这个基础，你写再多代码也是空中楼阁。