Cooperative Navigation for Car-Like Vehiclesemotion.inrialpes.fr/fraichard/publications/... · Introduction In the last decade, there has been a quickly growing literature on planning

Grenoble INP – Grenoble Institute of Technology – ENSIMAGEcole Nationale Superieure d’Informatique et de Mathematiques Appliquees de Grenoble

Master 2 MoSIG ProjectGraphics, Vision and Robotics

Performed at I.N.R.I.A.– Grenoble - Rhone Alpes

Cooperative Navigation for Car-LikeVehicles

Submitted by : Juan Alberto LAHERA PEREZ

August 30, 2009

I.N.R.I.A. Stage Supervisor655 av de l’Europe Dr. Thierry FRAICHARDMontbonnot School Tutor38334 Saint Ismier Cedex France Dr. James CROWLEY

2

Contact Information

Stage supervisor School tutor

Dr. Thierry FRAICHARD Dr. James CROWLEY

[email protected] [email protected]://emotion.inrialpes.fr/fraichard/ http://www-prima.inrialpes.fr/Prima/Homepages/jlc

Montbonnot Montbonnot

38334 Saint Ismier Cedex France 38334 Saint Ismier Cedex France

Student

Juan Alberto Lahera Perez

[email protected]. +33 650 40 61 45

Acknowledgements

Thanks to my tutor Dr. Thierry Fraichard for his guideness and remarks during the project.

Thanks to my collegues Luis Martinez, Vivien Delsart and Antoine Bautin for their patience,invaluable help and remarks.

Thanks to my friends Jorge Pena, Teresa Siso, Tejo Dallejo and Anne Marie forbeing always beside, supporting me during this year in France.

Contents

1 Introduction 1

2 Definition of the problem 22.1 Definition . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22.2 Assumptions and scope of the problem . . . . . . . . . . . . . . . . . . . . . . . . . 22.3 Objectives and constraints of the problem . . . . . . . . . . . . . . . . . . . . . . . 3

2.3.1 Constraints . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4

3 State of the Art 53.1 Motion Planning for a single robot . . . . . . . . . . . . . . . . . . . . . . . . . . . 5

3.1.1 Deliberative approaches . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63.1.2 Reactive approaches . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7

3.2 Collaborative planning for multi-robot systems . . . . . . . . . . . . . . . . . . . . 83.2.1 Centralized Vs Decentralized methods . . . . . . . . . . . . . . . . . . . . . 83.2.2 Priority based approaches . . . . . . . . . . . . . . . . . . . . . . . . . . . . 93.2.3 Coordination Methods . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9

4 Walking towards a solution 124.1 General Features of our solution . . . . . . . . . . . . . . . . . . . . . . . . . . . . 124.2 Overview of the solution . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13

4.2.1 Environment model construction . . . . . . . . . . . . . . . . . . . . . . . . 134.2.2 Three Layers Architecture . . . . . . . . . . . . . . . . . . . . . . . . . . . . 144.2.3 Remark about Trajectory Planning . . . . . . . . . . . . . . . . . . . . . . . 19

5 Solving the problem: Definitions & Methods 205.1 Environment model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20

5.1.1 Definitions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 205.1.2 Lanes & Streets . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 215.1.3 Objective of TG and GG Structures . . . . . . . . . . . . . . . . . . . . . . 21

5.2 Deliberative Model-Driven Layer . . . . . . . . . . . . . . . . . . . . . . . . . . . . 225.2.1 Why a Trajectory planning ? . . . . . . . . . . . . . . . . . . . . . . . . . . 245.2.2 Trajectory Planner: Tiji . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 245.2.3 Tiji in the Deliberative Layer . . . . . . . . . . . . . . . . . . . . . . . . . . 27

5.3 Urban Conflicts . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 325.3.1 Definitions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 325.3.2 Steps to calculate the conflict regions & Complexity . . . . . . . . . . . . . 34

5.4 Communication-Driven Layer & Aid-Coordination Framework . . . . . . . . . . . . 355.4.1 External surveillance system . . . . . . . . . . . . . . . . . . . . . . . . . . 365.4.2 Priority Evaluation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 395.4.3 Interval Calculation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 415.4.4 Tiji in the CDL Trajectory Planning . . . . . . . . . . . . . . . . . . . . . . 425.4.5 Pilot surveillance system . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 435.4.6 Platooning . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44

5.5 Sensor-Driven Layer . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 455.5.1 Trajectory planning: RRT . . . . . . . . . . . . . . . . . . . . . . . . . . . . 47

4

CONTENTS 5

6 Tests & Results 496.1 Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50

7 Conclusion 537.1 Contribution . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 537.2 Objectives Achievement . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 537.3 Future Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54

A Continuous Curvature Profile & Path planning 55A.1 Local Path-Planner . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 56A.2 Definition and properties of Clothoids . . . . . . . . . . . . . . . . . . . . . . . . . 58

List of Figures

2.1 Multiple autonomous car-like vehicles navigation problem in Grenoble streetmap. . 3

3.1 (a) Two agents with their paths from start to goal. (b) The coordination diagram (c)The partition of the diagram induced by the path decomposition. (d) The boundingbox representation of the obstacles and a solution path. . . . . . . . . . . . . . . . 10

3.2 Cylindrical structure of the Coordination Diagram. . . . . . . . . . . . . . . . . . . 103.3 (a) Two vehicles facing a crossroad, the first agent with path s1 and an agent f with

a fixed motion. (b) The Path-Time Diagram of the corresponding agents. . . . . . 11

4.1 General schema of the multi-robot navigation problem and multi layer resolution. . 134.2 Three Layers Architecture, which represents a global solution for the multi-agent

motion planning problem. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 154.3 State Diagram of an agent in the system. . . . . . . . . . . . . . . . . . . . . . . . 154.4 The external system is represented by the dotted arrows relationship between agents-

crossroads, and the pilot surveillance is achieved in an exploration area around thevehicle. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18

5.1 Translation from a Geometric Graph representation (a) to a Topological Graphrepresentation (b): The edge 1 of the TG will correspond with a list of edges (1, 2,3, 4, 5, 6) and nodes (b, c, d, e, f) of the GG, the node n will correspond with thenode a, and the node m with g. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20

5.2 We will symbolize the bidirectional relationship between adjacent lanes by means ofa dotted line. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21

5.3 Environment Representation with different lane configurations. . . . . . . . . . . . 225.4 (a) We have a Geometric Graph with the Start (S) and Goal (G) nodes for a vehi-

cle. (b) We obtain the corresponding Topological Graph from the initial GeometricGraph. (c) The path-finding method on the TG gets a resulting route. (d) Thanksto the relationship between graph elements, we get a raw polygonal path in the GG,which we call itinerary ΠA. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23

5.5 Different steps of the computation of the linear acceleration and velocity profile.Herein, it is depicted how the velocity and acceleration profiles of the control aretruncated where the bounds defined by the motion constraints, h(s, u) ≤ 0, areoverreached. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27

5.6 (a) The initial trajectory between s0 and sg defined by the distance δ, has a collisionwrt. the environment structure. (b) Application of Dichotomic Tiji algorithm tosubdivide the path until we find a free-collision and feasible trajectory between s0and sg. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31

5.7 Reactive Layer Architecture. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 335.8 Two intersecting vehicle traces, with CAi = [xAi

1 , xAi2 ] and CAj = [xAj

1 , xAj

2 ] respec-tively. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35

5.9 Conflict Regions in a crossroad between straight edges forming an angle α, indicatedby means of MFD(A1,A2) and MFD(A2,A1). . . . . . . . . . . . . . . . . . . . . . 36

6

LIST OF FIGURES 7

5.10 (a) Collision and proximity to non-return point (n.r.p) is detected. (b) The systemsends a coordination order. (c) The agents accept the coordination order. (d)After the priority evaluation, a Path-Time Diagram method is achieved betweenthe vehicles, following the corresponding priority assignment. . . . . . . . . . . . . 38

5.11 (a) Conflict Region definition between the agents A1, A2 and A3. We can appreciatethe non return points and the Maximum Forbidden Distances for each one of theagents. (b) Assuming the priority order: (A1, A2, A3), it is depicted a Path-TimeDiagram resolution for each one of the agents. . . . . . . . . . . . . . . . . . . . . . 39

5.12 Range cone of permissible slopes (velocities). . . . . . . . . . . . . . . . . . . . . . 425.13 Projection of the probabilistic model of gaussian distribution (on the plane path-time) 435.14 Flock Formation of robots changing shapes . . . . . . . . . . . . . . . . . . . . . . 445.15 Examples of flock formations depicted by means of a control graph structure. . . . 455.16 l-ψ control. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 455.17 Two different examples of short range environment reconstruction by means of oc-

cupancy grid mapping method. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 465.18 Two examples of RRT Trajectory Planning. . . . . . . . . . . . . . . . . . . . . . . 48

6.1 (a)Different Agents driving along the road in different states according to theircolor. (b)Example of Platooning Coordination (in orange) in a traffic jam situation.(c)Example of Conflict Region Coordination (in a crossroad), where several agentstake the corresponding “Coordination” state (in red) and modify their velocity pro-file in order to avoid a collision. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50

6.2 Table of values of the performance features. . . . . . . . . . . . . . . . . . . . . . . 506.3 Evolution of the performance features wrt. the number of vehicles in the system. . 516.4 Distribution of the number of vehicles wrt. the type of coordinations encountered

along the way. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52

A.1 car-like vehicle. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 55A.2 (a) Curvature profile of SCC-path in c (b) Derivative of the curvature profile of

SCC-path in c (c) SCC-path of type C+σ(1) L(2) C−σ(3) S(4) C−σ(5) R(6) C+σ(7). 57A.3 Cloithoids (or Cornu Spirals) function. . . . . . . . . . . . . . . . . . . . . . . . . . 59

Abstract

Cybernetic Transport Systems (CTS) based on fully automated urban vehicles will be seen oncity roads and on new dedicated infrastructures. Such systems have been developed and evaluatedin the scope of the European CyberCars (www.cybercars.org) and CyberMove (www.cybermove.org)projects and are now being deployed. However, presently these CTS can only operate in lowdemand environments where little interaction between vehicles is anticipated. In order for thesesystems to address high demands, more cooperation between vehicles is needed. A new cooperativenavigation framework is needed, based on vehicle-vehicle and vehicle-infrastructure communica-tions and vehicles coordination.

Chapter 1

Introduction

In the last decade, there has been a quickly growing literature on planning and cooperativecontrol for systems of many robots. As the costs of individual robots are decreasing, and therobots are getting much more compact, more capable, and more flexible, with sensing systems thatwill handle outside information faster and more accurately, it becomes easier and more interestingto consider groups of fleets of robotic agents interacting between them and our world to resolveproblems. In [INS01] some testbeds in Multi Robot Systems are described as a proof of the increaseof multi agents problems joint resolution, where the only interaction of an individual agent is notenough or sufficient whereas with several robots it is possible to apply “divide and conquer”strategies in decomposable tasks and reduce the complexity of a problem. Thus we can findforaging applications (rescue and search operations like toxic waste cleaning, mine cleaning, objectpicking, hazardous inspection, underwater or outer space exploration), multi target observation,object interaction, exploration and flocking, collaborative games like robot-soccer, or traffic controlapplications. The last one can be viewed as a problem of resource conflict, interaction of fleets ofvehicles and efficient traffic balance. The autonomous management of several robots achieving thistraffic control task by means of coordination and communication in static or/and dynamic obstaclesenvironment is a potential asset to be studied and already present in the ongoing researches inautonomous robotics field. This will be our branch of study in this work.

The objective is to get an overview of the different approaches in the multiple autonomous robotsnavigation problem and propose a solution for a structured environment. Thus, we will define theproblem, our scope, constraints and objectives. A state of the art will be exposed with a properclassification of classical and ongoing methods. Afterwards we will discuss some of these approachespointing out on their advantages and drawbacks and trying to outline a possible solution based onthe bibliography. Finally we will propose a solution and announce a study direction based on themost suitable methods for our specific problem.

1

Chapter 2

Definition of the problem

2.1 Definition

Our objective is to propose an approach for the multiple autonomous car-like vehicles nav-igation problem . The context of our study is a static or/and moving obstacles environmentwith an undefined number (and could be large) of autonomous car-like agents (with dynamic con-straints) that we can control, we will consider them our own fleet of vehicles in the same workspace,we will refer to them as agents from now on. We will call dynamic obstacles the other car-likevehicles in movement that we can not control. Each one of our agents have a start and a goalpoints within the environment, they have to reach their objectives computing feasible trajectories,while avoiding mutual collisions, dynamic obstacles collisions, and static obstacles collisions. Thecoordination is a key point in this context, thus a multi robot navigation and coordination prob-lem consists of exchanging motion and decision information between robots (under considerationof their limited sensing and communication capabilities and taking into account the environmentmodeling) to help them in their choice of trajectories, efficient collision detection mechanisms andsuitable collision avoidance methods.

In Figure 2.1 we can see a possible illustration of the mentioned problem.

2.2 Assumptions and scope of the problem

1. The problem will be placed in a structured environment (urban environment) in a 2Dworkspace.

2. The agents have several suitable capabilities in terms of:

• Sensing: The agents can build a local model of their environment (with regular update)identifying static and dynamic obstacles within a defined range. It is assumed accuratesensing capabilities with no failures.

• Communication: The agents are capable of broadcasting their current state and motiondata, like position, velocities, etc, to other systems (other agents, infrastructure system).It is assumed a communication with no failures.

3. A knowledge about all the vehicles in the system is assumed:

• Agents: It is known the current number of agents within the environment, their corre-sponding start and goal points, and their current data state by means of their commu-nication system.

• Dynamic Obstacles: It is known the current number of dyn. obstacles within the envi-ronment, and a model of future wrt. their motion. In our solution it will be assumeda deterministic model (we will know the position and velocity profile of each vehicle ineach instant by means of broadcasted data communication). Nevertheless, a probabilis-tic model based in gaussian distribution will be presented.

2

CHAPTER 2. DEFINITION OF THE PROBLEM 3

Figure 2.1: Multiple autonomous car-like vehicles navigation problem in Grenoble streetmap.

4. A knowledge about the environment is assumed: the static obstacles, the shape of the en-vironment (forms and boundaries) and the position of all the elements within, (the physicalstructure of a city for example).

5. There is no limit of time for the agents to reach their respective goals.

2.3 Objectives and constraints of the problem

We have to lead each one of our vehicles to its respective goal in a safe way, that is to say, avoidingmutual collisions, dynamic obstacles, and static obstacles.

This is not only a multi robot problem, but a multi robot cooperation problem; while the tradi-tional definition considers the situation of moving the robot in a limited space, where the constraintsare induced by the (non-controlled) obstacles in the environment, here part of the valid configu-ration space for the robots is controlled by the position of the other robots. In this way, we haveto evaluate whether the possibility of sharing information based in communication, sensing andcoordination capabilities among our agents, has benefits or provides some interesting opportunitiesin the final resolution of the multiple autonomous vehicles navigation problem . We will fixthe following objectives:

1. Each agent has to reach its corresponding goal.

2. Travel time minimization.

3. Path length minimization.

CHAPTER 2. DEFINITION OF THE PROBLEM 4

4. Reactivity (low computation complexity).

5. Flexibility

2.3.1 Constraints

1. No collision between agents-agents.

2. No collision between agents-dynamic obstacles.

3. No collision between agents-static obstacles.

Chapter 3

State of the Art

The multiple autonomous car-like vehicles navigation problem could be defined in manydifferent ways. We can find lots of approaches to face similar problems, and we realize that slightdifferences (that actually are not so slight), like the definition of multi-robots (we are talking about anumber of 4 or 200 agents), the type of environment (structured or non-structured environment) orthe characteristics of the vehicles (have all the robots the same features?: size, dynamic constraints,...) can completely change the way and the methods to use in the resolution of the problem.

We will decompose the works in navigation domain in two different parts:

1. Motion planning for a single autonomous vehicle.

2. Collaborative planning for multi vehicle systems.

It is important to describe the work from the point of view of a single vehicle control. We mustunderstand the motion planning for a simple robot before thinking of multi-agent systems. Theexposition of methods for single cases will provide us the possibility to extend individual planningapproaches to group environments, embracing collaborative behaviors where coordination andcommunication are key points to succeed in the final resolution of the problem.

3.1 Motion Planning for a single robot

