OPTIMISATION OF A TRAJECTORY INSIDE A 10X10

ROOM BY AVOIDING COLLISIONS AGAINST

OBSTACLES USING MATLAB

OPTIMISATION IN MECHANICS (OPTIM)

Submitted by:
Simone BANCORA
Swaminath VENKATESWARAN
January 2nd, 2017

Professor:
Fouad BENNIS
PROJECT DEFINITION
Before going into the definition of our project, it is necessary to
describe certain terms which are essential as a part of this project.
We look into the world of Optimisation. Optimisation refers to
finding the best possible solution which is defined according to
the objective function (Criteria, Cost) from a given set of feasible
solutions (admissible solutions defined according to the constraints).
The method of optimisation can be implemented manually or
through computer applications like Excel, Matlab etc. The main rule
of optimisation in programming is that it should intervene once the
program works and meets the functional specifications. Concerning
the project definition, we have a room of dimension 10x10
containing some obstacles in the form of 4 circles distributed within
this space and two rectangular walls. Our objective is that when
there exists a trajectory AB (starting from A and ending at B),
it should travel within this 10x10 space by avoiding collisions
with the circles and the walls. To say precisely, we need to find an
optimum trajectory AB such that it doesnt hit or go through the
circles and the walls. The optimisation is successfully implemented
in this project using the fmincon function provided by the Matlab
software. The fmincon is a powerful optimisation function for
finding the minimum of a constrained non-linear multivariable
function. Its syntax is defined by:
X=fmincon(fun,x0,A,b,Aeq,beq,Lb,Ub,nonlcon,options);
where it starts at x0 and attempts to find a minimizer x of the
function described in fun subject to the linear inequalities ! # %. the walls into the room along with the circles and optimised the
x0 can be a scalar, vector, or matrix. Aeq and beq refer to the equality
constraints and if they dont exist they can be set as [ ]. In our
program, the Lb and Ub is set according to our room space of 10x10
and they can be set to [ ] if not needed in general. The options
feature defines the parameters for the algorithm. Instead of defining
all constraints here in fmincon, they can also be called as a function,
which we have actually done in our project. The program structure
is divided into three portions: The objective function, the
constraint function and the main script. The field of interest i.e.,
the room which has the obstacles in the form of circles and walls is
shown below:
FIG 1: PROBLEM DEFINITION
Now our objective would be to optimize a trajectory AB that travels
from left side to the right side without hitting the obstacles. We will
now explain our approaches in the following sections.
THE DISCRETISATION APPROACH
The method that we employed to optimize the trajectory AB is the
method of discretisation. It is basically similar to the 1D meshing
of a line in Finite Element Analysis. The discretisation is done
according to the principle that if there exists a line segment AB and
there is a point M between AB, then the position of M can be found
as:
M = A + x(B-A) 0x1
This is the logic of discretisation for the position of one point M on
a segment AB. In our program, we have made the discretisation
ranging from 0 to 1 in steps of 0.05 and the logic explained in the
figure is implemented in our coding. A for loop is used for the
discretisation and it is shown in the code below:
alpha=0:0.05:1;
for i=1:length(alpha)
M=A+(alpha(i)*(B-A));
end
Before we arrived at the final solution, we started off the work with
a trajectory AB with one guess point and one circle, then we
implemented the logic to multiple guess points and two circles, then
we increased the number of circles to 4 and finally we implemented
trajectory AB. The reason why we went in steps was to basically
understand the method of optimisation and how the logic works for
one obstacle before implementing it on a complete system. We will
explain individually starting from our first step of the project.
(i) ONE CIRCLE AND ONE GUESS POINT
(Annex section 1.1 for codes)
In order to work with the optimisation in Matlab, it is essential to
provide a guess value which may be user input or pre-defined. The
guess point in this case is basically a [1,2] array (PN_0). The
trajectory AB, will be split into segments as APN_0 and PN_0B.
(a) In the objective function, we will minimize the sum of the
norm of the segments APN_0 and PN_0B.
(b) In the constraint function, which is the critical part of our
program, we first do the method of discretisation in steps of
0.05 ranging from 0 to 1 for segments APN_0 and PN_0B.
Thanks to the for loop, we can successfully discretize the
segments. Now, the primary step in the method of
discretisation is to calculate the distance for every M point on
both the segments with respect to the centre of the circle
defined. These distances are calculated after the discretisation
of the segments, in the same loop. With the help of some arrays
we store these distance values. Once the looping ends, we now
extract the distances that are minimum with respect to the
centre of the circle on both the segments. The constraint will
be an inequality form which ensures that this minimum distance
is at least equal to or greater than the radius of the circle.
(c) In the main script, we define the guess point PN_0, co-
ordinates A, B and plot the circle using viscircles. Using
fmincon, we run the optimisation technique as it calls the
objective function and the constraint function. The Lb and Ub
in our case is [0 0] and [10 10] pertaining to the dimensions of
the room. A hold on function after viscircles helps to
superimpose the plots of the segments APN_0 and PN_0B and
also the initial guess points at the end of the program. If we see
the results, the optimum point is found in such a way that it
1 | P a g e
 passes tangentially or away from the circle thanks to the
