基于LQR的车辆轨迹跟踪控制Matlab实现

1. 项目概述

在自动驾驶和智能车辆控制领域，轨迹跟踪是一个基础但至关重要的技术问题。最近我在一个车辆控制项目中，使用Matlab实现了基于LQR（线性二次调节器）的轨迹跟踪控制器，效果相当不错。这种方法相比传统的PID控制，在处理非线性系统时表现更稳定，特别适合高速场景下的车辆控制。

LQR控制本质上是一种最优控制方法，它通过最小化一个包含状态误差和控制量的二次型代价函数，来获得系统的最优控制律。在车辆轨迹跟踪问题中，我们可以将车辆的运动学模型线性化，然后应用LQR设计控制器，使车辆能够准确跟踪预定轨迹。

2. 车辆建模与线性化

2.1 车辆运动学模型

首先需要建立车辆的运动学模型。我们采用自行车模型作为基础，这是车辆控制中常用的简化模型：

code复制ẋ = v * cos(θ + β)
ẏ = v * sin(θ + β)
θ̇ = (v / L) * sin(β)

其中：

• (x,y)是车辆后轴中心的位置坐标
• θ是车辆航向角
• v是车速
• L是轴距
• β是滑移角，β = atan((lr/L)*tan(δf))，lr是后轴到质心的距离，δf是前轮转角

2.2 模型线性化

为了应用LQR控制，我们需要在参考轨迹附近对模型进行线性化。假设参考轨迹上的状态为(x_ref, y_ref, θ_ref, v_ref)，控制量为δf_ref。

定义误差状态：

code复制e1 = (x - x_ref) * cos(θ_ref) + (y - y_ref) * sin(θ_ref)
e2 = -(x - x_ref) * sin(θ_ref) + (y - y_ref) * cos(θ_ref)
e3 = θ - θ_ref

线性化后的误差动力学模型可以表示为：

code复制ė = A * e + B * u

其中A和B是雅可比矩阵，u是控制输入。

3. LQR控制器设计

3.1 LQR基本原理

LQR通过最小化以下代价函数来设计控制器：

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

其中：

• Q是状态权重矩阵，决定对状态误差的惩罚程度
• R是控制权重矩阵，决定对控制量的惩罚程度

通过求解Riccati方程，可以得到最优反馈控制律：

code复制u = -Kx

其中K是反馈增益矩阵。

3.2 Matlab实现步骤

在Matlab中实现LQR控制器的主要步骤如下：

1. 定义系统矩阵A和B

matlab复制A = [0, 0, -v_ref*sin(θ_ref);
     0, 0, v_ref*cos(θ_ref);
     0, 0, 0];
B = [cos(θ_ref), 0;
     sin(θ_ref), 0;
     tan(δf_ref)/L, v_ref/(L*cos(δf_ref)^2)];

2. 设计权重矩阵Q和R

matlab复制Q = diag([10, 10, 5]);  % 位置误差权重较大
R = diag([1, 0.1]);     % 控制量权重较小

3. 计算LQR增益

matlab复制[K, S, E] = lqr(A, B, Q, R);

4. 实现控制律

matlab复制u = -K * e;  % e是误差状态向量

4. 轨迹跟踪实现

4.1 参考轨迹生成

我们可以用多种方法生成参考轨迹，这里采用三次样条插值：

matlab复制% 定义路径点
waypoints = [0, 0; 10, 5; 20, 10; 30, 15];

% 生成样条曲线
pp = spline(waypoints(:,1)', waypoints(:,2)');
x_ref = linspace(0, 30, 100);
y_ref = ppval(pp, x_ref);

4.2 闭环控制实现

主控制循环的实现逻辑：

matlab复制for k = 1:length(x_ref)-1
    % 计算当前误差状态
    e = compute_error_state(x(k), y(k), theta(k), x_ref(k), y_ref(k), theta_ref(k));
    
    % 计算控制量
    u = -K * e;
    
    % 应用控制量并更新车辆状态
    [x(k+1), y(k+1), theta(k+1)] = vehicle_model(x(k), y(k), theta(k), u(1), u(2), dt);
end

5. 仿真结果分析

5.1 跟踪性能评估

通过仿真可以得到以下性能指标：

• 横向误差：通常控制在0.1m以内
• 航向误差：控制在0.05rad以内
• 控制量变化平滑度

5.2 参数调优经验

在实际调试中发现几个关键点：

1. Q矩阵中对横向误差的权重应大于纵向误差
2. R矩阵中对转向角的权重需要适当增大以避免剧烈转向
3. 车速越高，需要更大的前视距离

6. 实际应用中的问题与解决

6.1 模型失配问题

当车辆实际动力学特性与模型差异较大时，跟踪性能会下降。解决方法：

• 增加模型不确定性补偿
• 采用自适应LQR或鲁棒LQR

6.2 计算实时性问题

对于嵌入式系统实现，需要考虑：

• Riccati方程的离线求解
• 控制律的简化计算
• 采样周期的合理选择

7. 扩展与改进方向

1. 结合MPC（模型预测控制）实现更优性能
2. 引入轮胎力模型考虑动力学效应
3. 增加扰动观测器提高鲁棒性
4. 与路径规划模块集成实现完整自动驾驶栈

在实际项目中，我发现LQR虽然理论优美，但真正发挥其性能需要深入理解车辆动力学特性。特别是在高速场景下，简单的运动学模型可能不够精确，需要考虑更多的动力学因素。另外，权重矩阵的选择对控制性能影响很大，需要通过大量仿真和实车测试来确定最佳参数。