3种横向控制算法误差对比：Stanley/Pure Pursuit/LQR在Frenet坐标系下的表现差异
3种横向控制算法在Frenet坐标系下的性能深度评测
自动驾驶车辆的横向控制算法选择直接影响路径跟踪的精度和乘坐舒适性。本文将基于Carla仿真平台，对Stanley、Pure Pursuit和LQR三种主流算法在Frenet坐标系下的表现进行系统性对比分析。通过设计标准测试场景，量化评估各算法在横向误差收敛速度、稳态误差以及不同车速下的适应性等关键指标，为工程实践中的算法选型提供数据支撑。
1. 测试环境与方法论
1.1 仿真平台搭建
我们选择Carla 0.9.13作为测试平台，其物理引擎能够较真实地模拟车辆动力学特性。测试车辆采用Lincoln MKZ 2017模型，主要参数配置如下：
| 参数名称 | 数值 | 单位 |
|---|---|---|
| 轴距 | 2.84 | m |
| 最大转向角 | 70 | deg |
| 质量 | 1500 | kg |
| 前轮侧偏刚度 | 80000 | N/rad |
| 后轮侧偏刚度 | 120000 | N/rad |
测试场景包含三种典型道路类型：
- S型复合弯道：曲率连续变化，最大曲率0.15 m⁻¹
- 90度直角弯：测试算法对不连续路径的适应能力
- 长直道接发卡弯：评估高速稳定性与急弯响应
1.2 误差度量标准
在Frenet坐标系下定义两个核心误差指标：
# 横向位置误差计算 def lateral_error(frenet_s, frenet_d, ref_path): nearest_idx = find_nearest_point(frenet_s, ref_path) return abs(frenet_d - ref_path[nearest_idx].d) # 航向角误差计算 def heading_error(vehicle_yaw, ref_path, frenet_s): nearest_idx = find_nearest_point(frenet_s, ref_path) return normalize_angle(vehicle_yaw - ref_path[nearest_idx].theta)同时引入两个动态性能指标：
- 收敛时间：从初始偏差到进入±0.1m误差带所需时间
- 超调量：最大偏差与稳态偏差的差值百分比
2. 算法实现与参数整定
2.1 Stanley控制器实现
Stanley算法以前轮中心为参考点，其核心转向角计算公式为：
$$ \delta = \theta_e + \arctan\left(\frac{k \cdot e}{v}\right) $$
其中关键参数整定原则：
- k值选择：通过频域分析法确定，保证相位裕度>45°
- 速度补偿：引入非线性增益调度，防止低速时振荡
// Carla中的实现示例 double StanleyController::ComputeSteering() { double cross_track_error = ComputeCrossTrackError(); double heading_error = ComputeHeadingError(); // 非线性增益调度 double adaptive_gain = k_base_ * (1 + 0.5 * tanh(0.1 * (velocity_ - 10))); return heading_error + atan2(adaptive_gain * cross_track_error, std::max(velocity_, 1.0)); }2.2 Pure Pursuit优化版本
传统Pure Pursuit算法存在高速振荡问题，我们改进预瞄距离策略：
$$ L_d = L_{base} + k_v \cdot v + k_a \cdot a $$
动态调整效果对比：
| 速度区间 (m/s) | 固定预瞄距离误差 | 动态预瞄距离误差 |
|---|---|---|
| 5-10 | 0.12±0.05 | 0.08±0.03 |
| 10-15 | 0.25±0.08 | 0.15±0.05 |
| >15 | 0.40±0.12 | 0.22±0.07 |
2.3 LQR控制器设计
建立3自由度车辆模型作为状态方程：
$$ \dot{x} = \begin{bmatrix} \dot{e} \ \ddot{e} \ \dot{\psi}_e \end{bmatrix} = A x + B \delta $$
代价函数权重矩阵经过NSGA-II多目标优化得到：
Q = diag([10, 1, 5]); % 横向误差、误差变化率、航向角误差 R = 0.1; % 控制量权重3. 性能对比实验结果
3.1 S型弯道测试
在曲率连续变化的复合弯道中，三种算法的误差对比如下：
关键性能指标对比表：
| 算法类型 | 最大误差(m) | 稳态误差(m) | 收敛时间(s) | 超调量(%) |
|---|---|---|---|---|
| Stanley | 0.32 | 0.08 | 2.1 | 15 |
| PurePursuit | 0.41 | 0.12 | 3.5 | 25 |
| LQR | 0.28 | 0.05 | 1.8 | 8 |
注意：LQR在曲率突变点（t=12s）出现短暂误差增大，但快速恢复
3.2 不同车速下的稳定性
固定弯道半径30m，测试速度敏感性：
| 车速(m/s) | Stanley误差 | PurePursuit误差 | LQR误差 |
|---|---|---|---|
| 5 | 0.07±0.02 | 0.10±0.03 | 0.05±0.01 |
| 10 | 0.11±0.03 | 0.18±0.05 | 0.08±0.02 |
| 15 | 0.20±0.06 | 0.35±0.08 | 0.15±0.04 |
| 20 | 0.28±0.09 | 0.52±0.12 | 0.22±0.06 |
4. 工程实践建议
4.1 算法选型指南
根据测试结果给出场景化推荐：
城市道路场景（<15m/s）
- 优先选择：改进版Stanley
- 参数调整重点：
- 增益k随速度自适应调整
- 加入0.5-1°的死区抑制抖动
高速公路场景（>15m/s）
- 必选方案：LQR+前馈补偿
- 实现要点：
def lqr_with_feedforward(): fb_term = lqr_controller() # 反馈项 ff_term = (wheelbase * curvature) / (1 + K_us * v**2) # 前馈项 return fb_term + 0.8*ff_term # 前馈增益衰减因子
4.2 参数调试技巧
Stanley调参流程
- 静态测试：固定车速调试k值，观察阶跃响应
- 动态测试：设计速度斜坡输入，验证增益调度效果
- 极限测试：在湿滑路面（μ=0.3）验证鲁棒性
LQR权重调整经验
- 增大Q[0]：加强横向误差抑制，但可能引起转向抖动
- 增大R：平滑控制输出，但会降低响应速度
- 推荐调试步骤：
% Pareto前沿寻优 options = optimoptions('gamultiobj','PopulationSize',50); [x,fval] = gamultiobj(@lqr_tuning, 4, [], [], [], [], lb, ub, options);
在实际项目中，我们发现将Stanley与LQR混合使用能获得更好的综合性能——在低速区域使用Stanley保证灵活性，当速度超过12m/s时平滑过渡到LQR控制。这种混合策略在园区物流车项目中将平均跟踪误差降低了37%，同时减少了42%的方向盘调整次数。