自动驾驶控制算法——PID算法

自动驾驶控制算法——PID算法

一、PID 是什么？

PID 控制器是比例（Proportional）、积分（Integral）和微分（Derivative）**三种控制作用的组合，常用于工业过程控制系统中，是最经典、最常用的一种反馈控制算法。在**自动驾驶系统中，PID 控制被广泛用于速度控制（纵向控制）和路径跟踪（横向控制）任务，因其算法简单、响应迅速、适应性强等优势，尤其适合低速或规则场景下的车辆运动控制。

二、PID 原理

PID 控制的核心思想是通过误差反馈控制系统输入，进而使输出趋近于目标值。设目标值为 $r(t)$，实际输出为 $y(t)$，误差为：

$e(t)=r(t)-y(t)$

PID 控制器输出为：

$u(t)=K_{P}e(t)+K_I{\int }_0^{t}e(\tau )d\tau +K_D\frac{de(t)}{dt}$

2.1 比例环节（P）

比例项控制量：

$u_P(t)=K_{P}e(t)$

作用：对当前误差做出立即反应，误差越大，控制量越大，反应迅速。但单独使用时存在稳态误差问题。

2.2 积分环节（I）

积分项控制量：

$u_I(t)=K_I{\int }_0^{t}e(\tau )d\tau$

作用：消除稳态误差，长时间小误差也能被积累并控制系统。但积分作用过强会导致系统震荡或超调。

2.3 微分环节（D）

微分项控制量：

$u_D(t)=K_D\frac{de(t)}{dt}$

作用：预测误差变化趋势，在误差快速变化时产生抑制作用，可有效减小系统超调和响应振荡，但对噪声敏感。

2.4 特点总结

• 响应快（比例项）
• 可消除稳态误差（积分项）
• 抑制振荡（微分项）
• 适用于线性、慢变量控制系统

2.5 案例分析 —— 小车巡线控制

为了帮助读者更好地理解 PID 三项调参的意义，这里我们通过另一个典型案例：“小车巡线”来演示如何使用 PID 进行控制。

2.5.1 任务介绍

我们设想这样一个场景：

• 有一辆小车在道路中间的黑色轨迹线上巡航
• 小车可以左右转向（控制方向盘），根据摄像头识别出的横向偏差来判断是否偏离轨道
• 目标：让小车始终保持在线上行驶，尽可能贴近轨迹中心

2.5.2 控制流程

1. 摄像头图像处理后，输出一个横向误差（小车中心与轨迹中心的距离）
2. 使用 PID 控制器根据误差值计算方向盘角度
3. 调节转向，让小车回归轨道中心

2.5.3 调参现象观察

2.5.4 常见 PID 参数组合效果

1. 只调比例 $K_P$：
   小车来回摆动，频繁冲出轨迹后再回中间（类弹簧）
2. 加了微分 $K_D$：
   小车弯道更加平滑，不会冲太远；转弯更加提前（提前“察觉”误差）
3. 加了积分 $K_I$​：
   小车在某些长期偏离的一侧能逐渐拉回，但过大会“过头”或“死机”

2.6 PID参数介绍

在前文的“小球控制”与“小车巡线”案例中，我们从直观物理场景出发，形象地理解了 PID 控制器的三大组成部分（比例、积分、微分）在控制过程中的作用。现在，我们从更专业的角度回顾这些概念，结合自动控制领域的标准术语，对 PID 的系统结构与性能做进一步理解。

2.6.1 常用术语

2.6.2阶跃输入与响应分析

在系统响应性能分析中，一个重要方法是通过**阶跃输入（step input）**来评估系统特性。

阶跃输入

• 指系统在稳定状态下，目标值突然发生变化
• 例如小球的目标位置从 0 米跃升至 1 米，或小车巡线轨道突然向右偏移

阶跃响应

• 指系统输出随时间变化的响应曲线，用于评估 PID 控制器调节后的控制性能
• 不同参数下，系统的响应行为也不同（快慢、是否振荡等）

