无人驾驶车辆模型预测控制 -6.4

1. simulation model无人驾驶车辆模型预测控制 -6.4_第1张图片

2. simulation cases

2.1 10mps(36kph)

无人驾驶车辆模型预测控制 -6.4_第2张图片
无人驾驶车辆模型预测控制 -6.4_第3张图片

2.2 20mps(36kph)

无人驾驶车辆模型预测控制 -6.4_第4张图片
无人驾驶车辆模型预测控制 -6.4_第5张图片

2.3 30mps(108kph)

无人驾驶车辆模型预测控制 -6.4_第6张图片

无人驾驶车辆模型预测控制 -6.4_第7张图片

3. code

3.1 path plan

function [sys,x0,str,ts] = MPC_TrajPlanner(t,x,u,flag)
% 该程序功能:用点质量模型设计规划期,能够规避障碍物
% 程序版本 V1.0,MATLAB版本:R2011a,采用S函数的标准形式,
% 程序编写日期 2013.12.17
% 最近一次改写 2014.02.24
% 状态量=[y_dot,x_dot,phi,,Y,X],控制量为前轮偏角ay


switch flag,
 case 0  %flag=0表示处于初始化状态,此时用函数mdlInitializeSizes进行初始化
  [sys,x0,str,ts] = mdlInitializeSizes; % Initialization
  
 case 2  %flag=2表示此时要计算下一个离散状态
  sys = mdlUpdates(t,x,u); % Update discrete states
  
 case 3 %flag=3表示此时要计算输出
  sys = mdlOutputs(t,x,u); % Calculate outputs
 
%  case 4
%   sys = mdlGetTimeOfNextVarHit(t,x,u); % Get next sample time 

 case {1,4,9} % Unused flags
    % flag=1表示此时要计算连续状态的微分
    %flag=4表示此时要计算下一次采样的时间,只在离散采样系统中有用,主要用于变步长的设置
    %flag=9表示此时系统要结束,一般来说写上在mdlTerminate函数中写上sys=[]就可
  sys = [];
  
 otherwise
  error(['unhandled flag = ',num2str(flag)]); % Error handling
end
% End of dsfunc.

%==============================================================
% Initialization
%==============================================================

function [sys,x0,str,ts] = mdlInitializeSizes

% Call simsizes for a sizes structure, fill it in, and convert it 
% to a sizes array.

sizes = simsizes; %用于设置模块参数的结构体用simsizes来生成
sizes.NumContStates  = 0;%模块连续状态变量的个数
sizes.NumDiscStates  = 5;%模块离散状态变量的个数
sizes.NumOutputs     = 10;%输出期望的Y和phi,给出的是拟合曲线的系数如果有需要可以考虑dphi,dphi=ay/x_dot;
sizes.NumInputs      = 6;%模块输入变量的个数
sizes.DirFeedthrough = 1; %模块是否存在直接贯通
sizes.NumSampleTimes = 1;%模块的采样次数,至少是一个
sys = simsizes(sizes); %设置完后赋给sys输出

x0 =[0.001;0.0001;0.0001;0.00001;0.00001;];    %状态变量设置
%global U;
%U=[0];%控制量初始化,U为一维的
% global x;
% x = zeros(md.ne + md.pye + md.me + md.Hu*md.me,1);   
% Initialize the discrete states.
str = [];             %保留参数, Set str to an empty matrix.
ts  = [0.1 0];       % 采样周期: [period, offset],这里轨迹规划的周期设为100ms
%End of mdlInitializeSizes
		      
%==============================================================
% Update the discrete states
%==============================================================
function sys = mdlUpdates(t,x,u)
  
sys = x; %更新状态变量
%End of mdlUpdate.

