2、坐标系基础：世界坐标系、机器人坐标系、传感器坐标系、坐标变换原理
好，咱们正式开始啃坐标系这块硬骨头。
说实话，我刚开始做机器人时，觉得坐标系不就是个数学概念嘛，有啥好讲的？结果第一次做多传感器融合，数据怎么都对不上，折腾了两天，最后发现是传感器坐标系定义反了。嗯，从那以后我再也不敢小看坐标系了。
2.1 为什么需要多个坐标系？
你想想看，一个机器人身上挂着激光雷达、摄像头、IMU，每个传感器都有自己的“视角”。激光雷达说“前方2米有障碍物”，摄像头说“图像左上角有个红点”，IMU说“我感受到的加速度是X方向”。这些数据如果不统一到一个坐标系下，根本没法用。
说白了，坐标系就是给每个数据一个“户口本”。你得知道这个数据是在谁的坐标系下说的，才能把它翻译成大家都能理解的语言。
核心原则：所有传感器数据最终都要变换到同一个坐标系下，通常是机器人坐标系或世界坐标系。
2.2 世界坐标系（World Frame）
世界坐标系是最大的那个“上帝视角”。它固定在地面上，不随机器人移动而移动。我习惯用 W 或 world 来表示。
举个例子，你在建图时，地图上的每个栅格坐标，都是相对于世界坐标系说的。机器人走到哪里，地图原点都不会变。
- 原点：通常选在机器人启动位置，或者地图的某个固定点
- 轴方向：一般遵循右手定则，X向前，Y向左，Z向上
- 用途：全局路径规划、地图构建、定位
我的习惯：在代码里，我会把世界坐标系命名为 map 或 odom。ROS里常用 map 作为全局坐标系，odom 作为里程计坐标系。两者之间通常有一个漂移补偿变换。
2.3 机器人坐标系（Robot Frame / Base Frame）
机器人坐标系是跟着机器人走的。我一般叫它 base_link 或 robot。它的原点通常在机器人的几何中心，或者驱动轮轴中心。
为什么要搞个机器人坐标系？因为传感器是装在机器人身上的。激光雷达说“前方1米有障碍”，这个“前方”是相对于机器人自身说的，不是相对于世界。
- 原点：机器人中心点
- 轴方向：X指向机器人前进方向，Y指向左侧，Z向上
- 用途：运动控制、局部避障、传感器数据初步处理
注意：不同机器人厂商对机器人坐标系定义可能不同。有的把X定义为前进方向，有的定义为右侧。我曾经因为这个问题，把阿克曼转向的机器人当成差速驱动机器人来控制，结果原地打转。一定要先看文档！
2.4 传感器坐标系（Sensor Frame）
每个传感器都有自己的小世界。激光雷达的坐标系原点在激光头旋转中心，摄像头的坐标系原点在镜头光心，IMU的坐标系原点在芯片封装中心。
这些坐标系通常和机器人坐标系有固定的偏移和旋转。比如激光雷达装在机器人前方10厘米处，朝上仰角5度。这个偏移量就是传感器到机器人的外参。
| 传感器类型 | 常见坐标系命名 | 原点位置 |
|---|---|---|
| 激光雷达 | laser / lidar |
激光头旋转中心 |
| 摄像头 | camera / cam |
镜头光心 |
| IMU | imu |
芯片封装中心 |
| 深度相机 | depth_camera |
红外发射器中心 |
避坑指南：我曾经在标定激光雷达和摄像头时，发现投影总是偏了几个像素。查了半天，原来是激光雷达坐标系定义是X向前、Y向左，而摄像头坐标系是Z向前、X向右。这两个坐标系差了90度旋转。标定前一定要先搞清楚每个传感器的坐标系定义！
2.5 坐标变换原理
坐标变换说白了就是“搬家”。把一个点从传感器坐标系搬到机器人坐标系，再搬到世界坐标系。数学上就是旋转加平移。
2.5.1 刚体变换
一个刚体变换包含两部分：
- 旋转（Rotation）：用旋转矩阵 R 表示，3x3 矩阵
- 平移（Translation）：用平移向量 t 表示，3x1 向量
变换公式很简单：
P_world = R * P_sensor + t其中 P_sensor 是传感器坐标系下的点坐标，P_world 是世界坐标系下的点坐标。
我的习惯：我一般用齐次坐标来写，把旋转和平移合并成一个4x4的变换矩阵。这样连续变换时，直接矩阵乘法就行，代码写起来很清爽。
2.5.2 齐次变换矩阵
齐次变换矩阵长这样：
| R t |
| 0 1 |用齐次坐标表示点：
P_homogeneous = [x, y, z, 1]^T变换就变成了矩阵乘法：
P_world_h = T * P_sensor_h连续变换时，比如从传感器到机器人再到世界：
T_world_sensor = T_world_robot * T_robot_sensor注意顺序：矩阵乘法不满足交换律。先旋转后平移，和先平移后旋转，结果完全不同。我见过有人把变换顺序搞反了，结果点飞到十万八千里外。嗯，那个人就是我。
2.5.3 逆变换
有时候我们需要反过来，从世界坐标系变换回传感器坐标系。逆变换公式：
T_inv = | R^T -R^T * t |
| 0 1 |说白了，逆旋转就是转置矩阵，逆平移就是旋转后的负平移。
2.6 实战：坐标系变换在Occupancy Grid中的应用
在占用网格中，坐标系变换无处不在。我举个例子：
- 激光雷达扫描到一束激光，返回距离 d 和角度 θ
- 这个点在激光雷达坐标系下的坐标是
[d*cos(θ), d*sin(θ), 0] - 通过外参变换到机器人坐标系
- 再通过机器人位姿变换到世界坐标系
- 最后映射到占用网格的栅格坐标
每一步都离不开坐标变换。如果哪个环节的变换矩阵错了，地图上就会出现鬼影或者空洞。
我曾经踩过的坑：有一次做3D激光雷达建图，发现地图上总是有一层“雾”。排查了三天，最后发现是激光雷达的安装高度在变换矩阵里写错了，差了5厘米。就这5厘米，导致地面点被投影到了空中。所以，标定一定要精确，尤其是外参的平移量。
2.7 总结
坐标系这东西，说白了就是给数据找个家。世界坐标系是老家，机器人坐标系是移动的家，传感器坐标系是每个成员的小家。坐标变换就是搬家工具。
我个人建议，在写代码之前，先画一张坐标系关系图。把每个传感器、机器人本体、世界坐标系之间的变换关系画清楚。这样写代码时思路清晰，不容易出错。
下一章我们会讲如何用代码实现这些变换，以及如何在ROS中使用tf库来管理坐标系。到时候我会分享一些我写过的工具函数，保证实用。
小练习：假设激光雷达装在机器人前方0.3米处，朝上仰角10度。机器人当前在世界坐标系下的位姿是 (x=1.0, y=2.0, θ=30°)。激光雷达检测到一个点距离2米，角度15度。请计算这个点在世界坐标系下的坐标。
答案在下一章揭晓。