第二章 坐标系与状态表示：全局坐标系、车辆坐标系、Frenet坐标系、状态向量表示、状态转移方程

做轨迹预测和运动规划，第一件事是什么？

不是调算法，不是写代码。而是——搞清楚你的车在哪儿，要去哪儿，以及怎么描述这个「在哪儿」和「去哪儿」。

坐标系选错了，后面全白搭。我见过太多新手，上来就怼 Frenet，结果在弯道里把车规划到马路牙子上去了。嗯，咱们今天就把这事彻底聊透。

2.1 全局坐标系：你在地图上的绝对位置

全局坐标系，说白了就是「地球上的坐标」。最常用的是 UTM 坐标系，或者高精地图里的经纬度。

我个人习惯用 UTM，因为它直接给米，不用算球面距离。你想想看，规划路径时用经纬度算距离，还得转成米，多麻烦。

核心要点：

  • 原点：通常是地图上的某个固定点，或者 UTM 带的中央经线
  • X 轴：东向（Easting）
  • Y 轴：北向（Northing）
  • 单位：米

状态向量在全局坐标系下长这样：

// 全局坐标系下的状态向量
struct GlobalState {
    double x;      // 东向位置 (m)
    double y;      // 北向位置 (m)
    double yaw;    // 航向角 (rad)，相对于正东
    double v;      // 速度 (m/s)
    double a;      // 加速度 (m/s²)
};

我在项目中遇到过一个问题：用全局坐标系做局部规划时，数值会变得很大。比如 UTM 坐标，X 动辄几十万米。你算个雅可比矩阵，数值范围差好几个数量级，优化器直接崩了。

所以，全局坐标系适合做全局路径规划、地图匹配。但做局部运动规划？得换坐标系。

3.2 车辆坐标系：以车为本的视角

车辆坐标系，就是以自车为中心建立的坐标系。说白了就是「我坐在驾驶座上，看周围的东西」。

我个人的定义习惯：

  • 原点：车辆后轴中心（不是质心！不是前轴！）
  • X 轴：车头方向
  • Y 轴：车身左侧（左正右负）
  • Z 轴：向上

为什么用后轴中心？因为车辆的运动学模型是基于后轴的。阿克曼转向几何里，后轴中心才是真正的旋转中心。你如果用前轴，算出来的转弯半径全是错的。

状态向量在车辆坐标系下：

// 车辆坐标系下的状态向量
struct VehicleState {
    double x_rel;   // 相对于自车的纵向距离 (m)
    double y_rel;   // 相对于自车的横向距离 (m)
    double vx_rel;  // 相对纵向速度 (m/s)
    double vy_rel;  // 相对横向速度 (m/s)
};

车辆坐标系最大的好处是直观。传感器数据（雷达、摄像头）直接就是车辆坐标系下的。但有个坑——它跟着车转。车一拐弯，坐标系就变了。你没法用它做长距离规划。

避坑指南：

我曾经在项目里直接用车辆坐标系做轨迹预测，结果车转弯时，预测的目标位置突然从左边跳到了右边。为什么？因为坐标系旋转了，目标在全局坐标系下没动，但在车辆坐标系下位置变了。嗯，从那以后，我坚持用 Frenet 做预测，用车辆坐标系只做感知融合。

2.3 Frenet 坐标系：沿着道路走

Frenet 坐标系，是运动规划里最优雅的发明之一。它把问题分解成两个独立的方向：沿着路走（纵向 s），和偏离路中心（横向 d）。

你想想看，在全局坐标系下，一条弯弯曲曲的路，你描述起来多费劲。但在 Frenet 坐标系下，路就是一条直线（s 轴），你只需要关心偏离了多少。

Frenet 坐标定义：

  • s：弧长，沿着参考线从起点到投影点的距离
  • d：横向偏移，从参考线到车辆位置的垂直距离（左正右负）
  • 参考线：通常是道路中心线或车道中心线

状态向量在 Frenet 坐标系下：

