无人驾驶车辆模型预测控制Matlab代码

clc;
clear;

%%
%参考轨迹生成
N = 100;       %参考轨迹点数量
T = 0.05;      %采样周期
Xout = zeros（N3）;
Tout = zeros（N1）;
for k = 1:1:N
    Xout（k1） = k*T;        %参考x坐标
    Xout（k2） = 2;          %参考y坐标
    Xout（k3） = 0;          %参考航向角
    Tout（k1） = （k-1）*T;    %第k个参考点时刻
end
%%
%控制系统基本情况介绍
Nx = 3;        %状态量个数
Nu = 2;        %控制量个数
[NrNc] = size（Xout）;    %Nr=100Nc=3
Tsim = 20;     %仿真时间
X0 = [0 0 pi/3]; %车辆初始状态
L = 1;         %车辆轴距
vd1 = 1;       %参考系统的纵向速度
vd2 = 0;       %参考系统前轮偏角
%%
%根据控制系统的维度信息，提前定义好相关矩阵并赋值
x_real = zeros（NrNc）;   %状态量实测信息
x_piao = zeros（NrNc）;   %状态量预测信息
u_real = zeros（NrNu）;   %控制量实际信息
u_piao = zeros（NrNu）;   %控制量预测信息
x_real（1:） = X0;           
x_piao（1:） = x_real（1:）-Xout（1:）;
X_PIAO = zeros（NrNx*Tsim）;
XXX = zeros（NrNx*Tsim）; %用于保存每个时刻预测的所有状态值
q = [1 0 0;0 1 0;0 0 0.5];
Q_cell = cell（TsimTsim）;
for i = 1:1:Tsim
    for j = 1:1:Tsim
        if i == j
            Q_cell{ij} = q;
        else
            Q_cell{ij} = zeros（NxNx）;
        end
    end
end
Q = cell2mat（Q_cell）;           %目标函数中Q矩阵
R = 0.1 * eye（Nu*TsimNu*Tsim）; %目标函数中R矩阵
%%
%模型预测控制的主体，在每一周期开始，获取系统当前状态量，更新状态空间方程。
%求解一个标准二次规划问题。将所得第一个解施加到系统上。
for i = 1:1:Nr
    t_d = Xout（i3）;
    a = [1 0 -vd1*sin（t_d）*T;
         0 1 vd1*cos（t_d）*T;
         0 0 1;];               %状态空间中的矩阵A
    b = [cos（t_d）*T 0;
         sin（t_d）*T 0;
         vd2*T/L vd1*T/（cos（vd2）^2）;]; %状态空间中的矩阵B
    A_cell = cell（Tsim1）;
    B_cell = cell（TsimTsim）;
    for j = 1:1:Tsim
        A_cell{j1} = a^j;
        for k = 1:1:Tsim
            if k <= j
                B_cell{jk}=（a^（j-k））*b;
            else
                B_cell{jk}=zeros（NxNu）;
            end
        end
    end
    A = cell2mat（A_cell）;   %预测模型中A矩阵
    B = cell2mat（B_cell）;   %预测模型中B矩阵
    
    H = 2 * （B‘ * Q * B + R）;
    f = 2 * B‘ * Q * A * x_piao（i:）‘;
    A_cons = [];
    b_cons = [];
    lb = [-2.3; -0.64];       %控制量下界
    ub = [0.3 ; 0.64];        %控制量上界
    [Xfval（i1） exitflag（i1）output（i1）] = quadprog（HfA_consb_cons[][]lbub）;
    X_PIAO（i:） = （A*x_piao（i:）‘+ B * X）‘;
    if i+j < Nr
        for j = 1:1:Tsim
            XXX（i1 + 3 * （j-1））=X_PIAO（i1+3*（j-1））+Xout（i+j1）;
            XXX（i2 + 3 * （j-1））=X_PIAO（i2+3*（j-1））+Xout（