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

2. 端到端模仿学习

任务目标

实现纯跟踪（Pure Pursuit）横向控制算法，使车辆模型沿给定参考路径行驶。通过调整前视距离参数，观察并评估横向跟踪误差的变化规律。

核心原理

纯跟踪算法以车辆后轴中心为基准，在参考路径上寻找距车辆前视距离 Ld 处的目标点，通过计算车辆当前航向与该目标点方向的角度差，输出前轮转角指令，使车辆沿圆弧逼近目标点。前视距离是唯一可调参数，直接影响跟踪精度与稳定性。

实现代码

import numpy as np
import matplotlib.pyplot as plt

class PurePursuitController:
    def __init__(self, L=2.5, Ld=5.0, k=0.5):
        self.L = L          # 轴距 (m)
        self.Ld = Ld        # 前视距离 (m)
        self.k = k          # 前视距离速度系数

    def set_velocity(self, v):
        # 前视距离随速度自适应调整
        self.Ld = self.k * v + 2.0

    def find_target_point(self, path, pos):
        # 在路径上寻找距当前位置约Ld的目标点
        distances = np.linalg.norm(path - pos, axis=1)
        idx = np.argmin(np.abs(distances - self.Ld))
        return path[idx]

    def compute_steering(self, path, pos, yaw):
        target = self.find_target_point(path, pos)
        dx = target[0] - pos[0]
        dy = target[1] - pos[1]
        alpha = np.arctan2(dy, dx) - yaw
        # 纯跟踪转角公式
        delta = np.arctan2(2 * self.L * np.sin(alpha), self.Ld)
        return delta, target

def simulate(path, controller, v=5.0, dt=0.1, steps=500):
    # 车辆运动学模型（后轴中心）
    x, y, yaw = path[0][0], path[0][1], 0.0
    traj = []
    errors = []
    for _ in range(steps):
        controller.set_velocity(v)
        delta, target = controller.compute_steering(path, np.array([x, y]), yaw)
        # 运动学更新
        x += v * np.cos(yaw) * dt
        y += v * np.sin(yaw) * dt
        yaw += (v / controller.L) * np.tan(delta) * dt
        traj.append((x, y))
        # 计算横向误差（到最近路径点的距离）
        dist = np.linalg.norm(path - np.array([x, y]), axis=1)
        errors.append(np.min(dist))
        # 到达终点附近则停止
        if np.linalg.norm(np.array([x, y]) - path[-1]) < 1.0:
            break
    return np.array(traj), np.array(errors)

# 参考路径：圆形轨迹
theta = np.linspace(0, 2*np.pi, 200)
radius = 20.0
path = np.column_stack([radius * np.cos(theta), radius * np.sin(theta)])

# 测试不同前视距离
for Ld_val in [3.0, 5.0, 8.0]:
    controller = PurePursuitController(Ld=Ld_val)
    traj, errors = simulate(path, controller)
    print(f"Ld={Ld_val}m: 平均误差={np.mean(errors):.3f}m, 最大误差={np.max(errors):.3f}m")

# 可视化（取Ld=5.0的结果）
controller = PurePursuitController(Ld=5.0)
traj, errors = simulate(path, controller)
plt.figure(figsize=(10, 4))
plt.subplot(121)
plt.plot(path[:,0], path[:,1], 'b--', label='参考路径')
plt.plot(traj[:,0], traj[:,1], 'r-', label='实际轨迹')
plt.legend(); plt.axis('equal'); plt.title('轨迹跟踪')
plt.subplot(122)
plt.plot(errors)
plt.xlabel('时间步'); plt.ylabel('横向误差 (m)'); plt.title('横向误差变化')
plt.tight_layout(); plt.show()

运行与验证

依赖安装：需安装 numpy 和 matplotlib，执行 pip install numpy matplotlib。
运行方式：将代码保存为 pure_pursuit.py，执行 python pure_pursuit.py。
预期输出：控制台打印三组误差数据，并弹出轨迹跟踪与误差曲线图。
通过标准：
- 轨迹图中红色实际轨迹与蓝色参考路径基本重合，无发散；
- 横向误差曲线收敛且保持有界；
- 对比三组数据可见：Ld 过小（3m）时误差波动大，过大（8m）时稳态误差增大，Ld=5m 时跟踪效果最佳。

参数调优建议

前视距离 Ld 与车速正相关。低速时取较小值可提高跟踪精度，高速时需增大前视距离以维持稳定性。实际应用中常采用 Ld = k·v + Ld_min 的自适应策略，代码中已内置该机制，可通过修改 k 值观察不同速度下的表现。

上图展示了纯跟踪算法的几何关系：车辆后轴中心为基准，沿当前航向前方距离 Ld 处确定目标点，通过转角 δ 使车辆沿预测圆弧逼近目标点。圆弧半径由轴距 L 和转角共同决定。