坐标系与坐标变换：全局坐标系、局部坐标系、车辆坐标系、坐标变换矩阵、齐次坐标
各位同学，今天我们来聊聊路径规划里最基础、也最容易翻车的一个话题——坐标系。
说实话，我见过太多新手工程师，算法写得飞起，结果一上路就撞墙。为什么？坐标系搞错了。你想想看，传感器告诉你“前方2米有障碍”，但车辆自己到底在哪儿？这个“2米”是相对于谁说的？嗯，这就是我们今天要解决的问题。
一、三种坐标系，你得心里有数
在自动驾驶里，我们通常打交道的有三种坐标系。我个人习惯把它们比作三个“视角”：
- 全局坐标系：也叫世界坐标系。就是地图上的那个经纬度，或者UTM坐标。它固定不动，所有东西的绝对位置都靠它定义。
- 局部坐标系：通常是相对于某条参考线（比如车道中心线）的坐标系。它的原点在参考线上，x轴沿着道路方向，y轴垂直于道路。说白了，就是“贴着路走”的坐标系。
- 车辆坐标系：原点在车辆质心（或者后轴中心），x轴朝前，y轴朝左（或者朝右，各家定义略有不同）。这个坐标系跟着车动，传感器数据通常都在这个坐标系下。
我在项目中遇到过最典型的坑：高精地图给的是全局坐标，激光雷达给的是车辆坐标，规划模块却要用局部坐标。三个坐标系如果不打通，你算出来的路径就是错的。
核心要点：路径规划的核心工作之一，就是在这三个坐标系之间来回切换。你必须在脑子里时刻清楚：当前数据是在哪个坐标系下说的。
二、坐标变换矩阵——数学工具
坐标系之间怎么切换？靠的就是坐标变换矩阵。说白了，就是旋转加平移。
假设我有一个点P，在坐标系A下的坐标是(xA, yA)，我想知道它在坐标系B下的坐标(xB, yB)。那么：
[x_B] [cosθ -sinθ] [x_A] [tx]
[y_B] = [sinθ cosθ] [y_A] + [ty]其中θ是坐标系B相对于坐标系A的旋转角度，tx和ty是平移量。
嗯，这里要注意：旋转矩阵是正交矩阵，它的逆就是它的转置。这个性质在实际代码里非常有用，因为求逆矩阵计算量大，但转置几乎不花时间。
我的小技巧：写代码时，我习惯把旋转和平移分开处理。先旋转，再平移，逻辑清晰，不容易出错。别问我怎么知道的——我曾经把平移和旋转的顺序搞反了，调试了整整一个下午。
三、齐次坐标——让变换更优雅
刚才那个公式，旋转和平移是分开写的。如果连续做多次变换，公式会变得又长又丑。这时候，齐次坐标就派上用场了。
齐次坐标，说白了就是把二维点(x, y)变成三维向量(x, y, 1)。这样，旋转和平移就可以合并成一个3x3的矩阵：
[x_B] [cosθ -sinθ tx] [x_A]
[y_B] = [sinθ cosθ ty] [y_A]
[1 ] [ 0 0 1] [ 1 ]你看，一次矩阵乘法，搞定所有。而且多个变换可以连乘：T总 = T1 * T2 * T3 ... 代码写起来特别清爽。
避坑指南：我曾经在项目里直接用4x4齐次矩阵做三维变换，结果因为忘记把最后一行归一化，导致坐标漂移了几十米。记住：齐次坐标做完变换后，如果最后一维不是1，要除以它才能得到真正的坐标。
四、知识体系结构图
下面这张图，是我自己总结的坐标系与坐标变换的知识脉络。你把它理清楚了，后面学路径规划会轻松很多。
五、实战中的坐标系变换流程
在实际的自动驾驶系统中，坐标变换通常是这样流转的：
- 传感器数据采集：激光雷达、摄像头的数据都在车辆坐标系下。
- 数据预处理：将车辆坐标系下的障碍物位置，变换到局部坐标系（沿道路的SL坐标系）。这一步是为了方便路径规划——规划器只关心“沿着路走多远、偏离路中心多少”。
- 全局路径参考：高精地图给出的参考线是全局坐标，需要变换到局部坐标系下，作为规划器的输入。
- 规划结果输出：规划器在局部坐标系下算出的路径点，再变换回车辆坐标系，发给控制模块执行。
关键提醒：每一步变换都要记录变换矩阵。我习惯在代码里用一个TransformManager类来统一管理，避免到处写变换逻辑，容易出错。
六、代码示例：C++中的坐标变换
下面是一个简单的坐标变换示例，用Eigen库实现。我个人很喜欢Eigen，因为它语法简洁，性能也好。
#include <Eigen/Dense>
#include <cmath>
// 定义二维齐次变换矩阵
Eigen::Matrix3d getTransformMatrix(double theta, double tx, double ty) {
Eigen::Matrix3d T;
T << cos(theta), -sin(theta), tx,
sin(theta), cos(theta), ty,
0, 0, 1;
return T;
}
// 将点从坐标系A变换到坐标系B
Eigen::Vector2d transformPoint(
const Eigen::Vector2d& point_A,
const Eigen::Matrix3d& T_A_to_B)
{
Eigen::Vector3d homo_A(point_A.x(), point_A.y(), 1.0);
Eigen::Vector3d homo_B = T_A_to_B * homo_A;
return Eigen::Vector2d(homo_B.x(), homo_B.y());
}
// 使用示例
int main() {
// 假设车辆坐标系相对于全局坐标系：旋转30度，平移(5, 3)
double theta = M_PI / 6.0;
Eigen::Matrix3d T_vehicle_to_global = getTransformMatrix(theta, 5.0, 3.0);
// 车辆坐标系下有一个点(2, 1)
Eigen::Vector2d pt_vehicle(2.0, 1.0);
// 变换到全局坐标系
Eigen::Vector2d pt_global = transformPoint(pt_vehicle, T_vehicle_to_global);
std::cout << "全局坐标: (" << pt_global.x()
<< ", " << pt_global.y() << ")" << std::endl;
return 0;
}我的经验：写变换代码时，一定要给每个矩阵变量起清楚的名字。比如T_vehicle_to_global，一看就知道是从车辆坐标系到全局坐标系。别偷懒用T1、T2这种名字——我曾经吃过这个亏，代码三天后自己都看不懂了。
七、常见错误与避坑
| 错误类型 | 具体表现 | 解决方案 |
|---|---|---|
| 旋转方向搞反 | 车辆明明左转，规划路径却向右偏 | 确认旋转角度的正方向定义（通常逆时针为正） |
| 平移顺序错误 | 先平移后旋转，导致坐标严重偏离 | 始终先旋转，后平移 |
| 齐次坐标忘记归一化 | 坐标值逐渐漂移，越跑越远 | 变换后检查最后一维，不为1则除以它 |
| 坐标系定义不一致 | 不同模块用不同朝向的y轴 | 团队统一约定，写在接口文档里 |
嗯，最后说一句：坐标系变换这件事，看起来简单，但它是整个路径规划的基石。你花一小时把这块吃透，后面能省下十小时的调试时间。我自己刚入行时，就是在这上面栽过跟头，所以现在格外重视。
好了，坐标系的内容就讲到这里。记住：坐标系不乱，路径规划就成功了一半。