基于MPC的自适应巡航控制_蝎z莱莱的博客-CSDN博客

function u= fcn(drel,speed,setspeed)
% coder.extrinsic('myoptimizer');
coder.extrinsic('costfunction');
coder.extrinsic('nonclon');
numsteps=10;
Ts=0.1;
lb=ones(numsteps,1)*(-5);
ub=ones(numsteps,1)*5;
u=0.0;
acc0=rand(numsteps,1);

options = optimoptions('fmincon','Display','iter','Algorithm','sqp');
acc=fmincon(@(acc)costfunction(acc,speed,setspeed,Ts),acc0,[],[],[],[],lb,ub,...
    @(acc)nonclon(acc,Ts,drel,speed),options);
u=acc(1);
% y = myoptimizor(drel,speed,setspeed);
 end

function  cost=costfunction(acc,speed,setspeed,Ts)
        v=speed+cumsum(acc)*Ts;
        verr=v-setspeed;
        cost=verr(:)'*verr(:)+1/2*acc(:)'*acc;       
end

function [c,ceq]=nonclon(acc,Ts,drel,speed)
        ceq=[];
        v=speed+cumsum(acc)*Ts;
        p=drel-cumsum(v)*Ts;
        c=speed*Ts+20-p;
        
end

%%%

郝宁峰复现结果

function u= fcn(dist,speed,setspeed)
coder.extrinsic('myOptimizer');
u=0;
dT=0.1;
numSteps=10;
uopt=myOptimizer(dT,numSteps,speed,alpha);%mpc求解
u=uopt(1);
end

function acc = myOptimizer(dT,numSteps,speed,alpha)
opt=optimset('fmincon');
opt.MaxIter=10;
opt.Algorithm='sqp';
acc0=rand(numSteps,1);
lb=-50*ones(numSteps,1);
ub=650*ones(numSteps,1);
%%acc=Ttq
acc=fmincon(@(acc)localcost(acc,alpha,dT,speed),acc0,[],[],[],[],lb,ub,...
    @(acc)localconstr(acc,dT,speed,alpha),opt);

end

function o = localcost(acc,alpha,dT,speed)
%UNTITLED6 此处显示有关此函数的摘要
%   此处显示详细说明

%% 参数
m=4455;
ig=0.75;
i0=5.0;
yita=0.99;
r=0.51;
v_dot=zeros(11,1);
v=zeros(11,1);
v(1)=speed;
for i=2:11
    v_dot(i)=acc(i-1)*ig*i0*yita/m/r-(1/2)*1.25*0.69*6.8*v(i)^2-0.01*9.8*cos(alpha)-9.8*sin(alpha);
    
        
    v(i)=v(i-1)+v_dot(i)*dT;
end
v_dot=v_dot(2:end);
v=v(2:end);
% v_dot=acc*ig*i0*yita/m/r-(1/2)*1.25*0.69*6.8*speed^2-0.01*9.8*cos(alpha)-9.8*sin(alpha);
% v=speed+cumsum(v_dot)*dT;

T=acc;
f=0.002892 +0.00209 *(v.*60*ig*i0/(2*pi*r))...
+ 0.001245.*T + 0.0005709.*(v.*60*ig*i0/(2*pi*r)).^2 ...
+ 0.0009704.*(v .*60*ig*i0/(2*pi*r)).*T...
-0.0004742.*T.^2 ...
+0.0002978.*(v.*60*ig*i0/(2*pi*r)).^2.*T...
-0.0002978.*(v.*60*ig*i0/(2*pi*r)).*T.^2;

o=sum((f.*dT).^2)

end

function [c,ceq] = localconstr(acc,dT,speed,alpha)
%UNTITLED7 此处显示有关此函数的摘要
%   此处显示详细说明
%% 参数
m=4455;
ig=0.75;
i0=5.0;
yita=0.99;
r=0.51;
v_dot=zeros(11,1);
v=zeros(11,1);
v(1)=speed;
for i=2:11
    v_dot(i)=acc(i-1)*ig*i0*yita/m/r-(1/2)*1.25*0.69*6.8*v(i)^2-0.01*9.8*cos(alpha)-9.8*sin(alpha);
    
        
    v(i)=v(i-1)+v_dot(i)*dT;
end
v_dot=v_dot(2:end);
v=v(2:end);

x=sum(v*dT);
%
% ;v-100/3.6;100/3.6*10-0.75*40/3.6*10-x
c=[60/3.6-v;v-100/3.6;100/3.6*1-0.75*40/3.6*1-x];
ceq=[];
end