Reduce simulation time in predictive control using fmincon

11 次查看（过去 30 天）
Kasper
Kasper 2023-5-13
编辑： Rasmita 2023-6-6
Hi I am designing an model predictive controller (mpc) for a differential wheeled robot in Simulink.
I have a model of the robot in the Simulink model where I want to implement my mpc. I have choosen to define the mpc without the use of the nlmpc function in matlab to make the code more accesable by the user and for learning purposes.
I have made the mpc within a matlab function block where I use the fmincon function as my optimization solver.
The problem is that when I run the code the simulation time is extremely dependend on the weights. I have the weight matrices
Q (7X7 diagonal)
R (2x2 diagonal)
and the simulation runs smoothly when I only have weights on the first 3 variables in Q. However, adding weight just one one additional variable (such that I weight the first 4 in Q) makes the simulation run extremely slow.
So my question is there a way to ease the simulation time such that it has a more consistent simulation time regardless of the weights?
The code I have within the function block is given below
function [J_val,exitflag,mv] = fcn(x_ref, x_prev, u_prev,param,lb, ub,Ts_mpc, Nc, N,Q, R,Ref)
% Define optimization problem
options = optimoptions('fmincon', 'Display', 'none','Algorithm','sqp');
% Define initial guess for control input
u_0 = u_prev;
% Define cost function
cost = @(u_0) Cost_fun(u_0,x_prev,x_ref,param,Ts_mpc,N,Q,R,Ref);
% Solve the optimization problem
[u_sol, J_val, exitflag] = fmincon(cost, u_0, [], [], [],[], lb, ub,[], options);
% Output the optimal control input
mv = u_sol;
end
%%% ---------- Predictor ---------- %%%
function x_next = predictor(u_0,x_prev,param,Ts_mpc,kappa)
% Defining the parameters for the predictions
L_w = param(1);
L_OC = param(2);
L_OS_y1 = param(3);
r_wheel = param(4);
m_w = param(5);
m_body = param(6);
I_body = param(7);
I_w = param(8);
I_m = param(9);
kappa = 1;
% spray = param(10);
%Defining the current inputs
T_R = u_0(1);
T_L = u_0(2);
% Defining the current states
CTE = x_prev(1); % Lateral error
ye = x_prev(2); % Orientation error
S = x_prev(3); % Traveled distance along path
vs = x_prev(4); % Measured velocity at spray point
w = x_prev(5); % Measured angular velocity
dv = x_prev(6); % Change in linear velocity
dw = x_prev(7); % Change in angular velocity
v_cm = vs-L_OS_y1*w; % Linear velocity at CM
% defining the differential equation for the states
CTE_dot = vs*sin(ye);
ye_dot = w-(vs*cos(ye)/(1-CTE*kappa))*kappa;
v_dot = (r_wheel*(T_L + T_R - L_OC*m_body*r_wheel*w^2))/(2*I_w + m_body*r_wheel^2 + 2*m_w*r_wheel^2);
w_dot = (r_wheel*(L_w*T_R - L_w*T_L + L_OC*m_body*r_wheel*w*v_cm))/(2*I_w*L_w^2 + I_body*r_wheel^2 + 2*I_m*r_wheel^2 + L_OC^2*m_body*r_wheel^2 + 2*L_w^2*m_w*r_wheel^2);
S_dot = vs*cos(ye)/(1-CTE*kappa);
% Predict the next state based on the current state and input
CTE_next = CTE + Ts_mpc*CTE_dot;
ye_next = ye + Ts_mpc*ye_dot;
S_next = S + Ts_mpc*S_dot;
v_next = vs + Ts_mpc*(v_dot+w_dot*L_OS_y1);
w_next = w + Ts_mpc*w_dot;
dv_next = v_dot;
dw_next = w_dot;
% Save the state prediction in the vector
x_next = [CTE_next;ye_next;S_next;v_next;w_next;dv_next;dw_next];
end
%%% ---------- Optimization ---------- %%%
function J = Cost_fun(u_0,x_prev,x_ref,param,Ts_mpc,N,Q,R,Ref)
%%% Optimization loop %%%
J = 0;
for i =1:N-1
% Defining the distance along the path
S_ref = Ref(:,9);
S = x_prev(3);
% Updating reference based on robot location
if S <= S_ref(1)
j = 1;
else
j = find(S>=Ref(:,9),1,'last');
end
if j >= length(S_ref)-1
j = length(S_ref)-1;
end
kappa = Ref(j,7);
x_ref = [0;0;Ref(j,9);Ref(j,10);Ref(j,11);0;0];
% Predict the next states
x_next = predictor(u_0,x_prev,param,Ts_mpc,kappa);
% Calculate the state error
x_e = [x_next(1);x_next(2);x_ref(3) - x_next(3);x_ref(4) - x_next(4);x_ref(5) - x_next(5);x_next(6);x_next(7)];
% Compute the cost function
J = J + x_e'*Q*x_e + u_0'*R*u_0;
% Update the current state
x_prev = x_next;
end
end
Sorry for the long post I hope you can help me.
Best regards.

请先登录，再回答此问题。

回答（1 个）

Rasmita
Rasmita 2023-6-6
编辑：Rasmita 2023-6-6
Hi Kasper,
It is my understanding that, you are dealing with model predictive control (MPC) in Simulink and you are facing issues with simulation time while designing MPC using ‘fmincon’ function as optimization solver.
Here are a few suggestions to ease the simulation time and achieve more consistent results:
  1. Experiment with different solver options and algorithms provided by ‘fmincon’. Some algorithms may have better convergence properties for specific problem characteristics. For example, the ‘interior-point’ algorithm is known for handling larger-scale problems efficiently. Refer to the below documentation link to know more about interior-point algorithm:
https://www.mathworks.com/help/mpc/ref/mpcinteriorpointsolver.html
  1. The choice of simulation time step (Ts_mpc) can affect the convergence and simulation time. Try increasing or decreasing the time step to see if there has been any impact on the speed of convergence.
  2. If your system supports parallel computing, consider utilizing multiple cores or distributed computing to speed up the optimization process. Refer to the below documentation link for further assistance on using parallel computing toolbox: https://www.mathworks.com/help/parallel-computing/getting-started-with-parallel-computing-toolbox.html
Remember to validate the performance of your modified MPC implementation after making any adjustments and assess whether the changes introduced affect the overall control performance and stability of the system.
For more information on ‘fmincon’ solver and configure options, please refer to the below documentation links:
https://www.mathworks.com/help/optim/ug/fmincon.html
https://www.mathworks.com/help/mpc/ug/configure-optimization-solver-for-nonlinear-mpc.html
Hope this helps you resolve the query!

类别

在 Help Center 和 File Exchange 中查找有关 Model Predictive Control Toolbox 的更多信息

标签

产品


版本

R2022a

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!

Translated by