自动驾驶控制算法——Stanley 控制器

自动驾驶控制算法——Stanley 控制器

一、Stanley 控制器是什么？

---

1.1 定义与核心思想

Stanley 控制器是一种用于车辆横向控制（Lateral Control）的路径跟踪算法，由斯坦福大学在 2005 年 DARPA Grand Challenge 自动驾驶比赛中提出并成功应用，其搭载的“Stanley”无人车获得冠军，因此该方法得名。

其核心思想是通过同时修正航向误差与横向误差，使车辆前轮中心点尽量贴合参考路径。控制律利用车辆速度自适应地调整横向修正力度，高速时抑制过度转向，低速时提高响应速度，从而在不同速度下保持稳定的路径跟踪性能。

Stanley 控制器的控制律为：

$\delta =e_{\theta }+\arctan \left(\frac{k\cdot e_y}{v}\right)$

其中：

• $\delta$：转向角（Steering Angle）
• $e_{\theta }$：航向误差（Heading Error）
• $e_y$：横向误差（Cross-Track Error）
• $k$：增益系数（Stanley Gain）
• $v$：车辆速度（Vehicle Speed）

直观理解：

• 第一项 $e_{\theta }$ 负责对准路径方向；
• 第二项 $\arctan \left(\frac{k{e}_y}{v}\right)$ 负责回到路径中心，且随车速变化自动调整修正幅度。

---

1.2 Stanley 控制器与传统横向控制（如 Pure Pursuit、PID）的区别

对比维度	Stanley 控制器	纯跟踪法（Pure Pursuit）	PID 横向控制
误差类型	同时考虑航向误差与横向误差	主要基于几何圆弧拟合，考虑位置误差	直接用偏差计算控制量
速度适应性	自带速度自适应机制（低速修正大，高速修正小）	无速度自适应，需额外调节	无速度自适应，需额外调节
实现复杂度	简单（仅需计算两个误差）	简单	简单，但需调三个参数
高速稳定性	较好（速度项抑制过度转向）	高速易抖动	高速时可能超调大
低速响应性	响应快	响应快	取决于 PID 参数

---

1.3 自动驾驶中 Stanley 控制器的典型应用场景

1. 轨迹跟踪（Trajectory Tracking）
   在自动驾驶路径跟踪中，Stanley 控制器常用于将车辆的实际轨迹与规划轨迹进行对齐，确保车辆沿规划路线平稳行驶。
2. 车道保持（Lane Keeping）
   在高速公路或城市道路上，Stanley 控制器可保持车辆在车道中心行驶，即使车道有轻微弯曲，也能通过连续修正航向和横向误差保持稳定。
3. 低速自动驾驶平台
   适用于自动送货车、巡逻车、园区摆渡车等低中速场景，算法简单、计算量低，方便嵌入式系统实时运行。
4. 仿真验证与教学
   许多自动驾驶仿真平台（如 Carla、MATLAB Automated Driving Toolbox）内置 Stanley 控制器作为横向控制基线算法，便于研究人员和工程师进行算法比较与验证。

二、Stanley 控制器的原理（参考图文详解）

---

2.1 车辆运动学模型与误差定义

Stanley 控制器的推导一般基于 单轨（自行车）运动学模型（Kinematic Bicycle Model），该模型将四轮车辆简化为前后两个虚拟轮轴，忽略悬挂与车身侧倾等高阶动力学影响，适合低中速横向控制分析。

2.1.1 单轨（自行车）模型

