第二章 车辆运动学模型：从自行车模型到离散化实现
各位同学，今天我们来啃一块硬骨头——车辆运动学模型。
说实话，很多做控制的人一上来就怼动力学模型，觉得运动学太简单。但我个人经验告诉我，运动学模型要是没吃透，后面MPC的坑你一个都躲不掉。我在项目里见过太多人，明明动力学模型建得漂漂亮亮，结果实车一跑就翻车，最后发现是运动学假设出了问题。
2.1 为什么是自行车模型？
你想想看，一辆四轮车，四个轮子各有各的转角、各有各的速度，要是全建模进去，状态变量能堆到十几个。MPC算起来慢得像蜗牛，还谈什么实时控制？
自行车模型的核心思想很简单：把左右轮合并成一个虚拟轮。前轴两个轮子合并成一个前轮，后轴两个轮子合并成一个后轮。这样一来，四轮车就变成了两轮自行车。
关键假设：
- 车辆只在平面内运动（忽略垂向跳动）
- 轮胎无侧偏（低速场景下成立）
- 前后轴刚性连接（车架不变形）
嗯，这里要注意：自行车模型只适用于低速场景（一般车速 < 10 m/s）。高速时轮胎侧偏不可忽略，得用动力学模型。我刚开始做项目时就在这栽过跟头——高速泊车场景用了运动学模型，结果控制精度一塌糊涂。
2.2 自行车模型推导
我们来看推导过程。先定义几个变量：
| 符号 | 含义 | 单位 |
|---|---|---|
| (x, y) | 后轴中心位置 | m |
| θ | 航向角（车头朝向） | rad |
| δ | 前轮转角 | rad |
| v | 后轴中心速度 | m/s |
| L | 轴距 | m |
从几何关系出发，后轴中心的速度方向始终沿着车身方向。所以：
ẋ = v * cos(θ)
ẏ = v * sin(θ)航向角的变化率呢？这得看前轮转角。前轮速度方向与车身夹角为δ，前轮中心到后轴中心的距离是L。根据瞬心法：
θ̇ = (v / L) * tan(δ)完整的状态方程就是：
ẋ = v * cos(θ)
ẏ = v * sin(θ)
θ̇ = (v / L) * tan(δ)这就是经典的自行车运动学模型。三个状态量（x, y, θ），两个控制量（v, δ）。
我的小技巧：实际项目中，我习惯把控制量改成加速度a和转角变化率δ̇，这样控制更平滑。不过那是后话了，咱们先把基础打牢。
2.3 状态空间表示
有了微分方程，下一步就是写成状态空间形式。状态向量和控制向量分别是：
状态向量: z = [x, y, θ]ᵀ
控制向量: u = [v, δ]ᵀ状态空间方程：
ż = f(z, u) =
[v * cos(θ)]
[v * sin(θ)]
[v * tan(δ) / L]注意，这是个非线性系统。为什么？因为状态变量θ出现在三角函数里面。MPC处理非线性系统有两种思路：
- 直接非线性MPC：精度高，但计算量大，实时性差
- 线性化MPC：近似处理，计算快，工程上更常用
我个人更推荐第二种。为什么呢？因为自动驾驶对实时性要求极高，几十毫秒内必须算出控制量。非线性MPC算半天，黄花菜都凉了。
2.4 模型离散化方法
连续模型不能直接用在MPC里——控制器是离散的，每个控制周期算一次。所以必须离散化。
常用的方法有两种：欧拉法和龙格-库塔法。
2.4.1 欧拉法（一阶精度）
欧拉法最简单，公式就是：
z(k+1) = z(k) + T * f(z(k), u(k))其中T是采样周期。展开写：
x(k+1) = x(k) + T * v(k) * cos(θ(k))
y(k+1) = y(k) + T * v(k) * sin(θ(k))
θ(k+1) = θ(k) + T * v(k) * tan(δ(k)) / L代码实现：
def euler_step(state, control, dt, L):
x, y, theta = state
v, delta = control
x_next = x + dt * v * cos(theta)
y_next = y + dt * v * sin(theta)
theta_next = theta + dt * v * tan(delta) / L
return [x_next, y_next, theta_next]我曾经踩过的坑：欧拉法在采样周期较大时（比如T > 0.1s），误差会迅速累积。有一次我做低速跟踪，T设了0.2s，结果轨迹跑偏了半米多。后来换成RK4才解决。
2.4.2 龙格-库塔法（四阶精度）
四阶龙格-库塔法（RK4）精度高得多。它的思路是：在一个步长内取四个点的斜率，加权平均。
公式稍微复杂一点：
k1 = f(z(k), u(k))
k2 = f(z(k) + 0.5*T*k1, u(k))
k3 = f(z(k) + 0.5*T*k2, u(k))
k4 = f(z(k) + T*k3, u(k))
z(k+1) = z(k) + (T/6) * (k1 + 2*k2 + 2*k3 + k4)代码实现：
def rk4_step(state, control, dt, L):
x, y, theta = state
v, delta = control
def f(s):
return [
v * cos(s[2]),
v * sin(s[2]),
v * tan(delta) / L
]
k1 = f(state)
k2 = f([state[i] + 0.5*dt*k1[i] for i in range(3)])
k3 = f([state[i] + 0.5*dt*k2[i] for i in range(3)])
k4 = f([state[i] + dt*k3[i] for i in range(3)])
x_next = x + (dt/6) * (k1[0] + 2*k2[0] + 2*k3[0] + k4[0])
y_next = y + (dt/6) * (k1[1] + 2*k2[1] + 2*k3[1] + k4[1])
theta_next = theta + (dt/6) * (k1[2] + 2*k2[2] + 2*k3[2] + k4[2])
return [x_next, y_next, theta_next]2.4.3 两种方法对比
| 方法 | 精度 | 计算量 | 推荐场景 |
|---|---|---|---|
| 欧拉法 | O(T²) | 低 | T < 0.05s 或 精度要求不高 |
| RK4 | O(T⁵) | 中 | T > 0.05s 或 精度要求高 |
我个人习惯：采样周期小于50ms用欧拉法，大于50ms用RK4。当然，如果你算力够用，无脑RK4也行。
2.5 知识体系总览
下面这张图把本章的核心逻辑串起来了：
从图中可以看到，整个流程是：物理模型 → 数学表达 → 离散化 → MPC。每一步都有坑，每一步都有技巧。
避坑指南：我曾经在离散化这一步吃过亏。当时用欧拉法，采样周期0.1s，结果模型预测的轨迹和实际轨迹偏差越来越大。排查了半天才发现是离散化误差累积导致的。换成RK4后，问题迎刃而解。
好了，这一章的内容就到这里。自行车模型看似简单，但它是整个MPC控制的基础。后面的章节我们会在这个模型上搭建完整的MPC控制器。记住：基础不牢，地动山摇。把这一章吃透了，后面的路就好走了。
公众号:蓝海资料掘金营，微信deep3321