Use fmincon for solving optimal path planning (dynamic programming, NLP, nonlinear programming, optimal control, optimization, ode, ode45)

I want implement an algorithm for path optimization.

The system is a bicycle kinematic model, where we are able to modify only the steering wheel (the velocity is fixed).

The optimal control problem is stated in that way:

1. The manipulated variable (u): steering angle [rad];
2. Objective function (cost function): integral of the cross track distance squared from the desired path: XTE [m] (the desired path is the x-axis for simplicity).
3. Constraints:

STEERING_ANGLE_MIN <= steering angle <= STEERING_ANGLE_MAX, (limitation on the allowed values)

STEERING_ANGLE_DOT_MIN <= steering angle dot <= STEERING_ANGLE_DOT_MAX, (limitation on variation velocity)

dot := derivative respect the time.

My implenetation works if I don't set the constraint on the steering angle dot.

If I try to add this constraint the optimizer doesn't find the optimal solution.

What I tried:

1. Increase the max number of iteration;
2. Change the optimization algorithm;

Someone can help me?

Thanks <3

CODE IMPLEMENTATION:

clearvars;
dt = 0.02; % controller time step
t_vec = 0:dt:dt*200; % time vector

N = numel(t_vec);
u0_vec = zeros(N,1); % control input vector initial guess
x0 = [0, -1.5, 0]'; % vehicle initial position

v = 6; % vehicle velocity [m/s]
L = 2; % wheelbase [m]

u_min = -30*pi/180; % u lower limit -0.5236 [rad]
u_max = 30*pi/180; % u upper limit 0.5236 [rad]
u_min_vec = repmat(u_min, N, 1); 
u_max_vec = repmat(u_max, N, 1); 


delta_u_max = 120*(pi/180)*(dt); % [deg/time_step]
E=diff(eye(N));


A = [];
b = [];

%%%%%%% UNCOMMENT HERE FOR SEEING THE PROBLEM %%%%%%%%%%%%
% A=[ E;
%     -E];
% 
% b=[repmat( delta_u_max, N-1, 1); 
%    repmat(-delta_u_max, N-1, 1)];  %linear inequality matrices

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


% options = optimoptions(@fmincon, 'MaxFunctionEvaluations', 10000, ...
%                                  'Algorithm', 'interior-point', ...
%                                  'EnableFeasibilityMode', true, ...
%                                  'Display','iter-detailed', ...
%                                  'SubproblemAlgorithm', 'cg');

% options = optimoptions(@fmincon, 'MaxFunctionEvaluations', 20000, ...
%                                  'Algorithm', 'sqp', ...
%                                  'EnableFeasibilityMode', true, ...
%                                  'Display','iter-detailed', ...
%                                  'SubproblemAlgorithm', 'cg');

options = optimoptions(@fmincon, 'MaxFunctionEvaluations', 5000);

[u_optimal_vec, cost_vec] = fmincon(@(u_vec) cost_fun(t_vec, x0, u_vec, v, L), u0_vec, A, b, [], [], u_min_vec, u_max_vec, [], options);

%% PLOTS
BLUE = [0/255, 114/255, 189/255];
RED = [189/255, 76/255, 76/255];
WIDTH = 3;
figure()
plot(t_vec, u_optimal_vec*180/pi, 'LineWidth',2, 'Color', BLUE, 'LineWidth',WIDTH);
hold on
plot(t_vec, u_min_vec*180/pi, 'Color', RED,'LineStyle',  '--', 'LineWidth',WIDTH);
plot(t_vec, u_max_vec*180/pi, 'Color', RED, 'LineStyle', '--', 'LineWidth',WIDTH);
legend('u\_optimal', 'u\_min', 'u\_max');
xlabel('time [s]');
ylabel('steering angle [deg]');
grid on
ylim([-35.0 35.0])
title('Optimal steering command');