$\{\begin{array}{l}\dot{x}=v\cos \theta \\ \dot{y}=v\sin \theta \\ \dot{\theta }=\frac{v}{L}\tan \delta \end{array}$

其中：

• $x,y$：车辆质心坐标
• $\theta$：车辆航向角
• $v$：车速
• $L$：轴距（Wheelbase）
• $\delta$​：前轮转向角

---

2.1.2 横向误差 $e_y$ 与航向误差 $e_{\theta }$ 定义

• 横向误差 $e_y$
  前轮中心到参考路径最近点的垂直距离（Cross-Track Error）。
• 航向误差 $e_{\theta }$
  车辆当前航向与参考路径切线方向的夹角（Heading Error）。

几何直观：

• $e_y>0$：车辆在路径左侧；
• $e_y<0$：车辆在路径右侧；
• $e_{\theta }>0$：车辆朝向左偏离路径方向；
• $e_{\theta }<0$：车辆朝向右偏离路径方向。

---

2.2 控制律推导

Stanley 控制器的核心控制律为：

$\delta =e_{\theta }+\arctan \left(\frac{k\cdot e_y}{v}\right)$

2.2.1 控制律分解

1. 航向修正项 $e_{\theta }$
   直接使车头朝向路径切线方向。
2. 横向位置修正项 $\arctan \left(\frac{k\cdot e_y}{v}\right)$
   通过横向误差产生额外转向量，使车辆回到路径中心；速度 vvv 位于分母，意味着高速时修正幅度会减小，避免过度转向。

---

2.2.2 增益系数 $k$ 的作用

• $k$ 越大 → 横向误差修正力度越强，响应快但可能振荡；
• $k$ 越小 → 横向修正平缓，稳定性好但回归慢；
• 通常 $k$ 通过实验调优或经验公式选取（如考虑车辆轴距、最大转角等）。

---

2.3 控制特性分析

2.3.1 高速稳定性

由于修正项中有 $\frac{1}{v}$，高速时横向修正量减小，可防止频繁大角度转向，提高高速稳定性。

2.3.2 低速响应性

低速时 $\frac{k{e}_y}{v}$ 数值大，横向修正迅速，可以在狭窄道路或转弯时快速对齐路径。

2.3.3 误差收敛过程

在闭环控制中，$e_y$ 与 $e_{\theta }$ 会随时间逐渐减小，车辆轨迹会逐渐与参考路径重合。

三、Stanley 控制器的实现步骤

---

3.1 控制流程结构

Stanley 控制器的核心执行流程是实时获取车辆与路径的相对状态 → 计算误差 → 代入控制律求解转向角 → 发送执行命令，形成一个连续的闭环控制系统。

该过程在自动驾驶系统中通常位于横向控制模块，上层由轨迹规划模块提供参考路径，下层由执行器控制模块将转向命令发送给转向系统。

---

3.2 每一时刻的完整工作步骤

在离散时间 $t_k$（第 $k$ 个控制周期）执行以下步骤：

Step 1：获取车辆状态与路径信息

• 车辆位置$(x,y)$
• 航向角 $\theta$
• 车速$v$
• 路径曲线及其切线方向 $\psi (s)$

Step 2：计算横向误差 $e_y$

• 找到车辆前轮中心到参考路径的最近点 KaTeX parse error: Undefined control sequence: \* at position 3: P^\̲*̲

• 计算前轮中心与 KaTeX parse error: Undefined control sequence: \* at position 3: P^\̲*̲ 的有符号垂直距离

• 符号规则：

  $e_y>0\Rightarrow \text{车辆在路径左侧},e_y<0\Rightarrow \text{车辆在路径右侧}$

Step 3：计算航向误差 $e_{\theta }$

KaTeX parse error: Undefined control sequence: \* at position 40: …theta - \psi(s^\̲*̲))

其中 wrap 表示将角度归一化到区间 $[-\pi ,\pi ]$。

Step 4：代入控制律求解转向角 $\delta$

${\delta }_k=e_{\theta }+\arctan \left(\frac{k\cdot e_y}{v+ϵ}\right)$

• $ϵ$为防止低速时除零的极小常数（例如 0.1 m/s）
• $k$ 为横向增益系数

Step 5：发送转向命令至底层执行器

• 输出 ${\delta }_k$ 到车辆转向控制系统
• 若与纵向控制配合，则还需同时输出期望速度或加速度

---

3.3 伪代码示例

# Stanley 控制器执行流程
def stanley_control(x, y, theta, v, path_points, k, epsilon=0.1):# Step 1: 找到最近路径点idx, nearest_point = find_closest_point(x, y, path_points)# Step 2: 横向误差 e_ye_y = compute_signed_lateral_error(x, y, nearest_point, path_points)# Step 3: 航向误差 e_thetapath_heading = path_points[idx].headinge_theta = wrap_angle(theta - path_heading)# Step 4: 控制律计算delta = e_theta + math.atan2(k * e_y, v + epsilon)# Step 5: 输出转向角return delta

