Mixed-integer linear programming collision avoidance method in unmanned aerial vehicles trajectory planning

The four edges of each obstacle are given by 

x = xl; x = xu; y = yl; or y = yu

(2.14)

The constraints on vehicle position are formulated as 

xk ≤ xl +M bobst,1jk yk ≤ yl +M bobst,2jk xk ≥ xu −M bobst,3jk yk ≥ yu −M bobst,4jk   j=1, …, no, k=1, …, np

(2.15) (2.16) (2.17) (2.18) (2.19)

       where [x_k, y_k]^T gives the position of a vehicle at time k, M is a number larger than the size of the world map, n_o is the number of obstacles, n_p is the number of steps in the planning horizon, and b_obst,ijk is an i − j − k^th element of a binary matrix of size 4 by n_o by n_p. At time-step k, if b_obst,ijk = 0 in Eqs. 2.15 to 2.18, then the constraint is active and the vehicle is outside of the j^th obstacle. If not, the large number M relaxes the obstacle avoidance constraint.

The logical constraint in Eq. 2.19 requires that at least one of the four constraints be active for each obstacle at each time-step. The obstacle shape is assumed to be rectangular, but this formulation is extendable to obstacles with polygonal shapes. Also, non-convex obstacles can be easily formed by overlapping several rectangular obstacles. Figure 2-1 shows that we must expand the obstacle size at the planning level to account for the discrete steps taken by the vehicle. This increase is done at both the estimationand trajectory design phases. Since the avoidance constraints are only applied at discrete time steps shown as ⊗ marks, it is possible for the planned trajectory to “cut the corner” of the obstacle between time points. Each waypoint is separated by vΔt and an obstacle [x_l, y_l, x_u, y_u] must be expanded in each direction by , which is the maximum incursion distance, so that

(2.20)

Figure 2-1: Corner cut and obstacle expansion

2.3.2. Vehicle Avoidance 

Collision avoidance between vehicles is written in the same way as obstacle avoidance. Assume the i^th vehicle has a certain physical size and safety distance surrounding in that forms together a rectangle of 2d_i by 2d_i around its center. At each time k, the position of the i^th vehicle and that of the jt^h vehicle must satisfy the following relations [3]: 

xik ≤ xjk + (di + dj) +M bveh,1ijk yik ≤ yjk + (di + dj) +M bveh,2ijk   xik ≥ xjk − (di + dj) −M bveh,3ijk yik ≥ yjk − (di + dj) −M bveh,4ijk   i = 1, . . . , nv,   j= i + 1, . . . ,  nv     k = 1, . . . , np

(2.21) (2.22) (2.23) (2.24) (2.25)

where [x_ik, y_ik]^T gives the position of a vehicle i at time k, n_v is the number of vehicles, and b_veh,_lijk is the l − i − j − k^th element of a binary matrix of size 4 by n_v by n_v by n_p. If b_veh,_lijk = 0 in Eqs. 2.21 to 2.24, the constraint is active, and the safety boxes of the two vehicles, i and j, do not intersect each other. Again, in order to account for the discretized time, a margin is added to the vehicle rectangle of 2d_i by 2d_i. If two vehicles i and j are moving towards each other with a speed of v_i and v_j respectively, the distance between each waypoint in a relative coordinate frame, can be as large as (v_i + v_j)Δt. Thus, the size of the ith vehicle in this MILP optimization model must be expanded to a size of 2d_i|expanded by 2d_i|expanded where

(2.26)

2.4. Plan beyond the Planning Horizon 

The Receding Horizon Controller uses a coarse cost map based on straight lines to predict the trajectory beyond the planning horizon. There are two aspects involved in connecting the detailed trajectory over the planning horizon to this coarse cost map. First, MILP selects a cost point that leads the vehicle to the goal along the shortest path. Second, it ensures that the selected cost point is visible from a point on the planning horizon (i.e., the straight line segment connecting the selected cost point and the horizon point must be obstacle/collision free). Given the location of the obstacles and a goal, the cost points are defined as the obstacle corners and the goal itself. The shortest distance from a cost point to the goal along kinematically feasible straight-line segments forms a cost associated with the cost point and goal. If a cost point is inside of another obstacle, it has an infinite cost. Let [x_cp,i, y_cp,i]^T denote the it^h cost point, c_i the cost associated with the i^th cost point, and c_vis,k the cost-to-go at the cost point selected by vehicle k. The binary variables associated with cost point selection b_cp will have three dimensions for cost point, goal, and vehicle, where n_c is a number of cost points, and n_g is a number of goals [4]:

