1. 项目背景与核心价值
车辆轨迹跟踪控制在自动驾驶和高级驾驶辅助系统(ADAS)中扮演着关键角色。传统PID控制器在复杂工况下往往表现不佳，而LQR(线性二次调节器)作为现代控制理论中的经典最优控制算法，能够通过状态反馈实现多变量系统的最优控制。这个项目通过建立四自由度车辆动力学模型，将LQR控制算法应用于轨迹跟踪场景，为自动驾驶控制算法开发提供了可靠的仿真验证平台。
在实际工程应用中，车辆轨迹跟踪需要同时考虑横向位置误差、航向角误差以及它们的微分项，这正是LQR算法能够完美处理的典型多输入多输出(MIMO)控制问题。相比简单的几何跟踪算法如Pure Pursuit，基于动力学模型的LQR控制能够更好地反映车辆实际运动特性，特别是在高速或低附着系数路面条件下。
2. 四自由度车辆动力学建模
2.1 模型结构与假设条件
四自由度车辆模型考虑了纵向运动、横向运动、横摆运动以及侧倾运动，比常用的二自由度自行车模型更能准确描述车辆动态特性。建模时我们做了以下合理假设：
- 忽略悬架动力学，将车辆视为刚体
- 假设前后轮侧偏特性在线性区间
- 不考虑空气动力学影响
- 假设路面平整且附着系数均匀
2.2 关键动力学方程推导
车辆动力学模型的核心是建立运动微分方程。我们定义以下状态变量：
- 纵向速度u
- 横向速度v
- 横摆角速度r
- 侧倾角φ
控制输入为：
- 前轮转角δ
- 纵向驱动力Fx
通过牛顿-欧拉方程可推导出状态空间方程：
code复制ẋ = Ax + Bu
y = Cx + Du
其中系统矩阵A和控制矩阵B的具体形式需要通过轮胎模型(如魔术公式)和车辆参数计算得到。一个典型的轿车参数集包括：
- 质量m=1500kg
- 轴距L=2.7m
- 前轴距质心距离a=1.2m
- 后轴距质心距离b=1.5m
- 绕z轴转动惯量Iz=2500kg·m²
- 轮胎侧偏刚度Cf=Cr=80000N/rad
注意：实际建模时需要根据具体车辆参数调整这些值，不正确的参数会导致模型失真。
3. LQR控制器设计与实现
3.1 LQR算法原理
LQR算法的核心是求解Riccati方程，找到使以下代价函数最小的控制律：
J = ∫(xᵀQx + uᵀRu)dt
其中：
- Q是状态权