Hey!
I use acados in MATLAB for path planning as NMPC problem with collesion avoidance. I use circle to represent the obstacles. In addition, I add the obstacles as term in cost function as following in the code.
Where [x y] are the state vector and [x_obst y_obst] are the postion of the obstacle and r_x and r_y are the radius of the obstacle (r_x = r_y then the obstacle is circle). The goal when I add this term to the cost function is to be “as small as possible” and this can be acheived only when the difference between [x y] and [x_obst y_obst] is big in relative to the putted weights. The problem is when I choose the obstacle coordinates at position as (100,100) the solver dosn’t give any solution but when I give them the position (20,20) it gives me a solution. I the code I use 16 obstacle in the matrix obst_param and every obstacle from which is represented by (x_obs y_obst r_x r_y).
Here the dynamci model
function model = bicycle_model()
import casadi.*
model = struct();
L=0.65;
model.name = 'bicycle_model';
model.output_dir = './build';
model.new_solver = 1;
model.N = 15;
model.dt = 0.4;
model.T = 6;
model.nx = 4;
model.nu = 2;
%% symbolic variables
x_state = SX.sym('x_state', model.nx);
u_inputs = SX.sym('u_inputs', model.nu);
x_dot = SX.sym('x_dot', model.nx);
%% Dynamics
expr_f_expl = vertcat(cos(x_state(3))*x_state(4), ... % x1 dot
sin(x_state(3))*x_state(4), ... % x2 dot
tan(u_inputs(2))*x_state(4)/L, ... % x3 dot
u_inputs(1)); % x4 dot
model.x0 = zeros(model.nx, 1);
%% Collision avoidance nonlinear constraints
num_obst = 16;
obst = SX.sym('obst',num_obst);
obst_param = SX.sym('p',4 * num_obst);
sym_p = vertcat(obst_param);
for i = 1:num_obst
obst(i) = 1/exp(((x_state(1)-obst_param(i*4-3))/(obst_param(i*4-1)+0.5))^2 + ...
(((x_state(2)-obst_param(i*4-2))/(obst_param(i*4)+0.5))^2));
end
sum_obst = sum(obst);
%% cost - nonlinear least sqares
model.cost_expr_y = vertcat(x_state , u_inputs , sum_obst);
model.cost_expr_y_e = vertcat(x_state, sum_obst);
%% weights
W_x = diag([5e2 , 5e2 , 50 , 10]);
W_u = diag([0,1e-1]);
W_obst = 5e1*eye(1);
W = blkdiag(W_x, W_u , W_obst);
W_xe = diag([5e4 , 5e4 , 1e3 , 10]);
W_e = blkdiag(W_xe,W_obst);
model.W = W;
model.W_e = W_e;
%% Populate struct
model.x = x_state;
model.u = u_inputs;
model.xdot = x_dot;
model.dyn.expr_f = expr_f_expl;
model.sym_p = sym_p;
end
Here the solver creation
%% acados solver for NMPC problem with obstacle avoidance
close all
clear all
clc
%% ocp model,
model = bicycle_model();
ocp_model = acados_ocp_model();
ocp_model.set('name', model.name);
ocp_model.set('T', model.T);
%% symbolics
ocp_model.set('sym_x', model.x);
ocp_model.set('sym_u', model.u);
ocp_model.set('sym_xdot', model.xdot);
ocp_model.set('sym_p', model.sym_p);
%% dynamics
ocp_model.set('dyn_type', 'explicit');
ocp_model.set('dyn_expr_f', model.dyn.expr_f);
%% intial condition
ocp_model.set('constr_x0', model.x0);
%% constraints
Jbx = eye(model.nx);
ocp_model.set('constr_Jbx', Jbx);
ocp_model.set('constr_ubx', [10 10 pi 2]);
ocp_model.set('constr_lbx', [-10 -10 -pi 0]);
Jbu = eye(model.nu);
ocp_model.set('constr_Jbu', Jbu);
ocp_model.set('constr_lbu', [-1, -0.8]);
ocp_model.set('constr_ubu', [1, 0.8]);
%% weights
ocp_model.set('cost_W', model.W);
ocp_model.set('cost_W_e', model.W_e);
%% cost function
ocp_model.set('cost_type', 'nonlinear_ls');
ocp_model.set('cost_type_e', 'nonlinear_ls');
ocp_model.set('cost_expr_y', model.cost_expr_y);
ocp_model.set('cost_expr_y_e', model.cost_expr_y_e);
%% solver options
ocp_opts = acados_ocp_opts();
% integrator
ocp_opts.set('sim_method', 'erk');
ocp_opts.set('sim_method_num_stages', 2);
ocp_opts.set('sim_method_num_steps', 1);
ocp_opts.set('sim_method_newton_iter', 3);
ocp_opts.set('gnsf_detect_struct', 'true');
ocp_opts.set('param_scheme_N', model.N);
% code generation
ocp_opts.set('compile_interface', 'auto');
if model.new_solver
ocp_opts.set('codgen_model', 'true');
ocp_opts.set('compile_model', 'true');
else
ocp_opts.set('codgen_model', 'false');
ocp_opts.set('compile_model', 'false');
end
ocp_opts.set('output_dir', model.output_dir);
% NLP solver
ocp_opts.set('nlp_solver', 'sqp');
ocp_opts.set('nlp_solver_max_iter', 50);
ocp_opts.set('nlp_solver_tol_stat', 500);
ocp_opts.set('nlp_solver_tol_eq', 1E-1);
% QP solver
ocp_opts.set('qp_solver', 'partial_condensing_hpipm');
ocp_opts.set('qp_solver_cond_N', 5);
ocp_opts.set('qp_solver_iter_max', 50);
ocp_opts.set('qp_solver_warm_start', 0);
ocp_opts.set('qp_solver_cond_ric_alg', 1);
ocp_opts.set('qp_solver_ric_alg', 1);
% Hessian approximation
ocp_opts.set('nlp_solver_exact_hessian', 'false');
ocp_opts.set('regularize_method', 'no_regularize');
% other
ocp_opts.set('print_level', 0);
%% create ocp solver
ocp = acados_ocp(ocp_model, ocp_opts);
start_pos = [0;0];
goal_pos = [ 4; 6];
x0 = [start_pos;0;0];
obst_param = ([ 20 3 1 1 ...
2.5 1.5 1.1 1.1 ...
4 4 0.6 0.6 ...
20 20 0.6 0.6 ...
20 20 0.6 0.6 ...
20 20 0.6 0.6 ...
20 20 0.6 0.6 ...
20 20 0.6 0.6 ...
20 20 0.6 0.6 ...
20 20 0.6 0.6 ...
20 20 0.6 0.6 ...
20 20 0.6 0.6 ...
20 20 0.6 0.6 ...
20 20 0.6 0.6 ...
20 20 0.6 0.6 ...
20 20 0.6 0.6]');
ocp.set('p',obst_param);
dt = 0.4;
N = 15;
%% plooting figure
figure
hold on
grid on;
axis equal;
axis([-2 6 -2 6]);
xlabel('x [m]');
ylabel('y [m]');
for i = 1:4:length(obst_param)
circle(obst_param(i), obst_param(i+1), obst_param(i+2));
end
u_traj_init = zeros(model.nu, model.N);
guess = zeros(4,15);
guess(1,:) = linspace(0,goal_pos(1),15);
guess(2,:) = linspace(0,goal_pos(2),15);
guess(3,:) = atan2(goal_pos(2)-start_pos(2),goal_pos(1)-start_pos(1))*ones(15,1);
guess(4,:) = 0.2;
yref = guess;
for i=1:14
ocp.set('cost_y_ref', [yref(:,i);0;0;0], i);
end
ocp.set('cost_y_ref_e', [yref(:,end);0], 15);
x_traj_init = [guess guess(:,end)];
ocp.set('init_x', x_traj_init);
ocp.set('init_u', u_traj_init);
% solve OCP
ocp.solve();
status = ocp.get('status');
sqp_iter = ocp.get('sqp_iter');
time_tot = ocp.get('time_tot');
time_lin = ocp.get('time_lin');
time_qp_sol = ocp.get('time_qp_sol');
time_solver = 1000*time_tot;
% initialization NLP
x_traj = ocp.get('x');
u_traj = ocp.get('u');
plot(x_traj(1,:),x_traj(2,:),'b');
plot(guess(1,:),guess(2,:),'r');
axis([-2 6 -2 6]);
figure(2)
stairs(0:0.4:5.6,u_traj(2,:));
fprintf('Computation Time: %.2f ms, sqp iter: %d \n', time_solver, sqp_iter);
% disp('testing templated solver');
ocp.generate_c_code;
cd c_generated_code/
t_ocp = bicycle_model_mex_solver;
cd ..
next the results with a case that there are two obstacles near to the robot which is located at (0 , 0) and all 14 others are located at (20 , 20). The problem is the map can contain obstacle a different corrdinate which can be (100,100) or other.