%==============================================================
% Calculate outputs
%==============================================================
function sys = mdlOutputs(t,x,u)
    %u是CarSim的输出
    %x是状态量,x=[y_dot,x_dot,phi,,Y,X]
    %t是时间变量
    tic
    Nx=5;%状态量的个数, 
    %Nu=1;%控制量的个数,控制量为前轮偏角ay
    %Ny=2;%输出量的个数,这边falcone在LTV里面验证了两个输出量没有三个输出量的控制效果好,这边可以先用两个输出量
    Np =15;%预测步长
    Nc=2;%控制步长
    Nobs=6;%障碍物个数
    T=0.1;%Sample Time
   
    %输入接口转换,x_dot后面加一个非常小的数,是防止出现分母为零的情况
    % y_dot=u(1)/3.6-0.000000001*0.4786;%CarSim输出的是km/h,转换为m/s
    y_dot=u(1)/3.6; %速度单位是km/h,转换为m/s
    x_dot=u(2)/3.6+0.0001;%CarSim输出的是km/h,转换为m/s.加一个很小的数是为了防止分母为零
    phi=u(3)*pi/180; %CarSim输出的为角度,角度转换为弧度
    phi_dot=u(4)*pi/180;%角速度,角度转换为弧度
    Y=u(5);%单位为m
    X=u(6);%单位为米
   
%% 参考轨迹生成
    shape=2.4;%参数名称,用于参考轨迹生成
    dx1=25;dx2=21.95;%没有任何实际意义,只是参数名称
    dy1=4.05;dy2=5.7;%没有任何实际意义,只是参数名称
    Xs1=27.19;Xs2=56.46;%参数名称
  	X_phi=1:1:220;%这个点的区间是根据纵向速度(x_dot)来定的,如果速度为10m/s则区间=10*0.1=1(mdlInitializeSizes中已经定位周期为0.1s)
    z1=shape/dx1*(X_phi-Xs1)-shape/2;
    z2=shape/dx2*(X_phi-Xs2)-shape/2;
    Y_ref=dy1/2.*(1+tanh(z1))-dy2/2.*(1+tanh(z2));
    figure(1);
    plot(X_phi, Y_ref,'r--','LineWidth',2);%画出参考轨迹
    hold on;
   % phi_ref=atan(dy1*(1./cosh(z1)).^2*(1.2/dx1)-dy2*(1./cosh(z2)).^2*(1.2/dx2));
%% 矩阵转换。将状态变量转化为状态变量矩阵
    State_Initial=zeros(Nx,1);
    State_Initial(1,1)=y_dot;
    State_Initial(2,1)=x_dot;
    State_Initial(3,1)=phi;
    State_Initial(4,1)=Y;
    State_Initial(5,1)=X;    
%% 障碍物信息设置
    X_obstacle=zeros(Nobs,1);
    X_obstacle(1:2)=30;
    X_obstacle(3:4)=35;
    X_obstacle(5:6)=32.5;
    Y_obstacle=zeros(Nobs,1);
    Y_obstacle(1)=0.5;
    Y_obstacle(2)=1;
    Y_obstacle(3)=0.5;
    Y_obstacle(4)=1;
    Y_obstacle(5)=0.5;
    Y_obstacle(6)=1;
    figure(1);
    rectangle('Position',[X_obstacle(1),Y_obstacle(1),5,0.5],'FaceColor',[0 0 0]);
    hold on;
    Yref=(Y_ref(1,round(State_Initial(5,1))+1:round(State_Initial(5,1))+15))';%Yref采用的是近似算法,此处为局部期望路径
    Q=100*eye(Np,Np);%这里设置评价矩阵,都设为了1。可以根据跟踪情况加以调整
    R=20*eye(Nc,Nc); %
    S=100;%避障函数的权重    
%% 开始求解过程
    %设置约束
    mu=0.4;%地面摩擦系数
    g=9.8;
    lb=[-mu*g;-mu*g];
    ub=[mu*g;mu*g];
    A=[];
    b=[];
    Aeq=[];
    beq=[];
    options = optimset('Algorithm','active-set');
    [A,fval,exitflag]=fmincon(@(x)MY_costfunction(x,State_Initial,Np,Nc,Nobs,T,Yref,Q,R,S,X_obstacle,Y_obstacle),[0;0;],A,b,Aeq,beq,lb,ub,[],options);%有约束求解,但速度慢
