# Strange behaviour on a path planning problem with obstacle avoidance

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()

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.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

% 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

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.

@FreyJo @mss Could you give some hints to solve this issue? I have also tried it in python and the same problem exists. Thank in advance.

I’m not really sure what the issue is in your case. But from the logical point of view I don’t know if that yields to the expected behavior. So in general you want to have the biggest distance from all obstacles?! But what if the obstacle is behind you?

Can you specify the errors more exactly? Does it also occur if you have just one obstacle far away? When does the solver starts to fail? How does the solution look like when it is still feasible?

Bye

The robot is located at a position and there are 16 obstacles. The number of obstacle can’t be changed after creating solver but i can’t choose their coordinates every time calling the solver using solver parameters. So the obstacles that are located near to the robot i put their locations accurately and i choose positions for all other obstacle far from the robot.

Actually, I think that using cost function to describe the obstacle should work so I didn’t try the solution with constraints.

The errors occur when i put some obstacle (one or more) far away. For example if the initial position of the robot is at (0,0) and all obstacles are located in the range to (20,20) the solver gives a solution but if one obstacle or more are located at the coordinate (30,30) or far, the solver doesn’t give any solution.

In the real scenario, i have to put the positions of the all obstacles accurately and it can be some obstacle located at far away distance from the robot.