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

1. LQR车辆轨迹跟踪系统概述

在自动驾驶和高级驾驶辅助系统(ADAS)开发中，轨迹跟踪控制是核心算法之一。基于线性二次调节器(LQR)的控制器因其优秀的稳定性和可调性，成为工程实践中的热门选择。本文将详细介绍如何从二自由度车辆模型出发，结合Frenet坐标系和五次多项式轨迹规划，构建一套完整的LQR轨迹跟踪系统。

这个系统的独特之处在于将前馈控制与LQR反馈控制相结合，有效解决了纯反馈控制存在的稳态误差问题。我们在CarSim和Simulink联合仿真环境下验证，跟踪精度可达0.1米以内，完全满足自动驾驶对轨迹跟踪的基本要求。

2. 系统建模与状态空间方程

2.1 二自由度车辆动力学模型

二自由度模型(又称自行车模型)是车辆横向动力学分析的基础，它保留了横摆和侧向运动两个自由度，同时做了以下合理假设：

• 忽略悬架系统影响
• 假设左右轮胎特性相同
• 不考虑纵向动力学耦合
• 小角度假设(转向角小于15°)

在Frenet坐标系下，我们选择状态向量为[x, y, φ, v]，分别代表横向位置误差、横向速度、航向角误差和横摆角速度。控制输入为前轮转角δ。

2.2 模型线性化处理

原始非线性动力学方程需要在其平衡点附近进行泰勒展开线性化。关键步骤包括：

1. 确定工作点(通常选择直行工况)
2. 计算雅可比矩阵
3. 忽略高阶小量

得到的线性化状态空间方程为：

code复制ẋ = Ax + Bu
y = Cx + Du

其中系统矩阵A和输入矩阵B的推导过程如下：

matlab复制% 车辆参数初始化
m = 1573;       % 质量(kg)
Iz = 2873;      % 转动惯量(kg·m²)
lf = 1.1;       % 前轴到质心距离(m)
lr = 1.58;      % 后轴到质心距离(m) 
Caf = 80000;    % 前轮侧偏刚度(N/rad)
Car = 80000;    % 后轮侧偏刚度(N/rad)
vx = 20;        % 纵向速度(m/s)

% 状态矩阵A
A = [0, 1, 0, 0;
     0, -(Caf+Car)/(m*vx), (Caf+Car)/m, (lf*Caf - lr*Car)/(m*vx);
     0, 0, 0