上图展示了常见的三种阶跃响应曲线，用于帮助理解不同 PID 参数组合对系统性能的影响：

• 🔵 蓝线：响应快但超调大
  → 表示系统具有较强的比例项（$K_P$​大），但缺乏微分抑制，导致响应迅速但振荡明显。
• 🟢 绿线：响应慢但平稳
  → 表示一个过阻尼系统，PID 参数设置保守，响应慢但没有超调，适合对稳定性要求高的系统。
• 🔴 红线：超调小但响应时间长
  → 接近临界阻尼状态，系统调节较慢，但几乎不超调，适合对精度要求高的应用。

2.6.3 阶跃响应关键性能指标

2.6.4 PID 的计算与信号流程图

我们可以将 PID 的运行过程用下图表示为信号控制流：

2.6.5 PID 三项在响应中的典型作用总结

2.6.6 PID 与物理量之间的对应关系

在真实控制系统中，我们要为 PID 控制器指定对应的三个物理量：目标值、反馈值、输出值。如何选择？下面是实用经验：

设计准则

1. 目标值 & 反馈值： 一般为你要控制的“主量”，如位置、速度、温度等

2. 输出值： 控制系统施加的作用量（如力、电流、PWM占空比）

3. 物理维度关系： 输出值应是反馈值对时间的低阶物理量

   目标 & 反馈值	输出量应为
   位置（m）	速度（m/s） 或 加速度（m/s²）
   速度（m/s）	加速度（m/s²）或力
   角度（rad）	角速度（rad/s）或转矩

✅ 实际中，只要输出量经过系统作用能引起反馈值变化，PID 就可以工作；即便不是理想线性关系，也能通过参数整定达到良好效果。

三、PID 的分类

3.1 模拟 PID

• 传统的电路实现（如运放、RC 电路）
• 响应快，但不易调节参数
• 一般用于简单控制器或老旧系统

3.2 数字 PID

• 通过微控制器（如 STM32、Arduino）或嵌入式系统实现
• 易于调参，可加滤波处理
• 可结合自动驾驶感知/规划模块实现软硬件闭环控制

3.3 特点比较

四、PID 代码实现

在自动驾驶系统中，PID 控制器可以通过编程语言（如 C、Python、MATLAB）实现两种主要形式：

4.1 位置型 PID（Position-Type PID）

原理：
位置型 PID 直接计算“当前的控制输出” $u(t)$，等于比例、积分、微分三部分之和，适用于绝对控制系统。

公式：

$u(t)=K_{P}e(t)+K_I{\sum }_{i=0}^{t}e(i)\Delta t+K_D\frac{e(t)-e(t-1)}{\Delta t}$

优点： 思路直观，适合控制信号连续的系统
缺点： 控制值会直接累积误差，可能造成输出跳变

Python 示例代码（以速度控制为例）：

class PIDController:
    def __init__(self, Kp, Ki, Kd, dt):
        self.Kp = Kp
        self.Ki = Ki
        self.Kd = Kd
        self.dt = dt
        self.integral = 0
        self.prev_error = 0

    def compute(self, setpoint, actual):
        error = setpoint - actual
        self.integral += error * self.dt
        derivative = (error - self.prev_error) / self.dt
        output = self.Kp * error + self.Ki * self.integral + self.Kd * derivative
        self.prev_error = error
        return output

自动驾驶应用：

• 横向控制：根据横向偏差输出转向角
• 纵向控制：根据速度误差输出油门/刹车控制值

4.2 增量型 PID（Incremental-Type PID）

原理：
增量型 PID 只计算当前输出的变化量（增量），即：

$\Delta u(t)=K_P[e(t)-e(t-1)]+K_{I}e(t)\Delta t+K_D\left[e(t)-2e(t-1)+e(t-2)\right]/\Delta t$

再通过：

$u(t)=u(t-1)+\Delta u(t)$

优点： 控制平稳、避免输出突变，适合数字系统
缺点： 初始值和历史误差依赖较大

Python 示例代码：

