第1章：坐标系与坐标变换

各位同学，欢迎来到《多传感器融合路径规划实战指南》。我是你们的老朋友，一个在机器人行业摸爬滚打了十来年的工程师。今天咱们开篇就聊一个最基础、也最绕不开的话题——坐标系与坐标变换。

说实话，我见过太多新手，算法写得飞起，最后却栽在坐标变换上。车子明明该往左转，结果原地打转。为什么？坐标系搞反了。所以这一章，咱们把地基打牢。

1.1 常用坐标系：你总得知道自己在哪

做多传感器融合，你脑子里得时刻装着几个坐标系。我习惯把它们分成四类：

• 世界坐标系（World Frame）：也叫全局坐标系。它是整个场景的绝对参考。比如你建图时，地图的原点就在这。一般用 W 表示。
• 车体坐标系（Body Frame）：固定在机器人身上的坐标系。原点通常在车辆中心，X轴朝前，Y轴朝左，Z轴朝上。用 B 表示。
• 传感器坐标系（Sensor Frame）：每个传感器都有自己的小世界。比如激光雷达的坐标系，原点在雷达中心。用 S 表示。
• 相机坐标系（Camera Frame）：视觉传感器的专属坐标系。注意，相机坐标系通常 Z 轴朝前，X 轴朝右，Y 轴朝下。这个方向跟车体坐标系不一样，容易搞混。

1.2 坐标变换的数学表达：三种武器

坐标系之间怎么转换？说白了，就是旋转加平移。旋转的表达方式有三种，我一个个说。

1.2.1 欧拉角

欧拉角最直观。绕 X 轴转叫 Roll（横滚），绕 Y 轴转叫 Pitch（俯仰），绕 Z 轴转叫 Yaw（偏航）。你想想看，飞机飞行员就靠这三个角判断姿态。

但欧拉角有个致命问题——万向锁。当 Pitch 接近 90 度时，Roll 和 Yaw 会变得无法区分。我在做无人机飞控时遇到过，飞机突然失控，就是因为姿态解算碰到了万向锁。所以，千万别在插值或滤波里直接用欧拉角。

1.2.2 旋转矩阵

旋转矩阵是个 3x3 的矩阵，数学上很完美。没有奇异性，可以任意组合。但缺点也很明显：9 个参数，冗余，而且不直观。

我个人习惯，在代码里用旋转矩阵做中间计算，但调试时还是看欧拉角。毕竟人脑处理矩阵不如处理角度来得快。

1.2.3 四元数

四元数是我最推荐的表达方式。它用四个数（w, x, y, z）表示旋转，没有万向锁，插值平滑，计算效率高。

你可能会问：为什么是四个数？其实可以理解为一个标量加一个三维向量。标量 w 代表旋转角度的一半的余弦，向量 (x,y,z) 代表旋转轴的方向。

1.3 使用 TF2 进行坐标变换管理

好了，理论讲完，咱们来点实战。ROS2 里管理坐标变换的利器就是 TF2。它本质上是一个发布-订阅系统，专门用来维护坐标系之间的变换关系。

1.3.1 TF2 的核心概念

• Transform Broadcaster：发布坐标变换的节点。比如你的里程计节点，每隔一段时间就发布一次 "odom" 到 "base_link" 的变换。
• Transform Listener：监听并缓存所有变换的节点。你可以在任何地方查询任意两个坐标系之间的变换。
• TF Tree：所有坐标系构成的树状结构。注意，必须是树，不能有环。

1.3.2 实战：发布一个静态变换

假设你的激光雷达安装在车体前方 0.5 米处。你需要发布 "base_link" 到 "laser_frame" 的静态变换。

# 使用命令行发布静态变换
ros2 run tf2_ros static_transform_publisher \
  --x 0.5 --y 0.0 --z 0.0 \
  --roll 0.0 --pitch 0.0 --yaw 0.0 \
  --frame-id base_link \
  --child-frame-id laser_frame

这段代码的意思很简单：激光雷达坐标系原点，相对于车体坐标系，在 X 方向偏移了 0.5 米，没有旋转。

1.3.3 实战：在代码中监听变换

更常见的情况是，你需要在代码里实时获取变换。比如，你要把激光雷达的点云转换到车体坐标系下。

import rclpy
from rclpy.node import Node
from tf2_ros import TransformException
from tf2_ros.buffer import Buffer
from tf2_ros.transform_listener import TransformListener

class TransformDemo(Node):
    def __init__(self):
        super().__init__('transform_demo')
        self.tf_buffer = Buffer()
        self.tf_listener = TransformListener(self.tf_buffer, self)
        
        # 定时查询变换
        self.timer = self.create_timer(0.1, self.query_transform)
    
    def query_transform(self):
        try:
            # 查询 laser_frame 到 base_link 的变换
            trans = self.tf_buffer.lookup_transform(
                'base_link',      # 目标坐标系
                'laser_frame',    # 源坐标系
                rclpy.time.Time() # 当前时间
            )
            self.get_logger().info(
                f'Translation: {trans.transform.translation}'
            )
        except TransformException as ex:
            self.get_logger().warn(f'Could not get transform: {ex}')

1.4 坐标系变换的常见坑

最后，我总结几个我踩过的坑，希望能帮你省点时间。

1. 时间戳对齐：TF2 的变换是有时间戳的。如果你查询的是过去某个时刻的变换，但缓存里没有，就会报错。我建议用 lookup_transform 时，时间参数传 rclpy.time.Time() 表示最新变换。
2. 坐标系命名规范：ROS 社区有约定俗成的命名，比如 base_link、odom、map。别自己瞎起名字，否则别人看不懂你的代码。
3. 静态变换 vs 动态变换：传感器相对于车体的位置是固定的，用静态变换。车体相对于世界的位置是变化的，用动态变换。别搞混了。

好了，这一章的内容就到这里。坐标系和坐标变换是后续所有章节的基础。你想想看，如果你连自己在哪里、传感器朝哪都不知道，还谈什么路径规划？下一章，咱们聊聊传感器数据的时间同步，这也是个容易翻车的地方。

记住：坐标系搞对了，你的机器人才能走对路。