Initial commit
Jun Zeng
commit
fc9514494f
|
|
@ -0,0 +1,163 @@
|
|||
# MATLAB
|
||||
# Windows default autosave extension
|
||||
*.asv
|
||||
|
||||
# OSX / *nix default autosave extension
|
||||
*.m~
|
||||
|
||||
# Compiled MEX binaries (all platforms)
|
||||
*.mex*
|
||||
|
||||
# Packaged app and toolbox files
|
||||
*.mlappinstall
|
||||
*.mltbx
|
||||
|
||||
# Generated helpsearch folders
|
||||
helpsearch*/
|
||||
|
||||
# Simulink code generation folders
|
||||
slprj/
|
||||
sccprj/
|
||||
|
||||
# Matlab code generation folders
|
||||
codegen/
|
||||
|
||||
# Simulink autosave extension
|
||||
*.autosave
|
||||
|
||||
# Simulink cache files
|
||||
*.slxc
|
||||
|
||||
# Octave session info
|
||||
octave-workspace
|
||||
|
||||
# PYTHON
|
||||
# Byte-compiled / optimized / DLL files
|
||||
__pycache__/
|
||||
*.py[cod]
|
||||
*$py.class
|
||||
|
||||
# C extensions
|
||||
*.so
|
||||
|
||||
# Distribution / packaging
|
||||
.Python
|
||||
build/
|
||||
develop-eggs/
|
||||
dist/
|
||||
downloads/
|
||||
eggs/
|
||||
.eggs/
|
||||
lib/
|
||||
lib64/
|
||||
parts/
|
||||
sdist/
|
||||
var/
|
||||
wheels/
|
||||
pip-wheel-metadata/
|
||||
share/python-wheels/
|
||||
*.egg-info/
|
||||
.installed.cfg
|
||||
*.egg
|
||||
MANIFEST
|
||||
|
||||
# PyInstaller
|
||||
# Usually these files are written by a python script from a template
|
||||
# before PyInstaller builds the exe, so as to inject date/other infos into it.
|
||||
*.manifest
|
||||
*.spec
|
||||
|
||||
# Installer logs
|
||||
pip-log.txt
|
||||
pip-delete-this-directory.txt
|
||||
|
||||
# Unit test / coverage reports
|
||||
htmlcov/
|
||||
.tox/
|
||||
.nox/
|
||||
.coverage
|
||||
.coverage.*
|
||||
.cache
|
||||
nosetests.xml
|
||||
coverage.xml
|
||||
*.cover
|
||||
*.py,cover
|
||||
.hypothesis/
|
||||
.pytest_cache/
|
||||
|
||||
# Translations
|
||||
*.mo
|
||||
*.pot
|
||||
|
||||
# Django stuff:
|
||||
*.log
|
||||
local_settings.py
|
||||
db.sqlite3
|
||||
db.sqlite3-journal
|
||||
|
||||
# Flask stuff:
|
||||
instance/
|
||||
.webassets-cache
|
||||
|
||||
# Scrapy stuff:
|
||||
.scrapy
|
||||
|
||||
# Sphinx documentation
|
||||
docs/_build/
|
||||
|
||||
# PyBuilder
|
||||
target/
|
||||
|
||||
# Jupyter Notebook
|
||||
.ipynb_checkpoints
|
||||
|
||||
# IPython
|
||||
profile_default/
|
||||
ipython_config.py
|
||||
|
||||
# pyenv
|
||||
.python-version
|
||||
|
||||
# pipenv
|
||||
# According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control.
|
||||
# However, in case of collaboration, if having platform-specific dependencies or dependencies
|
||||
# having no cross-platform support, pipenv may install dependencies that don't work, or not
|
||||
# install all needed dependencies.
|
||||
#Pipfile.lock
|
||||
|
||||
# PEP 582; used by e.g. github.com/David-OConnor/pyflow
|
||||
__pypackages__/
|
||||
|
||||
# Celery stuff
|
||||
celerybeat-schedule
|
||||
celerybeat.pid
|
||||
|
||||
# SageMath parsed files
|
||||
*.sage.py
|
||||
|
||||
# Environments
|
||||
.env
|
||||
.venv
|
||||
env/
|
||||
venv/
|
||||
ENV/
|
||||
env.bak/
|
||||
venv.bak/
|
||||
|
||||
# Spyder project settings
|
||||
.spyderproject
|
||||
.spyproject
|
||||
|
||||
# Rope project settings
|
||||
.ropeproject
|
||||
|
||||
# mkdocs documentation
|
||||
/site
|
||||
|
||||
# mypy
|
||||
.mypy_cache/
|
||||
.dmypy.json
|
||||
dmypy.json
|
||||
|
||||
# Pyre type checker
|
||||
.pyre/
|
||||
|
|
@ -0,0 +1,21 @@
|
|||
MIT License
|
||||
|
||||
Copyright (c) 2020 Jun Zeng
|
||||
|
||||
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
of this software and associated documentation files (the "Software"), to deal
|
||||
in the Software without restriction, including without limitation the rights
|
||||
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
copies of the Software, and to permit persons to whom the Software is
|
||||
furnished to do so, subject to the following conditions:
|
||||
|
||||
The above copyright notice and this permission notice shall be included in all
|
||||
copies or substantial portions of the Software.
|
||||
|
||||
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
SOFTWARE.
|
||||
|
|
@ -0,0 +1,48 @@
|
|||
# MPC-CBF
|
||||
Model Predictive Control with Control Barrier Functions
|
||||
|
||||
|
||||
|
||||
# Prerequisites
|
||||
## Matlab
|
||||
The packages needed for running the code are [Yalmip](https://yalmip.github.io/) and [IPOPT](https://projects.coin-or.org/Ipopt/wiki/MatlabInterface).
|
||||
|
||||
We also provide the zipped version of precompiled .mex files for IPOPT in the folder `packages` in case you don't have it. Unzip that file and add those .mex files into your MATLAB path.
|
||||
|
||||
# Description
|
||||
## 2D double integrator
|
||||
The 2D double integrator is assigned to reach the target position at origin while avoiding obstacles. We have three classes for different controllers: `DCLF_DCBF.m` (DCLF-DCBF), `MPC_CBF.m` (MPC-CBF) and `MPC_DC` (MPC-DC), respectively.
|
||||
|
||||
Moreover, to illustrate the performance among them, we have:
|
||||
* `main.m`: Run DCLF-DCBF/MPC-CBF/MPC-DC respectively.
|
||||
* `analysis_gamma.m`: Run analysis for different hyperparameter $\gamma$.
|
||||
* `analysis_horizon.m`: Run analysis for different horizon.
|
||||
|
||||
We illustrate the performance between DCLF-DCBF/MPC-DC/MPC-CBF
|
||||
| DCLF-DCBF | MPC-DC (N=8) |
|
||||
| --- | --- |
|
||||
| <img src="double-integrator-2D/figures/dclf-dcbf-avoidance.png" width="200" height="200"> | <img src="double-integrator-2D/figures/mpc-dc-avoidance.png" width="200" height="200"> |
|
||||
|
||||
| MPC-CBF (N=1) | MPC-CBF (N=8) |
|
||||
| --- | --- |
|
||||
| <img src="double-integrator-2D/figures/mpc-cbf-avoidance-one-step.png" width="200" height="200"> | <img src="double-integrator-2D/figures/mpc-cbf-avoidance-several-steps.png" width="200" height="200"> |
|
||||
|
||||
and also the safety performance for different numbers of horizon and hyperparameters
|
||||
| Different hyperparameter | Different horizon |
|
||||
| --- | --- |
|
||||
| <img src="double-integrator-2D/figures/benchmark-gamma.png" width="200" height="200"> | <img src="double-integrator-2D/figures/benchmark-horizon.png" width="200" height="200"> |
|
||||
|
||||
## Car racing competition
|
||||
We have the speed profile and control input shown as follow,
|
||||
|
||||
<img src="car-racing/lmpc-speed-norm-profile.png" width="400">
|
||||
|
||||
<img src="car-racing/lmpc-deviation-profile.png" width="400">
|
||||
|
||||
<img src="car-racing/lmpc-input-profile.png" width="400">
|
||||
|
||||
The animation can be found on the top of this readme, we will release full code after the paper is accepted.
|
||||
|
||||
# References
|
||||
This repository is based on the following:
|
||||
* Jun Zeng, Bike Zhang and Koushil Sreenath. "Safety-Critical Model Predictive Control with Discrete-Time Control Barrier Function." [[PDF]](http://arxiv.org/abs/2007.11718)
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 212 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 795 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.3 MiB |
Binary file not shown.
|
After Width: | Height: | Size: 418 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.2 MiB |
|
|
@ -0,0 +1,105 @@
|
|||
classdef DCLF_DCBF < handle
|
||||
properties
|
||||
system
|
||||
params
|
||||
x0
|
||||
x_curr
|
||||
time_curr = 0.0
|
||||
xlog = []
|
||||
ulog = []
|
||||
u_cost = 0
|
||||
obs
|
||||
end
|
||||
|
||||
methods
|
||||
function self = DCLF_DCBF(x0, system, params)
|
||||
% Define DCLF-DCBF controller
|
||||
self.x0 = x0;
|
||||
self.x_curr = x0;
|
||||
self.system = system;
|
||||
self.params = params;
|
||||
end
|
||||
|
||||
function sim(self, time)
|
||||
% Simulate the system until a given time
|
||||
xk = self.x_curr;
|
||||
while self.time_curr <= time
|
||||
% Solve DCLF-DCBF
|
||||
uk = self.solve_dclf_dcbf();
|
||||
xk = self.system.A * xk + self.system.B * uk;
|
||||
% update system
|
||||
self.x_curr = xk;
|
||||
self.time_curr = self.time_curr + self.system.dt;
|
||||
self.xlog = [self.xlog, xk];
|
||||
self.ulog = [self.ulog, uk];
|
||||
self.u_cost = self.u_cost + uk'*uk*self.system.dt;
|
||||
end
|
||||
end
|
||||
|
||||
function uopt = solve_dclf_dcbf(self)
|
||||
% Solve DCLF-DCBF
|
||||
x = self.x_curr;
|
||||
u = sdpvar(2,1);
|
||||
delta = sdpvar(1,1);
|
||||
constraints = [];
|
||||
% add DCLF constraints
|
||||
x_next = self.system.A * x + self.system.B * u;
|
||||
v = x' * self.params.P * x;
|
||||
v_next = x_next' * self.params.P * x_next;
|
||||
constraints = [constraints; v_next - v + self.params.alpha * v <= delta];
|
||||
% add DCBF constraints
|
||||
pos = self.obs.pos;
|
||||
r = self.obs.r;
|
||||
b = (x(1:2)-pos)'*(x(1:2)-pos) - r^2;
|
||||
b_next = (x_next(1:2)-pos)'*(x_next(1:2)-pos) - r^2;
|
||||
constraints = [constraints; b_next - b + self.params.gamma*b >= 0];
|
||||
% input constraints
|
||||
constraints = [constraints; self.system.ul <= u <= self.system.uu];
|
||||
% cost
|
||||
cost = u'*self.params.H*u + delta'*self.params.p*delta;
|
||||
% solve optimization
|
||||
ops = sdpsettings('solver','ipopt','verbose',0);
|
||||
diagnostics = optimize(constraints, cost, ops);
|
||||
if diagnostics.problem == 0
|
||||
feas = true;
|
||||
uopt = value(u);
|
||||
else
|
||||
feas = false;
|
||||
uopt = [];
|
||||
end
|
||||
fprintf('solver time: %f\n', diagnostics.solvertime);
|
||||
end
|
||||
|
||||
function plot(self,figure_name)
|
||||
% Plot simulation
|
||||
figure('Renderer', 'painters', 'Position', [0 0 400 400]);
|
||||
set(gca,'LooseInset',get(gca,'TightInset'));
|
||||
hold on;
|
||||
% plot trajectory
|
||||
plot(self.xlog(1,:), self.xlog(2,:), 'ko-',...
|
||||
'LineWidth', 1.0,'MarkerSize',4);
|
||||
% plot obstacle
|
||||
pos = self.obs.pos;
|
||||
r = self.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(self.x0(1), self.x0(2), 'db', 'LineWidth', 1);
|
||||
plot(0.0, 0.0, 'dr', 'LineWidth', 1);
|
||||
h=get(gca,'Children');
|
||||
h_legend = legend(h([end]), {'DCLF-DCBF'}, 'Location', 'SouthEast');
|
||||
set(h_legend, 'Interpreter','latex');
|
||||
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,strcat('figures/',figure_name), '-depsc');
|
||||
end
|
||||
end
|
||||
end
|
||||
|
|
@ -0,0 +1,145 @@
|
|||
classdef MPC_CBF < handle
|
||||
% MPC with distance constraints
|
||||
properties
|
||||
system
|
||||
params
|
||||
x0
|
||||
x_curr
|
||||
time_curr = 0.0
|
||||
xlog = []
|
||||
ulog = []
|
||||
distlog = []
|
||||
solvertime = []
|
||||
xopenloop = {}
|
||||
uopenloop = {}
|
||||
u_cost = 0
|
||||
obs
|
||||
end
|
||||
methods
|
||||
function self = MPC_CBF(x0, system, params)
|
||||
% Define MPC_CBF controller
|
||||
self.x0 = x0;
|
||||
self.x_curr = x0;
|
||||
self.system = system;
|
||||
self.params = params;
|
||||
end
|
||||
|
||||
function sim(self, time)
|
||||
% Simulate the system until a given time
|
||||
xk = self.x_curr;
|
||||
while self.time_curr <= time
|
||||
% Solve CFTOC
|
||||
[~, uk] = self.solve_mpc_dc(self.x_curr);
|
||||
xk = self.system.A * xk + self.system.B * uk;
|
||||
% update system
|
||||
self.x_curr = xk;
|
||||
self.time_curr = self.time_curr + self.system.dt;
|
||||
self.xlog = [self.xlog, xk];
|
||||
self.ulog = [self.ulog, uk];
|
||||
self.u_cost = self.u_cost + uk'*uk*self.system.dt;
|
||||
end
|
||||
end
|
||||
|
||||
function [xopt, uopt] = solve_mpc_dc(self, xk)
|
||||
% Solve MPC-CBF
|
||||
[feas, x, u, J] = self.solve_cftoc(xk);
|
||||
if ~feas
|
||||
xopt = [];
|
||||
uopt = [];
|
||||
return
|
||||
else
|
||||
xopt = x(:,2);
|
||||
uopt = u(:,1);
|
||||
end
|
||||
end
|
||||
|
||||
function [feas, xopt, uopt, Jopt] = solve_cftoc(self, xk)
|
||||
% Solve CFTOC
|
||||
% extract variables
|
||||
N = self.params.N;
|
||||
% define variables and cost
|
||||
x = sdpvar(4, N+1);
|
||||
u = sdpvar(2, N);
|
||||
constraints = [];
|
||||
cost = 0;
|
||||
% initial constraint
|
||||
constraints = [constraints; x(:,1) == xk];
|
||||
% add constraints and costs
|
||||
for i = 1:N
|
||||
constraints = [constraints;
|
||||
self.system.xl <= x(:,i) <= self.system.xu;
|
||||
self.system.ul <= u(:,i) <= self.system.uu
|
||||
x(:,i+1) == self.system.A * x(:,i) + self.system.B * u(:,i)];
|
||||
cost = cost + x(:,i)'*self.params.Q*x(:,i) + u(:,i)'*self.params.R*u(:,i);
|
||||
end
|
||||
% add CBF constraints
|
||||
for i = 1:N
|
||||
pos = self.obs.pos;
|
||||
r = self.obs.r ;
|
||||
b = (x([1:2],i)-pos)'*((x([1:2],i)-pos)) - r^2;
|
||||
b_next = (x([1:2],i+1)-pos)'*((x([1:2],i+1)-pos)) - r^2;
|
||||
constraints = [constraints; b_next - b + self.params.gamma * b >= 0];
|
||||
end
|
||||
% add terminal cost
|
||||
cost = cost + x(:,N+1)'*self.params.P*x(:,N+1);
|
||||
ops = sdpsettings('solver','ipopt','verbose',0);
|
||||
% solve optimization
|
||||
diagnostics = optimize(constraints, cost, ops);
|
||||
if diagnostics.problem == 0
|
||||
feas = true;
|
||||
xopt = value(x);
|
||||
uopt = value(u);
|
||||
Jopt = value(cost);
|
||||
else
|
||||
feas = false;
|
||||
xopt = [];
|
||||
uopt = [];
|
||||
Jopt = [];
|
||||
end
|
||||
self.distlog = [self.distlog, sqrt((xk(1:2)-pos)'*(xk(1:2)-pos)-r^2)];
|
||||
self.xopenloop{size(self.xopenloop,2)+1} = xopt;
|
||||
self.uopenloop{size(self.uopenloop,2)+1} = uopt;
|
||||
self.solvertime = [self.solvertime, diagnostics.solvertime];
|
||||
fprintf('solver time: %f\n', diagnostics.solvertime);
|
||||
end
|
||||
|
||||
function plot(self, figure_name)
|
||||
% Plot simulation
|
||||
figure('Renderer', 'painters', 'Position', [0 0 400 400]);
|
||||
set(gca,'LooseInset',get(gca,'TightInset'));
|
||||
hold on;
|
||||
% plot closed-loop trajectory
|
||||
plot(self.xlog(1,:), self.xlog(2,:), 'ko-',...
|
||||
'LineWidth', 1.0, 'MarkerSize', 4);
|
||||
% plot open-loop trajectory
|
||||
for i = 1:size(self.xopenloop, 2)
|
||||
plot(self.xopenloop{i}(1,:), self.xopenloop{i}(2,:),...
|
||||
'k*-.', 'LineWidth', 0.5,'MarkerSize',0.5)
|
||||
end
|
||||
% plot obstacle
|
||||
pos = self.obs.pos;
|
||||
r = self.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(self.x0(1), self.x0(2), 'db', 'LineWidth', 1);
|
||||
plot(0.0, 0.0, 'dr', 'LineWidth', 1);
|
||||
h=get(gca,'Children');
|
||||
h_legend = legend(h([end]), {'MPC-CBF'}, 'Location', 'SouthEast');
|
||||
set(h_legend, 'Interpreter','latex');
|
||||
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,strcat('figures/',figure_name), '-depsc');
|
||||
end
|
||||
end
|
||||
end
|
||||
|
|
@ -0,0 +1,144 @@
|
|||
classdef MPC_DC < handle
|
||||
% MPC with distance constraints
|
||||
properties
|
||||
system
|
||||
params
|
||||
x0
|
||||
x_curr
|
||||
time_curr = 0.0
|
||||
xlog = []
|
||||
ulog = []
|
||||
distlog = []
|
||||
solvertime = []
|
||||
xopenloop = {}
|
||||
uopenloop = {}
|
||||
u_cost = 0
|
||||
obs
|
||||
end
|
||||
methods
|
||||
function self = MPC_DC(x0, system, params)
|
||||
% Define MPC_DC controller
|
||||
self.x0 = x0;
|
||||
self.x_curr = x0;
|
||||
self.system = system;
|
||||
self.params = params;
|
||||
end
|
||||
|
||||
function sim(self, time)
|
||||
% Simulate the system until a given time
|
||||
xk = self.x_curr;
|
||||
while self.time_curr <= time
|
||||
% Solve CFTOC
|
||||
[~, uk] = self.solve_mpc_dc(self.x_curr);
|
||||
xk = self.system.A * xk + self.system.B * uk;
|
||||
% update system
|
||||
self.x_curr = xk;
|
||||
self.time_curr = self.time_curr + self.system.dt;
|
||||
self.xlog = [self.xlog, xk];
|
||||
self.ulog = [self.ulog, uk];
|
||||
self.u_cost = self.u_cost + uk'*uk*self.system.dt;
|
||||
end
|
||||
end
|
||||
|
||||
function [xopt, uopt] = solve_mpc_dc(self, xk)
|
||||
% Solve MPC-DC
|
||||
[feas, x, u, J] = self.solve_cftoc(xk);
|
||||
if ~feas
|
||||
xopt = [];
|
||||
uopt = [];
|
||||
return
|
||||
else
|
||||
xopt = x(:,2);
|
||||
uopt = u(:,1);
|
||||
end
|
||||
end
|
||||
|
||||
function [feas, xopt, uopt, Jopt] = solve_cftoc(self, xk)
|
||||
% Solve CFTOC
|
||||
% extract variables
|
||||
N = self.params.N;
|
||||
% define variables and cost
|
||||
x = sdpvar(4, N+1);
|
||||
u = sdpvar(2, N);
|
||||
constraints = [];
|
||||
cost = 0;
|
||||
% initial constraint
|
||||
constraints = [constraints; x(:,1) == xk];
|
||||
% add constraints and costs
|
||||
for i = 1:N
|
||||
constraints = [constraints;
|
||||
self.system.xl <= x(:,i) <= self.system.xu;
|
||||
self.system.ul <= u(:,i) <= self.system.uu
|
||||
x(:,i+1) == self.system.A * x(:,i) + self.system.B * u(:,i)];
|
||||
cost = cost + x(:,i)'*self.params.Q*x(:,i) + u(:,i)'*self.params.R*u(:,i);
|
||||
end
|
||||
% add CBF constraints
|
||||
for i = 1:N
|
||||
pos = self.obs.pos;
|
||||
r = self.obs.r ;
|
||||
b = (x([1:2],i)-pos)'*((x([1:2],i)-pos)) - r^2;
|
||||
constraints = [constraints; b >= 0];
|
||||
end
|
||||
% add terminal cost
|
||||
cost = cost + x(:,N+1)'*self.params.P*x(:,N+1);
|
||||
ops = sdpsettings('solver','ipopt','verbose',0);
|
||||
% solve optimization
|
||||
diagnostics = optimize(constraints, cost, ops);
|
||||
if diagnostics.problem == 0
|
||||
feas = true;
|
||||
xopt = value(x);
|
||||
uopt = value(u);
|
||||
Jopt = value(cost);
|
||||
else
|
||||
feas = false;
|
||||
xopt = [];
|
||||
uopt = [];
|
||||
Jopt = [];
|
||||
end
|
||||
self.distlog = [self.distlog, sqrt((xk(1:2)-pos)'*(xk(1:2)-pos)-r^2)];
|
||||
self.xopenloop{size(self.xopenloop,2)+1} = xopt;
|
||||
self.uopenloop{size(self.uopenloop,2)+1} = uopt;
|
||||
self.solvertime = [self.solvertime, diagnostics.solvertime];
|
||||
fprintf('solver time: %f\n', diagnostics.solvertime);
|
||||
end
|
||||
|
||||
function plot(self, figure_name)
|
||||
% Plot simulation
|
||||
figure('Renderer', 'painters', 'Position', [0 0 400 400]);
|
||||
set(gca,'LooseInset',get(gca,'TightInset'));
|
||||
hold on;
|
||||
% plot closed-loop trajectory
|
||||
plot(self.xlog(1,:), self.xlog(2,:), 'ko-',...
|
||||
'LineWidth', 1.0, 'MarkerSize', 4);
|
||||
% plot open-loop trajectory
|
||||
for i = 1:size(self.xopenloop, 2)
|
||||
plot(self.xopenloop{i}(1,:), self.xopenloop{i}(2,:),...
|
||||
'k*-.', 'LineWidth', 0.5,'MarkerSize',0.5)
|
||||
end
|
||||
% plot obstacle
|
||||
pos = self.obs.pos;
|
||||
r = self.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(self.x0(1), self.x0(2), 'db', 'LineWidth', 1);
|
||||
plot(0.0, 0.0, 'dr', 'LineWidth', 1);
|
||||
h=get(gca,'Children');
|
||||
h_legend = legend(h([end]), {'MPC-DC'}, 'Location', 'SouthEast');
|
||||
set(h_legend, 'Interpreter','latex');
|
||||
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,strcat('figures/',figure_name), '-depsc');
|
||||
end
|
||||
end
|
||||
end
|
||||
|
|
@ -0,0 +1,66 @@
|
|||
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;
|
||||
umin = [-1; -1];
|
||||
umax = [1; 1];
|
||||
xmin = [-5; -5; -5; -5];
|
||||
xmax = [5; 5; 5; 5];
|
||||
|
||||
%% 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
|
||||
gamma_list = linspace(0.1, 0.5, 5);
|
||||
controller_mpc_cbf_list = {};
|
||||
for i = 1:size(gamma_list, 2)
|
||||
new_params = params;
|
||||
new_params.N = 5;
|
||||
new_params.gamma = gamma_list(i);
|
||||
controller_mpc_cbf = MPC_CBF(x0, system, new_params);
|
||||
controller_mpc_cbf.obs = obs;
|
||||
controller_mpc_cbf.sim(time_total);
|
||||
controller_mpc_cbf_list{i} = controller_mpc_cbf;
|
||||
end
|
||||
|
||||
%% Computational time benchmark
|
||||
for i = 1:size(gamma_list, 2)
|
||||
controller_mpc_cbf = controller_mpc_cbf_list{i};
|
||||
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.solvertime),...
|
||||
std(controller_mpc_cbf.solvertime),...
|
||||
min(controller_mpc_cbf.solvertime),...
|
||||
max(controller_mpc_cbf.solvertime),...
|
||||
controller_mpc_cbf.u_cost,...
|
||||
min(controller_mpc_cbf.distlog)]);
|
||||
end
|
||||
|
|
@ -0,0 +1,95 @@
|
|||
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;
|
||||
umin = [-1; -1];
|
||||
umax = [1; 1];
|
||||
xmin = [-5; -5; -5; -5];
|
||||
xmax = [5; 5; 5; 5];
|
||||
|
||||
%% 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_mpc_cbf.Q = Q;
|
||||
params_mpc_cbf.R = R;
|
||||
params_mpc_cbf.P = P;
|
||||
params_mpc_cbf.N = N;
|
||||
params_mpc_cbf.gamma = 0.5;
|
||||
|
||||
%% Obstacle
|
||||
obs.pos = [-2; -2.25];
|
||||
obs.r = 1.5;
|
||||
|
||||
%% Simulate MPC-CBF
|
||||
gamma_list = [0.1, 0.2, 0.3, 1.0];
|
||||
for ind = 1:size(gamma_list, 2)
|
||||
fprintf('Run MPC-CBF with gamma %f\n', gamma_list(ind));
|
||||
params_mpc_cbf.gamma = gamma_list(ind);
|
||||
controller_mpc_cbf_list{ind} = MPC_CBF(x0, system, params_mpc_cbf);
|
||||
controller_mpc_cbf_list{ind}.obs = obs;
|
||||
controller_mpc_cbf_list{ind}.sim(time_total);
|
||||
end
|
||||
|
||||
%% Simulate MPC-DC
|
||||
params_mpc_dc = params_mpc_cbf;
|
||||
controller_mpc_dc = MPC_DC(x0, system, params_mpc_cbf);
|
||||
controller_mpc_dc.obs = obs;
|
||||
controller_mpc_dc.sim(time_total);
|
||||
|
||||
%% Visualization benchmark
|
||||
figure('Renderer', 'painters', 'Position', [0 0 400 400]);
|
||||
set(gca,'LooseInset',get(gca,'TightInset'));
|
||||
hold on
|
||||
for ind = 1:size(gamma_list, 2)
|
||||
plot(controller_mpc_cbf_list{ind}.xlog(1,:), controller_mpc_cbf_list{ind}.xlog(2,:),...
|
||||
'-', 'LineWidth', 1.0, 'MarkerSize', 4);
|
||||
end
|
||||
plot(controller_mpc_dc.xlog(1,:), controller_mpc_dc.xlog(2,:),...
|
||||
'k--', '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]),...
|
||||
{'MPC-CBF ($\gamma=0.1$)', 'MPC-CBF ($\gamma=0.2$)',...
|
||||
'MPC-CBF ($\gamma=0.3$)', 'MPC-CBF ($\gamma=1.0$)', 'MPC-DC'}, 'Location', 'SouthEast');
|
||||
set(h_legend, 'Interpreter','latex');
|
||||
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:1);
|
||||
yticks(-5:1);
|
||||
xlim([-5,0.2]);
|
||||
ylim([-5,0.2]);
|
||||
print(gcf,'figures/benchmark-gamma.eps', '-depsc');
|
||||
print(gcf,'figures/benchmark-gamma.png', '-dpng', '-r800');
|
||||
|
|
@ -0,0 +1,198 @@
|
|||
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;
|
||||
umin = [-1; -1];
|
||||
umax = [1; 1];
|
||||
xmin = [-5; -5; -5; -5];
|
||||
xmax = [5; 5; 5; 5];
|
||||
|
||||
%% 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 = MPC_CBF(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 = MPC_CBF(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
|
||||
params.N = 7;
|
||||
controller_mpc_dc_1 = MPC_DC(x0, system, params);
|
||||
controller_mpc_dc_1.obs = obs;
|
||||
controller_mpc_dc_1.sim(time_total);
|
||||
|
||||
%% Simulate MPC-DC with larger horizon
|
||||
params.N = 15;
|
||||
controller_mpc_dc_2 = MPC_DC(x0, system, params);
|
||||
controller_mpc_dc_2.obs = obs;
|
||||
controller_mpc_dc_2.sim(time_total);
|
||||
|
||||
%%
|
||||
params.N = 30;
|
||||
controller_mpc_dc_3 = MPC_DC(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-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)]);
|
||||
File diff suppressed because it is too large
Load Diff
Binary file not shown.
|
After Width: | Height: | Size: 440 KiB |
File diff suppressed because it is too large
Load Diff
Binary file not shown.
|
After Width: | Height: | Size: 559 KiB |
File diff suppressed because it is too large
Load Diff
Binary file not shown.
|
After Width: | Height: | Size: 472 KiB |
File diff suppressed because it is too large
Load Diff
Binary file not shown.
|
After Width: | Height: | Size: 289 KiB |
File diff suppressed because it is too large
Load Diff
Binary file not shown.
|
After Width: | Height: | Size: 294 KiB |
File diff suppressed because it is too large
Load Diff
Binary file not shown.
|
After Width: | Height: | Size: 288 KiB |
File diff suppressed because it is too large
Load Diff
Binary file not shown.
|
After Width: | Height: | Size: 281 KiB |
|
|
@ -0,0 +1,235 @@
|
|||
close all
|
||||
clear
|
||||
|
||||
%% General Flags
|
||||
run_dclf_dcbf = true;
|
||||
display_dclf_dcbf = true;
|
||||
run_mpc_cbf_one = true;
|
||||
display_mpc_cbf_one = true;
|
||||
run_mpc_cbf_multiple = true;
|
||||
run_mpc_cbf_multiple = true;
|
||||
run_mpc_dc = true;
|
||||
display_mpc_dc = true;
|
||||
|
||||
%% Setup and Parameters
|
||||
x0 = [-5; -5; 0; 0];
|
||||
time_total = 30.0;
|
||||
dt = 0.2;
|
||||
P = 100*eye(4);
|
||||
Q = 10*eye(4);
|
||||
R = eye(2);
|
||||
N = 8;
|
||||
umin = [-1; -1];
|
||||
umax = [1; 1];
|
||||
xmin = [-5; -5; -5; -5];
|
||||
xmax = [5; 5; 5; 5];
|
||||
|
||||
%% 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;
|
||||
|
||||
%% DCLF-DCBF parameters
|
||||
p = 1e3;
|
||||
params_dclf_dcbf.P = P;
|
||||
params_dclf_dcbf.H = R;
|
||||
params_dclf_dcbf.p = p;
|
||||
params_dclf_dcbf.alpha = 1.0;
|
||||
params_dclf_dcbf.gamma = 0.4;
|
||||
|
||||
%% MPC-CBF parameters
|
||||
params_mpc_cbf.Q = Q;
|
||||
params_mpc_cbf.R = R;
|
||||
params_mpc_cbf.P = P;
|
||||
params_mpc_cbf.N = N;
|
||||
params_mpc_cbf.gamma = 0.4;
|
||||
|
||||
%% MPC-DC parameters
|
||||
params_mpc_dc.Q = Q;
|
||||
params_mpc_dc.R = R;
|
||||
params_mpc_dc.P = P;
|
||||
params_mpc_dc.N = N;
|
||||
|
||||
%% Obstacle
|
||||
obs.pos = [-2; -2.25];
|
||||
obs.r = 1.5;
|
||||
|
||||
%% Simulate DCLF-DCBF
|
||||
if run_dclf_dcbf
|
||||
fprintf('Run DCLF-DCBF\n');
|
||||
controller_dclf_dcbf = DCLF_DCBF(x0, system, params_dclf_dcbf);
|
||||
controller_dclf_dcbf.obs = obs;
|
||||
controller_dclf_dcbf.sim(time_total);
|
||||
end
|
||||
|
||||
%% Display DCLF-DCBF simulation
|
||||
if display_dclf_dcbf
|
||||
% Plot simulation
|
||||
figure('Renderer', 'painters', 'Position', [0 0 400 400]);
|
||||
set(gca,'LooseInset',get(gca,'TightInset'));
|
||||
hold on;
|
||||
% plot trajectory
|
||||
plot(controller_dclf_dcbf.xlog(1,:), controller_dclf_dcbf.xlog(2,:), 'ko-',...
|
||||
'LineWidth', 1.0,'MarkerSize',4);
|
||||
% plot obstacle
|
||||
pos = controller_dclf_dcbf.obs.pos;
|
||||
r = controller_dclf_dcbf.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(controller_dclf_dcbf.x0(1), controller_dclf_dcbf.x0(2), 'db', 'LineWidth', 1);
|
||||
plot(0.0, 0.0, 'dr', 'LineWidth', 1);
|
||||
h=get(gca,'Children');
|
||||
h_legend = legend(h([end]), {'DCLF-DCBF'}, 'Location', 'SouthEast');
|
||||
set(h_legend, 'Interpreter','latex');
|
||||
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/dclf-dcbf-avoidance.eps', '-depsc');
|
||||
print(gcf,'figures/dclf-dcbf-avoidance.png', '-dpng', '-r800');
|
||||
end
|
||||
|
||||
%% Simulate MPC-CBF with N=1;
|
||||
params_mpc_cbf.N = 1;
|
||||
if run_mpc_cbf_one
|
||||
fprintf('Run MPC-CBF\n');
|
||||
controller_mpc_cbf_one = MPC_CBF(x0, system, params_mpc_cbf);
|
||||
controller_mpc_cbf_one.obs = obs;
|
||||
controller_mpc_cbf_one.sim(time_total);
|
||||
end
|
||||
|
||||
%% Display MPC-CBF simulation with N=1
|
||||
if display_mpc_cbf_one
|
||||
% Plot simulation
|
||||
figure('Renderer', 'painters', 'Position', [0 0 400 400]);
|
||||
set(gca,'LooseInset',get(gca,'TightInset'));
|
||||
hold on;
|
||||
% plot trajectory
|
||||
plot(controller_mpc_cbf_one.xlog(1,:), controller_mpc_cbf_one.xlog(2,:), 'ko-',...
|
||||
'LineWidth', 1.0,'MarkerSize',4);
|
||||
% plot obstacle
|
||||
pos = controller_mpc_cbf_one.obs.pos;
|
||||
r = controller_mpc_cbf_one.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(controller_mpc_cbf_one.x0(1), controller_mpc_cbf_one.x0(2), 'db', 'LineWidth', 1);
|
||||
plot(0.0, 0.0, 'dr', 'LineWidth', 1);
|
||||
h=get(gca,'Children');
|
||||
h_legend = legend(h([end]), {'MPC-CBF ($N=1$)'}, 'Location', 'SouthEast');
|
||||
set(h_legend, 'Interpreter','latex');
|
||||
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/mpc-cbf-avoidance-one-step.eps', '-depsc');
|
||||
print(gcf, 'figures/mpc-cbf-avoidance-one-step.png', '-dpng', '-r800');
|
||||
end
|
||||
|
||||
%% Simulate MPC-CBF with other N
|
||||
params_mpc_cbf.N = 8;
|
||||
if run_mpc_cbf_one
|
||||
fprintf('Run MPC-CBF\n');
|
||||
controller_mpc_cbf_multiple = MPC_CBF(x0, system, params_mpc_cbf);
|
||||
controller_mpc_cbf_multiple.obs = obs;
|
||||
controller_mpc_cbf_multiple.sim(time_total);
|
||||
end
|
||||
|
||||
%% Display MPC-CBF simulation with other N
|
||||
if display_mpc_cbf_one
|
||||
% Plot simulation
|
||||
figure('Renderer', 'painters', 'Position', [0 0 400 400]);
|
||||
set(gca,'LooseInset',get(gca,'TightInset'));
|
||||
hold on;
|
||||
% plot trajectory
|
||||
plot(controller_mpc_cbf_multiple.xlog(1,:), controller_mpc_cbf_multiple.xlog(2,:), 'ko-',...
|
||||
'LineWidth', 1.0,'MarkerSize',4);
|
||||
% plot obstacle
|
||||
pos = controller_mpc_cbf_multiple.obs.pos;
|
||||
r = controller_mpc_cbf_multiple.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(controller_mpc_cbf_multiple.x0(1), controller_mpc_cbf_multiple.x0(2), 'db', 'LineWidth', 1);
|
||||
plot(0.0, 0.0, 'dr', 'LineWidth', 1);
|
||||
h=get(gca,'Children');
|
||||
h_legend = legend(h([end]), {'MPC-CBF ($N=8$)'}, 'Location', 'SouthEast');
|
||||
set(h_legend, 'Interpreter','latex');
|
||||
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/mpc-cbf-avoidance-several-steps.eps', '-depsc');
|
||||
print(gcf, 'figures/mpc-cbf-avoidance-several-steps.png', '-dpng', '-r800');
|
||||
end
|
||||
|
||||
%% Simulate MPC-DC
|
||||
if run_mpc_dc
|
||||
fprintf('Run MPC-DC\n');
|
||||
controller_mpc_dc = MPC_DC(x0, system, params_mpc_dc);
|
||||
controller_mpc_dc.obs = obs;
|
||||
controller_mpc_dc.sim(time_total);
|
||||
end
|
||||
|
||||
%% Display MPC-DC simulation
|
||||
if display_mpc_dc
|
||||
% Plot simulation
|
||||
figure('Renderer', 'painters', 'Position', [0 0 400 400]);
|
||||
set(gca,'LooseInset',get(gca,'TightInset'));
|
||||
hold on;
|
||||
% plot trajectory
|
||||
plot(controller_mpc_dc.xlog(1,:), controller_mpc_dc.xlog(2,:), 'ko-',...
|
||||
'LineWidth', 1.0,'MarkerSize',4);
|
||||
% plot obstacle
|
||||
pos = controller_mpc_dc.obs.pos;
|
||||
r = controller_mpc_dc.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(controller_mpc_dc.x0(1), controller_mpc_dc.x0(2), 'db', 'LineWidth', 1);
|
||||
plot(0.0, 0.0, 'dr', 'LineWidth', 1);
|
||||
h=get(gca,'Children');
|
||||
h_legend = legend(h([end]), {'MPC-DC ($N=8$)'}, 'Location', 'SouthEast');
|
||||
set(h_legend, 'Interpreter','latex');
|
||||
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/mpc-dc-avoidance.eps', '-depsc');
|
||||
print(gcf, 'figures/mpc-dc-avoidance.png', '-dpng', '-r800');
|
||||
end
|
||||
Binary file not shown.
Loading…
Reference in New Issue