第三章：坐标系与变换——世界坐标系、机器人坐标系、传感器坐标系、欧拉角与四元数、TF变换

说到多传感器融合，绕不开的一个话题就是坐标系。我刚开始做SLAM那会儿，觉得坐标系这东西太基础了，不就是几个轴嘛。结果第一次做激光雷达和IMU融合，数据怎么都对不上，折腾了两天才发现——哦，原来IMU的坐标系定义和我想的不一样。从那以后，我再也不敢小看坐标系了。

说白了，多传感器融合的本质，就是把不同传感器看到的东西，统一到同一个坐标系下。你想想看，激光雷达看到的是它自己坐标系下的点云，摄像头看到的是像素坐标，IMU测量的是加速度和角速度。这些数据要融合，就得先搞清楚它们之间的变换关系。

3.1 世界坐标系与机器人坐标系

先聊最基础的。世界坐标系，也叫全局坐标系，是固定不动的参考系。我们通常用W表示。机器人坐标系，也叫本体坐标系，跟着机器人走，用B或R表示。

我习惯把世界坐标系的原点设在机器人启动的位置，x轴指向正北，y轴指向正东，z轴朝天。当然，这不是硬性规定。你完全可以把原点设在房间角落，或者地图的某个特征点上。关键是——一旦定下来，就别再改了。

核心概念：机器人坐标系到世界坐标系的变换，就是机器人的位姿（位置+姿态）。位置用三维向量(x,y,z)表示，姿态用旋转矩阵或四元数表示。

举个例子。机器人在世界坐标系下的位置是(1, 2, 0)，朝向是正北偏东30度。那么激光雷达扫描到前方1米处有个障碍物，这个障碍物在机器人坐标系下的坐标是(1, 0, 0)。要把它转换到世界坐标系，就得做一次坐标变换。

3.2 传感器坐标系

每个传感器都有自己的坐标系。这是最容易出坑的地方。

传感器	坐标系定义	常见问题
激光雷达	x向前，y向左，z向上	不同厂家定义可能不同
摄像头	z向前，x向右，y向下	图像坐标系和相机坐标系容易混淆
IMU	x向前，y向右，z向上	加速度和角速度的符号方向
轮式里程计	通常与机器人坐标系对齐	安装位置偏移会导致误差

我曾经在一个项目里，激光雷达用的是Velodyne，IMU用的是某国产型号。两个传感器的坐标系定义完全相反——激光雷达的y轴向左，IMU的y轴向右。结果融合出来的轨迹，机器人明明在直行，轨迹却画了个弧线。排查了半天才发现是坐标系定义的问题。

避坑指南：拿到一个新传感器，第一件事就是看它的坐标系定义文档。不要想当然。我曾经因为偷懒没看文档，浪费了整整两天时间。

3.3 欧拉角与四元数

姿态描述，说白了就是告诉别人「机器人朝哪个方向」。常用的有两种方式：欧拉角和四元数。

3.3.1 欧拉角

欧拉角用三个角度表示旋转：偏航角(yaw)、俯仰角(pitch)、横滚角(roll)。直观，好理解。但有个致命问题——万向锁。

什么是万向锁？当俯仰角达到±90度时，偏航和横滚会变得无法区分。我刚开始做无人机的时候，就遇到过这个问题。无人机垂直朝上飞行，欧拉角突然就乱了，姿态估计直接崩了。

我的建议：欧拉角适合人机交互，比如显示给操作员看。但做内部计算，尤其是滤波和优化，尽量用四元数。

3.3.2 四元数

四元数用四个数表示旋转：一个实部和三个虚部。没有万向锁问题，计算效率高，插值平滑。缺点是——不直观。你很难一眼看出四元数(0.707, 0, 0.707, 0)代表什么旋转。

四元数的基本形式：

q = w + xi + yj + zk
其中 w² + x² + y² + z² = 1

ROS里用的就是四元数。我记得第一次写代码，要把欧拉角转成四元数，查了半天API。后来发现ROS自带tf::createQuaternionFromYaw()，一行代码搞定。

3.4 TF变换

TF是ROS里管理坐标变换的框架。说白了，就是维护一棵坐标系树，随时告诉你任意两个坐标系之间的变换关系。

TF的核心概念：

TF树：所有坐标系的父子关系，必须是一棵树，不能有环
TF广播器：发布坐标系之间的变换
TF监听器：接收并缓存变换，供其他节点查询

我习惯在launch文件里用static_transform_publisher发布固定变换，比如激光雷达到机器人基座的变换。动态变换，比如机器人位姿，用tf::TransformBroadcaster实时发布。

下面是一个简单的TF发布代码示例：

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>

int main(int argc, char** argv) {
    ros::init(argc, argv, "tf_broadcaster");
    ros::NodeHandle nh;
    tf::TransformBroadcaster br;
    tf::Transform transform;

    ros::Rate rate(10);
    while (ros::ok()) {
        // 设置平移
        transform.setOrigin(tf::Vector3(1.0, 2.0, 0.0));
        // 设置旋转（四元数）
        tf::Quaternion q;
        q.setRPY(0, 0, 1.57); // roll=0, pitch=0, yaw=90度
        transform.setRotation(q);
        // 发布变换：从"base_link"到"odom"
        br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "odom", "base_link"));
        rate.sleep();
    }
    return 0;
}

重要提醒：TF树必须是一棵树，不能有环。如果你不小心发布了循环变换，TF会报错并丢弃你的变换。我曾经在调试时，不小心把parent和child写反了，结果TF树乱了，所有坐标变换都错了。

3.5 知识体系总览

下面这张图展示了本章的核心知识结构。我习惯用这种图来梳理思路，你也可以试试。

3.6 实践中的常见问题

最后，分享几个我在项目中踩过的坑：

坐标系定义不一致：不同传感器厂家对坐标轴的定义可能不同。拿到传感器后，先用简单实验验证一下。比如把激光雷达转90度，看看点云是不是也跟着转。
TF树时间戳问题：TF变换带时间戳。如果你查询变换时用的时间戳和实际时间差太多，TF会报错。我习惯用ros::Time(0)获取最新变换。
四元数归一化：四元数必须归一化，否则旋转会变形。ROS的tf库会自动处理，但如果你自己写代码，记得做归一化。
欧拉角转四元数顺序：不同的旋转顺序（比如XYZ还是ZYX）结果完全不同。ROS里默认是ZYX顺序，也就是先偏航、再俯仰、最后横滚。

一个小技巧：调试坐标系变换时，可以用rviz的TF显示功能。把各个坐标系显示出来，看看它们之间的相对位置和朝向对不对。我每次做新传感器集成，都会先用rviz验证一遍。

嗯，坐标系与变换就讲到这里。内容不多，但很重要。你想想看，如果坐标系搞错了，后面所有的融合、建图、避障都是白搭。所以，花点时间把这块吃透，值得。

📚 课程目录

第三章：坐标系与变换——世界坐标系、机器人坐标系、传感器坐标系、欧拉角与四元数、TF变换

3.1 世界坐标系与机器人坐标系

3.2 传感器坐标系

3.3 欧拉角与四元数

3.3.1 欧拉角

3.3.2 四元数

3.4 TF变换

3.5 知识体系总览

3.6 实践中的常见问题