探索横纵向车辆轨迹跟踪：LQR与模糊PID的奇妙之旅

ifqhzJwse

160人浏览 · 2026-03-22 10:00:00

ifqhzJwse · 2026-03-22 10:00:00 发布

横纵向车辆轨迹跟踪（LQR/模糊PID）基于二自由度动力学模型与自然坐标系下建立误差模型，设计前馈LQR控制器，控制车辆的横向和横摆运动；在此基础上，设计双PID纵向控制器控制车辆纵向速度与位移，为了提高控制效果，纵向位移跟踪采用模糊PID控制，这可以自适应的调整PID的参数。轨迹规划基于五次多项式拟合所得。通过Simulink与Carsim联合仿真，验证了控制器的控制效果。设置了三条轨迹，单移线，双移线和蛇形工况。资料中有matlab代码，simulink模型和介绍资料(自制)，资料包括详细的建模过程、算法内容，以及使用方法。

嘿，各位对车辆控制感兴趣的小伙伴们！今天咱们来聊聊超酷的横纵向车辆轨迹跟踪，这里面用到了LQR和模糊PID控制算法，可有意思啦。

一、基本思路概述

咱们这次的目标是实现车辆横纵向的轨迹跟踪。简单来说，就是让车辆能按照我们规划好的轨迹稳稳地行驶。为了达到这个目标，我们采用了一些巧妙的方法。

首先基于二自由度动力学模型与自然坐标系建立了误差模型，这就像是给车辆的运动状态做了一个精准的“体检表”，能清楚知道车辆实际运动和我们期望运动之间的差距。基于这个误差模型，我们设计了前馈LQR控制器，它的作用是控制车辆的横向和横摆运动，就像给车辆装了个智能“方向盘”，让车辆能灵活地转弯。

在纵向方面，设计了双PID纵向控制器来控制车辆的纵向速度与位移。而且为了让控制效果更好，纵向位移跟踪采用了模糊PID控制，它能自适应地调整PID的参数，就像给车辆的“油门和刹车”加了个智能调节装置。

轨迹规划呢，是基于五次多项式拟合得到的，这就保证了我们规划的轨迹既平滑又符合实际行驶需求。

二、代码与分析

前馈LQR控制器代码示例

% 二自由度动力学模型参数
m = 1500; % 车辆质量
Iz = 2500; % 车辆转动惯量
lf = 1.2; % 质心到前轴距离
lr = 1.3; % 质心到后轴距离
Cf = 80000; % 前轮侧偏刚度
Cr = 80000; % 后轮侧偏刚度

% 建立状态空间方程
A = [-2*(Cf+Cr)/(m*vx), -2*(lf*Cf - lr*Cr)/(m*vx^2) - vx;
    -2*(lf*Cf - lr*Cr)/Iz, -2*(lf^2*Cf + lr^2*Cr)/(Iz*vx)];
B = [2*Cf/m, 0;
    2*lf*Cf/Iz, 1/Iz];

% LQR控制器设计
Q = [10, 0;
    0, 1]; % 状态权重矩阵
R = 1; % 输入权重矩阵
K = lqr(A, B, Q, R); % 计算LQR增益矩阵

这段代码首先定义了二自由度动力学模型的一些关键参数，比如车辆质量、转动惯量、前后轮侧偏刚度等。然后根据这些参数建立了状态空间方程，这里的A和B矩阵描述了车辆状态的变化和输入的影响。接着通过lqr函数计算LQR增益矩阵K，这个矩阵就是LQR控制器的核心，它决定了如何根据车辆的状态误差来调整控制输入。

双PID纵向控制器代码示例

% 初始化PID参数
Kp_v = 1; % 速度PID比例系数
Ki_v = 0.1; % 速度PID积分系数
Kd_v = 0.01; % 速度PID微分系数

Kp_d = 1; % 位移PID比例系数
Ki_d = 0.1; % 位移PID积分系数
Kd_d = 0.01; % 位移PID微分系数

% 模拟纵向控制过程
for i = 1:length(time)
    % 速度误差
    error_v = desired_v(i) - current_v(i);
    % 位移误差
    error_d = desired_d(i) - current_d(i);
    
    % 速度PID控制
    integral_v = integral_v + error_v * dt;
    derivative_v = (error_v - prev_error_v) / dt;
    control_v = Kp_v * error_v + Ki_v * integral_v + Kd_v * derivative_v;
    
    % 位移PID控制
    integral_d = integral_d + error_d * dt;
    derivative_d = (error_d - prev_error_d) / dt;
    control_d = Kp_d * error_d + Ki_d * integral_d + Kd_d * derivative_d;
    
    prev_error_v = error_v;
    prev_error_d = error_d;
end

这里定义了速度和位移PID控制器的参数，然后模拟了纵向控制的过程。通过计算速度误差和位移误差，利用PID公式计算控制量。integral用于积分项的计算，derivative用于微分项的计算，不断更新控制量，让车辆的速度和位移接近我们期望的值。

模糊PID控制代码示例

% 模糊规则库
fis = newfis('fuzzy_pid');

% 输入变量
fis = addvar(fis, 'input', 'error', [-10, 10]);
fis = addvar(fis, 'input', 'derror', [-5, 5]);

% 输出变量
fis = addvar(fis, 'output', 'Kp', [0, 2]);
fis = addvar(fis, 'output', 'Ki', [0, 0.5]);
fis = addvar(fis, 'output', 'Kd', [0, 0.1]);

% 添加隶属度函数和模糊规则（此处省略具体规则添加）

% 模糊推理
output = evalfis([error, derror], fis);
Kp = output(1);
Ki = output(2);
Kd = output(3);

模糊PID控制的核心就是根据误差和误差变化率来调整PID的参数。这里首先创建了一个模糊推理系统fis，定义了输入变量（误差和误差变化率）和输出变量（PID的三个参数）。然后可以添加隶属度函数和模糊规则，最后通过evalfis函数进行模糊推理，得到调整后的PID参数。