《无人驾驶车辆模型预测控制》3.3.3工程实例

《无人驾驶车辆模型预测控制》3.3.3工程实例

• 主要针对《无人驾驶车辆模型预测控制》这本书的第三章3.3.3的工程实例进行详细的代码分析，结合自己在学习以及推导的MPC算法的一些理解。

书中源代码

• 以无人驾驶车辆的轨迹跟踪问题作为应用的背景，无人驾驶车辆在一个给定的位置出发，通过离散轨迹点（本工程实例使用的是离散的轨迹点）或者连续轨迹函数的指引，最终跟上期望的轨迹。
• 轨迹跟踪仿真基本设定：车辆从坐标原点出发，以期望纵向速度v=1m/s跟踪一条直线轨迹y=2。采样时间为0.05s（50ms），仿真的总时间设定为5s，在Matab中进行仿真。控制的目标是无人驾驶车辆在低速情况下的跟踪控制，因此考虑使用车辆的运动学方程作为预测模型。通过将车辆的运动学方程进行线性化（还有离散化的处理），得到线性时变模型。

%% 生成目标轨迹
N = 100; % 目标轨迹点的数量
T = 0.05; % 采样周期
Xout = zeros(N,3); % Xout用于存储目标轨迹点上的车辆状态；车辆共有3个状态变量：X轴坐标、Y轴坐标、航向角φ；Xout就是用来表示期望轨迹上车辆的状态
Tout = zeros(N,1); % Tout用于存储离散的时间序列
for k = 1:1:N
    Xout(k,1) = k*T; % 设置目标轨迹点的X轴坐标
    Xout(k,2) = 2; % 设置目标轨迹点的Y轴坐标；此仿真实例轨迹是一条直线y=2
    Xout(k,3) = 0; % 设置目标轨迹点的航向角;此仿真实例中由于轨迹是一条直线，所以车辆的航向角也是固定的，为0
    Tout(k,1) = (k-1)*T; % 由此可见，目标轨迹点对应的时刻，和采样时刻保持一致；整个仿真时长为100*0.05共计5秒（默认单位时长为1秒）
    % 由上可知，车辆在目标轨迹上，每两个轨迹点间的X轴的坐标相差0.05，每两个轨迹点对应的时刻相差0.05，因此车辆在X轴上的速度为1m/s
end

代码第一部分主要是根据仿真要求设定的一些基本的情况，参考轨迹后面可以自己进行修改，比如圆形轨迹或者8字形的的轨迹 ，本文的后面还有对轨迹进行修改的一个测试

%% 描述系统的基本情况
Nx = 3; % 状态量共有3个：X轴坐标、Y轴坐标、航向角φ；
Nu = 2; % 控制量共有2个：车辆的纵向速度v（后轴速度），前轮偏角δ

[Nr,Nc] = size(Xout); % 获取目标轨迹点车辆状态矩阵的维度，Nr代表目标点的数目100，Nc代表车辆状态的数目3；即100行3列
Tsim = 20; % MPC的预测时域，即当车辆处于时刻k时，对k+1,k+2,k+3,...,k+Tsim时刻的车辆状态进行预测
X0 = [0 0 pi/3]; % 车辆初始状态，X轴坐标为0，Y坐标为0，航向角pi/3
L = 1; % 车辆轴距
vd1 = 1; % 车辆的目标纵向速度v=1m/s
vd2 = 0; % 车辆的目标前轮偏角δ=0

代码的第二部分也没有什么好说的，也是一些车辆基本情况和MPC预测时域的设定

