第2章车辆运动学模型

第2章：车辆运动学模型：自行车模型推导、状态空间方程建立、离散化方法

各位同学，欢迎来到第二章。上一章我们聊了MPC的整体框架，今天咱们要啃一块硬骨头——车辆运动学模型。

说实话，很多做MPC的同学，最后调参调不好，根子就出在模型上。模型不准，你控制器设计得再花哨，也是白搭。我当年刚入行时，就吃过这个亏，辛辛苦苦调了一周的参数，实车一跑，直接偏到隔壁车道去了。后来才发现，是模型简化得太狠了。

2.1 为什么是自行车模型？

你想想看，一辆真正的汽车，四个轮子，每个轮子都有转向、悬架、轮胎侧偏……如果全建模进去，那状态变量得有几十个。MPC算起来，别说实时了，一帧都算不完。

所以，我们得简化。自行车模型（Bicycle Model）就是最经典的简化方式。它把左右两个前轮合并成一个虚拟前轮，左右后轮合并成一个虚拟后轮。说白了，就是想象你的车变成了一辆自行车。

核心假设：

忽略车辆的侧倾和俯仰（只考虑平面运动）
左右车轮的转向角相同（阿克曼转向几何的近似）
轮胎的侧偏特性简化为线性（小角度假设）
车辆纵向速度恒定（或者变化缓慢）

嗯，这里要注意。这些假设在低速（比如泊车、拥堵跟车）时非常准。但如果你开到80km/h以上做紧急避障，轮胎侧偏就会变得很严重，这时候自行车模型就不够用了。我在项目中遇到过，高速场景下用自行车模型做MPC，控制效果明显变差，后来换成了动力学模型才解决。

2.2 自行车模型的数学推导

好，咱们开始推公式。别怕，我会一步步来。

首先，定义车辆的状态。通常我们用三个量来描述车辆的位置和朝向：

x：车辆后轴中心在大地坐标系下的X坐标
y：车辆后轴中心在大地坐标系下的Y坐标
ψ：车辆的航向角（yaw angle），也就是车头指向

控制输入有两个：

v：纵向速度（通常假设为常数，或者由上层规划给出）
δ：前轮转向角

为什么选后轴中心作为参考点？我个人习惯用后轴中心，因为后轴不转向，运动学关系更简洁。当然也有人用质心，但那是动力学模型的事了。

现在，我们来看车辆的运动关系。想象一下，后轮的速度方向永远沿着车身的纵轴方向（因为后轮不能转向）。所以：

ẋ = v * cos(ψ)
ẏ = v * sin(ψ)

那航向角的变化率呢？这就要看前轮了。前轮的转向角δ，决定了车辆转弯的曲率。根据几何关系，车辆的转弯半径R和转向角δ的关系是：

R = L / tan(δ)

其中L是轴距（前轮到后轮的距离）。而角速度ψ_dot = v / R，所以：

ψ_dot = v * tan(δ) / L

小技巧：当δ很小时（比如高速公路上微调方向），tan(δ) ≈ δ。这时候模型就变成了线性模型，很多控制理论可以直接套用。我刚开始做车道保持时，就用这个线性近似，效果还不错。

2.3 状态空间方程建立

有了上面的微分方程，我们就可以写出连续时间下的状态空间方程了。

状态向量：ξ = [x, y, ψ]^T

控制输入：u = [v, δ]^T

那么状态方程就是：

ξ_dot = f(ξ, u) = 
[ v * cos(ψ) ]
[ v * sin(ψ) ]
[ v * tan(δ) / L ]

你看，这是一个非线性系统。因为状态方程里含有cos(ψ)、sin(ψ)和tan(δ)。MPC处理非线性系统，要么用非线性MPC（NMPC），要么在某个工作点附近线性化。

我个人更推荐线性化。为什么呢？因为NMPC的计算量太大了，在ADAS的嵌入式平台上，算力有限，线性MPC更实用。

