3. 坐标系与坐标变换：机器人常用坐标系与TF原理
说实话，坐标系和坐标变换是SLAM里最基础也最容易搞混的东西。我见过不少新手，代码写得挺溜，但一问他“激光雷达的数据是在哪个坐标系下发布的”，立马就懵了。嗯，今天咱们就把这事儿彻底理清楚。
3.1 机器人常用的三大坐标系
做激光SLAM，你天天都会跟这三个坐标系打交道：世界坐标系、机器人坐标系、激光雷达坐标系。它们之间的关系，说白了就是“我在哪”、“我身上哪个部件在哪”。
| 坐标系 | 常用名称 | 固定点 | 典型用途 |
|---|---|---|---|
| 世界坐标系 | map / odom | 地图原点 | 全局定位、建图 |
| 机器人坐标系 | base_link / base_footprint | 机器人中心 | 运动控制、里程计 |
| 激光雷达坐标系 | laser / lidar_link | 激光雷达安装点 | 激光数据采集 |
世界坐标系是全局的参考系。你建的地图、规划的路径，都是在这个坐标系下表达的。我习惯把map坐标系的原点设在机器人启动的位置，这样调试时比较直观。
机器人坐标系跟着机器人走。它的原点通常在机器人的几何中心，或者轮子着地点的投影。你想想看，如果机器人转了个身，base_link也跟着转，但map不动。
激光雷达坐标系就更有意思了。激光雷达装在机器人上，它扫描到的点，一开始都是在激光雷达自己的坐标系下表达的。这些数据要变成地图上的点，就得先转到机器人坐标系，再转到世界坐标系。
核心理解：激光雷达看到的是“相对于我自己的障碍物位置”，而我们要的是“障碍物在世界地图上的位置”。中间的桥梁就是坐标变换。
3.2 TF坐标变换原理
TF（Transform）是ROS里管理坐标变换的框架。它的核心思想很简单：维护一棵坐标系之间的变换树。
每个坐标系相对于父坐标系都有一个变换关系，包含平移（x, y, z）和旋转（roll, pitch, yaw）。你只要知道任意两个坐标系在树上的路径，就能算出它们之间的变换。
举个例子。激光雷达装在机器人前方0.3米处，那么laser相对于base_link的变换就是 (0.3, 0, 0)。机器人当前在世界坐标系中的位置是 (1.0, 2.0, 0.5)，那么激光雷达在世界坐标系中的位置就是 (1.3, 2.0, 0.5)。
我的经验：TF树一定要画出来看。用 rqt_tf_tree 或者 view_frames 工具，一眼就能看出哪个坐标系没连上。我曾经花了一整天排查一个定位漂移的问题，最后发现是TF树里少了一个静态变换。
3.3 使用tf2进行坐标变换
ROS里现在推荐用 tf2，它是tf的升级版。API更简洁，性能也更好。我个人建议新项目直接上tf2，别再用老tf了。
3.3.1 发布变换
发布变换有两种方式：静态变换和动态变换。先看一个发布动态变换的例子：
// C++ 示例：发布机器人位置到odom的变换
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>
tf2_ros::TransformBroadcaster broadcaster;
geometry_msgs::TransformStamped transformStamped;
transformStamped.header.stamp = ros::Time::now();
transformStamped.header.frame_id = "odom";
transformStamped.child_frame_id = "base_link";
transformStamped.transform.translation.x = 1.0;
transformStamped.transform.translation.y = 2.0;
transformStamped.transform.translation.z = 0.0;
transformStamped.transform.rotation.x = 0.0;
transformStamped.transform.rotation.y = 0.0;
transformStamped.transform.rotation.z = 0.0;
transformStamped.transform.rotation.w = 1.0;
broadcaster.sendTransform(transformStamped);这段代码每执行一次，就告诉TF系统：当前时刻，base_link相对于odom的位置是 (1.0, 2.0, 0.0)，朝向是正方向。你想想看，如果机器人移动了，你得不断更新这个变换。
3.3.2 监听变换
监听变换更常用。比如你要把激光雷达的数据转换到map坐标系下：
// C++ 示例：监听变换并转换点
#include <tf2_ros/transform_listener.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
tf2_ros::Buffer tfBuffer;
tf2_ros::TransformListener listener(tfBuffer);
geometry_msgs::PointStamped laser_point;
laser_point.header.frame_id = "laser";
laser_point.header.stamp = ros::Time::now();
laser_point.point.x = 1.0;
laser_point.point.y = 0.5;
laser_point.point.z = 0.0;
geometry_msgs::PointStamped map_point;
try {
tfBuffer.transform(laser_point, map_point, "map");
ROS_INFO("Map point: (%.2f, %.2f, %.2f)",
map_point.point.x, map_point.point.y, map_point.point.z);
} catch (tf2::TransformException &ex) {
ROS_WARN("Transform failed: %s", ex.what());
}注意：transform() 函数会阻塞等待变换可用。如果TF树还没建立好，它会抛出异常。一定要加try-catch，否则程序会崩溃。我曾经在调试时忘了加，结果程序启动就挂，查了半天才发现是TF还没发布。
3.4 静态变换与动态变换
这两种变换的区别，说白了就是：变还是不变。
| 类型 | 变换关系 | 发布频率 | 典型场景 |
|---|---|---|---|
| 静态变换 | 固定不变 | 一次发布，或低频发布 | 激光雷达相对于机器人、相机相对于机器人 |
| 动态变换 | 随时间变化 | 高频发布（10-100Hz） | 机器人位置、里程计、关节角度 |
静态变换用 tf2_ros::StaticTransformBroadcaster 发布。它只需要发布一次，TF系统会记住这个变换。比如激光雷达装在机器人前方0.3米处，这个关系不会变，用静态变换最合适。
// 静态变换示例：激光雷达相对于机器人
geometry_msgs::TransformStamped static_transform;
static_transform.header.stamp = ros::Time::now();
static_transform.header.frame_id = "base_link";
static_transform.child_frame_id = "laser";
static_transform.transform.translation.x = 0.3;
static_transform.transform.translation.y = 0.0;
static_transform.transform.translation.z = 0.1;
static_transform.transform.rotation.w = 1.0;
static_broadcaster.sendTransform(static_transform);动态变换则用普通的 TransformBroadcaster，每帧都发。机器人位置、里程计数据都属于动态变换。我建议动态变换的发布频率至少是机器人控制频率的两倍，否则坐标会滞后。
避坑指南：我曾经在项目里把激光雷达的静态变换写成了动态变换，结果每帧都发同样的数据，浪费CPU不说，还导致TF树里出现大量冗余信息。后来用 tf2_monitor 工具一看，发现变换延迟高得离谱。改成静态变换后，问题立刻解决。
3.5 坐标变换的常见坑
- 时间戳不一致：发布变换时，时间戳要用
ros::Time::now()，不要用旧时间。否则TF系统会认为数据过期。 - 坐标系命名混乱：我建议统一用小写加下划线，比如
base_link、laser_link。别用大写或空格，否则容易出错。 - 忘记发布父坐标系：如果父坐标系没有发布，子坐标系的变换就无法计算。检查TF树时，确保每个节点都有父节点。
- 旋转表示搞错：ROS里用四元数表示旋转，不是欧拉角。如果你用欧拉角，记得用
tf2::Quaternion转换一下。
嗯，坐标系和坐标变换就讲这么多。记住一句话：所有传感器数据，最终都要统一到同一个坐标系下。理解了这一点，后面的建图和导航就好办了。