《无人驾驶车辆模型预测控制第二版》第四章代码详细注释版

%第4章
%第一部分主函数

function [sys,x0,str,ts] = MY_MPCController3(t,x,u,flag)
%t为当前仿真时间，x为状态变量，u是输入（simulink的输入）
%flag为仿真过程的标志位
switch flag,%sys输出根据flag的不同而不同
 case 0
  [sys,x0,str,ts] = mdlInitializeSizes; % Initialization初始化
  
 case 2
  sys = mdlUpdates(t,x,u); % Update discrete states 更新离散状态量
  
 case 3
  sys = mdlOutputs(t,x,u); % Calculate outputs 计算输出
 
 case {1,4,9} % Unused flags 滞空
  sys = [];
  
 otherwise%未知的flag值
  error(['unhandled flag = ',num2str(flag)]); % Error handling
end
% End of dsfunc.
 s函数主程序结束
%==============================================================
% Initialization
%==============================================================
 %第二部分初始化子函数

function [sys,x0,str,ts] = mdlInitializeSizes
 
sizes = simsizes; %初始化模块参数的结构体，sizes实际上是一个结构体变量
sizes.NumContStates  = 0; %连续状态量的个数
sizes.NumDiscStates  = 3; %离散状态量的个数 [X,Y,PHI]
sizes.NumOutputs     = 2; %输出量的个数 [speed, steering]
sizes.NumInputs      = 3; %输入量的个数 [X,Y,PHI]
sizes.DirFeedthrough = 1; % Matrix D is non-empty. 模块是否存在直接贯通（标示）（输入能直接控制输出）
sizes.NumSampleTimes = 1; %采样周期的个数
sys = simsizes(sizes);    %设置完后赋值给sys输出
x0 =[0;0;0];          %状态量初始化，横纵坐标以及横摆角都为0     
global U;             % store current ctrl vector:[vel_m, delta_m] %store chi_tilde=[vel-vel_ref; delta - delta_ref]
U=[0;0];%初始的控制量为0 , 0两行一列  
% Initialize the discrete states.
str = [];             % Set str to an empty matrix.说明变量，一般在初始化中将其滞空即可
ts  = [0.1 0];        % sample time: [period, offset] 采样周期+偏移量 仿真开始0s后每0.1秒运行一次
%End of mdlInitializeSizes
              
%==============================================================
% Update the discrete states
%==============================================================
%第三部分更新离散状态量子函数

function sys = mdlUpdates(t,x,u) %flag=2表示此时要计算下一个离散状态，即x（k+1)
  
sys = x; %更新前面算过的x状态，将x更新
%End of mdlUpdate.
 
%==============================================================
% Calculate outputs
%==============================================================
%第四部分计算输出子函数

function sys = mdlOutputs(t,x,u)   %flag=3表示此时要计算输出，如果sys=[],则表示没有输出
    global a b u_piao; %a,b和u_piao矩阵为全局矩阵
    global U; %store chi_tilde=[vel-vel_ref; delta - delta_ref]
    global kesi;%kesi为全局，新的状态向量，[k时刻的状态量误差；k-1时刻的控制量误差],5*1矩阵（误差即偏差）
 
    tic
    Nx=3;%状态量的个数
    Nu =2;%控制量的个数
    Np =60;%预测步长
    Nc=30;%控制步长
    Row=10;%松弛因子
    fprintf('Update start, t=%6.3f\n',t)%u(1)为X；u(2)为Y
    t_d = u(3)*3.1415926/180;% t_d为横摆角，CarSim输出的为角度，角度转换为弧度。u(3)是车辆的实际航向角
 