The motion planning is a large discipline which covers different domains and gives answers fromdifferent points of view: artificial intelligence, control theory, computer graphics, data structures& algorithms, etc. In [LaV04] we can find the reference work in this area; here a deep studyis presented. In a general way, the motion planning can be establish as a suitable choice anddevelopment of a motion strategy for a mobile object. The objective is to define a free-collisionpath or trajectory in a specific environment from a start point qs to a goal point qg. Moreoverthe context and constraints of the problem could be defined, as context requirements, environmentrestrictions, dynamic or kinematic constraints, etc.

Considerable research has been done in this field trying to justify the complexity of motionplanning methods which guarantee some completeness. It has been proved that this is a “difficultproblem” to solve with NP-hard complexity. Schwartz and Sharir [SS83] described a cell decompo-sition method for multiple discs moving among polygonal obstacles. They announce an algorithmbased in critical curves, its complexity is O(n3) for 2 discs and O(n13) for 3 discs.

In [SY84], Spirakis and Yap showed that the problem of moving many discs among polygonalobstacles is NP-Hard. Even the apparently simple problem of motion planning for arbitrarilymotion of many rectangular robots in an empty workspace is PSPACE-Hard [HSS84].

5

CHAPTER 3. STATE OF THE ART 6

We can conclude that methods which guarantee completeness (like centralized methods whereall combinations are explored) lead to a difficult to solve problem with high complexity costs and,for a medium number of vehicles, even untreatable. Thus, other fast processing methods, whichdo not guarantee completeness but probabilistic or heuristic convergence, have been proposed withsatisfactory results.

There exist two big families in motion planning methods:

1. Deliberative approaches

2. Reactive approaches

3.1.1 Deliberative approaches

Deliberative methods are model-driven approaches and involve planning before acting.

Graph based approaches

The main idea of these methods is try to capture the topology of the configuration space with theobjective of simplifying the problem of finding a path in a graph research process. Thus, we builta graph in the Free Space and we add two additional links in order to connect the initial and thefinal configurations. Once the graph is built, path-finding methods can be achieved (A*, Dijkstra,D* [Ste94], [Ste95], [MLT05]) in order to find a connection between start and goal nodes.

Visibility Graph

In these graphs each node of convex hulls belonging to polygonal obstacles, is considered. Eachnode of this set is linked to all the other visible nodes of the set. Then, we link the initial and finalconfiguration to the closest node of the mentioned set. Finally, a planning from start to goal canbe executed.

Voronoi Diagram

We call Voronoi region or Voronoi cell associated to an element p of S to the set of points whichare closer to p than to any other element in S.

V ors(p) = {x ∈ E/∀q ∈ Sd(x, p) ≤ d(x, q)}

For two points “a” and “b” of S, the set Π(a, b) of equidistant points from “a” and “b” is anaffine hyperplane. This hyperplane is the boundary between the set of points closer to “a” thanto “b” and the set of points closer to “b” than to “a”.

Π(p, q) = {x ∈ E/d(x, p) = d(x, q)}

Thus, we define the Voronoi Diagram of a configuration space C as the set of equidistanthyperplanes from the obstacles closest for each one of the Voronoi regions of the free space Cfree.

Once the Voronoi Diagram is defined, we are provided of an induced graph where each nodecorresponds to a vertex of the Diagram edges, and the links between nodes correspond to theedges themselves. As aforementioned, we can execute a path finding method between a initialconfiguration and a final configuration.

Cell Decomposition

The cell decomposition method consists on breaking down the configuration free space Cfree in afinite number of cells (e.g. triangulation). Then, we can define the vertices of a graph with thecentroids of each cell and, eventually, also with the middle points of the neighboring edges. Thelinks of the graph will be the segments joining each centroid to the neighboring cells centroids orthe segments between each centroid or the middle edges points defined. Once the graph is builtwe can apply the same path-finding techniques.


Probabilistic Roadmaps

The construction of probabilistic roadmaps consists of choosing random positions within configu-ration free space Cfree and link them by means of edges to other visible positions in a surroundingarea.

Tree Methods

These methods start with an initial configuration, and little by little, the free configuration spaceis discovered with the hope of joining the final configuration.

Rapidly Exploring Random Tree

The key idea of the RRT’s is the iterative construction of a spanning tree, covering a large part ofthe Configuration Free Space Cfree as fast as possible. This method has been propose by LaValle[LaV04]. The main steps of this algorithm are:

1. Choice of a random configuration qrand in Cfree.

2. We get the nearest existing configuration qnear from qrand.

3. We define the new configuration to add within the tree by means of a displacement ∆q fromqnear towards qrand.

These three steps are iteratively applied, starting from the initial configuration qi until a neigh-boring area of the final configuration qf is reached.

3.1.2 Reactive approaches

Reactive methods are sensor-driven approaches and behavior must emerge from interaction.

Potential Fields & Navigation Functions

These type of approaches will not search for a connection between start and goal configurations.They will define a function in a continuous evaluation loop, which will control and guide themotion of the vehicles along the way, hoping to make the agents reach the goal configuration. Thenavigation function approach is a decentralized method, and it entails a very reactive evaluation,sensitive to changes and robust to uncertainty.

The navigation functions are normally based in potential forces, that is to say, that an individ-ual robots motion is controlled by the resultant artificial force (inspired on electromagnetic physicforces) imposed by other robots and other components of the system. These methods have beenpreviously used for obstacle-avoidance path planning pioneered by Khatib [Kha86]. The assump-tion is based in the configuration space of a robot where we assign negative electrical charges tothe agent and the configuration obstacles, and positive charges to the goal configuration. Thus,a potential field is formed with very high potential forces close to the configuration obstacles andminimum potential forces at the goal configuration. The robot, guided by these forces, will gofrom high potential configurations to low potential configurations and it will eventually reach itsgoal avoiding collision with static obstacles and other agents [War90]

We can find other type of navigation functions in flocking multi-agent methods [Os06], [LJ05].Here the control and guidance is not only focused in a single robot, but in a fleet o several agents.Flocking is a form of collective behavior of large number of interacting agents with a common groupobjective. The success of these techniques is partly owed to the pioneering work of Reynolds whointroduced three heuristic rules that led to creation of the first computer animation of flocking:cohesion, separation and alignment (matching the velocity with nearby flock mates). This approachcan be useful for determined traffic situations in which several robots have to be managed dueto their proximity and common path direction, thus they could establish a communication tocoordinate their actions according with a consensus (we can see some notions of consensus among


agents in [WRA07]). Once the common path is over, they could continue their respective plan toreach their goals.

There are some works about potential functions and flocking methods for non-holonomic vehi-cles, which could be more suitable as support for our current problem due to our non-holonomicconstraints ([RCR06], [LK03], [WH09]).

Finally we can find other kind of Navigation functions like the work of Xianyi Yan and Max Meng[YM99], where a neural network approach for non-holonomic car-like robots, capable of planningin real time motion and biologically inspired, is evaluated.

3.2 Collaborative planning for multi-robot systems

3.2.1 Centralized Vs Decentralized methods

One of the most important and obvious classifications in motion planning in multi-agent systemsare centralized and decentralized approaches.

Centralized:

The centralized-paradigm assumes a single control which collects the information of the entiresystem, and then plans the motion for all the robots based in this full knowledge. When thecomplete motion plan has been computed, the actual motion takes place, that is to say that thisis an off-line method (defined below).

Advantages:

• They have an evident conceptual simplicity.

• Since everything is known, everything can be computed, and completeness is generally as-sured, so an optimal solution (shortest, smoothest, cheapest, etc), in terms of trajectories forall the agents, can be achieved.

Drawbacks:

• The price of the whole system calculation is the complexity of the problem. We get bottle-necks and too much data to process for a real-time performance (one of our key points orconstraints in the search of the solution), the complexity increases with the number of vehiclesin the system, and the sacrifice for completeness leads us to an unacceptable lag of time (ofmany seconds or a few/some minutes depending on the number of robots). Hence, solutionslike [Ove98], [SO95] or [LH98] are not attractive to us because of the entailed complexity.

• Complete recalculation of motion or paths is required if the environment changes. So it cansuppose a huge lag of time in the middle of the motion process. Neither flexible nor robustto changes and uncertainty.

• The requirement that all the system, parameters, and model of future of the robots is localizedat a single place to make the calculations, often turns out to be not practical.

Decentralized:

They are also called distributed. This approach implements local solutions in the global system.In the distributed-paradigm each robot determines its motion by observing the environment andby using some rules. This is done locally in the sense that a robots plan is not based on thewhole information of the system, but only on information accessible to itself, so it does not knowthe plans of other robots. However, with properly designed rules, the system may display desiredglobal behaviors to achieve their goals.


Advantages:

• This approach does not have the bottlenecks and problems of complexity of the centralizedmethods.

• The complexity can be independent of the number of agents in the system.

• It is inherently more stable and robust than centralized methods, in the sense that this is anapproach which can tolerate changes and uncertainty, so failures of some agents will not stopand break the whole system, and will not force to recalculation. The locality of the taskswill suppose the adaptability and flexibility to the changes of the system.

Drawbacks:

• This approach, does not assure either completeness nor optimality due to the local processes.

• It cannot assure an optimal performance, since at any moment each agent is lacking someinformation, contrary to centralized methods. Thus, optimality cannot be satisfied in general.However, an acceptable performance is sought. So, nevertheless, we can conclude that thismethod achieves a good performance in complex systems.

• Depending on the type of approaches and implementations, decentralized methods can leadto a deadlock situation.

3.2.2 Priority based approaches

When we have to deal with a possible collision or crossing roads situation, we can assign prioritiesor priority functions to evaluate by the agents, in order to resolve the order of the movements,the vehicles will adapt their motion with respect to the priorities assignment in order to avoidcollisions. We can notice this kind of approaches in [WP95], [YLP05] with the notion of rulebased local payoff functions to manage crossroads with certain priorities, here the coordinationgraph method is presented. In [ELP87] we can notice another priority based approach used in theframework of Configuration Time-Space.

Other systems will resolve the situation with no priorities assignment. For instance, in coordi-nation diagram methods [LpLT].

3.2.3 Coordination Methods

Coordination Diagram method

Coordination Diagrams [LpLT] are cooperative methods to resolve conflicts in shared regions ofthe environment. With this option we can adjust the velocity profile of each one of the agents inconflict with a N-dimension cube where each one of the axis represents a normalized path (withcartesian coordinates from 0 to 1) belonging to each vehicle, as we can notice in Figure 3.1. Insidethis N-Cube we will have N-dimensional cylinders which represent the collision regions among thecorresponding agents as shown in Figure 3.2. The solution to the problem is a N-dimensional free-collision path-finding in this N-Cube from the initial configuration of all the agents (0, 0, 0, ..., 0)Nto the final configuration (1, 1, 1, ..., 1)N which avoids all the forbidden cylindrical regions (thecollisions).

In [LpLT] this solution is proposed as a centralized method to coordinate the whole system ofagents from their start to their goals.

Path-Time Diagram

In this approach, an agent has to adapt its velocity profile to the fixed movement of anotheragent (unlike to coordination diagrams where none agent has a fixed predefined movement). Therepresentation for 2 agents of this method, would be a 2-Dimension Diagram with certain regionsto avoid in a path-planning problem from the initial configuration path-time (0,0) to the final one(1,t) with t(time) ≥ 0. An example is shown in Figure 3.3.


Figure 3.1: (a) Two agents with their paths from start to goal. (b) The coordination diagram(c) The partition of the diagram induced by the path decomposition. (d) The bounding boxrepresentation of the obstacles and a solution path.

Figure 3.2: Cylindrical structure of the Coordination Diagram.

We can notice that the solution of the problem lies in a path-planning, the same that coordi-nation diagram resolution, however two constraints with respect to the time axis are introduced:

1. We can not move back in time.

2. The slope path-time for any of our vehicles has to correspond to a feasible velocity for theagents.

Since the motion of one of the agents is fixed, this is a suitable method to combine with priorityassignment approaches.


S1

t

x1 x2

t 1

t 2

A

f

1

t 2

t 1x1 x2

(a) (b)

(f)

Figure 3.3: (a) Two vehicles facing a crossroad, the first agent with path s1 and an agent f witha fixed motion. (b) The Path-Time Diagram of the corresponding agents.

Chapter 4

Walking towards a solution

In the previous chapter we have exposed the state of the art of motion planning for single roboticsystems and multi-robot coordination system. Regarding these approaches it is time to choose ourown direction. We have overviewed several ways to classify the methods used in multi robots fieldto solve different kind of problems. Our work now is to extract the most suitable mechanisms forour particular context trying to create a new approach and achieving an improvement with respectto the previous works in the resolution of the subject.

4.1 General Features of our solution

Following some of the ideas inherent to the assumptions, objectives and constraints of the problem,we can establish some characteristics that our solution should have.

The assumption 1 made in section 2.2 was: The problem will be placed in a structured environ-ment (urban environment) in a 2D workspace. Thus, our environment will be composed by streets,lanes, crossroads,... Intuitively, this context suggests the possibility to use a graph model for ourworld representation. Since we have a geometric model of the environment and we must go to agoal point within this structure, a first Deliberative approach should be applied, that is to say, amodel-driven based method, hence a route/path planning from start to goal based in our graphrepresentation. So, the structured environment establishes our choice: a path/roadmap approach.

As we can notice in section 2.3.1, this is a real-time system, so we have to be as reactive aspossible. Adapting the motion of each agent to the route. Avoiding collisions between agentsthemselves, between agents and dynamic obstacles and between agents and static obstacles.Global-Centralized approaches which propose a global crossed calculation of all the possible solu-tions of the system, provide completeness and optimality with a corresponding increase of complex-ity with the number of agents. Our search of reactivity, robustness to uncertainty and flexibility,invalidate global high complexity calculations and guides us towards a local and decentralizedapproach in our navigation process.

Some literature propose the design of multi-level architectures to deal with dynamic systems,where high variety of circumstances can take place. In this line we can find pioneer works like[Bro85] or [Gat99], where multi-layer systems receive inputs coming from the environment (bymeans of sensors), and according to task decomposition criterias and the type of inputs, differentkind of methods are executed to produce suitable responses.This is a multi-robot problem which will have to deal with multiple collision avoidance situations,thus, different answers will be achieved depending on the context. We propose a Multi-Layerarchitecture to manage the collision avoidance situations that the agents will encounter alongtheir ways.

12

CHAPTER 4. WALKING TOWARDS A SOLUTION 13

4.2 Overview of the solution

Figure 4.1 shows a general schema of our problem and solution.

GO TO GOAL

AVOID STATIC OBST

AVOID DYN OBST

AVOID OTHER AGENTS

INPUTPROCESSIN

G

OUTPUTPROCESSIN

GExternal Communication

Sensors

INPUT DEVICES RESOLUTION

Actuators

OUTPUT DEVICES

External Communication

City

Agents

GPS Dyn. Obst. ?

Env. Model Env. ModelTHREE - LAYERS

MOTION PLANNING

Figure 4.1: General schema of the multi-robot navigation problem and multi layer resolution.

As aforementioned, the key points of our problem are:

1. Each one of the agents must find a free-collision path from start to goal.

2. Each agent will avoid collisions with static obstacles along the way (pedestrians, other vehi-cles,...).

3. Each agent will avoid collisions with dynamic obstacles along the way (vehicles that we cannot control).

4. Each agent will avoid collisions with other agents along the way (vehicles that we can control).

First, we will describe how the environment model of our solution will be, afterwards we willpresent a Three Layers Architecture to resolve the multi-agent motion planning problem.

4.2.1 Environment model construction

Initially, an external platform, placed in the infrastructure, will build the environment model. Wehave chosen a graph representation because this is an easy an intuitive structure to represent astructured-urban environment and it is a suitable one for path-finding methods (that we will use).It also represents an easy way to describe the intersection points of the roads (by means of nodes),which are one of the most important regions in our environment with respect to the collision con-flict management. This graph structure will be part of the inputs of the system. It will containthe topology and geometry information of the structured environment, representing lanes, streets,crossroads, ... by means of edges and nodes.

Remark : As we will mention in section 4.2.2, another short-range representation of the envi-ronment will be constructed by local sensor capture. This will be an occupancy grid model whichwill represent the data obtained by the sensors. It will be updated continuously as the vehicle goesforward reconstructing a local field. This model will be built by each agent itself, not for the exter-nal platform, and it responds to a reactive navigation context. Thus, it corresponds to a differentpurpose from the initial urban reconstruction, which maps the whole structured environment in agraph to carry out deliberative methods.

