坐标系与坐标变换：自动驾驶的“空间语言”

做自动驾驶调试，说白了就是在跟坐标系打交道。我记得刚入行那会儿，看着 rviz 里满屏的坐标轴，心里直犯嘀咕——这玩意儿到底怎么转的？后来踩了不少坑才明白，坐标系搞不清楚，轨迹调试就是空中楼阁。

今天咱们就把这块硬骨头啃下来。我会从最常用的三种坐标系讲起，再带你理解 TF 树的调试方法，最后聊聊 tf2 和 Eigen 这两个库怎么用。嗯，保证都是实战干货。

三种核心坐标系：你总得知道自己在哪儿

自动驾驶里坐标系很多，但最核心的就三个。我习惯把它们比作“世界观”、“自我观”和“地图观”。

坐标系	原点	典型用途	我踩过的坑
世界坐标系	地球上的固定点（如 UTM 原点）	全局路径规划、高精地图	不同地图源的原点偏差
机器人坐标系	车辆质心或后轴中心	控制指令、传感器数据	IMU 安装偏移忘补偿
地图坐标系	地图文件定义的参考点	定位匹配、语义地图	经纬度转笛卡尔时的投影变形

TF 树：坐标系关系的“族谱”

TF（Transform）树是 ROS 生态里管理坐标系关系的核心机制。说白了，它就是一个树状结构，记录了所有坐标系之间的相对位姿。

我曾经遇到一个 case：车辆定位突然跳变，查了两天发现是 TF 树里多了一个冗余的父节点，导致坐标变换链出现了歧义。从那以后，我每次调试必先 tf2_monitor 看一眼树结构。

TF 树调试三板斧

1. 看频率：用 tf2_monitor 检查每个变换的发布频率是否稳定。低于 10Hz 的变换链，轨迹控制基本没戏。
2. 看延迟：tf2_echo 可以看两个坐标系之间的延迟。延迟超过 100ms，说明某个节点在“偷懒”。
3. 看连续性：用 rviz 的 TF 显示插件，观察坐标轴是否平滑运动。突然跳变，八成是定位丢了。

坐标变换的数学本质：旋转 + 平移

任何坐标变换，拆开来看就两件事：先转个角度，再挪个位置。用齐次矩阵表示就是：

// 齐次变换矩阵 T = [R t; 0 1]
// 其中 R 是 3x3 旋转矩阵，t 是 3x1 平移向量
// 点 P 从坐标系 A 变换到 B：P_b = T_ab * P_a

我个人习惯用四元数表示旋转，因为避免了欧拉角的万向锁问题。你想想看，如果车辆在坡道上大角度转弯，用欧拉角算出来的俯仰和横滚可能直接乱掉。

tf2 库实战：ROS 里的坐标变换利器

tf2 是 ROS 官方推荐的坐标变换库。相比老版 tf，它更轻量、支持更灵活的数据类型。

常用 API 速查

功能	代码示例	说明
等待变换	tf_buffer->canTransform(target, source, time, timeout)	阻塞等待，直到变换可用
查询变换	tf_buffer->lookupTransform(target, source, time)	获取两个坐标系间的变换
变换点云	tf2::doTransform(input_cloud, output_cloud, transform)	直接对点云应用变换

Eigen 库：数学运算的“瑞士军刀”

Eigen 是 C++ 里做线性代数运算的首选库。在轨迹调试中，我主要用它做三件事：

• 矩阵运算：构建齐次变换矩阵、求逆、插值
• 四元数操作：旋转合成、角速度积分
• 最小二乘：用于标定传感器外参

#include <Eigen/Dense>
#include <Eigen/Geometry>

// 构建旋转矩阵（绕 Z 轴转 30 度）
Eigen::AngleAxisd rot_z(30.0 * M_PI / 180.0, Eigen::Vector3d::UnitZ());
Eigen::Matrix3d R = rot_z.toRotationMatrix();

// 构建平移向量
Eigen::Vector3d t(1.0, 2.0, 0.5);

// 构建齐次变换矩阵
Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
T.block<3,3>(0,0) = R;
T.block<3,1>(0,3) = t;

// 变换一个点
Eigen::Vector4d point(0.5, 0.3, 0.0, 1.0);
Eigen::Vector4d transformed = T * point;

知识体系总览

下面这张图是我自己总结的坐标系与坐标变换知识框架，每次带新人我都会先让他们看这个：

实战中的常见问题与解决思路

说了这么多理论，咱们来点实际的。我在项目里遇到过几个典型问题，分享出来大家少走弯路：

• 问题一：定位跳变——检查 TF 树是否有多个父节点，或者某个变换的协方差矩阵异常大。
• 问题二：控制指令反向——大概率是机器人坐标系的前向定义和控制器不一致。我习惯统一用 REP-105 标准。
• 问题三：点云投影错位——先确认传感器外参标定是否准确，再用 tf2_echo 看变换是否随时间变化。