%% 定义相关矩阵
% x_real矩阵，用于存储每一个仿真时刻，车辆的实际位置状态，初始状态为上面定义的X0
x_real = zeros(Nr,Nc); %生成一个用来存储每一个仿真时刻车辆实际位置的空矩阵
x_real(1,:) = X0;%将车辆初始时刻的状态加入x_real
% x_piao矩阵，用于存储每一个仿真时刻，车辆的实际位置状态与目标位置状态之间的偏差
x_piao = zeros(Nr,Nc); %同样地，生成一个用来存储每一个仿真时刻车辆的实际位置状态与目标位置状态的偏差
x_piao(1,:) = x_real(1,:)-Xout(1,:);%车辆初始时刻所在的位置状态与目标位置状态之间的偏差
% X_PIAO的每一行代表一个仿真时刻（对应一个轨迹点）
% 在每一个仿真时刻，都需要对车辆未来20（Tsim）个时刻的实际位置状态与目标位置状态的偏差进行预测，所以X_PIAO每一行的数据可以分为20（Tsim）个组
% 每一组有3（Nx）列，分别对应车辆3个状态的偏差；100行，20X3列
X_PIAO = zeros(Nr,Nx*Tsim);%生成一个用来存储在每一个仿真时刻预测未来20（Tsim）个时域车辆与目标轨迹之间的状态偏差
% u_real矩阵，用于存储每一个仿真时刻，车辆的实际控制量（实际运动状态）（纵向速度、前轮偏角）
u_real = zeros(Nr,Nu);%100行2列
% u_piao矩阵，用于存储每一个仿真时刻，车辆的实际控制量（实际运动状态）与目标控制量（运动状态）之间的偏差
u_piao = zeros(Nr,Nu);%同样100行2列
% XXX的每一行代表一个仿真时刻（对应一个轨迹点）
% 在每一个仿真时刻，都需要对未来20个时刻的状态值进行预测，所以XXX每一行的数据可以分为20（Tsim）个组
% 每一组有3（Nx）列，分别对应车辆3个状态
XXX = zeros(Nr,Nx*Tsim);% 用于保存每个时刻预测的所有状态值100行60列
% q为加权矩阵，用于调节每个状态量（X轴坐标、Y轴坐标、航向角）在目标函数中比重（权重系数）
q = [1 0 0;0 1 0;0 0 0.5];
Q_cell = cell(Tsim,Tsim); % 元组Q_cell的作用是，将q作用于预测的每一个点上（共计Tsim个点）
for i = 1:1:Tsim
    for j = 1:1:Tsim
        if i == j
            Q_cell{i,j}=q;%将加权矩阵q赋给二维元胞数组行列索引位置相等处的数组元素
        else
            Q_cell{i,j}=zeros(Nx,Nx);%其他的位置都赋给0加权矩阵
        end
    end
end
Q = cell2mat(Q_cell); % 通过cell2mat()函数将元胞数组转化成最终的状态加权矩阵Q，调节每个预测点上，三个状态量各自在目标函数中比重；20X3行20X3列
R = 0.1*eye(Nu*Tsim,Nu*Tsim); % 最终的控制量加权矩阵，用于调节控制量偏差在目标函数中的比重

代码的第三部分是对相关矩阵的定义，其中XXX表示存储在每个仿真时刻预测的未来20个时刻的状态值这部分表示的含义需要进一步理解

