LQR控制在自动驾驶横向控制中的实践与优化

1. LQR控制算法在自动驾驶横向控制中的应用背景

线性二次型调节器（Linear Quadratic Regulator, LQR）作为现代控制理论中的经典方法，在车辆横向控制领域展现出独特优势。不同于传统的PID控制，LQR通过状态空间建模能够系统性地处理多变量耦合问题，这正是自动驾驶车辆在复杂路况下维持轨迹跟踪精度所需要的核心能力。

在CarSim与Simulink联合仿真环境中，LQR控制器通常被部署在横向控制模块中，主要解决以下三个关键问题：

• 车辆航向角与道路中心线的偏差控制
• 横向位置误差的快速收敛
• 方向盘转角指令的平滑生成

实际工程经验表明：当车速超过60km/h时，传统预瞄控制算法的横向误差会显著增大，而LQR通过权重矩阵的合理配置，能在高速工况下保持±0.1m以内的跟踪精度。

2. CarSim-Simulink联合仿真平台搭建

2.1 软件环境配置要点

实现CarSim 2020与MATLAB/Simulink R2021a的联合仿真需要特别注意版本兼容性。以下是经过验证的配置方案：

安装完成后，需在CarSim界面设置Simulink接口路径：

1. 进入"VS Solvers"菜单
2. 选择"Export to Simulink"
3. 指定MATLAB安装目录下的/extern/lib/win64文件夹

2.2 车辆动力学模型参数化

CarSim提供的B-Class Hatchback车型参数需要针对控制算法进行二次开发：

matlab复制% 车辆关键参数配置脚本
vehicle.mass = 1350;       % 整车质量(kg)
vehicle.Iz = 2100;         % 横摆转动惯量(kg·m²)
vehicle.lf = 1.1;          % 前轴到质心距离(m)
vehicle.lr = 1.6;          % 后轴到质心距离(m)
vehicle.Cf = 95000;        % 前轮侧偏刚度(N/rad)
vehicle.Cr = 110000;       % 后轮侧偏刚度(N/rad)

调试中发现：当侧偏刚度设置误差超过15%时，会导致LQR控制器的鲁棒性显著下降。建议通过脉冲转向实验实测修正该参数。

3. 横向动力学建模与LQR设计

3.1 状态空间方程推导

基于自行车模型建立的状态方程包含四个核心状态量：

1. 横向位置误差 e
2. 航向角误差 Δψ
3. 横向速度 v
4. 横摆角速度 r

状态方程矩阵形式为：

code复制ẋ = A·x + B·u
y = C·x + D·u

其中系统矩阵A的计算涉及车辆动力学参数：

matlab复制A = [0, vx, 1, 0;
     0, 0, 0, 1;
     0, 0, -(Cf+Cr)/(m*vx), (Cr*lr-Cf*lf)/(m*vx)-vx;
     0, 0, (Cr*lr-Cf*lf)/(Iz*vx), -(Cf*lf²+Cr*lr²)/(Iz*vx)];

3.2 权重矩阵调参方法论

Q和R矩阵的配置直接影响控制性能：

1. 初始权重设置原则：

   • Q = diag([10, 1, 0.1, 0.01])
   • R = 0.01
   • 对应状态量重要性排序：e > Δψ > v > r

2. 迭代优化流程：

   • 首先增大Q(1,1)直到横向振荡出现
   • 然后增大R直到转向指令变得平滑
   • 最后微调Q(3,3)改善侧向加速度

典型调试案例对比：

4. Simulink实现与联合仿真技巧

4.1 控制模块封装要点

在Simulink中实现LQR控制器时，推荐采用如下结构：

1. State Estimator模块：处理CarSim输出的原始信号
2. LQR Core模块：包含求解Riccati方程的嵌入式MATLAB Function
3. Actuator Interface模块：将前轮转角转换为方向盘指令

关键实现代码片段：

matlab复制function [u, K] = lqr_controller(x, A, B, Q, R)
    [K,~,~] = lqr(A,B,Q,R);
    u = -K*x;
end

4.2 实时调试中的常见问题解决方案

1. 代数环问题：

   • 现象：仿真时报错"Algebraic loop detected"
   • 解决方法：在LQR模块输出端添加Unit Delay模块
   • 原理：打破信号瞬时依赖关系

2. 采样时间不匹配：

   • CarSim默认步长5ms，而Simulink可能设为variable-step
   • 统一设置为固定步长10ms可获得最佳平衡

3. 信号单位转换：

   • CarSim输出角度单位为度，需转换为弧度
   • 方向盘转角增益建议设为16.5（对应常见转向传动比）

5. 进阶优化方向

5.1 考虑道路曲率的LQR改进

在弯道工况下，基础LQR表现受限。可通过以下改进提升性能：

1. 曲率前馈补偿：

   math复制δ_ff = (lr + mlr*vx²/(Cr*(lf+lr)))/R
   

2. 时变Q矩阵设计：
   • 根据曲率半径动态调整Q(1,1)
   • 大曲率时增加横向误差权重

5.2 与MPC的混合控制架构

高速场景实测数据表明：

• LQR在计算效率上占优（<1ms）
• MPC在极端工况下更鲁棒

混合架构实现方案：

1. 正常工况使用LQR
2. 当预测误差超过阈值时切换至MPC
3. 采用平滑过渡算法避免控制跳变

在最近的项目实测中，这种架构在保持90%时间使用LQR的情况下，将紧急避障成功率提升了40%。