基于Matlab使用LQR实现车辆轨迹跟踪…

在自动驾驶领域，车辆轨迹跟踪是一项核心技术，它关乎车辆能否按照预设路径安全、准确地行驶。今天咱们就来聊聊基于Matlab使用线性二次型调节器（LQR）实现车辆轨迹跟踪这一有趣的话题。

LQR原理简述

LQR的核心思想是通过设计一个反馈控制器，使得系统在满足一定性能指标的情况下，输出尽可能地跟踪参考输入。简单理解，就是让系统在稳定运行的同时，尽可能减小实际输出与期望输出之间的差距。性能指标通常定义为一个二次型函数，通过调整该函数中的权重矩阵，可以平衡系统的不同性能需求，比如快速响应和较小的控制输入变化。

Matlab实现车辆轨迹跟踪

车辆模型建立

首先，咱们得建立一个车辆模型。这里以简单的自行车模型为例，车辆的运动状态可以用以下状态方程描述：

\[

\begin{align*}

\dot{x} &= v \cos(\theta + \beta) \\

\dot{y} &= v \sin(\theta + \beta) \\

\dot{\theta} &= \frac{v}{L} \tan(\delta) \\

\dot{v} &= a

\end{align*}

\]

基于Matlab使用LQR实现车辆轨迹跟踪…

其中，\((x, y)\)是车辆的位置坐标，\(\theta\)是车辆的航向角，\(v\)是车辆速度，\(a\)是车辆加速度，\(\delta\)是前轮转向角，\(L\)是车辆轴距，\(\beta\)是车辆质心侧偏角（这里假设为小角度可忽略）。

在Matlab中可以这样简单表示车辆模型（这里只是简化示意，实际应用可能更复杂）：

% 车辆参数
L = 2.5; % 轴距
dt = 0.1; % 采样时间

% 状态变量初始化
x = 0; y = 0; theta = 0; v = 0;

% 控制输入（这里假设简单给定加速度和转向角）
a = 1;
delta = 0.1;

% 离散化模型更新状态
x = x + v * cos(theta) * dt;
y = y + v * sin(theta) * dt;
theta = theta + (v / L) * tan(delta) * dt;
v = v + a * dt;

这段代码首先定义了车辆的一些基本参数，然后初始化了车辆的状态变量。接着给定了简单的控制输入（加速度和转向角），最后通过离散化的方式更新车辆的状态。

LQR控制器设计

设计LQR控制器，关键在于确定反馈增益矩阵 \(K\)。这需要先定义系统的状态空间模型 \(A\)、控制输入矩阵 \(B\)，以及权重矩阵 \(Q\) 和 \(R\)。

假设系统状态向量 \(\mathbf{x} = [x, y, \theta, v]^T\)，控制输入向量 \(\mathbf{u} = [a, \delta]^T\)。

线性化后的状态空间模型为：

\[

\dot{\mathbf{x}} = A \mathbf{x} + B \mathbf{u}

\]

这里的 \(A\) 和 \(B\) 矩阵需要根据车辆模型在某一工作点线性化得到。具体计算过程比较复杂，咱们这里假设已经得到了合适的 \(A\) 和 \(B\) 矩阵。

权重矩阵 \(Q\) 用于惩罚状态变量偏离期望轨迹的程度，\(R\) 用于惩罚控制输入的大小。比如：

% 权重矩阵
Q = [100 0 0 0; 0 100 0 0; 0 0 10 0; 0 0 0 1];
R = [1 0; 0 1];

% 求解LQR反馈增益矩阵K
K = lqr(A, B, Q, R);

上述代码中，通过 lqr 函数，根据给定的 \(A\)、\(B\)、\(Q\) 和 \(R\) 矩阵，计算出反馈增益矩阵 \(K\)。

轨迹跟踪实现

有了车辆模型和LQR控制器，就可以实现轨迹跟踪了。假设我们有一个预先定义好的参考轨迹 \(\mathbf{x}{ref} = [x{ref}, y{ref}, \theta{ref}, v_{ref}]^T\)。

% 参考轨迹（假设简单定义一段直线轨迹）
x_ref = 0:0.1:10;
y_ref = zeros(size(x_ref));
theta_ref = zeros(size(x_ref));
v_ref = ones(size(x_ref)) * 5;

% 初始化状态
x = 0; y = 0; theta = 0; v = 0;
trajectory_x = [];
trajectory_y = [];

for i = 1:length(x_ref)
    % 当前状态向量
    x_state = [x; y; theta; v];
    
    % 计算控制输入
    u = -K * (x_state - [x_ref(i); y_ref(i); theta_ref(i); v_ref(i)]);
    
    a = u(1);
    delta = u(2);
    
    % 更新车辆状态
    x = x + v * cos(theta) * dt;
    y = y + v * sin(theta) * dt;
    theta = theta + (v / L) * tan(delta) * dt;
    v = v + a * dt;
    
    % 记录轨迹
    trajectory_x = [trajectory_x; x];
    trajectory_y = [trajectory_y; y];
end

% 绘图
figure;
plot(x_ref, y_ref, 'b--', 'DisplayName', '参考轨迹');
hold on;
plot(trajectory_x, trajectory_y, 'r', 'DisplayName', '实际轨迹');
legend;
xlabel('X坐标');
ylabel('Y坐标');
title('基于LQR的车辆轨迹跟踪');

这段代码首先定义了一个简单的直线参考轨迹，然后在循环中，根据当前车辆状态与参考轨迹的偏差，通过LQR控制器计算控制输入，更新车辆状态，并记录实际行驶轨迹。最后通过绘图展示参考轨迹和实际轨迹。

通过以上步骤，我们基于Matlab使用LQR实现了车辆轨迹跟踪。当然，实际的自动驾驶场景要复杂得多，比如需要考虑更多的车辆动力学因素、传感器噪声等，但这种基本的实现方式为更深入的研究和应用奠定了基础。希望这篇博文能给对自动驾驶轨迹跟踪感兴趣的小伙伴们一些启发！