# Optimal strategies for guidance-by-repulsion model with IpOpt and AMPL

Author: - 27 May 2019

### A sample result This video describes an optimal control steering red dots(evaders) to the position $(4,4)$, which is calculated by IpOpt with AMPL. See the post explaining IpOpt and AMPL for detailed description.

### The guidance-by-repulsion model

Let $u_{ei}$ and $u_{dj}$ be the positions of the evaders $(i=1,\ldots,N)$ and drivers $(j=1,\ldots,M)$, respectively, in ${\mathbb R}^2$. We consider the following ODE system:

where $\kappa_j(t)$ is the control interface and the interaction functions are given by

In the dynamics, the evaders always get forced from all of the drivers, while the drivers wants to keep a stable distance ($f_d(1)-f_e(1)=0$). By choosing proper $\kappa_j(t)$, we wants to steer the evaders into a predetermined region.

The objective of the control is given by the cost function with the fixed final time $t_f>0$,

where $\delta_1$ is a regularization coefficient, $0.1$ in this post.

### AMPL code for solving the optimal control problem

Let us explain an AMPL code step by step. First, we need to set relative parameters.

# GBR_fixedtime.mod

# This file solves the Guidance-by-repulsion model with 2 drivers and 16 evaders to

# the target point [4;4] in a fixed final time T=10.

# Define the parameters of the problem

param pi      = 3.141592653589793238462643;

param Nt      = 100;      # number of discretized points in time

param M_sqrt  = 4;        # sqrt of Number of evaders

param M_e     = M_sqrt^2; # Number of evaders

param M_d     = 2;        # Number of drivers

param T       = 10;

param M_kappa = 5;        # Bound on the control

# Target point

param uf1 = 4;
param uf2 = 4;


For the initial condition, we use uniform distribution of evaders in a square $[-1,1]\times[-1,1]$, and the drivers are in a circle with radius $5$.

# Initial values

param ue_zero {k in 1..M_e, j in 1..2};
for {k1 in 1..M_sqrt, k2 in 1..M_sqrt, j in 1..2} {
let ue_zero[M_sqrt*(k1-1)+k2,1] := ((k1-1)/(M_sqrt-1))*2-1;
let ue_zero[M_sqrt*(k1-1)+k2,2] := ((k2-1)/(M_sqrt-1))*2-1;
}
param ud_zero {l in 1..M_d, j in 1..2};
for {l in 1..M_d, j in 1..2} {
let ud_zero[l,1] := 5*cos(2*pi/M_d*(l-1));
let ud_zero[l,2] := 5*sin(2*pi/M_d*(l-1));
}

# State variables

var ue {k in 1..M_e, i in 0..Nt, j in 1..2};
var ve {k in 1..M_e, i in 0..Nt, j in 1..2};
var ud {l in 1..M_d, i in 0..Nt, j in 1..2};
var vd {l in 1..M_d, i in 0..Nt, j in 1..2};
var uec {i in 0..Nt, j in 1..2} = 1/M_e*(sum {k in 1..M_e} (ue[k,i,j]));

# initialize

subject to ue_init {k in 1..M_e, j in 1..2} : ue[k,0,j] = ue_zero[k,j];
subject to ud_init {l in 1..M_d, j in 1..2} : ud[l,0,j] = ud_zero[l,j];
subject to vd_init {k in 1..M_e, j in 1..2} : ve[k,0,j] = 0;
subject to ve_init {l in 1..M_d, j in 1..2} : vd[l,0,j] = 0;
let {k in 1..M_e, i in 0..Nt, j in 1..2} ue[k,i,j] := ue_zero[k,j];
let {l in 1..M_d, i in 0..Nt, j in 1..2} ud[l,i,j] := ud_zero[l,j];
let {k in 1..M_e, i in 0..Nt, j in 1..2} ve[k,i,j] := 0;
let {l in 1..M_d, i in 0..Nt, j in 1..2} vd[l,i,j] := 0;


For the initial guess, we also put the initial data for the whole timeline. If we don’t specify this, then the default values are zero, which can make redundant errors or iterations.

Now, we define the controls and cost.

# The control function

var kappa {l in 1..M_d, i in 0..Nt}, default 2.8;
subject to kappa_bounds {l in 1..M_d, i in 0..Nt} : -M_kappa <= kappa[l,i] <= M_kappa;
# initial guess on control

let {l in 1..M_d, i in 0..Nt} kappa[l,i] := 2.8;

# The cost function

minimize cost:
1/M_e*(sum {k in 1..M_e} ((ue[k,Nt+1,1]-uf1)^2+(ue[k,Nt+1,2]-uf2)^2)) + 0.1/M_d/(Nt+1)*( sum {l in 1..M_d, i in 0..Nt} (( kappa[l,i]^2)) );


The dynamics need to be described in the form of restriction condition of states and control variables. In order to avoid divide-by-zero, we additionally put the restriction on the distance. Since our equation has unbounded interaction kernels, it is convenient to use Euler’s forward method. Backward or central method may result unexpected high velocities by closing to the infinite values.

# Restriction on distance

subject to distance {k in 1..M_e, l in 1..M_d, i in 0..Nt} :
(ue[k,i,1]-ud[l,i,1])^2+(ue[k,i,2]-ud[l,i,2])^2 >= 0.001;
# Dynamics