class IncrementalPID:
    def __init__(self, Kp, Ki, Kd, dt):
        self.Kp = Kp
        self.Ki = Ki
        self.Kd = Kd
        self.dt = dt
        self.e1 = 0  # e(t-1)
        self.e2 = 0  # e(t-2)
        self.output = 0

    def compute(self, setpoint, actual):
        e0 = setpoint - actual
        delta = self.Kp * (e0 - self.e1) \
              + self.Ki * e0 * self.dt \
              + self.Kd * (e0 - 2 * self.e1 + self.e2) / self.dt
        self.output += delta
        self.e2 = self.e1
        self.e1 = e0
        return self.output

自动驾驶应用：

• 特别适用于控制器输出范围受限的系统（如 PWM 占空比控制电机）
• 更适合在嵌入式系统（STM32、ROS 控制器）中实现

五、PID 调试方法（参数整定）

为了让 PID 控制器在自动驾驶中表现良好（如转弯不漂移、速度不震荡），我们需要合理设置 KPK_PKP、KIK_IKI、KDK_DKD 参数。以下是四种常见的调参方法：

5.1 🔁 Ziegler–Nichols 临界响应法

步骤：

1. 将 $K_I=K_D=0$，逐步增加$K_P$ 直到系统出现持续震荡，记录此时的增益为 $K_u$，振荡周期为 $T_u$
2. 根据下表设置 PID 参数：

优点： 简单有效，适用于较线性的系统
缺点： 对系统存在延迟的情况效果较差

5.2 Cohen-Coon 法（阶跃响应模型拟合法）

📌 基本思想

Cohen-Coon 方法是基于过程响应建模的整定法，适用于一阶惯性 + 纯滞后系统（FOPDT）。主要用于慢响应系统，如：

• 自动驾驶车辆的电机驱动响应
• 线控转向系统中存在滞后的执行器

🧠 模型假设

系统阶跃响应可以近似为如下形式的过程：

$G(s)=\frac{K{e}^{-Ls}}{\tau s+1}$

其中：

🔍 操作步骤

1. 对控制对象施加阶跃输入（如设定速度从 0 → 1 m/s）

2. 记录输出响应曲线，拟合参数 $K,L,\tau$

   • $K=\frac{y_{\infty }}{u_0}$
   • $L$ 为响应开始变化所需时间
   • $\tau$为系统响应从 0% 到 63% 的时间减去$L$

3. 代入 Cohen-Coon 表格公式估算 PID 参数：

   设 $\theta =L/\tau$

   控制器	$K_P$	$T_I$	$T_D$
   P	$\frac{1}{K}\cdot \left(\frac{\tau }{L}\cdot \left(1+\frac{L}{3\tau }\right)\right)$	-	-
   PI	同上	$L\cdot \left(\frac{30+3\theta }{9+20\theta }\right)$	-
   PID	同上	同上	$L\cdot \left(\frac{4}{11+2\theta }\right)$

🚗 自动驾驶应用案例

• 控制对象：电机或伺服转向电机的响应过程
• 优势：能考虑系统延迟特性，特别适合低速精细控制
• 实用：用于车辆启动/停止时调速，或慢速避障微调

5.3 📉 频率响应法（Phase Margin Method）

📌 基本思想

• 从频域角度分析系统稳定性
• 核心目标是设计一个 PID 控制器，使系统具有合适的：
  • 相位裕度（Phase Margin）：防止振荡
  • 增益裕度（Gain Margin）：防止超调失控

🧠 背景原理

系统的开环传递函数为：

$G_{open}(s)=C(s)G(s)$

我们通过 Bode 图 或 Nyquist 图 观察开环系统的频率响应，调节 PID 控制器参数使得：

• 相位裕度 ≥ 45°
• 增益裕度 ≥ 6 dB
• 截止频率 ${\omega }_c$ 位于响应带宽范围内

🔍 操作步骤

1. 线性化目标系统（如车辆动力学模型）
2. 绘制开环频率响应（Bode图）
3. 调节 PID 参数：
   • 增大 $K_P$：提高增益，增大带宽
   • 调整 $K_D$：补偿相位，提高相位裕度
   • 适当调 $K_I$：引入低频增益，提高稳态性能