constraints defined. The guess point is now shifted to an
optimum point as per our objective. The result of this approach
is shown below:

FIG 3: TWO CIRCLES, MULTIPLE POINTS

(iii) FOUR CIRCLES AND MULTIPLE GUESS POINTS

(Annex section 1.3 for codes)
FIG 2: ONE CIRCLE, ONE GUESS POINT
Here we implement all the four obstacles into the 10x10 room. The
(ii) TWO CIRCLES AND THREE GUESS POINTS guess points are no more 3 but instead we have 5 guess points ([5,2]
(Annex section 1.2 for codes) size array). Hence the trajectory AB will now be split as
APN_0(1)..PN_0(5)B. In other words, we now have six
Now that we were able to understand and implement the segments. Even though, we found the technique to optimise the
optimisation of a trajectory for a single guess point and a single circle trajectory against two circles in the previous section, it can also be
we now extend the logic to two circles and three guess points. The extended to four circles. But the problem is, the coding was done
guess points array PN_0 is now of size [3, 2]. Hence the trajectory step by step for each segments and each circles. If we adopt the same
AB now will be split into four segments as APN_0(1), method, we can achieve the results but to code for multiple circles
PN_0(1)PN_0(2), PN_0(2)PN_0(3) and PN_0(3)B. will be highly tedious and time consuming. Even in the objective
(a) In the objective function, the logic is similar to the previous function, it will be tedious to find the norms of individual segments.
case but here we find the sum of the norms of the four In order to avoid this problem, we used a system of nested for
segments. loops to reduce and generalize the code. A comparison between the
(b) In the constraint function, we run the discretisation segment old lengthy code and the new reduced loop is shown below:
by segment within a single for loop. The coding is done
individually for each of the segments. Also, we calculate the
distance of each point on each segment individually with
respect to the two circles within this same loop. At the end of
this loop, with the help of multiple array storages, we extract
the values of the minimum distances on each segment from
each circle in individual lines of codes. Also, the constraint
function is similar to the previous wherein the minimum
distances are at least equal to or greater than the radii of the
circles.
(c) In the main script, we define the array of the guess points, co-
ordinates of A, B. But here we employ two viscircles since
we have two circles. With the help of fmincon, the optimisation
of the trajectory is done and followed by the plots of the results
of the segments and the optimum points. The program is FIG 4: CODE REDUCTION
enormously big and consumes more time for coding as the
logic is defined step by step in order to ensure that the system (a) In the objective function, the code is modified with the help
works effectively and also to have a clear understanding of the of a for loop. Before the loop starts, we initialize an array L
optimisation technique. The results obtained are shown in the which stores the A as its first value, B as its last value and the
next figure: guess point values in between them. Now the looping starts
which calculates in steps the norms of segments starting from
APN_0(1) to PN_0(5)B. At the end of the loop, we calculate
the sum of these norms which will be our objective function.

2 | P a g e

 This technique is simpler and optimised and can be
