第2章坐标系与变换

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

各位同学，欢迎来到第二章。

说实话，坐标系与变换这个话题，是AMR定位与建图的基石。你想想看，机器人要在一个未知环境里知道自己“在哪儿”，又要知道“周围有什么”，这背后全靠坐标系在撑腰。我当年刚入行时，就因为搞混了传感器坐标系和机器人坐标系，导致一整个下午的建图数据全部作废。嗯，从那以后，我再也不敢小看这个“基础”了。

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

先聊世界坐标系。说白了，它就是整个地图的“绝对参考系”。我们通常用 map 或 odom 来表示。世界坐标系的原点一般设在机器人启动的位置，或者地图的某个固定点。它的X轴指向东，Y轴指向北，Z轴指向天——当然，这只是惯例，实际项目中你可以自己定。

机器人坐标系呢？它跟着机器人走。我们叫它 base_link 或 base_footprint。机器人坐标系的原点通常在机器人的几何中心，或者轮子着地平面的中心。它的X轴指向机器人前方，Y轴指向左侧，Z轴指向上方。

我个人习惯把世界坐标系想象成“大地图上的经纬度”，而机器人坐标系就是“你手机上的指南针”。机器人每动一下，它的坐标系就跟着变。而我们要做的，就是通过变换，把传感器看到的东西，从传感器自己的坐标系，转换到机器人坐标系，再转换到世界坐标系。

  核心要点： 世界坐标系是静态的，机器人坐标系是动态的。所有感知数据最终都要统一到世界坐标系下，才能进行建图和定位。

2.2 传感器坐标系

传感器坐标系，这个坑我踩过。激光雷达、摄像头、IMU、轮式里程计……每个传感器都有自己的坐标系。比如激光雷达，它的坐标系原点通常在激光头的旋转中心，X轴指向正前方，Y轴指向左侧，Z轴指向上方。摄像头呢？它的坐标系原点在光心，Z轴指向镜头前方，X轴指向右侧，Y轴指向下方——嗯，和激光雷达的朝向不一样。

我曾经在一个项目中，把激光雷达的坐标系和摄像头的坐标系搞反了，结果融合出来的点云和图像完全对不上。排查了整整两天，最后发现是标定文件里一个符号写错了。所以，我建议你在写代码之前，先把每个传感器的坐标系画出来，贴在工位上。

传感器类型	坐标系名称	原点位置	X轴方向	Y轴方向	Z轴方向
激光雷达	laser_frame	激光头旋转中心	正前方	左侧	上方
摄像头	camera_frame	光心	右侧	下方	正前方
IMU	imu_frame	IMU芯片中心	正前方	左侧	上方
轮式里程计	odom	机器人启动位置	正前方	左侧	上方

小技巧： 在ROS2中，每个传感器发布数据时，都会带上自己的坐标系名称。你可以在 rviz2 里把坐标系显示出来，直观地检查它们之间的相对位置。

2.3 欧拉角与四元数

聊完坐标系，我们得说说怎么描述一个坐标系相对于另一个坐标系的旋转。这里有两个主流方法：欧拉角和四元数。

欧拉角，说白了就是用三个角度来描述旋转：绕X轴的翻滚角（roll）、绕Y轴的俯仰角（pitch）、绕Z轴的偏航角（yaw）。听起来很直观，对吧？但有个大坑——万向锁。当pitch接近±90度时，roll和yaw会变得无法区分。我在做无人机项目时就遇到过，飞控在俯仰角过大时，姿态解算直接崩了。

四元数，它用四个数来描述旋转：一个实部和三个虚部。公式是 q = w + xi + yj + zk，其中 w 是实部，x, y, z 是虚部。四元数没有万向锁问题，而且插值平滑，所以ROS2里默认用四元数。

你可能会问：“那我怎么把欧拉角转成四元数？”别急，ROS2提供了现成的工具：

# Python示例：欧拉角转四元数
from tf_transformations import quaternion_from_euler
import math

roll = 0.0
pitch = 0.0
yaw = math.pi / 2  # 90度

q = quaternion_from_euler(roll, pitch, yaw)
print(q)  # 输出：[0.0, 0.0, 0.707, 0.707]

注意： 欧拉角的旋转顺序很重要！ROS2默认使用ZYX顺序（先绕Z轴，再绕Y轴，最后绕X轴）。如果你用其他顺序，结果会完全不同。

2.4 TF树基础

好了，坐标系和旋转都讲完了，怎么把它们串起来？答案就是TF树。

TF树，全称Transform Tree，是ROS2里管理坐标系之间变换关系的核心机制。它是一棵树，根节点通常是 map 或 odom，叶子节点是各个传感器坐标系。每个节点到父节点的变换，就是一个平移加旋转。

我举个例子：你的机器人有一个激光雷达，安装在机器人前方0.5米处，高度0.2米，朝前。那么从 base_link 到 laser_frame 的变换就是：平移 (0.5, 0.0, 0.2)，旋转 (0, 0, 0)。

在ROS2中，你可以用 tf2_ros 库来发布和监听TF变换：

# Python示例：发布静态TF变换
import rclpy
from rclpy.node import Node
from tf2_ros import StaticTransformBroadcaster
from geometry_msgs.msg import TransformStamped

class StaticTFPublisher(Node):
    def __init__(self):
        super().__init__('static_tf_publisher')
        self.broadcaster = StaticTransformBroadcaster(self)
        
        t = TransformStamped()
        t.header.stamp = self.get_clock().now().to_msg()
        t.header.frame_id = 'base_link'
        t.child_frame_id = 'laser_frame'
        t.transform.translation.x = 0.5
        t.transform.translation.y = 0.0
        t.transform.translation.z = 0.2
        t.transform.rotation.x = 0.0
        t.transform.rotation.y = 0.0
        t.transform.rotation.z = 0.0
        t.transform.rotation.w = 1.0
        
        self.broadcaster.sendTransform(t)

def main():
    rclpy.init()
    node = StaticTFPublisher()
    rclpy.spin(node)

TF树的好处是，你不需要手动计算坐标变换。你只需要告诉ROS2“激光雷达相对于机器人坐标系的位置”，然后ROS2会自动帮你把激光雷达的数据转换到世界坐标系下。这就是TF树的魅力。

  避坑指南： 我曾经在调试时发现TF树断掉了——某个传感器没有发布变换，导致整个定位系统崩溃。所以，我建议你在启动机器人后，先用 tf2_tools 里的 view_frames 命令查看TF树是否完整。

2.5 知识体系总览

为了让你更直观地理解本章的知识结构，我画了一张图：

这张图把本章的核心逻辑串起来了：世界坐标系是根，机器人坐标系是干，传感器坐标系是叶。欧拉角和四元数是描述旋转的工具，TF树是管理变换的框架。你只要理解了这张图，后面的建图和定位就顺了。

好了，这一章就到这里。坐标系与变换是AMR的“语言”，你越早掌握它，后面就越轻松。下一章我们聊传感器数据处理，到时候你会感谢今天打下的基础。

公众号:蓝海资料掘金营，微信deep3321

📚 课程目录

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

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

2.2 传感器坐标系

2.3 欧拉角与四元数

2.4 TF树基础

2.5 知识体系总览