3、CACC核心算法：车辆动力学模型、经典ACC控制算法（PID、MPC）、CACC协同控制策略、队列稳定性分析
好，我们进入正题。这一章是CACC的“心脏”。说白了，你V2X通信搞得再好，如果控制算法拉胯，那车队的跟车效果就是一坨浆糊。我个人习惯把这一章拆成四个部分来讲：先搞清楚车怎么动（动力学模型），再搞定单车的ACC控制（PID和MPC），然后引入V2X变成CACC，最后用数学验证一下这个车队稳不稳。
3.1 车辆动力学模型：你得知道车会怎么动
做控制，第一步就是建模。你想想看，连被控对象的行为都描述不清楚，你怎么设计控制器？
在CACC里，我们最常用的是纵向动力学模型。我不建议搞得太复杂，什么悬架、轮胎侧偏全塞进去，那是做车辆动力学仿真的人干的。做控制，我们关注的是“给油”和“刹车”怎么影响车速和位置。
一个经典的简化模型长这样：
// 纵向动力学简化模型
// 加速度 a 由驱动力 F_drive、阻力 F_resist 决定
// m * a = F_drive - F_resist
// 阻力通常包括：空气阻力 + 滚动阻力 + 坡度阻力
// F_resist = 0.5 * ρ * Cd * A * v^2 + Cr * m * g * cos(θ) + m * g * sin(θ)
嗯，这里要注意。在实际项目中，我一般会把空气阻力项做线性化处理。为什么？因为非线性项会让后面的MPC求解变得很慢。我在一个量产项目里就吃过这个亏，模型太精确，结果控制器算不过来，周期跑飞了。后来我改成在平衡点附近做泰勒展开，用线性模型近似，效果反而更好。
对于CACC，我们更常用的是三阶状态空间模型：
| 状态变量 | 物理意义 | 单位 |
|---|---|---|
| p_i | 第i辆车的绝对位置 | m |
| v_i | 第i辆车的速度 | m/s |
| a_i | 第i辆车的实际加速度 | m/s² |
控制输入通常是期望加速度 u_i，而实际加速度 a_i 和 u_i 之间有一个一阶惯性环节：
// 执行器延迟模型
// τ 是时间常数，通常取 0.1~0.5s
// a_i_dot = (1/τ) * (u_i - a_i)
这个τ值很关键。我记得有一次在实车测试时，发现跟车总是有0.3秒的延迟震荡，查了半天，就是τ值标定错了。避坑指南：τ一定要通过实车数据辨识，别拍脑袋给。
3.2 经典ACC控制算法：PID与MPC
搞定了模型，我们来看看怎么控制单车。ACC的核心目标就两个：跟上车速和保持车距。
3.2.1 PID控制：简单粗暴，但够用
PID在ACC里是最常见的。你想想看，一个距离误差，一个速度误差，组合一下就是控制量。
// 经典ACC的PID控制律
// e_d = d_actual - d_desired (距离误差)
// e_v = v_ego - v_target (速度误差)
// 控制量 u = Kp * e_d + Kd * e_v + Ki * ∫e_d dt
// 其中 d_desired = d0 + t_gap * v_ego
// d0 是静止安全距离，t_gap 是时距
我个人习惯把时距t_gap设成1.2秒到2.0秒之间。为什么？太短了急刹车容易追尾，太长了容易被加塞。我在高速测试时试过1.0秒，结果前车急刹，我这边差点没刹住，冷汗都出来了。
但PID有个硬伤——它不知道未来。说白了，它只能根据当前的误差做反应。如果前车突然急减速，PID的响应天生就慢半拍。这就是为什么我们需要MPC。
3.2.2 MPC控制：看得更远，算得更准
MPC（模型预测控制）的核心思想是：基于当前状态，预测未来N步，然后求解一个最优控制序列。
对于ACC，MPC的代价函数通常长这样：
// MPC代价函数
// min J = Σ (w1 * e_d(k)^2 + w2 * e_v(k)^2 + w3 * u(k)^2)
// 约束条件：
// v_min ≤ v(k) ≤ v_max
// a_min ≤ a(k) ≤ a_max
// u_min ≤ u(k) ≤ u_max
// d(k) ≥ d_safe (安全距离约束)
我在项目中遇到过一个问题：MPC的预测时域选多长？选太短（比如5步），效果跟PID差不多；选太长（比如30步），计算量爆炸，嵌入式芯片根本跑不动。我建议预测时域取10~15步，控制时域取3~5步，这是一个比较折中的选择。
3.3 CACC协同控制策略：V2X加持下的新玩法
好了，单车的ACC搞定了。现在加上V2X，我们就能玩出花来。
CACC和ACC最大的区别是什么？ACC只能“看”前车，CACC能“看”前前车，甚至头车。你想想看，如果你能提前知道前前车在刹车，你的反应时间是不是就多了零点几秒？
经典的CACC控制策略有两种：
- 前馈+反馈结构：用V2X接收前车的加速度作为前馈项，再用雷达测距做反馈修正。
- 一致性协议：所有车辆共享状态信息，通过图论和一致性算法让整个车队的速度和间距趋于一致。
我个人更推荐第一种，因为它对通信丢包更鲁棒。我在一个demo项目里试过第二种，结果通信一断，整个车队就乱套了。
前馈+反馈的CACC控制律可以写成：
// CACC控制律（前馈+反馈）
// u_i = K1 * (p_{i-1} - p_i - d_desired)
// + K2 * (v_{i-1} - v_i)
// + Kff * a_{i-1} // 前馈项，来自V2X
// 注意：a_{i-1} 是前车的实际加速度，通过V2X广播获得
这个Kff前馈增益很关键。理论上，如果模型完全准确，Kff=1就能完美补偿前车的加减速。但实际上，由于执行器延迟和模型误差，我一般把Kff设在0.7~0.9之间，留点余量。
3.4 队列稳定性分析：你的车队会不会“抖”起来？
这是CACC里最容易被忽视，但也是最要命的问题。队列稳定性（String Stability）指的是：一个扰动在车队中传播时，会不会被放大？
你想想看，如果头车轻轻点了一下刹车，结果传到第10辆车时变成了急刹车，那这个车队就是“不稳定”的。现实中，这种“幽灵堵车”就是这么来的。
判断队列稳定性的数学条件，通常用传递函数的H∞范数：
// 队列稳定性条件
// 定义相邻两车的间距误差传递函数：
// G(s) = E_i(s) / E_{i-1}(s)
// 稳定的条件是：||G(jω)||∞ ≤ 1
// 即：对于所有频率ω，|G(jω)| ≤ 1
说白了，就是误差从上一辆车传到下一辆车时，幅值不能放大。这个条件在频域里很好验证。
对于CACC，由于有了前馈项，我们可以把传递函数写成：
// CACC的间距误差传递函数
// G(s) = (s^2 + K2*s + K1) / (s^2 + K2*s + K1 + Kff*s^2*e^{-τs})
// 其中τ是通信延迟
嗯，这里要注意。通信延迟τ是队列稳定性的“杀手”。我做过仿真，当τ超过200ms时，即使Kff调得再好，高频段也会出现幅值放大。所以，CACC对V2X的延迟要求非常苛刻，通常要求端到端延迟小于100ms。
最后，我分享一个实战经验。在做队列稳定性分析时，不要只看理论条件，一定要做蒙特卡洛仿真。把通信延迟、模型参数误差、传感器噪声都随机化，跑上千次，看看最坏情况下的误差放大倍数。我在一个项目里就是这么发现问题的——理论分析说稳定，但仿真一跑，有5%的概率会发散。后来加了鲁棒性约束才解决。
好，这一章的内容就到这。下一章我们会讲具体的CACC代码实现，包括如何在嵌入式平台上部署MPC求解器。到时候我会把踩过的坑一个一个说清楚。