extended to n number of points.
(b) In the constraint function, we now no longer run the code
segment by segment or circle by circle. We have made a nesting
of two loops within one main for loop where the main for
loop runs for all points including A and B, the first nested for
loop runs for all circles and the next nested loop does the
discretisation. Within the second nesting, we do the
discretisation segment by segment and calculate the distances
from each circle. At the end of this loop, we find the minimum
distances with respect to all circles and the main for loop ends.
After this, with the help of another for loop, we calculate the
constraints with respect to the radii and centre of circles one by
one. Also, the co-ordinates of A, B, C, R and the guess points
PN_0 are defined only in the main script and will be called as
parameters in the constraint and objective functions using
function handles.
(c) In the main script, the guess point array PN_0 is defined
followed by the co-ordinates A,B. With the help of four
viscircles commands, we plot the desired circles. The fmincon
is then made to run as it calls the objective and the constraint
functions. The results are now plotted with the co-ordinates of
AB and the guess points along with the circles using hold on.
The result obtained is shown below:
FIG 5: FOUR CIRCLES ARE INTRODUCED (a) In the constraint function we did some modifications on the
(iv) PRE-FINAL SOLUTION
(Annex section 1.4 for codes)
Now, we found the solution to optimise a trajectory AB with some
guess point values against 4 circles. But the problem has
rectangular walls to be introduced into the coding and ensure that
the trajectory doesnt hit these walls either. The solution for this
method is explained below:
(a) In the objective function, there is no significant changes in the
coding, the same set of coding defined in the previous section
will be the final one.
(b) In the constraint function, there is no change concerned with
the discretisation technique, distance calculation and
constraining the minimum distances. We introduce now two
additional constraints that concerns the walls. What we do here
basically is that with the help of a variable we extract the value
of the middle position in the guess point array PN_0. For
example if we have the size as 8, the rounded value that is
extracted will be 4. For ease of calculation, we assumed that two
of the guess points have to pass in between the walls. So
the middle value in the guess point array and its successive point
will be made to pass between the walls without hitting them i.e.,
in our case it will be the fourth and fifth points in the guess
points matrix. The constraint here is achieved by fixing two co-
ordinates between walls. The trajectory will now be strictly
made to pass through the fixed co-ordinates between the
walls.
(c) In the main script, there is no significant change when
compared to the previous section except that we introduce the
command rectangle in order to plot the walls. The results for
this approach is shown below:
FIG 6: FIXED POINTS BETWEEN WALLS
(v) FINAL IMPROVED SOLUTION
(Annex section 1.5 for codes)
Now that we almost implemented the solution for the definition of
our project, we made some changes to the previous section in order
to have a better solution and clarity. Basically we dont want to fix
two points to cross the wall, but we can obtain a better solution by
allowing a range of space between the walls. Below are the steps
implemented to obtain the final results:
constraints of the walls. We now make the middle point in the
guess point array and its successive guess point to pass along
the vertical boundary lines of the walls.
(b) In the main script we used some refinement on Lb and Ub, so
that the middle point in the guess point array and its successive
guess point now no longer strictly pass through fixed co-
ordinates between the walls but instead can go through open
range in between the walls.
We also added the possibility to randomly generate the initial
points with the help of rand rather than giving individual values
point by point. To use both the pre-defined points and these
new random points, in the final program we introduced a
prompt that gives the possibility to choose which set of initial
points to use (pre-defined or random).
The optimum path line (continuous line) along with the non-
optimum path line (dotted lines) are generated in the result in
order to understand how the path is optimised. The results for
this approach is shown in the next two figures, with both pre-
defined and randomly generated initial points:
3 | P a g e
 1. Check the extreme points F and G of the segment: if they fall
inside the circle (FC or GC < R), move them outside.
2. Calculate the position of the point (N) which is the normal
projection of the centre of the circle on the segment.
3. Evaluate if this point N falls on the segment between F and G
4. If N falls between F and G, constrain the distance of the
segment from the centre (NC) to be R.
5. If N doesnt fall between F and G, no constraint is applied on
the distance.
We implement this logic in the constraint function where we
eliminate the for loop which is used for discretisation and use an
if-else condition to express this logic. To check if the new
constraint is working properly, a non optimized test environment
is created to simulate all the possible cases:

