坐标变换与TF树：机器人坐标系定义

做机器人导航，说白了就是搞清楚三个问题：我在哪？我要去哪？我怎么去？而这三个问题的核心，都绕不开坐标系。我刚开始接触机器人时，总觉得坐标变换是个纯理论的东西，直到有一次在实验室里，激光雷达的数据和里程计数据怎么都对不上，折腾了两天才发现是TF树配错了。嗯，从那以后我再也不敢小看坐标系了。

机器人三大核心坐标系

在ROS的导航框架里，我们主要跟三个坐标系打交道：base_link、odom、map。它们各自扮演什么角色？我画了张图，你看一眼就明白了。

1. base_link：机器人的"身体"

base_link 是固定在机器人本体上的坐标系。它的原点通常在机器人底盘的几何中心，X轴指向正前方，Y轴指向左侧，Z轴指向上方。说白了，这就是机器人的"身体坐标系"——激光雷达装在车头还是车尾，IMU装在哪个位置，都是以 base_link 为参考的。

2. odom：里程计的"世界"

odom 坐标系是里程计数据的世界坐标系。它的原点通常是机器人启动时的位置。里程计通过轮子编码器、IMU或者视觉数据，不断推算机器人相对于起点的位移。但有个问题——里程计有累积误差，跑得越久，偏差越大。

我记得有一次做室内巡检项目，机器人沿着走廊走了200米，里程计显示走了198米，实际只有195米。3米的误差，在狭窄的走廊里足以让机器人撞墙。这就是为什么我们需要第三个坐标系。

3. map：全局的"上帝视角"

map 坐标系是全局固定的，它的原点通常和建图时的起点对齐。定位模块（比如AMCL）会不断修正 map 到 odom 的变换，把里程计的累积误差"拉回来"。你想想看，如果没有 map 坐标系，机器人跑远了就不知道自己到底在哪了。

坐标系	发布者	特点	误差
map	定位模块（AMCL等）	全局固定，长期稳定	无累积误差
odom	里程计（轮式/视觉/激光）	短时精度高，连续平滑	有累积误差
base_link	机器人底盘驱动	跟随机器人运动	实时更新

TF树：坐标系的"家族谱"

TF（Transform）树，说白了就是记录所有坐标系之间变换关系的"族谱"。每个坐标系都是一个节点，父子关系通过变换连接。ROS里的 tf2 包负责维护这棵树，你只需要广播变换，系统会自动帮你管理。

TF广播：告诉世界"我在哪"

广播TF变换，就是告诉其他节点：坐标系A相对于坐标系B的位置和姿态。比如，激光雷达装在机器人前方0.3米处，你就要广播 laser_frame 相对于 base_link 的变换。

// C++ 示例：广播 base_link 到 laser_frame 的变换
#include <ros/ros.h>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>

int main(int argc, char** argv) {
    ros::init(argc, argv, "laser_tf_broadcaster");
    ros::NodeHandle nh;
    tf2_ros::TransformBroadcaster broadcaster;
    
    geometry_msgs::TransformStamped transformStamped;
    transformStamped.header.stamp = ros::Time::now();
    transformStamped.header.frame_id = "base_link";
    transformStamped.child_frame_id = "laser_frame";
    transformStamped.transform.translation.x = 0.3;  // 前方0.3米
    transformStamped.transform.translation.y = 0.0;
    transformStamped.transform.translation.z = 0.1;  // 离地0.1米
    transformStamped.transform.rotation.x = 0.0;
    transformStamped.transform.rotation.y = 0.0;
    transformStamped.transform.rotation.z = 0.0;
    transformStamped.transform.rotation.w = 1.0;
    
    ros::Rate rate(10.0);
    while (ros::ok()) {
        transformStamped.header.stamp = ros::Time::now();
        broadcaster.sendTransform(transformStamped);
        rate.sleep();
    }
    return 0;
}

TF监听：查询任意两个坐标系的关系

监听TF变换，就是问系统：现在坐标系A相对于坐标系B的变换是多少？比如，激光雷达的数据是在 laser_frame 坐标系下的，但路径规划需要 base_link 坐标系下的数据，这时候就需要TF监听来转换。

// C++ 示例：监听并转换激光雷达数据到 base_link
#include <ros/ros.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <sensor_msgs/LaserScan.h>

class LaserTransformer {
public:
    LaserTransformer() : tf_listener(tf_buffer) {
        sub = nh.subscribe("/scan", 1, &LaserTransformer::scanCallback, this);
        pub = nh.advertise<sensor_msgs::LaserScan>("/scan_in_base_link", 1);
    }
    
    void scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan) {
        sensor_msgs::LaserScan scan_transformed = *scan;
        scan_transformed.header.frame_id = "base_link";
        
        try {
            // 将激光雷达数据从 laser_frame 转换到 base_link
            tf_buffer.transform(scan_transformed, scan_transformed, "base_link", ros::Duration(0.1));
            pub.publish(scan_transformed);
        } catch (tf2::TransformException& ex) {
            ROS_WARN("TF变换失败: %s", ex.what());
        }
    }
    
private:
    ros::NodeHandle nh;
    tf2_ros::Buffer tf_buffer;
    tf2_ros::TransformListener tf_listener;
    ros::Subscriber sub;
    ros::Publisher pub;
};

坐标变换实战：从数据到决策

理论说完了，咱们来点实际的。假设你有一个激光雷达，装在机器人前方0.3米处，现在要做一个简单的避障：如果激光雷达检测到前方0.5米内有障碍物，机器人就停下来。

这里的关键是：激光雷达检测到的障碍物坐标是在 laser_frame 下的，但机器人的运动控制是在 base_link 下的。你需要把障碍物坐标从 laser_frame 转换到 base_link，才能判断它是不是在机器人前方。

// 坐标变换实战：激光雷达避障
#include <ros/ros.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <sensor_msgs/LaserScan.h>
#include <geometry_msgs/Twist.h>

class ObstacleAvoidance {
public:
    ObstacleAvoidance() : tf_listener(tf_buffer) {
        sub = nh.subscribe("/scan", 1, &ObstacleAvoidance::scanCallback, this);
        pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
    }
    
    void scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan) {
        // 取正前方的激光点（假设索引在中间）
        int mid_index = scan->ranges.size() / 2;
        float range = scan->ranges[mid_index];
        
        if (range > scan->range_min && range < 0.5) {
            // 障碍物在0.5米内，停车
            geometry_msgs::Twist stop;
            stop.linear.x = 0.0;
            stop.angular.z = 0.0;
            pub.publish(stop);
            ROS_INFO("障碍物距离: %.2f米，停车！", range);
        }
    }
    
private:
    ros::NodeHandle nh;
    tf2_ros::Buffer tf_buffer;
    tf2_ros::TransformListener tf_listener;
    ros::Subscriber sub;
    ros::Publisher pub;
};

TF树调试：用工具说话

写代码容易，调TF树难。我推荐几个实用工具：

• tf2_monitor：查看TF树中所有变换的延迟和频率，哪个节点慢了一目了然。
• tf2_echo：实时打印两个坐标系之间的变换，比如 rosrun tf2_ros tf2_echo base_link laser_frame。
• view_frames：生成TF树的PDF图，方便检查父子关系是否正确。
• rviz：在可视化界面中查看TF树，最直观的方式。

我记得有一次，机器人定位总是跳变，用 tf2_monitor 一看，发现 odom 到 base_link 的变换延迟高达200ms，原来是里程计线程被阻塞了。调优之后，延迟降到10ms以内，定位就稳了。