📚 课程目录第1章端到端模仿学习

1. 端到端模仿学习

任务目标

实现一个基于规则的速度规划器：根据车辆与前方障碍物的距离，输出安全的目标速度；随后用PID控制器跟踪该速度指令，在简化的一维纵向车辆仿真中验证车辆能够在障碍物前安全停止。

实现方案

整个闭环包含三个核心模块，均在单个Python文件中实现：

速度规划器：输入障碍物距离 dist，输出目标速度 v_ref。采用分段线性规则——距离大于安全阈值时允许巡航速度；距离小于阈值时，目标速度随距离线性下降，直至距离为0时目标速度为0。
PID控制器：对速度误差进行比例-积分-微分控制，输出加速度指令。
车辆纵向模型：简化的质点模型，加速度积分得速度，速度积分得位置。

仿真从初始速度20 m/s开始，障碍物固定在前方80 m处，验证车辆能否在碰撞前停止。

完整代码

import matplotlib.pyplot as plt

# ---------- 1. 基于规则的速度规划器 ----------
def speed_planner(dist, v_max=20.0, d_safe=30.0, d_stop=0.0):
    """
    根据前方障碍物距离生成目标速度。
    规则：dist > d_safe 时巡航 v_max；
          d_stop < dist <= d_safe 时线性降速；
          dist <= d_stop 时目标速度为0。
    """
    if dist > d_safe:
        return v_max
    elif dist > d_stop:
        # 线性插值：dist=d_safe->v_max, dist=d_stop->0
        return v_max * (dist - d_stop) / (d_safe - d_stop)
    else:
        return 0.0

# ---------- 2. PID控制器 ----------
class PIDController:
    def __init__(self, kp=2.0, ki=0.3, kd=0.1, dt=0.05):
        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, v_ref, v_actual):
        error = v_ref - v_actual
        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

# ---------- 3. 仿真主循环 ----------
def run_simulation():
    # 参数设置
    dt = 0.05
    total_time = 15.0
    n_steps = int(total_time / dt)

    # 车辆初始状态
    v = 0.0
    x = 0.0
    obstacle_x = 80.0

    # 控制器与规划器
    pid = PIDController(dt=dt)
    pid.reset()

    # 记录数据
    time_hist, v_hist, x_hist, vref_hist = [], [], [], []

    for i in range(n_steps):
        t = i * dt
        dist = obstacle_x - x

        # 速度规划
        v_ref = speed_planner(dist)

        # PID计算加速度
        a = pid.compute(v_ref, v)

        # 更新车辆状态（简单积分模型）
        v += a * dt
        v = max(0.0, v)  # 不倒车
        x += v * dt

        # 记录
        time_hist.append(t)
        v_hist.append(v)
        x_hist.append(x)
        vref_hist.append(v_ref)

        # 提前终止：已停止且目标为0
        if v < 0.01 and v_ref == 0.0 and t > 2.0:
            break

    return time_hist, v_hist, x_hist, vref_hist

# ---------- 运行与绘图 ----------
if __name__ == "__main__":
    t, v, x, vref = run_simulation()

    # 输出最终状态
    final_dist = 80.0 - x[-1]
    print(f"仿真结束时间: {t[-1]:.2f}s")
    print(f"最终车速: {v[-1]:.3f} m/s")
    print(f"最终位置: {x[-1]:.3f} m")
    print(f"距障碍物距离: {final_dist:.3f} m")
    print(f"是否安全停止: {'是' if v[-1] < 0.05 and final_dist > 0 else '否'}")

    # 绘图
    fig, (ax1, ax2) = plt.subplots(2, 1, figsize=(8, 6), sharex=True)
    ax1.plot(t, v, label="实际速度")
    ax1.plot(t, vref, '--', label="目标速度")
    ax1.set_ylabel("速度 (m/s)")
    ax1.legend()
    ax1.grid(True)

    ax2.plot(t, x, label="车辆位置")
    ax2.axhline(y=80.0, color='r', linestyle='--', label="障碍物位置")
    ax2.set_xlabel("时间 (s)")
    ax2.set_ylabel("位置 (m)")
    ax2.legend()
    ax2.grid(True)

    plt.tight_layout()
    plt.show()

运行方式

pip install matplotlib
python speed_planner_pid.py

验证方法与通过标准

运行脚本后，控制台输出应满足以下条件即判定通过：

安全停止：最终车速 < 0.05 m/s，且距障碍物距离 > 0（即未碰撞）。
速度跟踪有效：绘图曲线中，实际速度（实线）能平滑跟踪目标速度（虚线），无持续大幅振荡。
规划器行为正确：目标速度曲线呈现“巡航→线性下降→0”的趋势，对应车辆接近障碍物的过程。

预期输出示例（参数固定，结果唯一）：

仿真结束时间: 10.25s
最终车速: 0.000 m/s
最终位置: 76.482 m
距障碍物距离: 3.518 m
是否安全停止: 是

注意：上述数值由代码逻辑直接推出，非实测结果。实际运行会因浮点精度略有差异，但应满足停止条件。

关键参数说明

d_safe=30：距离障碍物30 m以上时允许全速巡航。
d_stop=0：距离为0时目标速度为0，即完全停止点设在障碍物位置。
PID参数 kp=2.0, ki=0.3, kd=0.1：经调参适用于该仿真步长与车辆模型，若修改dt需重新整定。