高速场景轨迹跟踪控制工程 - 第4节

4. 纯跟踪算法进阶

纯跟踪算法里，最让我头疼的就是预瞄距离怎么选。固定值？不行。我早期做项目时吃过这个亏——高速时预瞄太近，车像喝醉了酒；低速时预瞄太远，转弯又像在画龙。

说白了，预瞄距离就是个「看多远」的问题。你看得近，反应快但容易抖；看得远，稳当但转弯会切内线。那怎么办？动态调整呗。

1.1 基于速度的自适应

最朴素的做法：让预瞄距离跟着车速走。

// 伪代码：速度自适应预瞄
float Ld = k * v + L0;  // k是增益，L0是基础预瞄距离

我习惯把k设在0.3~0.5之间。比如车速20m/s，k=0.4，L0=2，那Ld就是10米。嗯，这个值在高速场景下还算靠谱。

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%以上。

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，跟踪精度明显下降
• 不考虑动力学：轮胎滑移、侧偏刚度这些，纯跟踪一概不管
• 对路径质量敏感：路径不平滑，跟踪就会抖

3.3 适用场景

场景	推荐度	说明
园区低速物流车	★★★★★	速度低，路径简单，纯跟踪足够
高速公路巡航	★★★☆☆	直道OK，急弯需配合速度规划
城市道路	★★☆☆☆	曲率变化大，纯跟踪容易失稳
泊车场景	★★★★☆	速度极低，纯跟踪表现不错

四、核心知识体系

下面这张图，是我梳理的纯跟踪进阶知识结构。你看一眼，心里就有数了。