自动驾驶路径跟踪控制：LQR与双PID的工程实践

1. 自动驾驶路径跟踪控制实战：LQR与双PID的完美配合

上周在调试一台实验车时，当车速提到60km/h过急弯时，传统PID控制器直接让车子画起了蛇形走位。这让我下定决心把实验室积累的LQR控制方案整理出来——这个把横向误差稳定压在0.1米内的方案，已经在我们多个实车项目中得到验证。

核心方案采用前馈+反馈LQR处理横向控制，配合位置-速度双PID实现纵向控制。这种架构在Simulink+Carsim联合仿真环境下表现出色，特别适合中等车速（30-80km/h）的城市道路场景。下面我就从原理到实操，拆解这个经过实战检验的控制方案。

2. 横向控制：LQR的工程化实现技巧

2.1 车辆建模的实用简化

真实车辆动力学复杂，但控制算法需要兼顾实时性和精度。我们采用经典的二自由度自行车模型：

code复制m*v²/R = Fyf + Fyr
Iz*r_dot = a*Fyf - b*Fyr

其中关键假设：

• 忽略悬架 dynamics
• 小角度假设（转向角<15°）
• 轮胎力处于线性区间

在Simulink中实现时，用State-Space模块直接构建状态方程。状态量选择很有讲究：

• 横向误差e1（车体中心到参考路径的垂直距离）
• 航向角误差e2（车体朝向与路径切向的夹角）
• 它们的微分项e1_dot和e2_dot

实测发现，加入横向误差的积分项反而会降低系统鲁棒性，这与教科书上的建议不同。可能是由于路径曲率变化导致的积分饱和问题。

2.2 LQR权重矩阵的调参艺术

权重矩阵Q和R的选择直接影响控制效果。经过数十组对比测试，得出以下经验值：

matlab复制Q = diag([50, 1, 20, 0.1]);  % 对应[e1, e2, e1_dot, e2_dot] 
R = 0.01;  % 控制量权重

调试时注意这些现象：

• 横向误差权重(Q(1))过大 → 方向盘高频抖动
• 航向角权重(Q(2))过大 → 过弯时出现"割草机效应"
• 微分项权重不足 → 超调明显

建议调试流程：

1. 先固定R=0.01
2. 从Q=diag([10,0.1,5,0.01])开始
3. 每次只调整一个权重，变化幅度不超过50%
4. 用3倍速仿真观察整体趋势

2.3 前馈补偿的工程实现

纯反馈控制在曲率突变路段会显现滞后。我们的解决方案：

matlab复制delta_ff = (L + Kv*v²)*K;  % L为轴距，Kv为不足转向梯度

关键改进点：

• 对期望路径曲率进行低通滤波（cutoff=2Hz）
• 根据车速v动态调整前馈增益
• 最终控制量：delta = delta_ff + delta_fb

实测数据显示，加入前馈后：

• 曲率突变处的最大误差降低42%
• 方向盘转角变化更平滑

3. 纵向控制：双PID的进阶玩法

3.1 位置-速度环的耦合设计

外层位置环和内层速度环的配合需要特别注意：

matlab复制// 位置控制器
target_speed = Kp_pos * (s_ref - s_actual) + Ki_pos * ∫(s_ref - s_actual)

// 速度控制器
throttle/brake = Kp_vel * (v_ref - v_actual) + Ki_vel * ∫(v_ref - v_actual)

必须加入的工程化处理：

• 加速度限制（建议≤2.5m/s²）
• 加加速度限制（建议≤1.0m/s³）
• 积分抗饱和（使用Clamping方法）

3.2 参数自整定策略

不同误差区间需要不同的控制策略：

2m | 1.2 | 0.05 | 快速接近
0.5-2m | 0.8 | 0.1 | 平滑过渡
<0.5m | 0.3 | 0.15 | 精细调节

在Simulink中可以用Switch模块实现这种变参数控制。注意切换时要做参数插值，避免跳变。

3.3 制动与驱动的无缝切换

实测中发现的问题：

• 油门到刹车的切换延迟导致超调
• 刹车释放时的"点头"现象

我们的解决方案：

1. 设置0.1s的重叠区间
2. 在重叠区内线性混合控制量
3. 加入0.5Hz的低通滤波

4. 联合仿真中的那些坑

4.1 Carsim接口配置

最容易出错的三个地方：

1. 坐标系转换（Y轴方向相反）
2. 单位系统（角度制vs弧度制）
3. 采样时间同步（建议固定步长0.01s）

接口模块建议配置：

matlab复制function y = carsim_interface(u)
    y(1:3) = u(1:3);  % x,y,psi
    y(4) = -u(4);  % 速度Y分量取反
    y(5) = u(5);  % 速度X分量
end

4.2 仿真加速技巧

调试时的时间管理策略：

1. 首次运行：3倍速观察整体趋势
2. 问题排查：0.5倍速逐帧分析
3. 参数微调：实时模式+Scope监控

推荐监控信号：

• 横向误差
• 方向盘转角
• 轮胎侧偏角
• 路径曲率

4.3 四轮转向的集成方案

后轮转角计算公式：

matlab复制beta = atan(lr/(lr+lf)*tan(delta_front));
delta_rear = -k * beta;  // k∈[0.2,0.35]

注意事项：

• 后轮转角不超过3°
• 车速>40km/h时逐渐减小k值
• 与LQR协同时要重新调整Q矩阵

5. 实战问题排查指南

调试时的一个小技巧：在Carsim中开启Tire Marker显示，观察轮胎侧偏角是否处于线性区间（通常应<3°），这是检验车辆模型是否合理的关键指标。

6. 模型部署的注意事项

当把仿真模型部署到实车时：

1. 增加执行器动力学模型：

   • 转向系统：一阶惯性环节（τ≈0.1s）
   • 动力系统：一阶延迟（τ≈0.3s）

2. 加入故障检测逻辑：

   matlab复制if abs(steering_angle) > max_angle
       enable_emergency_stop = 1;
   end
   

3. 设计降级模式：

   • GPS信号丢失 → 切换航迹推算
   • 控制误差过大 → 触发缓停

这个方案已经在我们的实验车上累计测试超过2000公里，最令人满意的表现是在城市道路连续弯道上，60km/h速度下仍能保持0.15m以内的横向误差。下次可以聊聊如何在这个基础上加入MPC预测控制，进一步提升极限工况下的表现。