(2.27)   (2.28)

Eq. 2.28 enforces the constraint that each vehicle must choose a combination of goal and cost point, and Eq. 2.27 extracts the cost-to-go at the selected point from the cost map c_i. In order to ensure that the selected cost point [x_vis,k, yv_is,k]^T is visible from the terminal point [(x_np)k, (y_np)k]^T of vehicle k, obstacle avoidance is checked at n_t test points that are placed on a line segment connecting the two points. This requires binary variables b_v  is that have four dimensions: the obstacle corner, vehicle, test point, and obstacle. The test conditions can be written as:

	(2.29)   (2.30)         (2.31)
k=1, …, nv, m=1, …, nt , n=1, …, no	(2.32) (2.33) (2.34) (2.35) (2.36)

where [x_LOS,k, y_LOS,k]^T in Eq. 2.30 is a line-of-sight vector from the terminal point to the selected cost point for vehicle k, and the binary variable b_vis has four dimensions (obstacle boundary, vehicle, test point, and obstacle). 

2.5. Target Visit 

The highest goal of the mission is to search, attack, and assess specific targets. These tasks can be generalized as visiting goals and performing the appropriate action. The heading direction at the target can be included if assessment from a different angle is required. In this section, these goals are assumed to be allocated among several vehicles so that each vehicle has an ordered list of goals to visit. The goals are assumed to be set further apart than the planning horizon, so each vehicle can visit at most one goal point in each plan. The constraint on the target visit is active at only one time-step if a plan reaches the goal, and is relaxed at all of the waypoints if it does not. If a vehicle visits a goal, the rest of the steps in the plan are directed to the next goal.

Therefore, only the first two unvisited goals are considered in MILP (i.e., the number of goals n_g is set to be 2). The selection of the cost point and the decision of the time of arrival are coupled as 

(2.37)   (2.38)

where b_arrival,ij = 1 if the j^th vehicle visits the first target at the i^th time step, and equals 0 if not. Also, i = n_t + 1 indicates the first target was not reached within the planning horizon. If a vehicle cannot reach the goal or reaches the goal at the final step, Eq.2.37 must use the cost map built for the first goal. If a vehicle can visit the first goal in the planning horizon, Eq. 2.38 requires it to use the cost map built about the second goal. Agoal has a v_iΔt by v_iΔt square region of tolerance around it because the system in operated in discrete time. This tolerance is encoded using the intermediate variable

derr          i=1, …, nt, j = 1, …, nv .

(2.39) (2.40) (2.41) (2.42)   (2.43)

If a vehicle j reaches its goal at time-step i, then b_arrival,ij = 1 and the constraints in Eqs. 2.39 to 2.42 are active; if not, all the constraints are relaxed. Including d_err in the objective function causes the point of visit x_ij (where b_arrival,ij = 1) to move closer to the goal. The heading constraints can be written in the same way 

2.6. Cost Function 

This formulation minimizes the time of arrival as a primary objective. Under the assumption that each vehicle flies at a constant speed, the minimization of distance is equivalent to the minimization of time. The distance to the goal is made up of three segments: 1) initial position to the terminal point of the plan; 2) terminal point to the cost point; and 3) the precalculated cost-to-go at the selected cost point. Since the distance from the initial position to the terminal point is constant, the MILP optimizer minimizes the sum of the length of the line-of-sight vector 2) and the cost-to-go at the selected cost point 3) by choosing the best combination of terminal and cost-to-go points [3].

The length of the line-of-sight vector is a two norm of the line-of-sight vector defined in Eq. 2.30. This is calculated in the MILP by minimizing an upper bound:

(2.44)   (2.45)

