Optimizing system of nonlinear differential equations

[Farhad Pakro]

Hello,

I am trying to optimize a system of nonlinear differential equations, there seems to be no problem in coding, but this is the answer Yalmip gives me:

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

ans = 

    solvertime: 0

       problem: 14

          info: 'Model creation fa...'

    yalmiptime: 1.6160

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

These are main lines of my code:

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

u = sdpvar(repmat(nu,1,N),ones(1,N));

x = sdpvar(repmat(nx,1,N+1),ones(1,N+1));

objective = 0;

constraints = [];

for kk = 1:N

    dx=equations(nfs,x{kk},u{kk});

    objective = objective +(u{kk}'*R*u{kk})^2;

    constraints = [constraints, x{kk+1} == x{kk}+step*dx];

    constraints = [constraints, -8*pi/180 <= u{kk}(1,1)<= 8*pi/180];

end

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

Here "dx" is a vector of derivatives in each step "kk".

Please help me with the case.

P.S. I have attached my codes, if they are needed.

Thank you in advance.

[Johan Löfberg]

http://users.isy.liu.se/johanl/yalmip/pmwiki.php?n=Extra.SQRT

However, you would have to be really lucky for this to work. You introduce a lot of singularities etc by the division of variables, and fmincon for instance will not be able to cold-start the optimization (it tries to start at the origin, which leads to failure). You will have to tweak with algorithm choices to even to get it to start.

Perhaps you could start by removing singularities by introducing new variables alpha and beta with constraints tan(alpha)*u == w etc. Still, you are faced with a nasty nonlinear nonconvex program

[Farhad Pakro]

Thanks a lot for your answer and your devoted time, professor.

I will work on that and try to get a solution and I will keep in touch with you.

Thank you again.

[Farhad Pakro]

I have made some changes to my coding and it worked. New codes are attached.

Now I need to use an objective function that I have no Idea how to do that. I want to solve the problem as "fixed final position and free final time" and in the objective function I want to have minimum time (a time optimal problem) and minimum final position error.

I mean consider that the final position is x{end}(3,1). I add the following line right after the for loop in my code, but this doesn't work:

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

objective = objective + abs(value(x{end}(3,1))-href);

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

In addition, I need the time to be minimum. As far as there is no time defined in the program and all we have is iteration "N", I can consider the lowest value of "N" which satisfies the final position, instead of the minimum time (because after a number of "N's" we reach the answer and for values more than that all "u's" are 0). So, how can I define a stopping criteria for the algorithm which stops running, after the final position reaches to the desired value? or are there any other ways to add "time" to objective function?

Thanks for your consideration in the case.

[Johan Löfberg]

I would say that you are simply using the wrong tool for the task. For a classical nonlinear optimal control problem with end-time etc, you should probably try to use a classical optimal control approach (Euler Lagrange, HJB, adjoints etc) and software particlarily developed for that. Minimizing the final time is not easily put into a standard optimization framework. One approach is to start with N=1 and the constraint x{N}(3,1))-href==0. If infeasible, solve with N=2, etc, until feasibility.

[Farhad Pakro]

I understand. Thank you very much for your devoted time.

[Preetham Harinath]

Hello,

I have also been trying out MPC for an obstacle avoidance maneuver using a Two track Vehicle Dynamics model. The problem is that I am used to forming the characteristic equations as an ODE and I tried out the same using Euler discrete method. Unfortunately I cannot get any optimised control sequence, which in this case is steering input. Have a look at my code though....% Obstacle Avoidance - Optimized Steering Sequence Using MPC 

clear all; close all;

global Fy1_curr Fy2_curr Fy3_curr Fy4_curr

%% Parameters definition

% model parameters

params.l_f                  = 1;                        % distance between center of gravity and front axle

params.l_b                  = 1;                        % distance between center of gravity and rear axle

params.mass                 = 1435;                     % Vehicle Mass

params.Cf                   = 100000;                   % Front Cornering Stiffness

params.Cr                   = 100000;                   % Rear Cornering Stiffness

params.Jz                   = 2380;                     % Vehicle Moment of Inertia

params.tw                   = 1;                        % half track vehicle width

params.vehicle_width        = 2;                        % vehicle width

params.Ts                   = 0.1;                        % sampling time (both of MPC and simulated vehicle)

params.nstates              = 10;                        % number of states

params.ninputs              = 1;                        % number of inputs

% environment parameters

params.track_end            = 20;                       % end of the track

params.activate_obstacles   = 1;                        % 0 if there are no obstacle, 1 otherwise

params.obstacle_centers     = [11 0];                   % x and y coordinate of the 4 obstacles

params.obstacle_size        = [2 1];                    % size of the obstacles

params.lane_semiwidth       = 4;                        % semi-width of the lane

% control parameters

params.controller           = 'MPC';                    % 'SF' for state-feedback controller, 'MPC' for MPC controller

objective = 0;

% simulation parameters

params.x0                   = 0;                        % initial x coordinate

params.y0                   = 1;                        % initial y coordinate

params.vx                   = 14;                       % initial longitudinal speed

params.vy0                  = 0;                        % initial lateral speed

params.psi0                 = 0;                        % initial heading angle

params.psidot0              = 0;                        % initial heading angle rate

params.N_max                = 1000;                     % maximum number of simulation steps

% plotting parameters

params.window_size          = 10;                       % plot window size for the during simulation, centered at the current position (x,y) of the vehicle

params.plot_full            = 0;                        % 0 for plotting only the window size, 1 for plotting from 0 to track_end

%% Simulation environment

% initialization

z                           = zeros(params.N_max,params.nstates);             % z(k,j) denotes state j at step k-1

