4. 基于PID的速度跟踪控制：目标速度生成、PID控制器设计、抗积分饱和

速度跟踪控制，说白了就是让车按照我们想要的速度跑。你想想看，自动驾驶里最基础也最关键的一环，就是速度控制。油门踩深了车窜出去，踩浅了又跟不上前车。这里面的门道，我今天跟你好好聊聊。

我个人习惯把速度跟踪控制拆成三个部分来看：目标速度怎么来、PID控制器怎么设计、以及那个让人头疼的积分饱和问题怎么解决。咱们一个一个说。

4.1 目标速度生成

目标速度不是拍脑袋定的。它来自上层规划模块，但到了我们控制层，还得做一层处理。

目标速度的来源通常有这几个：

全局路径规划：比如道路限速、弯道限速。这些是静态的，提前知道的。
行为决策层：比如前方有车，决策层说“跟车”，那目标速度就是前车速度。
安全约束：比如遇到行人、障碍物，目标速度可能直接降到0。

我在项目中遇到过一个问题：规划层给的目标速度是阶跃变化的。比如从30km/h直接跳到0km/h。你想想看，如果PID控制器直接去追这个阶跃目标，那加速度会非常大，乘客体验极差。

所以，我一般会加一个速度平滑模块。说白了，就是对目标速度做一阶低通滤波，或者用S型曲线生成。

核心思路：目标速度的变化率也要受约束。加速度不能超过舒适性阈值（比如2m/s²），加加速度（jerk）也要限制。

举个例子，假设规划层突然要求从50km/h减速到0：

// 伪代码：速度平滑
float target_velocity_raw = 50.0; // km/h
float target_velocity_smooth = 0.0;
float accel_limit = 2.0; // m/s²

// 每帧计算
float delta_v = target_velocity_raw - target_velocity_smooth;
float max_delta_v = accel_limit * dt * 3.6; // 转换为km/h
if (delta_v > max_delta_v) {
    target_velocity_smooth += max_delta_v;
} else if (delta_v < -max_delta_v) {
    target_velocity_smooth -= max_delta_v;
} else {
    target_velocity_smooth = target_velocity_raw;
}

这样处理之后，目标速度的变化就平滑多了。嗯，这里要注意，平滑后的目标速度不能超过原始目标速度，否则就失去了跟踪的意义。

4.2 PID控制器设计

PID控制器，大家都不陌生。但用在车辆速度控制上，有几个细节需要留意。

标准的PID公式：

u(t) = Kp * e(t) + Ki * ∫e(τ)dτ + Kd * de(t)/dt

其中e(t)是速度误差，也就是目标速度减去实际速度。

我个人习惯这样调参：

先调Kp：让系统能跟上目标速度，但允许有稳态误差。Kp太大容易震荡。
再加Ki：消除稳态误差。比如上坡时，光靠Kp可能一直有误差，Ki就起作用了。
最后加Kd：抑制超调。但Kd对噪声敏感，我一般加得比较保守。

我的经验：速度控制中，Kd的作用其实没有位置控制那么明显。因为速度本身就是一个微分量的结果。我有时候甚至只用PI控制，效果也不错。

离散化实现：实际代码里，我们用的是离散PID。位置式PID和增量式PID，我推荐用增量式。

// 增量式PID实现
float pid_update(float target, float current, float dt) {
    float error = target - current;
    float delta_error = error - last_error;
    
    // P项
    float p_out = Kp * delta_error;
    // I项
    float i_out = Ki * error * dt;
    // D项
    float d_out = Kd * (delta_error - last_delta_error) / dt;
    
    float delta_u = p_out + i_out + d_out;
    
    last_error = error;
    last_delta_error = delta_error;
    
    return delta_u;
}

增量式的好处是，输出的是控制量的增量，不容易出现大幅度跳变。而且积分饱和处理起来也方便。

4.3 抗积分饱和

积分饱和，这是PID控制里最让人头疼的问题之一。我曾经在一个项目中吃过这个亏，车在坡道上起步，积分项一直累积，结果油门一给就窜出去了。

为什么会这样？说白了，就是执行器已经饱和了（比如油门踩到底了），但误差还在，积分项还在不断累积。等误差反向时，积分项需要很长时间才能退出来，导致系统响应变慢，甚至震荡。

注意：积分饱和在速度控制中特别常见。因为车辆的执行器（电机、发动机、制动系统）都有物理极限。油门不可能无限大，刹车也不可能无限强。

常用的抗积分饱和方法：

积分限幅法：给积分项设置一个上限。比如积分项最大不超过油门开度的50%。
积分分离法：当误差较大时，停止积分。误差小到一定范围内再开始积分。
反计算法（Back-calculation）：当输出饱和时，把超出部分反馈回积分项，让它退回来。

我推荐用反计算法，效果最好：

// 反计算法抗积分饱和
float pid_with_anti_windup(float target, float current, float dt) {
    float error = target - current;
    
    // 计算未饱和的输出
    float u_unlimited = Kp * error + integral + Kd * (error - last_error) / dt;
    
    // 限幅
    float u_saturated = clamp(u_unlimited, -max_output, max_output);
    
    // 反计算：把超出部分反馈回积分项
    float aw_gain = 1.0 / Kp; // 反计算增益，经验值
    integral += (Ki * error * dt) + aw_gain * (u_saturated - u_unlimited) * dt;
    
    last_error = error;
    return u_saturated;
}

你看，当输出饱和时，u_saturated - u_unlimited 是负的，它会拉着积分项往回退。这样一旦误差反向，积分项已经在一个合理的位置了，系统响应会快很多。

小技巧：反计算增益aw_gain一般取1/Kp。但如果你觉得退得不够快，可以适当加大。我一般取1.5/Kp到2/Kp之间。

4.4 知识体系总览

为了让你更直观地理解这一章的内容，我画了一张流程图。它展示了从目标速度生成到PID控制，再到抗积分饱和的完整链路。

这张图把整个流程串起来了。你看，目标速度经过平滑后进入PID控制器，PID输出给执行器，执行器有饱和限制，而抗积分饱和策略就是用来处理这个饱和问题的。它把执行器的状态反馈回来，修正积分项，形成一个闭环。

好了，这一章的内容就到这里。速度跟踪控制看起来简单，但真正做好，需要你对每个环节都有深刻的理解。尤其是抗积分饱和，很多新手容易忽略，但实际项目中它往往是决定系统稳定性的关键。

可可精品资料库

📚 课程目录

4. 基于PID的速度跟踪控制：目标速度生成、PID控制器设计、抗积分饱和

4.1 目标速度生成

4.2 PID控制器设计

4.3 抗积分饱和

4.4 知识体系总览