%   [A,fval,exitflag]=fminbnd(@(x)MY_costfunction(x,State_Initial,Np,Nc,T,Yref,Q,R,S),lb,ub);%只有上下界约束,但容易陷入局部最小
%   [A,fval,exitflag]=fminsearch(@(x)MY_costfunction(x,State_Initial,Np,Nc,Nobs,T,Yref,Q,R,S,X_obstacle,Y_obstacle),[0;0]);%无约束求解,速度最快
    fprintf('exitflag=%d\n',exitflag);
   
%% 计算输出
% 以下根据计算出的控制量推导所有的状态量
    y_dot_predict=zeros(Np,1);
    x_dot_predict=zeros(Np,1);
    phi_predict=zeros(Np,1);
    Y_predict=zeros(Np,1);
    X_predict=zeros(Np,1);

 for i=1:1:Np
     if i==Nc-1 
            ay(i)=A(1);
             % 以下完成状态量更新
            y_dot_predict(i,1)=State_Initial(1,1)+T*ay(i);
            x_dot_predict(i,1)=State_Initial(2,1);
            phi_predict(i,1)=State_Initial(3,1)+T*ay(i)/State_Initial(2,1);
            Y_predict(i,1)=State_Initial(4,1)+T*(State_Initial(2,1)*sin(State_Initial(3,1))+State_Initial(1,1)*cos(State_Initial(3,1)));
            X_predict(i,1)=State_Initial(5,1)+T*(State_Initial(2,1)*cos(State_Initial(3,1))-State_Initial(1,1)*sin(State_Initial(3,1)));  
      else %if i<=5
            ay(i)=A(2);%这种写法是仅仅考虑两个控制周期
            y_dot_predict(i,1)=y_dot_predict(i-1,1)+T*ay(i);
            x_dot_predict(i,1)=State_Initial(2,1);
            phi_predict(i,1)=phi_predict(i-1,1)+T*ay(i)/x_dot_predict(i-1,1);
            Y_predict(i,1)=Y_predict(i-1)+T*(State_Initial(2,1)*sin(phi_predict(i-1))+y_dot_predict(i-1)*cos(phi_predict(i-1)));
            X_predict(i,1)=X_predict(i-1)+T*(State_Initial(2,1)*cos(phi_predict(i-1))-y_dot_predict(i-1)*sin(phi_predict(i-1)));
      end 
 end
    Paramater_X_Y=polyfit(X_predict,Y_predict,4);
    Paramater_X_PHI=polyfit(X_predict,phi_predict,4);
    OutPut(1:5)=Paramater_X_Y;
    OutPut(6:10)=Paramater_X_PHI;
    sys=OutPut;
    figure(1);
    xx =X_predict(1,1):1:X_predict(Np,1);
    yy= Paramater_X_Y(1)*xx.^4+ Paramater_X_Y(2)*xx.^3+ Paramater_X_Y(3)*xx.^2+ Paramater_X_Y(4)*xx+ Paramater_X_Y(5);
    plot(xx,yy);
    hold on;
    toc
% End of mdlOutputs.
 %% 求代价函数的功能子函数
function cost = MY_costfunction(x,State_Initial,Np,Nc,Nobs,T,Yref,Q,R,S,X_obstacle,Y_obstacle)
    cost=0;
    y_dot=State_Initial(1,1);
    x_dot=State_Initial(2,1);
    phi=State_Initial(3,1);
    Y=State_Initial(4,1);
    X_start=State_Initial(5,1);
    
    y_dot_predict=zeros(Np,1);
    x_dot_predict=zeros(Np,1);
    phi_predict=zeros(Np,1);
    Y_predict=zeros(Np,1);
    X_predict=zeros(Np,1);
    Y_error=zeros(Np,1);
    J_obst=zeros(Np,1);
    ay=zeros(Np,1);
    
    for i=1:1:Np
        if i==Nc-1 
            ay(i,1)=x(1);
             % 以下完成状态量更新
            y_dot_predict(i,1)=y_dot+T*ay(i,1);
            x_dot_predict(i,1)=x_dot;
            phi_predict(i,1)=phi+T*ay(i)/x_dot;
            Y_predict(i,1)=Y+T*(x_dot*sin(phi)+y_dot*cos(phi));
            X_predict(i,1)=X_start+T*(x_dot*cos(phi)-y_dot*sin(phi));
            for j=1:1:Nobs
                J_obst(i,1)=J_obst(i,1)+1/(((X_predict(i,1))-X_obstacle(j,1))^2+(Y_predict(i,1)-Y_obstacle(j,1))^2+0.000001);
            end
        else %if i<=5
            ay(i,1)=x(2);%这种写法是仅仅考虑两个控制周期
            y_dot_predict(i,1)=y_dot_predict(i-1,1)+T*ay(i,1);
            x_dot_predict(i,1)=x_dot;
            phi_predict(i,1)=phi_predict(i-1,1)+T*ay(i)/x_dot_predict(i-1,1);
            Y_predict(i,1)=Y_predict(i-1)+T*(x_dot*sin(phi_predict(i-1))+y_dot_predict(i-1)*cos(phi_predict(i-1)));
            X_predict(i,1)=X_predict(i-1)+T*(x_dot*cos(phi_predict(i-1))-y_dot_predict(i-1)*sin(phi_predict(i-1)));
            for p=1:1:Nobs
                J_obst(i,1)=J_obst(i,1)+1/(((X_predict(i,1))-X_obstacle(p,1))^2+(Y_predict(i,1)-Y_obstacle(p,1))^2+0.000001);
            end
