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

做自动驾驶规划控制，第一件事是什么？

不是调参数，不是写算法。而是搞清楚——车在哪，要去哪，周围的东西又在哪。

这三个问题，全得靠坐标系来回答。我刚开始入行时，觉得坐标系不就是个数学概念嘛，有啥好学的？结果第一次实车调试，就因为坐标系搞混，车直接往墙上怼……嗯，从那以后我再也不敢小看它了。

2.1 三种坐标系，你得门儿清

自动驾驶里，我们主要跟三种坐标系打交道。说白了，就是三种不同的“看世界”的角度。

2.1.1 全局坐标系

全局坐标系，也叫世界坐标系。它固定在地球上，不随车动。通常用经纬度（WGS84）或者UTM坐标来表示。

我个人习惯用UTM，因为它单位是米，算距离方便。高精地图里的车道线、红绿灯位置，存的都是全局坐标。

2.1.2 车辆坐标系

车辆坐标系是固定在车上的。原点一般在后轴中心，或者车辆质心。

标准定义是这样的：

• x轴：车头方向为正
• y轴：车身左侧为正（注意，有些标准用右侧为正，容易搞混）
• z轴：车顶方向为正

我在项目中遇到过，不同传感器厂商对y轴正方向定义不一样。有的用左正，有的用右正。如果你不做统一，融合出来的数据就是乱的。

2.1.3 局部坐标系

局部坐标系是个“临时工”。它跟着车走，但原点不一定在车上。

比如，你规划一条路径，路径上的每个点，都是相对于路径起点来描述的。这个起点就是局部坐标系的原点。

为什么要用局部坐标系？因为全局坐标数值太大，动不动就几十万米，计算起来浮点误差大。局部坐标数值小，精度高。

2.2 坐标变换：平移和旋转

有了坐标系，接下来就是怎么把一个点从一个坐标系变到另一个坐标系。这就是坐标变换。

坐标变换就两件事：平移和旋转。

2.2.1 平移

平移最简单。就是两个坐标系原点之间的偏移。

比如，车辆坐标系原点相对于全局坐标系原点的偏移是 (dx, dy, dz)。那么车上的一个点 (x车, y车, z车) 在全局坐标系下的坐标就是：

// 平移变换
x全 = x车 + dx
y全 = y车 + dy
z全 = z车 + dz

就这么简单。但要注意，平移的前提是两个坐标系的朝向完全一致。如果朝向不同，就得先旋转。

2.2.2 旋转

旋转就稍微复杂一点。它描述的是两个坐标系之间的朝向差异。

旋转可以用旋转矩阵来表示。绕x轴、y轴、z轴旋转，各有各的矩阵。我直接给你代码：

// 绕z轴旋转角度θ
Rz = | cosθ  -sinθ  0 |
     | sinθ   cosθ  0 |
     | 0      0     1 |

// 绕x轴旋转角度φ
Rx = | 1   0      0    |
     | 0   cosφ  -sinφ |
     | 0   sinφ   cosφ |

// 绕y轴旋转角度ψ
Ry = |  cosψ  0   sinψ |
     |  0     1   0    |
     | -sinψ  0   cosψ |

实际应用中，我们通常先旋转，再平移。顺序不能反，反了结果就错了。我曾经因为搞反顺序，让车在停车场里画了半小时的圈……

2.3 欧拉角：直观但有个坑

欧拉角是描述旋转最直观的方式。它用三个角度来表示：横滚角（Roll）、俯仰角（Pitch）、偏航角（Yaw）。

• Yaw（偏航）：绕z轴转，就是车头左右摆
• Pitch（俯仰）：绕y轴转，就是车头上下翘
• Roll（横滚）：绕x轴转，就是车身左右侧倾

你想想看，这三个角度是不是很符合直觉？开车时你主要关心Yaw，上坡下坡看Pitch，过弯侧倾看Roll。

2.4 四元数：工程师的救星

为了解决万向锁，我们引入了四元数。

四元数听起来玄乎，其实就是一个四维向量 (w, x, y, z)。它表示绕某个轴旋转某个角度。没有奇点，插值平滑，计算效率高。

我个人建议：内部计算全用四元数，只在人机交互时转成欧拉角。

四元数和欧拉角的转换代码，我直接给你：

// 欧拉角转四元数 (ZYX顺序)
double cy = cos(yaw * 0.5);
double sy = sin(yaw * 0.5);
double cp = cos(pitch * 0.5);
double sp = sin(pitch * 0.5);
double cr = cos(roll * 0.5);
double sr = sin(roll * 0.5);

w = cy * cp * cr + sy * sp * sr;
x = cy * cp * sr - sy * sp * cr;
y = sy * cp * sr + cy * sp * cr;
z = sy * cp * cr - cy * sp * sr;

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

double sinp = 2 * (w * y - z * x);
if (abs(sinp) >= 1)
    pitch = copysign(M_PI / 2, sinp);
else
    pitch = asin(sinp);

double siny_cosp = 2 * (w * z + x * y);
double cosy_cosp = 1 - 2 * (y * y + z * z);
yaw = atan2(siny_cosp, cosy_cosp);

注意，上面这个转换假设旋转顺序是ZYX。不同顺序结果不一样，一定要跟你的传感器对齐。

2.5 知识体系总览

下面这张图，是我自己整理的坐标系与坐标变换知识结构。你照着这个学，不会乱。

2.6 实战中的坐标系管理

说了这么多理论，来点实际的。在代码里，你怎么管理这些坐标系？

我推荐用TF树（坐标变换树）。每个坐标系都是一个节点，节点之间的变换关系就是边。你只需要维护好这棵树，任何两个坐标系之间的变换都能自动算出来。

ROS里的TF库就是干这个的。我自己写代码时，也会模仿TF的思路，建一个轻量级的变换管理器。

嗯，坐标系这块就讲这么多。内容不多，但都是基础中的基础。你把这些搞懂了，后面学路径规划、控制算法，才能站得稳。

可可精品资料库