% Simulate the result
[pos_x, pos_y, att_psi] = system_simulation_fun(t_vec', u_optimal_vec, x0, v, L);

figure()
plot(0:7, zeros(8,1),'LineStyle', '--', 'Color', RED, 'LineWidth',WIDTH);
hold on
plot(pos_x, pos_y, 'Color', BLUE, 'LineWidth',WIDTH);

legend('desired\_path', 'actual\_path');
grid on
xlabel('pos x [m]')
ylabel('pos y [m]')
title('Vehicle position')

function [pos_x, pos_y, att_psi] = system_simulation_fun(t_vec, u_optimal_vec, x0, v, L)

N= numel(u_optimal_vec);
sol_t = [];
sol_x = [];

for i = 1:N-1
    sol = ode45(@(t, x) dynamic_system(t, x, u_optimal_vec(i), v, L), [t_vec(i) t_vec(i+1)], x0);
    sol_t_partial = (sol.x)'; % extract ODE solution: time
    sol_x_partial = (sol.y)'; % extract ODE solution: states
    
    x0 = sol_x_partial(end, :);
    
    
    if i == N-1
        sol_t = [sol_t; sol_t_partial];
        sol_x = [sol_x; sol_x_partial];
    else % don't store the last point because the next step is the inital point for the next step
        sol_t = [sol_t; sol_t_partial(1:end-1)];
        sol_x = [sol_x; sol_x_partial(1:end-1, :)];
    end
end

x_vec = interp1(sol_t, sol_x, t_vec);

pos_x = x_vec(:, 1);
pos_y = x_vec(:, 2);
att_psi = x_vec(:, 3);

end


function cost = cost_fun(t_vec, x0, u_vec, v, L)
sol_t = [];
sol_x = [];
N = numel(t_vec);
% simulate ODE
for i = 1:N-1
    sol = ode45(@(t, x) dynamic_system(t, x, u_vec(i), v, L), [t_vec(i) t_vec(i+1)], x0);
    sol_t_partial = (sol.x)'; % extract ODE solution: time
    sol_x_partial = (sol.y)'; % extract ODE solution: states
    
    x0 = sol_x_partial(end, :);
    
    if i == N-1
        sol_t = [sol_t; sol_t_partial];
        sol_x = [sol_x; sol_x_partial];
    else % don't store the last point because the next step is the inital point for the next step
        sol_t = [sol_t; sol_t_partial(1:end-1)];
        sol_x = [sol_x; sol_x_partial(1:end-1, :)];
    end
end



x_vec = interp1(sol_t, sol_x, t_vec);


%%%%%%%%%%% COST FUNCTION %%%%%%%%%%%
%cost = abs(x_vec(:,2))'*t_vec'; % = sum(|pos_y|*t);
cost = x_vec(:,2)' * x_vec(:,2); %+ 0.1*(x_vec(:,3)' * x_vec(:,3)); % = sum(|pos_y|*t);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
end

function dx = dynamic_system(t, x, u, v, L)
dx= [v*cos(x(3)); ...
    v*sin(x(3)); ...
    v*tan(u)/L];
end


% function [C, Ceq] = non_linear_constraints(u)
% C= [];
% Ceq = [];
% end

[Torsten]

Why do you use ODE45 to solve your ODEs ? They seem easy enough to be solved analytically - and thus a help to stabilize the optimization.

[Steven Caliari]

I want build a generic framework because after I want use different vehicle models. I just use ode45 for solving numerically.

[Torsten]

In my opinion, stability of the code at present is more important than generic framework as a vision for the future.

But it's your decision, of course.

And of course I don't know if this change in solving your ODEs will really make your code work.

[Steven Caliari]

Do you think that the problem is the use of ode45? I can try to increase the precision of ode45 or use another solver for ODE.

I have to fix in some way because with the inequality constraint the optimization not converge.

[Steven Caliari]

Thanks for helping me.

What I don't understant is why the optimal solution is reached without constraint but with constraints (inequality constraints) the optimal solution diverge completely.

NO CONTRAINTS: the vehicle position reach XTE = 0 (desired condition).

[Torsten]

But these constraints on STEERING_ANGLE_DOT are not yet implemented in the code you posted, are they ?

[Steven Caliari]

I implement the differential constraints on the control input rate (steering angle rate) in this way:

delta_u_max = 1; % use 1 only for example
E=diff(eye(N));
A=[ E; -E];
b=[repmat( delta_u_max, N-1, 1); 
    repmat(-delta_u_max, N-1, 1)];  %linear inequality matrices

I use the solution proposed in another question in the matlab community.

The differential constraint is expressed as difference constraint using linear constraints matrix: A, b.

The constraint is splitted in this way:

steering angle dot <= STEERING_ANGLE_MAX

-steering angle dot <= -STEERING_ANGLE_MIN

PS: I have found my error!!!! was in the second part of the b vector, the sign was wrong! >:(

delta_u_max = 1; % use 1 only for example
delta_u_min = -1; % use -1 only for example
E=diff(eye(N));
A=[ E; -E];
b=[repmat( delta_u_max, N-1, 1); 
    repmat(-delta_u_min, N-1, 1)];  %linear inequality matrices

Thanks! <3