%             else 
%             ay(i)=x(2);
%             y_dot_predict(i,1)=y_dot_predict(i-1,1)+T*ay(i);
%             x_dot_predict(i,1)=x_dot;
%             phi_predict(i,1)=phi_predict(i-1,1)+T*ay(i)/x_dot_predict(i-1,1);
%             Y_predict(i,1)=Y_predict(i-1)+T*(x_dot*sin(phi_predict(5,1))+y_dot_predict(5,1)*cos(phi_predict(5,1)));
%             X_predict(i,1)=X_predict(i-1)+T*(x_dot*cos(phi_predict(5,1))-y_dot_predict(5,1)*sin(phi_predict(5,1)));
%             end 
        end
    %J_obst=J_obst+1/(((X_predict(i,1))-X_obstacle(2,1))^2+(Y_predict(i,1)-Y_obstacle(2,1))^2+0.00001);
        Y_error(i,1)=Y_predict(i,1)-Yref(i,1);%注意此处Yref与Y_ref要区分开来,Yref是局部期望路径,Y_ref为全局期望路径
    end 
        cost=cost+Y_error'*Q*Y_error+ay(1:2)'*R*ay(1:2)+S*sum(J_obst(:));
% End of CostFunction

3.2 path tracking

function [sys,x0,str,ts] = MPC_Controller(t,x,u,flag)
% 该程序功能:用LTV MPC 和车辆简化动力学模型(小角度假设)设计控制器,接受来自规划器的期望轨迹,实现轨迹跟踪控制功能
% 程序版本 V1.0,MATLAB版本:R2011a,采用S函数的标准形式,
% 程序编写日期 2013.12.17
% 最近一次改写 2013.12.18
% 状态量=[y_dot,x_dot,phi,phi_dot,Y,X],控制量为前轮偏角delta_f
% QP解法不再用之前的方法,而是用falcone学位论文中的方法(和之前的方法其实等价)
%

switch 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 4
%   sys = mdlGetTimeOfNextVarHit(t,x,u); % Get next sample time 

 case {1,4,9} % Unused flags
  sys = [];
  
 otherwise
  error(['unhandled flag = ',num2str(flag)]); % Error handling
end
% End of dsfunc.

%==============================================================
% Initialization
%==============================================================

function [sys,x0,str,ts] = mdlInitializeSizes

% Call simsizes for a sizes structure, fill it in, and convert it 
% to a sizes array.

sizes = simsizes;
sizes.NumContStates  = 0; %
sizes.NumDiscStates  = 6; %
sizes.NumOutputs     = 1;
sizes.NumInputs      = 16;
sizes.DirFeedthrough = 1; % Matrix D is non-empty.
sizes.NumSampleTimes = 1;
sys = simsizes(sizes); 
x0 =[0.001;0.0001;0.0001;0.00001;0.00001;0.00001];    
global U;
U=[0];%控制量初始化,这里面加了一个期望轨迹的输出,如果去掉,U为一维的
% global x;
% x = zeros(md.ne + md.pye + md.me + md.Hu*md.me,1);   
% Initialize the discrete states.
str = [];             % Set str to an empty matrix.
ts  = [0.02 0];       % sample time: [period, offset]
%End of mdlInitializeSizes
		      