%    %直线路径
%     r(1)=5*t; %ref_x-axis
%     r(2)=5;%ref_y-axis
%     r(3)=0;%ref_heading_angle
%     vd1=5;% ref_velocity
%     vd2=0;% ref_steering
 
   
    %半径为25m的圆形轨迹, 圆心为(0, 35), 速度为5m/s
    r(1)=25*sin(0.2*t); %参考的X
    r(2)=35-25*cos(0.2*t);%参考的Y
    r(3)=0.2*t; %  参考的横摆角，w = (v/r) = (vr*tan(cita))/l = 0.2
    vd1=5; %速度5m/s，这是参考速度
    vd2=0.104; %前轮偏角=(轴距/半径）=(2.6/25)=0.104，这是参考前轮转角
 
%     %半径为25m的圆形轨迹, 圆心为(0, 35), 速度为3m/s
%     r(1)=25*sin(0.12*t);
%     r(2)=25+10-25*cos(0.12*t);
%     r(3)=0.12*t;
%     vd1=3;
%     vd2=0.104;
    %半径为25m的圆形轨迹, 圆心为(0, 35), 速度为10m/s
%      r(1)=25*sin(0.4*t);
%      r(2)=25+10-25*cos(0.4*t);
%      r(3)=0.4*t;
%      vd1=10;
%      vd2=0.104;
%     %半径为25m的圆形轨迹, 圆心为(0, 35), 速度为4m/s
%      r(1)=25*sin(0.16*t);
%      r(2)=25+10-25*cos(0.16*t);
%      r(3)=0.16*t;
%      vd1=4;
%      vd2=0.104;
 
   
    kesi=zeros(Nx+Nu,1); %构造新的状态量矩阵 ，5*1维矩阵
    kesi(1) = u(1)-r(1);%u(1)==X(1),x_offset    u（1）是指纵向位置x;  r(1)是指参考的纵向位置
    kesi(2) = u(2)-r(2);%u(2)==X(2),y_offset    u(2)是指横向位置；r(2)是指参考的横向位置。
    kesi(3) = t_d - r(3); %第三行是横摆角的误差。t_d是指车辆的横摆角；r(3)是指参考的横摆角。
    kesi(4)=U(1); % vel-vel_ref 上一时刻的速度误差
    kesi(5)=U(2); % steer_angle - steering_ref 上一时刻前轮偏角误差
    fprintf('vel-offset=%4.2f, steering-offset, U(2)=%4.2f\n',U(1), U(2))
 
    T=0.1;%采样时间为0.1s
    T_all=40;%临时设定，总的仿真时间。设置总的仿真时间防止仿真溢出。
    % Mobile Robot Parameters
    L = 2.6; % wheelbase of carsim vehicle 轴距为2.6m
    % Mobile Robot variable
    
    
    %矩阵初始化  
    u_piao=zeros(Nu,1); %构造3*2维度（认为这应该是Nu=2*1维度）的矩阵来存放控制量误差的变化量
    %u_piao=zeros(Nx,Nu);%构造3*2维度的矩阵来存放控制量误差
    Q=100 * eye(Nx*Np,Nx*Np);  %权重矩阵Q为180*180的单位矩阵，Q是指输出状态矩阵
    R=5*eye(Nu*Nc); %权重矩阵R为60*60的单位矩阵。R是指控制量误差的状态矩阵
    %R=5*eye(Nu*Nc); %权重矩阵R为60*60的单位矩阵
    t_d1 = r(3);%这里应该是参考值  原代码写的是实际输出的侧偏角（有误）
    a=[1    0   -vd1*sin(t_d1)*T;
       0    1   vd1*cos(t_d1)*T;
       0    0   1;];                         %a为我们线性离散化后的第一个系数矩阵
    b=[cos(t_d1)*T        0;
       sin(t_d1)*T        0;
       tan(vd2)*T/L      vd1*T/(cos(vd2)^2);];%b为我们线性离散化后的第二个系数矩阵
  
   %预测方程的A,B
    A_cell=cell(2,2); % 构建2*2的元胞数组
    B_cell=cell(2,1); %构建2*1的元胞数组
    A_cell{1,1}=a; %将a矩阵放到A_cell的第一行第一个位置
    A_cell{1,2}=b; %将b矩阵放到A_cell的第一行第二个位置
    A_cell{2,1}=zeros(Nu,Nx); %将2*3的零矩阵放到A_cell第二行的第一个位置
    A_cell{2,2}=eye(Nu); %将2*2的单位阵放到A_cell第二行的第二个位置
    B_cell{1,1}=b; %将b矩阵放到B_cell的第一行
    B_cell{2,1}=eye(Nu); %将2*2的单位阵放到B_cell第二行
    A=cell2mat(A_cell); %这里的A就是我们在推导下一时刻的状态空间时候的A
    B=cell2mat(B_cell); %这里的B就是我们在推导下一时刻的状态空间时候的B
    C=[ 1 0 0 0 0;
        0 1 0 0 0;
        0 0 1 0 0];%这个C矩阵是我们输出方程yita的系数矩阵，因为我们可能不需要把每个状态量都输出所以就可以通过设置C矩阵来输出我们想要的状态量，在这里我们输出的是X的误差、Y的误差以及横摆角的误差
 
    PHI_cell=cell(Np,1);%这个PHI是我们通过总结规律得到的等式右边的第一个系数矩阵，60*1维度
    THETA_cell=cell(Np,Nc);%这里的THETA为我们通过总结规律的到的等式右边的第二个系数矩阵，60*30维度
    for j=1:1:Np 
        PHI_cell{j,1}=C*A^j; %通过循环来给第一个系数矩阵赋值
        for k=1:1:Nc
            if k<=j
                THETA_cell{j,k}=C*A^(j-k)*B; %C为3*5矩阵；A为5*5；B为5*2，所以C*A*B为3*2矩阵
            else 
                THETA_cell{j,k}=zeros(Nx,Nu); 与C*A*B同维
            end
        end
    end
    PHI=cell2mat(PHI_cell);    %size(PHI)=[Nx*Np Nx+Nu] 180*5维度
    THETA=cell2mat(THETA_cell);%size(THETA)=[Nx*Np Nu*(Nc+1)]
    
    H_cell=cell(2,2); %这里的H为我们二次规划中的H矩阵，以下来构造二次规划中的H矩阵
    H_cell{1,1}=THETA'*Q*THETA+R;
    H_cell{1,2}=zeros(Nu*Nc,1); %60*1维度
    H_cell{2,1}=zeros(1,Nu*Nc); %1*60维度
    H_cell{2,2}=Row; %H矩阵的右下角的元素就只有一个就是我们的松弛因子
    H=cell2mat(H_cell);%由于松弛因子的影响，最终的H矩阵为61*61
    H=(H+H')/2;
 
    error=PHI*kesi; %这里的error就是我们所设的E矩阵
    f_cell=cell(1,2);%f为二次规划的第二个向量，下面我们来构造它
    f_cell{1,1} = 2*(error'*Q*THETA);
    f_cell{1,2} = 0;
    f=cell2mat(f_cell);%将元胞数组转化为矩阵
 %% 以下为约束生成区域
 %不等式约束
    A_t=zeros(Nc,Nc);%见falcone论文 P181
    for p=1:1:Nc
        for q=1:1:Nc
            if q<=p 
                A_t(p,q)=1;
            else 
                A_t(p,q)=0;
            end
        end 
    end 
    A_I=kron(A_t,eye(Nu)); %对应于falcone论文约束处理的矩阵A,求克罗内克积
    Ut=kron(ones(Nc,1), U);%此处的U表示的应该是上一时刻控制量误差（即偏差）组成的矩阵。
    umin=[-0.2;  -0.54];   %[min_vel, min_steer]维数与控制变量的个数相同
    umax=[0.2;   0.332];   %[max_vel, max_steer],%0.436rad = 25deg
    delta_umin = [-0.05;  -0.0082]; % 0.0082rad = 0.47deg
    delta_umax = [0.05;  0.0082];
    Umin=kron(ones(Nc,1),umin);
    Umax=kron(ones(Nc,1),umax);
    
    %二次规划不等式约束 Ax <= b
    A_cons_cell={A_I zeros(Nu*Nc, 1); -A_I zeros(Nu*Nc, 1)};
    b_cons_cell={Umax-Ut;-Umin+Ut};
    A_cons=cell2mat(A_cons_cell);%（求解方程）状态量不等式约束增益矩阵，转换为绝对值的取值范围
    b_cons=cell2mat(b_cons_cell);%（求解方程）状态量不等式约束的取值
   % 状态量约束
    M = 10;%松弛因子上限
    delta_Umin = kron(ones(Nc,1),delta_umin);
    delta_Umax = kron(ones(Nc,1),delta_umax);
    lb = [delta_Umin; 0];%（求解方程）状态量下界,包含控制时域内控制增量和松弛因子下界
    ub = [delta_Umax; M];%（求解方程）状态量上界，包含控制时域内控制增量和松弛因子上界M
  
    %% 开始求解过程
%     options = optimset('Algorithm','active-set');
    options = optimset('Algorithm','interior-point-convex');    
    [X, fval,exitflag]=quadprog(H, f, A_cons, b_cons,[], [],lb,ub,[],options);
    fprintf('quadprog EXITFLAG = %d\n',exitflag);
    %% 计算输出   
    u_piao(1)=X(1);   % X1为二次规划求出来的是控制时域内最优的速度控制增量
    u_piao(2)=X(2);   % X2为二次规划求出来的最优前轮转角控制增量
    U(1)=kesi(4)+u_piao(1);%用于存储上一个时刻的控制量  %kesi4为速度误差。kesi4为速度误差与速度误差增量u_piao(1)相加得到我们下一时刻的kesi矩阵的第四行
    U(2)=kesi(5)+u_piao(2);%kesi5为k-1时刻前轮偏角误差，道理同上，与前轮偏角误差的增量相加得到我们下一时刻的kesi矩阵的第五行
    u_real(1) = U(1) + vd1;%再与下面的参考速度相加才是真正的速度
    u_real(2) = U(2) + vd2;%道理同上
 
    sys= [u_real(1); u_real(2)]; % vel, steering, x, y 将算出来的真正的控制量输出输出
    toc
% End of mdlOutputs.