Bachelor Semester Project - UPCommons · Bachelor Semester Project Collision avoidance control design Author: Marc Mitjans Professor: Colin JONES ... This semester project has been

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


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.


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


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:


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.


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)


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).


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


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.


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.


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


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


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


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.


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


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


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


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


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


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


[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

Bachelor Semester Project - UPCommons · Bachelor Semester Project Collision avoidance control design Author: Marc Mitjans Professor: Colin JONES ... This semester project has been

Documents