---

3.4 关键实现细节与注意事项

1. 低速抖动问题
   • 低速时 $\frac{k{e}_y}{v}$ 项会非常大，容易导致转向过大，应在分母加上 $ϵ$ 或限制最大转向角。
2. 路径平滑与采样
   • 路径需平滑处理，否则计算出的最近点和切线方向会在离散点之间突变，导致控制不稳定。
3. 与纵向控制的协同
   • 高速行驶时横向修正幅度会自动减小，但仍需配合速度规划，避免在急弯高速进入。

四、参数设计与调优

---

4.1 增益系数 $k$ 的作用

Stanley 控制器只有一个核心调节参数——横向增益系数 $k$。它直接影响横向误差修正项的幅度，从而影响车辆回到路径中心的速度与稳定性。

控制律回顾：

$\delta =e_{\theta }+\arctan \left(\frac{k\cdot e_y}{v+ϵ}\right)$

• $k$ 大：横向修正力度强，响应速度快，但可能导致振荡甚至超调；
• $k$ 小：修正力度弱，稳定性好，但收敛速度慢，可能在急弯处偏离路径过大。

---

4.2 参数选择原则

在实际车辆或仿真中，可根据以下经验步骤调节 $k$：

1. 初始值设置
   • 一般从 $k=0.5\sim 1.0$ 开始（单位：m/s）。
2. 低速测试
   • 在低速（3–5 m/s）测试急弯路径，确保车辆能够平稳回到路径中心。
3. 高速测试
   • 在高速（≥15 m/s）下测试长直道和缓弯，确保转向平滑，不发生高速抖动。
4. 微调与平衡
   • 如果低速响应慢 → 增大 $k$；
   • 如果高速易振荡 → 减小$k$。

---

4.3 速度自适应优化

虽然 Stanley 控制律中 $\frac{1}{v}$ 已经实现了速度自适应，但在极端低速时，横向修正项会非常大，容易导致急剧转向。
可在分母加上一个极小常数 ϵ\epsilonϵ：

$\delta =e_{\theta }+\arctan \left(\frac{k\cdot e_y}{v+ϵ}\right)$

其中 $ϵ$ 通常取 0.1–0.5 m/s，用于抑制低速过度转向。

---

4.4 附加优化策略

4.4.1 转向角限制

在机械转向极限和舒适性范围内限制$\delta$：

${\delta }_{\text{cmd}}=\text{clip}(\delta ,{\delta }_{\min },{\delta }_{\max })$

4.4.2 滤波抖动抑制

• 对横向误差 $e_y$ 和航向误差 $e_{\theta }$ 使用低通滤波器，减少传感器噪声带来的控制抖动：

${\hat{e}}_y(k)=\alpha e_y(k)+(1-\alpha ){\hat{e}}_y(k-1)$

其中 $\alpha \in [0.1,0.3]$

4.4.3 与纵向控制协同

• 在高速急弯时，纵向控制应主动减速，避免因转向限制导致路径跟踪误差过大。

---

4.5 调试建议

1. 仿真优先：在 Carla、MATLAB 等平台先调整出合适的 $k$ 区间，减少实车调试风险。
2. 分速段调参：根据低速/中速/高速场景，建立分段$k$ 值或速度映射表。
3. 数据记录：记录 $e_y,e_{\theta },\delta$ 等时序数据，便于分析振荡和延迟来源。
4. 渐进试验：从低速直道 → 低速弯道 → 高速直道 → 高速弯道，逐步增加测试难度。

五、Stanley 控制器与其他横向控制方法对比

---

5.1 与纯跟踪法（Pure Pursuit）的对比

原理差异

• Pure Pursuit：基于几何圆弧模型，将车辆转向设定为使车辆驶向路径上某个“前视点”（look-ahead point），转向角由车辆到前视点的圆弧半径计算。
• Stanley 控制器：直接利用横向误差 $e_y$ 与航向误差 $e_{\theta }$ 构建转向控制律，并在公式中引入速度自适应项 $\frac{1}{v}$。

性能差异

• Pure Pursuit 依赖前视距离 $L_d$ 选取，前视距离过短易振荡，过长则响应慢；
• Stanley 控制器自适应速度变化，低速响应快，高速稳定性好；
• Pure Pursuit 在曲率变化剧烈的路径上易产生超调，而 Stanley 控制器在此类场景下更稳。