subject to ue_dyn {k in 1..M_e, i in 0..Nt-1, j in 1..2} :
(ue[k,i+1,j]-ue[k,i,j])*Nt/T = ve[k,i,j];
subject to ud_dyn {l in 1..M_d, i in 0..Nt-1, j in 1..2} :
(ud[l,i+1,j]-ud[l,i,j])*Nt/T = vd[l,i,j];
subject to ve_dyn {k in 1..M_e, i in 0..Nt-1, j in 1..2} :
(ve[k,i+1,j]-ve[k,i,j])*Nt/T = sum {l in 1..M_d} ( -2/((ud[l,i,1] - ue[k,i,1])^2+(ud[l,i,2] - ue[k,i,2])^2)*(ud[l,i,j] - ue[k,i,j]) )  - 2*ve[k,i,j];
subject to vd1_dyn {l in 1..M_d, i in 0..Nt-1} :
(vd[l,i+1,1]-vd[l,i,1])*Nt/T = (-5.5/((ud[l,i,1] - uec[i,1])^2+(ud[l,i,2] - uec[i,2])^2)+10/(((ud[l,i,1] - uec[i,1])^2+(ud[l,i,2] - uec[i,2])^2)^2) -2)*(ud[l,i,1] - uec[i,1]) + kappa[l,i] * (-(ud[l,i,2]-uec[i,2])) - 2*vd[l,i,1];
subject to vd2_dyn {l in 1..M_d, i in 0..Nt-1} :
(vd[l,i+1,2]-vd[l,i,2])*Nt/T = (-5.5/((ud[l,i,1] - uec[i,1])^2+(ud[l,i,2] - uec[i,2])^2)+10/(((ud[l,i,1] - uec[i,1])^2+(ud[l,i,2] - uec[i,2])^2)^2) -2)*(ud[l,i,2] - uec[i,2]) + kappa[l,i] * ((ud[l,i,1]-uec[i,1])) - 2*vd[l,i,2];


The next step is to solve and get the output. It is convenient to describe the parameters of the problem to visualize later.

# Solve with IpOpt

option solver ipopt;
option ipopt_options "max_iter=20000 linear_solver=mumps hessian_approximation=limited-memory halt_on_ampl_error yes";
solve;
# Display solution

printf : "%24.16e\n", T;
printf : "%d\n", Nt;
printf : "%d\n", M_d;
printf : "%d\n", M_e;
printf : "%24.16e\n", uf1;
printf : "%24.16e\n", uf2;

printf {l in 1..M_d, i in 0..Nt} : "%24.16e\n", kappa[l,i];
printf {k in 1..M_e, i in 0..Nt, j in 1..2} : "%24.16e\n", ue[k,i,j];
printf {l in 1..M_d, i in 0..Nt, j in 1..2} : "%24.16e\n", ud[l,i,j];
printf {k in 1..M_e, i in 0..Nt, j in 1..2} : "%24.16e\n", ve[k,i,j];
printf {l in 1..M_d, i in 0..Nt, j in 1..2} : "%24.16e\n", vd[l,i,j];
printf : "End. \n";
end;



Finally, we used MATLAB to show the trajectories. After copying the result to a file ‘output.txt’, we read it with the following code.

fid= fopen('output.txt','r');            % Open out.txt
temp = fscanf(fid,'%f');
M_d = 2; M_e = 9; index1 = 1;
T = temp(index1); index1 = index1+1;
Nt = temp(index1); index1 = index1+1;
M_d = temp(index1); index1 = index1+1;
M_e = temp(index1); index1 = index1+1;
uf1 = temp(index1); index1 = index1+1;
uf2 = temp(index1); index1 = index1+1; index2 = index1+M_d*(Nt+1)-1;
kappa = temp(index1:index2); kappa = reshape(kappa, [Nt+1 M_d]); index1 = index2+1; index2 = index1+2*(Nt+1)*M_e-1;
ue = temp(index1:index2); ue = reshape(ue, [2 Nt+1 M_e]); index1 = index2+1; index2 = index1+2*(Nt+1)*M_d-1;
ud = temp(index1:index2); ud = reshape(ud, [2 Nt+1 M_d]); index1 = index2+1; index2 = index1+2*(Nt+1)*M_e-1;
ve = temp(index1:index2); ve = reshape(ve, [2 Nt+1 M_e]); index1 = index2+1; index2 = index1+2*(Nt+1)*M_d-1;
vd = temp(index1:index2); vd = reshape(vd, [2 Nt+1 M_d]);
index2 == length(temp)      % Check the file is properly read.
tline = linspace(0,T,Nt+1);

fclose(fid);

%% Cost calculation

u_f = [uf1;uf2];
Final_Time = tline(end);
Final_Position = reshape(ue(:,end,:),[2 M_e]);
Final_cost = sum( (Final_Position(1,:) - u_f(1)).^2+(Final_Position(2,:) - u_f(2)).^2 )/M_e;
Running_cost = 0.001*trapz(tline,mean(kappa(:,1:M_d).^2,2))/M_d;

%% Visualize the trajectories

f1 = figure('position', [0, 0, 1000, 400]);
subplot(1,2,1)
hold on
for k = 1:M_d
plot(ud(1,:,k),ud(2,:,k),'b--','LineWidth',1.3);
end
for l = 1:M_e
plot(ue(1,:,l),ue(2,:,l),'r-','LineWidth',1.5);
end
xlabel('abscissa')
ylabel('ordinate')
title(['Position error = ', num2str(Final_cost)])
grid on


Then, one can get the following trajectories and optimal control solution. subplot(1,2,2)
plot(tline,kappa','LineWidth',1.3);
xlabel('Time')
ylabel('Control \kappa(t)')
title(['Total Time = ',num2str(tline(end)),' and running cost = ',num2str(Running_cost)])
grid on 