LQR算法在自动驾驶轨迹跟踪中的实践与优化

1. 项目概述

在自动驾驶技术快速发展的今天，车辆轨迹跟踪控制作为核心功能之一，其精度和稳定性直接决定了自动驾驶系统的性能表现。LQR（线性二次型调节器）作为一种经典的最优控制算法，通过数学上的最优解来平衡系统状态跟踪和控制输入消耗，为轨迹跟踪问题提供了优雅的解决方案。

我曾在多个自动驾驶项目中应用LQR算法进行轨迹跟踪控制，发现其特别适合处理车辆在低速到中速范围内的轨迹跟踪问题。相比PID控制，LQR能够更系统地考虑多个状态变量之间的耦合关系；而与MPC（模型预测控制）相比，LQR又具有计算量小的优势，非常适合实时性要求高的场景。

2. 控制模型建立

2.1 四自由度动力学模型

我们采用的车辆动力学模型包含四个关键状态变量：

• 质心侧偏角β：描述车辆实际运动方向与车头指向的夹角
• 横摆角速度ωr：车辆绕垂直轴的旋转角速度
• 横向误差ey：车辆当前位置与期望轨迹的横向距离
• 航向误差eθ：车辆当前航向与期望航向的角度差

这个四自由度模型能够较好地平衡模型精度和计算复杂度。在实际项目中，我发现这个模型对于车速在60km/h以下的城市自动驾驶场景已经足够精确。

2.2 模型方程详解

2.2.1 质心侧偏角动力学

质心侧偏角的微分方程为：
[ \dot{\beta} = \frac{1}{mV_x} \left( -2C_f \left(\beta + \frac{l_f}{V_x}\omega_r\right) + 2C_r \left(\frac{l_r}{V_x}\omega_r - \beta\right) \right) ]

其中各参数含义：

• m：车辆质量（kg）
• Vx：纵向速度（m/s）
• Cf, Cr：前后轮侧偏刚度（N/rad）
• lf, lr：质心到前后轴距离（m）

注意：侧偏刚度会随轮胎磨损、路面条件变化，实际应用中需要考虑自适应估计或鲁棒控制。

2.2.2 横摆角速度动力学

横摆角速度的微分方程为：
[ \dot{\omega_r} = \frac{1}{I_z} \left( 2l_fC_f \left(\beta + \frac{l_f}{V_x}\omega_r\right) - 2l_rC_r \left(\frac{l_r}{V_x}\o