2. 坐标系与变换：世界坐标系、机器人坐标系、传感器坐标系、坐标变换基础、四元数与欧拉角
各位同学，欢迎来到第二章。说实话，坐标系这个东西，刚入行时我觉得它太简单了——不就是几个箭头嘛。直到我第一次在真实机器人上调试定位，发现地图里的墙和激光雷达扫到的墙怎么都对不上，折腾了两天才意识到是某个传感器坐标系装反了。嗯，从那以后我再也不敢小看坐标系了。
这一章，我们就把坐标系和变换彻底讲透。你想想看，一个机器人要导航，它得知道三件事：我在哪？我要去哪？我怎么去？这三个问题，每一个都离不开坐标系。
2.1 世界坐标系：机器人的“绝对参考系”
世界坐标系，说白了就是整个场景的“大地图坐标系”。它固定不动，所有物体的位置都相对于它来描述。
我个人习惯把世界坐标系的原点选在机器人启动位置，或者地图的某个角落。比如在室内场景，我经常把原点设在房间的西南角，X轴指向东，Y轴指向北，Z轴指向上。这样符合右手定则，后续计算也方便。
在实际项目中，世界坐标系通常由建图算法（比如GMapping、Cartographer）自动定义。但如果你手动指定，记得在代码里写清楚，不然换个人来维护代码，可能得猜半天。
2.2 机器人坐标系：以机器人为中心看世界
机器人坐标系（也叫车体坐标系、基坐标系）是固定在机器人本体上的。原点一般在机器人的几何中心或驱动轮轴中心，X轴指向机器人前进方向，Y轴指向左侧，Z轴指向上。
为什么要搞个机器人坐标系？因为传感器都装在机器人身上，它们测到的数据天然就是相对于机器人自身的。比如激光雷达扫到前方1米有障碍物，这个“前方1米”就是相对于机器人坐标系说的。
我记得有一次，团队里新来的同事把机器人坐标系的Y轴方向搞反了，结果机器人明明想左转，却往右拐，差点撞墙。从那以后，我要求所有人在代码里显式注释坐标轴方向。
2.3 传感器坐标系：每个传感器都有自己的“小世界”
激光雷达、摄像头、IMU、轮式里程计……每个传感器都有自己的坐标系。它们相对于机器人坐标系有一个固定的偏移和旋转，这个关系叫做外参。
举个例子，激光雷达装在机器人顶部前方10厘米处，朝前看。那么激光雷达坐标系的原点就在机器人坐标系中 (0.1, 0, 0) 的位置，朝向与机器人坐标系一致。
| 传感器 | 常见安装位置 | 坐标系特点 |
|---|---|---|
| 激光雷达 | 顶部、前方 | X轴朝前，Z轴朝上 |
| 摄像头 | 前方、侧面 | Z轴朝前（相机坐标系惯例） |
| IMU | 中心附近 | 与机器人坐标系对齐 |
| 轮式里程计 | 驱动轮轴 | 通常与机器人坐标系重合 |
2.4 坐标变换基础：从一个坐标系到另一个坐标系
坐标变换，说白了就是把一个点从一个坐标系“翻译”到另一个坐标系。比如激光雷达测到前方1米有障碍物，这个点是在激光雷达坐标系下的。但机器人需要知道这个障碍物相对于自己（机器人坐标系）的位置，这就得做变换。
变换公式其实很简单：
P_robot = R * P_sensor + T其中：
P_sensor是传感器坐标系下的点R是旋转矩阵（3x3）T是平移向量（3x1）P_robot是机器人坐标系下的点
我在项目中经常用ROS的 tf2 库来处理变换。你只需要发布传感器到机器人的变换关系，剩下的计算交给库去搞定。但千万别以为用了库就可以不懂原理——有一次tf树断链了，我花了半小时才定位到问题，就是因为某个变换没发布。
2.5 四元数与欧拉角：旋转的两种表达方式
旋转怎么表示？最直观的是欧拉角：绕X轴转叫翻滚（Roll），绕Y轴转叫俯仰（Pitch），绕Z轴转叫偏航（Yaw）。
但欧拉角有个大坑——万向锁。当俯仰角接近±90度时，翻滚和偏航会变得无法区分。我在做无人机项目时就遇到过这个问题，飞控在俯仰角90度时突然乱转，查了半天才发现是欧拉角导致的。
所以，实际工程中我更推荐用四元数。它用四个数表示旋转，没有万向锁问题，插值也平滑。
四元数的形式是：
q = w + x*i + y*j + z*k其中 w 是实部，x, y, z 是虚部。单位四元数满足 w² + x² + y² + z² = 1。
欧拉角和四元数可以互相转换。ROS里提供了现成的函数：
# 欧拉角转四元数
from tf.transformations import quaternion_from_euler
q = quaternion_from_euler(roll, pitch, yaw)
# 四元数转欧拉角
from tf.transformations import euler_from_quaternion
roll, pitch, yaw = euler_from_quaternion([x, y, z, w])2.6 知识体系总览
下面这张图总结了本章的核心内容。你可以看到，所有传感器数据最终都要变换到世界坐标系下，才能用于导航决策。
2.7 本章小结
好了，这一章的内容就这些。总结一下：
- 世界坐标系是绝对参考系，固定不动
- 机器人坐标系随机器人移动，是传感器数据的汇聚点
- 传感器坐标系各有各的“小世界”，需要通过外参标定统一
- 坐标变换就是旋转+平移，先旋转后平移
- 四元数比欧拉角更可靠，建议内部运算都用它
说实话，坐标系这个东西，理解起来不难，但真正用好需要大量实践。我建议你找个开源数据集，自己动手写几行变换代码，把激光雷达点云从传感器坐标系变换到世界坐标系下可视化出来。看到点云和地图完美对齐的那一刻，你就真正懂了。
公众号:蓝海资料掘金营，微信deep3321