where n_l is the number of unit vectors onto which the line-of-sight vector is projected and l_j gives an upper bound of the line-of-sight vector length for vehicle j. In this case, the objective function J₀ to be minimized in

(2.46)

 

 where c_vis,j is a cost-to-go at the cost point selected by the jth vehicle as defined in Eq. 2.28. The time-step of arrival (TOA) for the jth vehicle is expressed as

(2.47)

Note that in Eq. 2.47, if k = n_t+1 when b_arrival,kj = 1, then the j^th vehicle will not reach the target in the planning horizon, thereby resulting in a higher cost. Combined with Eq. 2.46, the term TOA forms the objective function for the minimum time of arrival problem:

(2.48)

where v_jΔt converts the discrete time-step (TOA_j is an integer) into distance and α is a large weighting factor that makes the first term dominant. With a small α, the vehicle tends to minimize the cost-to-go, so that it arrives at the first goal at the terminal step in the planning horizon. This is in contrast to the more desirable case of arriving at the first goal before the terminal step, and then minimizing the cost-to-go to the next goal. Thus the weight α must be larger than the number of steps required to go from the first goal to the next goal.  Finally, the term J_a, containing two auxiliary terms, is added to the primary cost in Eq. 2.48 to decrease the position and heading error at the target. The new cost to be minimized is defined as

(2.49)

where

(2.50)

Note that an overly large value of β results in “wobbling” near the target. The wobbling occurs as the vehicle attempts to maneuver, such that on the same time-step when it enters the target region, it arrives at the center of the target. Also note that the equations involving the dynamics, constraints, and costs are not dependent of the number of vehicles except for vehicle collision avoidance, and thus these can be solved separately if vehicle collision avoidance is not an issue [7]. 

 

 

 

 

 

 

 

 

 

3. SOFTWARE

The total optimization problem from the previous section can be readily solved by a mixed integer program solver implemented in the CPLEX software package [5]. CPLEX requires the standard MPS-format as input, which can be generated by running a specific m-script in MATLAB. However, writing such an MPS-file generating script is an error prone task and such scripts are usually not very flexible in adapting the cost function or the type of constraints. I have therefore used the A Mathematical Programming Language (AMPL) to formulate the path planning problem. Implementing the constraints in AMPL is straightforward, requiring minimum translation. The forms of the problem and the constraints are defined in a model file, while the parameter values are in a separate data file.

Therefore, changes to the problem may be made without rebuilding the constraints expressions. The data files can be edited directly or generated by a simple MATLAB script. AMPL combines the model and data to an MPS-file, which is then solved by CPLEX. Moreover, AMPL can handle piece-wise linear variables and thus absolute values: it hence generates the necessary slack variables and extra constraints automatically. I thus specify the system matrices, the obstacle coordinates and other optional dynamical parameters in MATLAB, and use a specific script to generate an AMPL input file. An AMPL script selects the appropriate model and data file, calls the solver and writes the resulting solution to an output file readable by MATLAB. Other MATLAB -scripts then plot the path and visualize the state and input sequence.    

 

 

 

4. MODELING AND ITS RESULTS
1. Annotation

Introductory passage. In accordance to previously decribed method the program “Modeling of unmanned aerial vehicle trajectory on the base of MILP” was created in programming environment MATLAB version 7.10.0.499. As it was mentioned, in order to implement MILP, the additional complex of computer program was used, namely AMPL CPLEX [5] software optimization package, that is an algebraic modeling language for describing and solving high-complexity problems for large-scale mathematical computation.

CPLEX solves linear and convex quadratic programs by simplex or interior-point methods, and linear and convex quadratic integer programs by a branch-and-bound procedure.

Functional area. The main objective of the program is simulation of UAV trajectory planning and finding of optimal way from the initial to final point with a glance to time and fuel consumption with the overcoming of obstacles. The program can be used on all computers the operating systems of which are compatible with MATLAB. The implementation of the program requires the using of AMPL modules. During the creation of program the student version of AMPL (CPLEX) was used. That’s why there is a limitation on input data because the student edition is limited to 300 variables and 300 constraints.

