212 lines
8.2 KiB
Matlab
212 lines
8.2 KiB
Matlab
|
close all
|
||
|
|
clear
|
||
|
|
|
||
|
|
%% Setup and Parameters
|
||
|
|
x0 = [-5; -5; 0; 0];
|
||
|
|
time_total = 20.0;
|
||
|
|
dt = 0.2;
|
||
|
|
P = 100*eye(4);
|
||
|
|
Q = 10*eye(4);
|
||
|
|
R = eye(2);
|
||
|
|
N = 8;
|
||
|
|
xmin = [-5; -5; -5; -5];
|
||
|
|
xmax = [5; 5; 5; 5];
|
||
|
|
umin = [-1; -1];
|
||
|
|
umax = [1; 1];
|
||
|
|
|
||
|
|
%% Discrete-time double integrator 2D
|
||
|
|
system.dt = dt;
|
||
|
|
system.A = [1 0 dt 0;
|
||
|
|
0 1 0 dt;
|
||
|
|
0 0 1 0;
|
||
|
|
0 0 0 1];
|
||
|
|
system.B = [0.5*dt^2 0;
|
||
|
|
0 0.5*dt^2;
|
||
|
|
dt 0;
|
||
|
|
0 dt];
|
||
|
|
system.xl = xmin;
|
||
|
|
system.xu = xmax;
|
||
|
|
system.ul = umin;
|
||
|
|
system.uu = umax;
|
||
|
|
|
||
|
|
%% MPC-CBF parameters
|
||
|
|
params.Q = Q;
|
||
|
|
params.R = R;
|
||
|
|
params.P = P;
|
||
|
|
params.N = N;
|
||
|
|
params.gamma = 0.5;
|
||
|
|
|
||
|
|
%% Obstacle
|
||
|
|
obs.pos = [-2; -2.25];
|
||
|
|
obs.r = 1.5;
|
||
|
|
|
||
|
|
%% Simulate MPC-CBF
|
||
|
|
params.N = 5;
|
||
|
|
params.gamma = 0.25;
|
||
|
|
controller_mpc_cbf_1 = MPCCBF(x0, system, params);
|
||
|
|
controller_mpc_cbf_1.obs = obs;
|
||
|
|
controller_mpc_cbf_1.sim(time_total);
|
||
|
|
|
||
|
|
%% Simulate MPC-CBF with lower gamma
|
||
|
|
params.gamma = 0.20;
|
||
|
|
controller_mpc_cbf_2 = MPCCBF(x0, system, params);
|
||
|
|
controller_mpc_cbf_2.obs = obs;
|
||
|
|
controller_mpc_cbf_2.sim(time_total);
|
||
|
|
|
||
|
|
params.gamma = 0.15;
|
||
|
|
controller_mpc_cbf_3 = MPC_CBF(x0, system, params);
|
||
|
|
controller_mpc_cbf_3.obs = obs;
|
||
|
|
controller_mpc_cbf_3.sim(time_total);
|
||
|
|
|
||
|
|
%% Simulate MPC-DC
|
||
|
|
|
||
|
|
% The problem is infeasible at N=5
|
||
|
|
% params.N = 5;
|
||
|
|
% controller_mpc_dc_0 = MPCDC(x0, system, params);
|
||
|
|
% controller_mpc_dc_0.obs = obs;
|
||
|
|
% controller_mpc_dc_0.sim(time_total);
|
||
|
|
|
||
|
|
params.N = 7;
|
||
|
|
controller_mpc_dc_1 = MPCDC(x0, system, params);
|
||
|
|
controller_mpc_dc_1.obs = obs;
|
||
|
|
controller_mpc_dc_1.sim(time_total);
|
||
|
|
|
||
|
|
params.N = 15;
|
||
|
|
controller_mpc_dc_2 = MPCDC(x0, system, params);
|
||
|
|
controller_mpc_dc_2.obs = obs;
|
||
|
|
controller_mpc_dc_2.sim(time_total);
|
||
|
|
|
||
|
|
%%
|
||
|
|
params.N = 30;
|
||
|
|
controller_mpc_dc_3 = MPCDC(x0, system, params);
|
||
|
|
controller_mpc_dc_3.obs = obs;
|
||
|
|
controller_mpc_dc_3.sim(time_total);
|
||
|
|
|
||
|
|
%% Visualization benchmark
|
||
|
|
figure('Renderer', 'painters', 'Position', [0 0 400 400]);
|
||
|
|
set(gca,'LooseInset',get(gca,'TightInset'));
|
||
|
|
hold on
|
||
|
|
plot(controller_mpc_cbf_3.xlog(1,:), controller_mpc_cbf_3.xlog(2,:),...
|
||
|
|
'-', 'Color', [0, 0.4470, 0.7410], 'LineWidth', 1.0, 'MarkerSize', 4);
|
||
|
|
plot(controller_mpc_cbf_2.xlog(1,:), controller_mpc_cbf_2.xlog(2,:),...
|
||
|
|
'-', 'Color', [0.8500, 0.3250, 0.0980], 'LineWidth', 1.0, 'MarkerSize', 4);
|
||
|
|
plot(controller_mpc_cbf_1.xlog(1,:), controller_mpc_cbf_1.xlog(2,:),...
|
||
|
|
'-', 'Color', [0.9290, 0.6940, 0.1250], 'LineWidth', 1.0, 'MarkerSize', 4);
|
||
|
|
plot(controller_mpc_dc_1.xlog(1,:), controller_mpc_dc_1.xlog(2,:),...
|
||
|
|
'--', 'Color', [0.4940, 0.1840, 0.5560], 'LineWidth', 1.0, 'MarkerSize', 4);
|
||
|
|
plot(controller_mpc_dc_2.xlog(1,:), controller_mpc_dc_2.xlog(2,:),...
|
||
|
|
'--', 'Color', [0.4660, 0.6740, 0.1880], 'LineWidth', 1.0, 'MarkerSize', 4);
|
||
|
|
plot(controller_mpc_dc_3.xlog(1,:), controller_mpc_dc_3.xlog(2,:),...
|
||
|
|
'--', 'Color', [0.3010, 0.7450, 0.9330], 'LineWidth', 1.0, 'MarkerSize', 4);
|
||
|
|
% plot obstacle
|
||
|
|
pos = obs.pos;
|
||
|
|
r = obs.r;
|
||
|
|
th = linspace(0,2*pi*100);
|
||
|
|
x = cos(th) ; y = sin(th) ;
|
||
|
|
plot(pos(1) + r*x, pos(2) + r*y, 'Color', [0.8500, 0.3250, 0.0980],...
|
||
|
|
'LineWidth', 2);
|
||
|
|
plot(pos(1), pos(2), 'Color', [0.8500, 0.3250, 0.0980],...
|
||
|
|
'MarkerSize', 5, 'LineWidth', 2);
|
||
|
|
% plot target position
|
||
|
|
plot(x0(1), x0(2), 'db', 'LineWidth', 1);
|
||
|
|
plot(0.0, 0.0, 'dr', 'LineWidth', 1);
|
||
|
|
h=get(gca,'Children');
|
||
|
|
h_legend = legend(h([end, end-1, end-2, end-3, end-4, end-5]),...
|
||
|
|
{'MPC-CBF ($N = 5, \gamma = 0.15$)', 'MPC-CBF ($N = 5, \gamma = 0.20$)',...
|
||
|
|
'MPC-CBF ($N = 5, \gamma = 0.25$)', 'MPC-DC ($N = 7$)', 'MPC-DC ($N = 15$)',...
|
||
|
|
'MPC-DC ($N = 30$)'}, 'Location', 'SouthEast');
|
||
|
|
set(h_legend, 'Interpreter','latex', 'FontSize', 10);
|
||
|
|
set(gca,'LineWidth', 0.2, 'FontSize', 15);
|
||
|
|
grid on
|
||
|
|
xlabel('$x (m)$','interpreter','latex','FontSize',20);
|
||
|
|
ylabel('$y (m)$','interpreter','latex','FontSize',20);
|
||
|
|
xticks(-5:0);
|
||
|
|
yticks(-5:0);
|
||
|
|
xlim([-5,0.2]);
|
||
|
|
ylim([-5,0.2]);
|
||
|
|
print(gcf,'figures/benchmark-horizon.eps', '-depsc');
|
||
|
|
print(gcf,'figures/benchmark-horizon.png', '-dpng', '-r800');
|
||
|
|
|
||
|
|
%% Computational time benchmark
|
||
|
|
figure('Renderer', 'painters', 'Position', [0 0 800 400]);
|
||
|
|
xlabel('Computational time (s)', 'interpreter', 'latex', 'FontSize', 16);
|
||
|
|
ylabel('Percentage', 'interpreter', 'latex', 'FontSize', 16);
|
||
|
|
hold on
|
||
|
|
% [N_mpc_3, edges_mpc_3] = histcounts(controller_mpc_cbf_3.solvertime, 10, 'Normalization', 'probability');
|
||
|
|
% plot(edges_mpc_3(1:end-1), N_mpc_3, '-', 'Color', [0, 0.4470, 0.7410], 'LineWidth', 1);
|
||
|
|
% [N_mpc_2, edges_mpc_2] = histcounts(controller_mpc_cbf_2.solvertime, 10, 'Normalization', 'probability');
|
||
|
|
% plot(edges_mpc_2(1:end-1), N_mpc_2, '-', 'Color', [0.8500, 0.3250, 0.0980], 'LineWidth', 1);
|
||
|
|
[N_mpc_1, edges_mpc_1] = histcounts(controller_mpc_cbf_1.solvertime, 10, 'Normalization', 'probability');
|
||
|
|
plot(edges_mpc_1(1:end-1), N_mpc_1, '-', 'Color', [0.9290, 0.6940, 0.1250], 'LineWidth', 1);
|
||
|
|
[N_dc_1, edges_dc_1] = histcounts(controller_mpc_dc_1.solvertime, 10, 'Normalization', 'probability');
|
||
|
|
plot(edges_dc_1(1:end-1), N_dc_1, '--', 'Color', [0.4940, 0.1840, 0.5560], 'LineWidth', 1);
|
||
|
|
[N_dc_2, edges_dc_2] = histcounts(controller_mpc_dc_2.solvertime, 10, 'Normalization', 'probability');
|
||
|
|
plot(edges_dc_2(1:end-1), N_dc_2, '--', 'Color', [0.4660, 0.6740, 0.1880], 'LineWidth', 1);
|
||
|
|
[N_dc_3, edges_dc_3] = histcounts(controller_mpc_dc_3.solvertime, 10, 'Normalization', 'probability');
|
||
|
|
plot(edges_dc_3(1:end-1), N_dc_3, '--', 'Color', [0.3010, 0.7450, 0.9330], 'LineWidth', 1);
|
||
|
|
h=get(gca,'Children');
|
||
|
|
h_legend = legend(h([end, end-1, end-2, end-3]),...
|
||
|
|
{'MPC-CBF ($N = 5, \gamma = 0.25$)',...
|
||
|
|
'MPC-DC ($N = 7$)',...
|
||
|
|
'MPC-DC ($N = 15$)',...
|
||
|
|
'MPC-DC ($N = 30$)'}, 'Location', 'NorthEast');
|
||
|
|
set(h_legend, 'Interpreter','latex');
|
||
|
|
set(gca,'LineWidth', 0.2, 'FontSize', 15);
|
||
|
|
print(gcf,'figures/benchmark-horizon-computational-time.eps', '-depsc');
|
||
|
|
print(gcf,'figures/benchmark-horizon-computational-time.png', '-dpng', '-r800');
|
||
|
|
%% Computational time table
|
||
|
|
fprintf('Computational time for MPC-CBF3: mean %.3f, std %.3f, min %.3f, max %.3f, input cost %.3f, min dist %f\n',...
|
||
|
|
[mean(controller_mpc_cbf_3.solvertime),...
|
||
|
|
std(controller_mpc_cbf_3.solvertime),...
|
||
|
|
min(controller_mpc_cbf_3.solvertime),...
|
||
|
|
max(controller_mpc_cbf_3.solvertime),...
|
||
|
|
controller_mpc_cbf_3.u_cost,...
|
||
|
|
min(controller_mpc_cbf_3.distlog)]);
|
||
|
|
|
||
|
|
fprintf('Computational time for MPC-CBF2: mean %.3f, std %.3f, min %.3f, max %.3f, input cost %.3f, min dist %f\n',...
|
||
|
|
[mean(controller_mpc_cbf_2.solvertime),...
|
||
|
|
std(controller_mpc_cbf_2.solvertime),...
|
||
|
|
min(controller_mpc_cbf_2.solvertime),...
|
||
|
|
max(controller_mpc_cbf_2.solvertime),...
|
||
|
|
controller_mpc_cbf_2.u_cost,...
|
||
|
|
min(controller_mpc_cbf_2.distlog)]);
|
||
|
|
|
||
|
|
fprintf('Computational time for MPC-CBF1: mean %.3f, std %.3f, min %.3f, max %.3f, input cost %.3f, min dist %f\n',...
|
||
|
|
[mean(controller_mpc_cbf_1.solvertime),...
|
||
|
|
std(controller_mpc_cbf_1.solvertime),...
|
||
|
|
min(controller_mpc_cbf_1.solvertime),...
|
||
|
|
max(controller_mpc_cbf_1.solvertime),...
|
||
|
|
controller_mpc_cbf_1.u_cost,...
|
||
|
|
min(controller_mpc_cbf_1.distlog)]);
|
||
|
|
|
||
|
|
% fprintf('Computational time for MPC-DC0: mean %.3f, std %.3f, min %.3f, max %.3f, input cost %.3f, min dist %f\n',...
|
||
|
|
% [mean(controller_mpc_dc_0.solvertime),...
|
||
|
|
% std(controller_mpc_dc_0.solvertime),...
|
||
|
|
% min(controller_mpc_dc_0.solvertime),...
|
||
|
|
% max(controller_mpc_dc_0.solvertime),...
|
||
|
|
% controller_mpc_dc_0.u_cost,...
|
||
|
|
% min(controller_mpc_dc_0.distlog)]);
|
||
|
|
|
||
|
|
fprintf('Computational time for MPC-DC1: mean %.3f, std %.3f, min %.3f, max %.3f, input cost %.3f, min dist %f\n',...
|
||
|
|
[mean(controller_mpc_dc_1.solvertime),...
|
||
|
|
std(controller_mpc_dc_1.solvertime),...
|
||
|
|
min(controller_mpc_dc_1.solvertime),...
|
||
|
|
max(controller_mpc_dc_1.solvertime),...
|
||
|
|
controller_mpc_dc_1.u_cost,...
|
||
|
|
min(controller_mpc_dc_1.distlog)]);
|
||
|
|
|
||
|
|
fprintf('Computational time for MPC-DC2: mean %.3f, std %.3f, min %.3f, max %.3f, input cost %.3f, min dist %f\n',...
|
||
|
|
[mean(controller_mpc_dc_2.solvertime),...
|
||
|
|
std(controller_mpc_dc_2.solvertime),...
|
||
|
|
min(controller_mpc_dc_2.solvertime),...
|
||
|
|
max(controller_mpc_dc_2.solvertime),...
|
||
|
|
controller_mpc_dc_2.u_cost,...
|
||
|
|
min(controller_mpc_dc_2.distlog)]);
|
||
|
|
|
||
|
|
fprintf('Computational time for MPC-DC3: mean %.3f, std %.3f, min %.3f, max %.3f, input cost %.3f, min dist %f\n',...
|
||
|
|
[mean(controller_mpc_dc_3.solvertime),...
|
||
|
|
std(controller_mpc_dc_3.solvertime),...
|
||
|
|
min(controller_mpc_dc_3.solvertime),...
|
||
|
|
max(controller_mpc_dc_3.solvertime),...
|
||
|
|
controller_mpc_dc_3.u_cost,...
|
||
|
|
min(controller_mpc_dc_3.distlog)]);
|