高速场景路径跟踪策略 - 第4节
一、预瞄距离自适应策略
纯跟踪算法里，最让我头疼的就是预瞄距离怎么选。固定值？不行。我早期做项目时吃过这个亏——高速时预瞄太近，车像喝醉了酒；低速时预瞄太远，转弯又像在画龙。
说白了，预瞄距离就是个「看多远」的问题。你看得近，反应快但容易抖；看得远，稳当但转弯会切内线。那怎么办？动态调整呗。
1.1 基于速度的自适应
最朴素的做法：让预瞄距离跟着车速走。
// 伪代码：速度自适应预瞄
float Ld = k * v + L0; // k是增益，L0是基础预瞄距离我习惯把k设在0.3~0.5之间。比如车速20m/s，k=0.4，L0=2，那Ld就是10米。嗯，这个值在高速场景下还算靠谱。
关键点：预瞄距离不能无限增长。我见过有人设成Ld=100米，结果车在直道上都开始画龙——因为预瞄点太远，横向误差被放大了。
1.2 基于曲率的自适应
光看速度不够。你想想看，同样60km/h，直道和急弯能一样吗？
我在项目中遇到过这种情况：高速进弯，预瞄距离没变，结果车直接冲出车道。后来加了曲率补偿：
// 曲率自适应预瞄
float curvature = getCurvature(path, lookahead_index);
float Ld = base_Ld / (1 + alpha * fabs(curvature));曲率越大，预瞄距离越短。alpha我一般取2~5，具体看车辆响应延迟。响应慢的车，alpha要小一点，别让预瞄缩得太狠。
我的经验：曲率可以从路径点直接算，也可以用三次样条插值后求导。后者更平滑，但计算量大一点。
二、曲率补偿与速度规划
纯跟踪的另一个坑：它只考虑位置误差，不管车辆动力学。说白了，它以为车能瞬间转到目标方向——这可能吗？
2.1 曲率补偿原理
车辆有惯性，有转向延迟。纯跟踪算出来的目标曲率，实际执行时会打折扣。我习惯加一个前馈补偿：
// 曲率补偿
float kappa_desired = 2 * sin(alpha) / Ld; // 纯跟踪原始曲率
float kappa_compensated = kappa_desired / (1 + tau * v); // 补偿后tau是转向系统的时间常数。我测过几款车，一般在0.1~0.3秒之间。补偿后，弯道跟踪精度能提升30%以上。
注意：补偿系数不能太大。我曾经设过tau=0.5，结果直道上都开始震荡——过补偿了。
2.2 速度规划策略
纯跟踪本身不规划速度，它只输出转向角。但实际应用中，速度必须跟着路径走。
我常用的策略：
- 直道加速：曲率小于阈值时，按目标速度跑
- 弯道减速：根据曲率计算最大安全速度
- 过渡平滑：用S曲线或梯形速度规划，别让加速度突变
// 弯道速度限制
float max_v_curve = sqrt(mu * g / fabs(curvature)); // 基于侧向加速度限制
float target_v = min(max_v_curve, desired_v);mu是路面摩擦系数，干沥青取0.8，湿路面取0.5。嗯，这个值我一般留20%余量——安全第一。
三、纯跟踪的优缺点与适用场景
3.1 优点
- 实现简单：核心就一个公式，调参也直观
- 计算量小：不需要复杂的优化求解，嵌入式上跑得动
- 路径跟踪稳定：预瞄距离调好了，直道和缓弯表现不错
3.2 缺点
- 高速性能差：超过80km/h，跟踪精度明显下降
- 不考虑动力学：轮胎滑移、侧偏刚度这些，纯跟踪一概不管
- 对路径质量敏感：路径不平滑，跟踪就会抖
我的总结：纯跟踪适合低速、路径平滑的场景。高速场景，我建议用MPC或LQR做底层控制，纯跟踪做上层参考。
3.3 适用场景
| 场景 | 推荐度 | 说明 |
|---|---|---|
| 园区低速物流车 | ★★★★★ | 速度低，路径简单，纯跟踪足够 |
| 高速公路巡航 | ★★★☆☆ | 直道OK，急弯需配合速度规划 |
| 城市道路 | ★★☆☆☆ | 曲率变化大，纯跟踪容易失稳 |
| 泊车场景 | ★★★★☆ | 速度极低，纯跟踪表现不错 |
四、核心知识体系
下面这张图，是我梳理的纯跟踪进阶知识结构。你看一眼，心里就有数了。
避坑指南：我曾经在高速测试时，预瞄距离设得太激进，结果80km/h过弯时车辆直接推头。后来加了曲率补偿和速度限制，才把问题解决。记住：纯跟踪不是万能的，它只是个基础框架。
好了，这一章的内容就这些。预瞄距离自适应、曲率补偿、速度规划——这三个点你吃透了，纯跟踪就算入门了。下一章我们聊点更刺激的：模型预测控制在路径跟踪中的应用。