LQR算法在自动驾驶路径跟踪中的优化实践

1. 项目背景与核心问题

自动驾驶车辆路径跟踪控制是智能驾驶系统的核心技术之一。当车辆以较高速度行驶时，单纯依靠几何跟踪方法（如纯追踪算法）往往会出现较大跟踪误差，甚至导致系统失稳。动力学跟踪误差模型结合LQR（线性二次调节器）算法，能够有效解决中高速场景下的路径跟踪问题。

我在实际工程测试中发现，当车速超过60km/h时，传统PID控制器的跟踪误差会急剧增大。而基于动力学模型的LQR控制器在100km/h速度下仍能保持小于0.3m的横向误差。这种性能差异主要源于LQR算法对系统动态特性的全局优化能力。

2. 动力学跟踪误差模型构建

2.1 车辆动力学建模基础

采用经典的自行车模型作为基础框架，该模型假设：

• 忽略悬架动态特性
• 左右轮胎特性合并处理
• 小角度转向近似成立

模型状态变量选取为：

x = [横向误差 e, 航向误差 Δψ, 横向误差变化率 ė, 航向误差变化率 Δψ̇]

在MATLAB/Simulink中建模时，需要特别注意轮胎侧偏刚度的参数辨识。实测数据显示，普通乘用车的前轮侧偏刚度通常在-50000N/rad到-80000N/rad之间，后轮则要高出约20%。

2.2 误差状态方程推导

通过泰勒展开将非线性模型在工作点附近线性化，得到状态空间表达式：

ẋ = A·x + B·u y = C·x

其中控制输入u为前轮转角。系数矩阵A包含车辆质量、轴距、速度等参数，B矩阵则反映转向系统传动比等特性。

关键提示：当车速变化超过±15%时，建议重新线性化模型以保证控制精度。我在实车测试中发现，使用固定参数的LQR控制器在车速从60km/h变化到90km/h时，跟踪误差会增大40%左右。

3. LQR控制器设计与实现

3.1 代价函数参数整定

LQR的核心是设计二次型代价函数：

J = ∫(xᵀQx + uᵀRu)dt

Q矩阵对角元素对应状态变量的权重。根据工程经验，建议初始设置：

Q = diag([10, 5, 1, 0.1]) R = 1

这个配置表示更关注横向位置误差(e)的控制，而对航向角变化率(Δψ̇)的约束相对宽松。

调试时可以采用"先粗后细"的策略：

1. 先固定R=1，调整Q使各状态量达到相同数量级
2. 再微调Q元素间的比例关系
3. 最后整体缩放Q矩阵大小

3.2 黎卡提方程求解

使用MATLAB的lqr()函数求解时，要特别注意算法收敛性检查。当系统可控性矩阵秩不足时，会出现以下典型报错：

无法求解黎卡提方程的解

解决方法包括：

1. 检查B矩阵是否包含全零行
2. 验证系统可控性矩阵的秩
3. 适当调整Q、R矩阵的数值比例

4. 联合仿真平台搭建

4.1 Carsim-MATLAB联合仿真配置

推荐采用以下硬件配置：

• CPU: Intel i7及以上
• 内存: 16GB以上
• Carsim版本: 2019或更新

软件接口配置关键步骤：

1. 在Carsim中设置VS_COMMAND模式
2. MATLAB配置S-Function路径
3. 设置固定步长求解器（建议0.01s）

4.2 典型测试场景设计

建议包含以下测试用例：

1. 双移线测试（车速60-100km/h）
2. 连续S弯道（曲率半径50-100m）
3. 紧急变道工况
4. 不同路面附着系数（μ=0.3-1.0）

在仿真中遇到过的一个典型问题是Carsim输出单位与MATLAB不匹配，导致控制异常。解决方案是在接口层添加单位转换模块，特别是注意：

• 角度单位（deg/rad）
• 力单位（N/kN）
• 长度单位（m/mm）

5. 性能优化与实测分析

5.1 跟踪误差指标对比

在80km/h双移线工况下，不同控制器的RMS误差对比：

实测数据显示，LQR在保证实时性的前提下（平均计算时间<1ms），获得了接近MPC的控制精度。

5.2 实时性优化技巧

通过以下方法可以提升代码运行效率：

1. 将黎卡提方程求解移至初始化阶段
2. 采用查表法存储不同车速对应的K矩阵
3. 使用C代码生成（MATLAB Coder）

在dSPACE快速原型系统中测试时，优化后的LQR控制器仅占用约15%的CPU资源，而同等条件的MPC控制器则需要60%以上的资源。

6. 常见问题排查指南

6.1 系统发散问题

现象：仿真中误差不断增大直至失控 可能原因：

1. 轮胎参数与实际不符（检查Carsim轮胎模型）
2. 控制器采样时间过长（建议≤0.02s）
3. 状态观测器噪声过大（添加合适的滤波器）

6.2 稳态误差问题

现象：车辆无法完全收敛到参考路径 解决方案：

1. 在状态变量中添加误差积分项
2. 调整Q矩阵中位置误差的权重
3. 检查参考路径的曲率连续性

实际项目中遇到过因路径规划模块输出的曲率不连续导致LQR控制失效的情况。解决方法是在规划层添加曲率平滑处理，或者在前馈控制项中考虑路径曲率变化率。

7. 进阶改进方向

对于更高性能要求的场景，可以考虑以下扩展方案：

1. 增益调度LQR：根据车速动态调整控制器参数
2. 结合前馈控制：利用路径曲率信息提高跟踪精度
3. 鲁棒LQR设计：考虑参数不确定性范围

在某个量产项目中，我们采用车速分段的LQR参数集（每20km/h一个区间），相比固定参数方案将高速段的跟踪误差降低了35%。参数切换时需要注意平滑过渡，避免控制量突变。