3. Stanley横向控制与路径跟踪

任务目标

实现Stanley横向控制算法，在给定参考路径（含不同曲率路段）上完成路径跟踪仿真，并与纯跟踪（Pure Pursuit）算法进行横向误差对比。通过仿真曲线直观评估两种算法在弯道处的跟踪精度差异。

核心原理与实现

Stanley算法以车辆前轴中心为控制点，将横向误差e和前轮转角δ建立非线性比例关系：

δ = ψ_e + arctan(k·e / v)

其中ψ_e为航向角误差（车辆航向与最近路径点切线夹角），v为纵向速度，k为增益系数。纯跟踪算法则基于车辆后轴中心，通过前瞻距离L_d在路径上选取目标点，计算圆弧曲率得到前轮转角。

仿真环境与依赖

使用Python 3.8+，依赖NumPy和Matplotlib。无需ROS或车辆动力学库，采用简化的自行车运动学模型：

# 自行车模型更新
x += v * cos(yaw) * dt
y += v * sin(yaw) * dt
yaw += v / L * tan(delta) * dt

完整实现代码

import numpy as np
import matplotlib.pyplot as plt

# ========== 车辆参数 ==========
L = 2.5          # 轴距(m)
v = 5.0          # 纵向速度(m/s)
dt = 0.05        # 仿真步长(s)

# ========== 参考路径生成（含不同曲率） ==========
def generate_reference_path():
    t = np.linspace(0, 8*np.pi, 1000)
    # 组合正弦曲线：低频段+高频段，形成曲率变化
    x = t
    y = 3.0*np.sin(0.5*t) + 1.5*np.sin(1.5*t)
    return np.vstack((x, y)).T

# ========== 最近路径点查找 ==========
def find_nearest_point(path, pos):
    dist = np.linalg.norm(path - pos, axis=1)
    return np.argmin(dist)

# ========== Stanley 控制器 ==========
def stanley_control(path, pos, yaw, k=1.0):
    idx = find_nearest_point(path, pos)
    nearest = path[idx]
    
    # 航向角误差
    dx = path[min(idx+1, len(path)-1)] - nearest
    path_yaw = np.arctan2(dx[1], dx[0])
    yaw_error = path_yaw - yaw
    yaw_error = np.arctan2(np.sin(yaw_error), np.cos(yaw_error))
    
    # 横向误差（带符号）
    vec_veh = np.array([np.cos(yaw), np.sin(yaw)])
    vec_err = pos - nearest
    cross = vec_veh[0]*vec_err[1] - vec_veh[1]*vec_err[0]
    lateral_error = cross
    
    # 前轮转角
    delta = yaw_error + np.arctan2(k*lateral_error, v)
    return np.clip(delta, -0.6, 0.6), idx

# ========== 纯跟踪控制器 ==========
def pure_pursuit_control(path, pos, yaw, Ld=3.0):
    # 寻找前瞻点
    dist = np.linalg.norm(path - pos, axis=1)
    candidates = np.where(dist >= Ld)[0]
    if len(candidates) == 0:
        target_idx = len(path)-1
    else:
        target_idx = candidates[0]
    target = path[target_idx]
    
    # 计算转角
    alpha = np.arctan2(target[1]-pos[1], target[0]-pos[0]) - yaw
    alpha = np.arctan2(np.sin(alpha), np.cos(alpha))
    delta = np.arctan2(2*L*np.sin(alpha), Ld)
    return np.clip(delta, -0.6, 0.6)

# ========== 仿真主函数 ==========
def simulate(controller, path, k=1.0, Ld=3.0):
    pos = np.array([path[0,0], path[0,1]+0.5])  # 初始横向偏移
    yaw = 0.0
    traj = [pos.copy()]
    errors = []
    
    for _ in range(2000):
        if controller == 'stanley':
            delta, _ = stanley_control(path, pos, yaw, k)
        else:
            delta = pure_pursuit_control(path, pos, yaw, Ld)
        
        # 更新运动学模型
        pos[0] += v * np.cos(yaw) * dt
        pos[1] += v * np.sin(yaw) * dt
        yaw += v / L * np.tan(delta) * dt
        yaw = np.arctan2(np.sin(yaw), np.cos(yaw))
        
        traj.append(pos.copy())
        
        # 记录横向误差
        idx = find_nearest_point(path, pos)
        nearest = path[idx]
        vec_veh = np.array([np.cos(yaw), np.sin(yaw)])
        vec_err = pos - nearest
        cross = vec_veh[0]*vec_err[1] - vec_veh[1]*vec_err[0]
        errors.append(abs(cross))
        
        # 终止条件：接近终点
        if np.linalg.norm(pos - path[-1]) < 0.5:
            break
    
    return np.array(traj), np.array(errors)

# ========== 运行仿真 ==========
path = generate_reference_path()

# Stanley 仿真
traj_s, err_s = simulate('stanley', path, k=1.0)
# 纯跟踪仿真
traj_p, err_p = simulate('pure_pursuit', path, Ld=3.0)

# ========== 可视化 ==========
fig, (ax1, ax2) = plt.subplots(2, 1, figsize=(10, 8))

# 路径跟踪对比
ax1.plot(path[:,0], path[:,1], 'k--', linewidth=2, label='参考路径')
ax1.plot(traj_s[:,0], traj_s[:,1], 'b-', linewidth=1.5, label='Stanley')
ax1.plot(traj_p[:,0], traj_p[:,1], 'r-', linewidth=1.5, label='纯跟踪')
ax1.set_xlabel('X (m)')
ax1.set_ylabel('Y (m)')
ax1.set_title('路径跟踪对比')
ax1.legend()
ax1.grid(True)
ax1.axis('equal')

# 横向误差对比
ax2.plot(err_s, 'b-', label='Stanley')
ax2.plot(err_p, 'r-', label='纯跟踪')
ax2.set_xlabel('仿真步数')
ax2.set_ylabel('横向误差 (m)')
ax2.set_title('横向误差随时间变化')
ax2.legend()
ax2.grid(True)

plt.tight_layout()
plt.show()

# 输出统计指标
print(f"Stanley 平均横向误差: {np.mean(err_s):.3f} m")
print(f"纯跟踪 平均横向误差: {np.mean(err_p):.3f} m")
print(f"Stanley 最大横向误差: {np.max(err_s):.3f} m")
print(f"纯跟踪 最大横向误差: {np.max(err_p):.3f} m")

运行与验证

运行方式：将上述代码保存为tracking_compare.py，在已安装NumPy和Matplotlib的Python环境中执行python tracking_compare.py。

预期输出：程序将显示两个子图——上图为两种算法的跟踪轨迹与参考路径对比，下图为横向误差随时间的变化曲线。控制台输出两种算法的平均和最大横向误差统计值。

通过判定标准：

轨迹图中，Stanley算法（蓝线）在弯道处应比纯跟踪（红线）更贴近参考路径（黑色虚线），尤其在曲率变化较大的区段（如正弦波峰/波谷处）差异明显；
误差曲线图中，Stanley的蓝色曲线整体应低于纯跟踪的红色曲线，且波动幅度更小；
控制台输出的Stanley平均横向误差应小于纯跟踪的平均横向误差。

参数调优建议

若对比效果不明显，可调整以下参数：

增大Stanley增益k（如从1.0调至2.0）可加快收敛，但过大会引起振荡；
纯跟踪的前瞻距离Ld与速度相关，当前速度5m/s下Ld=3.0偏小，可尝试Ld=5.0观察跟踪平滑度变化；
初始横向偏移0.5m用于测试收敛能力，可改为0或1.0观察不同初始误差下的表现差异。

📚 课程目录