4. 目标设计：
   • $\text{PM}>45^{\circ }$
   • $\text{dB}GM>6dB$
   • ${\omega }_c$ 满足响应时延要求

🛠 工具推荐

• MATLAB：使用 bode(), margin(), pidtune() 工具箱
• Python：使用 control 或 matplotlib + scipy.signal

🚗 自动驾驶应用案例

• 控制对象：车辆线控转向系统、LQR 状态反馈前的低层调试
• 特别适合 高精度场景：如高速自动驾驶（L3/L4级）需控制震荡、确保稳定性
• 可用于设计 鲁棒控制器，适应外部扰动（风、路面）

5.4 经验调参法（推荐初学者）

调参流程建议如下：

1. 调 $K_P$：
   • 从小开始，逐步增大，直到出现微震
2. 调 $K_D$：
   • 增加 D 抑制震荡，减少超调
3. 调 $K_I$：
   • 最后调整积分，消除稳态误差，但防止积分饱和

🚗 实践建议：

• 横向控制一般设置$K_P=0.5\sim 1.5$，$K_D$ 略大，$K_I$ 极小或为 0
• 纵向控制中，$K_I$ 有助于稳定速度；PID 三项比例适中即可

六、实例分析：电机开环 vs 闭环控制

为了进一步理解 PID 在实际工程中的应用效果，本节我们通过一个非常典型的场景：电机速度控制，来对比分析开环系统与闭环系统的区别，并说明 PID 在其中的关键作用。

该控制方式广泛应用于：

• 自动驾驶车辆中的轮毂电机驱动
• 机器人底盘速度控制
• 无人小车的直线巡航与加减速控制等

6.1 任务场景

我们需要控制一个直流电机，使其保持在期望的速度（如 1.5 m/s）进行巡航。

控制目标：

• 目标值：期望速度（设定值）
• 反馈值：电机当前速度（通过编码器实时测量）
• 控制输出：PWM 占空比，作用于驱动器控制电机电压（或力）

6.2 什么是开环控制？

定义： 开环控制系统中，控制器根据目标值直接输出控制量，而不依赖被控对象的反馈值。

🎯 特点：

🚫 举例说明：

目标速度 = 1.5 m/s → 控制器直接设定 PWM = 60%

假设当前电机空载时达到目标速度刚好需要 60% 占空比。但如果负载加重（如上坡或轮胎打滑），此占空比就不足以维持速度，电机速度将下降，且系统无法自行修正。

6.3 什么是闭环控制（反馈控制）？

定义： 闭环系统中，控制器会根据当前实际反馈值和目标值之间的误差进行调节，从而实现精准控制。

这正是 PID 控制器 适用的场景。

🎯 特点：

6.4闭环系统的工作原理（以 PID 为例）

当前速度 = 1.2 m/s（通过编码器读取）

→ 误差 e = 0.3 m/s  
→ PID 计算控制输出 u  
→ u = PWM 占空比调整到 65%  
→ 电机加速，反馈值提升  
→ 误差减小，控制输出自动回调
→ 最终稳定在目标速度

这是一种动态修正、自动趋近目标的过程。

🔄 比较总结：开环 vs 闭环控制

结果说明

• 上图（速度响应）：
  • 红色虚线为开环速度：无法精确达到目标值
  • 蓝色实线为 PID 闭环控制：快速稳定逼近目标值，并自动调整
• 下图（控制输出）：
  • 开环控制保持恒定 PWM
  • 闭环 PID 输出不断调整 PWM，使速度保持在目标附近

✅ 总结

PID 控制器以其结构简单、响应快速的特点，成为自动驾驶初期控制算法的基础选择。无论是速度控制还是路径跟踪，PID 在中低速、规则道路中效果显著。但在面对非线性、高速、复杂交通环境时，需进一步引入如MPC、LQR、强化学习等高级控制算法进行协同控制。