Description of logic.The program consists of the following structural parts: initializing of structure, dynamics model specifying, environmental conditions specifying, solving of the problem by MILP, plotting of the input and output parameters.

 The program starts with the initialization of structure. Each field in the structure appears to be a separate MATLAB array. After that the parameters of dynamic model (speed, turn rate, time step and number of steps) and environmental model (initial state of UAV, its final position (one or more) and obstacle (one or more) must be inserted.

The next step after initial data entry is converting the described dynamic model into acceleration limits and definition of approximations for magnitude limits. After that the data file can be started to be written. AMPL allows defining the initial data and rewriting it into the new file. All previously specified and all following data is written to the files Trajectory.dat and Trajectory.mod. During the writing of these files the following steps of problem solving are implemented with the help of AMPL module: convertion of the system into 2-dimensional in AMPL format, defining of the number of timesteps, vehicles and obstacles, initial and final states which are specified at the beginning of the program. It is compulsory to define speed and force limits and circle approximation for acceleration limit. Also the significant part is the calculation of fuel and time completion penalty, which allow choosing the most fuel and time-effective way for UAV. At the end of the program after all required computations the plots of the obstacles and UAV trajectories are created.

MATLAB (matrix laboratory) is a numerical computing environment and fourth-generation programming language. Developed by MathWorks, MATLAB allows matrix manipulations, plotting of functions and data, implementation of algorithms, creation of user interfaces, and interfacing with programs written in other languages [8].

The algorithm of program “Modeling of unmanned aerial vehicle trajectory on the base of MILP” is presented on the Fig 4-1:

 

 

 

 

 

 

 

 

 

 

 

 

 

Figure 4-1. The algorithm of mixed-integer linear programming model of UAV trajectory

2. Text of the program

The text of the program “Modeling of unmanned aerial vehicle trajectory on the base of MILP” on the initial language:

clear all %cleaning of command window and graphical windows

clc

clf

% initializing of structure

Nt             = [];

dt             = [];

Vmax           = [];

Wmax           = [];

x0             = [];

xT             = [];

obst           = [];

vehBox         = [];

targetBox      = [];

targetPenalty  = [];

capab          = [];

% specifying of dynamics models

Vmax=0.2;                % speed [m/s]

Wmax=(pi/180)*4;         % turn rate [rad/s]

dt=2;                    % time step [sec]

Nt=10;                   % number of steps

% specifying of environment

theta=50/180*pi;

x0=[-4.5 -3, Vmax*.5*[cos(theta) sin(theta)]];   % initial states

xT=[-1.5 -3; -3 -3; -2 -1];                      % target positions

obst=[ -4 -3 -2 -3];                             % obstacles

%----------------------------------------------------------------------

% solve MILP

%----------------------------------------------------------------------

% convert dynamics to acceleration limits

Amax=(Vmax).*(Wmax);

% approximations for magnitude limits

Ncf=8;

Ncs=8;

alphas=2*pi/Ncs;

alphaf=2*pi/Ncf;

% start of writing file

c=0;

fid=fopen('Trajectory.dat','w');

c = c + AMPLcomment(fid,'Matlab generated AMPL data file\n');

c = c + AMPLcomment(fid,'');

c = c + AMPLcomment(fid,'For use with model Trajectory.mod');

c = c + AMPLcomment(fid,'');

% convertion of all in two dimensions

c = c + AMPLscalarint(fid,'nDim', 2);

% defining of number of vehicles

nVeh = size(x0,1);

c = c + AMPLscalarint(fid,'N',nVeh);

% defining of number of timesteps

T=Nt;

c = c + AMPLscalarint(fid,'T',T);

% discrete system

Ad = [eye(2),   dt*eye(2);

      zeros(2),    eye(2)];

Bd = [0.5*dt^2*eye(2);

            dt*eye(2)];

% writing of system matrices

c = c + AMPLmatrix(fid,'A',Ad);

c = c + AMPLmatrix(fid,'B',Bd);

% size

[nState,nInput] = size(Bd);

c = c + AMPLscalarint(fid,'nState',nState);

c = c + AMPLscalarint(fid,'nInput',nInput);

% defining of number of obstacles

nObst = size(obst,1);

c = c + AMPLscalarint(fid,'nObst', nObst );

if nObst

    % obstacles