一文看懂 Stanley 算法：让自动驾驶小车 “不跑偏” 的经典方案

如果你玩过自动驾驶小车（比如实验室的差速小车、ROS 小车），可能会遇到一个问题：明明设定了直线或弯道路径，小车却总是 “画龙”—— 要么离路径越来越远，要么车头方向不对。

今天要讲的 Stanley 算法，就是解决这个 “跑偏” 问题的经典方案。它是 MIT 团队为 DARPA 自动驾驶挑战赛设计的，核心逻辑特别简单：同时纠正 “离路径的距离” 和 “车头的方向”，让小车稳稳跟着路线走。哪怕你是刚接触路径跟踪的新手，也能轻松理解它的原理。

一、先搞懂：小车 “跑偏” 的两个原因

要解决跑偏，得先知道小车怎么判断自己 “偏了”。Stanley 算法只关注两个关键偏差，就像人走路偏了会看 “离路边多远” 和 “头朝没朝前方”：

1. 横向偏差（y）：我离路径有多远？

假设小车沿着一条参考路径（比如你用代码画的直线、圆弧）行驶，我们以小车后轮中心为 “观察点”（因为车辆动力学通常以后轮为基准，就像人走路以脚为支点），这个点到参考路径的垂直距离，就是 “横向偏差”，记为 y。

• 如果 y>0：小车在路径的左边（假设路径是向右的）；
• 如果 y<0：小车在路径的右边；
• 如果 y=0：小车正好在路径上，没偏！

2. 航向偏差（θₑ）：我的车头朝对了吗？

光在路径上还不够，车头方向得和路径一致。比如路径前方是 “向右拐”，但小车车头还朝正前方，这时候就有 “航向偏差”，记为 θₑ。

它的定义很简单：小车当前的航向角（车头朝的方向）减去参考路径在 “最近点” 的切线角（路径朝的方向）。

• 如果 θₑ>0：车头比路径 “偏右” 了；
• 如果 θₑ<0：车头比路径 “偏左” 了；
• 如果 θₑ=0：车头正好顺着路径方向，没歪！

二、Stanley 的核心：一个公式算出路转角

知道了两个偏差，Stanley 算法用一个公式就能算出小车前轮该转多大角度（转角 δ），让小车纠正偏差、回到路径上。这就是它的核心公式，也是唯一需要记住的公式：

我们来逐个拆解公式里的每个部分：

符号	含义	作用
δ	前轮转角	最终要控制的量：δ 正表示右拐，δ 负表示左拐
θₑ	航向偏差	直接补偿 “车头方向”：比如 θₑ正（车头偏右），就左拐一点抵消
k	调节系数（增益）	控制 “横向偏差补偿” 的力度：k 越大，对横向偏差的反应越灵敏
y	横向偏差	反映 “离路径的距离”：y 越大，需要拐的角度越大
v	小车当前速度	防止 “高速猛拐”：速度越快，v 越大，横向偏差的补偿项（ky/v）越小，避免小车晃荡

举个例子帮你理解：假设小车以 v=5km/h 行驶，横向偏差 y=10cm（离路径有点远），航向偏差 θₑ=5°（车头偏右），调节系数 k=0.5。代入公式：横向补偿项 = arctan ((0.5×0.1)/5) ≈ arctan (0.01) ≈ 0.57°前轮转角 δ = 5° + 0.57° ≈ 5.57°（右拐约 5.6°）这样一来，小车既纠正了 “车头偏右” 的问题，又慢慢回到了路径上，不会因为离路径远就猛拐。

三、Stanley 算法怎么 “工作”？四步走

公式看起来简单，但算法实际运行时，是一个 “实时循环” 的过程。就像人走路时 “不断看路、不断调整方向”，小车也会每秒跑几十次这个循环：

1. 找 “最近的路点”：小车通过 GPS、激光雷达或里程计知道自己的位置后，先在参考路径上找到 “离自己后轮中心最近的那个点”（比如路径是由 1000 个点组成的，找其中距离最小的点）。
2. 算两个偏差：根据最近路点，计算当前的横向偏差 y 和航向偏差 θₑ（用几何方法或坐标计算，不需要复杂公式）。
3. 算转角：把 θₑ、y、v、k 代入核心公式，算出前轮该转的角度 δ。
4. 控制车轮：把转角 δ 发给小车的转向电机，让车轮转起来；同时进入下一次循环，重复 1-4 步。

四、Stanley vs PurePursuit：新手该选哪个？

如果你之前学过 PurePursuit（纯追踪）算法，可能会问：这两个都是几何跟踪算法，该用哪个？

简单说，两者各有优势，适合不同场景：

对比维度	Stanley 算法	PurePursuit 算法
核心逻辑	同时补偿 “航向偏差 + 横向偏差”	盯着 “前方一个点”，画圆弧跟踪
低速精度	更高（因为考虑了航向，不容易晃）	稍低（可能因为只看前点，方向调整慢）
高速稳定性	一般（速度快时对参数 k 敏感，需调小）	稍好（前视距离调大后，转弯更平缓）
实现难度	简单（只需算两个偏差，一个公式）	简单（只需找前视点，算圆弧半径）

新手建议：先学 Stanley，因为它能帮你理解 “偏差补偿” 的逻辑，而且低速场景（比如实验室小车）下精度更高，和 PurePursuit 对比时，差异也更明显。

总结

Stanley 算法之所以经典，就是因为它用 “最简单的逻辑解决了最核心的问题”—— 没有复杂的数学推导，只靠 “两个偏差 + 一个公式”，就让小车能稳定跟踪路径。

如果你是自动驾驶新手，想从路径跟踪算法入门，Stanley 绝对是值得优先学习的方案。试着用仿真实现它，再和 PurePursuit 对比一下两者的跟踪效果，你会对 “路径跟踪” 的理解更深一层～