%==============================================================
% Update the discrete states
%==============================================================
function sys = mdlUpdates(t,x,u)
  
sys = x;
%End of mdlUpdate.

%==============================================================
% Calculate outputs
%==============================================================
function sys = mdlOutputs(t,x,u)
    global a b; 
    %global u_piao;
    global U;
    %global kesi;
    tic
    Nx=6;%状态量的个数
    Nu=1;%控制量的个数
    Ny=2;%输出量的个数,这边falcone在LTV里面验证了两个输出量没有三个输出量的控制效果好,这边可以先用两个输出量
    Np =25;%预测步长
    Nc=10;%控制步长
    Row=1000;%松弛因子
    fprintf('Update start, t=%6.3f\n',t)
   
    %输入接口转换, x_dot后面加一个非常小的数,是防止出现分母为零的情况
   % y_dot=u(1)/3.6-0.000000001*0.4786; %CarSim输出的是km/h,转换为m/s
    y_dot=u(1)/3.6;
    x_dot=u(2)/3.6+0.0001;%CarSim输出的是km/h,转换为m/s
    phi=u(3)*3.141592654/180; %CarSim输出的为角度,角度转换为弧度
    phi_dot=u(4)*3.141592654/180;
    Y=u(5);%单位为m
    X=u(6);%单位为米
    
    
%% 根据规划器的输入确定参考轨迹
%从端口读入局部规划器的数据,传递进来的是4次曲线的参数
    Paramater_X_Y(1,1)=u(7);
    Paramater_X_Y(1,2)=u(8);
    Paramater_X_Y(1,3)=u(9);
    Paramater_X_Y(1,4)=u(10);
    Paramater_X_Y(1,5)=u(11);
    Paramater_X_phi(1,1)=u(12);
    Paramater_X_phi(1,2)=u(13);
    Paramater_X_phi(1,3)=u(14);
    Paramater_X_phi(1,4)=u(15);
    Paramater_X_phi(1,5)=u(16);

%% 车辆参数输入
%syms sf sr;%分别为前后车轮的滑移率,需要提供
    Sf=0.2; Sr=0.2;
%syms lf lr;%前后车轮距离车辆质心的距离,车辆固有参数
    lf=1.232;lr=1.468;
%syms C_cf C_cr C_lf C_lr;%分别为前后车轮的纵横向侧偏刚度,车辆固有参数
    Ccf=66900;Ccr=62700;Clf=66900;Clr=62700;
%syms m g I;%m为车辆质量,g为重力加速度,I为车辆绕Z轴的转动惯量,车辆固有参数
    m=1723;g=9.8;I=4175;
   

%% 参考轨迹生成
%     shape=2.4;%参数名称,用于参考轨迹生成
%     dx1=25;dx2=21.95;%没有任何实际意义,只是参数名称
%     dy1=4.05;dy2=5.7;%没有任何实际意义,只是参数名称
%     Xs1=27.19;Xs2=56.46;%参数名称
    X_predict=zeros(Np,1);%用于保存预测时域内的纵向位置信息,这是计算期望轨迹的基础
    phi_ref=zeros(Np,1);%用于保存预测时域内的期望轨迹
    Y_ref=zeros(Np,1);%用于保存预测时域内的期望轨迹
    dphi_ref=zeros(Np,1);
