LQR控制在自动驾驶轨迹跟踪中的应用与实践

1. 车辆轨迹跟踪的核心挑战与LQR方案选择

在自动驾驶和高级驾驶辅助系统(ADAS)开发中，轨迹跟踪是最基础也最关键的环节之一。我曾在多个量产项目中负责控制算法开发，发现传统PID控制在高速、大曲率工况下容易出现超调甚至失稳。而LQR(线性二次调节器)因其最优控制特性，能很好地平衡跟踪精度与能耗效率。

为什么选择二自由度模型？在实车调试中，我们发现四自由度模型虽然更精确，但参数辨识成本高且实时性差。二自由度模型通过合理简化，保留了横摆和侧向运动这两个对轨迹跟踪影响最大的自由度，在保证精度的同时计算量减少60%以上。

2. 动力学建模与Frenet坐标系转换

2.1 二自由度车辆模型构建

建立如图1所示的自行车模型：

• 假设前后轮等效为单轮
• 忽略悬架运动和载荷转移
• 考虑侧偏刚度和轮胎非线性特性

状态方程推导关键步骤：

1. 建立车辆运动学方程：

   code复制m(v̇y + vxγ) = Fyf + Fyr
   Izγ̇ = lfFyf - lrFyr
   

2. 线性化轮胎模型（小侧偏角假设）：

   code复制Fyf = -Cfαf = -Cf(δ - (vy + lfγ)/vx)
   Fyr = -Crαr = -Cr(vy - lrγ)/vx 
   

2.2 Frenet坐标系转换技巧

全局坐标系到Frenet系的转换是工程实现中的难点。我的经验方法是：

1. 通过三次样条插值生成参考路径s(l)
2. 建立投影关系：

   python复制def cart_to_frenet(x, y, ref_path):
       # 寻找最近点索引
       distances = np.sqrt((ref_path[:,0]-x)**2 + (ref_path[:,1]-y)**2)
       idx = np.argmin(distances)
       
       # 计算横向偏差
       dx = x - ref_path[idx,0]
       dy = y - ref_path[idx,1]
       ey = -dx * np.sin(ref_path[idx,2]) + dy * np.cos(ref_path[idx,2])
       
       return ref_path[idx,3], ey  # 返回s和ey
   

3. 状态变量选择：

   code复制X = [ey, eψ, vx, vy, γ]^T
   

   其中ey为横向偏差，eψ为航向角偏差

3. LQR控制器设计与实现

3.1 状态空间方程离散化

采用零阶保持法离散化：

code复制Ad = I + A*Ts
Bd = B*Ts

其中Ts为控制周期，实测表明10ms周期下离散误差<2%

3.2 权重矩阵调参经验

Q和R矩阵的选取直接影响控制效果。经过多个项目积累，我总结出以下调参规律：

典型参数范围：

code复制Q = diag([1.0, 0.5, 0.1, 0.3, 0.2])  
R = diag([0.1])

3.3 实时求解优化

使用代数Riccati方程求解反馈矩阵K：

python复制from scipy.linalg import solve_continuous_are

P = solve_continuous_are(A, B, Q, R)
K = np.linalg.inv(R) @ B.T @ P

实测在树莓派4B上单次求解耗时<5ms

4. 工程实现中的关键问题

4.1 速度自适应调节

我们发现固定权重在速度变化时性能下降。解决方案：

1. 建立vx与Q/R的映射关系：

   code复制Q(1,1) = 1.0 + 0.05*(vx - 20)  # ey权重随速度调整
   

2. 采用增益调度策略，分低速(<30kph)、中速、高速(>80kph)三档

4.2 前馈补偿设计

单纯反馈控制会导致稳态误差。增加前馈项：

code复制δ_ff = (lr/R - lf/(2Cf) * (m*vx²)/(R*(lf+lr))) 

实测可将弯道跟踪误差降低40%

4.3 执行器延迟补偿

实测发现转向电机存在80-120ms延迟。解决方案：

1. 建立一阶延迟模型：

   code复制δ_actual = δ_cmd * 1/(0.1s + 1)
   

2. 在状态观测器中加入延迟补偿

5. 实测效果与参数整定建议

在某电动SUV上的测试数据：

给开发者的建议：

1. 先调Q矩阵确保稳定性，再调R矩阵优化控制量
2. 冬季测试时轮胎刚度需下调15-20%
3. 对于EPS系统，需在R矩阵中加入转向速率惩罚项

6. 扩展改进方向

在实际项目中，我们还尝试了以下增强方案：

1. 结合MPC实现预测控制，处理大延迟系统
2. 引入轮胎参数在线估计，适应低附路面
3. 采用强化学习自动优化Q/R参数

有个特别实用的调试技巧：在CANoe中建立Q/R参数与工程量的映射关系，通过面板滑块实时观察控制效果变化，这比离线调参效率提升5倍以上。