---

5.2 与 PID 横向控制的对比

原理差异

• PID 横向控制：直接将横向误差作为输入，通过比例、积分、微分三项计算转向角。
• Stanley 控制器：结合横向误差与航向误差两种信息，并在横向修正项中加入速度因子。

性能差异

• PID 参数多（Kp, Ki, Kd），调试复杂；
• Stanley 控制器参数少（仅一个增益 $k$），调参简单；
• PID 对高速场景敏感，容易振荡；Stanley 控制器高速抑制性更好；
• PID 可在低速精准对齐路径，但在曲率变化剧烈的弯道表现不如 Stanley 控制器稳定。

---

5.3 与 MPC 横向控制的对比

原理差异

• MPC：基于车辆模型和预测时域，优化未来一段时间内的控制量，使得目标函数（误差、控制量变化等）最小化。
• Stanley 控制器：基于几何与误差反馈的单步控制律，不进行未来状态预测。

性能差异

• MPC 可同时考虑路径跟踪与控制约束（如最大转角、加速度限制），在复杂场景更优；
• Stanley 控制器计算量小，实时性强，适合嵌入式平台和低速应用；
• MPC 对模型精度依赖大，建模不准可能引入偏差，而 Stanley 控制器依赖较少；
• MPC 调参涉及权重矩阵 Q、R，较复杂；Stanley 控制器仅需调整 $k$。

---

5.4 总结对比表

方法	核心思想	参数数量	优点	缺点	典型应用
Stanley 控制器	航向 + 横向误差反馈 + 速度自适应	1（k）	稳定性好，调参简单，实时性强	高速急弯性能有限	自动驾驶低/中速横向控制
Pure Pursuit	圆弧几何追踪前视点	1（前视距离）	原理简单，易实现	对前视距离敏感，高速不稳	移动机器人、低速路径跟踪
PID 横向控制	横向误差比例/积分/微分控制	3（Kp, Ki, Kd）	控制平滑，可精准对齐	调参复杂，高速易振荡	工业控制、低速 AGV
MPC 横向控制	基于模型的预测优化控制	多（Q, R, 时域长度等）	能处理约束，性能最优	计算量大，调参复杂	高速公路自动驾驶、赛车

六、典型应用与扩展

---

6.1 无人车直道与弯道跟踪案例

场景描述

• 路径包含直道与中等曲率弯道；
• 车速在 $5\text{ m/s}$（弯道）到 $15\text{ m/s}$（直道）之间变化；
• 车辆配备厘米级 GPS 与 IMU，参考路径由规划模块提供。

表现特点

• 直道段：由于航向误差 $e_{\theta }$ 较小，转向主要由横向误差修正项控制，车辆快速贴近路径中心，转向平滑。
• 弯道段：横向误差与航向误差同时发挥作用，使车辆顺畅进入曲线段；高速进入急弯时需依赖纵向减速配合。

实际效果

• 在中低速下，Stanley 控制器能在 1–2 秒内将横向误差收敛至 ±0.05 m；
• 高速急弯中，如果纵向速度控制不足，可能出现轻微外飘现象。

---

6.2 车道保持（Lane Keeping）

应用背景
在高速公路或城市道路中，Stanley 控制器可直接用于基于车道线的车道保持系统（LKS）。

原理

• 横向误差由车辆与车道中心线的垂距计算；
• 航向误差由车辆航向与车道切线方向的夹角计算；
• 控制律实时输出转向角，实现车辆始终保持在车道中间。

优点

• 不依赖全局路径，只需局部车道线信息；
• 对轻微曲率的高速路段稳定性好。

---

6.3 低速自动驾驶平台

典型场景

• 园区摆渡车、物流车、巡检车等低速（< 8 m/s）自动驾驶车辆；
• 通常路径规划为低曲率曲线，环境中包含较多转角与避障操作。

优势

• 计算量小，嵌入式控制器（如 STM32、ROS 节点）可轻松实时运行；
• 在低速场景中响应灵敏，几乎无需复杂模型预测；
• 调参简单，仅需设置合理的 $k$ 值。

---

6.4 改进型 Stanley 控制器

