4、坐标系与坐标变换：自动驾驶常用坐标系

做自动驾驶仿真，说白了就是在跟坐标系打交道。我记得刚入行那会儿，被各种坐标系绕得晕头转向——世界坐标、车身坐标、传感器坐标，再加上旋转平移，一个不小心数据就对不上了。今天咱们就把这块硬骨头啃下来。

4.1 自动驾驶常用坐标系

自动驾驶里常用的坐标系，其实就三种。我习惯把它们理解成「上帝视角」、「自我视角」和「眼睛视角」。

4.1.1 世界坐标系

世界坐标系是固定不动的。它描述的是「车在地图上的哪里」。通常用 X 指向东，Y 指向北，Z 指向天。嗯，这就是我们常说的 ENU 坐标系。

4.1.2 车身坐标系

车身坐标系跟着车走。它的原点在车辆后轴中心（或者质心），X 朝前，Y 朝左，Z 朝上。我在项目中遇到过一个问题——不同车型的车身坐标系原点定义不一样，有的用后轴中心，有的用几何中心。这要是没对齐，控制指令全偏了。

4.1.3 传感器坐标系

每个传感器都有自己的坐标系。比如激光雷达，X 朝前，Y 朝左，Z 朝上。摄像头则复杂一些，它的 Z 轴指向镜头前方。你想想看，一个车上有七八个传感器，每个都有自己的坐标系，不搞坐标变换根本没法玩。

坐标系	原点	X轴	Y轴	Z轴
世界坐标系	固定点	东	北	天
车身坐标系	后轴中心	前	左	上
激光雷达坐标系	雷达中心	前	左	上
摄像头坐标系	光心	右	下	前

4.2 欧拉角与四元数

坐标系之间怎么转？这就涉及到旋转表示了。常用的有两种：欧拉角和四元数。

4.2.1 欧拉角

欧拉角用三个角度描述旋转：偏航角（Yaw）、俯仰角（Pitch）、横滚角（Roll）。说白了就是「先转脑袋、再点头、再歪头」。

但欧拉角有个大坑——万向锁。我曾经在调试一个路径跟踪算法时，车辆姿态突然跳变，查了半天发现是欧拉角在俯仰角接近90度时出了问题。嗯，从那以后我尽量用四元数。

4.2.2 四元数

四元数用四个数表示旋转：q = [w, x, y, z]。它没有万向锁问题，插值也平滑。我个人的习惯是：所有内部计算都用四元数，只在可视化或人机交互时转成欧拉角。

四元数转欧拉角的公式长这样：

// 四元数转欧拉角
double roll = atan2(2*(w*x + y*z), 1 - 2*(x*x + y*y));
double pitch = asin(2*(w*y - z*x));
double yaw = atan2(2*(w*z + x*y), 1 - 2*(y*y + z*z));

4.3 TF2坐标变换库实战

ROS2里做坐标变换，用的就是 TF2 库。它帮你维护了一棵坐标变换树，你只需要发布变换关系，剩下的查询、计算它全包了。

4.3.1 发布静态变换

传感器和车身的相对位置是固定的，用静态变换发布器：

# 发布激光雷达到车身的静态变换
ros2 run tf2_ros static_transform_publisher \
  --x 0.0 --y 0.0 --z 0.5 \
  --roll 0.0 --pitch 0.0 --yaw 0.0 \
  --frame-id base_link \
  --child-frame-id laser_link

4.3.2 监听变换

在代码里监听变换，我一般这么写：

# Python示例
import rclpy
from tf2_ros import TransformListener, Buffer

def get_transform():
    tf_buffer = Buffer()
    tf_listener = TransformListener(tf_buffer, node)
    
    # 等待变换可用
    transform = tf_buffer.lookup_transform(
        'base_link',      # 目标坐标系
        'laser_link',     # 源坐标系
        rclpy.time.Time()
    )
    return transform

4.4 使用rviz2可视化坐标变换

光看数字不够直观，咱们用 rviz2 把坐标系画出来。

4.4.1 启动rviz2

ros2 run rviz2 rviz2

在 rviz2 里添加 TF 显示插件，你就能看到各个坐标系的小箭头了。红色是 X 轴，绿色是 Y 轴，蓝色是 Z 轴。

4.4.2 验证变换是否正确

我有个习惯——每次写完坐标变换代码，先在 rviz2 里看一眼。如果激光雷达的坐标系和车身坐标系对不上，那肯定是变换参数写错了。你想想看，要是传感器数据投影到错误的位置，整个感知系统就废了。

知识体系总览

下面这张图总结了本章的核心内容，你可以把它当作坐标系与坐标变换的「地图」：

坐标系与坐标变换是自动驾驶仿真的基石。我个人觉得，花时间把这块搞透，后面做感知融合、路径规划都会顺手很多。记住一句话：数据对齐了，问题就解决了一半。

可可精品资料库