u                           = zeros(params.N_max,params.ninputs);             % u(k,j) denotes input j at step k-1

z(1,:)                      = [params.x0,params.vx,params.y0,0.1,0,0,params.x0,0,params.y0,0]; % definition of the intial state

% u(1,:)                      = [0.01];

comp_times                  = zeros(params.N_max);                            % total computation time each sample

solve_times                 = zeros(params.N_max);                            % optimization solver time each sample

N = 3;

i = 0;

objective = 0;

u_predicted = [];  

z_predicted = [];

% ref = [50 0 10 0];

Q = eye(10);

R = eye(2);

while (z(i+1,1)<params.track_end) && (i<params.N_max)

    i = i+1;

    switch params.controller

        case 'SF'                                                   % state-feedbaack controller definition

            K = [0 0 0 0 ];

            u(i,:) = K * z(i,:)';

            pause(0.1);

        case 'MPC'                                                  % MPC controller definition

            yalmip('clear')

            constraints = [];

            

            u_var = sdpvar(N,1);

%             u_var(1,:) =  u(i,:)

            z_var = sdpvar(N,10);

            z_var(1,:) = z(i,:);

            

            for k = 1:N-1

 Fy1 = -params.Cf * (atan( (z_var(k,4) + params.l_f * z_var(k,5))/(params.vx - params.tw * z_var(k,5)) ) - u_var(k,1));  %Fy1

                Fy2 = -params.Cf * (atan( (z_var(k,4) + params.l_f * z_var(k,5))/(params.vx + params.tw * z_var(k,5)) ) - u_var(k,1));  %Fy2

                Fy3 = -params.Cr * atan( (z_var(k,4) - params.l_b * z_var(k,5))/(params.vx - params.tw * z_var(k,5)) );               %Fy3

                Fy4 = -params.Cr * atan( (z_var(k,4) - params.l_b * z_var(k,5))/(params.vx + params.tw * z_var(k,5)));              %Fy4

                if isnan(value(Fy1)) == 1

                    Fy1 = 0;

                end

                if isnan(value(Fy2)) == 1

                    Fy2 = 0;

                end

                if isnan(value(Fy2)) == 1

                    Fy3 = 0;

                end

                if isnan(value(Fy2)) == 1

                    Fy4 = 0;

                end

                

              

                

                z_var(k+1,1) = z_var(k,1) + params.Ts * z_var(k,2); %x

                z_var(k+1,2) = z_var(k,2) + params.Ts * (1/params.mass * (   -( value(Fy1) + value(Fy2) )*sin(u_var(k,1))   ) + z_var(k,5)*z_var(k,4)); %Vx

                

                

                

                z_var(k+1,3) = z_var(k,3) + params.Ts * z_var(k,4); %y

                z_var(k+1,4) = z_var(k,4) + params.Ts * (1/params.mass * (   ( value(Fy1) + value(Fy2) )* cos(u_var(k,1))  + value(Fy3) + value(Fy4) ) - z_var(k,5)*z_var(k,2)); %V_y

                

                z_var(k+1,5) = z_var(k,5) + params.Ts * z_var(k,6);    %psi_dot

                z_var(k+1,6) = z_var(k,6) + params.Ts * (1/params.Jz * (    ( value(Fy1) - value(Fy2) )* sin(u_var(k,1))*params.tw  + ( value(Fy1) + value(Fy2) )* cos(u_var(k,1))*params.l_f - ( value(Fy3) + value(Fy4) )*params.l_b )  );  %psi_double_dot

            

               

                

                

                

                z_var(k+1,7) = z_var(k,7) + params.Ts * z_var(k,8); %Global_X

                z_var(k+1,8) = z_var(k,8) + params.Ts *( z_var(2) -  z_var(k,5)*z_var(k,9)); %Global_Vx

                

                  

                z_var(k+1,9) = z_var(k,9) + params.Ts * z_var(k,10); %Global_Y

                z_var(k+1,10) = z_var(k,10) + params.Ts *( z_var(4) +  z_var(k,5)*z_var(k,7)); %Global_Vy

                

                

                

                objective = objective + (z_var(k,3)) * 10 *(z_var(k,3))'  ;

                

                constraints = [constraints, -10 <= u_var(k,1) <= 10]

                constraints = [constraints, -params.lane_semiwidth <= z_var(k,2) <= params.lane_semiwidth]

%                 constraints = [constraints, (z_var(k,1) - 11)^2/2 + (z_var(k,2))^2/0.5 >= 1]

                  

            end

            tic()

            options = sdpsettings('solver', 'fmincon', 'usex0', 1);

            diagnostic = optimize(constraints, objective, options);

            comp_times(i) = toc();

            solve_times(i) = diagnostic.solvertime;

            

            u(i,:) = u_var(1,1);    

            

            

            for j = 1:N

                z_predicted(j,:) = z_var(j,:);

            end

            

            for h = 1:N-1

                u_predicted(h,:) = u_var(h,:);

            end

        

    end  

    

    

    % simulate the vehicle

    z(i+1,:) = car_sim_test(z(i,:), u(i,:), params);

objective = 0;

              

end

[Johan Löfberg]

You definitely don't want to define dynamics by propagating it by assignments. The complexity of the state expressions will become asolutely horrible when you step forward. 

Beyond that once you would define dynamics through equalities instead, you still have a pretty nonlinear model, and blindly trying to throw this into YALMIP hoping a reasonable model will finally make its way into the nonlinear solver, is a bit optimistic. In particular when you have operators with singularities (atan, divisions). Much more likely to work would be to use a tool particularly developed for optimal control of nonlinear systems, such as casadi