针对标准 Stanley 控制器的不足，可进行以下扩展：

6.4.1 加速度补偿项

在高速急弯中加入横向加速度补偿：

$\delta =e_{\theta }+\arctan \left(\frac{k{e}_y}{v+ϵ}\right)+\lambda \cdot a_y$

其中 $a_y$ 为横向加速度，$\lambda$ 为补偿系数。

6.4.2 动态增益调节

根据速度或曲率动态调整 kkk：

$k(v)=k_{\min }+(k_{\max }-k_{\min })e^{-\beta v}$

低速取较大 $k$，高速取较小 $k$。

6.4.3 与 MPC 融合

在普通路段用 Stanley 控制器保证实时性，在复杂场景（急弯、避障）切换至 MPC 控制器，提高轨迹跟踪精度与稳定性。

---

6.5 总结

Stanley 控制器因其原理简单、实时性强、调参容易，在自动驾驶横向控制中得到广泛应用，尤其在低速至中速场景表现出色。
通过合理调节增益系数 $k$，并结合纵向速度控制，可以在绝大多数常规驾驶场景中实现稳定且平滑的路径跟踪。
针对高速急弯和噪声敏感问题，可以采用加速度补偿、动态增益调节或与 MPC 混合控制的方式进行优化。

图 1：Stanley Path Tracking: Trajectory vs Reference (Fixed)
该图展示了修正后的 Stanley 控制器在参考路径上的跟踪效果。

• 蓝色曲线表示规划好的参考路径（Reference Path），包含一个中等幅度的 S 弯段和直道段。
• 橙色曲线为车辆实际行驶轨迹（Vehicle Trajectory），起点与参考路径存在一定的横向与航向偏差。
• 起点用“Path Start”和“Vehicle Start”标记，直观对比初始偏移。
• 图中每隔一定距离绘制车辆姿态箭头，表明车辆的实时航向变化。
• 从运行结果可见，车辆轨迹在短时间内快速贴合参考路径，进入稳态后几乎与参考路径完全重合，表明控制器能够有效消除初始横向与航向误差，并实现平滑跟踪。

图 2：Errors and Steering Over Time
该图给出了同一实验中横向误差、航向误差与转向角随时间的变化过程。

• Lateral Error $e_y$（蓝线）：初始时有明显的正值，表示车辆位于参考路径左侧；在约 3 秒内迅速收敛到接近零，并在后续保持稳定，说明横向偏移已被消除。
• Heading Error $e_{\theta }$（橙线）：初始偏航角约为 0.17 rad（约 10° 左偏），控制器快速调整使其接近零，且无明显振荡。
• Steering Angle $\delta$（绿色线）：初始转向角较大，用于快速回到路径；随后转角减小并趋于稳定，表明车辆已进入稳态巡航。
  整体来看，控制过程平稳、误差收敛迅速，转角变化无剧烈抖动，验证了 Stanley 控制器在该路径上的有效性与鲁棒性。

七、python可视化代码