// Frenet 坐标系下的状态向量
struct FrenetState {
    double s;       // 纵向弧长 (m)
    double s_dot;   // 纵向速度 (m/s)
    double s_ddot;  // 纵向加速度 (m/s²)
    double d;       // 横向偏移 (m)
    double d_dot;   // 横向速度 (m/s)
    double d_ddot;  // 横向加速度 (m/s²)
};

我个人特别喜欢 Frenet 的一点是：纵向和横向可以分开规划。比如，纵向做速度规划（加速、减速、跟车），横向做路径规划（变道、避障）。两个问题解耦了，算法简单很多。

但注意，Frenet 不是万能的。如果参考线质量差（比如地图精度不够），Frenet 坐标转换会引入误差。我踩过的坑是：高精地图在匝道处曲率变化剧烈，Frenet 投影点跳变，导致规划出的轨迹突然抖动。

2.4 状态转移方程：预测下一时刻的状态

有了状态表示，下一步就是预测——给定当前状态和控制输入，下一时刻的状态是什么？

这就是状态转移方程。说白了，就是「车怎么动」的数学模型。

最常用的模型是自行车模型（Bicycle Model）：

// 自行车模型的状态转移方程
// 状态: [x, y, yaw, v]
// 控制: [a, delta]  (加速度, 前轮转角)

x_next     = x + v * cos(yaw) * dt
y_next     = y + v * sin(yaw) * dt
yaw_next   = yaw + (v / L) * tan(delta) * dt
v_next     = v + a * dt

其中 L 是轴距。这个模型假设车辆只有前后两个轮子，前轮转向。实际车辆当然不是这样，但在低速（< 10 m/s）下，精度完全够用。

我建议：

如果做高速规划（比如高速公路上 30 m/s），自行车模型就不太够了。需要考虑侧偏角、轮胎力。那时候得用动力学模型。但初学者，先把自行车模型吃透。

在 Frenet 坐标系下，状态转移方程更简单：

// Frenet 坐标系下的状态转移
s_next     = s + s_dot * dt + 0.5 * s_ddot * dt²
s_dot_next = s_dot + s_ddot * dt
d_next     = d + d_dot * dt + 0.5 * d_ddot * dt²
d_dot_next = d_dot + d_ddot * dt

你看，没有三角函数，没有非线性项。这就是 Frenet 的威力——把非线性问题线性化了。

2.5 坐标系之间的转换

实际工程中，三个坐标系来回切换是家常便饭。我总结了一套转换流程：

从 到 方法
全局 → 车辆 车辆 平移 + 旋转（绕 Z 轴转 -yaw）
车辆 → 全局 全局 旋转 + 平移（逆操作）
全局 → Frenet Frenet 找到参考线上最近点，计算 s 和 d
Frenet → 全局 全局 沿参考线走 s 距离，再沿法向偏移 d

注意：

全局到 Frenet 的转换，核心是找最近点。如果参考线是离散点，需要用 KD-Tree 加速。我见过有人用暴力搜索，1000 个点每帧都遍历一遍，CPU 直接拉满。嗯，别这么干。

2.6 我的工程建议

说了这么多，最后给几条实在的建议：

  1. 规划用 Frenet，控制用车辆坐标系。 规划层关心的是「沿着路怎么走」，Frenet 最合适。控制层关心的是「方向盘打多少」，车辆坐标系最直接。
  2. 状态向量里一定要包含高阶信息。 只存位置和速度不够，加速度、甚至 jerk 都要存。为什么？因为轨迹平滑性靠高阶项约束。
  3. 状态转移方程要离散化。 连续方程好看，但计算机只能处理离散时间。dt 选多大？我一般用 0.1s，规划 8 秒就是 80 个点。dt 太小计算量大，太大精度不够。
  4. 坐标系转换要写单元测试。 我吃过亏：Frenet 转全局时，参考线曲率符号搞反了，车直接往反方向拐。从那以后，每个转换函数我都写测试用例。

好了，坐标系和状态表示就聊到这儿。下一章咱们讲轨迹预测的经典方法——卡尔曼滤波和粒子滤波。到时候你会看到，今天讲的这些状态向量和转移方程，就是滤波器的核心输入。

← 上一章 📖 返回目录 下一章 →