The occupancy grid mapping will be explained in section 5.5.


4.2.2 Three Layers Architecture

We propose a Three layers architecture, to construct our motion planning.

Each one of these layers will control the agent according to different urban situations:

1. Deliberative Layer, Model-Driven Layer (MDL): When the agent is driving towards the so-lution without encountering any conflict. The model will be the basis to guide the vehicleuntil its corresponding goal.

2. Reactive Layer, Communication-Driven Layer (CDL): When the agent reaches a conflict sit-uation with another vehicle. It will be resolved by means of communication and coordination.

3. Reactive Layer, Sensor-Driven Layer (SDL): When the agent reaches a conflict situationwith a static obstacle in a short-range distance. The resolution of this conflict will be basedin the data coming from sensor capture.

Each one of these situations will correspond to the execution of the respective Layer. The out-put from these layers will always be a Local Trajectory Planning, which represents the motionplanning resolution to the specific current situation of the agent.We have indicated the notion of “locality” because we will merely calculate pieces of trajectoriesin a reactive way (for each current situation). The final result at the end of the path would be thecomposition of the piecewise trajectory parts.Since the environment is in constant change, we can not define a suitable trajectory from thebeginning for the total path between start and goal. We will reactively compute appropriate localtrajectories according to the needs of the different situations that an agent will experience alongits path.

We have to point out a hierarchy among layers. The MDL is executed when no conflicts aredetected, this is the first level because its execution can be interrupted by the other two layers whena conflict detection takes place. The level below will correspond to the CDL which will trigger upan exception when a conflict between vehicles turns up, interrupting the trajectory execution ofthe MDL to compute its own planning; so, we will consider this layer more priority than the MDL.Finally the most priority level is the SDL, which has to provide fast answers when short-rangeconflicts are detected by the sensors. It will interrupt with an exception any of the upper layerswhich has the current control of the vehicle and a fast trajectory planning to avoid the staticobstacles will be executed.Once a priority layer has resolved the conflict situation, the control will be released to the previ-ously interrupted layer.

In Figure 4.2 the three-layers architecture is depicted.

An agent will always be under the control of one of the three layers, according to the situationsmentioned above. The states that an agent can adopt during its way to the goal are: Free State,Coordination State, and Dodge State. Each one of the states will correspond to the control ofone of the layers. When an agent is under the control of the Deliberative Layer, it will take the“Free” State. When it is the Communication-Driven Layer which controls the agent, the statewill be in “Coordination”. Finally “Dodge” state will correspond to the control achieved by theSensor-Driven Layer.

Figure 4.3 illustrates the State Diagram of an Agent.

We can describe the transitions as follows:

1. Start → Free: As aforementioned in 4.2.1, an external platform will build the environment.Then, the MDL will calculate an agent’s path from start to goal with a path-finding algorithmin the graph structure; finally the vehicle will begin the travel. This will represent the startupof the agent in the system.


Communication - Driven Layer

MOTION PLANNING

DELIBERATIVE LAYER

REACTIVE LAYER

External Communication

Sensors Sensor - Driven Layer

Model - Driven Layer

+ Priority

- Priority

Env. Model

Exception Propagation

Local Traj. Planning



Figure 4.2: Three Layers Architecture, which represents a global solution for the multi-agentmotion planning problem.

Free

Dodge

Coordination

Start

EndPlatooning

CrossingCoordination

Figure 4.3: State Diagram of an agent in the system.

2. Free → Free: An agent under the MDL control, will produce a local trajectory planning(from the current position until a certain horizon) because, due to the constant dynamic ofthe system, there is no sense in calculating a global start-to-goal planning. The MDL willbe responsible to achieve local plannings in an iterative way as the agent goes forward andthere is no conflict on the road.

3. Free → Coordination: There are two communication platforms in our solutions. Oneplaced onboard the own agent and the other in the infrastructure (external). We use twoplatforms to detect two different kinds of conflict between vehicles (detailed below); in addi-tion, the active assistance of the environment represents a “horizon extension” to the vehicles.When the communication platform (either onboard or external) detects a conflict betweenvehicles, the CDL launches an exception to the upper layer (MDL). The MDL Trajectoryplanning will get interrupted and the CDL will assume the control of the vehicle.

4. Free → Dodge: The sensors of the agent detect a conflict with a static obstacle in theway. An exception will be launched to the corresponding upper layer with current control(MDL) and the corresponding Trajectory planning will be interrupted. The SDL will takethe control of the vehicle.


5. Coordination → Free: The CDL Trajectory planning has been completed. The control isreleased to the previously interrupted layer (MDL).

6. Coordination → Dodge: The sensors of the agent detect a conflict with a static obstaclein the way. An exception will be launched to the corresponding upper layer with currentcontrol (CDL) and the corresponding Trajectory planning will be interrupted. The SDL willtake the control of the vehicle.

7. Dodge → Free: The SDL Trajectory planning has been completed. The control is releasedto the previously interrupted layer (MDL).

8. Dodge → Coordination: The SDL Trajectory planning has been completed. The controlis released to the previously interrupted layer (CDL).

9. Free → End: The agent reaches its corresponding goal.

Now, we will detail each one of the layers of the solution.

Deliberative Layer: Model-Driven Layer (MDL)

The graph model exposed in 4.2.1 will represent the input for a first layer in the motion planningprocess. The Deliberative Layer, model-driven, which entails planning before acting. This layerwill be mainly responsible to attain the first objective: reach the goal. The agent will be treatedas if it represented the only vehicle in the system, because, a priori, we can not evaluate and takedecisions about the global evolution of the environment according to the conflicts within. We willwait the on-line execution to detect and avoid the possible conflict situations “on the fly”, this willbe the responsibility of the Reactive Layer (CDL and SDL).

Only in the startup of the agent, MDL calculates a route from start to goal by means of apath-finding method (A* or D*) executed on the graph model.Afterward, MDL will calculate a local trajectory planning with a specific horizon, with a periodicalupdate as the vehicle moves forward (without taking into account other vehicles in the system).We will use a variational method described in [DFMG09] called Tiji, which performs and executionin real time preserving the kinematic and dynamic constraints of a non-holonomic vehicle. A controltrajectory will be obtained between an initial state s0 and a goal state sg in time tg.

Reactive Layer

The Reactive Layer will provide a behavior emerged from interaction with the environment. Thisinteraction will come from the external communication inputs (information coming from the infras-tructure, the broadcasted data of the other agents, and possibly the broadcasted data of dynamicobstacles) and sensors which provide data of a specific range. Thus, we can subdivide this ReactiveLayer in two sublayers: CDL and SDL.

The Reactive Layer will give answers to the different conflict situations that an agent will facealong its way:

1. Conflict with Static Obstacles:

i. Conflict Agent-Static Obstacle.

2. Conflict with Dynamic Vehicles:

(a) Same direction of movement

i. Conflict Agent-Agentii. Conflict Agent-Dynamic Obstacle

(b) Opposite or crossing directions of movement



Communication-Driven Layer (CDL)

This layer will be responsible to detect and resolve some of the conflict situations listed above(point 2.: conflicts with vehicles). Since this layer will control the relationship between all theelements of the system and the conflict interactions between them (which represents the globalmotion and evolution of the system), it will typify the inherent coordination of the global behav-ior.

To carry out the different coordination actions, two processing platforms will take part ofthe solution. One Onboard platform embedded/placed in each one of the agents; the other, anExternal platform placed in the infrastructure (normally in a distributed unit which represents aglobal processing platform of the city, entailing an active assistance of the environment). Bothplatforms will be in constant surveillance and collision resolution (if it is needed) by means ofcommunication.

1. External surveillance system: This system will be implemented in the External platform. Itwill be focused on some fixed conflict regions. The characteristics of these conflict regionsmonitored by the external surveillance system are: intersecting regions between differentagents in opposite or crossing directions of movement, e.g. crossroads and double directionstreets. The monitoring process will carry out a constant checking of two conditions for eachconflict region:

(a) Proximity to the non-return point (n.r.p) of this conflict region: We will define a n.r.p.for each couple (agent, crossing region) as the geometric point in the abscissa of thepath of the agent after which, no matter the control of the vehicle, the agent will notbe able to stop before coming into the conflict region.

(b) Collision in the conflict region: We know for all the vehicles their instantaneous currentvelocity and position in their paths (information broadcasted to the system), so theexternal platform will be able to decide if a collision will take place on the conflictregion or not.

We will use the external surveillance system to detect the conflict situations with oppositeor crossing directions of movement (2.(b).i and 2.(b).ii).

If the evaluation of both conditions is positive (the agents are in proximity to their n.r.p.and there will be a collision between them) there will take place two actions:

(a) Priority Evaluation of the vehicles in conflict: This process will establish an order ofcrossing the conflict region between the vehicles.

(b) Time Boundary Calculation: This process will establish an approximation of the coordi-nation crossing time for each agent wrt. the limits of the conflict region. This timing willfix our reference to calculate the local trajectory (path + time) of the vehicle throughthe region.

Both steps will use a Path Time Diagram [HJ02] as a tool to get the result.

Finally, the trajectory planner used in MDL (Tiji [DFMG09]), will be applied here as well.This time, the final state (sg) and the final time (tg), will be fixed according to the coordi-nation timing constraints.

2. Pilot surveillance system: This system will be implemented in the Onboard platform oneach agent. We will use the pilot surveillance system to detect the conflict situations inthe same direction of movement (2.(a).i and 2.(a).ii). The surveillance will be focusedon a range ahead the agent by means of the communication with the surrounding vehicles.The broadcasted data about the position and velocity of the other agents makes possible toestablish a proximity criteria which will trigger a coordination in case a minimum distance


inter-vehicles is reached. This coordination consists of a Platooning (leader follower) methodwhich basically entails a trajectory planning based in the vehicle ahead.

Remark : It looks logical to think that this pilot system could be achieved by the sensorsthat we use for the static obstacles detection. It is true that a data fusion coming from com-munication and sensor interfaces could be an interesting subject of study. Nevertheless, wewill consider more accurate the data coming from the communication than the appreciationmade by the sensors, so we will accept that the broadcasted information between agents isprecise enough and sufficient to resolve this surveillance process. This can not be the case ofthe static obstacles, because they do not have communication to transmit their state.

In Figure 4.4 we show a schema of our on-line collision detection made up of both surveillancesystems.

The details of this process can be found in section 5.4.

1 2

a

b

c

Pilot Surveillance System

External Surveillance System

Figure 4.4: The external system is represented by the dotted arrows relationship between agents-crossroads, and the pilot surveillance is achieved in an exploration area around the vehicle.

Sensor-Driven Layer (SDL)

This layer will be responsible to detect and resolve the conflict with static obstacles placed alongthe way (1.i in the conflict situations listed above). They can be objects, pedestrians, static vehi-cles, ... devoid of communication, only detectable by means of onboard sensors when the agentsare located in a proximity range. Here the notion of a second environment representation takesimportance. We need a local or short-range model to map the information coming from the sensorinterface. Thus, we will use an occupancy grid (continuously updated) to represent the sensordata.Once the agent is located in a proximity range wrt. these obstacles, a collision evaluation will becomputed regarding the current trajectory and, if it is needed, an escape new trajectory will becalculated and executed by this layer.

The Sensor-Driven Layer will be the most priority one. It means that if a collision situationwith a static obstacle is detected by the sensors (whenever it takes), this layer will take control of


the vehicle modifying its current trajectory.

If a collision is detected, a Rapidly-Exploring Random Tree (RRT) will be used for the gener-ation of a trajectory, preserving the dynamic constraints of the system.

Each one of the previous methods will be described in section 5.

4.2.3 Remark about Trajectory Planning

We have to point out another important feature in our local trajectory planning: We will definevmax as the maximal velocity allowed for the vehicles in the system. The behavior of each one of ouragents will always be to attain vmax, in order to reach the goal point as soon as possible (timeto goal minimization criteria). Nevertheless, the agents will confront several conflict situationsthat force them to coordination or dodge maneuvers; in this cases, they will have to slow down inorder to avoid a possible collision. Once the agents have overtaken the conflict, they will retakethe standard behavior in “Free” state, i.e. to achieve a local trajectory calculation, always tryingto reach the maximal velocity vmax.

Chapter 5

Solving the problem: Definitions& Methods

5.1 Environment model

5.1.1 Definitions

A graph will be used as representation of our urban environment. It will be a DiGraph (DirectedGraph). We propose two different graph representations for the environment model:

• Geometric Graph (GG): This model represents a polygonal spine of the infrastructure,so, it will contain a geometric information (in distances and shape) of the real environment.We will need this representation for a path planning process.

• Topological Graph (TG): This model represents a simplification of the geometric graph.It only models the connection between lanes (edges) and crossroads (nodes) and not thegeometric relationship between them (in contrast to the GG). We will need this representationfor a route planning process.

We can appreciate the difference and relationship between both structures in Figure 5.1 wherea correspondence between the edges and nodes of the models can be established.

Geometric Graph(a) (b)

1

23 4

5

6

a

b

c de

f

g

n m1

Figure 5.1: Translation from a Geometric Graph representation (a) to a Topological Graph repre-sentation (b): The edge 1 of the TG will correspond with a list of edges (1, 2, 3, 4, 5, 6) and nodes(b, c, d, e, f) of the GG, the node n will correspond with the node a, and the node m with g.

20

CHAPTER 5. SOLVING THE PROBLEM: DEFINITIONS & METHODS 21

5.1.2 Lanes & Streets

The directed edges of the TG & GG structures correspond to lanes with a direction in the realenvironment. A street is normally made by several lanes. We will represent all the lanes of thestreet with corresponding edges in the graph models. To make possible the displacement of avehicle to the lane beside and to represent the connection between the lanes of the same street, wewill establish a relationship between the corresponding edges with a link. In this way, we providethe access between adjacent lanes of the same street as we can notice in Figure 5.2.

Remark : Even though we announce a complete graph representation which can contributewith a model for lane maneuvers, the solution will be focused on resolving the conflicts and trafficsituations described in section 4.2.2, because we consider they are the most important cases tocover and the study of all driving situations would correspond to a larger work.

Figure 5.2: We will symbolize the bidirectional relationship between adjacent lanes by means of adotted line.

This lane representation will be provided either for the GG or the TG structure. We also haveto point out that no matter if an edge belongs to a GG or a TG, it will contain the followinginformation:

• The length of the street (L).

• Links to the adjacent lanes.

• The width of the lane (τ).

• The orientation of the street (represented by an arrow, as a directed edge).

In Figure 5.3 we can appreciate some of these features in a graph representation that includesmultiple adjacent lanes with different orientations (connected by dotted lines), a crossroad ofmultiple lanes, a fork and a union of lanes.

5.1.3 Objective of TG and GG Structures

We are going to need both, TG and GG models for the following purposes: Route/path plan-ning, Trajectory planning. Both are necessary. We need a topological model to support a routecalculation and also a suitable and detailed geometric model of the environment to support a pathand trajectory generation (taking into account the non-holonomic and dynamic constraints of thevehicles applied to the paths).

For each agent we will choose two nodes as input parameters of the mentioned plannings: thestart node and the goal node.


Figure 5.3: Environment Representation with different lane configurations.

5.2 Deliberative Model-Driven Layer

Once the environment has been constructed, the agents can be registered in the system. This isa multi-vehicle context, therefore a new agent can access to the environment in any moment withtheir corresponding start and goal points.

As aforementioned, the Deliberative Layer, is a model-driven level. The environment modelproposed in the previous section represents the main input for this phase. A first path-planningstep will be achieved before the agent begins its navigation within the environment. In this layerwe will not take into account the presence of the other vehicles in the system, so, the differentcalculations will be focused on the navigation planning for a single agent.

The control of the Deliberative Layer will be in charge and will take place:

1. In the startup of an agent in the system.

2. When the state of the agent is set to “Free“, either if it is driving the way with no con-flicts along or it has been released from a ”Dodge“ or ”Coordination“ state after a conflictsituation.

Startup of an agent

The Deliberative Layer will execute the following steps when an agent comes into the system:

