Bachelor Semester Project - UPCommons · Bachelor Semester Project Collision avoidance control...
Transcript of Bachelor Semester Project - UPCommons · Bachelor Semester Project Collision avoidance control...
Ecole Polytechnique Federale de Lausanne
Bachelor Semester Project
Collision avoidance control design
Author:
Marc Mitjans
Professor:
Colin JONES
Supervisor:
Andrea Alessandretti
A report submitted in fulfilment of the requirements
for the degree in Engineering in Industrial Technologies
in the
Escola Tecnica Superior d’Enginyeria Industrial de Barcelona, UPC
July 2014
ECOLE POLYTECHNIQUE FEDERALE DE LAUSANNE
Abstract
Automatic Control Laboratory
Engineering in Industrial Technologies
Collision avoidance control design
by Marc Mitjans
This semester project has been developed at the Automatic Control Laboratory, at the
Swiss Federal Institute of Technology in Lausanne. It is oriented at the implementation
of obstacle avoidance algorithms, more concretely at the development of three of the
most common algorithms using a user-friendly interface to allow third-party consumers
to use them, either for simulation or implementation on real systems.
In the paper attached to this report, the disadvantages of one of the implemented algo-
rithms will be discussed in detail, and the model predictive control method (MPC) will
be described and used to improve its performance to a great extent.
Contents
Abstract i
Contents ii
List of Figures iii
1 Collision Avoidance Algorithms 1
1.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1
1.2 Collision avoidance algorithms . . . . . . . . . . . . . . . . . . . . . . . . 1
1.2.1 Bug Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2
1.2.2 Vector Field Histogram (VFH) . . . . . . . . . . . . . . . . . . . . 2
1.2.3 Bubble Band Technique . . . . . . . . . . . . . . . . . . . . . . . . 5
1.2.4 Curvature velocity methods (CVM) . . . . . . . . . . . . . . . . . 6
1.2.5 Dynamic Window . . . . . . . . . . . . . . . . . . . . . . . . . . . 7
2 MATLAB Implementations 9
2.1 Bug algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
2.1.1 Regular implementation . . . . . . . . . . . . . . . . . . . . . . . . 10
2.1.2 Implementation with VirtualArena . . . . . . . . . . . . . . . . . . 11
2.2 Vector Field Histogram . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
2.3 Potential Field algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
Bibliography 19
ii
List of Figures
1.1 Bug algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
1.2 Ackerman configuration . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4
1.3 VFH algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
1.4 Bubble Band Technique . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
1.5 Curvature velocity method . . . . . . . . . . . . . . . . . . . . . . . . . . 7
1.6 Dynamic Window approach . . . . . . . . . . . . . . . . . . . . . . . . . . 8
2.1 Bug: graph of states . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
2.2 Bug implementation (1): Trajectory . . . . . . . . . . . . . . . . . . . . . 12
2.3 Bug implementation (2): Trajectory . . . . . . . . . . . . . . . . . . . . . 13
2.4 Control input for Bug implementation . . . . . . . . . . . . . . . . . . . . 13
2.5 Trajectory for VFH implementation . . . . . . . . . . . . . . . . . . . . . 14
2.6 Control input for VFH implementation . . . . . . . . . . . . . . . . . . . . 15
2.7 PF implementation (1): Trajectory . . . . . . . . . . . . . . . . . . . . . . 17
2.8 PF implementation (2): Trajectory . . . . . . . . . . . . . . . . . . . . . . 17
2.9 PF implementation (2): Control input . . . . . . . . . . . . . . . . . . . . 18
iii
Chapter 1
Collision Avoidance Algorithms
1.1 Introduction
The aim of this report is to provide the reader with some basic ideas of the most common
strategies used in mobile robotics for collision avoidance. It also contains the description
of the implementation of some of the following algorithms using MATLAB. The main
objective of obstacle avoidance algorithms is to allow the agent to somehow avoid the
different obstructions it might encounter during its search for the goal – and which have
not been taken into account during path planning (e.g. due to mobile obstacles). The
agent’s sensors are crucial during the execution of the algorithms in question, as they are
the main agents responsible for detecting and sending information to the agent about
the obstacle position, and therefore allowing the agent to encircle it and keep searching
for the target 1.
1.2 Collision avoidance algorithms
In the following section some of the most common collision avoidance algorithms will
be described, together with their respective advantages and disadvantages in order to
compare and contrast the different methods.
1Even though the use of sensors is essential in obstacle avoidance, they will not be treated in depththroughout this report. More information on how the agent’s sensors work can be found in [1], Chapter4.
1
Chapter 1. Collision Avoidance Algorithms 2
1.2.1 Bug Algorithm
This is the simplest obstacle avoidance algorithm. Among all the different versions of
this algorithm, Bug1 and Bug2 will be commented below.
Using Bug1, once an obstacle is found blocking the computed path towards the goal, it
makes the agent surround it completely following its contour, and then, based on the
sensors readings, go to the point in the contour of the obstacle which is closest to the
goal and keep the trail from that point.
Unlike Bug1, Bug2 uses a more efficient algorithm, as it doesn’t need to fully surround
the obstacle. The agent keeps surrounding the obstacle’s contour until it finds a position
from which it can keep its trail towards the goal following the same direction as before
encountering the object (in the pre-computed path). An example of the algorithm is
depicted at Figure 1.1
Advantages:
1. It guarantees completeness, as the obstacles are always considered to have finite
boundaries, and thus it will always be possible to surround it, regardless of its size
and shape.
2. Its simplicity allows a fairly easy implementation.
Disadvantages:
1. Both versions of the algorithm are far from being optimal in terms of the path’s
length (although Bug2 will require less travelling to reach the goal).
2. This algorithm and its versions depend only on the present readings from the
sensors and not on previous information, so the agent might sometimes not take
the correct decision for its next movement when trying to avoid certain complex
obstacles (e.g. taking the longest direction to encircle the object).
1.2.2 Vector Field Histogram (VFH)
This second algorithm gets rid of the last limitation commented above. Using the read-
ings of the sensors, an occupancy grid of the recent surroundings of the agent is created
Chapter 1. Collision Avoidance Algorithms 3
Figure 1.1: The trajectories followed by an agent with a Bug1 controller.
in the memory 2. Then, VFH generates a histogram in which for each angle α, with
respect to the position where the obstacle was found, is represented the probability to
find an obstacle in that direction, based on the values of the cells in the occupancy grid.
Then, the candidate paths are selected if their value is less than a certain threshold.
Once the histogram is defined, a cost function (Gi) is calculated for each possible path
in order to help choosing the best direction to approach the goal, and the direction with
the least value will be chosen. The cost function defined in [1] is the following:
Gi = a · direction+ b · orientation+ c · previous direction (1.1)
Where direction refers to the deviation between the direction of path i and the direction
of the goal, orientation is the difference between the direction of the wheels and path
i, and previous direction is the difference between the direction chosen at step n-1 and
the direction of the current evaluated path.
a, b, and c are coefficients for each of the three terms commented above in order to
express different weights which are directly proportional to the importance given to
each of them.
More effective versions of the aforesaid algorithm have been released throughout years.
One of them is VFH+. In this case, the agent’s kinematic constraints (mainly due to its
type of wheels or its ability to drastically change its direction) are taken into account
2The occupancy grid is a representation of the environment surrounding the agent discretized insmall cells, each with an associated value. This value represents the number of times the sensors of theagent detected a potential obstacle in the respective cell. Thus, a greater number will be associated to ahigher probability to encounter an obstacle in that direction, and therefore will prevent the agent fromcolliding.
Chapter 1. Collision Avoidance Algorithms 4
during the plot of the histogram. Thus, one direction which could be considered a
potential path in VFH (such as a perpendicular direction to the fixed wheels of an
Ackerman configuration (Figure 1.2)) will be treated as an obstacle in the histogram
plot and therefore prevent the agent from trying to follow it.
Figure 1.2: The Ackerman configuration for the wheels.
Advantages:
1. To select the new direction, the use of the map allows the agent to take into
account not only the instantaneous sensor reading, but the information obtained
during the previous readings as well. This improvement can allow the agent to
select a better path towards the target.
2. The cost function allows analyzing several options and finally choosing the most
efficient.
Disadvantages
1. The occupancy grid representation requires a considerable big amount of memory
to be able to keep count of the values in each cell.
2. Also, it consists of a local map. In big and concave-shaped objects this can cause
the agent to be bounded to move away from the optimal path.
A third update of the algorithm was created which relatively solves the last disadvantage
of VFH and VFH+. It is called VFH* because of its use of the A∗ algorithm. VFH*
computes the candidate paths in the same way as its predecessors, but instead of applying
the cost function Gi to determine the best path, it computes the next position for each
of the potential paths after having travelled a distance di, and applies A∗ to each of them
(computing for each node the function f(i) = h(i) + g(i), being g(i) a cost function and
Chapter 1. Collision Avoidance Algorithms 5
h(i) a heuristic which indicates the proximity of the path i to a certain objective). This
procedure is repeated n steps, so a graph of length n is created. Then, for the least
costly end node of the graph, the algorithm is able to determine which path has led to it.
Being well-known the optimality of A* search, the improvement from VFH+ to VFH*
can easily be noticed.
Figure 1.3: The histograms shown in b) represent the world configuration in a). Thesecond histogram has deleted those directions which are not possible for the agent to
follow.
1.2.3 Bubble Band Technique
The bubble band technique merges both path planning and obstacle avoidance. The
bubble band represents for a given position the maximum distance between the agent
and the surrounding obstacles in every direction. In path planning, the agent follows a
previously computed path without allowing an excessive distension between bubbles of
consecutive positions, and thus getting to draw smoother trajectories.
When an obstacle is encountered, it acts as a compressive force to the bubble, decreasing
its size and increasing its tension (as if it was an elastic band). The objective of the
controller is to find the position for the next time step which will minimize this bubble
tension.
Advantages:
1. This method can consider the agent’s actual shape.
Disadvantages:
Chapter 1. Collision Avoidance Algorithms 6
1. It generally requires some previous knowledge about the world’s configuration
beforehand in order to compute a first approach for the band at every time step.
Figure 1.4: An example of an implementation using the bubble band technique.
1.2.4 Curvature velocity methods (CVM)
When trying to compute the best path to get to the goal, this method and its variations
allow taking into account not only the sensor’s readings, but also the kinematic and
some dynamic constraints of the agent itself.
Based on these constraints, the algorithm first creates a set of possible velocities (transla-
tional and rotational) for the next step, called a velocity space. Moreover, it is considered
that the agent travels along arcs of radius r = v/ω.
Among all the velocities contained in the set which are admissible according to the
agent’s constraints, the ones defining a curvature which ends up colliding with an obstacle
are identified and therefore removed from the set. Then, for the valid pairs of velocities
the best trajectory is selected using a determined objective function (maximizing or
minimizing a certain value), similar to the one described in the VFH algorithm.
For a better implementation of the algorithm, all obstacles are simplified to circles in
order to convert them from the Cartesian coordinates to the velocity space, and here
lies its main disadvantage. Some complex obstacles might not be accurate enough once
approximated to a circular object, and this might make the algorithm fail.
Advantages:
1. Takes into account the agent’s kinematic and dynamic limitations.
Chapter 1. Collision Avoidance Algorithms 7
Disadvantages:
1. The circular approximation may not be valid for certain obstacles and environ-
ments.
2. It only considers circular trajectories, so it may not be accurate for path intersec-
tions.
3. The information used for this algorithm is restricted to instantaneous readings.
Figure 1.5: An example of cmin and cmax in a CVM algorithm.
1.2.5 Dynamic Window
Another approach which considers the agent’s kinematic constraints is the dynamic
window approach. It consists of a velocity space similar to the one described above,
formed by tuples of pairs of reachable translational and rotational velocities (v, ω)
during the next time step. Then, the window is reduced deleting all those tuples which
will cause the agent to collide with any surrounding obstacle.
Among all the remaining pair of tuples, the final pair of velocities will be chosen using
an objective function Oi (v, ω) for each pair of tuples i. Similar to the one from the
VFH, this function can be defined as follows:
Oi = a ·Hi(v, ω) + b · Vi(v, ω) + c ·Di(v, ω) (1.2)
Chapter 1. Collision Avoidance Algorithms 8
Where a, b and c are weights; and H, V and D are respectively the progress towards the
target, the magnitude of the speed of the tuple i, and the minimum distance to collision.
A modified version of the algorithm (named global dynamic window approach) adds a
heuristic (called NF1 or grassfire) to the objective function, which for each cell in the
discretized map evaluates the remaining total distance to reach the goal (only in the
direct region between the agent and the target). This improvement applies some of the
advantages of path planning to the collision avoidance algorithm.
Advatnages
1. As well as in the previous method, the dynamic window approach takes into ac-
count the agent’s restriction to determine the possible directions.
2. The global dynamic window approach also has accurate results in high-speed mod-
els.
Disadvatnages
1. It only considers the agent’s motion along circular arcs as well.
Figure 1.6: The dynamic window. The window is made by all the possible velocities(v, ω) the agent can assume, and shows the tuples which will make it collide with the
obstacle.
Chapter 2
MATLAB Implementations
In this section some approximated implementations of the algorithms will be presented,
coded in MATLAB. It is important to mention that the following algorithms have not
been reproduced exactly as described in this report, as there have been included some
improvements and simplifications over the original algorithms 3.
The examples have been implemented using object-oriented programming; that is, one
object has been created for every element needed in the execution of the algorithm: An
agent, a sensor (sonar), an obstacle (a rectangle in this first approach) and a controller.
The object-oriented paradigm allows creating any instance of any class independently,
and later linking all of them automatically. The type of agent used throughout the
implementations is the Unicycle model (without considering uncertainty), which obeys
the following dynamics:
x = v · cos(θ)
y = v · sin(θ)
θ = ω
(2.1)
Where v corresponds to the norm of the forward velocity, θ to the current orientation of
the agent with respect to the global frame of reference and ω to the rotational velocity.
The first and the third algorithms have been designed using two different methods,
and thus the results are slightly different. At first they were created using the objects
independently without implementing the concept of state space, used in control theory.
That is, the Robot instance only indicated the current position, the Sonar was in charge
3All the code corresponding to the implementations can be found in the .zip file that has beenprovided together with this report.
9
Chapter 2. Implementations 10
of taking the measurements and the Controller computed everything that is needed for
the execution of the algorithms: from obtaining the outputs of the Sonar to updating the
position of the agent for the next step, through all the needed computations. Therefore,
the function run of the Controller object has a main loop which ends once the robot
reaches the goal.
This first approach was more focused on finding an easy way to implement the algorithm
in terms of programming, but clearly moves away from the control theory concepts
(which involve a system, an input and its corresponding output). In order to develop
an algorithm more related to control theory, a special MATLAB Toolbox has been used
for this purpose, called VirtualArena. This Toolbox is made of a whole set of objects all
linked among them that, in rough outlines, create an environment which computes the
next position for the agent, using as input vector to the state space system the value
obtained from the object Controller at every step. This type of implementation makes
it easier and more intuitive to implement the designed algorithms on a real system.
The second implementation (for the VFH algorithm) has been directly coded using the
VirtualArena Toolbox.
2.1 Bug algorithm
This first example is based on the Bug2 algorithm. However, there are some considerable
differences between both. The main one is the type of sensors used. In order to follow
the contour of the obstacle the agent is generally provided with tactile sensors (i.e.
the agent gets information from physical interaction), whereas in the implementation a
sonar sensor has been considered more appropriate (to actually avoid colliding with the
obstacle).
2.1.1 Regular implementation
As explained above, Bug2 has two general states: tracking the goal and encircling the
object, so the agent keeps switching between them under certain conditions, as depicted
in Figure 2.1, a.
In our implementation, however, the transition from state 2 to state 1 is reached every
time the agent surpasses one of the edges of the obstacle (Figure 2.1, b). This solution
not only allows smoother trajectories, but also improves the optimality with respect to
the general definition of Bug2, as the agent can go straight to the goal as soon as it has
no obstacles in front of it (always considering a small range of detection).
Chapter 2. Implementations 11
Figure 2.1: The general graph of states that typify Bug2. a) corresponds to thegeneral definition, and b) to the actual implementation.
In order to detect the relative position to the agent of the obstacle, the sonar uses a
function called sense, which gathers for every 5o, from 0o to 360o, the distance from
the agent to the obstacle following that direction, similarly to the way the Vector Field
Histogram algorithm behaves. Then, as soon as it detects an obstacle in front of him,
it starts turning right/left until the closest point to the obstacle is approximately at
±90o respectively (depending on the angle of incidence to the obstacle). At that point
it keeps moving straight until either it gets too close to the obstacle, which will make
it move away from it, or surpasses the edge it was following. During the tracking state
the controller simply computes from a range of rotational velocities the one which will
bring the agent closer to the goal in the next step.
It is important to mention that when used in a simulation this function has a tremendous
disadvantage. As the sonar has to simulate the gathering of data, in order to approximate
the distance to the closest obstacle in a certain direction it samples the space and
checks for every 0,02 units whether the point in question is inside the obstacle or not
(using the built-in MATLAB function inpolygon). As this test has to be executed for
every iteration, the simulated algorithm turns out to be drastically slow, and its speed
decreases with the number of agents using the algorithm at the same time.
2.1.2 Implementation with VirtualArena
In the implementation of the algorithm using the VirtualArena Toolbox, the last dis-
advantage has been considerably reduced, together with the amount of code in the
Controller object, making its principal method, computeInput (which takes as input
Chapter 2. Implementations 12
Figure 2.2: This image shows the trajectory of a simulated agent using the Bugregular implementation. It uses a sonar sensor with a range of 1,5 units. Its initial
position is (-3,0) with an angle of -20o, and the goal state is at (3,1).
the position of the agent), much more comprehensible. In this case, another type of
sensor, called RangeFinder, has been used. Its main difference with the previous one is
the usage of the function findClosestIntersectionWithAPolytope, which finds the closest
distance to an obstacle much more efficiently than the previous sensor.
Then, once the Controller object computes the input vector (which is the Euclidean
distance to the goal for the forward velocity and the angle offset for the angular velocity),
a P controller is applied to the input vector and the new position for the agent is
obtained. The resulted trajectory might draw some sharp edges, due to the control
input (Figure 2.4) for the rotational velocity (which is 10 times the difference between
the current angle and the angle of reference). This strong offset has been introduced to
force the agent to turn drastically enough to allow it to avoid colliding.
Some other features that have been added with respect to the previous implementation
can be perceived in Figure 2.3. On one hand, as soon as the agent has the goal position
closer to itself than the minimum distance to any obstacle it leaves the “following” state
to go to the goal, as there will not be any possibility of collision. On the other hand,
multiple obstacle avoidance have also been included. Any time it detects an obstacle in
front of him, the controller checks in which side there are the most obstructed angles (by
checking the histogram obtained from the sensor), and chooses the other side to follow
the obstacle.
Chapter 2. Implementations 13
Figure 2.3: The path drawn using the VirtualArena Toolbox. The mentioned sharpedges can be noticed, together with multi-obstacle avoidance.
Figure 2.4: The evolution of the control input throughout time.
The only disadvantage between the original definition and our implementation is that
the first one assures completeness (as it can be implemented for single-integrator systems
and the obstacles will always have a finite perimeter), but certain configurations which
for a unicycle model would require very high rotational velocities the obstacles can turn
to be unavoidable.
Chapter 2. Implementations 14
2.2 Vector Field Histogram
This algorithm uses exactly the same strategy described above with a slight modification.
From the histogram obtained by the sensor (which indicates the minimum distances to
collision for every sampled angle) it selects all the angles which apparently indicate
an infinite distance to collision. However, in this case, as the agent is simplified to a
point, it would surpass the obstacles infinitely close to them (as it tries to follow the
most optimal path subjected to the objective function). This cannot be acceptable in
real implementations, as the agent would have a certain shape. In order to avoid this
problem, a safety contour has been added to every obstacle, deceiving the sensor and
thus considering the dimensions of the obstacle greater than they actually are.
Among all the acceptable angles, the objective function described in page 5 is applied
and the best angle is selected. Then, the controller sends as input vector to the object
Robot the Euclidean distance to the goal as speed and the variation from the selected
angle with the actual angle of the agent as the rotational velocity. In both cases, the P
controller is applied to get the new position for the agent.
Figure 2.5: The path followed using the VFH algorithm. The separation between theagent and the obstacles at their corners has been obtained increasing their boundaries.
2.3 Potential Field algorithm
Even though this algorithm is generally used in path planning, it can also be implemented
for obstacle avoidance. This algorithm guides the agent to the goal following an artificial
potential field U(q), where q = q(x, y) is the position of the agent. The total potential
field acting on the agent can be defined as Ut(q) = Uatt(q) +∑
i Urep,i(q), where Uatt
Chapter 2. Implementations 15
Figure 2.6: The evolution of the control input for the VFH throughout time. Thevariations in the input for the rotational velocity are still present, but the values are alot lower than the ones in the Bug algorithm, which allows the agent to draw smoother
trajectories.
is the field created by the goal (attractive) and∑
i Urep,i is the field created by the
obstacles (repulsive). These two fields have been implemented as in [1]:
Uatt =1
2· katt · ρ2g (2.2)
Urep,i(q) =1
2krep
(1
ρi− 1
ρ0
)2
ifρ0 ≥ ρi
= 0 otherwise
(2.3)
ρg is the Euclidean distance to the goal, ρi is the closest distance to an obstacle Oi, and
katt and krep are weight factors. ρ0 is a safety distance: the controller won’t take into
account the obstacle if it is not inside a certain danger zone.
As the expressions above are differential functions, it is possible to get the virtual force
that generates the defined potential field: ~Ft = −∇Ut. This results in the total force:
~Ft(q) = −katt · (q − qg) + ~Frep (2.4)
~Frep(q) = krep ·(
1
ρi− 1
ρ0
)· 1
ρ2i· q − qi
ρiifρ0 ≥ ρi
= 0 otherwise
(2.5)
The first term corresponds to the attractive force and the second one to the repulsive
force, and qi corresponds to the position of the closest point of the obstacle with respect
Chapter 2. Implementations 16
to the agent. Notice that the closer the agent gets to the goal, the lower the value of ~Ft
gets, which will finally converge to 0. Note also that the function ~F (q) is an Euclidean
vector, as q = q(x, y), and evaluated at q0 = (x0, y0) it will results in the greatest
descending gradient of the potential field Ut at the current position q0.
In order to compute the repulsive field Urep, two different ways of gathering the data have
been considered. In the first one, to get ρi in MATLAB the algorithm uses a method
called minSense (which at the same time uses a function called distPoly4) to compute
the closest distance from a given point to any polygon (in this case, the obstacles),
and the final repulsive field will be the sum of the fields generated by every obstacle.
This method allows a fast simulation and the consideration of several repulsive forces.
However, it is not realistic, as this behavior does not correspond to the one of a real
sensor. Thus, a second version has been designed in which the histogram of distances
thats outputs the sensor is taken into account, and one repulsive force is computed
considering the closest distance to collision. Even though this method produces slower
simulation results and cannot take into account the superposition of reoulsive forces, its
behavior is closer to a practical example.
To compute the function corresponding to the potential field and its corresponding
gradient, the Symbolic Math Toolbox for MATLAB has been used. At every step the
new potential field function is computed, taking into account the new distance to the
obstacles. The control input for the rotational velocity is the difference between the
current orientation of the agent and the direction of the negative gradient of the field
Ut, and the forward velocity corresponds to ‖~Fatt + ~Frep‖
It is important to mention that depending on the weights in the attractive and repulsive
potential fields and the proportional controller P , the agent may have propensity to
generate oscillatory movements when an edge is detected, alternating between escaping
from it and trying to re-face the goal, until it finds the equilibrium or surpasses the edge.
This algorithm, however, has one big limitation: the existence of one or more local
minima. As the agent tries to follow the minimum gradient of the field, it can be
possible that at some point the sum of all attractive and repulsive forces converges to
zero. In this situation, the obstacle will turn to be unavoidable using the potential field
algorithm. In practice, in order to overcome these local minimas heuristic functions can
be applied to the agent, which can make it move away from the minima using a different
criterion than the virtual forces.
4This function does not belong to any MATLAB R© toolbox, but it has been obtained from thewebsite in [3]: http://www.mathworks.com/matlabcentral/fileexchange/12744-distance-from-a-point-to-polygon
Chapter 2. Implementations 17
For a better performance, it is recommended to use this algorithm when the obstacles
are known to have a great probability to be relatively small, to avoid unnecessary turns.
Figure 2.7: The trajectory followed using the regular implemented version of thePotential Field.
Figure 2.8: The path drawn using the Potential Field controller with the VirtualArenaToolbox.
Chapter 2. Implementations 18
Figure 2.9: The evolution of the control input for the potential field algorithmthroughout time. The variations in the input for the rotational velocity are still present,but their values are much lower than the ones in the Bug algorithm. The evolution of
the forward velocity is due the variation in the norm of Ft.
Bibliography
[1] Roland Siegwart and Illah R. Nourbakhsh, 2004. Introduction to autonomous mo-
bile robots.
[2] J. Borenstein and Y. Koren, 1991. The Vector Field Histogram - Fast obstacle
avoidance for mobile robots. Journal of Robotics and Automotion, June 1991.
[3] George Mason University, Department of Computer Science. Potential Field Meth-
ods.
[4] J. Borenstein and Y. Koren, 1988. High-speed obstacle avoidance for mobile robots.
Intelligent Controll, August 1988.
19
EPFL • June 2014 • Collision Avoidance Control Design
Potential Field Algorithm with MPC
École Polytechnique Féderale de Lausanne
Marc Mitjans
Supervisor: Andrea Alessandretti
10/06/2014
Abstract
The usage of model predictive control algorithms applied to the field of collision avoidance will bediscussed. Our aim is to show the simplicity in the implementation of a commonly used algorithm in thisarea, the Potential Field, together with its main disadvantages, and how the introduction of the concept ofMPC can considerably improve its performance. Some simulation examples are also included to supportthe content of the paper.
I. Introduction
An autonomous agent doesn’t only haveto be able to move around and tracka specific target, but it is also needed
to ensure that in the case which an unwantedobstacle is obstructing the path between theagent and the target, the first one will be ableto recompute another path to allow it to keepits search for the target, together with avoid-ing the collision that would make the trackingfail, or even damage the agent. The collisionavoidance algorithms are the ones to ensurethat such behavior is followed by the agent.
The main contribution of this paper is topresent the Potential Field (PF) algorithm thatis used for this purpose, and how it can belinked with a Model predictive control opti-mization problem to obtain an optimal controlthat not only will be able to overcome previ-ously unavoidable obstacles, but also followingthe shortest path towards the goal. In orderto develop the different concepts that will fol-low, the unicycle model has been consideredas example.
The theory on MPC described in the follow-ing sections will be similar as the one described
in [3], which corresponds to discrete time MPC,whereas [1] and [2] are more focused on con-tinuous time MPC.
The remainder of this paper is organized asfollows: The definition of the PF algorithm isintroduced in Section II, together with a firstapproach to MPC. In Section III the main re-sults are presented. The simulation results areincluded in Section IV. Finally, Section V coversthe conclusive remarks of the study and therelated future work.
II. Problem Statement
Consider the following dynamics for a discrete-time system:
x(k + 1) = f (x(k),u(k)) (1)
Where x corresponds to the state at timesk + 1 and k respectively. Consider also a pro-portional P controller that seeks to drive anagent led by that system to a certain target po-sition xt = xt(x,y). A simple controller for theforward and rotational velocities of a unicyclemodel could be one such that
1
EPFL • June 2014 • Collision Avoidance Control Design
v = ‖~x(q)−~xt‖ω = θerr(q)
(2)
Where ~x(q) is the current position and
θerr = arctan(
yt − yxt − x
)− θ, which corresponds
to the orientation of the agent with respectto the target. It can be easily perceived thatboth control inputs tend to 0 as the agent ap-proaches the target.
This controller gives satisfactory results ifand only if the environment is free of unde-sired artifacts. If that is not the case, the con-troller above should be modified, and the PFalgorithm is introduced for that purpose. Thismethod drives the system towards the targetfollowing the descent of an artificial potentialfield U(q) created by the obstacles and the tar-get, generating repulsive and attractive forcesrespectively. The total field Ut at the stateq(x,y) is defined as follows:
Ut(q) = Uatt(q) + ∑i
Urep,i(q) (3)
Uatt and Urep have to be set in such a waythat the first one decreases the closer the agentgets to the target, and the second one increasesthe closer it comes to an obstacle (and becomesinfinity at its border). As in [4] and [8], aneffective example for both cases can be the fol-lowing:
Uatt(q) =12· katt · ρ2
t (q) (4)
Urep,i(q) =12
krep
(1ρi− 1
ρ0
)2i f ρ0 ≥ ρi
= 0 otherwise(5)
Where ρt refers to the Euclidean distance tothe target, ρ0 to the distance of influence of therepulsive forces, and ρi to the closest distanceto the obstacle Oi. Along the study in this pa-per we will only consider obstacles with an
existing derivative along the whole perimeterof the object. Thus, the closest distance ~ρi willalways be perpendicular to the perimeter ofOi.
Then, the PF algorithm will make the agentreach the target by following the greatest de-scent direction, given by ~Ft =−∇Ut(q).
However, despite the simplicity and effec-tiveness of the aforesaid algorithm, it fails inthose situations were Ut reaches a local minima(i.e. the resultant force ~Ft = ~Fatt + ~Frep appliedto the agent sums up to 0). In other words, ifwe consider the vector~t(q) = ~xt −~x(q) as thevector between the state q(x,y) and the targetxt, and φ(x,y) as the trajectory followed bythe system, a certain obstacle Oi can be consid-ered as avoidable if one of the following twosufficient conditions hold ∀q(x,y) ∈ φ(x,y):
1. 〈 ~t‖~t‖ , −∇Urep(q)
‖∇Urep(q)‖ 〉 6=−1 (6)
2. ‖~t‖ 6= ‖∇Urep(q)‖ (7)
As the force ~Fatt will always be parallel to~t.Then, if ~Fatt and ~Frep are parallel (6) and withthe same norm (7), the system will reach a localminima.
III. Main Results
I. Introduction to MPC
In order to fully understand the link betweenthe PF algorithm and MPC that is reached atthe end, a small introduction to discrete timeMPC must be introduced.
As described in [1], Model predictive con-trol is a control method in which the closed-loop system is obtained at every iteration bycomputing an open-loop optimal control prob-lem in a finate horizon length N with the cur-rent state as the initial state. This problem leadsto a finite sequence of optimal control inputs,the first of which is applied to the system, andthe whole procedure is repeated again for the
2
EPFL • June 2014 • Collision Avoidance Control Design
new state of the system. Its main advantagewith respect to offline control computation isthat it allows to adapt the control problem tothe current state of the system at each instant.
The system to be controlled usually has thefollowing general definition:
x(k + 1) = f (x(k),u(k)) (8)
x(k) and u(k) must satisfy the followingconstraints:
x(k) ∈X (9)
u(k) ∈U (10)
Where U and X are usually convex andclosed sets which contain the origin. As wecan consider the current state of the system asthe initial state, (8) can also be expressed asx+ = f (x,u), where x+ corresponds to the im-mediately next state. In order to determine thesequence of control inputs, u(x), the followingobjective function is defined:
V(x,u) =N−1
∑i=0
`(x(i),u(i)) + m(x(N)) (11)
Which leads to the following optimizationproblem:
Vo(x,uo) = minu
(V(x,u)) (12a)
subject to : x(k) ∈X (12b)
u(k) ∈U (12c)
x = {x0, ..., xN} corresponds to the sequenceof states x under the effects of the sequenceof the control inputs u(x) = {u(0; x), ...u(N −1; x)}. The functions `(x,u) and m(x(N)) arenamed stage and cost functions respectively.
This optimization problem leads to theoptimal value Vo(x,uo(x)). Then, the firstelement from the optimal control sequenceu∗(x) = uo(0; x) is applied to the system. Note
that uo(x) refers to the sequence of opti-mal control inputs at the current state, but{uo(1; x), ..., uo(N− 1; x)}might not be the finaloptimal sequence in future states.
In order to assure Lyapunov stability (i.e.that the algorithm converges to the optimal so-lution) V(x,u) must be a Lyapunov function.As in [5] and [6]:
Theorem 1. A function V(x, t) is said to be expo-nentially stable if and only if there exists an ε > 0and satisfies the following criteria:
α1(‖x‖2)≤V(x, t)≤ α2(‖x‖2) (13a)
V(x, t)≤−α3(‖x‖2) (13b)
‖ δVδx‖ ≤ α4(‖x‖) (13c)
For αi belonging to class K functions and‖x‖ ≤ ε.
Which means that it has to be strictly de-creasing and 0 at x = 0. To prove this criterionin a discrete time system ([3]), let’s assumethat there exists a suboptimal control inputuk which drives the system from the state xN
to the state xN+1. Then, since the control se-quence uo(x) = {uo(1; x), ...,uo(N − 1; x)} is al-ready feasible, adding the last term uk to itwill make it remain feasible. With this, theresulting control sequence that would drivethe system from the state x1 to the statexN+1 will end up being the following: u(x) ={uo(1; x), ...,uo(N − 1; x),uk}. Then, the valuefor V(x,u) at x = x+ will be the following:
V(x+, u) = V(x,uo)−m(xoN)− `(x∗0 ,u∗)
+m(xN+1,uk) + `(xoN ,uk)
(14)
And (13b) can be expressed as:
V(x+, u)−V(x,uo)≤−α(‖x‖2) (15)
3
EPFL • June 2014 • Collision Avoidance Control Design
II. MPC vs Potential Field
In order to assure Lyapunov stability forV(x,u), `(x,u) and m(x) have to be set ac-cordingly. Quadratic functions are an ex-ample of clas K functions, so for this study`(x,u) = ‖x− xt‖2 and m = 0 whill be chosen.
If (15) is replaced with the new values of `and m, the following constraint is obtained:
− ‖x0 − xt‖2 + ‖xoN − xt‖2 ≤−α(‖x‖)2 (16)
The above expression involves that, for ev-ery initial state q0(x,y) 6= 0, the ending pointobtained from the optimization problem mustbe closer to the goal than the initial state.
Now, let’s consider a horizon length ofN = 1. In this case, the optimal state x+ willalways have to be closer to the goal.
At this point, we can define a sufficientecondition for the set of obstacles S such thatthey will allow the optimization problem toconverge to 0 (i.e. avoidable obstacles) for agiven initial condition. Let x−∞ be the real ini-tial state in our configuration. Let also T be thegeometrical line segment between x−∞ and xt:
T = λxt + (1− λ) x−∞ λ ∈ [0,1] (17)
Then, the following definition can be an-nounced:
Definition 1. Consider d as the furthest intersec-tion point from xt between a certain obstacle Oiand the interpolated segment T between x−∞ andxt. Let C be the circumference with the center at xt
and radius r = ‖d− xt‖, and P the perimeter of Oi.Assume that the system is governed by an MPCcontroller with ` = ‖x − xt‖2 and m = 0, and ahorizon length of N = 1.
Then, the mentioned obstacle Oi will be avoid-able if the following sufficient condition holds:
{∀q ∈ P | ‖~q−~xt‖ ≤ r; 〈 ~xt −~q‖~xt −~q‖ ,~n〉 6=−1}
(18)
This definition comes from the assumptionthat the agent is in the worst case scenario,which would be directly in the perimeter ofOi. In this case, if at that point ~n is parallel tothe direction towards the target xt, in the nexttime step the agent wouldn’t be able to enterthe circumference C, which corresponds to theequidistant points to xt, and thus wouldn’t beable to overcome Oi. This statement can clearlybe identified with (6).
Furthermore, consider the soft constraintsformulation for (12):
Vo(x,uo) = minu
(V(x,u) + βI(x)) (19)
Where I(x) acts as an indicator functionand β as a certain weight. In order to make(19) similar to (12), the term βI has to increaseas the agent gets closer to any obstacle, so thatthe system will respect the stage constraintsin (12b). This can be done by selecting thepenalty function I as some repulsive field (i.e.as described in section II). Then, the resultingV(x,u) will become the following:
V(x,u) = ‖x− xt‖2 + Urep (20)
Which is exactly the same definition for (3),as the term ‖x− xt‖2 acts as an attractive field.Considering again that the MPC optimizationproblem seeks to minimize the total value ofV(x,u), at this point we can finally state that anMPC controller with a horizon length of N = 1,a stage cost of ‖x − xt‖2, a terminal cost ofm = 0 and a soft constraint set to move the sys-tem away from a certain position, is equivalentto the Potential Field algorithm.
This statement can allow us to concludethat, considering the optimality for the MPCmethod with the increase of the horizon lengthN, an MPC controller definied with the afore-said characteristics and N > 1 can increase theperformance of the Potential Field algorithm.
Let’s consider now a horizon length of N>1and the MPC defined by the function in (20):
4
EPFL • June 2014 • Collision Avoidance Control Design
V(x,u) =N−1
∑i=0
(‖xi − xt‖2 + Urep,i
)(21)
If now we replace (15) with the new func-tion V(x,y), the following statement is ob-tained:
−‖x0 − xt‖2 −Urep(0) + ‖xoN − xt‖2+
+Urep(N)≤−α(‖x‖2)(22)
Which is equivalent to (16) but with theaddition and substraction of the repulsive po-tential fields Urep at states xN and x0 respec-tively. Then, considering the function in (21)and Definition 1, we can redefine the sufficientcondition of avoidability for a new set of obsta-cles, S′. In this case, a certain obstacle Oi willbe avoidable if
{∀q ∈ P | ‖~q− ~xt‖ ≤ r ∧−∇Ut = 0;
Vo(q,u) < N(‖~q− ~xt‖2 + Urep,q
)}
(23)
And if (22) holds.
IV. Simulation Results
For the following examples, consider the dy-namics of a unicycle model:
x = v · cos(θ) (24a)
y = v · sin(θ) (24b)
θ = ω (24c)
Consider also a P controller similar to (2),defined as:
v = k · ‖~Fatt + ~Frep‖ (25a)
ω =∠ (θ,−∇Ut) (25b)
Where k is a certain constant. This con-troller will drive the system to the target as
long as it doesn’t reach any local minima. Forthis example, two configurations of obstacleshave been set, as depicted in Figure 1 and Fig-ure 2. In both cases, the system reaches a localminima and fails, due to the cancellation offorces.
In the first case, the only repulsive forcegenerated by the obstacle is always parallel tothe trajectory, and thus (6) never holds for anyq(x,y). Besides, as the agent gets closer to thegoal ~Frep keeps increasing, until it reaches apoint where ~Fatt = ~Frep, and therefore v = 0.
In Figure 2 each repulsive force individu-ally is never parallel to~t, but the symmetry ofthe configuration cancels the horitzontal com-ponent, leaving the resultant repulsive force~Frep always parallel to~t, which ends up in thesame way as in the previous case.
Figure 1. Configuration 1. Velocityperpendicular to the obstacle.
Figure 2. Configuration 2. Obstacles placedsymmetrically around the trajectory.
However, when the horizon length is broadenough, the system can find a path that mini-
5
EPFL • June 2014 • Collision Avoidance Control Design
mizes (21), and thus overcomes that local min-ima. Figures 3 and 4 show the path drawn bythe agent with a horizon of N = 6.
Figure 3. MPC solution for configuration 1.
Figure 4. MPC solution for configuration 2.
V. Conclusive remarks and future
work
In this paper the definition of the PotentialField algorithm for path planning and obstacleavoidance has been introduced, together withits main limitation. By including a brief in-troduction to model predictive control (MPC),and choosing the right functions for the stageand terminal costs and the penalty function,we have seen that an MPC with a unitary hori-zon length resembles the description of the PF.This allowed us to conclude that an MPC withthe same characteristics and a horizon of N > 1will make the system overcome some of thoseconfigurations in which the PF failed.
In further studies, the choice of the stageand terminal costs, ` and m, can be treated and
analyzed in greater depth, in order to find a bet-ter optimization problem that allows a betterperfomance (e.g. to get the same results witha smaller horizon length). Also, even thoughthe content of the paper is described for anyrepulsive force Urep, the simulations have beenobtained by applying a known repulsive field(i.e. the obstacles are known a priori). Futurestudies can work on applying the methods de-scribed in this paper to unknown obstaclesdetected directly by the sensor (such as cre-ating tiny round obstacles on the go for everyobstructed position that is detected).
References
[1] Fernando ACC Fortes, 2001. A generalframework to design stabilizing nonlinearmodel predictive controllers.
[2] Andrea Alessandretti and António PedroAguilar and Colin N. Jones, 2013. A modelpredictive control scheme with additionalperformance index for transient behavior.
[3] D.Q. Mayne and J.B. Rawlings and C.V.Rao and P.O.M. Scokaert, 2000. Con-strained model predictive control: Stabil-ity and optimality.
[4] Roland Siegwart and Illah R. Nourbakhsh,2004. Introduction to autonomous mobilerobots.
[5] Stanford University, 2008. Basic Lyapunovtheory.
[6] R.M. Murray and Z. Li and and S.S. Sas-try, 1994. A mathematical introduction torobotic manipulation. Lyapunov stabilitytheory.
[7] Eric C. Kerrigan and Jan M. Maciejowski.Soft constraints and exact penalty func-tions in model predictive control.
6
EPFL • June 2014 • Collision Avoidance Control Design
[8] Adeleh Mohammadi and MohammadBagher Menhaj, 2013. Formation controland obstacle avoidance for nonholonomicrobots using decentralized MPC.
[9] George Mason University, Department ofComputer Science. Potential Field Meth-ods.
7