线性化的方法很简单——泰勒展开，取一阶项。假设我们在参考点(ξ_ref, u_ref)附近工作，那么：

ξ_dot ≈ f(ξ_ref, u_ref) + A * (ξ - ξ_ref) + B * (u - u_ref)

其中A和B是雅可比矩阵：

A = ∂f/∂ξ, B = ∂f/∂u

具体算一下：

A = [0, 0, -v_ref * sin(ψ_ref)]
    [0, 0,  v_ref * cos(ψ_ref)]
    [0, 0,  0]

B = [cos(ψ_ref), 0]
    [sin(ψ_ref), 0]
    [tan(δ_ref)/L, v_ref / (L * cos²(δ_ref))]

避坑指南：我曾经在写代码时，忘了把cos²(δ_ref)写成(cos(δ_ref))^2，结果分母算出来是cos(2*δ_ref)，整个B矩阵都错了。这种细节错误，调试起来特别痛苦。建议大家写代码时，每一步都手动验算一下。

2.4 离散化方法

好，现在我们有连续模型了。但MPC是在数字计算机上运行的，必须把连续模型变成离散模型。离散化的方法有很多，我挑三种最常用的讲。

2.4.1 前向欧拉法（一阶近似）

这是最简单的方法，也是我最早学的方法。公式就是：

ξ(k+1) = ξ(k) + Ts * ξ_dot(k)

其中Ts是采样时间。代入我们的模型：

x(k+1) = x(k) + Ts * v(k) * cos(ψ(k))
y(k+1) = y(k) + Ts * v(k) * sin(ψ(k))
ψ(k+1) = ψ(k) + Ts * v(k) * tan(δ(k)) / L

优点：简单，计算量小。缺点：精度一般，当Ts较大时误差明显。

2.4.2 零阶保持法（ZOH）

对于线性化后的系统，我们可以用零阶保持法。假设控制输入在两个采样点之间保持不变，那么：

ξ(k+1) = A_d * ξ(k) + B_d * u(k)

其中：

A_d = e^(A * Ts)
B_d = ∫₀ᵀˢ e^(A * τ) dτ * B

这个矩阵指数计算起来有点麻烦，但在MATLAB里直接用c2d函数就行。在C++里，可以用Eigen库的矩阵指数函数。

我的习惯：在仿真阶段，我一般用ZOH法，精度高。但在实车代码里，为了实时性，我经常用前向欧拉法。只要Ts选得足够小（比如20ms以内），欧拉法的精度完全够用。

2.4.3 龙格-库塔法（RK4）

如果你对精度要求很高，又不想用ZOH那么复杂，RK4是个折中方案。它用四个中间步长来逼近积分结果，精度比欧拉法高一个数量级。

公式我就不展开了，大家记住一点：RK4的计算量是欧拉法的4倍，但精度提升很明显。我在做泊车辅助时，因为车速极低（< 5km/h），用欧拉法就够了。但在做高速车道保持时，我换成了RK4。

2.5 离散化后的状态空间方程

最终，我们得到离散化的线性时变模型：

ξ(k+1) = A_d(k) * ξ(k) + B_d(k) * u(k) + d(k)

其中d(k)是线性化带来的偏差项：

d(k) = ξ_ref(k+1) - A_d(k) * ξ_ref(k) - B_d(k) * u_ref(k)

这个模型，就是后续MPC优化的基础。每一时刻，我们都要根据当前的工作点重新计算A_d和B_d，然后求解一个二次规划问题。

总结一下本章要点：

自行车模型把四轮车简化为两轮，适用于低速场景
状态空间方程是非线性的，需要线性化后才能用于线性MPC
离散化方法选欧拉法还是ZOH，取决于你的采样时间和精度要求
线性化后的模型是时变的，每个控制周期都要重新计算

下一章，我们会基于这个离散模型，搭建完整的MPC控制器。到时候，你会看到这些矩阵是怎么变成控制量的。咱们下章见。

📚 课程目录