第四节：Stanley方法——横向误差与航向误差的协同控制

Stanley方法，说白了就是让车像老司机一样，同时盯着两个东西：车头朝向对不对，以及车身离参考线偏了多少。我最早接触这个算法是在2016年，当时做园区物流车项目，Pure Pursuit在低速还行，一上20km/h就开始画龙。后来换成Stanley，嗯，稳多了。

4.1 核心思想：前轮转角 = 航向误差 + 横向误差的非线性反馈

Stanley的控制律非常简洁，就一个公式：

δ(t) = θ_e(t) + arctan(k * e(t) / v(t))

其中：

δ(t) — 前轮转角（控制量）
θ_e(t) — 航向误差（车头方向与参考线切线方向的夹角）
e(t) — 横向误差（车辆前轴中心到参考线的垂直距离）
k — 增益系数（我一般取0.5~2.0之间）
v(t) — 当前车速

你想想看，这个公式妙在哪里？
航向误差是直接补偿，横向误差通过反正切函数做非线性映射。车速越快，横向误差的权重自动降低——这很符合直觉：高速时你不敢猛打方向。

关键洞察：Stanley本质上是一个状态反馈控制器，它把横向误差和航向误差统一映射到前轮转角上。与Pure Pursuit的几何预瞄不同，Stanley更接近现代控制理论中的李雅普诺夫设计法。

4.2 横向误差与航向误差的数学定义

这里我习惯用向量法来定义，避免坐标系搞混。假设参考线上最近点为P，车辆前轴中心为C：

横向误差 e(t)：向量CP在参考线法向上的投影。左偏为正，右偏为负。
航向误差 θ_e(t)：车辆纵轴与P点切线方向的夹角。车头偏左为正。

计算时有个坑：最近点的搜索。我曾经在项目中直接用欧氏距离遍历，结果在弯道处跳点，导致控制量突变。后来改用投影法+局部搜索窗口才解决。

避坑指南：我曾经在高速测试时发现横向误差突然跳变，排查了半天，原来是参考线采样不均匀导致的。建议参考线点间距不超过0.5米，且做平滑处理。

4.3 非线性反馈的物理意义

为什么用反正切？我解释给你听：

当横向误差很大时（比如1米），arctan(k*e/v) 趋近于 ±90°，给出最大转角——快速回正。
当横向误差很小时（比如0.05米），arctan(k*e/v) ≈ k*e/v，近似线性——微调即可。
车速v在分母上：高速时横向误差的贡献被压缩，避免过度转向。

这个非线性特性，说白了就是大误差大增益，小误差小增益。我做过对比实验：用线性反馈（直接比例）在弯道入口处容易超调，而Stanley的反正切曲线天然带有饱和特性，控制更平滑。

个人经验：k值的选取跟车辆轴距和最大转角有关。轴距3米的车，k=1.0是个不错的起点。如果发现车辆在直线上来回摆动，说明k太大了；如果弯道跟踪滞后明显，说明k太小。

4.4 与Pure Pursuit的对比分析

这两个方法我都在实车上跑过，下面这张表是我自己的总结：

对比维度	Stanley方法	Pure Pursuit
控制原理	状态反馈（横向+航向误差）	几何预瞄（圆弧拟合）
预瞄机制	无显式预瞄，依赖最近点	有预瞄距离Ld
高速表现	优秀（车速自适应）	一般（预瞄距离需随速调整）
弯道跟踪	平滑，无超调	可能内切或外切
参数调优	1个参数（k）	1个参数（Ld）
计算复杂度	低（只需最近点）	低（需预瞄点）
适用场景	结构化道路、高速公路	低速、越野、大曲率弯道

我个人认为：Stanley更适合高速场景，因为它的车速自适应是天然的；Pure Pursuit更适合低速大弯，因为预瞄机制可以提前规划路径。我在一个项目中把两者做了融合：低速用Pure Pursuit，高速切Stanley，效果还不错。

4.5 代码实现与调参建议

下面是一个简化的Stanley控制器实现，我删掉了滤波和异常处理，保留核心逻辑：

def stanley_control(vehicle_state, reference_path, k=1.0):
    # 1. 找到参考线上最近点
    nearest_idx = find_nearest_point(vehicle_state.position, reference_path)
    nearest_point = reference_path[nearest_idx]
    
    # 2. 计算横向误差（带符号）
    e = compute_lateral_error(vehicle_state, nearest_point)
    
    # 3. 计算航向误差
    heading_error = normalize_angle(
        vehicle_state.yaw - nearest_point.tangent_yaw
    )
    
    # 4. 计算前轮转角
    v = max(vehicle_state.speed, 0.1)  # 防止除零
    delta = heading_error + math.atan2(k * e, v)
    
    # 5. 限幅
    delta = clamp(delta, -MAX_STEER_ANGLE, MAX_STEER_ANGLE)
    
    return delta

调参时我有个习惯：先固定k=1.0，在直线上跑，观察横向误差的收敛速度。如果收敛太慢，增大k；如果出现振荡，减小k。然后在弯道上微调，一般k在0.8~1.5之间就能有不错的效果。

注意：代码中的find_nearest_point如果直接用暴力搜索，在长路径上会很慢。我建议用KD树或者分段线性搜索，实时性会好很多。

4.6 本章知识体系

下面这张图展示了Stanley方法的完整知识结构，我画了很久才理清楚：

嗯，到这里Stanley的核心内容就讲完了。我个人觉得，理解这个算法的关键在于想明白一件事：为什么反正切函数比线性比例更适合做横向控制？答案就在它的饱和特性——大误差时快速响应，小误差时精细调节，这种非线性特性天然适合车辆这种非完整约束系统。

最后一个小技巧：如果你发现Stanley在弯道出口处有轻微的超调，可以尝试在航向误差项上加一个小的阻尼项，比如 -kd * dθ_e/dt。我试过，效果立竿见影。

可可精品资料库

📚 课程目录

第四节：Stanley方法——横向误差与航向误差的协同控制

4.1 核心思想：前轮转角 = 航向误差 + 横向误差的非线性反馈

4.2 横向误差与航向误差的数学定义

4.3 非线性反馈的物理意义

4.4 与Pure Pursuit的对比分析

4.5 代码实现与调参建议

4.6 本章知识体系