【carsim+simulink 联合仿真——车辆轨迹MPC跟踪】

学习北理工的无人驾驶车辆模型预测控制第2版第四章，使用的仿真软件为Carsim8和MatlabR2019a联合仿真，使用MPC控制思想对车辆进行轨迹跟踪控制，并给出仿真结果。

mpc控制器函数：s-function

function [sys,x0,str,ts] = MY_MPCController3(t,x,u,flag)
%   该函数是写的第3个S函数控制器(MATLAB版本：R2011a)
%   限定于车辆运动学模型，控制量为速度和前轮偏角，使用的QP为新版本的QP解法
%   [sys,x0,str,ts] = MY_MPCController3(t,x,u,flag)
%
% is an S-function implementing the MPC controller intended for use
% with Simulink. The argument md, which is the only user supplied
% argument, contains the data structures needed by the controller. The
% input to the S-function block is a vector signal consisting of the
% measured outputs and the reference values for the controlled
% outputs. The output of the S-function block is a vector signal
% consisting of the control variables and the estimated state vector,
% potentially including estimated disturbance states.

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 {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  = 3; % this parameter doesn't matter
sizes.NumOutputs     = 2; %[speed, steering]
sizes.NumInputs      = 5; %[x,y,yaw,vx,steer_sw]
sizes.DirFeedthrough = 1; % Matrix D is non-empty.
sizes.NumSampleTimes = 1;
sys = simsizes(sizes);
x0 =[0;0;0];
global U; % store current ctrl vector:[vel_m, delta_m]
U=[0;0];
% Initialize the discrete states.
str = [];             % Set str to an empty matrix.
ts  = [0.05 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 u_piao;
%u_piao矩阵，用于存储每一个仿真时刻，车辆的实际控制量（实际运动状态）与目标控制量（运动状态）之间的偏差

global U; %store chi_tilde=[vel-vel_ref; delta - delta_ref]
global kesi;

tic
Nx=3;%状态量的个数
Nu =2;%控制量的个数
Np =60;%预测步长
Nc=30;%控制步长
Row=10;%松弛因子
fprintf('Update start, t=%6.3f\n',t)
t_d =u(3)*3.1415926/180;%CarSim输出的Yaw angle为角度，角度转换为弧度

%    %直线路径
%     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);
    r(2)=35-25*cos(0.2*t);
    r(3)=0.2*t;
    vd1=5;
    vd2=0.104;

%     %半径为35m的圆形轨迹, 圆心为(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;


%     t_d =  r(3);
kesi=zeros(Nx+Nu,1);
kesi(1) = u(1)-r(1);%u(1)==X(1),x_offset
kesi(2) = u(2)-r(2);%u(2)==X(2),y_offset
heading_offset = t_d - r(3); %u(3)==X(3),heading_angle_offset

if (heading_offset < -pi)
    heading_offset = heading_offset + 2*pi;
end
if (heading_offset > pi)
    heading_offset = heading_offset - 2*pi;
end
kesi(3)=heading_offset;

%      U(1) = u(4)/3.6 - vd1; % vel, km/h-->m/s
%      steer_SW = u(5)*pi/180;
%      steering_angle = steer_SW/18.0;
%      U(2) = steering_angle - vd2;

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.05;
T_all=40;%临时设定，总的仿真时间，主要功能是防止计算期望轨迹越界
% Mobile Robot Parameters
L = 2.6; % wheelbase of carsim vehicle
% Mobile Robot variable


%矩阵初始化
u_piao=zeros(Nx,Nu);
Q=eye(Nx*Np,Nx*Np);
R=5*eye(Nu*Nc);
a=[1    0   -vd1*sin(t_d)*T;
    0    1   vd1*cos(t_d)*T;
    0    0   1;];
b=[cos(t_d)*T        0;
    sin(t_d)*T        0;
    tan(vd2)*T/L      vd1*T/(cos(vd2)^2)];

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=cell2mat(A_cell);
B=cell2mat(B_cell);
C=[ 1 0 0 0 0;
    0 1 0 0 0;
    0 0 1 0 0];

PHI_cell=cell(Np,1);
THETA_cell=cell(Np,Nc);
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;
        else
            THETA_cell{j,k}=zeros(Nx,Nu);
        end
    end
end
PHI=cell2mat(PHI_cell);%size(PHI)=[Nx*Np Nx+Nu]
THETA=cell2mat(THETA_cell);%size(THETA)=[Nx*Np Nu*(Nc+1)]

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);
%     H=(H+H')/2;

error=PHI*kesi;
f_cell=cell(1,2);
f_cell{1,1} = (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);%
umin=[-0.2;  -0.54];%[min_vel, min_steer]维数与控制变量的个数相同
umax=[0.05;   0.436]; %[max_vel, max_steer],%0.436rad = 25deg
delta_umin = [-0.5;  -0.0082]; % 0.0082rad = 0.47deg
delta_umax = [0.5;  0.0082];

Umin=kron(ones(Nc,1),umin);
Umax=kron(ones(Nc,1),umax);
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);%（求解方程）状态量不等式约束的取值

% 状态量约束
delta_Umin = kron(ones(Nc,1),delta_umin);
delta_Umax = kron(ones(Nc,1),delta_umax);
lb = [delta_Umin; 0];%（求解方程）状态量下界
ub = [delta_Umax; 10];%（求解方程）状态量上界

%% 开始求解过程
%     options = optimset('Algorithm','active-set');
options = optimset('Algorithm','interior-point-convex');
warning off all  % close the warnings during computation
[X, fval,exitflag]=quadprog(H, f, A_cons, b_cons,[], [],lb,ub,[],options);

fprintf('quadprog EXITFLAG = %d\n',exitflag);

%% 计算输出
if ~isempty(X)
    u_piao(1)=X(1);
    u_piao(2)=X(2);
end;
U(1)=kesi(4)+u_piao(1);%用于存储上一个时刻的控制量
U(2)=kesi(5)+u_piao(2);
u_real(1) = U(1) + vd1;
u_real(2) = U(2) + vd2;

sys=u_real % vel, steering, x, y
toc
% End of mdlOutputs.

新版的matlab没有active-set算法，需要更改为'interior-point-convex'，但是会出现X出差索引范围，需要采用旧版的quadprog函数算法，这里更改为quadprog2011，具体代码如下；

function [X,fval,exitflag,output,lambda] = quadprog2011(H,f,A,B,Aeq,Beq,lb,ub,X0,options,varargin)
%QUADPROG Quadratic programming. 
%   X = QUADPROG(H,f,A,b) attempts to solve the quadratic programming 
%   problem:
%
%            min 0.5*x'*H*x + f'*x   subject to:  A*x <= b 
%             x    
%
%   X = QUADPROG(H,f,A,b,Aeq,beq) solves the problem above while 
%   additionally satisfying the equality constraints Aeq*x = beq.
%
%   X = QUADPROG(H,f,A,b,Aeq,beq,LB,UB) defines a set of lower and upper
%   bounds on the design variables, X, so that the solution is in the 
%   range LB <= X <= UB. Use empty matrices for LB and UB if no bounds 
%   exist. Set LB(i) = -Inf if X(i) is unbounded below; set UB(i) = Inf if 
%   X(i) is unbounded above.
%
%   X = QUADPROG(H,f,A,b,Aeq,beq,LB,UB,X0) sets the starting point to X0.
%
%   X = QUADPROG(H,f,A,b,Aeq,beq,LB,UB,X0,OPTIONS) minimizes with the 
%   default optimization parameters replaced by values in the structure 
%   OPTIONS, an argument created with the OPTIMSET function. See OPTIMSET 
%   for details. Used options are Display, Diagnostics, TolX, TolFun, 
%   HessMult, LargeScale, MaxIter, PrecondBandWidth, TypicalX, TolPCG, and 
%   MaxPCGIter. Currently, only 'final' and 'off' are valid values for the 
%   parameter Display ('iter' is not available).
%
%   X = QUADPROG(PROBLEM) finds the minimum for PROBLEM. PROBLEM is a
%   structure with matrix 'H' in PROBLEM.H, the vector 'f' in PROBLEM.f,
%   the linear inequality constraints in PROBLEM.Aineq and PROBLEM.bineq,
%   the linear equality constraints in PROBLEM.Aeq and PROBLEM.beq, the
%   lower bounds in PROBLEM.lb, the upper bounds in PROBLEM.ub, the start
%   point in PROBLEM.x0, the options structure in PROBLEM.options, and 
%   solver name 'quadprog' in PROBLEM.solver. Use this syntax to solve at 
%   the command line a problem exported from OPTIMTOOL. The structure 
%   PROBLEM must have all the fields. 
%
%   [X,FVAL] = QUADPROG(H,f,A,b) returns the value of the objective 
%   function at X: FVAL = 0.5*X'*H*X + f'*X.
%
%   [X,FVAL,EXITFLAG] = QUADPROG(H,f,A,b) returns an EXITFLAG that 
%   describes the exit condition of QUADPROG. Possible values of EXITFLAG 
%   and the corresponding exit conditions are
%
%   All algorithms:
%     1  First order optimality conditions satisfied.
%     0  Maximum number of iterations exceeded.
%    -2  No feasible point found.
%    -3  Problem is unbounded.
%   Interior-point-convex only:
%    -6  Non-convex problem detected.
%   Trust-region-reflective only:
%     3  Change in objective function too small.
%    -4  Current search direction is not a descent direction; no further 
%         progress can be made.
%   Active-set only:
%     4  Local minimizer found.
%    -7  Magnitude of search direction became too small; no further 
%         progress can be made. The problem is ill-posed or badly 
%         conditioned.
%
%   [X,FVAL,EXITFLAG,OUTPUT] = QUADPROG(H,f,A,b) returns a structure
%   OUTPUT with the number of iterations taken in OUTPUT.iterations,
%   maximum of constraint violations in OUTPUT.constrviolation, the 
%   type of algorithm used in OUTPUT.algorithm, the number of conjugate
%   gradient iterations (if used) in OUTPUT.cgiterations, a measure of 
%   first order optimality (large-scale algorithm only) in 
%   OUTPUT.firstorderopt, and the exit message in OUTPUT.message.
%
%   [X,FVAL,EXITFLAG,OUTPUT,LAMBDA] = QUADPROG(H,f,A,b) returns the set of 
%   Lagrangian multipliers LAMBDA, at the solution: LAMBDA.ineqlin for the 
%   linear inequalities A, LAMBDA.eqlin for the linear equalities Aeq, 
%   LAMBDA.lower for LB, and LAMBDA.upper for UB.
%
%   See also LINPROG, LSQLIN.

%   Copyright 1990-2010 The MathWorks, Inc.
%   $Revision: 1.1.6.14 $  $Date: 2010/11/01 19:41:32 $

defaultopt = struct( ...
    'Algorithm','trust-region-reflective', ...
    'Diagnostics','off', ...
    'Display','final', ...
    'HessMult',[], ... 
    'LargeScale','on', ...
    'MaxIter',[], ...    
    'MaxPCGIter','max(1,floor(numberOfVariables/2))', ...   
    'PrecondBandWidth',0, ... 
    'TolCon',1e-8, ...
    'TolFun',[], ...
    'TolPCG',0.1, ...    
    'TolX',100*eps, ...
    'TypicalX','ones(numberOfVariables,1)' ...    
    );

% If just 'defaults' passed in, return the default options in X
if nargin == 1 && nargout <= 1 && isequal(H,'defaults')
   X = defaultopt;
   return
end

if nargin < 10
    options = [];
    if nargin < 9
        X0 = [];
        if nargin < 8
            ub = [];
            if nargin < 7
                lb = [];
                if nargin < 6
                    Beq = [];
                    if nargin < 5
                        Aeq = [];
                        if nargin < 4
                            B = [];
                            if nargin < 3
                                A = [];
                            end
                        end
                    end
                end
            end
        end
    end
end

% Detect problem structure input
if nargin == 1
   if isa(H,'struct')
       [H,f,A,B,Aeq,Beq,lb,ub,X0,options] = separateOptimStruct(H);
   else % Single input and non-structure.
        error(message('optim:quadprog:InputArg'));
   end
end

if nargin == 0 
   error(message('optim:quadprog:NotEnoughInputs'))
end

% Check for non-double inputs
% SUPERIORFLOAT errors when superior input is neither single nor double;
% We use try-catch to override SUPERIORFLOAT's error message when input
% data type is integer.
try
    dataType = superiorfloat(H,f,A,B,Aeq,Beq,lb,ub,X0);
catch ME
    if strcmp(ME.identifier,'MATLAB:datatypes:superiorfloat')
        dataType = 'notDouble';
    end
end

if ~strcmp(dataType,'double')
    error(message('optim:quadprog:NonDoubleInput'))
end
                     
% Set up constant strings
activeSet =  'active-set';
trustRegReflect = 'trust-region-reflective';
interiorPointConvex = 'interior-point-convex';

if nargout > 4
   computeLambda = true;
else 
   computeLambda = false;
end
if nargout > 3
   computeConstrViolation = true;
   computeFirstOrderOpt = true;
else 
   computeConstrViolation = false;
   computeFirstOrderOpt = false;
end

% Options setup
largescale = isequal(optimget(options,'LargeScale',defaultopt,'fast'),'on');
Algorithm = optimget(options,'Algorithm',defaultopt,'fast'); 

diagnostics = isequal(optimget(options,'Diagnostics',defaultopt,'fast'),'on');
display = optimget(options,'Display',defaultopt,'fast');
detailedExitMsg = ~isempty(strfind(display,'detailed'));
switch display
case {'off', 'none'}
   verbosity = 0;
case {'iter','iter-detailed'}
   verbosity = 2;
case {'final','final-detailed'}
   verbosity = 1;
case 'testing'
   verbosity = 3;
otherwise
   verbosity = 1;
end


% Determine algorithm user chose via options. (We need this now to set
% OUTPUT.algorithm in case of early termination due to inconsistent
% bounds.) This algorithm choice may be modified later when we check the
% problem type.
algChoiceOptsConflict = false;
if strcmpi(Algorithm,'active-set')
    output.algorithm = activeSet;
elseif strcmpi(Algorithm,'interior-point-convex')
    output.algorithm = interiorPointConvex;
elseif strcmpi(Algorithm,'trust-region-reflective')
    if largescale
        output.algorithm = trustRegReflect;
    else
        % Conflicting options Algorithm='trust-region-reflective' and
        % LargeScale='off'. Choose active-set algorithm.
        algChoiceOptsConflict = true; % Warn later, not in case of early termination
        output.algorithm = activeSet;
    end
else
    error(message('optim:quadprog:InvalidAlgorithm'));
end 

mtxmpy = optimget(options,'HessMult',defaultopt,'fast');
% Check for name clash
functionNameClashCheck('HessMult',mtxmpy,'hessMult_optimInternal','optim:quadprog:HessMultNameClash');
if isempty(mtxmpy)
    % Internal Hessian-multiply function
    mtxmpy = @hessMult_optimInternal;
    usrSuppliedHessMult = false;     
else
    usrSuppliedHessMult = true;
end

% Set the constraints up: defaults and check size
[nineqcstr,numberOfVariablesineq] = size(A);
[neqcstr,numberOfVariableseq] = size(Aeq);
if isa(H,'double') && ~usrSuppliedHessMult
   % H must be square and have the correct size 
   nColsH = size(H,2);
   if nColsH ~= size(H,1)
      error(message('optim:quadprog:NonSquareHessian'));
   end
else % HessMult in effect, so H can be anything
   nColsH = 0;
end

% Check the number of variables. The check must account for any combination of these cases:
% * User provides HessMult
% * The problem is linear (H = zeros, or H = [])
% * The objective has no linear component (f = [])
% * There are no linear constraints (A,Aeq = [])
% * There are no, or partially specified, bounds 
% * There is no X0
numberOfVariables = ...
    max([length(f),nColsH,numberOfVariablesineq,numberOfVariableseq]);

if numberOfVariables == 0
    % If none of the problem quantities indicate the number of variables,
    % check X0, even though some algorithms do not use it.
    if isempty(X0)
        error(message('optim:quadprog:EmptyProblem'));
    else
        % With all other data empty, use the X0 input to determine
        % the number of variables.
        numberOfVariables = length(X0);
    end
end

ncstr = nineqcstr + neqcstr;

if isempty(f)
    f = zeros(numberOfVariables,1);
else 
    % Make sure that the number of rows/columns in H matches the length of
    % f under the following conditions:
    % * The Hessian is passed in explicitly (no HessMult)
    % * There is a non-empty Hessian
    if ~usrSuppliedHessMult && ~isempty(H)
        if length(f) ~= nColsH
            error(message('optim:quadprog:MismatchObjCoefSize'));
        end
    end
end
if isempty(A)
    A = zeros(0,numberOfVariables);
end
if isempty(B)
    B = zeros(0,1);
end
if isempty(Aeq)
    Aeq = zeros(0,numberOfVariables); 
end
if isempty(Beq)
    Beq = zeros(0,1);
end

% Expect vectors
f = f(:);
B = B(:);
Beq = Beq(:);

if ~isequal(length(B),nineqcstr)
    error(message('optim:quadprog:InvalidSizesOfAAndB'))
elseif ~isequal(length(Beq),neqcstr)
    error(message('optim:quadprog:InvalidSizesOfAeqAndBeq'))
elseif ~isequal(length(f),numberOfVariablesineq) && ~isempty(A)
    error(message('optim:quadprog:InvalidSizesOfAAndF'))
elseif ~isequal(length(f),numberOfVariableseq) && ~isempty(Aeq)
    error(message('optim:quadprog:InvalidSizesOfAeqAndf'))
end

[X0,lb,ub,msg] = checkbounds(X0,lb,ub,numberOfVariables);
if ~isempty(msg)
   exitflag = -2;
   X=X0; fval = []; lambda = [];
   output.iterations = 0;
   output.constrviolation = [];
   output.algorithm = ''; % Not known at this stage
   output.firstorderopt = [];
   output.cgiterations = []; 
   output.message = msg;
   if verbosity > 0
      disp(msg)
   end
   return
end

% Check that all data is real
if ~(isreal(H) && isreal(A) && isreal(Aeq) && isreal(f) && ...
     isreal(B) && isreal(Beq) && isreal(lb) && isreal(ub) && isreal(X0))
    error(message('optim:quadprog:ComplexData'))
end

caller = 'quadprog';
% Check out H and make sure it isn't empty or all zeros
if isa(H,'double') && ~usrSuppliedHessMult
   if norm(H,'inf')==0 || isempty(H)
      % Really a lp problem
      warning(message('optim:quadprog:NullHessian'))
      [X,fval,exitflag,output,lambda]=linprog(f,A,B,Aeq,Beq,lb,ub,X0,options);
      return
   else
      % Make sure it is symmetric
      if norm(H-H',inf) > eps
         if verbosity > -1
            warning(message('optim:quadprog:HessianNotSym'))
         end
         H = (H+H')*0.5;
      end
   end
end


% Determine which algorithm and make sure problem matches.
hasIneqs = (nineqcstr > 0);  % Does the problem have any inequalities?
hasEqsAndBnds = (neqcstr > 0) && (any(isfinite(ub)) || any(isfinite(lb))); % Does the problem have both equalities and bounds?
hasMoreEqsThanVars = (neqcstr > numberOfVariables); % Does the problem have more equalities than variables?
hasNoConstrs = (neqcstr == 0) && (nineqcstr == 0) && ...
    all(eq(ub, inf)) && all(eq(lb, -inf)); % Does the problem not have equalities, bounds, or inequalities?

if (hasIneqs || hasEqsAndBnds || hasMoreEqsThanVars || hasNoConstrs) && ...
        strcmpi(output.algorithm,trustRegReflect) || strcmpi(output.algorithm,activeSet)
   % (has linear inequalites OR both equalities and bounds OR has no constraints OR
   % has more equalities than variables) then call active-set code
   if algChoiceOptsConflict
       % Active-set algorithm chosen as a result of conflicting options
       warning('optim:quadprog:QPAlgLargeScaleConflict', ...
           ['Options LargeScale = ''off'' and Algorithm = ''trust-region-reflective'' conflict. ' ...
           'Ignoring Algorithm and running active-set algorithm. To run trust-region-reflective, set ' ...
           'LargeScale = ''on''. To run active-set without this warning, set Algorithm = ''active-set''.']);
   end

   if strcmpi(output.algorithm,trustRegReflect)
     warning('optim:quadprog:SwitchToMedScale', ...
            ['Trust-region-reflective algorithm does not solve this type of problem, ' ...
            'using active-set algorithm. You could also try the interior-point-convex ' ...
            'algorithm: set the Algorithm option to ''interior-point-convex'' ', ...
            'and rerun. For more help, see %s in the documentation.'], ...
            addLink('Choosing the Algorithm','choose_algorithm'))
   end
   output.algorithm = activeSet;
   Algorithm = 'active-set';
   if issparse(H)  || issparse(A) || issparse(Aeq) % Passed in sparse matrices
       warning(message('optim:quadprog:ConvertingToFull'))
   end
   H = full(H); A = full(A); Aeq = full(Aeq);
else
    % Using trust-region-reflective or interior-point-convex algorithms
   if ~usrSuppliedHessMult
     H = sparse(H);
   end
   A = sparse(A); Aeq = sparse(Aeq);
end
if ~isa(H,'double') || usrSuppliedHessMult &&  ...
        ~strcmpi(output.algorithm,trustRegReflect)
    error(message('optim:quadprog:NoHessMult', Algorithm))
end

if diagnostics 
   % Do diagnostics on information so far
   gradflag = []; hessflag = []; line_search=[];
   constflag = 0; gradconstflag = 0; non_eq=0;non_ineq=0;
   lin_eq=size(Aeq,1); lin_ineq=size(A,1); XOUT=ones(numberOfVariables,1);
   funfcn{1} = [];ff=[]; GRAD=[];HESS=[];
   confcn{1}=[];c=[];ceq=[];cGRAD=[];ceqGRAD=[];
   msg = diagnose('quadprog',output,gradflag,hessflag,constflag,gradconstflag,...
      line_search,options,defaultopt,XOUT,non_eq,...
      non_ineq,lin_eq,lin_ineq,lb,ub,funfcn,confcn,ff,GRAD,HESS,c,ceq,cGRAD,ceqGRAD);
end

% Trust-region-reflective
if strcmpi(output.algorithm,trustRegReflect)
    % Call sqpmin when just bounds or just equalities
    [X,fval,output,exitflag,lambda] = sqpmin(f,H,mtxmpy,X0,Aeq,Beq,lb,ub,verbosity, ...
        options,defaultopt,computeLambda,computeConstrViolation,varargin{:});

    if exitflag == -10  % Problem not handled by sqpmin at this time: dependent rows
        warning(message('optim:quadprog:SwitchToMedScale'))
        output.algorithm = activeSet;
        if ~isa(H,'double') || usrSuppliedHessMult
            error('optim:quadprog:NoHessMult', ...
                'H must be specified explicitly for active-set algorithm: cannot use HessMult option.')
        end
        H = full(H); A = full(A); Aeq = full(Aeq);
    end
end
% Call active-set algorithm
if strcmpi(output.algorithm,activeSet)
   if isempty(X0)
      X0 = zeros(numberOfVariables,1); 
   end
   % Set default value of MaxIter for qpsub
   defaultopt.MaxIter = 200;
   % Create options structure for qpsub
   qpoptions.MaxIter = optimget(options,'MaxIter',defaultopt,'fast');
   % A fixed constraint tolerance (eps) is used for constraint
   % satisfaction; no need to specify any value
   qpoptions.TolCon = [];
    
   [X,lambdaqp,exitflag,output,~,~,msg]= ...
      qpsub(H,f,[Aeq;A],[Beq;B],lb,ub,X0,neqcstr,...
      verbosity,caller,ncstr,numberOfVariables,qpoptions); 
   output.algorithm = activeSet; % have to reset since call to qpsub obliterates
   
end

if strcmpi(output.algorithm,interiorPointConvex)
    defaultopt.MaxIter = 200;
    defaultopt.TolFun = 1e-8;
    % If the output structure is requested, we must reconstruct the
    % Lagrange multipliers in the postsolve. Therefore, set computeLambda
    % to true if the output structure is requested.
    flags.computeLambda = computeFirstOrderOpt; 
    flags.detailedExitMsg = detailedExitMsg;
    flags.verbosity = verbosity;
    [X,fval,exitflag,output,lambda] = ipqpcommon(H,f,A,B,Aeq,Beq,lb,ub,X0, ...
                                          flags,options,defaultopt,varargin{:});
    
    % Presolve may have removed variables and constraints from the problem.
    % Postsolve will re-insert the primal and dual solutions after the main
    % algorithm has run. Therefore, constraint violation and first-order
    % optimality must be re-computed.
    %  
    % If no initial point was provided by the user and the presolve has
    % declared the problem infeasible or unbounded, X will be empty. The
    % lambda structure will also be empty, so do not compute constraint
    % violation or first-order optimality if lambda is missing.
    
    % Compute constraint violation if the output structure is requested
    if computeFirstOrderOpt && ~isempty(lambda)
        output.constrviolation = norm([Aeq*X-Beq; max([A*X - B;X - ub;lb - X],0)],Inf);        
    end
end

% Compute fval and first-order optimality if the active-set algorithm was
% run, or if the interior-point-convex algorithm was run (not stopped in presolve)
if (strcmpi(output.algorithm,interiorPointConvex) && ~isempty(lambda)) || ...
    strcmpi(output.algorithm,activeSet)
    % Compute objective function value
    fval = 0.5*X'*(H*X)+f'*X;
   
   % Compute lambda and exit message for active-set algorithm
   if strcmpi(output.algorithm,activeSet)
       if computeLambda || computeFirstOrderOpt
           llb = length(lb);
           lub = length(ub);
           lambda.lower = zeros(llb,1);
           lambda.upper = zeros(lub,1);
           arglb = ~isinf(lb); lenarglb = nnz(arglb);
           argub = ~isinf(ub); lenargub = nnz(argub);
           lambda.eqlin = lambdaqp(1:neqcstr,1);
           lambda.ineqlin = lambdaqp(neqcstr+1:neqcstr+nineqcstr,1);
           lambda.lower(arglb) = lambdaqp(neqcstr+nineqcstr+1:neqcstr+nineqcstr+lenarglb);
           lambda.upper(argub) = lambdaqp(neqcstr+nineqcstr+lenarglb+1: ...
               neqcstr+nineqcstr+lenarglb+lenargub);
       end
       if exitflag == 1
           normalTerminationMsg = sprintf('Optimization terminated.');
           if verbosity > 0
               disp(normalTerminationMsg)
           end
           if isempty(msg)
               output.message = normalTerminationMsg;
           else
               % append normal termination msg to current output msg
               output.message = sprintf('%s\n%s',msg,normalTerminationMsg);
           end
       else
           output.message = msg;
       end
   end
   % Compute first order optimality if needed
   if computeFirstOrderOpt && ~isempty(lambda)
      output.firstorderopt = computeKKTErrorForQPLP(H,f,A,B,Aeq,Beq,lb,ub,lambda,X); 
   else
      output.firstorderopt = []; 
   end
   output.cgiterations = [];  
end

结果：绿线为目标轨迹，红虚线为mpc控制车辆运行轨迹