2. 坐标系与运动学基础：车辆坐标系、全局坐标系、自行车模型、运动学方程

各位同学，欢迎来到第二章。

这一章，咱们聊聊坐标系和运动学。听起来有点基础是吧？但说实话，我在项目里见过太多因为坐标系搞反了，导致车辆原地画圈的惨案。你想想看，一个紧急避障算法，如果连车头朝哪都算错了，那后果……嗯，咱们还是先把地基打牢。

2.1 两个坐标系：全局坐标系与车辆坐标系

做运动规划，脑子里必须同时装着两套坐标系。一套是全局的，一套是跟着车跑的。

• 全局坐标系（World Frame）：通常用 X_w 和 Y_w 表示。它固定在地面上，比如高精地图上的经纬度，或者我们常说的笛卡尔坐标系。所有静态障碍物的位置，都是用这个坐标系描述的。
• 车辆坐标系（Vehicle Frame）：用 X_v 和 Y_v 表示。原点在车辆后轴中心（我个人习惯用后轴，因为跟自行车模型更配），X_v 指向车头方向，Y_v 指向左侧。

为什么要分两套？因为传感器（比如激光雷达）看到的是车周围的东西，是车辆坐标系下的数据。而路径规划是在全局坐标系下做的。所以，你必须在两者之间来回切换。

// 从车辆坐标系 (x_v, y_v) 转到全局坐标系 (x_w, y_w)
x_w = x_v * cos(θ) - y_v * sin(θ) + x_car
y_w = x_v * sin(θ) + y_v * cos(θ) + y_car

// 其中 (x_car, y_car) 是车辆在全局坐标系下的位置
// θ 是车辆的航向角（yaw）

2.2 自行车模型：把四轮车简化成两轮

真正的车辆有四个轮子，转向机构也很复杂。但在运动规划里，我们通常用自行车模型（Bicycle Model）来近似。说白了，就是把前后轮分别合并成一个虚拟轮子。

为什么可以这么干？因为在低速、小转向角的情况下，这种简化带来的误差是可以接受的。而且它让计算变得简单很多。

我曾经在一个项目中，直接用四轮模型做规划，结果计算量太大，控制器根本跑不动。后来换成自行车模型，效果反而更好。嗯，这里要注意：自行车模型只适用于低速场景（通常 < 10 m/s），高速时轮胎侧偏特性会变得明显，模型就不准了。

2.3 运动学方程：车是怎么动的？

有了模型，我们得知道车怎么动。自行车模型的运动学方程长这样：

状态量: [x, y, θ, δ, v]
控制量: [a, ω]

运动学方程:
x_dot = v * cos(θ)
y_dot = v * sin(θ)
θ_dot = v * tan(δ) / L
δ_dot = ω
v_dot = a

其中:
- (x, y): 后轴中心位置（全局坐标系）
- θ: 航向角
- δ: 前轮转角
- v: 纵向速度
- L: 轴距（前后轴距离）
- a: 加速度
- ω: 前轮转角变化率

这个方程组描述了车辆在每一时刻的状态变化。你看，θ_dot 跟 tan(δ) 成正比，跟轴距 L 成反比。这意味着：轴距越长，转弯越迟钝；前轮转角越大，转弯越灵活。

2.4 离散化：从连续到离散

上面的方程是连续的，但我们的控制器是离散的（比如 50 Hz 的控制周期）。所以需要把方程离散化。最常用的是欧拉法：

// 假设时间步长为 dt
x(k+1) = x(k) + v(k) * cos(θ(k)) * dt
y(k+1) = y(k) + v(k) * sin(θ(k)) * dt
θ(k+1) = θ(k) + v(k) * tan(δ(k)) / L * dt
δ(k+1) = δ(k) + ω(k) * dt
v(k+1) = v(k) + a(k) * dt

这个离散模型，就是我们在代码里实际用的东西。每次规划，就是在这个模型上往前推演 N 步，看看未来几秒钟车辆会走到哪里。

2.5 总结一下

这一章的内容，说白了就是三件事：

1. 坐标系要分清：全局坐标系管地图，车辆坐标系管感知。
2. 模型要简化：自行车模型是低速规划的好帮手。
3. 方程要记牢：运动学方程是规划算法的数学基础。

下一章，我们会在这个基础上，加入障碍物约束，开始讲真正的避障算法。到时候你会发现，今天这些基础，就像盖楼的地基——看不见，但决定了楼能盖多高。