1. Route planning: From start to goal a route on our graph representation will be calculated.A route will be the result of a path-finding method applied on a TG. Our intention is to use aA* path-finding or its dynamic version, D* algorithm [Ste94] (with a cost function defined bymeans of the length of the streets-edges). We can find other possible dynamic path-findingoptions like [Ste95], [MLT05].Our TG is a simplification of the corresponding GG, so the reason to use TG to execute thepath-finding method is the efficiency.Complexity: O(N) with N the number of nodes in TG.Remark: We can point out that another case will lead us to carry out a route planning: whena crossroad or street becomes blocked, the agents with this element (node or edge) in theirroutes, will have to recalculate another way to attain their respective goals.


2. Get the Itinerary: Our route calculation in the TG model will entail a polygonal pathresolution in our GG model. This polygonal path is not a real path since it does not takeinto account the non-holonomic constraints of a car-like vehicle; thus, we will call itineraryΠA of the agent A to this polygonal path.Figure 5.4 shows the steps to get the itinerary ΠA. For the sake of simplicity, we havemodeled TG and GG with single lanes.

(b)(a)

(c)

(d)

S G

S G

GS

GSa b

c

de

f

A

Figure 5.4: (a) We have a Geometric Graph with the Start (S) and Goal (G) nodes for a vehicle.(b) We obtain the corresponding Topological Graph from the initial Geometric Graph. (c) Thepath-finding method on the TG gets a resulting route. (d) Thanks to the relationship betweengraph elements, we get a raw polygonal path in the GG, which we call itinerary ΠA.

3. Conflict Regions calculation: Once the agents know their respective itineraries ΠA, theywill probably have to face several conflicts along their trajectories. A conflict region will be apiece or stretch of the agent’s itinerary, common to other vehicles’ itineraries (i.e. crossroadsand two-way streets). Hence, this region will represent a potential collision. How to calculatethese conflict regions is described in section 5.3.2

4. Trajectory Planning: At this point an agent has an itinerary to follow, however this isnot a feasible path according to the non-holonomic, kinematic and dynamic constraints. Theobjective is to get a trajectory planning computed in real time to satisfy a smooth andfeasible motion towards the goal, and to provide a control for the vehicle along its way. Wehave to notice that there is no sense to calculate a total trajectory from start to goal due tothe dynamic of the system (the trajectory would be changing constantly with the interactionof other vehicles), therefore the objective will be to get local trajectory plannings along thepath. As aforementioned, the Deliberative Layer will use a variational method called Tiji forthis step. This process will be described below in section 5.2.2 and 5.2.3.

State set to ”Free“

The Deliberative Layer takes control of the vehicle after a coordination or a collision avoidancemaneuver. In this case, the only task of this layer is to guarantee the continuous trajectory towardsthe goal point. Thus, this will be again the Trajectory Planning execution just explained above.


5.2.1 Why a Trajectory planning ?

In our motion planning research we have looked for a way to compute a feasible path betweenan initial and a final state, preserving the non-holonomic and dynamic constraints inherent to acar-like vehicle. We think that it is important and convenient to underline that path/trajectoryplanning has been a key point in our solution. We needed to bind our three-layers architecturewith a feasible non-holonomic path calculation method. Even though we have finally chosen atrajectory generation method, it was not our initial option. We invested considerable time inour first choice, therefore we consider important to mention it: path-velocity decomposition usingcontinuous curvature profile. Hence, we have included our first path-planning work in the Annex A.

Even if the path-velocity decomposition can be an answer adaptable to our current solution(because it guarantees our objective: feasible paths taking into account our constraints), the nom-inal geometric path reduces the solution space (i.e. we are reducing the possible solutions, andmaybe we are missing some important ones).Furthermore, path-velocity decomposition was attractive years ago, when it was mandatory todecrease the complexity of the algorithms (and direct trajectory calculations represented heavycomputation processes). Nowadays, there is not this problem anymore, and trajectories can beresolved in real time.

These ideas made us change our mind towards another direction: Trajectory generation.

We will now define the mentioned ”Tiji” trajectory planner. Later, the specific behavior of thismethod in the Deliberative Layer will be exposed.

5.2.2 Trajectory Planner: Tiji

Tiji is a trajectory generation method, which provides some important features that answer to ourneeds:

1. Calculate a feasible trajectory between a start and a goal state taking into account thedynamic and kinematic constraints for a non-holonomic wheeled vehicle.

2. Computation of the solution in real-time.

3. The method takes into account a final time constraint. It is important not only to reacha goal state, but also to reach this state at a certain time. This characteristic is speciallysuitable for collision avoidance situations with other dynamic vehicles in the system.

4. If the defined goal is unreachable, the method returns a trajectory that ends as close aspossible to the final state-time.

We can find different trajectory generation approaches in the literature. They can mainly beclassified in four categories:

1. Primitive Combination category: Geometric paths are initially built with fixed geometricprimitives concatenation. One of the pioneers in this field was Dubins [Dub57] with pathscomposed by straight segments and circular arcs. More complex primitives where developedlater using, for instance, clothoid curves [SFaG97], [Sch98]. These approaches are based inan initial path construction, and a velocity profile calculation after. This is the well-knownapproach path-velocity decomposition analyzed in 5.2.1.

2. Two-points boundary value problem: A high-degree type of curve is used to connect start andgoal states, like B-splines, quintic polynomials or cubic spirals [KH97]. The main drawback inthis group of methods is the difficulty to satisfy some of the internal constraints (curvature,...).

3. Variational approaches with initial connection: An initial trajectory (generally not feasible)connects start and goal. It is iteratively modified until it satisfies all the constraints of the


system (similar to Expectation-Maximization methods). A parametric trajectory represen-tation is assumed, and a numerical optimization on a trajectory cost function is searched inthe parameters space.

4. Variational approaches without initial connection: This category is similar to the previousone. Herein, the initial trajectory is feasible but does not connect start and goal states. Itis also iteratively deformed preserving the initial feasibility (constraints satisfaction) untilthe goal is reached or approached as much as possible (if it is unreachable). A solution withsums of harmonics is presented in [GG00]. The generality and computational efficiency ofthis category has determined our choice.Tiji, presented in [DFMG09], is a variational method which belongs to this last category.

Overview of the approach

We will describe the dynamic of an Agent A by:

s = f(s, u) (5.1)

where s ∈ S (state space) is the state of A, s its derivative wrt. the time and u ∈ U (controlspace) a control. Let u : [0, tf ]→ U denote a control trajectory, that is a time sequence of controls.We will indicate by u(s0, t), the state of the agent A at time t, with initial state s0. The couple(s0, u) defines a state trajectory for A, i.e. a curve in S xT where T is the time dimension.The set of kinematic and dynamic constraints corresponding to A will be represented by:

h(s, u) ≤ 0 (5.2)

Given two states s0 and sg, a trajectory generation will correspond to find a control trajectoryu from s0 to sg. We will indicate a parametrization of the control trajectory as follows:

u = u(p) (5.3)

where p = (p1, ..., pk) is a vector of parameters. Thus, we will reformulate the dynamics of Aas follows:

s = f(s(p), u(p)) (5.4)

Trajectory generation by constraints optimization turns the problem (optimal control) intoone of constraints optimization (nonlinear programming). Let us consider a cost function J(s0, u)which represents a weighted distance between the goal state sg that we want to reach, and thestate sf = u(s0, tf ) (the state reached from s0 applying the control u).Considering J and the set of motion constraints h (bounds over the state and control space), thisoptimization process will be indicated as:

minimize : J(s0, u) (5.5)subject to : h(s, u) ≤ 0 (5.6)

The optimization to get a suitable trajectory consists in modifying the set of parameters inorder to reduce the cost function J (distance between sg and sf ). The update of the parametersp has to preserve the feasibility of the resulting control trajectory (satisfying the equation (5.2)).

Algorithm 1 describes Tiji : the optimization algorithm for trajectory generation.The algorithm ends when J is small enough (i.e. when the parametrized control u(p) brings the

state s0 to a final state sf close enough to the goal state sg) or after a fixed number of iterations,after a process of iterative minimization of J and sf corrections.

We can distinguish 3 main steps in the execution of Tiji :

1. Computation of an admissible control ubdd and the resulting state sf :The control ubdd has to satisfy the constraints of the system; and sf will be the state reachedby means of the current parameters p.


Algorithm 1: Tijiinput : s0, sg, tgoutput: p, success flag

i = 0;1

p = InitialGuess(s0, sg, tg);2

repeat3

Compute ubdd and sf ;4

Compute J;5

Compute corp and Update p;6

i = i+ 1;7

until J ≤ ε or i = imax ;8

return (p, sf = sg?);9

Given a parametric representation of the control trajectory u(p), we can calculate sf as theintegration of equation (5.4) as follows:

sf = s0 +∫ tg

t0

f(s(p), u(p))dt (5.7)

We have to take into account that after the correction of the parameters p in order to reachsg, the motion constraints h(s, u) ≤ 0 (boundary constraints) could have been violated.Furthermore, as aforementioned, given the final time constraint fixing the time dt = tg − t0between both, final and initial state, the goal state may be unreachable (returning a solutionas close as possible to the goal).The idea to preserve the feasibility of the trajectory is to truncate the parametric profiles ofthe control where the bounds defined over it are overreached. Thus, we will use a piecewiseparametric profile to describe the control of the vehicle. Therefore, having h(s, u) ≤ 0 as theset of constraints defined over the control and state profiles, and admissible control trajectoryubdd would be:

ubdd(t) ={ubdd(p, t) if h(s, u) ≤ 0Uextl(t) otherwise (5.8)

where Uextl(t) is the extremal applicable control at time t such that h(s, u) ≤ 0, i.e. themaximal or minimal control that preserves the defined constraints. Thus, we define two typesof intervals in the time axis wrt. the dynamic of the vehicle: Ifea={Ifea1 ,..., Ifean } ⊆ [t0, tg]in which the parametric control trajectory respects the constraints, and Iovr={Iovr1 ,..., Iovrm }⊆ [t0, tg] \{Ifea} in which the constraints are violated. In this way, we can reformulate thefinal state sf as:

sf = s0 +n∑i=0

∫ tg

Ifeai

f(s(p), u(p))dt+m∑j=0

∫ tg

Iovrj

Uextl(t)dt (5.9)

So, sf is obtained from the current parameters by using the parametric control if it respectsthe bounds of the constraints, or by using maximal or minimal applicable controls otherwise.

Figure 5.5 illustrates the mentioned bounding process to get the intervals Ifea and Iovr in a1D double integrator system with velocity and acceleration bounds.

2. Computation of J :As mentioned J is the weighted distance between sf and sg. The error between them iscalculated as follows:

∆s = sg − sf (5.10)


(a) Step 1: Linear acceleration profile withoutany bounding process.

(b) Step 2: Linear acceleration profile withacceleration bounding process.

(c) Step 3: Linear acceleration profile withacceleration and velocity bounding processes.

(d) Step 1: Linear velocity profile without any bounding process.

(e) Step 2: Linear velocity profile with accelerationbounding process.

(f) Step 3: Linear velocity profile with accelerationand velocity bounding processes.

Figure 5.5: Different steps of the computation of the linear acceleration and velocity profile. Herein,it is depicted how the velocity and acceleration profiles of the control are truncated where thebounds defined by the motion constraints, h(s, u) ≤ 0, are overreached.

The distance J will be evaluated by:

J2 =p∑j=1

λj (∆sj)2 (5.11)

where ∆sj is the j th feature of the error between the goal and the current final state, andλj weighting coefficients.If the evaluation of J is under a certain threshold ε we will consider that sf and sg are closeenough, and convergence would be achieved.

3. Computation of correction corp:A correction will be applied to the current set of parameters p in order to reduce the costfunction J. A steepest descent method is chosen. We linearize the equation (5.4) as follows:

∆s '[∂f

∂p

]∆p (5.12)

where ∆s is the state error given by eq. (5.10), ∆p is the supposed error made over the setof parameters and ∂f

∂p are the partial derivatives of the state wrt. parameters.To decrease the distance in the cost function J, the correction over the set of parameters willbe:

corp = −τ[∂f

∂p

]−1

∆s (5.13)

where τ is the correction coefficient. The inverted matrix of the partial derivatives representsthen the direction of the correction applied, and τ∆s describes the step length of the steepestdescent method.

5.2.3 Tiji in the Deliberative Layer

Now we will apply Tiji to our model.


The state of an agent will be characterized by (x, y, θ, φ, v) where (x, y) indicates the position,θ is the heading angle, φ is the steering angle (orientation of the front wheels) and v describes thelinear velocity. A control of the agent is defined by u = (a, ζ) where a is the linear accelerationand ζ is the steering velocity. The dynamics of the vehicle are defined by:

xy

θ

φv

=

v cos(θ)v sin(θ)

v tan(φ)/Lζa

(5.14)

where L is the wheelbase of the agent.Assuming that the agent is moving forward, we establish the following constraints:

v ∈ [0, vmax] , |φ| ≤ φmax, |a| ≤ amax and |ζ| ≤ ζmax (5.15)

Now, we have to choose a parametric representation of the control trajectory according to theequation (5.3). We will choose a 2nd order polynomial parametric control (a, ζ):

ζ(t) = α1 + 2β1t+ 3γ1t2 (5.16)

a(t) = α2 + 2β2t+ 3γ2t2 (5.17)

where p = (α1, β1, γ1, α2, β2, γ2) is a selected set of parameters. The choice of these parametersis responsibility of the step number 2 of the Tiji Algorithm: InitialGuess(s0, sg, tg). We will notdescribe this function, it is enough to know that a possible strategy would be the use of a lookuptable, trained with orientative values according to the initial and final state characterizations, ora simple choice of orientative values. Anyway after some iterations of Tiji, these parameters willconverge towards the control trajectory to reach sg.

Computation of the input values s0, sg and tg

We have to define an initial and final states s0, sg and a final time tg to execute Tiji.

As we have indicated in section 4.2.2, we will achieve a local trajectory planning in a reactiveway as we goes forwards along the road.

We assign to s0 the current state (x0, y0, θ0, φ0, v0).Instead, a priori, we do not have reference points along the way to fix sg; and we do not haveneither a timing reference to choice tg. Hence, we will propose the following strategy:

1. How to get sg:We fix a certain distance δ ahead the position of the vehicle along the itinerary ΠA in order todefine the final state sg. Thus, the position (xg, yg) will be defined by means of a geometriccalculation with ΠA, δ and the current position (x0, y0). θg and φg will be calculated wrt.the orientation of the final edge/street in the position (xg, yg) in ΠA. And finally, sincewe are in “Free” state (we are under the control of the Deliberative Layer) and followingthe instruction defined in 4.2.3 about attaining the maximal velocity vmax, we will definevg = vmax. This is an approximation because we do not really know if we are going to beable to attain vmax at this point of the path, however, Tiji is robust to not exact parameters;so, if sg is not reachable, it will get the state sf as close as possible.

2. How to get tg:We take the distance δ of the stretch along ΠA in which we are achieving our trajectoryplanning and we approximate the value of tg by means of the following kinematic equation:

δ = v0tg +12at2g (5.18)


where a is the acceleration of the vehicle.This is an approximative value of tg because it has been calculated assuming a constantand continuous linear acceleration along the δ distance. This will not be true in general,because the shape of the road (e.g. curves) will not usually allow us to perform a continuousacceleration. Therefore, tg should also be taken into account in the correction computationcorp as we do with parameter p, so we will adjust the equations (5.12) and (5.13) to make tgtake part of the J cost function minimization as follows:

∆s '[

∂f

∂(p, tg)

]∆(p, tg) (5.19)

corp = −τ[

∂f

∂(p, tg)

]−1

∆s (5.20)

Computation of the linear acceleration profile

The linear acceleration profile described in equation (5.17) could overreach the acceleration boundsdefined by the constraints. Let us define Iamin

and Iamaxthe acceleration boundaries over which

the motion constraints are not respected, and Ia = [t0, tg] \ {Iamin , Iamax} the acceleration intervalin which the constraints are fulfilled. Then, we define the truncate acceleration profile as follows:

a(t) =

α2 + 2β2t+ 3γ2t2 over Ia

−amax over Iamin

amax over Iamax

(5.21)

This is a piecewise polynomial acceleration profile which respects the constraints of the system.

Computation of the linear velocity profile

This profile is obtained by means of the linear acceleration integration:

v(t) = v0 +∑i∈ Ia

(α2dti + β2dt2i + γ2dt

3i ) +

∑i∈ Iamin

(−amax)dti +∑

i∈ Iamax

amaxdti (5.22)

