运行MPC的时候 fmincon报错

xiang18 · 发表于 2022-7-22 08:17:40

for j=1:1:N
tic;
[uopt,fval,exitflag]=fmincon(@(deta_u)cost(deta_u,state0,Np,exp,j,u0),detau_u0,A,b,Aeq,beq,detau_lb,detau_ub,[],options);
toc;
%%获得控制输入
u0=uopt+u0;
%    detau_lb=Q*(detau_lb-u0);
%    detau_ub=Q*(detau_ub-u0);
[stateout,Yout]=dyna_model(state0,u0(1,:));
state0=stateout;
figure(100)
plot(Yout(1,:),Yout(2,:),'b*');
hold on;
plot(exp(1,j),exp(2,j),'r*');
hold on;
axis([-2 2 -2 2])
Stateout=[Stateout,stateout]; %存储状态
Yout=[Yout,yout];          %存储位置
uout=[uout;u0];             %存储控制
end

function obj = cost(deta_u,state0,Np,exp,j,u)
%%Np是预测时域
%%循环获得预测位姿
%%矩阵初始化，包含当前状态、预测状态、状态误差和控制量
Xref=exp(1,j:j+Np-1);
Yref=exp(2,j:j+Np-1);
% x=Yout(1,:);
% y=Yout(2,:);
x_predict=zeros(1,Np);
y_predict=zeros(1,Np);
q=zeros(1,Np);
r=zeros(1,Np);
x_error=zeros(1,Np);
y_error=zeros(1,Np);
state_i=state0;
for i=1:1:Np
deta_U=deta_u(i,:);
U=deta_U+u(i,:);
[state_i,Yout]=dyna_model(state_i,U);
%    x_predict(1,i)=state_i(1,:);
%    y_predict(1,i)=state_i(2,:);
x_predict(1,i)=Yout(1,:);
y_predict(1,i)=Yout(2,:);
q(i)=(Np+1-i)*50;
r(i)=(Np+1-i)*50;
%%解算预测位姿和参考轨迹的误差
x_error(1,i)=x_predict(1,i)-Xref(1,i);
y_error(1,i)=y_predict(1,i)-Yref(1,i);
end
Q=diag(q);
R=diag(r);
obj=4.5*(x_error)*Q*(x_error)'+1.8*(y_error)*R*(y_error)';
% obj=(x_error)*(x_error)'+(y_error)*(y_error)';
end

错误使用 barrier
Objective function is undefined at initial point. Fmincon cannot
continue.

出错 fmincon (line 798)
[X,FVAL,EXITFLAG,OUTPUT,LAMBDA,GRAD,HESSIAN] =
barrier(funfcn,X,A,B,Aeq,Beq,l,u,confcn,options.HessFcn, ...

出错 model1 (line 70)
[uopt,fval,exitflag]=fmincon(@(deta_u)cost(deta_u,state0,Np,exp,j,u0),detau_u0,A,b,Aeq,beq,detau_lb,detau_ub,[],options);