第2章车辆运动学模型基础

一、车辆运动学模型基础

做纯跟踪算法，第一步要搞清楚的，就是车到底怎么动。

你想想看，控制算法说白了就是给车下指令——「左转30度，油门踩20%」。但车收到指令后，实际会怎么走？这中间缺的就是运动学模型。

我个人习惯，先搭运动学模型再搞控制。因为模型不对，控制再花哨也是白搭。

真实车辆有四个轮子，转向时左右轮转角不一样。但如果我们把四个轮子「压缩」成两个轮子——前轮负责转向，后轮负责驱动——就得到了自行车模型。

核心假设：

我记得第一次做实车测试时，发现高速下模型偏差很大。后来才意识到，自行车模型只适合低速场景（一般< 5m/s）。高速时轮胎侧偏不可忽略，得用动力学模型。

推导过程其实不复杂。设车辆后轴中心为参考点，前轮转角为δ，轴距为L。那么车辆的瞬时转弯半径R满足：

tan(δ) = L / R

这个公式，就是自行车模型的核心。它把前轮转角δ和转弯半径R联系起来了。

为什么会这样？简单画个几何图就明白了——前轮方向与车身方向的夹角δ，加上轴距L，构成了一个直角三角形。嗯，这里要注意：δ是前轮转角，不是方向盘转角。方向盘到前轮还有转向传动比，一般10:1到20:1不等。

刚才说的自行车模型，其实忽略了一个重要问题——真实车辆转弯时，左右前轮的转角是不一样的。

你想想看，车辆转弯时，内侧轮走的圆弧半径小，外侧轮走的圆弧半径大。如果左右轮转角相同，内侧轮就会打滑。阿克曼转向几何就是解决这个问题的。

阿克曼原理：转向时，所有车轮的轴线应交于同一点（瞬时转动中心）。

设左前轮转角为δ_L，右前轮转角为δ_R，轮距为W，轴距为L，则有：

cot(δ_L) - cot(δ_R) = W / L

我在项目中遇到过一个问题：有些低成本车辆的阿克曼率只有60%-70%，导致纯跟踪算法在低速大转角时跟踪误差突然变大。后来我加了个阿克曼补偿项，才把这个问题压下去。

实际应用中，我们通常用自行车模型做控制律设计，用阿克曼几何做转向执行机构的映射。说白了，就是控制算法算出一个「等效前轮转角δ」，然后通过阿克曼关系拆成左右轮的独立转角。

车辆在平面上的位置和朝向，我们叫「位姿」。一般用三个量表示：

位姿向量通常写作：[x, y, θ]^T

这里有个细节——为什么选后轴中心而不是前轴中心？我个人习惯选后轴中心，因为后轴中心在运动学上更「干净」。后轮没有转向，它的运动方向就是车身朝向。前轮有转向角，运动方向会偏，做积分时容易乱。

位姿的更新方程（离散时间，步长dt）：

x(t+1) = x(t) + v * cos(θ) * dt
y(t+1) = y(t) + v * sin(θ) * dt
θ(t+1) = θ(t) + (v / L) * tan(δ) * dt

其中v是后轴中心速度，δ是前轮转角，L是轴距。

注意：航向角θ的取值范围。我见过有人用[-π, π]，有人用[0, 2π)。两种都可以，但做差值时要小心。比如π和-π其实只差一点点，但直接相减会得到2π。建议用atan2处理角度差。

这个关系其实前面已经提到了，但值得单独拿出来说说。

从自行车模型出发：

R = L / tan(δ)

当δ很小时（比如< 10°），tan(δ) ≈ δ，所以：

R ≈ L / δ

这意味着什么？意味着在小转角范围内，转弯半径和转角成反比。转角翻倍，半径减半。

我曾经在调试时发现，车辆在直道上轻微摆动。查了半天，发现是转角分辨率不够——最小可执行的转角变化是0.1°，但对应的半径变化在直道附近特别敏感。后来我限制了最小转角步长，问题就解决了。

几个关键点总结一下：

嗯，到这里运动学基础就差不多了。这些公式虽然简单，但纯跟踪算法的每一步都离不开它们。后面讲预瞄点选取、曲率计算时，你会发现全是在用这些东西。