%% 矩阵转换
    %  以下计算kesi,即状态量与控制量合在一起   
    kesi=zeros(Nx+Nu,1);
    kesi(1)=y_dot;%u(1)==X(1)
    kesi(2)=x_dot;%u(2)==X(2)
    kesi(3)=phi; %u(3)==X(3)
    kesi(4)=phi_dot;
    kesi(5)=Y;
    kesi(6)=X;
    kesi(7)=U(1);
    delta_f=U(1);
    fprintf('Update start, u(1)=%4.2f\n',U(1))

    T=0.02;%仿真步长
    T_all=20;%临时设定,总的仿真时间,主要功能是防止计算期望轨迹越界
     
    %权重矩阵设置 
    Q_cell=cell(Np,Np);
    for i=1:1:Np;
        for j=1:1:Np;
            if i==j
                %Q_cell{i,j}=[200 0;0 100;];
                Q_cell{i,j}=[2000 0;0 10000;];
            else 
                Q_cell{i,j}=zeros(Ny,Ny);               
            end
        end 
    end 
    %R=5*10^4*eye(Nu*Nc);
    R=5*10^5*eye(Nu*Nc);
    %最基本也最重要的矩阵,是控制器的基础,采用动力学模型,该矩阵与车辆参数密切相关,通过对动力学方程求解雅克比矩阵得到
    a=[                 1 - (259200*T)/(1723*x_dot),                                                         -T*(phi_dot + (2*((460218*phi_dot)/5 - 62700*y_dot))/(1723*x_dot^2) - (133800*((154*phi_dot)/125 + y_dot))/(1723*x_dot^2)),                                    0,                     -T*(x_dot - 96228/(8615*x_dot)), 0, 0
        T*(phi_dot - (133800*delta_f)/(1723*x_dot)),                                                                                                                  (133800*T*delta_f*((154*phi_dot)/125 + y_dot))/(1723*x_dot^2) + 1,                                    0,           T*(y_dot - (824208*delta_f)/(8615*x_dot)), 0, 0
                                                  0,                                                                                                                                                                                  0,                                    1,                                                   T, 0, 0
            (33063689036759*T)/(7172595384320*x_dot), T*(((2321344006605451863*phi_dot)/8589934592000 - (6325188028897689*y_dot)/34359738368)/(4175*x_dot^2) + (5663914248162509*((154*phi_dot)/125 + y_dot))/(143451907686400*x_dot^2)),                                   0, 1 - (813165919007900927*T)/(7172595384320000*x_dot), 0, 0
                                          T*cos(phi),                                                                                                                                                                         T*sin(phi),  T*(x_dot*cos(phi) - y_dot*sin(phi)),                                                   0, 1, 0
                                         -T*sin(phi),                                                                                                                                                                         T*cos(phi), -T*(y_dot*cos(phi) + x_dot*sin(phi)),                                                   0, 0, 1];
   
    b=[                                                               133800*T/1723
       T*((267600*delta_f)/1723 - (133800*((154*phi_dot)/125 + y_dot))/(1723*x_dot))
                                                                                 0
                                                5663914248162509*T/143451907686400
                                                                                 0
                                                                                 0];  
    d_k=zeros(Nx,1);%计算偏差,即falcone42页中的d(k,t)
    state_k1=zeros(Nx,1);%预测的下一时刻状态量,用于计算偏差,即falcone42页的kesi(k+1,t)
    %以下即为根据离散非线性模型预测下一时刻状态量
    %注意,为避免前后轴距的表达式(a,b)与控制器的a,b矩阵冲突,将前后轴距的表达式改为lf和lr
    state_k1(1,1)=y_dot+T*(-x_dot*phi_dot+2*(Ccf*(delta_f-(y_dot+lf*phi_dot)/x_dot)+Ccr*(lr*phi_dot-y_dot)/x_dot)/m);
    state_k1(2,1)=x_dot+T*(y_dot*phi_dot+2*(Clf*Sf+Clr*Sr+Ccf*delta_f*(delta_f-(y_dot+phi_dot*lf)/x_dot))/m);
    state_k1(3,1)=phi+T*phi_dot;
    state_k1(4,1)=phi_dot+T*((2*lf*Ccf*(delta_f-(y_dot+lf*phi_dot)/x_dot)-2*lr*Ccr*(lr*phi_dot-y_dot)/x_dot)/I);
    state_k1(5,1)=Y+T*(x_dot*sin(phi)+y_dot*cos(phi));
    state_k1(6,1)=X+T*(x_dot*cos(phi)-y_dot*sin(phi));
    d_k=state_k1-a*kesi(1:6,1)-b*kesi(7,1);%根据falcone公式(2.11b)求得d(k,t)
    d_piao_k=zeros(Nx+Nu,1);%d_k的增广形式,参考falcone(B,4c)
    d_piao_k(1:6,1)=d_k;
    d_piao_k(7,1)=0;
    
    A_cell=cell(2,2);
    B_cell=cell(2,1);
    A_cell{1,1}=a;
    A_cell{1,2}=b;
    A_cell{2,1}=zeros(Nu,Nx);
    A_cell{2,2}=eye(Nu);
    B_cell{1,1}=b;
    B_cell{2,1}=eye(Nu);
    %A=zeros(Nu+Nx,Nu+Nx);
    A=cell2mat(A_cell);
    B=cell2mat(B_cell);
   % C=[0 0 1 0 0 0 0;0 0 0 1 0 0 0;0 0 0 0 1 0 0;];%这是和输出量紧密关联的
    C=[0 0 1 0 0 0 0;0 0 0 0 1 0 0;];
    PSI_cell=cell(Np,1);%原来版本的PHI
    THETA_cell=cell(Np,Nc);
    GAMMA_cell=cell(Np,Np);%按照新的计算方法添加的,即falcone179页最后一行公式
    PHI_cell=cell(Np,1);%注意,这个版本与之前所有的PHI不一样,原来版本的PHI为这个版本的PSI
    for p=1:1:Np;
        PHI_cell{p,1}=d_piao_k;%理论上来说,这个是要实时更新的,但是为了简便,这里又一次近似
        for q=1:1:Np;
            if q<=p;
                GAMMA_cell{p,q}=C*A^(p-q);
            else 
                GAMMA_cell{p,q}=zeros(Ny,Nx+Nu);
            end 
        end
    end
    for j=1:1:Np
     PSI_cell{j,1}=C*A^j;
        for k=1:1:Nc
            if k<=j
                THETA_cell{j,k}=C*A^(j-k)*B;
            else 
                THETA_cell{j,k}=zeros(Ny,Nu);
            end
        end
    end
    PSI=cell2mat(PSI_cell);%size(PSI)=[Ny*Np Nx+Nu]
    THETA=cell2mat(THETA_cell);%size(THETA)=[Ny*Np Nu*Nc]
    GAMMA=cell2mat(GAMMA_cell);%大写的GAMMA
    PHI=cell2mat(PHI_cell);
    Q=cell2mat(Q_cell);
    H_cell=cell(2,2);
    H_cell{1,1}=THETA'*Q*THETA+R;
    H_cell{1,2}=zeros(Nu*Nc,1);
    H_cell{2,1}=zeros(1,Nu*Nc);
    H_cell{2,2}=Row;
    H=cell2mat(H_cell);
    error_1=zeros(Ny*Np,1);
    Yita_ref_cell=cell(Np,1);
    for p=1:1:Np
        if t+p*T>T_all
            X_DOT=x_dot*cos(phi)-y_dot*sin(phi);%惯性坐标系下纵向速度
            X_predict(Np,1)=X+X_DOT*Np*T;
            Y_ref(p,1)=polyval(Paramater_X_Y, X_predict(Np,1));
            phi_ref(p,1)=polyval(Paramater_X_phi, X_predict(Np,1));
            %dphi_ref(p,1)=-((2916*X_DOT*sinh((12*X_predict(Np,1))/125 - 11907/3125))/(78125*cosh((12* X_predict(Np,1))/125 - 11907/3125)^3) - (65664*X_DOT*sinh((48* X_predict(Np,1))/439 - 80922/10975))/(963605*cosh((48* X_predict(Np,1))/439 - 80922/10975)^3))/((243/(1250*cosh((12*X_predict(Np,1))/125 - 11907/3125)^2) - 684/(2195*cosh((48*X_predict(Np,1))/439 - 80922/10975)^2))^2 + 1);
            %Yita_ref_cell{p,1}=[phi_ref(p,1);dphi_ref(p,1);Y_ref(p,1)];%如果越界了,以预测时域内最后一个点始终作为期望值
            Yita_ref_cell{p,1}=[phi_ref(p,1);Y_ref(p,1)];
            
        else
            X_DOT=x_dot*cos(phi)-y_dot*sin(phi);%惯性坐标系下纵向速度
            X_predict(p,1)=X+X_DOT*p*T;%首先计算出未来X的位置,X(t)=X+X_dot*t
            Y_ref(p,1)=polyval(Paramater_X_Y, X_predict(p,1));
            phi_ref(p,1)=polyval(Paramater_X_phi, X_predict(p,1));
            Yita_ref_cell{p,1}=[phi_ref(p,1);Y_ref(p,1)];
            %dphi_ref(p,1)=-((2916*X_DOT*sinh((12* X_predict(p,1))/125 - 11907/3125))/(78125*cosh((12* X_predict(p,1))/125 - 11907/3125)^3) - (65664*X_DOT*sinh((48* X_predict(p,1))/439 - 80922/10975))/(963605*cosh((48* X_predict(p,1))/439 - 80922/10975)^3))/((243/(1250*cosh((12*X_predict(p,1))/125 - 11907/3125)^2) - 684/(2195*cosh((48*X_predict(p,1))/439 - 80922/10975)^2))^2 + 1);
            %Yita_ref_cell{p,1}=[phi_ref(p,1);dphi_ref(p,1);Y_ref(p,1)];%3个期望参数整合在一起
        end
    end
    Yita_ref=cell2mat(Yita_ref_cell);
