第四章 横向控制算法：从PID到Stanley的实战之路
横向控制，说白了就是让车沿着我们想要的路走。你想想看，车道保持的核心就两个事：一是看清路在哪（感知），二是让车听话地跟着路走（控制）。这一章，我们就来聊聊后者。
我个人习惯把横向控制算法分成两类：一类是基于几何的，比如纯跟踪和Stanley；另一类是基于反馈的，比如PID。实际项目中，我通常会把它们结合起来用。嗯，这里要注意，没有银弹，只有最适合场景的方案。
4.1 PID控制器设计：最朴素的反馈
PID控制器，搞过控制的应该都不陌生。它的核心思想很简单：看偏差，算控制量。偏差大就多打点方向，偏差小就少打点。
核心公式：
u(t) = Kp * e(t) + Ki * ∫e(τ)dτ + Kd * de(t)/dt其中 e(t) 是横向偏差，u(t) 是方向盘转角。
我在项目中遇到过一个问题：只用P控制，车在直道上还行，一到弯道就偏得厉害。后来加了I项才解决。但I项也不能太大，否则过弯时会有明显的超调。
我的调参经验：
- 先调Kp，让车能大致跟着路走
- 再加Kd，抑制震荡
- 最后加Ki，消除稳态误差
我曾经因为Ki调得太大，车在高速上像蛇一样扭动...那场面，别提多尴尬了。
4.2 预瞄距离模型：看得远才能走得稳
预瞄距离，说白了就是看多远。你开车的时候，肯定不会只盯着车头前面那点路吧？横向控制也一样。
预瞄距离 L 通常和车速 v 有关：
L = k * v + L0其中 k 是时间常数（一般取 0.5~2 秒），L0 是最小预瞄距离。
我建议你根据实际场景来调这个参数。高速上预瞄距离要长一些，低速泊车时就要短一些。你想想看，如果高速上预瞄距离太短，车就会频繁修正方向，乘客会晕车的。
注意：预瞄距离不是越长越好。太长的话，车对近处的弯道反应会变慢，容易冲出车道。
4.3 纯跟踪（Pure Pursuit）算法：追着目标点跑
纯跟踪算法的思路很直观：在目标路径上选一个预瞄点，然后控制车辆去追这个点。就像你开车时盯着前方某个点，然后打方向让车头对准它。
核心公式：
δ = arctan(2 * L * sin(α) / ld)其中：
- δ 是前轮转角
- L 是轴距
- α 是车辆航向与预瞄点的夹角
- ld 是预瞄距离
代码实现（Python）：
def pure_pursuit_control(vehicle_state, path_points, lookahead_distance):
# 找到预瞄点
nearest_idx = find_nearest_point(vehicle_state, path_points)
target_idx = find_target_point(path_points, nearest_idx, lookahead_distance)
target_point = path_points[target_idx]
# 计算夹角
alpha = atan2(target_point.y - vehicle_state.y,
target_point.x - vehicle_state.x) - vehicle_state.yaw
# 计算转角
delta = atan2(2 * vehicle_state.wheelbase * sin(alpha), lookahead_distance)
return delta我记得有一次在测试场，纯跟踪算法在低速时表现很好，但一上高速就出问题。后来发现是预瞄距离没随车速动态调整。嗯，这个坑我踩过，你们别踩了。
4.4 Stanley控制器：更聪明的跟路方法
Stanley控制器是斯坦福大学在DARPA挑战赛中提出的方法。它比纯跟踪更聪明的地方在于：同时考虑了横向偏差和航向偏差。
核心公式：
δ = θe + arctan(k * e / v)其中：
- θe 是航向偏差
- e 是横向偏差
- k 是增益系数
- v 是车速
Stanley vs Pure Pursuit：
| 特性 | Pure Pursuit | Stanley |
|---|---|---|
| 收敛速度 | 较慢 | 较快 |
| 弯道表现 | 容易切内线 | 更贴线 |
| 参数调优 | 预瞄距离 | 增益k |
| 高速稳定性 | 一般 | 较好 |
我曾经在项目中对比过这两种算法。说实话，Stanley在大多数场景下表现更好，但Pure Pursuit在低速大曲率弯道时反而更稳。所以我的建议是：根据场景选算法，不要迷信某一个。
避坑指南：我曾经因为Stanley的增益k调得太大，导致车辆在车道内来回震荡。后来发现，k值应该和车速成反比——车速越快，k越小。
4.5 实战建议：如何选择横向控制算法
说了这么多，到底该用哪个？我的经验是这样的：
- 低速场景（< 30 km/h）：Pure Pursuit 就够用，调参简单
- 中高速场景（30-80 km/h）：Stanley 更稳，收敛快
- 高速场景（> 80 km/h）：建议用PID+前馈，或者LQR
- 极端弯道：可以考虑用MPC，但计算量较大
你想想看，实际项目中哪有那么多完美场景？我一般会在代码里做个算法切换逻辑：根据当前车速和曲率，自动选择最合适的控制器。这样既保证了性能，又降低了调参难度。
核心要点回顾：
- PID：简单可靠，适合线性区域
- 预瞄距离：动态调整是关键
- Pure Pursuit：追点思路，低速好用
- Stanley：兼顾横向和航向，高速更优
好了，这一章的内容就到这里。下一章我们会聊纵向控制——怎么让车该快的时候快，该慢的时候慢。到时候见。
公众号:蓝海资料掘金营，微信deep3321