第2章：坐标系与变换基础

做机器人这行，坐标系就是我们的“世界观”。

我刚开始接触SLAM时，总觉得坐标系这东西太基础了，不就是个参考系嘛。直到有一次，我在调试一个视觉惯导融合系统，明明算法逻辑都对，轨迹就是飘得离谱。折腾了两天，最后发现——IMU的坐标系定义和相机差了90度。嗯，从那以后，我再也不敢小看坐标系了。

说白了，世界坐标系就是那个“绝对参考系”。你把它想象成房间里的一个固定墙角，所有东西的位置都相对于它来描述。在SLAM里，世界坐标系通常就是我们建图的基准，一般用W表示。

机器人坐标系呢？它跟着机器人走。我习惯叫它机体坐标系，用B或R表示。原点在机器人中心，x轴朝前，y轴朝左，z轴朝上——这是最常见的定义方式。

关键点：世界坐标系是静止的，机器人坐标系是运动的。SLAM的核心任务之一，就是估算出这两个坐标系之间的变换关系。

你想想看，机器人每时每刻都在动。它身上的传感器——激光雷达、相机、IMU——测得的数据都是相对于机器人坐标系的。但我们要建的地图、规划的路径，都得用世界坐标系来表达。所以，坐标系变换就成了绕不开的坎。

齐次变换矩阵，我愿称之为“机器人工程师的瑞士军刀”。

一个4x4的矩阵，同时包含了旋转和平移信息。它的形式长这样：

T = [ R   t ]
    [ 0   1 ]

其中R是3x3的旋转矩阵，t是3x1的平移向量。最后一行是[0 0 0 1]，这是齐次坐标的固定格式。

为什么非要用4x4？直接用3x3旋转矩阵加3x1平移向量不行吗？

行是行，但连续变换时就很麻烦。比如你要把点从相机坐标系转到机器人坐标系，再转到世界坐标系。用齐次矩阵，直接三个矩阵乘起来就行。不用齐次矩阵？你得先旋转再平移，每一步都得小心顺序。

我的习惯：在代码里，我通常用Eigen库的Isometry3d来表示齐次变换矩阵。它内部就是4x4，但用起来比手动操作矩阵安全得多。

举个例子，假设机器人坐标系B相对于世界坐标系W的变换是T_WB。那么世界坐标系中的一个点p_W，在机器人坐标系下的坐标就是：

p_B = T_WB.inverse() * p_W

反过来，机器人测到的点p_B，要转到世界坐标系：

p_W = T_WB * p_B

就这么简单。但要注意——矩阵乘法不满足交换律，顺序搞反了，结果就完全不对了。

我曾经踩过的坑：有一次在写多传感器融合代码时，把T_WB和T_BW搞混了。结果融合出来的轨迹直接反着跑。调试了整整一个下午才发现，就是乘法的顺序反了。所以我现在写代码时，一定会用注释标清楚每个变换矩阵的“从哪到哪”。

聊完平移，咱们聊聊旋转。表示旋转的方式有好几种，最常用的就是欧拉角和四元数。

欧拉角很直观。绕x轴转叫roll，绕y轴转叫pitch，绕z轴转叫yaw。飞机、无人机、机器人里都这么叫。

但欧拉角有个致命问题——万向锁。当pitch接近±90度时，roll和yaw的旋转轴会重合，丢失一个自由度。我在做机械臂运动规划时就遇到过这个问题，规划出来的轨迹在奇异点附近剧烈抖动。

建议：欧拉角适合给人看，不适合给机器算。做插值、优化、滤波时，尽量用四元数或旋转矩阵。

四元数听起来玄乎，其实就是一个四维向量：q = [w, x, y, z]。它没有万向锁问题，插值平滑，计算效率高。

我个人的习惯是：

四元数和旋转矩阵可以互相转换。Eigen库里有现成的函数：

// 四元数转旋转矩阵
Eigen::Quaterniond q(1.0, 0.0, 0.0, 0.0);
Eigen::Matrix3d R = q.toRotationMatrix();

// 旋转矩阵转四元数
Eigen::Quaterniond q2(R);

这里要注意，四元数在使用前一定要归一化。我见过不少新手直接拿未归一化的四元数去算，结果旋转出来的点位置全错了。

避坑指南：四元数的乘法顺序和旋转矩阵一样，不满足交换律。q1 * q2表示先做q2的旋转，再做q1的旋转。这个顺序我刚开始也经常搞反。

下面这张图，是我自己总结的坐标系与变换的知识结构。每次带新人时，我都会先让他们看这张图。

在实际项目中，坐标系管理是个容易被忽视但极其重要的事。我总结了几条经验：

重要提醒：ROS里用的坐标系约定是REP-105。x轴朝前，y轴朝左，z轴朝上。但有些传感器厂商有自己的定义，比如相机坐标系z轴朝前。接入外部数据时，一定要先确认坐标系定义，再做变换。

嗯，坐标系和变换这块，说难不难，说简单也不简单。关键是要养成好习惯，每次写变换代码时多问自己一句：“这个变换是从哪到哪的？”

可可精品资料库