自动驾驶路径跟踪：LQR控制与动力学模型实践

1. 自动驾驶路径跟踪的核心挑战

在自动驾驶系统中，路径跟踪是最基础也最关键的环节之一。想象一下你开车时双手离开方向盘，车辆需要自动沿着预定路线行驶——这就是路径跟踪要解决的问题。但现实情况远比这复杂：车辆运动存在惯性、轮胎与地面存在滑动摩擦、传感器存在测量误差，这些因素都会导致实际轨迹偏离预期路径。

传统PID控制器在低速场景下表现尚可，但当车速超过60km/h时，由于未考虑车辆动力学特性，控制效果会急剧恶化。这正是我们需要动力学跟踪误差模型配合LQR算法的根本原因。

2. 动力学跟踪误差模型解析

2.1 车辆动力学建模基础

建立准确的车辆动力学模型是控制的前提。我们采用经典的自行车模型（Bicycle Model）作为基础框架，该模型通过以下假设简化问题：

• 忽略车辆悬架运动
• 将左右轮胎合并为单个等效轮胎
• 仅考虑二维平面运动

模型的核心状态变量包括：

math复制x = [e_y, e_\psi, v_x, v_y, \omega]^T

其中e_y为横向位置误差，e_\psi为航向角误差，v_x/v_y为纵向/横向速度，ω为横摆角速度。

2.2 跟踪误差的微分方程

在 Frenet 坐标系下推导得到误差动力学方程：

code复制de_y/dt = v_x * sin(e_ψ) + v_y * cos(e_ψ)
de_ψ/dt = ω - κ * v_x

其中κ为路径曲率。通过小角度近似（sinθ≈θ, cosθ≈1）线性化后，可得状态空间表达式：

code复制ẋ = A x + B u

这个线性化模型将成为LQR控制器设计的基础。

注意：当路径曲率较大或车速较高时，小角度近似会导致模型失准。此时需要采用更复杂的非线性控制方法，如MPC。

3. LQR控制算法实现

3.1 LQR基本原理

线性二次调节器（LQR）通过最小化代价函数来求解最优控制律：

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

其中：

• Q：状态权重矩阵（调节跟踪误差的惩罚）
• R：控制权重矩阵（调节控制量的惩罚）

通过求解Riccati方程得到最优反馈矩阵K，控制律为：

code复制u = -Kx

3.2 权重矩阵设计实战

Q和R的选择直接影响控制性能。经过多次实车测试，我们总结出以下经验参数：

python复制Q = np.diag([10, 5, 1, 1, 2])  # 横向误差权重最大
R = np.diag([0.1, 0.1])        # 转向和加速度控制权重

具体调参技巧：

1. 先固定R为小单位矩阵，只调整Q
2. 从最大误差项开始（这里是e_y），逐步增加直到振荡
3. 最后微调R防止执行器饱和

4. Simulink仿真实现

4.1 仿真框架搭建

在Simulink中构建完整仿真模型：

code复制[路径生成] → [车辆模型] → [LQR控制器] → [执行器模型] → [可视化]

关键模块参数设置：

• 车辆质量：1500kg
• 轮胎侧偏刚度：-60000N/rad
• 转向系统延迟：100ms
• 执行器速率限制：0.5rad/s

4.2 典型测试场景

我们设计了三类测试路径：

1. 双移线测试（检验瞬态响应）
2. 圆形路径（检验稳态精度）
3. 曲率连续变化路径（检验适应性）

仿真结果指标：

5. 工程实践中的关键问题

5.1 模型失配补偿

实际车辆参数会随载重、胎压等因素变化。我们采用在线最小二乘法实时估计关键参数：

matlab复制function [C_alpha] = estimateTireStiffness(v_y, omega, delta)
    % 基于滑模观测器的实时估计
    ...
end

5.2 控制延迟处理

针对CAN总线延迟（典型值80-120ms），采用Smith预估器进行补偿：

code复制u(t) = K * x(t + τ) ≈ K * (x(t) + τ * ẋ(t))

6. 算法优化方向

当前系统的局限性及改进方案：

1. 大曲率路径跟踪：引入前馈控制项

   code复制u_ff = (m*v²/R) * (a/L)
   

2. 执行器饱和：增加抗饱和补偿
3. 路面附着变化：融合μ估计模块

实测数据表明，经过优化的LQR控制器在干燥路面可实现：

• 100km/h速度下<0.2m的跟踪精度
• 横向加速度3m/s²时的稳定控制

这种方案已成功应用于园区物流车和港口AGV等低速场景，下一步将针对乘用车高速场景进行迭代优化。