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

各位同学，今天我们来聊聊SLAM里最基础、也最绕不开的一个话题——坐标系与刚体运动。

说实话，我刚开始做机器人导航那会儿，觉得坐标系这东西太简单了，不就是几个箭头嘛。结果第一次做多传感器融合，IMU和激光雷达的数据怎么都对不上，折腾了两天才发现——哦，原来两个传感器的坐标系定义压根不一样。从那以后，我再也不敢小看坐标系了。

1.1 世界坐标系与机器人坐标系

先说说最常用的两个坐标系。

世界坐标系，也叫全局坐标系。你可以把它想象成一个参考系。比如我们在一栋楼里做导航，世界坐标系的原点可以定在大门口，X轴指向东，Y轴指向北，Z轴指向天。不管机器人怎么动，这个坐标系是固定不变的。

机器人坐标系，也叫本体坐标系。这个坐标系是跟着机器人走的。原点在机器人的中心，X轴通常指向机器人前进方向，Y轴指向左侧，Z轴指向上方。

为什么要区分这两个？

你想想看，激光雷达扫描到的障碍物，是在机器人坐标系下的坐标。但我们要建的地图，是在世界坐标系下的。所以必须做坐标变换。

1.2 传感器坐标系

做SLAM，我们至少会用到三种传感器：激光雷达、IMU、相机。每个传感器都有自己的坐标系。

传感器	坐标系定义	常见问题
激光雷达	原点在旋转中心，X轴向前	安装时可能倾斜，需要标定
IMU	原点在芯片中心，三轴与芯片对齐	加速度计和陀螺仪的轴可能不重合
相机	原点在光心，Z轴指向拍摄方向	图像坐标系和相机坐标系容易搞混

我个人习惯，在项目一开始就把所有传感器的坐标系画出来，贴在墙上。别笑，这招真的管用。有一次团队新来的同事，把IMU的Z轴方向搞反了，结果机器人一直在原地打转。嗯，后来我们就把坐标系图做成了团队标配。

1.3 欧拉角

欧拉角是用来描述旋转的。说白了，就是绕三个轴分别转多少度。

常用的顺序是ZYX：先绕Z轴转（偏航角yaw），再绕Y轴转（俯仰角pitch），最后绕X轴转（滚转角roll）。

举个例子：

// 欧拉角转旋转矩阵（ZYX顺序）
double yaw = 0.5;    // 绕Z轴转0.5弧度
double pitch = 0.2;  // 绕Y轴转0.2弧度
double roll = 0.1;   // 绕X轴转0.1弧度

// 对应的旋转矩阵
R = Rz(yaw) * Ry(pitch) * Rx(roll);

1.4 四元数

四元数，听起来高大上，其实就是一个四维向量：q = [w, x, y, z]。其中w是实部，x、y、z是虚部。

为什么用四元数？三个原因：

• 没有万向锁问题
• 插值平滑
• 计算效率高

四元数表示旋转的公式：

// 绕单位向量 [nx, ny, nz] 旋转 theta 角度
q.w = cos(theta/2)
q.x = nx * sin(theta/2)
q.y = ny * sin(theta/2)
q.z = nz * sin(theta/2)

我个人建议，所有SLAM系统内部都用四元数。只有在给用户看的时候，才转成欧拉角。这样既避免了万向锁，又方便人理解。

1.5 坐标变换基础

坐标变换，说白了就是回答一个问题：一个点在A坐标系下的坐标，在B坐标系下是多少？

变换公式很简单：

// 点P在B坐标系下的坐标 = 旋转矩阵 * 点P在A坐标系下的坐标 + 平移向量
P_B = R * P_A + t

但实际项目中，我们通常用齐次坐标来写：

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

// 变换
P_B = T * P_A

为什么要用齐次坐标？因为可以把旋转和平移写在一个矩阵里，连续变换时直接矩阵相乘就行，方便得很。

1.6 知识体系总览

下面这张图，是我做SLAM这些年总结出来的坐标系知识体系。你把它理解了，后面学建图和路径规划会轻松很多。

这张图把整个知识体系串起来了。从三个坐标系出发，到三种旋转表示方法，再到坐标变换和齐次变换，最后落到实际应用。你每次做SLAM项目，都可以拿这张图来对照，看看自己卡在哪一步。

好了，坐标系和刚体运动就讲到这里。记住一句话：坐标系是SLAM的基石，搞不清楚坐标系，后面全是坑。我当年踩过的坑，希望你们能绕过去。

可可精品资料库