%% 模型预测控制的主体函数
%在每一个仿真时刻，模型预测控制是如何进行预测、滚动优化以及反馈矫校正
for i = 1:1:Nr
    t_d = Xout(i,3); % t_d为i时刻的期望航向角
    % 下一预测点的状态偏差 = a*当前点的状态偏差 + b*当前点的控制量偏差   （状态偏差，即车辆位置偏差；控制量偏差，即车辆运动偏差）
    a = [1 0 -vd1*sin(t_d)*T;   % 矩阵a，是关于目标车速和目标航向角的函数；
        0 1 vd1*cos(t_d)*T; % 由于目标车速和目标航向角保持恒定
        0 0 1;]; % 该仿真中给出的期望车速和航向都是不变的，所以，矩阵a恒定不变
    b = [cos(t_d)*T 0; % 与矩阵a相似，矩阵b也保持恒定
        sin(t_d)*T 0;
        tan(vd2)*T/L vd1*T/(L*cos(vd2)^2);];% 此处已经对书中的错误进行了更正，原书中给出的代码为：vd2*T/L vd1*T/(cos(vd2)^2);使用了近似
    % 目标函数，是预测时域内预测点与目标点的方差之和；  预测时域为Tsim，即存在20个预测点；  预测点与目标点方差之和为“状态方差（位置方差）” + “控制量方差（运动方差）”；
    A_cell = cell(Tsim,1);%A_cell为预测模型状态方差的系数矩阵
    B_cell = cell(Tsim,Tsim);B_cell为预测模型控制量方差的系数矩阵
    for j = 1:1:Tsim
         % 因为目标速度vd1、目标航向角φ保持不变，所以A(k|k)、A(k+1|k)、...A(k+Tsim|k)全都相同
        A_cell{j,1} = a^j;
        for k = 1:1:Tsim
            if k<=j
                B_cell{j,k} = (a^(j-k))*b;
            else
                B_cell{j,k} = zeros(Nx,Nu);
            end
        end
    end
    %通过cell2mat()函数将A_cell和B_cell分别转化成矩阵形式，得到预测模型的两个重要系数矩阵
    A = cell2mat(A_cell);%60行3列
    B = cell2mat(B_cell);%60行40列
    %接下来将预测模型带入到给定的目标函数（目标函数的形式是别人给出的）并通过转化为标准的二次规划问题（quadratic programming）
    %按照标准形式1/2x'Hx+f'x来求解；这里求解使用matlab中这种[x,fval,exitflag,output] = quadprog(H,f,A,b,Aeq,beq,lb,ub)形式的方法来求解
    H = 2*(B'*Q*B + R);%根据二次规划的标准型，定义相关的矩阵
    f = 2*B'*Q*A*x_piao(i,:)';
    A_cons = [];
    b_cons = [];
    %对两个控制量偏差进行约束，分别定义速度偏差和前轮转向偏差的上下限
    % 每一步， -2.2 <= v - vr <= 0.2
    % 每一步， -0.64 <= 前轮偏角 - 目标前轮偏角 <= 0.64
    ub = [0.2; 0.64];    lb = [-2.2; -0.64]; 
    % 通过二次规划求解，得到的X，为最优控制偏差矩阵[Dlt_u,Dlt_u1,Dlt_u2,Dlt_u3,...,Dlt_u19]
    [X,fval(i,1),exitflag(i,1),output(i,1)] = quadprog(H,f,A_cons,b_cons,[],[],lb,ub);
    % 通过运动学公式，计算得到在控制偏差矩阵[Dlt_u,Dlt_u1,Dlt_u2,Dlt_u3,...,Dlt_u19]作用下的状态偏差矩阵
    % [Dlt_x_1,Dlt_x_2,Dlt_x_3,Dlt_x1_1,Dlt_x2_1,Dlt_x3_1, Dlt_x1_2,Dlt_x2_2,Dlt_x3_2, ..., Dlt_x1_19,Dlt_x2_19,Dlt_x3_19]
    % 并将其存储在 X_PIAO(i,:) 中
    X_PIAO(i,:) = (A*x_piao(i,:)'+B*X)';%1行60列
    
    % 下面的j，虽然是上面for循环的变量，但是MATLAB不会将j自动清零；所以，j的大小即为Tsim(20)
    if i+j < Nr % i代表当前时刻，j代表相对于当前时刻向前预测的Tsim时刻；此句代表预测的时刻没有超出整个仿真时长
        for k = 1:1:Tsim
            % 存储预测到的未来Tsim（20）个时域内状态值（预测值 = 在最优控制偏差下得到的状态偏差值 + 目标状态值
            XXX(i,1+3*(k-1)) = X_PIAO(i,1+3*(k-1)) + Xout(i+k,1); 
            XXX(i,2+3*(k-1)) = X_PIAO(i,2+3*(k-1)) + Xout(i+k,2);
            XXX(i,3+3*(k-1)) = X_PIAO(i,3+3*(k-1)) + Xout(i+k,3);
        end
    else
    %如果预测时域超出了整个的仿真的时长，预测的状态都是相对目标点的
        for k = 1:1:Tsim
            XXX(i,1+3*(k-1)) = X_PIAO(i,1+3*(k-1)) + Xout(Nr,1);
            XXX(i,2+3*(k-1)) = X_PIAO(i,2+3*(k-1)) + Xout(Nr,2);
            XXX(i,3+3*(k-1)) = X_PIAO(i,3+3*(k-1)) + Xout(Nr,3);
        end
    end
    %分别取最优控制序列的第一个元素，包括纵向速度和前轮转向偏角；将他们分别赋给u_piao，
    %即将这个最优解作为当前时刻车辆实际的控制量与目标控制量之间的控制量偏差
    u_piao(i,1) = X(1,1); 
    u_piao(i,2) = X(2,1); 
    
    Tvec = [0:0.05:4];%定义的这个矩阵是何意？
    x00 = x_real(i,:); % x00为当前时刻车辆的位置状态
    vd11 = vd1 + u_piao(i,1); % 依据MPC求得的最优控制偏移u_piao(i,1)，以及当前的控制量vd1，计算出下一时刻的控制量vd11
    vd22 = vd2 + u_piao(i,2); % 依据MPC求得的最优控制偏移u_piao(i,2)，以及当前的控制量vd2，计算出下一时刻的控制量vd22
    % 知道了当前时刻的位置状态、运动状态（控制量）
    % 下面根据运动学方程，求解下一时刻车辆的位置状态
    %下面是求解的微分方程
    XOUT = dsolve('Dx - vd11*cos(z) = 0',...
        'Dy - vd11*sin(z) = 0',...
        'Dz - vd22 = 0',...
        'x(0) = x00(1)',...
        'y(0) = x00(2)',...
        'z(0) = x00(3)');
    t = T; % T为采样周期
    x_real(i+1,1) = eval(XOUT.x);
    x_real(i+1,2) = eval(XOUT.y);
    x_real(i+1,3) = eval(XOUT.z);
    if (i<Nr)
        x_piao(i+1,:) = x_real(i+1,:) - Xout(i+1,:);
    end
    % 根据目标运动状态（目标速度和目标前轮转角），以及上述求得的最优控制偏差（运动量偏差）
    % 计算当前最佳的控制量（当前速度、当前转向角）
    u_real(i,1) = vd1 + u_piao(i,1); % vd1为目标控制量：目标速度
    u_real(i,2) = vd2 + u_piao(i,2); % vd1为目标控制量：目标前轮转向角
    
    figure(1); 
    plot(Xout(1:Nr,1),Xout(1:Nr,2));    
    hold on;
    plot(x_real(i,1),x_real(i,2),'r *')
    xlabel('X[m]'); 
    axis([-1 5 -1 3]);
    ylabel('Y[m]');
    hold on;
    %绘制未来Tsim时域内预测的车辆的状态（黄线）
    for k = 1:1:Tsim
        X(i,k+1) = XXX(i,1+3*(k-1));
        Y(i,k+1) = XXX(i,2+3*(k-1));
    end
    X(i,1) = x_real(i,1);
    Y(i,1) = x_real(i,2);
    plot(X(i,:),Y(i,:),'y')
    hold on    
end

第四部分的代码也是代码的主要核心的部分，需要多次反复的来阅读理解。。。最后绘制了figure(1)将车辆在MPC作用下跟踪参考轨迹的过程表现了出来

%系统状态量随时间变化
figure(2)
subplot(3,1,1);
plot(Tout(1:Nr),Xout(1:Nr,1));
hold on;
plot(Tout(1:Nr),x_real(1:Nr,1),'r');
xlabel('采样时间T');
ylabel('横向位置X')%绘制了状态量X随着时间的变化（红线是车辆实际的状态量X）
subplot(3,1,2);
plot(Tout(1:Nr),Xout(1:Nr,2));
hold on;
plot(Tout(1:Nr),x_real(1:Nr,2),'r');
xlabel('采样时间T');
ylabel('纵向位置Y')%绘制了状态量Y随时间的变化（红线是车辆实际的状态量Y）
subplot(3,1,3);
plot(Tout(1:Nr),Xout(1:Nr,3));
hold on;
plot(Tout(1:Nr),x_real(1:Nr,3),'r');
hold on;
xlabel('采样时间T');
ylabel('\theta')%绘制了状态量φ随时间的变化（红线是车辆的实际状态量φ）

第五部分的代码主要是绘制了车辆的实际状态量和目标状态量随着时间的变化

figure(3)
subplot(2,1,1);
plot(Tout(1:Nr),u_real(1:Nr,1),'r');%绘制车辆实际速度随时间变化的曲线
xlabel('采样时间T');
ylabel('纵向速度')
subplot(2,1,2)
plot(Tout(1:Nr),u_real(1:Nr,2),'r');绘制车辆前轮偏角随时间变化的曲线
xlabel('采样时间T');
ylabel('角速度')%#####这里应该不是指的角速度而是指前轮（方向盘）转角吧

第六部分的代码绘制了车辆的两个控制量（速度v和前轮转角δ）随着时间的变化曲线

figure(4)
subplot(3,1,1);
plot(Tout(1:Nr),x_piao(1:Nr,1));
xlabel('采样时间T');
ylabel('e(x)');%绘制车辆实际状态量X与参考状态量X之间的偏差曲线
subplot(3,1,2);
plot(Tout(1:Nr),x_piao(1:Nr,2));
xlabel('采样时间T');
ylabel('e(y)');%绘制车辆实际状态量Y与参考状态量Y之间的偏差曲线
subplot(3,1,3);
plot(Tout(1:Nr),x_piao(1:Nr,3));
xlabel('采样时间T');
ylabel('e(\theta)');%绘制车辆实际状态量φ与参考状态量φ之间的偏差曲线

第七部分的代码绘制了车辆当前实际的三个状态量与目标状态量之间的偏差，至此，整个的程序结束。