In the same way that happens for the acceleration boundaries, the velocity profile can also violatethe limit constraints defined by Ivmin

, Ivmax. The interval within the boundaries will correspond

to Iv = [t0, tg] \ {Ivmin, Ivmax

}. Thus, the final velocity profile will be constructed as a piecewisepolynomial representation as follows:

vbd(t) =

Eq.(5.22) over Iv−vmax over Ivmin

vmax over Ivmax

(5.23)

If the parametric velocity has been modified, the input control acceleration profile must be modifiedconsequently once again. Its final profile add is then computed by derivation of vbd wrt. the time:

abd(t) =

0 over Ivmin

∪ Ivmax

−amax over Iamin\ {Ivmin

∪ Ivmax}

amax over Iamax \ {Ivmin ∪ Ivmax}α2 + 2β2t+ 3γ2t

2 over Iav

(5.24)

where Iav is defined as the interval where neither linear acceleration nor linear velocity bounds areoverreached. This equation gives the final profile for the acceleration control of the vehicle.

Figure 5.5 illustrates the truncate process followed in this section.

Computation of the steering angle and steering speed profiles

The same successive bounding process applied above will be used for this calculation wrt. thesteering angle and steering speed constraints defined in equation (5.15). So we will get φbd andζbd.


Computation of the final state sf

Once we have obtained the control abd, ζbd and the profiles vbd and φbd we will get the headingand path profiles of the state sf by means of integration in equation (5.14) as follows:

θbd(t) = θ0 +∫ t

0

vbd(t)tan(φbd(t))

Ldt (5.25)

xbd(t) = x0 +∫ t

0

vbd(t) cos(θbd(t))dt (5.26)

ybd(t) = y0 +∫ t

0

vbd(t) sin(θbd(t))dt (5.27)

Finally, with all the previous results we will be able to calculate sf using the equation (5.9).

Trajectory Adjustment: DL-Tiji

It is possible, as shown in Figure 5.6 (a), that after the Tiji execution, we get a feasible trajectoryfulfilling all the constraints of the system but it is not free-collision wrt. the geometry of the road.To solve this problem we propose a splitted approach in the execution of Tiji.

The Algorithm 2 presents a 2-splitted version of Tiji that we will call Dichotomic Tiji.

Algorithm 2: Dichotomic Tijiinput : s0, sg, tg, Itinerary ΠA, Distance δoutput: p, success flag

[ p, success flag ] = Tiji(s0, sg, tg);1

path = GetPath(p,ΠA);2

if Collision(path,ΠA) then3

[ s1, t1 ] = Split Trajectory states(s0, sg, δ/2);4

[ p1, success flag1 ] = Dichotomic Tiji(s0, s1, t1, ΠA, δ/2);5

[ p2, success flag2 ] = Dichotomic Tiji(s1, sg, tg, ΠA, δ/2);6

return ({p1, p2} , success flag1 && success flag2);7

else8

return (p, success flag);9

As we can see, we execute Tiji, we check if there is a collision wrt. the road boundaries. Ifthe collision is positive we split the trajectory path in two parts and we recalculate Tiji for eachone of the half paths (by means of the corresponding states). We can see an example of that inFigure 5.6 (b).This process will be executed recursively until the moment we get a feasible total free-collision(wrt. the road) trajectory between the original states s0, sg, and tg, by means of a control list{p1, p2, ..., pk} corresponding to the piecewise control, built during the recursive dichotomic pro-cess.

Nevertheless, there exist the possibility that Dichotomic Tiji does not find the solution even ifit exists. If in Figure 5.6 (b), the state s1 in the subdivision, matches up with the crossroad point,there will be no solution for Dichotomic Tiji.


s0

sg

(a) (b)

s0

sg s1

Figure 5.6: (a) The initial trajectory between s0 and sg defined by the distance δ, has a collisionwrt. the environment structure. (b) Application of Dichotomic Tiji algorithm to subdivide thepath until we find a free-collision and feasible trajectory between s0 and sg.

Algorithm 3: n-Splitted Tijiinput : s0, sg, tg, Itinerary ΠA, Distance δ, noutput: p, success flag

[ p, success flag ] = Tiji(s0, sg, tg);1

path = GetPath(p,ΠA);2

if Collision(path,ΠA) then3

{[ s1, t1 ] , ..., [ sn−1, tn−1 ]} = Split Trajectory states(s0, sg, δ/n);4

for i = 1 to n− 1 do5

[ pi, success flagi ] = n-Splitted Tiji(si−1, si, ti, ΠA, δ/n);6

[ pn, success flagn ] = n-Splitted Tiji(sn−1, sg, tg, ΠA, δ/n);7

return ({p1, ... , pn} , success flag1 && ... && success flagn);8

else9


To solve this inconvenient, we propose a general n-Splitted Tiji described in Algorithm 3. Then,we will iterate the execution of n-Splitted Tiji, increasing the number of subdivisions (n) in eachiteration until a successful solution is found or n reaches a certain threshold ε.

This method will be the definitive Trajectory Planning for the Deliberative Layer that we willcall DL-Tiji. This process is depicted in Algorithm 4.

Algorithm 4: DL-Tijiinput : s0, sg, tg, Itinerary ΠA, Distance δoutput: p, success flag

i = 2;1

success flag = false;2

while ( i ≤ ε and not (success flag) ) do3

[ p, success flag ] = n-Splitted Tiji(s0, sg, tg, ΠA, δ, i);4

i = i+ 15



5.3 Urban Conflicts

5.3.1 Definitions

Once the agents know their respective paths, they will probably have to face several conflicts alongtheir trajectories.

We will define a conflict element of TG or GG for the agent A, as a common node or edgecontained in the route/itinerary of A and in another vehicle’s (agent or dynamic obstacle) path.

We will define a conflict region of an agent A, as a geometric stretch of its itinerary subject toa possible collision against another agent or dynamic obstacle in the system, that is to say, a stretchin common with another vehicle’s itinerary (urban resource to share). We will note CAi = [xA1 , x

A2 ]i

the ith conflict region along the itinerary ΠA of the Agent A limited between the interval pointsxA1 and xA2 .

We will call conflict situations the characterization of a conflict region. This characterizationis expressed by means of two features:

• The type of vehicles involved in the conflict: dynamic vehicles (conflict agent-agent or conflictagent-dynamic obstacle); or static vehicles blocking a lane which will be considered as conflictagent-static obstacle.

• The relative orientation between the vehicles in this conflict region, i.e. if the vehicles havethe same direction of movement or not.

We can list the different Conflict situations:

1. Conflict with Static Obstacles:

i. Conflict Agent-Static Obstacle.

2. Conflict with Dynamic Vehicles:

(a) Same direction of movement


(b) Opposite or crossing directions of movement


We have to give answers to the previous situations in terms of detection and conflict resolution.We will distinguish the detection of this Conflict situations, and the resolution process for each

one of them:

• Detection:

– The Sensor Driven Layer (3rd layer in the three layers architecture which belongs tothe Reactive Layer) will be in charge of detecting the conflict situations with staticobstacles (1.i.) by means of the evaluation of the specific sensor data of the vehicleand the construction/update of the corresponding occupancy grid for the sensor-rangeenvironment. This layer will be described in section 5.5.

– The Communication Driven Layer (2nd layer which belongs to the Reactive Layer) willbe in charge of detecting the conflict situations with vehicles in the same directionof movement: between Agent-Agent (2.(a).i.) and between Agent-Dynamic Obstacle(2.(a).ii.). For these cases, the detection process will correspond to the pilot surveillancesystem, described in section 5.4.5.


– The Communication Driven Layer will be also in charge of detecting the conflict situa-tions with vehicles in opposite or crossing directions of movement: between Agent-Agent(2.(b).i.) and between Agent-Dynamic Obstacle (2.(b).ii.). For these cases, the detec-tion process will be carried out by the external surveillance system, explained in section5.4.1.

• Resolution Method:

– Conflict Agent-Static Obstacle: Reactive Layer (Sensor-Driven Layer), method:RRT.

– Conflict in the same direction of movement: Reactive Layer (Communication-Driven Layer), method: Platooning.

– Conflict in opposite or crossing directions of movement: Reactive Layer (Communication-Driven Layer), method: Path-Time Diagram + Tiji Trajectory Planning.

The detection process and each one of the resolution methods will be exposed later.

In Figure 5.7 we can see a specific description of the Reactive Layer.

SENSOR DRIVEN LAYER

Static Obstacles

REACTIVE LAYER

t

s

Dynamic vehicles in the same direction Dynamic vehicles in opposite or crossing directions

External Platform placed in Infrastructure

Detection: External Surveillance

Resolution Method: Path-Time Diagram + TijiResolution Method: Platooning

Detection: Pilot Surveillance

Detection: Sensor Range + Occupancy Grid Update

Resolution Method: RRT Algorithm

COMMUNICATION DRIVEN LAYER

Figure 5.7: Reactive Layer Architecture.


5.3.2 Steps to calculate the conflict regions & Complexity

To evaluate a conflict situation, we previously need to calculate the conflict regions in the systemsubject to possible collision.

As we have indicated in the previous section, the detection of a conflict with static obstacles (1.i)and with dynamic obstacles in the same direction of movement (2.(a).i and 2.(a).ii) will be carriedout by sensor scanning and the pilot surveillance system respectively, so they will be detected andcalculated in a reactive way “on the fly” by the onboard platform of the vehicle. On the otherhand, the external surveillance system is focused on conflict regions and the process unit is placedin the infrastructure. The external surveillance needs to know where are the conflict regions wherewe will possibly have a collision in order to watch these “black points” and the vehicles approachingto them. Thus, first we will have to calculate the different CAi for each agent corresponding to cases2.(b).i and 2.(b).ii (opposite or crossing directions of movement); the result will be crossroads andtwo-way streets.

1. Conflict Elements Calculation: Each time the system calculates a new route for an Agent A,the external platform will store this route on the TG structure. Furthermore, this externalplatform will execute a process to detect common regions or crossing points between routeA

and the routes of the other vehicles (agents and dynamic obstacles). We will mark and storethese nodes or edges and the vehicles with these elements in common (i.e. common crossroadsand common two-way streets). Finally, the system will also update the GG structure withthe corresponding itinerary ΠA and the respective conflict elements, coming from the conflictelements in the TG.This process will visit each one of the K nodes and K-1 edges of routeA to check the possiblecorrespondence with another existing route, so we will have a complexity of O(K).

2. Conflict Regions Calculation: Due to the route planning process has been executed previ-ously, we have the itinerary ΠA. Moreover the conflict elements have been calculated in theprevious step. At this point, the external platform already knows which are the crossroadsand two-way streets subjects to have a potential collision and which are the vehicles involvedin it. Now, the external platform will calculate the stretches of the itinerary ΠA where acollision could take place, that is to say: CAi = [xA1 , x

A2 ]i. So, the result for an agent A will be

a list of pairs of coordinates [xA1 , xA2 ]i along ΠA. If ΠA were a real geometric path instead of

a simplification made by connection of edges (that we call itinerary), this process would bea curve intersection calculation, defining the interval [xA1 , x

A2 ] as a conflict trace (i.e the area

swept by the vehicle when moving along the path). Figure 5.8 would be an example of that.

Nevertheless, the real geometric path will be calculated “on the fly” while the vehicle ismoving, because the potential and continuous coordination along the way (e.g. coordina-tions to avoid collisions in the crossroads or against ahead vehicles) make impossible topredict/construct an intial trajectory/path to follow from the beginning. It implies thatcalculations following the model in Figure 5.8 (In [SLL98] is described how to compute suchtraces efficiently) are impossible at this point for us. Hence, we propose a simplification ofthe geometric model; such conflict regions will be calculated from the straight edges of ΠA.We will call Maximum Forbidden Distance between Ai and Aj (MFD(Ai,Aj)) to the lenghtof the conflict region CAi which crosses CAj ; and Forbidden Time Interval (FTI(Ai,Aj)) tothe time covered in the distance MFD(Ai,Aj). So MFD(Ai,Aj) represents a contact distanceof agent Ai with agent Aj , measured along the itinerary of the agent Ai, and MFD(Aj ,Ai)along the itinerary of Aj as depicted in Figure 5.9.

Considering a standard length L of the agents, we will estimate the MFD’s from the geometryof collisions as follows:

MFD(Ai, Aj) = 2L cotα+ 2L cscα (5.28)

MFD(Aj , Ai) = 2L cotα+ 2L cscα (5.29)


i

j

j

i i

i

j

jx1i

x2i

x2j

x1j

(a) (b)

Figure 5.8: Two intersecting vehicle traces, with CAi = [xAi1 , xAi

2 ] and CAj = [xAj

1 , xAj

2 ] respectively.

where the collision angle α will be replaced by 180◦ - α if the collision angle is greater than 90◦.

[xAi1 , xAi

2 ]i will be the interval points which limit MFD(Ai,Aj) in CAii . Iterating this process

for all the conflict regions along ΠA, we will obtain the MFD’s distances and the list of pairsof coordinates for each one of them.

We can also calculate the maximum contact time between Ai and Aj , FTI(Ai, Aj). AssumingAj crosses first the conflict region, Ai will have to wait until Aj overtakes the end of theconflict region (xAj

2 ), covering MFD(Aj ,Ai). FTI(Ai, Aj) will be calculated as follows:

FTI(Ai, Aj) =MFD(Aj , Ai)

Vj(5.30)

where Vj is the estimated velocity of Aj for the stretch MFD(Aj ,Ai).

5.4 Communication-Driven Layer & Aid-Coordination Frame-work

Collision Avoidance is one of the main objectives in mobile robotics, it becomes even more criticaland complex in a multi-robot environment. Several methods execute complete N-crossed calcula-tions which lead to a prohibitive increase of complexity when we are working with many agents.We propose to avoid exhaustive calculations implementing a solution with local “on the fly” coor-dinations only in the necessary moments for the right agents.

As we have already mentioned, our idea is to achieve a Collision Avoidance Strategy basedin a conflict elements surveillance, this will constitute an aid-coordination framework performedby the mentioned Communication-Driven Layer (CDL) within the Reactive Layer of our solution.

As we have previously exposed in section 4.2.2, we will use two different systems in our CDLto successfully detect and order a coordination in the conflict regions/situations:


A2

A1

MFD(A ,A )2 1

MFD(A ,A )1 2

Figure 5.9: Conflict Regions in a crossroad between straight edges forming an angle α, indicatedby means of MFD(A1,A2) and MFD(A2,A1).

1. External Surveillance System: Surveillance system focused on Conflict Urban Elements(e.g. Crossroads).

2. Pilot Surveillance System: Surveillance system focused on close ahead agents (agents inproximity).

We would want to emphasize the choice of this two surveillance systems which form an aid-coordination framework, not only for their functional characterization as we have already done,but for their conceptual meaning.The combination of both systems represents an attempt to achieve an effective union of short rangeawareness (inherent to the local acquisition of surrounding information made by the agent itself)and long range awareness (provided by the environment and its global access to the broadcasteddata), as we have depicted in Figure 4.4. Each agent has a lack of information that can be completedwith the help of an active environment assistance. This will represent a horizon extension for thevehicle. A correct and efficient balance of local and global information should locate us in anunbeatable context to resolve the different conflicts that the agents will find along their paths andfinally attain successfully their objectives.

5.4.1 External surveillance system

This is a process carried out by the external platform located in the infrastructure in order to de-tect an resolve the conflict situations with vehicles in opposite or crossing directions (cases 2.(b).i.and 2.(b).ii. of section 5.3).Since we know the itineraries ΠAi for the Agents and we have a model of future of the dynamicobstacles (assumption 3 of section 2.2), we will also know the possible conflict regions on theroadmap, that is, crossroads or streets. As described in section 5.3.2, these conflict areas can becalculated, therefore an entry in a Conflict-Element Table (CET) will be created for each one ofthem. For each registered crossroad/street in the CET, the external platform will track the posi-tion, velocity and possible collision of its conflict vehicles. The external platform will also calculatea “non-return point” for each couple: conflict area-vehicle. This “non-return point” (n.r.p.) isdefined as a geometric point in the itinerary of an agent after which it would be inevitable cominginto the conflict region (even in maximum braking), so this point will be calculated to take asecurity margin for our posterior coordination process, in this way, if our coordination requests to


stop the agent before arriving to the crossroad letting other vehicles pass, it will be able to do,achieving the coordination just before arriving to the n.r.p.

