nonlinear-mpc fmincon error

Question

Marshall Botta 2024-2-28

0
链接

此问题的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/2088251-nonlinear-mpc-fmincon-error

评论： Emmanouil Tzorakoleftherakis 2024-2-28

I am trying to make a bicycle vehicle model follow a reference trajectory that makes a vehicle go straight and slows down before reaching its final destination. I am getting an error from this Error Report from the mpc and can't seem to understand what its asking for since its an error under the mask of the nlmpc...

------------------------------------------------------
Start of Error Report                
------------------------------------------------------
Error using sqpInterface
Nonlinear constraint function is undefined at initial point. Fmincon cannot continue.
Error in fmincon (line 904)
[X,FVAL,EXITFLAG,OUTPUT,LAMBDA,GRAD,HESSIAN] = sqpInterface(funfcn,X,full(A),full(B),full(Aeq),full(Beq), ...
    
Error in nlmpc/nlmpcmove (line 174)
[z, cost, ExitFlag, Out] = fmincon(CostFcn, z0, A, B, [], [], zLB, zUB, ConFcn, fminconOpt);
Error in nmpcblock_interface (line 163)
[mv, ~, Info] = nlmpcmove(nlobj, x, lastmv, ref, md, Options);
------------------------------------------------------
End of Error Report                
------------------------------------------------------
Error:Error occurred when calling NLP solver "fmincon". See the error report displayed above.
Error in nmpcblock_interface.m (line 165)
throw(ME)
Error in 'noMatlabFunctionscen/MPC controller/Nonlinear MPC Controller/MPC/NLMPC' (line 24)

Here is my nlmpc settup code, nonlinear model code, and nonlinear jacobian model...

%% nlmpc settup code
clear all, close all, clc
Rho = 0.2; 
Amax_accel = 4; %% Ego's max acceleration
Amax_brake = 5; %% Object's max brake
Amin_brake = 3; %% Object's min brake
Amin_brake_correct = 2; %% Ego's min brake
% During time [0,Rho] two cars will apply lateral acceleration towards each other then apply lateral deacceleration and the distance between both cars are at the minimal distance of u
Amax_lat_accel = 2; %% Maximum Lateral acceleration both cars are heading towards each other during [0,rho]... This is assuming the things coming by 
Amin_lat_brake = 5; %% Minimum lateral brake acceleration both cars will do until they reach to 0 velocity or no collision...
VP = Rho*Amax_accel;
V1LatP= Rho*Amax_lat_accel;
%%MPC controller
nx = 10; % # of state variables
ny = 4; % # of output variables
nu = 2; % # of input variables [acceleration steeringangle]
nlobj = nlmpc(nx,ny,nu);
Zero weights are applied to one or more OVs because there are fewer MVs than OVs.
Ts = 0.01;
nlobj.Ts = Ts;
nlobj.PredictionHorizon = 10;
nlobj.ControlHorizon = 3;
nlobj.MV(1).Min = Amin_brake_correct;
nlobj.MV(1).Max = Amax_accel;
nlobj.MV(2).Min = -1.0472; %% degrees:-60 (radians:-1.0472 )
nlobj.MV(2).Max = 1.0472; %% degrees:60 (radians:1.0472 )
nlobj.Model.StateFcn = @(x,u) NonlinModel(x,u);
nlobj.Jacobian.StateFcn = @(x,u) NonlinModelJacobian(x,u);
nlobj.Model.OutputFcn = @(x,u) [x(3);x(8);x(9);x(10)];
nlobj.Jacobian.OutputFcn = @(x,u) [0 0 1 0 0 0 0 0 0 0;...
                                   0 0 0 0 0 0 0 1 0 0;...
                                   0 0 0 0 0 0 0 0 1 0;...
                                   0 0 0 0 0 0 0 0 0 1];
nlobj.Weights.OutputVariables = [1 1 1 1];
nlobj.Weights.ManipulatedVariables = [0.3 0.1];
x0 = [-12.4781 7.3388 20 0 0 -0.0226 0 155.4781 0.0612 -0.0226]; %% [X Y Vx Vxdot Vy Yaw Yawdot e1 e2 e3] % e1=Xd-X e2=Yd-Y e2=Yawd-Yaw 
u0 = [0 0];
ref0 = [0; 0; 0; 0].';
validateFcns(nlobj,x0,u0,{},{},ref0)
Error using validateFcns
Expected x to be an array with number of elements equal to 10.

Error in nlmpc/validateFcns (line 43)
validateattributes(x,{'double'},{'vector','real','finite','numel',nlobj.nx},FcnName,'x');

This is the new error...

%% nonlinear model code
function dxdt = NonlinModel(x,u)
%%[X Y Vx Vxdot Vy Yaw Yawdot e1 e2 e3]
m = 1600;
Iz = 3500;
lf = 3;
lr = 1.5;
Cf = 35000;
Cr = 30000;
T = 0.2;
Xd = 143;
Yd = 7;
Yawd = 0;
a1 = -(2*Cf+2*Cr)/m/x(3); 
a2 = -(2*Cf*lf-2*Cr*lr)/m/x(3) - x(3);
a3 = -(2*Cf*lf-2*Cr*lr)/Iz/x(3);
a4 = -(2*Cf*lf^2+2*Cr*lr^2)/Iz/x(3);
b1 = 2*Cf/m;
b2 = 2*Cf*lf/Iz;
dxdt = x;
dxdt(1) = x(3);                        %% X  
dxdt(2) = x(5);                        %% Y
dxdt(3) = x(7)*x(5)+x(4);              %% Vx ?
dxdt(4) = (1/T)*(-x(4) + u(1));      %% Vxdot ?
dxdt(5) = a1*x(5) + a2*x(7) + b1*u(2); %% Vy
dxdt(6) = x(7);                        %% Yaw
dxdt(7) = a3*x(5) + a4*x(7) + b2*u(2); %% Yawdot
dxdt(8) = Xd-sin(x(6));                 %% Error 1 X difference  Vxd-cos(x(6))
dxdt(9) = Yd+cos(x(6));                 %% Error 2 Y difference  Vyd-sin(x(6))
dxdt(10) = Yawd-x(6);                  %% Error 3 Yaw difference Yawdotd - x(6)
end

%% nonlinear jacobian model
function [A,B] = NonlinModelJacobian(x,u)
%% [X Y Vx Vxdot Vy Yaw Yawdot e1 e2 e3]
m = 1600;
Iz = 3500;
lf = 3;
lr = 1.5;
Cf = 35000;
Cr = 30000;
T = 0.2;
Xd = 143;
Yd = 7;
Yawd = 0;
a1 = -(2*Cf+2*Cr)/m/x(3); 
a2 = -(2*Cf*lf-2*Cr*lr)/m/x(3) - x(3);
a3 = -(2*Cf*lf-2*Cr*lr)/Iz/x(3);
a4 = -(2*Cf*lf^2+2*Cr*lr^2)/Iz/x(3);
b1 = 2*Cf/m;
b2 = 2*Cf*lf/Iz;
A = zeros(10,10);
A(1,3) = 1;
A(2,5) = 1;
A(3,4) = 1;
A(3,5) = x(7);
A(3,7) = x(5);
A(4,4) = -1/T;
A(5,3) = ((2*Cf+2*Cr)/m/(x(3)^2))*x(1) + (((2*Cf*lf-2*Cr*lr)/m/(x(3)^2))-1)*x(2);
A(5,5) = a1;
A(5,7) = a2;
A(7,3) = ((2*Cf*lf-2*Cr*lr)/Iz/(x(3)^2))*x(1) + ((2*Cf*lf^2+2*Cr*lr^2)/Iz/(x(3)^2))*x(2);
A(7,5) = a3;
A(7,7) = a4;
A(8,6) = -cos(x(6));
A(9,6) = -sin(x(6));
A(10,10) = -1;
B = zeros(10,2);
B(4,1) = 1/T;
B(5,2) = b1;
B(7,2) = b2;
end

Here is the simulink model...

10 个评论
显示 8更早的评论隐藏 8更早的评论

Torsten 2024-2-28

编辑：Torsten 2024-2-28

在 MATLAB Online 中打开

x=sym('x',[10 1]);

u = sym('u',[2 1]);

dxdt = NonlinModel(x,u);

jacobian(dxdt,x)

ans =

jacobian(dxdt,u)

ans =

%% nonlinear model code

function dxdt = NonlinModel(x,u)

%%[X Y Vx Vxdot Vy Yaw Yawdot e1 e2 e3]

m = 1600;

Iz = 3500;

lf = 3;

lr = 1.5;

Cf = 35000;

Cr = 30000;

T = 0.2;

Xd = 143;

Yd = 7;

Yawd = 0;

a1 = -(2*Cf+2*Cr)/m/x(3);

a2 = -(2*Cf*lf-2*Cr*lr)/m/x(3) - x(3);

a3 = -(2*Cf*lf-2*Cr*lr)/Iz/x(3);

a4 = -(2*Cf*lf^2+2*Cr*lr^2)/Iz/x(3);

b1 = 2*Cf/m;

b2 = 2*Cf*lf/Iz;

dxdt = x;

dxdt(1) = x(3); %% X

dxdt(2) = x(5); %% Y

dxdt(3) = x(7)*x(5)+x(4); %% Vx ?

dxdt(4) = (1/T)*(-x(4) + u(1)); %% Vxdot ?

dxdt(5) = a1*x(5) + a2*x(7) + b1*u(2); %% Vy

dxdt(6) = x(7); %% Yaw

dxdt(7) = a3*x(5) + a4*x(7) + b2*u(2); %% Yawdot

dxdt(8) = Xd-sin(x(6)); %% Error 1 X difference Vxd-cos(x(6))

dxdt(9) = Yd+cos(x(6)); %% Error 2 Y difference Vyd-sin(x(6))

dxdt(10) = Yawd-x(6); %% Error 3 Yaw difference Yawdotd - x(6)

end

Marshall Botta 2024-2-28

编辑：Marshall Botta 2024-2-28

Ok I didn't even know you could solve jacobians like that. This works and its validated completely with no errors! Also in my simulink model the error was still occurring and then I directly fed vx, vy, yaw and yawdot data into the mux block rather than using gotos and froms and no more errors! The simulation is running im so happy!