# -*- coding: utf-8 -*-
"""
Stanley 路径跟踪可视化（修正版）
- 误差在“前轴中心”计算
- e_theta = wrap(psi - yaw)
- 横向误差符号统一（e_y = -signed_lateral_error(...))
- 转角饱和，常速行驶
"""import math
import numpy as np
import matplotlib.pyplot as plt# ---------------- 工具函数 ----------------
def wrap(a: float) -> float:"""将角度归一化到 [-pi, pi]"""return (a + np.pi) % (2 * np.pi) - np.pidef nearest_point_index(px, py, path_xy):d2 = (path_xy[:, 0] - px) ** 2 + (path_xy[:, 1] - py) ** 2return int(np.argmin(d2))def signed_lateral_error(px, py, path_xy, path_yaw, idx):"""计算带符号的横向误差（以路径左法向为正）n = (-sin(psi), cos(psi))"""p = path_xy[idx]psi = path_yaw[idx]nx, ny = -math.sin(psi), math.cos(psi)  # 左法向ex, ey = px - p[0], py - p[1]return ex * nx + ey * ny# ---------------- 参考路径 ----------------
def make_reference_path():"""S 弯 + 缓弯 + 直道的平滑路径"""t1 = np.linspace(0, 40, 400)x1 = t1y1 = 1.5 * np.sin(0.2 * t1)t2 = np.linspace(0, 30, 300)x2 = 40 + t2y2 = 1.5 * np.sin(0.2 * 40) + 0.03 * (t2 ** 2)t3 = np.linspace(0, 40, 400)x3 = 70 + t3y3 = y2[-1] + 0.0 * t3x = np.concatenate([x1, x2, x3])y = np.concatenate([y1, y2, y3])dx = np.gradient(x)dy = np.gradient(y)yaw = np.arctan2(dy, dx)return np.stack([x, y], axis=1), yaw# ---------------- Stanley 控制律 ----------------
def stanley_delta(front_x, front_y, yaw, v, path_xy, path_yaw, k=1.5, eps=0.2):"""输入：前轴中心(front_x, front_y)、当前航向 yaw、速度 v输出：转向角 delta 及误差"""idx = nearest_point_index(front_x, front_y, path_xy)e_signed = signed_lateral_error(front_x, front_y, path_xy, path_yaw, idx)# 统一符号：让 e_y>0 时朝向回路径中心的转向为正e_y = -e_signed# 航向误差：期望(路径切线) - 实际e_theta = wrap(path_yaw[idx] - yaw)# 控制律delta = e_theta + math.atan2(k * e_y, v + eps)return delta, idx, e_y, e_theta# ---------------- 主程序 ----------------
if __name__ == "__main__":# 路径path_xy, path_yaw = make_reference_path()# 仿真参数dt = 0.02T = 28.0N = int(T / dt)L = 2.8                  # 轴距Lf = 1.4                 # 质心到前轴距离（近似）max_steer = np.deg2rad(35)v = 10.0                 # 常速# 初始状态：略偏离路径x = path_xy[0, 0] - 2.0y = path_xy[0, 1] + 1.5yaw = path_yaw[0] + np.deg2rad(10)xs, ys, yaws, deltas = [], [], [], []eys, eths = [], []for _ in range(N):# 前轴中心fx = x + Lf * math.cos(yaw)fy = y + Lf * math.sin(yaw)# Stanley 控制delta, idx, e_y, e_th = stanley_delta(fx, fy, yaw, v, path_xy, path_yaw,k=1.5, eps=0.2)# 转角饱和delta = float(np.clip(delta, -max_steer, max_steer))# 自行车模型更新（质心点）x += v * math.cos(yaw) * dty += v * math.sin(yaw) * dtyaw += (v / L) * math.tan(delta) * dtyaw = wrap(yaw)xs.append(x); ys.append(y); yaws.append(yaw); deltas.append(delta)eys.append(e_y); eths.append(e_th)# -------- 图1：轨迹对比 --------plt.figure(figsize=(11, 5))plt.plot(path_xy[:, 0], path_xy[:, 1], label="Reference Path", linewidth=2)plt.plot(xs, ys, label="Vehicle Trajectory", linewidth=2)plt.scatter(path_xy[0, 0], path_xy[0, 1], s=40, label="Path Start")plt.scatter(xs[0], ys[0], s=40, marker="x", label="Vehicle Start")# 朝向箭头for i in range(0, len(xs), 60):plt.arrow(xs[i], ys[i], 0.8 * np.cos(yaws[i]), 0.8 * np.sin(yaws[i]),head_width=0.2, length_includes_head=True)plt.axis("equal"); plt.grid(True)plt.xlabel("x [m]"); plt.ylabel("y [m]")plt.title("Stanley Path Tracking: Trajectory vs Reference (Fixed)")plt.legend(loc="best")plt.tight_layout()# plt.savefig("stanley_tracking.png", dpi=200)# -------- 图2：误差与转角 --------t = np.arange(len(eys)) * dtplt.figure(figsize=(11, 4.6))plt.plot(t, eys, label="Lateral Error e_y [m]")plt.plot(t, eths, label="Heading Error e_theta [rad]")plt.plot(t, deltas, label="Steering Angle delta [rad]")plt.grid(True); plt.xlabel("time [s]"); plt.ylabel("value")plt.legend(loc="best")plt.title("Errors and Steering Over Time")plt.tight_layout()# plt.savefig("stanley_errors.png", dpi=200)plt.show()