We can formalize the notion of n.r.p:Let us note u as a control ∈ U the control space of the agent A; s(t) = u(s0, t) as the state of A attime t; pos(si) as the geometric position of a certain state si and CA the next conflict region alongthe itinerary ΠA. We define n.r.p as follows:

nrp(A) = pos(si) iff ∀u, ∃ t (u(si, t) = sj ∧ pos(sj) ∈ CA ) (5.31)

Our external surveillance system will check, in constant loop, the possibility of collision and theproximity to the “non-return points” of the corresponding vehicles for each conflict region. Thus,a CET entry will trigger a collision alarm if both following conditions are satisfied:

1. The vehicles are about crossing their “non-return points”, after which we couldrisk to come into a conflict region with no guarantees of avoiding a crash.We can calculate if we are reaching the n.r.p. (approximating in 1 Dimension by meansof the abscissa of the itinerary ΠA), using the kinematic equation 2(xf − xi)a = v2

f − v2i

where xi and xf are the initial and final velocity respectively, and vi and vf are the initialand final velocity respectively. We assume a maximum braking −amax and a final velocityof 0 (the vehicle stops). We will consider the next conflict region (already calculated) asCA = [xA1 , x

A2 ]. Hence, we will approximate the n.r.p. by:

2(xA1 − nrp)(−amax) = 0− v2c (5.32)

nrp = xA1 −v2c

2amax(5.33)

where vc is the current velocity of the agent.

2. We are going to have a collision.Having access to all the broadcasted data coming from the agents (position, velocity profiles,etc), this is an easy calculation using simple kinematic equations. So, by means of the velocityof the agent and its distance to the crossroad, we can determine the period of time in whichthe agent is going to reach the conflict region. If this period overlaps with another vehicle’speriod, we know for sure there is going to be a collision.

If the evaluation of both conditions is positive, the alarm will trigger an order of coordinationto all the agents in conflict. This coordination order will be checked and confirmed back by theagents with an acknowledge signal (otherwise they will be considered as dynamic obstacles be-cause they might have a problem in their communication system). All the vehicles in conflict (ouragents and the dynamic obstacles) will be marked in “Coordination state”, they will be added in aCoordination List (CL) pending to resolve; finally, the Communication-Driven Layer will take thecontrol of the vehicle.

Afterwards, the external platform will execute a Priority Evaluation between the agents in theCoordination List to determine the order of crossing the conflict region. This priority assignmentwill take into account the traffic balance and the time to goal of the vehicles to satisfy an efficientevaluation. If there exist dynamic obstacles, they will take the maximum priority due to we haveno control on them, so it is our responsibility to avoid them.The final objective is to get a trajectory planning for each one of the agents in conflict, avoidingcollision between them in a coordinated way. To achieve the coordination, the first step is de-termine the priorities. The second step is to calculate the interval of time that agents reach theentrance and exit of the conflict region.Once the timing is obtained, our Trajectory Planning method Tiji will take place to define themotion between the corresponding states and times. The execution of these steps will entail theinherent coordination behavior.


Once the vehicles surpass the coordination region, they will be marked in ”Free“ state, theywill be deleted from the coordination list and the Deliberative Layer will recover the control of theagents.

1

a

b

1

a

b

Coord. Order

1

a

b

CETCE : 11

CETCE : 11

CETCE : 11

1

a

bCETCE : 11

CLab

(a) (b)

(c) (d)

n.r.p.

n.r.p.

n.r.p.

n.r.p.

n.r.p.

n.r.p.

ACK

n.r.p.

n.r.p.

ACK

b

t

S

1st

2nd

a

t

S

Figure 5.10: (a) Collision and proximity to non-return point (n.r.p) is detected. (b) The sys-tem sends a coordination order. (c) The agents accept the coordination order. (d) After thepriority evaluation, a Path-Time Diagram method is achieved between the vehicles, following thecorresponding priority assignment.

In Figure 5.10 we show the main steps in this process:

(a) The surveillance system checks in loop the state of the vehicles. Both conditions, proximityto the non-return points (n.r.p) and collision, are evaluated true (by means of the dynamicdata provided by the agents).

(b) The system triggers an alarm, thus a coordination order signal is sent to the agents in conflict.

(c) The vehicles send an ACK signal to acknowledge the system their positive coordination, anda Coordination List (CL) is created with the corresponding agents.

(d) The Priority Evaluation is executed by the external platform giving as result the priorityassignment and the order of crossing. The Agent “a” passes first and the Agent “b”, second.A Path-Time Diagram method is calculated to determine the interval of times of crossingthe conflict region for each agent (after determining the priority order of crossing). FinallyTiji will use the state and time information to compute a feasible trajectory for each one ofthe vehicles.


5.4.2 Priority Evaluation

This process will be executed by the external platform.

The objective is to construct a priority function which guarantees certain efficiency or optimal-ity in the coordination process. Since the evolution of the system is impossible to calculate, wecan not guarantee a global optimality, therefore the only purpose of the priority function has tobe the local optimality.

We establish two requirements to accomplish:

1. Reduction of traffic density in the incident paths to the conflict region (minimize the numberof affected vehicles).

2. Reduction of maximum time to goal for the affected vehicles.

Thus, the key of the problem is to find out which are going to be the vehicles affected by thecoordination.

(a)

n.r.p.

n.r.p.

t12

t22

t 21

t11

n.r.p. t31

t32MFD(A , A , A )2 1 3

MFD(A , A , A )3 1 2

MFD(A , A , A )1 2 3

n.r.p. A1S

t

x11 x12

t 11

t 12

MFD(A ,A )

FTI

A1

1 2A,

3

(A ,A )1 2A,

3

A1

(b)

S

t

x21 x22

t 11

t 12

A2n.r.p.

FTI

A2

t 22

t 21=

MFD(A ,A )2 1A,

3

(A ,A )2 1A,

3A1

A2

A2

S

t

x31 x32

t 11

t 12

A3n.r.p.

FTI

A3

t 22

t 21=

MFD(A ,A )3 1A,

2

A1

(A ,A )3 1A,

2

A2

A3

t 32

t 31=A3

A1

Figure 5.11: (a) Conflict Region definition between the agents A1, A2 and A3. We can appreciatethe non return points and the Maximum Forbidden Distances for each one of the agents. (b)Assuming the priority order: (A1, A2, A3), it is depicted a Path-Time Diagram resolution for eachone of the agents.

Let us show in Figure 5.11 an example of a conflict region in a crossroad between A1, A2 andA3. For this case we have 6 different permutations of priority order:

1. σ1 = (A1, A2, A3).

2. σ2 = (A1, A3, A2).

3. σ3 = (A2, A1, A3).

4. σ4 = (A2, A3, A1).

5. σ5 = (A3, A1, A2).


6. σ6 = (A3, A2, A1).

In general, having k agents, we will have k ! permutations of priority order (k will always be asmall number, because it can be as maximum the number of incident streets to a crossroad).

The velocity profile vi(t) of Ai will depend on its position in the priority permutation σj (thefirst priority agent will keep its velocity profile, the second will slow down wrt. the first one, thethird will slow down wrt. the second one, and so on).We will call ξ(Ai, σj) the set of vehicles behind the agent Ai, forced to modify its velocity profileto avoid a collision with Ai (if there exist this collision it will depend on the velocity profile of Aiwhich are determined by the priority permutation σj). If Ai is being coordinated in a crossroad,it may will slow down to hand over the priority to another agent. Having k vehicles in collisionconflict to coordinate, we will define the set χ(σj) as the union of all the vehicles affected by thecoordination with priority order permutation σj :

χ(σj) =k⋃i=1

ξ(Ai, σj) (5.34)

Our intention is to calculate the set χ(σj).

Coming back to the example in Figure 5.11, we will choose the first permutation order σ1 =(A1, A2, A3), and we will construct a Path-Time Diagram for each one of the agents followingthe established priority. Let us assume, for instance, that the initial velocity profile (discontinuousblue line) for each agent is our maximal velocity vmax mentioned in section 4.2.3, it intersects thegrey squares that represents the collision region wrt. the other agents. Then, we will keep thevelocity profile of the most priority agent (A1) and we will modify (depending on σj) the currentvelocity profile of the following priority agents, A2 and A3, to avoid the collision region as follows:

1. We have to execute a free-collision path planning from (0,0) to the end of the Conflict Region(with abscissa x22 for A2 and x32 for A3). We establish (0,0) as (n.r.p. , time(n.r.p.)).

2. We compute a straight line from our (0,0) to (x21, t12) for A2 and to (x31, t22) for A3. Theslope of this segment is the velocity for the stretch of itinerary between [n.r.p.A2),x21] and[n.r.p.A3 ,x31] respectively. From this point we calculate another line with the slope of themaximal velocity vmax.Remark: Actually, this would be a simplification process to calculate the resulting velocityprofile vi(t) for each agent Ai using the Path-Time Diagram wrt. a certain priority permua-tion (σ1 in this case).

3. Finally, we have the velocity profile for each agent in conflict assuming the permutation orderσ1. We also know the velocity for the agents coming behind A1, A2 and A3, so with simplekinematic equations we can estimate how many and which agents will have to modify theirvelocity to avoid the collision with A1, A2 and A3, i.e. ξ(A1, σ1), ξ(A2, σ1) and ξ(A3, σ1)

respectively. In this way we will obtain χ(σ1) =3⋃i=1

ξ(Ai, σ1).

This has been the calculation for the priority permutation σ1, but we will have to carry out thesame Path-Time Diagram evaluation for the other k !−1 permutations to obtain the correspondingχ(σi). It is important to underline that k is a very small positive number, so we will assume acomputation for all k ! permutations in real time.

Remark: If a vehicle Ai is not an agent, but a dynamic obstacle, it takes immediately thepriority in the permutation order.

Now, we define the requirements that we have initially presented as follows:

1. Traffic density in the incident paths to the conflict region:This is the cardinality of χ(σi): |χ(σi)|.


2. Maximum time to goal for the affected vehicles:This is max(T1, T2, ..., TN ) where Tk is the time of the agent Ak ∈ ξ(Ai, σj) ⊆ χ(σj), to reachthe goal point assuming a maximal velocity vmax along the rest of its itinerary.

Hence, we will choose the permutation order which minimizes the following performance index:

Υ(σi) = γ1max(T1, T2, ..., TN ) + γ2 |χ(σi)|︸︷︷︸N

(5.35)

where γ1 and γ2 are positive weighting constants.

5.4.3 Interval Calculation

In the Priority Evaluation step, we have obtained, for each priority permutation, a velocity profileon a Path-Time Diagram Representation for each agent (as shown in Figure 5.11 (b) for (σ1)).Now we will work on the Path-Time Diagrams of the chosen priority permutation σi which hasminimized Υ.

Let us assume (σ1) illustrated in Figure 5.11 as the chosen priority permutation. We can noticethat the entrance time of A2 corresponds to the exit time of A1 (t21 = t12) and the entrance timeof A3 corresponds to the exit time of A2 (t31 = t22), following the corresponding established orderof priority. By means of the slope of the segments in the Path-Time Diagram (the velocity profile)and using simple geometric operations, we can get the time interval [t11, t12] for A1 wrt. [x11, x12],[t21, t22] for A2 wrt. [x21, x22] and [t31, t32] for A3 wrt. [x31, x32].For each one of the agents we notice a first phase of deceleration (only the most priority agent willkeep its current velocity profile in this phase), and a second phase of acceleration in order to reachvmax.We will call the first phase as coordination phase, with an interval [n.r.p.Ai , x1i] along thepath axis and [ti1, ti2] in the time axis. The state was set to “Coordination” when the externalsurveillance system detected the collision and triggered the alarm, now, the state will be releasedto “Free” once the agent surpasses the coordination phase; so, the CDL will have the control ofthe vehicle along the path interval [n.r.p.Ai

, x1i] and time interval [ti1, ti2], and it will be releasedunder the control of the Deliberative Layer from (x1i, ti2) onward.In this way, our interest is focused on the coordination phase. Its corresponding interval will bethe result of this step and will represent the input for the Trajectory Planner Tiji.

Remarks about Velocity Profile Constraints

Using the Path-Time Diagram method exposed in the Priority Evaluation and Interval Calculationsteps, we will have to notice two important key points:

1. Constraints of Velocity Profile Generation: The velocity of our agents have a limit range dueto the physical and dynamic constraints such as the maximum torque of actuators. So, thereexist some limits for the speed of the vehicle as depicted in Figure 5.12.

The exposed figure describes a speed cone (inherent to any vehicle) which has to be takeninto account in the previous segment calculation. This cone defines a range of permissibleslopes; if the solution does not correspond to this range, it will have to be adjusted.

2. Continuity of Velocity Profile: The union of straight segments between the coordinationphase and the acceleration phase, represents a peak of infinite acceleration forbidden forthe dynamics of the vehicle. However, we will not take into account this fact because itwould complicate the calculation and the approximation using the current model proposedis enough to get suitable results.

Dynamic Obstacles and Model of Future

We would want to make some remarks about the Dynamic Obstacles vehicles of our system.


Figure 5.12: Range cone of permissible slopes (velocities).

• We follow a known model of future for the dynamic obstacles, as we have noticed in theassumption number 7 in section 2.2. We assume a deterministic model of future, that is to say,we know exactly the parameters of the vehicles along their paths (position and velocity profile,constantly updated and broadcasted), it determines which would be the collision region inthe Path-Time Diagram either for interval of time (FTI) and for interval of distance (MFD)with a uniform probability equal to 1 (i.e. certainty in the trajectories of the obstacles).However, we could also propose an alternative (and more real-life suitable) model, for therepresentation of the future. That is a probabilistic model based on gaussian distributionalong the radius of the collision regions.In Figure 5.13 we show a projection of this probabilistic model.Thus, we will have to take into account this fact to get the free-collision path solution of theproblem in the Path-Time Diagram.We would follow a bivariate gaussian distribution (si: itinerary curved abscissa, t : time):

P (si, t) = P (si)P (t|si) (5.36)

P (si) =1

σsi

√2πexp

(−νsi

2

)(5.37)

P (t |si) =1

σt√

2π√

1− ρexp

(− (νt − ρνsi)

2

2(1− ρ2)

)(5.38)

νsi=si − µsi

σsi

, νt =t− µtσt

(5.39)

where µ is the the corresponding mean, σ is the corresponding standard deviation and ρ isthe correlation between si and t.

• The dynamic obstacles are not supposed to have a spiteful or non-intelligent behavior. Therewill be situations, that it will be impossible to coordinate some agents with dynamic obstacles(e.g. When some vehicles are being coordinated in a crossroad and a dynamic obstacle arrivesto the conflict region with maximum velocity, there will be a collision). When some of thesesituations take place and we can not really coordinate the rest of the agents with the mobileobstacle, we will make the assumption that the dynamic obstacle will behave in oder toprevent the collision (e.g. modifying its velocity profile by itself).

5.4.4 Tiji in the CDL Trajectory Planning

The Trajectory Planning will be executed onboard.


S

t

t1

t 2

x x1 2

s

t

P(s ,t)

Figure 5.13: Projection of the probabilistic model of gaussian distribution (on the plane path-time)

We will execute the method Tiji described in section 5.2.2.

Considering the agent Ai, we assign to s0 the current state (x0,y0,θ0,φ0,v0) of Ai. The currentposition (x0,y0) represents the n.r.p.Ai

in the abscissa of the itinerary ΠAi.

We set sg , as follows: xg and yg will correspond to the geometric position of the entrance to theconflict region x1i (already known); θg and φg will be determined by the orientation wrt. the posi-tion x1i within the itinerary ΠAi

; finally, vg will correspond to the slope value of the coordinationphase of Ai, defined in 5.4.3.

The goal time tg will be the exit time tj2 of the agent Aj , where Aj is the agent with thepriority immediately on top of Ai.Unlike section 5.2.3, tg is fixed by the conflict situation and the coordination process, so, tg willnot be taken into account in the correction computation. Therefore, in contrast to equations (5.19)and (5.20), we will apply the original formulation (5.12) and (5.13):

∆s '[∂f

∂p

]∆p

corp = −τ[∂f

∂p

]−1

∆s

The calculation of linear acceleration profile, linear velocity profile, steering angle and steeringspeed profile and sf , will remain the same that in section 5.2.3.

Finally we will simply execute Tiji with the corresponding inputs detailed above.

