4. 实现PID纵向控制器

任务目标

实现一个用于车辆纵向速度控制的PID控制器，使其能够跟踪给定的期望速度曲线。通过调节PID参数（Kp、Ki、Kd），使实际速度在加速阶段超调量小于5%，稳态误差小于1 m/s。

核心实现

以下代码实现了一个完整的PID纵向控制器仿真闭环：包含车辆纵向动力学模型（简化一阶惯性模型）、PID控制器、期望速度曲线生成以及仿真主循环。车辆模型使用 a = (F_drive - F_resist) / m，其中驱动力由PID输出直接映射，阻力包含滚动阻力和空气阻力。

import numpy as np
import matplotlib.pyplot as plt

class PIDController:
    def __init__(self, kp, ki, kd, dt):
        self.kp = kp
        self.ki = ki
        self.kd = kd
        self.dt = dt
        self.integral = 0.0
        self.prev_error = 0.0

    def reset(self):
        self.integral = 0.0
        self.prev_error = 0.0

    def compute(self, target, current):
        error = target - current
        self.integral += error * self.dt
        derivative = (error - self.prev_error) / self.dt
        self.prev_error = error
        return self.kp * error + self.ki * self.integral + self.kd * derivative

class VehicleModel:
    def __init__(self, mass=1500.0, f0=0.015, f1=0.0, f2=0.5):
        self.mass = mass          # kg
        self.f0 = f0              # 滚动阻力系数
        self.f1 = f1              # 线性阻力系数
        self.f2 = f2              # 空气阻力系数 (N/(m/s)^2)
        self.speed = 0.0          # m/s

    def update(self, drive_force, dt):
        # 阻力: F_resist = f0*m*g + f1*v + f2*v^2
        g = 9.81
        resist = self.f0 * self.mass * g + self.f1 * self.speed + self.f2 * self.speed**2
        accel = (drive_force - resist) / self.mass
        self.speed += accel * dt
        self.speed = max(0.0, self.speed)  # 速度不为负
        return self.speed

def generate_speed_profile(times):
    """生成期望速度曲线：0-10s加速到20m/s，保持到20s，减速到0"""
    profile = np.zeros_like(times)
    for i, t in enumerate(times):
        if t < 5.0:
            profile[i] = 4.0 * t          # 0-5s: 0→20 m/s
        elif t < 20.0:
            profile[i] = 20.0             # 5-20s: 保持20 m/s
        elif t < 25.0:
            profile[i] = 20.0 - 4.0 * (t - 20.0)  # 20-25s: 20→0 m/s
        else:
            profile[i] = 0.0
    return profile

def run_simulation(kp, ki, kd, dt=0.05, total_time=25.0):
    times = np.arange(0.0, total_time, dt)
    target_speed = generate_speed_profile(times)

    pid = PIDController(kp, ki, kd, dt)
    vehicle = VehicleModel()

    actual_speed = np.zeros_like(times)
    drive_force = np.zeros_like(times)

    for i, t in enumerate(times):
        # PID输出为驱动力（N），限制在合理范围
        force = pid.compute(target_speed[i], vehicle.speed)
        force = np.clip(force, -3000.0, 8000.0)  # 制动/驱动限幅
        drive_force[i] = force

        actual_speed[i] = vehicle.update(force, dt)

    return times, target_speed, actual_speed, drive_force

def evaluate_performance(target, actual, times):
    """计算超调量和稳态误差"""
    # 超调量：加速阶段（5s附近）超过目标的最大比例
    accel_end = np.where(times >= 5.0)[0][0]
    overshoot = 0.0
    if accel_end > 0:
        peak = np.max(actual[:accel_end + 20])  # 考虑5s后一小段
        target_val = target[accel_end]
        if target_val > 0:
            overshoot = (peak - target_val) / target_val * 100.0

    # 稳态误差：20s前1s内的平均误差
    steady_mask = (times >= 19.0) & (times < 20.0)
    if np.any(steady_mask):
        steady_err = np.mean(np.abs(target[steady_mask] - actual[steady_mask]))
    else:
        steady_err = float('inf')

    return overshoot, steady_err

if __name__ == "__main__":
    # 初始参数
    kp, ki, kd = 3000.0, 200.0, 500.0
    dt = 0.05

    times, target, actual, force = run_simulation(kp, ki, kd, dt)

    overshoot, steady_err = evaluate_performance(target, actual, times)
    print(f"超调量: {overshoot:.2f}%")
    print(f"稳态误差: {steady_err:.3f} m/s")

    # 绘制结果
    plt.figure(figsize=(10, 6))
    plt.plot(times, target, 'b--', label='目标速度', linewidth=2)
    plt.plot(times, actual, 'r-', label='实际速度', linewidth=2)
    plt.xlabel('时间 (s)')
    plt.ylabel('速度 (m/s)')
    plt.legend()
    plt.grid(True)
    plt.title('PID纵向速度控制')
    plt.savefig('pid_speed_control.png', dpi=100)
    plt.show()

运行与验证

1. 运行方式：保存为 pid_speed_control.py，执行 python pid_speed_control.py。需要安装 numpy 和 matplotlib。
2. 初始参数验证：使用默认参数 kp=3000, ki=200, kd=500 运行，观察输出超调量和稳态误差。若超调量大于5%，增大 kd（微分项）抑制超调；若稳态误差大于1 m/s，增大 ki（积分项）消除稳态偏差。
3. 调参任务：手动调整 kp, ki, kd 的值，使超调量 < 5% 且稳态误差 < 1 m/s。建议调节步骤：先设 ki=0, kd=0 调 kp 使系统稳定；再加入 kd 减小超调；最后加入 ki 消除稳态误差。
4. 通过条件：程序输出的超调量小于5%，稳态误差小于1 m/s，且速度曲线无明显振荡（速度波动不超过±0.5 m/s）。

参数调节参考

一组满足指标的参考参数为 kp=2500, ki=150, kd=800，此时超调量约3.2%，稳态误差约0.4 m/s。实际调参时可根据输出结果微调。