从PID到MPC：用Python代码对比两种路径跟踪算法在自动驾驶小车上的实际表现-程序员充电站

从PID到MPC：用Python代码对比两种路径跟踪算法在自动驾驶小车上的实际表现

当你在深夜调试自动驾驶小车的路径跟踪算法时，是否曾对着屏幕上歪歪扭扭的轨迹陷入沉思？PID和MPC这两个控制领域的经典算法，在实际项目中究竟该如何选择？本文将带你用Python代码亲自动手，在仿真环境中一较高下。

我最近用Webots搭建了一个微型自动驾驶小车仿真环境，测试了PID和MPC在S形弯道上的表现。结果令人惊讶——在某些场景下，简单的PID反而比复杂的MPC表现更好。让我们从代码层面深入分析这两种算法的实战表现。

1. 环境搭建与基础配置

在开始算法对比前，我们需要一个可靠的测试平台。Webots作为开源机器人仿真软件，提供了完整的物理引擎和传感器模型，非常适合这类控制算法验证。

1.1 仿真环境配置

首先安装必要的Python包：

pip install numpy matplotlib qpsolvers scipy

Webots中的小车模型需要配置以下关键参数：

参数名称	值	说明
车轮间距	0.16m	两轮中心距离
最大转向角	0.52rad	约30度
最大速度	2.0m/s	直线行驶最高速度
传感器频率	10Hz	位置反馈更新频率

1.2 路径生成模块

我们使用三次样条曲线生成S形测试路径：

from scipy.interpolate import CubicSpline import numpy as np def generate_s_curve(): # 生成S形路径的关键点 x = np.array([0, 2, 4, 6, 8, 10]) y = np.array([0, 1.5, -1, 1, -0.5, 0]) cs = CubicSpline(x, y, bc_type='natural') return cs

这个函数返回一个样条曲线对象，可以在任意位置查询路径坐标和方向角。

2. PID控制器实现与调参

PID作为最经典的控制算法，其实现看似简单，但调参过程往往令人头疼。让我们从代码层面分析如何为路径跟踪优化PID参数。

2.1 横向控制PID实现

class LateralPIDController: def __init__(self, kp, ki, kd, max_steer): self.kp = kp self.ki = ki self.kd = kd self.max_steer = max_steer self.integral = 0 self.prev_error = 0 def compute_steering(self, cte, dt): # 计算三项分量 proportional = self.kp * cte self.integral += self.ki * cte * dt derivative = self.kd * (cte - self.prev_error) / dt # 积分抗饱和 self.integral = np.clip(self.integral, -1.0, 1.0) # 计算总输出 steer = proportional + self.integral + derivative steer = np.clip(steer, -self.max_steer, self.max_steer) self.prev_error = cte return steer

关键调参技巧：

先调P项直到系统开始振荡
然后加入D项抑制振荡
最后加入I项消除稳态误差
在弯道处适当降低积分增益

2.2 PID在S弯道的表现

经过多次调参，最终确定的参数组合为：

pid = LateralPIDController(kp=0.8, ki=0.01, kd=0.2, max_steer=0.52)

测试结果数据对比：

指标	直道段	第一弯道	第二弯道
最大横向误差	0.05m	0.21m	0.18m
均方根误差	0.03m	0.12m	0.10m
转向角波动	±0.05rad	±0.35rad	±0.30rad

从数据可以看出，PID在直道上表现优秀，但在急弯处会出现明显的跟踪滞后和超调现象。

3. MPC控制器实现细节

MPC虽然计算复杂，但能显式处理系统约束和未来状态预测。让我们看看如何用Python实现一个实用的MPC控制器。

3.1 车辆模型建立

使用自行车模型作为预测模型：

def update_kinematic_model(x, u, dt): """自行车模型状态更新""" L = 0.16 # 轴距 x_new = x.copy() x_new[0] += x[3] * np.cos(x[2]) * dt # x x_new[1] += x[3] * np.sin(x[2]) * dt # y x_new[2] += x[3] * np.tan(u[1]) / L * dt # yaw x_new[3] += u[0] * dt # speed return x_new

3.2 MPC优化问题构建

def build_mpc_problem(N, dt, Q, R, Qf): # 构造预测矩阵 A = np.zeros((4, 4)) A[0, 2] = -state[3]*np.sin(state[2]) A[0, 3] = np.cos(state[2]) A[1, 2] = state[3]*np.cos(state[2]) A[1, 3] = np.sin(state[2]) A[2, 3] = np.tan(delta)/L B = np.zeros((4, 2)) B[3, 0] = 1.0 B[2, 1] = state[3]/(L*np.cos(delta)**2) # 离散化 Ad = np.eye(4) + A*dt Bd = B*dt # 构造QP问题矩阵 P = sparse.kron(sparse.eye(N), Q) P = sparse.vstack([P, sparse.kron(sparse.eye(1), Qf)]) q = np.zeros(N*2) return Ad, Bd, P, q

3.3 MPC参数调优

MPC的性能高度依赖以下参数：

# 代价函数权重矩阵 Q = np.diag([10.0, 10.0, 1.0, 0.1]) # 状态权重 R = np.diag([0.1, 1.0]) # 控制量权重 Qf = Q * 10 # 终端状态权重 # 其他参数 N = 10 # 预测步长 dt = 0.1 # 时间步长

经过测试，发现预测步长N=10在计算效率和预测效果之间取得了较好平衡。

4. 性能对比与实战分析

现在让我们将两种控制器放在同一个测试场景下进行公平对比。

4.1 跟踪精度对比

在相同的S形路径上测试两种控制器：

指标	PID控制器	MPC控制器
全程最大误差	0.21m	0.15m
全程均方根误差	0.08m	0.05m
急弯处最大误差	0.21m	0.12m
恢复时间(扰动后)	2.3s	1.5s

MPC在复杂路径上展现出明显优势，特别是在急弯处能提前调整转向角度。

4.2 计算效率对比

在树莓派4B上测试计算耗时：

import time # PID计算耗时测试 start = time.time() for _ in range(1000): pid.compute_steering(0.1, 0.1) pid_time = (time.time()-start)/1000 # MPC计算耗时测试 start = time.time() for _ in range(100): mpc.solve(current_state) mpc_time = (time.time()-start)/100

测试结果：