5.4.5 Pilot surveillance system

As shown in Figure 5.7, the pilot surveillance is part of the Coordination Driven Layer, and isplaced onboard the vehicle.

A continuous evaluation of a range area ahead of an agent will be executed (by the agentitself). If the inter-vehicle distance reaches a minimum threshold, an alarm will be triggered anda short-range coordination order will be evaluated. If the coordination is accepted, the agents in


conflict will be marked in (Platooning)“Coordination” state. Then, the agents will carry out theexecution of a Platooning coordination method.After this process each agent will adjust its distance and velocity profile wrt. the vehicle aheadthat we will call its leader.Once the vehicles split up from their line formation (that is, their paths split and move away fromeach other, therefore the inter-vehicle distance increases and the line gets broken), they will getback to the “Free” state or they will continue in platooning formation in two different splittedgroups.

5.4.6 Platooning

We will often find several vehicles trying to circulate in the same road and in the same direction.That could be considered a conflict situation due to several agents sharing the same urban resource,nevertheless, knowing that they are taking a common path in the same direction and this isa temporary situation (because commonly they have different goals and they will separate atsome point), we could establish a flocking criteria as we can find in [Os06], [LJ05], [WRA07],[HGTP05]. The most significant articles, in our case, to achieve our flocking formation might be[DOK98] and [DOK01], where we can find changing shape formations for nonholonomic vehicles(as we can appreciate in Figure 5.14) based on the election of a flock leader and using a controlgraph representation that describes the behaviors of the robots in the formation.

Figure 5.14: Flock Formation of robots changing shapes

When the pilot surveillance system launches a coordination order for an agent in a flockingcoordination, we will add this robot to a control graph structure which represents a flock. We willfollow the model proposed by [DOK98] and [DOK01] to achieve our group formation and control.From the moment a robot belongs to the flocking, the flock leader will assume the responsibilitiesof the group in terms of cooperation and coordination with the environment and other agents todeal with conflict points and collisions. If two flocks surpass the threshold of proximity they willbe merged in a single one

If a robot reaches a point in its path where it has to abandon the formation, ending the coordi-nation (because its direction follows another route), we will delete the node from the correspondingcontrol graph.

In Figure 5.15 we can see some examples of flock formations in a control graph structure followingthe flocking control rules l-ψ and l-l as it is described in [DOK01].

The control rules l-ψ and l-l lead to the accomplishment of the three heuristic rules ofReynolds which characterize the flocking behavior: cohesion, separation and alignment .

Nevertheless, it is not necessary for our case to use both control rules. As we can notice in theimage “H” of Figure 5.15, we can manage a line formation of vehicles using only the l-ψ control,where a vehicle has to adjust its distance and velocity only wrt. the vehicle ahead. This is asuitable formation for our work usually known as Platooning.

In Figure 5.16 we can find a description for l-ψ control.In the case of l-ψ control, the state of the follower robot can be written in coordinates relative

to the lead robot as (l12, ψ12, θ2)T . In our case, the aim is to maintain a desired length ld12 and


Figure 5.15: Examples of flock formations depicted by means of a control graph structure.

Figure 5.16: l-ψ control.

a desired relative angle ψd12 between two consecutive robots. The kinematic equations for thefollower robot having l-ψ control is given by:

l12 = v2 cos γ1 − v1 cosψ12 + dω2 sin γ1 (5.40)

ψ12 =1l12

(v1 sinψ12 − γv2 sin γ1 + dω2 cos γ1 − l12ω1) (5.41)

θ2 = ω2 (5.42)

where γ1 = θ1 +ψ12− θ2 and vi, ωi (i = 1,2) are the linear and angular velocities at the centerof the axle of each robot.

Thus, the trajectory planning of platooning coordination corresponds to compute the kinematicequations above in order to copy/adopt the trajectory planning profile from the correspondingleader.

5.5 Sensor-Driven Layer

The SDL is part of the Reactive Layer. It is in charge of the detection and avoidance of the static-obstacles that an agent can find along the way while it moves forward towards its goal. Sinceit is a sensor-driven layer, it has to evaluate a response wrt. a short range environment as fastas possible, thus it has to guarantee a high level of reactivity. Therefore, it represents the most


priority level of the three-layers architecture.

In this layer a new environment representation will be built by means of the sensor data. Thewell known Occupancy grid mapping will be used to work with the short-range environment. Thebasic idea of the occupancy grid is to represent a map of the environment as an evenly spaced fieldof binary random variables each representing the presence of an obstacle at certain location. Thegoal of an occupancy mapping algorithm is to estimate the posterior probability over maps giventhe data: p(m|z1:t, x1:t) where m is the map, z1:t is the set of measurements from time 1 to t, andx1:t is the set of vehicle poses from time 1 to t. The controls and odometry data play no part inthe occupancy grid mapping algorithm since the path is assumed known.

Occupancy grid algorithms represent the map m as a fine-grained grid over the continuousspace of locations in the environment. The most common type of occupancy grid maps are 2Dmaps that describe a slice of the 3D world.

If we let mi denote the grid cell with index i (often in 2D maps, two indices are used to representthe two dimensions), then the notation p(mi) represents the probability that cell i is occupied.

Figure 5.17 illustrates two different examples of sensor capture and occupancy grid reconstruc-tion.

Figure 5.17: Two different examples of short range environment reconstruction by means of occu-pancy grid mapping method.

The occupancy grid map will be continuously updated by the sensor data. Along the way, anagent can find an obstacle partially or totally blocking a stretch of the planned path (we have acollision check, combining the occupancy grid map with the current path defined by the currenttrajectory). If this fact takes place, an exception will be triggered from the SDL to upper layers(depending which layer had the control of the vehicle: if the agent state was “Free” the exceptionwill be sent to the Deliberative Layer MDL, if the agent state was “Coordination” it will be sent tothe CDL). This exception will be break the current trajectory planning in order to give a responseto the immediate collision in the short-range environment detected by the sensors onboard. Thenan RRT method (exposed in [LaV04] by LaValle) will be executed to resolve a new free-collisiontrajectory planning, the solution will be also sent to the same interrupted layer to allow a replanningof the previously interrupted motion.


5.5.1 Trajectory planning: RRT

The trajectory planning for this layer will consist of constructing, in an iterative way, a treestructure covering the majority of the free Configuration Space Cfree as fast as possible.This process will take into account the non-holonomic model and the kinematic and dynamicconstraints of the agents as we have already mentioned for other trajectory plannings in section5.2.3:

xy

θ

φv

=

v cos(θ)v sin(θ)

v tan(φ)/Lζa

where L is the wheelbase of the agent.Assuming that the agent is moving forward, we establish the following constraints:

v ∈ [0, vmax] , |φ| ≤ φmax, |a| ≤ amax and |ζ| ≤ ζmaxOnce the agent detects a collision by means of the sensor data, it will construct the RRT modelfollowing the Algorithm 5.

Algorithm 5: RRTFunction: RRT(K ∈ N, xinit ∈ Xfree,∆t ∈ R)1

G.init(xinit);2

for i=0 to K do3

xrand ← random config(Xfree);4

Extend(G, xrand);5

end6

return G;7

Function: Extend(G, xrand)8

xnear ← nearest neighbor(G, xrand);9

u← select input(xrand, xnear);10

xnew ← new state(xnear, u,∆t);11

if collision free path(xnear, xnew) then12

G.add node(xnew);13

G.add edge(xnear, xnew, u);14

end15

return G;16

The tree G is initialized by adding an initial node xinit. A random configuration xrand ∈ Xfree

is chosen. Then, the nearest neighbor function finds xnear, the node of G that offers the best ap-proach to xrand. A control u is then chosen to drive the agent from xnear towards xrand respectingthe dynamic constraints noted above, the result will be a new state xnew reachable from xnear andas close as possible from xrand fulfilling the dynamic constraints of the system. If xnew ∈ Xfree,this state will be added to G.The path will be found when xnew ∈ Xfree ∩ Xgoal.

Figure 5.18 shows two examples of RRT Trajectory Planning taking into account the dynamicconstraints.

Once the trajectory has been obtained by means of the RRT between and initial (s0,t0) andfinal (sf ,tf ) state-time, (sf ,tf ) will be communicated to the corresponding upper layer, it will bethe responsible to recalculate a new trajectory from this point on, according to the previouslyinterrupted context, that is to say, an interrupted coordination in a crossroad (CDL), a “free”driving without coordinations (MDL), ...


Figure 5.18: Two examples of RRT Trajectory Planning.

Chapter 6

Tests & Results

Our solution of multiple autonomous vehicles navigation problem is a large subject difficult to to-tally implement for a master project. One of the most important parts in our three layers solutionis the Communication-Driven Layer (CDL). The ideas and methods of this layer represents ourmain contribution to this field. The communication, coordination and avoidance between differentagents to achieve an intelligent cooperative behavior, is one of the key points of our work. Theother layers (MDL and SDL) are also interesting and necessary to construct a final and globalsolution, but they are based in already tested and well-known methods, hence, our tests and re-sults will be only focused on the CDL implementation. In this way, we have programmed a trafficsimulation environment in order to test the Communicaton-Driven Layer.

The CDL has been implemented in C++ and tested on a desktop computer (Intel Core2DuoT2400, 1.83 GHz, 1 GB RAM, in Linux Ubuntu 8.04).

We have evaluated different characteristics of the evolution of the multi-agent system wrt. thenumber of vehicles.We have checked the following performance features:

1. Number of Possible Conflicts.

2. Average Number of Coordinations in Crossroads.

3. Average Number of Coordinations in Platooning.

4. Average Percentage of delay in the real arrival time of the agents to their corresponding goalswrt. an optimal arrival time (with no coordinations along).

5. Average Time spent in Crossroad Coordination.

6. Number of vehicles with no Coordination all along its way.

7. Number of vehicles which have only encounter Crossroad Coordinations along its way.

8. Number of vehicles which have only encounter Platooning Coordinations along its way.

9. Number of vehicles which have encounter both, Crossroad and Platooning Coordinations,along its way.

We have evaluated this features with the number of agents: 250, 200, 150, 100, 50, 25 and 10.We have obtained a good response in terms of reactivity and complexity of the system. Even witha large number of agents (250), the system has responded in real time. The cooperative behaviorhas been accomplished. The external surveillance system and the pilot surveillance system havebeen proved to be a good aid-coordination framework in the experiments.

In Figure 6.1 we can appreciate some screenshots of our solution. They describe some traf-fic situations in our implementation: platooning coordination, conflict regions coordination (e.g.

49

CHAPTER 6. TESTS & RESULTS 50

crossroads). The green squares represent “free” state agents under the control of MDL (we haveachieved a simple implementation of the Deliberative Layer based in a A* path following), the bluesquares represent dynamic obstacles, the orange squares represent agents in platooning coordina-tion, and the red squares are agents in crossroad coordination (oranges and reds under the controlof the CDL).

(a) (b) (c)

Figure 6.1: (a)Different Agents driving along the road in different states according to their color.(b)Example of Platooning Coordination (in orange) in a traffic jam situation. (c)Example ofConflict Region Coordination (in a crossroad), where several agents take the corresponding “Co-ordination” state (in red) and modify their velocity profile in order to avoid a collision.

6.1 Results