%     fprintf('X_predict=%4.2f\n',X_predict(1));    
%     fprintf('实际位置,X=%4.2f\n',X);
%     fprintf('Update Yita, phi_ref=%4.2f\n',Yita_ref(1));
%     fprintf('Update Yita, dphi_ref=%4.2f\n',Yita_ref(2));
%     fprintf('Update Yita, phi=%4.2f\n',phi);
%     fprintf('实际位置,Y=%4.2f\n',Y);

    error_1=Yita_ref-PSI*kesi-GAMMA*PHI; %求偏差
    f_cell=cell(1,2);
    f_cell{1,1}=2*error_1'*Q*THETA;
    f_cell{1,2}=0;
%     f=(cell2mat(f_cell))';
    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(1));%此处感觉论文里的克罗内科积有问题,暂时交换顺序
    umin=-0.1744;%维数与控制变量的个数相同
    umax=0.1744;
    delta_umin=-0.0148*0.4;
    delta_umax=0.0148*0.4;
    Umin=kron(ones(Nc,1),umin);
    Umax=kron(ones(Nc,1),umax);
    
    %输出量约束(硬约束设置)暂时先让所有输出为硬约束
    ycmax=[0.21;5];
    ycmin=[-0.3;-3];
    Ycmax=kron(ones(Np,1),ycmax);
    Ycmin=kron(ones(Np,1),ycmin);
    
    %结合控制量约束和输出量约束
    A_cons_cell={A_I zeros(Nu*Nc,1);-A_I zeros(Nu*Nc,1);THETA zeros(Ny*Np,1);-THETA zeros(Ny*Np,1)};
    b_cons_cell={Umax-Ut;-Umin+Ut;Ycmax-PSI*kesi-GAMMA*PHI;-Ycmin+PSI*kesi+GAMMA*PHI};
    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];%(求解方程)状态量上界,包含控制时域内控制增量和松弛因子
    
    %% 开始求解过程
%    [X,fval,exitflag]=quadprog(H,f,A_cons,b_cons,[],[],lb,ub);
       options = optimset('Algorithm','active-set');
       x_start=zeros(Nc+1,1);%加入一个起始点
      [X,fval,exitflag]=quadprog(H,f,A_cons,b_cons,[],[],lb,ub,x_start,options);
      fprintf('exitflag=%d\n',exitflag);
      fprintf('H=%4.2f\n',H(1,1));
      fprintf('f=%4.2f\n',f(1,1));
    %% 计算输出
    u_piao=X(1);%得到控制增量
    U(1)=kesi(7,1)+u_piao;%当前时刻的控制量为上一刻时刻控制+控制增量
   %U(2)=Yita_ref(2);%输出dphi_ref
    sys= U;
    toc
% End of mdlOutputs.



你可能感兴趣的:(无人驾驶车辆模型预测控制)