FIG 7: FINAL DISCRETE SOLUTION WITH PRE-DEFINED INITIAL POINTS

FIG 9: CONTINUOUS TEST GROUND (Annex section 2.1 for codes)

After the successful testing, this constraint logic is optimized by

for loops to be written in a general form and it is applied for all
segments and all circles in the main program. The other parts of the
program including the constraints of the walls remain the same.
Here is the final result with both pre-defined and randomly
generated initial points:
FIG 8: FINAL DISCRETE SOLUTION WITH RANDOM INITIAL POINTS

AN ALTERNATIVE METHOD:
THE CONTINUOUS APPROACH
Now that we have successfully implemented the method of
optimisation for a trajectory AB inside a 10x10 room by avoiding
collisions against circles and walls by the method of discretisation,
we now follow another approach where we no longer do the method
of discretisation. In other words, we employ a continuous
approach on trajectory AB, where the segments are no longer
discretised. AB will be as usual split into segments based on the
number of guess points. To calculate an optimal trajectory, we attain
the objective of making the segments of AB to pass tangentially or
away from the circles, but only in the case that a segment would
have crossed the circle. This change will affect the logic of the
constraint function.
To explain the solution we found, we will start with a simple
example: lets take one segment FG and one circle of centre C and
radius R. We want to make sure FG does not cross the circle, but
FIG 10: FINAL CONTINUOUS SOLUTION WITH PRE-DEFINED INITIAL POINTS
avoid to move FG away if there is no need to. We will achieve this
by using the following logic:
4 | P a g e

 INSTRUCTIONS TO RUN THE CODES:
The programs relative to the various steps described have been
collected in the folder Codes. It is possible to see that the code
experienced a progressive improvement from the first exercise to
the last. The final programs for the two approaches are in the 5.Final
Discrete and in the 7.Final Continuous folders named respectively
final_discrete.m and final_cont.m, both working with the
option to use a pre-defined matrix of points and a randomly
generated one. If Pre-defined is selected, a matrix of points PN_0
will be used with the following values:
PN_0=[8 8; 3 7; 3.5 3; 5 5; 5.5 4.5; 7 3.5; 6 1];
If Random is selected instead, a (7,2) matrix of values will be
randomly generated with a fixed rng value. In the folder Master
Program (Discrete and Continuous) it is possible to find one last
program, which is called Optim_project_both_methods.m
(Annex 3 for the codes). This code includes both the discrete and
the continuous approaches in the same program, and the method
to be used can be decided at startup.
FIG 11: FINAL CONTINUOUS SOLUTION WITH RANDOM INITIAL POINTS
(Annex section 2.2 for codes)

CONCLUSION AND FUTURE WORKS

The optimisation technique was successfully implemented on a
trajectory AB which has to travel from one side of a room to the
other by avoiding collisions against the circles and walls. We were
able to implement effectively the discrete approach for the
segments and understand how the system works when it is divided
into elements. Also, an alternative approach was implemented
wherein the discretisation is no longer done but we use a
continuous approach instead. After implementing the discrete FIG 12: DISCRETE OR CONTINUOUS APPROACH
method, it was easier to address and successfully implement the
continuous method by doing some modifications to the codes.
While successfully calculating some satisfying trajectories for the
problem, both the discrete and continuous solutions proved to be
very much depending on the initial points positions. In some
cases, when generating too many random points, the algorithm
failed to find a good solution, and output some redundant
trajectories. This is due to the fact that the points cannot make
jumps around the obstacles, because this would require a local
temporary increase in the function value and the algorithm will not
allow it: this is the limitation of this fmincon approach. To
overcome this limitation and further improve the solution, some
stochastic optimisation techniques like Simulated Annealing
would have to be implemented in order to obtain some more solid
solutions and further explore the optimisation techniques in-depth.

FIG 13: PRE-DEFINED OR RANDOM INITIAL VALUES

5 | P a g e