The tests have been executed in a graph representation of Grenoble, using a map coming fromOpenStreetMap (http://www.openstreetmap.org). After parsing the structure, we have adapted itto our graph model representation. The resulting number of nodes of the GG stucture (see section5.1) is 846 and the number of edges 985. The map covers all the “centre ville“ of Grenoble city.We have executed our tests for different number of vehicles in the system: 250, 200, 150, 100, 50,25 and 10. For each one of them we have launched 10 executions, and finally we have calculatedthe averages.Remark: For each execution we have fixed the number of dynamic vehicles to 25% of the totalquantity (e.g. for the tests with 100 vehicles, 75 have been agents and 25 have been dynamicobstacles).

Figure 6.2 illustrates the different values of the defined performance features.

Figure 6.2: Table of values of the performance features.

In Figure 6.3 we can notice the evolution of some features when the number of vehicles in thesystem vary. And Figure 6.4 depicts some other features, which describe the distribution of thenumber of vehicles wrt. the type of coordinations encountered along their way to goal.


Figure 6.3: Evolution of the performance features wrt. the number of vehicles in the system.

Regarding the corresponding results, we can extract some conclusions about the behavior andevolution of the system:

1. Number of Possible Conflicts: The number of possible crossing situations between agents iscalculated at the beginning. Afterwards, the agents will have to be coordinated (if they aregoing to collide) or not. It is normal to notice a decreasing tendency of possible crossingsituations as the number of vehicles decreases as well.

2. Average Number of Coordinations in Crossroads: It seems also correct that the number ofcoordinations in crossroads along the way of a vehicle, decreases as the number of vehiclesdecreases as well. We can appreciate that the average of coordinated crossroads for an agentvary between 3 to ∼0, from 250 to 10 agents

3. Average Number of Coordinations in Platooning: The results of Platooning coordination arenot so intuitive as others. The number increases as the number of vehicles decreases untilthe value of 150, afterwards it declines. The explanation has to do with the saturation of thecity. If there are a lot number of vehicles driving in an environment, they enter in platooningand keeps the formation as they go forward, that is to say, a traffic jam. When they abandonthe formation and join another platooning, the number of coordinations increases, but theyhave already covered a long distance. Whereas with few agents, they abandon and joingroups of vehicles more often, so the number of coordinations is higher than with traffic jamsituations. Nevertheless, after a certain peak quantity of vehicles in the environment, theyare not enough to meet and abandon formations, so the average decreases. In our case, thispeak which balances the increasing and decreasing tendency is placed in 150 vehicles.

4. Average Percentage of delay in arrival time: This is an important value in our results. Itrepresents the effect produced by the coordinations in the arrival time. With 250 vehicles inour system, the agents arrive, in average, with a 50% of delay wrt. an optimal arrival time(when an agent in maximal velocity, drives from start to goal with no coordination along theway, like if it was the only one in the system). Logically the value of this feature declineswith the quantity of vehicles (less conflicts entail to be closer of our optimal situation).

5. Average Time spent in Crossroad Coordination: At the beginning the coordination time ishigher because there are more number of vehicles to manage per crossroad, it takes moreaverage time to cross 4 than 2 vehicles. As the number of vehicles declines, logically, this


Figure 6.4: Distribution of the number of vehicles wrt. the type of coordinations encountered alongthe way.

value will decrease as well. We can also notice that the maximum time spent in crossroadcoordination (since an agent enters the crossroad until it leaves), is 2.24 s. for 250 agents(logically it is proportional to the velocity and distance parameters of the system). It indicatesa good management time in crossroads which does not produce high delays.

6. Distribution of the number of vehicles wrt. the type of coordinations: The distribution de-picted in Figure 6.4 describes a logical evolution wrt. the decreasing number of vehicles inthe system.

We can conclude a complete evaluation of the performance features which entail a good and logicalresponse of the CDL implementation.

Chapter 7

Conclusion

At the end of this master project we have been able to face a large subject, the multi-agentsnavigation problem in a structured environment.We have successfully combined methods found in the existing literature (some of them detailed inthe state of the art) with original own ideas.

7.1 Contribution

Our contribution to the multi-robot field with this work is based in the following key points:

1. Construction of a suitable Three Layers Architecture to control the multi-agent system underthe different kind of situations they can encounter along their way.

2. Construction of an aid-coordination framework within the CDL made by 2 platforms:

(a) Onboard Platform, with a pilot surveillance system.

(b) External Platform placed in the infrastructure, with a External surveillance system.

3. Combination of collision detection and avoidance mechanisms with a suitable Trajectoryplanning, Tiji, which allows to include this algorithm in a multi-robot coordination context(never done before).

4. Original combination of well known methods (flocking, path-finding methods like D* al-gorithm, path-time diagram coordination, RRT method, occupancy grid mapping,...) toconstruct a compact solution which allows to control a multi-agent environment in differentkind of conflict situations.

7.2 Objectives Achievement

The proposed method fulfills all the initial objectives and constraints of the system detailed insection 2.3:

Objectives:

1. Each agent has to reach its corresponding goal.

2. Travel time minimization.

3. Path length minimization.

4. Reactivity (low computation complexity).

5. Flexibility

Constraints:

53

CHAPTER 7. CONCLUSION 54

1. No collision between agents-agents.

2. No collision between agents-dynamic obstacles.

3. No collision between agents-static obstacles.

So, we can assert that the solution has accomplished all the goals fixed at the beginning of thisproject.

We have also to point out that we have successfully implemented one of the key layers ofour work, the Communication-Driven Layer, in charge of the conflicts detection and the globalcoordination behavior. The results have been evaluated and they have matched with a logical andgood performance of the system.

7.3 Future Work

Even though one of the main layers of the solution has been implemented, it will be important toinclude the Deliberative Model-Driven Layer and the Sensor-Driven Layer to a global solution.

In this work we have only considered some of the possible maneuvers a vehicle can encounteralong the route. However, other kind of maneuvers could be considered like passing maneuvers.

This has been a very interesting project which has totally fulfilled my expectations.

Annex A

Continuous Curvature Profile &Path planning

There exist several works in the path-planning domain trying to satisfy the non-holonomic con-straints of a car-like vehicle. Numerous articles, e.g. [BL89], [JPLM94], have been written in thisfield, but a large number of them generate sequence of Dubin’s curves [Dub57], i.e. path made ofcircular arcs connected by tangential line segments. These paths are the shortest ones for such avehicle, but their curvature is not continuous. Therefore, a vehicle following such a path wouldhave to stop at each curvature discontinuity to reorient its front wheels.Our objective is a forward path-planning. We want a path which geometrically allows us a con-tinuous movement without maneuvers, without having to stop the vehicle to reorient its wheels,i.e. with continuous curvature profile. Of course, in our coordination process, we might stop avehicle in order to synchronize the traffic in conflict regions, but this stop action will be achievedbecause of the coordination, not because of the path. Our path will remain feasible1 for continuousmovements.Let us now define the car-like model, the constraints and context that gives us the ContinuousCurvature Profile.

Let A denote an agent in the workspace ω, with R the reference point in the midpoint of the rearwheels axle. A configuration of A is defined by the 4-tuple (x,y,θ,κ) where x, y are the coordinatesof R in ω, θ is the orientation of A w.r.t the x axis, and κ the inverse of the turning radius of A(defined by the orientation of its front wheels by κ = tanϕ/L with ϕ the steering angle and L thelength of the vehicle). We can see a representation of a car-like vehicle in Figure A.1.

x

y

G

R

L

Figure A.1: car-like vehicle.

1Defined in A.1.

55

ANNEX A. CONTINUOUS CURVATURE PROFILE & PATH PLANNING 56

The classical constraints for a vehicle of forward movement are:

1. It can only move along a direction perpendicular to its rear wheels axle (continuous tangentdirection) with center of rotation G:

−x sin θ + y cos θ = 0 (A.1)

2. The distance ρ between R and G, i.e. the turning radius, is lower bounded by a given valueρmin, in this way, the curvature κ, is upper bounded by κmax = 1/ρmin (maximum curvature):

|κ| ≤ κmax (A.2)

Actually the equation (A.1) can be simplified, if we consider the derivative of x and y w.r.ttime t. So, taking v the velocity of the vehicle:

dx

dt= v cos θ

dy

dt= v sin θ

(A.3)

The orientation θ is the direction tangent to the curve followed by the point R (from (A.1)),its derivative wrt. the curved abscissa of R is the curvature κ. Thus, the derivative of θ wrt. timeis κ multiplied by the derivative of the curved abscissa of R wrt. time (the velocity v), so:

dθ

dt= vκ (A.4)

To these classical constraints, we will add a new one:The curvature derivative is upper bounded, so the vehicle can only reorient its front wheels with afinite velocity:

|κ| ≤ σmax (A.5)

In summary, we can express the kinematic constraints of the agent A by the system:

dx

dt= v cos θ

dy

dt= v sin θ

dθ

dt= vκ

|κ| ≤ κmax

|κ| ≤ σmax

(A.6)

Remark: we will not take into account dynamic constraints. So we will work with the assump-tion not-sliding wheels.

A.1 Local Path-Planner

As we have already said, we want a forward path-planning, feasible and smooth (without maneu-vers).

Let C be the configuration space of the vehicle A: C ⊂ ω × S1 × [−κmax, κmax]. A path is acontinuous oriented curve of C, a continuous sequence of configurations of A. A path is a continuousfunction R→ C: s→ γ(s) ∀s ∈ [0, l] where l is the length of γ(s).


Feasible Path: A path will be feasible if and only if, it has a finite length and it follows thekinematic constraints of the equations (A.6), i.e. it has a continuous curvature profile.

A feasible path will be smooth (without maneuvers) if and only if, the vehicle can drive allalong this path with strictly positive velocity (v > 0).

We can notice that paths based on Dubins’ curves [Dub57] can be considered as feasible, butnot as smooths. So, as it is proposed in work [SFaG97] and [Sch98], we will define a set of pathsderived from Dubins’ curves, which keep the feasible and smooth properties. These paths containat most eight pieces, each piece being either a line segment, a circular arc of maximum curvature,or a clothoid arc. Following the Scheuer’s notation, they will be called SCC-paths (for SimpleContinuous Curvature paths). So we present our solution as an extension of the Dubins’ work.His work about curves covered six types of configurations: LSL, LSR, RSL, RSR, LRL and RLR,where S denotes a straight line segment, and L,R left turn circular arc and right turn circular arc,respectively. The proposed extension, in order to avoid the not-smoothness, is to add clothoid arcsas part of the left turn action and right turn action, becoming:L 7→ C+σ L C−σ andR 7→ C−σ R C+σ

where C+σ is a clothoid arc with κ = σ, and C−σ a clothoid arc with κ = −σ.The curvature properties of clothoid guarantees the desired smoothness. We can check A.2 toreview the definition and properties of clothoid curves.In Figure A.2 we show a SCC-path between two configurations qa and qb of type:C+σ L C−σ S C−σ R C+σ.

qa

qb

1

2

3

4

56

7

_

1

2

3

4

5

6

7

_

1

2

3

4

5

6

7

(a)

(b)

(c)

Figure A.2: (a) Curvature profile of SCC-path in c (b) Derivative of the curvature profile ofSCC-path in c (c) SCC-path of type C+σ(1) L(2) C−σ(3) S(4) C−σ(5) R(6) C+σ(7).

Hence, the SCC-path will represent a local path-planning resolution between the configurationqa and qb. To make complete this process we will have to check if this SCC-path corresponds to a


free-collision path or not.So we will assume the existence of a Local Path Planner which achieves this process using SCC-paths (calculation of a suitable and free-collision sequence of line segments, circular arcs andclothoid arcs between configurations qa and qb ).

A.2 Definition and properties of Clothoids

A Clothoid, is a 2D curve in the affine space whose curvature changes linearly with its curvelength. Clothoids are also commonly referred to as Euler Spirals or Cornu spirals. We note κ asthe curvature and σ as the derivative of the curvature wrt. the curved abscissa:

κ =dθ

ds(A.7)

σ =dκ

ds=d2θ

ds2(A.8)

When σ is null, the clothoid is a straight line or a circular arc.We can get the following equations from (A.7) and (A.8):

∀s ∈ R, κ(s) = σs

∀s ∈ R, θ(s) =∫ s

0

κ(u)du = σs2/2

Thus, taking this reference, the curve coordinates are:

∀s ∈ R,

x(s) =

∫ s

0

cos θ(u)du =∫ s

0

cos (σ

2u2)du

y(s) =∫ s

0

sin θ(u)du =∫ s

0

sin (σ

2u2)du

These integrals are not calculable, but they could be obtained from two classical integrals calledFresnel Integrals, defined respectively as:

∀x ∈ R, F rC(x) =∫ x

0

cos (π

2u2)du, FrS(x) =

∫ x

0

sin (π

2u2)du

Using these integrals, the previous coordinates of the curve are translated into:

∀s ∈ R,

x(s) =

√π|σ| FrC

(√|σ|π s

)y(s) = sgn(σ)

√π|σ| FrS

(√|σ|π s

)An example of the shape of a clothoid is shown in Figure A.3.

Finally we can notice that even if the Fresnel Integrals can not be calculated, they can beestimated by two options:

1. Using integral approximation methods (Newton’s method, Simpsons’, etc...)

2. These integrals correspond to the following Taylor series [LG90]:

∀x ∈ R,

F rC(x) =∞∑n=0

(−1)nπ2nx4n+1

22n(4n+ 1)(2n)!, F rS(x) =

∞∑n=0

(−1)nπ2n+1x4n+3

22n+1(4n+ 3)(2n+ 1)!


- 3 - 2 - 1

1 2 3

- 3

- 2

- 1

0.5

0.0

−0.5

−1.0

x

1.0

−1.0 −0.5 0.0 0.5 1.0

y

Figure A.3: Cloithoids (or Cornu Spirals) function.

Bibliography

[BL89] J. Barraquand and J.-C. Latombe. On non-holonomic mobile robots and optimalmaneuvering. In Revenue d’Intelligence Artificielle, pages 3(2): 77–103, 1989.

[Bro85] Rodney A. Brooks. A robust layered control system for a mobile robot. Technicalreport, Cambridge, MA, USA, 1985.

[DFMG09] Vivien Delsart, Thierry Fraichard, and Luis Martinez Gomez. Real-Time TrajectoryGeneration for Car-like Vehicles Navigating Dynamic Environments. In IEEE Int.Conf. on Robotics and Automation, Kobe Japan, 2009.

[DOK98] Jaydev P. Desai, Jim Ostrowski, and Vijay Kumar. Controlling formations of multiplemobile robots. In in Proceedings of the IEEE International Conference on Roboticsand Automation, pages 2864–2869, 1998.

[DOK01] J. Desai, J. Ostrowski, and V. Kumar. Modeling and control of formations of nonholo-nomic mobile robots. IEEE Transactions on Robotics and Automation, 17(6):905–908,December 2001.

[Dub57] L.E. Dubins. On curves of minimal length with a constraint on average curvature, andwith prescribed initial and terminal positions and tangents. In American Journal ofMathematics, pages 79: 497–516, October 1957.

[ELP87] Michael Erdmann and Tomas Lozano-Perez. On Multiple Moving Objects, 2:477–521,1987.

[Gat99] Erann Gat. On three-layer architectures. In Artificial Intelligence and Moble RObots,David Kortenkamp, R. Peter Bonnasso, and Robin Murphy, eds., AAAI Press, Cali-fornia Institute of Technology, 4800 Oak Grove Drive, Pasadena, 1999.

[GG00] Paolo Gallina and Alessandro Gasparetto. A technique to analytically formulate andto solve the 2-dimensional constrained trajectory planning problem for a mobile robot.J. Intell. Robotics Syst., 27(3):237–262, 2000.

[HGTP05] Ali Jadbabaie Herbet G. Tanner and George J. Pappas. Flocking in teams of nonholo-nomic agents. 309, 2005.

[HJ02] Kao-Shing Hwang and Ming-Yi Ju. Speed planning for a maneuvering motion. J.Intell. Robotics Syst., 33(1):25–44, 2002.

[HSS84] J.E. Hopcroft, J.T. Schwartz, and M. Sharir. On the complexity of motion planningfor multiple independent objects: pspace-hardness of the ‘warehouseman’s problem’.International J. of Robotics Research, 3(4):76–88, 1984.

[INS01] Luca Iocchi, Daniele Nardi, and Massimiliano Salerno. Reactivity and deliberation: Asurvey on multi-robot systems, 2001.

[JPLM94] M. Tax J.-P. Laumond, P.E. Jacobs and R. M. Murray. A motion planner for non-holonomic mobile robots. In IEEE Trans. Robotics and Automation, pages 10(5):577–593, October 1994.

60

BIBLIOGRAPHY 61

[KH97] Yutaka J. Kanayama and Bruce I. Hartman. Smooth local-path planning for au-tonomous vehicles. Int. J. Rob. Res., 16(3):263–284, 1997.

[Kha86] O. Khatib. Real-time obstacle avoidance for manipulators and mobile robots. Inter-national Journal of Robotic Research, 5(1):90–98, 1986.

[LaV04] Steven M LaValle. Planning algorithms, 2004.

[LG90] R. Liscano and D. Green. A method of computing smooth transitions between a se-quence of straight line paths for an autonomous vehicle. In Technical Report ERB-1031NRC No. 31776, National Research Council of Canada, Div. of Electrical Engineering,Ottawa (CA), June 1990.

[LH98] Steven M. Lavalle and Seth A. Hutchinson. Optimal motion planning for multiplerobots having independent goals. IEEE Trans. on Robotics and Automation, 14:912–925, 1998.

[LK03] Savvas G. Loizou and Kostas J. Kyriakopoulos. Closed loop navigation for multiplenon-holonomic vehicles. In IEEE Int. Conf. on Robotics and Automation, pages 420–425, 2003.

[LpLT] Stephane Leroy, Jean paul Laumond, and All The. Path coordination for multiplemobile robots: a resolution complete algorithm.

[LJ05] Magnus Lindh, Petter gren, and Karl Henrik Johansson. Flocking with obstacle avoid-ance: A new distributed coordination algorithm based on voronoi partitions. In ICRA,pages 1785–1790. IEEE, 2005.

[MLT05] Geoffrey Gordon Anthony (Tony) Stentz Maxim Likhachev, David Ferguson and Sebas-tian Thrun. Anytime dynamic a*: An anytime, replanning algorithm. In Proceedingsof the International Conference on Automated Planning and Scheduling (ICAPS), June2005.

[Os06] Reza Olfati-saber. Flocking for multi-agent dynamic systems: Algorithms and theory.IEEE Transactions on Automatic Control, 51:401–420, 2006.

[Ove98] Mark H. Overmars. Coordinated path planning for multiple robots. Robotics andAutonomous Systems, 23:125–152, 1998.

[RCR06] C. de la Cruz R. Carrelli and F. Roberti. Centralized formation control of non-holonomic mobile robots. 2006.

[Sch98] Alexis Scheuer. Planification de chemins courbure continue pour robot mobile non-holonome. Informatique, L’Institut National Polytechnique de Grenoble, January 1998.

[SFaG97] A. Scheuer, Th. Fraichard, and Rhone alpes Gravir. Continuous-curvature path plan-ning for car-like vehicles. In In Proc. of the IEEE-RSJ Int. Conf. on Intelligent Robotsand Systems, pages 997–1003, 1997.

[SLL98] T. Simon, S. Leroy, and J. P. Laumond. A collision checker for car-like robots coordi-nation. In In IEEE Int. Conf. on Robotics and Automation, Leuven (Belgium, pages46–51, 1998.

[SO95] Petr Svestka and Mark H. Overmars. Coordinated motion planning for multiple car-likerobots using probabilistic roadmaps. In ICRA, pages 1631–1636, 1995.

[SS83] J.T. Schwartz and M. Sharir. On the piano movers’ problem: III. Coordinating themotion of several independent bodies: The special case of circular bodies moving amidstpolygonal obstacles. International Journal of Robotics Research, 2:46–75, 1983.

[Ste94] Anthony Stentz. Optimal and efficient path planning for partially-known environments.In In Proceedings of the IEEE International Conference on Robotics and Automation,pages 3310–3317, 1994.

BIBLIOGRAPHY 62

[Ste95] Anthony Stentz. The focussed d* algorithm for real-time replanning. In In Proceedingsof the International Joint Conference on Artificial Intelligence, pages 1652–1659, 1995.

[SY84] Paul G. Spirakis and Chee-Keng Yap. Strong np-hardness of moving many discs. Inf.Process. Lett., 19(1):55–59, 1984.

[War90] Charles W. Warren. Multiple robot path coordination using artificial potential fields.1990.

[WH09] Augie Widyotriatmo and Keum-Shik Hong. A new navigation method of multiplenonholonomic vehicles with collision avoidance. 2009.

[WP95] Jing Wang and Suparerk Premvuti. Distributed traffic regulation and control for multi-ple autonomous mobile robots operating in discrete space. In ICRA, pages 1619–1624,1995.

[WRA07] Randal W. Beard Wei Ren and Ella M. Atkins. Information consensus in multivehiclecooperative control. 2007.

[YLP05] Kamal Gupta Yi Li and Shahram Payandeh. Motion planning of multiple agents invirtual environments using coordination graphs. 2005.

[YM99] Xianyi Yang and Max Meng. Real-time motion planning of car-like robots. 1999.

Documents

Cooperative Navigation for Car-Like Vehiclesemotion.inrialpes.fr/fraichard/publications/... · Introduction In the last decade, there has been a quickly growing literature on planning