school of industrial and information engineering · tuata attraverso un metodo randomizzato noto in...

POLITECNICO DI MILANO

Laurea Magistrale (MSc) in Automation and Control Engineering

School of Industrial and Information Engineering

Multi-RRT?: A sample-based algorithm

for multi-agent planning

Supervisor: Prof. Maria Prandini

Master thesis of:

Paolo Verbari, matricola 849854

Academic Year 2016-2017

� God, grant me the serenity to

accept the things I cannot change,

courage to change the things I can,

and wisdom to know the

difference.�

Tommaso Moro

To my family, the reason that i have to improve myself, and overcome

my limits. There is nothing i cannot do if they are by my side

1

Contents

1 Introduction 1

2 Sample-based planning algorithms 4

2.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4

2.2 Sample-based planning algorithms . . . . . . . . . . . . . . . . . . . . . . 6

2.3 Rapidly exploring Random Trees planners . . . . . . . . . . . . . . . . . 7

2.4 Multi-agent planning . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13

3 Multi-RRT* 14

3.1 Problem description . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15

3.2 Bargaining solutions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15

3.3 Proposed solution . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17

3.4 Conclusions about the algorithm . . . . . . . . . . . . . . . . . . . . . . . 19

3.5 Pseudocode . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20

3.6 Main functions description . . . . . . . . . . . . . . . . . . . . . . . . . . . 22

3.6.1 Find path tree noplot . . . . . . . . . . . . . . . . . . . . . . . . . 23

3.6.2 Check collision . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23

3.6.3 Removing nodes noplot . . . . . . . . . . . . . . . . . . . . . . . . 23

3.6.4 Count missing nodes . . . . . . . . . . . . . . . . . . . . . . . . . . 24

3.6.5 Continue rrt noplot . . . . . . . . . . . . . . . . . . . . . . . . . . . 24

3.6.6 Compara path . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24

3.6.7 steernew2 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24

3.6.8 noCollision . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25

3.6.9 Interpola path . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25

3.7 Other functions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25

2

3.8 Made by Say Vemprala . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26

4 Simulation results 27

4.1 Hypothesis and more . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27

4.2 Data structure . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29

4.3 Solution found, weakness and strength . . . . . . . . . . . . . . . . . . . 30

4.4 Some improvements . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33

4.5 Interpolation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36

4.6 Configuration 1 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37

4.7 Configuration 2 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43

5 Conclusions 56

Bibliography 57

A Appendix: MATLAB code 60

3

Sommario

Questa tesi affronta il problema della pianificazione delle traiettorie nel caso di piu

agenti che condividono la stessa regione dello spazio e devono mantenere una certa

distanza di sicurezza l’uno dall’altro ed evitare eventuali ostacoli. Il problema viene

affrontato secondo uno schema decentralizzato coperativo, in cui gli agenti comunicano

tra di loro le informazioni relative alla traiettoria prescelta, che viene eventualmente

modificata se vengono rilevate situazioni di conflitto. L’idea e di far risolvere il con-

flitto all’agente per il quale il peggioramento della funzione di costo e piu limitato.

Iterazioni multiple sono richieste nel caso in cui la modifica di traiettoria da parte del

singolo agente prescelto non sia risolutiva per l’intero sistema. La convergenza ad una

soluzione senza conflitti e garantita in un numero finito di iterazioni per costruzione,

dato che ad ogni iterazione il numero di conflitti viene diminuito. Tale soluzione e per

construzione Pareto ottima. La pianificazione di ogni singola traiettoria viene effet-

tuata attraverso un metodo randomizzato noto in letteratura come rapidly exploring

random tree star (RRT?) che e in grado di gestire cifre di merito e vincoli, e viene

quindi esteso in questa tesi al caso multi-agente. L’algoritmo multi-RRT? proposto e

stato testato in alcuni scenari e ha mostrato risultati promettenti.

Abstract

This thesis addresses the problem of trajectory planning for multiple agents moving

in the same region. Agents need to be at some safety distance one from the other

and to avoid collisions with obstacles. The resulting multi-agent planning problem

is addressed via a cooperative decentralized scheme where agents exchange informa-

tion on their planned trajectories, which are then possibly modified if if some conflict

is detected The resolution scheme is iterative, and at the first iteration, all agents

replan to avoid conflicts, and the trajectory of the agent with minimal performance

degradation is chosen. If there is still a conflict, then, at each iteration the same

procedure is repeated by all agents except the ones that already changed their tra-

jectory. Given that the number of conflicts is reduced at each iteration, convergence

is guaranteed in a finite number of iterations. Furthermore, the obtained solution is

Pareto optimal. Trajectory planning for each single agent is performed via the rapidly

exploring random tree star (RRT?) algorithm, which is able to handle cost functions

and constraints, and is thus extended here to a multi-agent setting. The resulting

multi-RRT? algorithm is tested in different scenarios and shows promising results.

Chapter 1

Introduction

In artificial intelligence, planning means searching for a sequence of logical operators

or actions that transform an initial world state into a desired goal state ([15]), where

the world state is the way the problem is modeled, and a plan does not necessarily

imply any motion. A plan can be used to solve a puzzle, like the 8 puzzles, through the

State Space Search, or like Sudoku through the Constraint satisfaction problem. This

is exactly the kind of planning that is included in the definition given before, charac-

teristic of the artificial intelligence viewpoint. These examples further demonstrate

that planning does not involve designing any trajectory for a moving agent.

In robotics instead, reference is made to “motion planning”, which means designing

the trajectory that drives an agent from an initial configuration to a final one without

colliding with any obstacle. This notion of planning can be applied to many different

contexts. We could plan the trajectory of a robotic manipulator inside a working cell,

minimize its effort to pick an object while avoiding collisions with objects or even

humans present in the working cell. We could plan the trajectory for an autonomous

vehicle, moving in our homes. We could plan the movement of virtual agents, like

characters of a video-game.

Many planning problems are also common life problems. Like ”The Moving fur-

niture problem”, better known as the ”Piano problem”, or also the ””Wreckless”

driving problem”. We steadily solve many planning problems everyday. For other

planning examples the interested reader is referred to [15, chap 1, pag 5-17].

It is perhaps worth noting that in order to move a robot, or an agent in an envi-

ronment in order to perform a task, it is not always necessary to plan a trajectory.

1

1 – Introduction

Indeed, there exists robots perfectly able to achieve their goal without planning a

trajectory at all. These are the “reactive navigation robot”, [4, pag 87-91]. Example

of this kind of robots are the ”Braitenberg Vehicles”, the Simple Automata, and the

bugs. The interested reader is referred to [3]. One commercial, very common example

of an autonomous vehicle performing a task inside an environment without planning

a trajectory is the ”Roomba” robot. This robot, at least in its early versions, is mov-

ing randomly inside an environment, continuously cleaning the floor with brushes,

and collecting the dust. It is only able to perceive, through bumpers, the obstacles

that finds on its way, and to find a direction to avoid them after bumping into them.

Roomba is not able to plan a complete trajectory, not even to keep in memory its

own position, or information about the environment, but it fulfills its task to fully

clear the floor anyway.

In this thesis, we shall address the problem of collision-free trajectory design but we

shall use the term “planning” in place of “motion planning” for the sake of simplicity.

Moving an artificial agent in an environment is a complex problem, which involves

some tasks. The agent has to identify its configuration in the space correctly, i.e.,

it has to solve the localization task, [20, chap 7-8]. At the same time, the agent

should be able to represent correctly the environment, including obstacles, leading to

the so-called mapping task. These tasks are not simple to achieve, in fact an agent

could lose knowledge on its position and orientation inside a map due to numerical

uncertainties. Also, building a map could be difficult, due to error in sensors, or

the changing nature of the environment itself. Furthermore, these two tasks must be

often solved together, leading to the SLAM problem (simultaneous localization and

mapping), [20, chap 9-13].

After the agent has built a map of the environment where it is moving and knows

its position and orientation inside the map, it has to determine a trajectory that

guarantees to avoid obstacles. Among all collision free trajectories, the agent typically

looks for an optimal one, e.g., trying to minimize the time to reach the goal region,

or some other cost function weighting the control effort, [15], [18].

This trajectory is found through some collision detection and resolution algorithm,

[15], [4, chap 5], possibly taking also into account uncertainties due to sensors or

actuators, or the environment, [15], [24], besides the other agents sharing the same

environment. Possible solutions to account for uncertainty consists in adopting a

2

1 – Introduction

probabilistic approach, or a robust approach where obstacles are grown in size to

compensate the estimation errors.

We can then conclude that designing an autonomous agent that is able to move,

or, in other words, an autonomous vehicle, is not easy, and is an interdisciplinary

problem involving different expertise.

In this master thesis, we shall consider a multi-agent setting, where agents have

to agree on a joint maneuver that drives each one of them from origin to destination,

avoids possible obstacles, and maintains each pair of agents at a prescribed safe dis-

tance so as to avoid conflicts. This multi-agent conflict detection and resolution task

is motivated by (but not restricted to) air traffic management.

According to the Single European sky project, [1], “The EU’s main objective is

to reform ATM in Europe in order to cope with sustained air traffic growth and op-

erations under the safest, most-cost and flight-efficient and environmentally friendly

conditions. This implies de-fragmenting the European airspace, reducing delays, in-

creasing safety standards and flight efficiency to reduce the aviation environmental

footprint, and reducing cost related to service provision.”

In a few words, the Single European Sky project will:

• allow the European Union to manage and regulate the European airspace;

• give more freedom to the airplanes above a certain flight level (not enforcing

them to fly along pre-defined “air corridors”);

• permit a more flexible management of trajectories, in such a way to reduce the

costs of each airline, and also to provide an increment in terms of number of

flights, reducing the workload of both the air traffic controllers in the control

towers on the ground and the pilots in the aircraft in the air.

This is an interesting and valuable project, with a significant impact of society since

it will improve the efficiency in air transportation. However, to make it a reality,

automatic algorithms are needed to identify and avoid conflict situations in which

two or more aircraft get closer than a minimum safe distance. This is instrumental

to improve the quality of the service while assuring safety of the trajectories.

This thesis tries to contribute to this project by developing automated conflict

detection and resolution algorithms for a multi-agent system.

3

Chapter 2

Sample-based planning algorithms

2.1 Introduction

Planning algorithms have been widely treated in literature, mainly with reference to

a single agent, addressing the problem of obtaining an optimal and feasible solution in

both known and partially known environments (the interested reader is referred to,

e.g.,[15], [4, chap 5]). Before presenting the different categories of planning algorithms,

we need to introduce a commonly adopted representation of an agent to the purpose

of motion planning.

An efficient scheme for motion planning is obtained by viewing the robot as a mobile

point in an appropriate space. Workspace obstacles are also represented in the same

space by adopting an appropriate transformation mapping. To this end it is natural to

refer to the generalized coordinates of the mechanical system, whose value identifies

the configuration of the robot. This allows to associate to each pose of the robot a

point in the C-space (configuration space), which is then defined as the set of all the

admissible poses that the robot can assume, [15, chap 1 pag 5-17],[21, chap 12 pag

535-536 ]. The configuration space is the joint space, the space whose coordinates are

the state of each single joint of the robot.

Whilst this approach is mostly used in robotics for trajectory planning of a robotic

manipulator, the described uniform representation and the introduction of the C-

space allows to apply the algorithms described in this chapter to a wide range of

planning problems.

These algorithms can be divided into two main categories:

4

2 – Sample-based planning algorithms

Sample-based planning algorithms conduct a search that probes the C-space with a

sampling scheme. More precisely, a set of points is sampled from the free-space

and connected in order to build a tree of feasible trajectories, that are then

used to determine the solution to the planning problem. Obstacle avoidance is

achieved by adopting a collision detection module that eliminates those trajecto-

ries where a collision occurs from the set of feasible trajectories, which simplify

the construction of the tree. This kind of algorithms are Probabilistic complete,

which means that, if there exists a solution, the probability that they find a

feasible solution converges to one, as the number of samples tend to infinity.

Certain versions of these algorithms, and can account for a cost criterion rating

the quality of different feasible trajectories and are also asymptotically optimal,

in probability. The most important sampling-based planners to date are Proba-

bilistic RoadMaps (PRM) [12, 13] and Rapidly-exploring Random Trees (RRT)

[14], which are further discussed in Section 2.2.

Search based planning algorithms look for the sequence of actions that drive the

agent from a start to a goal state in an optimal way, by constructing a graph

of states and searching a solution into the graphs, [16, 17]. In particular, they

require to discretize the continuous state space into a finite set of states, defining

the possible actions between these states and applying graph search to find an

optimal solution. The solution found strictly depends on the adopted discretiza-

tion scheme, and therefore efforts have been spent to efficiently divide the state

space, or creating a State Lattice discretization, a discretization of the C-space

into a set of states representing configurations and connections between them,

where every connection represents a feasible path, [16].

These two different families of algorithms have many differences: The first one

contain probabilistic complete algorithm, the latter instead contain algorithms that,

if the state space is well defined, guarantee the termination, and the completeness of

the algorithm, that means that if a result is returned it is a solution, otherwise the

algorithm will return a fail.

In this thesis, we adopt a sample-based algorithm for the following reasons

i) it can easily account for obstacles via a collision detection module

ii) it allows to account for the agent’s dynamics and related constraints, [15, chap

5


14]1

iii) it is typically faster with respect to a search based algorithm.

2.2 Sample-based planning algorithms

There are many sample-based planning algorithms in the literature. Here we mention

the most known and relevant ones, those for which probabilistic completeness has been

proven.

The most important sampling-based planners to date are Probabilistic RoadMaps

(PRM) [12, 13] and Rapidly-exploring Random Trees (RRT) [14].

Probabilistic RoadMap (PRM) planner is a multiple query method that first con-

structs a topological graph called roadmap representing a set of collision-free

trajectories, and then answers queries by computing the shortest path connect-

ing the initial state with the final state through the roadmap. PRM algorithms

are probabilistically complete but they are valuable in a static, a-priori known,

highly structured environment, where it is possible to compute a roadmap a

priori. This is instead computationally challenging (or even unfeasible) in most

on-line planning problems. This is the main reason behind the success of single-

query counterpart to PRM: the rapidly exploring random tree planner. that, in

order to easily solve future queries try to construct a topological graph called

Roadmap. The advantage of PRM is that relatively few points need to be tested

to ascertain that the points and the paths between them are obstacle free, also

it allows to solve the problems, and further possible problems sampling and

building a map only one time, [4, pag 99-102].

Rapidly exploring Random Trees (RRT) planner is an incremental sampling approach

that was developed for searching high-dimensional continuous state spaces. The

incremental nature of RRT does not require to set the number of samples a

priori, and returns a solution as soon as a path from the start to the goal state

is found, enabling fast on-line implementations. A sequence of samples is used

to build a tree that cover the whole C-space, having as root node the starting

1Indeed also in the search based algorithms it is possible to account for the dynamics of the agentas in [17], but it is easier in sample-based algorithms.

6


point of the agent. After the tree has been built, the algorithm will search the

shortest path connecting the start, and the goal point. 2

Rapidly exploring Random Tree star (RRT?) planner is a natural extension of the

RRT algorithm, it is probabilistic complete, but also guarantee to find an op-

timal solution, asymptotically in the number of samples, with probability one,

[11]. RRT* can be obtained from RRT by simply removing cycles from graph,

ensuring that vertices are reached through minimal-cost paths. This algorithm

will be described in more detail in Section ??

Flat-RRT* planner is a variant of the RRT* planner that exploit the flatness property

of the dynamical system describing the agent motion so as to finitely parame-

terize trajectories, and design a set of motion primitives that represent optimal

constrained trajectories joining two configurations in a grid space. It is a variant

of RRT* where possible trajectories joining a sample configuration to an existing

node are pre-computed offline and collected in a look-up table tabulated, allow-

ing a faster execution usefull in a changing environment that calls for on-line

replanning, [2].

2.3 Rapidly exploring Random Trees planners

Consider a single agent that has to reach some destination, starting from an origin

without colliding with possible obstacles. A trajectory that join origin to destination

while avoiding collisions with obstacles is called an admissible collision-free trajectory.

We assume that each agent has an utility function which rates the quality of its

trajectory: the higher the utility function value, the better is the trajectory. The

goal is then to find the collision-free trajectory δ that maximizes the utility u(δ) of the

agent. Two different algorithm for determining an optimal collision-free trajectory

will be described next: the RRT algorithm in Section 2.3, and then its variant the

RRT? algorithm in Section 2.3.

2Indeed RRT algorithms belongs to a wider family the RDT. RDT refers to ”Rapidly exploringdense trees”,and regard also trees where the sequence of samples has been chosen using a determin-istic approach, like in example the ”Van der Corput sequence”. Because under the assumption of agreat number of random samples we can assume that a sequence is dense in practice RRT belognsto the RDT family. The interested reader is referred to [15, pag 196,200,228].

7


RRT algorithm

The RRT algorithm computes a feasible solution δ by building a tree S that has as

root node the initial configuration of the agent. At each step of the algorithm a

new configuration n is sampled randomly from the obstacle-free space, and a feasible

trajectory connecting the sampled node n to the closest node belonging to the tree

S is computed through the so called steering function. If this feasible trajectory is

collision-free, then the sampled node is added to the tree S. The tree is completely

built if a goal configuration has been added as leaf of the tree, or if the tree reached

its maximum length. Once the tree has been completely built, if one of the leaf is the

goal configuration, then the path of the tree connecting the root to the goal node is

our solution δ, otherwise the goal node is connected to the closer nodes belonging to

the tree S through a feasible trajectory, and the so-obtained path connecting the root

node to the goal configuration is our solution δ.

This algorithm is, like all the other sample based algorithms, only probabilistic

complete, which means that the probability to find a feasible solution δ with this

algorithm tends to 1 if the number of samples tends to infinity. We have guarantee

of termination, because, even if the tree S never go close to the goal point, the tree

will be completely built, and a solution (possible not collision-free) will be found. In

Figure 2.1 a tree built with RRT is reported. Note that RRT does not guarantee

optimality, even if an optimality criterion like an utility function is adopted by the

steering function to select the feasible trajectory.

RRT* algorithm

The RRT* algorithm is derived from the RRT algorithm, it inherits all its features, but

provides a guarantee of (asymptotic) optimality of the solution. The algorithm follows

the same steps of RRT but adds the “Rewire phase”. This phase takes place after

that a new node is sampled, and connected to the existing tree S through the closer

node, say c. In this phase we look for a node k ∈ S with k /= c, with distance(k, n) < r

and with k ∈ S such that the utility to reach the node n from the node k is greater

than the utility to reach the node n from the closer node c, and where r is the wiring

distance, the limit of distance in which we scout for possible candidates k. If such a

node k exists, then, the node n is connected to it through a feasible trajectory, and is

added to the tree S.

8


Figure 2.1. An example of a tree built with the RRT algorithm (light-green), looking for a paththat avoid the obstacle in the center of the plane (green).

This modification in the algorithm gives the possibility to improve the solution,

detecting at each step the possible best connection of the new node to the tree. It

remain a sample based planning algorithm like RRT, with all his limits and weakness,

but now we have that the probability to find an optimal solution tends to one, with

the number of samples that tends to infinity. In Figure 2.2 a tree developed with the

RRT* algorithm is reported,

Both the RRT and the RRT* algorithms implementation can compromise between

the quality of the solution and the time to obtain it. When speed is critically impor-

tant, then, these algorithms give the possibility to tune the time needed to obtain

a solution. For this reason, they have been preferred to our multi-agent planning

algorithm.

We next refer to some graphical examples.

In Figure 2.3, a complete RRT* plan is shown. In blue we can see the RRT tree,

in red the wiring connections, in dark green all the sampled points and in light green

the final path. In the following plots only the final path will be typically showen for

sake of simplicity.

In Figures 2.4 and 2.5, a tree and the final path avoiding fixed obstacles and the

9


Figure 2.2. An example of a tree built with the RRT* algorithm (light-green), looking for a paththat avoid the obstacle in the center of the plane (green).

trajectories of other agents are showed. In the example in 2.5 the green path crosses

the yellow path, but there is no collision since the time instants at which the agents

occupy the crossing point is different: agent trajectories define time varying obstacles.

10


Figure 2.3. This is an example of full plot of an RRT* algorithm, where we can distinguish: thefinal path found (light green), the obstacle in the center of the plane (green), the samples used tobuild the tree (green cross), the connection made by the wiring , and the ones not made by thewiring (respectively magenta and blue).

Figure 2.4. Agent green avoiding agents yellow and magenta.

11


Figure 2.5. Agent magenta avoiding agents green and yellow.

12


2.4 Multi-agent planning

Up to now algorithms that have been developed for a single agent case have been

described. Indeed, in a multi-agent setting, the problem becomes more complex.

Algorithms for a multi-agent system depends also on the solution we are looking

for. In particular, we can distinguish between a cooperative and a non-cooperative

set-up, and then adopt tools from optimization, possibly distributed, rather than

game theory. More information about the different kinds of solution for a multi-

agent problem can be found in Chapter 3. Here, some planning algorithms used in a

multi-agent setting are described.

Traffic-light The dual arm robot algorithm that works like a traffic-light has been

proposed. If a collision between two agents is expected, one will stop and wait

till the other has crossed its path. It is absolutely similar to the traffic-light

system of our cross-roads. The problem of this algorithm is that it is not always

possible for an agent to stop in any position, because stopping would mean to

violate a fundamental constraint, like in the case of an airplane.

Optimal coordinated maneuver When a collision is expected, possible straight-legged

conflict free maneuver are generated, and between all maneuvers the ones that

minimize a certain energy cost function are chosen, [10]

Multi-agent Poli-RRT* It is an extension of the Poli-RRT* algorithm to a multi-agent

setting, by adopting a priority-based approach. Agents are sorted according to a

given hierarchy, and the Poli-RRT* algorithm is executed for each agent following

the assigned priority: each agent will compute his own trajectory avoiding all

the other agents with a priority higher than its own. The problem of this scheme

is that it is difficult to find a priority order that allows to determine a good

solution, [19].

13

Chapter 3

Multi-RRT*

Multi-RRT* extends the RRT* algorithm to a multi-agent set-up.

The Multi-RRT* algorithm is a priority based algorithm, where all agents at the

same time compute their own trajectory, trying to avoid all the other agents. At

each step of the algorithm all the active agents compute their own trajectory trying

to avoid all the other agents; and the agent that deteriorates less its utility is chosen

as first candidate to solve the problem to find a joint collision-free trajectory. This

agent will be deactivated, will not compute any other trajectory, and will maintain

its current trajectory. The cycle will restart, and at each step we will have one agent

that worsen less its utility. At each step if the problem becomes more complex than

before (if all the next agents worsen their trajectories more than they worse at the

previous step), a backtracking strategy applies, reactivating the last agent that was

deactivated, checking again who is the agent that worsen less its utility, not don’t

considering the ones previously reactivated, and going on with the algorithm. This

until the collision is solved, or all the agents have been deactivated.

The rest of the chapter is organized as follows. In Section 3.1 the problem is

precisely formalized, in Section 3.2 an overview about some of the possible solutions

in a bargaining problem is given, highlighting strength and weaknesses of each of

them. In Sections 3.3 and 3.4 the final scheme to solve the problem is presented and

analyzed.

In Section 3.5 the pseudocode of the algorithm will be presented and explained, in

Section 3.6, 3.7 and 3.8 all function involved in the algorithm are described.

14

3 – Multi-RRT*

3.1 Problem description

We consider N agents moving in the same region. Each agent has to reach some des-

tination, starting from a starting point without colliding with possible obstacles, and

staying far enough from the other agents. The joint trajectory that avoids collisions

with obstacles and preserves some safety distance between all agents pairs is called

collision-free. We assume that each agent has an utility function which rates the qual-

ity of its trajectory: the higher the utility function value, the better is the trajectory.

The goal is then to find a joint collision-free trajectory that is Pareto optimal for the

multi-agent system. A solution, set of collision free trajectories δ = (δ1, δ2, . . . , δN ), is

called Pareto optimal if any other set of collision free trajectories δ′ = (δ′1, δ′2, . . . , δ

′N )

will cause a degradation of the utility function of at least one agent.

∃ i such that i ∈ N and ui(δ′) < ui(δ) (3.1)

3.2 Bargaining solutions

In a multi-agent setting there exists different kinds of solutions, some of which are

next described and analyzed. [22]:

Remark

Utilitarian solution δ = (δ1, δ2, . . . , δN ) is a Utilitarian solution if

/∃ δ′ = (δ′1, δ′2, . . . , δ

′N ) such that

∑Ni=1 ui(δ) <

∑Nj=1 uj(δ

′). Or in other

words, the Utilitarian solution is the solution that maximize the overall

sum of the utilities of all the agents. The Utilitarian solution is usually

considered a good solution, but maximize the overall utility is not al-

ways the best choice, agents of different companies could not accept to

worse too much their trajectory, and also is not trivial to find. Usually

this solution can be useful if the ”owner” of all the agent is the same, or

if all the agents share a common resource that is limited. If for example

all the agents are vehicles sharing from some reason the same fuel, then

maximize the sum of the utility of all the agents (or minimize the cost),

15

3 – Multi-RRT*

means to save fuel common resource to everyone, and therefor could

be an interesting solution to look for.

Egalitarian solution δ = (δ1, δ2, . . . , δN ) is an Egalitarian solution if

∀ i, j ∈ (1 . . . N) with i /= j, ui(δ) = uj(δ) and /∃δ′ = (δ′1, δ′2, . . . , δ

′N ) such that∑N

i=1 ui(δ) <∑Nj=1 uj(δ

′) that is, in the egalitarian solution the gains from

cooperation are split equally among the agents. That is, the egalitarian

solution is the one where all the agents receive the same utility and

the sum of their utilities is maximal. The Egalitarian solution is pretty

useless to solve our problem, because is nonsense asking at each agent

to reach his own goal with the same utility, our utility function must be

related to the time to reach a goal, and is useless to look for a solution

where all the agents spend the same time to reach different goals. Also

there is not guarantee that this solution can exist. For these reasons

we will not look for this kind of solution in this study.

Egalitarian Social welfare solution δ = (δ1, δ2, . . . , δN ) is an egalitarian social

welfare solution if ∃ i, j ∈ (1 . . . N) and /∃ l, k ∈ (1 . . . N) and /∃δ′ =

(δ′1, δ′2, . . . , δ

′N ) such that ui(δ) > ul(δ) and uj(δ

′) > uk(δ′) and ui(δ) < uj(δ

′).

Or in other words, the Egalitarian Social welfare solution δ is the solu-

tion that maximize the utility of the agent with lower utility. Again this

is usually classified as a very good solution, but is not meaning-full in

our setting, simply because increasing the performances of the agents

with lower utility does not give any kind of guarantees for all the other

agents.

Nash Bargaining solution δ = (δ1, δ2, . . . , δN ) is a Nash Bargaining solution if

/∃δ′ = (δ′1, δ′2, . . . , δ

′N ) such that

∏Ni=1 ui(δ

′) <∏Nj=1 ui(δ). That is, the Nash

Bargaining solution δ is the solution that maximize the product of all

the utilities. In general this would be the best kind of solution to look

for, it is the only kind of solution that fulfill these four requirements:

• Pareto efficiency.

• Independent from utility units.

• Symmetric.

• Independent from irrelevant alternatives.

16

3 – Multi-RRT*

For these reasons it would be better to look for this kind of solution,

but the point is that is not trivial to find.

A Pareto optimal solution, defined above, can then be the right trade-off between

a good sub-optimal solution, and the efforts to find it. Which motivates our choice to

look for a Pareto optimal joint collision free trajectory.

Definition: the Performance measure of the agent i, Li(δ, δ′) is defined as:

Li(δ, δ′) = ui(δ)

ui(δ′), and it represent how much the solution δ′ worsen the solu-

tion δ for the agent i, higher is Li(δ, δ′), worse is the solution δ′ with respect

to the solution δ.

3.3 Proposed solution

To solve this problem we looked for a solution with specific characteristic. We wanted

a decentralized solution to distribute uniformly the workload between all the agents,

the lowest possible number of information exchanged between the agents, to preserve

privacy, and also to reduce the possibilities of malfunctions due to transmission errors.

We wanted also to find a solution as fast as possible, and that is fair, and acceptable

for all the agents (implicit due to the Pareto optimality nature of the collision free

trajectories found) . In the next session we shall describe a scheme that satisfies these

properties.

The N agents will firstly compute the trajectory that allow them to reach their

goal point, using the RRT* algorithm described in chapter ??. Then they will send

in broadcast to all the other N − 1 agents the trajectory found, and will check if a

collision is expected, or if safety distance is not respected between two or more agents.

If this is the case the first agent that has detected the danger will communicate it

through a warning message in broadcast. All the N agents, once received the signal,

will recompute their trajectory avoiding the other N − 1 agents. To recompute the

trajectory each agent will prune his own tree, discarding the branches of the tree

that risk the collision. This is a technique that allow the agents to reuse data from

previous computations, avoiding to discard useful data, and saving time and memory

resources, without this trick computation times would grow too much, violating one

17

3 – Multi-RRT*

of the fundamentals requirement (time efficiency); through this technique we can also

keep constant the number of nodes of a tree,Simply counting the number of nodes

removed by a tree, and adding the same number of feasible samples to it. In such a

way each tree will always keep its original cardinality, allowing us to guarantee the

goodness of the related path found. The new trajectories found are compared with

the original, ideal one found at the first step, that was discarded due to the collision,

and each agent i send in broadcast the performance measure Li(δ, δ′) , with δ the so-

lution found at the first step, and δ′ the solution found at the actual step. Between

all the agents the one that worsen less his trajectory, that has a lower performance

measure, Li(δ, δ′), is chosen as candidate to solve the problem, which will confirm the

change of trajectory in broadcast to all the other agents, and will be removed from

the N setting. The remaining N −1 agents will perform again the algorithm, trying to

identify another subject as candidate to solve the problem. Until the set of agents N

is empty, which means that each agent has found a collision-free trajectory connecting

the goal and the starting point.

The only guaranties of this algorithm as described is the termination, at each

iteration of the algorithm a candidate is always extracted from the N set, and removed.

Therefore the setting of the N agents is reduced by one at each iteration. Now or then

the N set will be empty. But the Pareto optimality of the solution is not guaranteed

yet. To do so another passage has been proposed. At each iteration of the algorithm

the performance measure of the agent i, Li(δ, δ′) is computed, and compared to Li(δ, δ),

∀i ∈ N with δ the solution found at the first step, δ′ the solution found at the actual

step, and δ the solution found at the previous step. So each agent checks if has

obtained an improvement and communicate this information to all the remaining

agents, in order to be sure that at least one agent is always improving his solution at

each step. If this is not the case a backtracking approach is proposed. From N − 1

agents we come back to N adding the previous deleted one, as if the last iteration

did not happen, and will select a new candidate, different from the ones that have

been already tried. If all the agents have already been selected as chosen one at

least once, the first, that was chosen is selected again. Note that the backtracking

process always guarantee the termination of the algorithm, in fact at each iteration

the set of agents N will remain N , or will become N − 1, never increasing, and also

18

3 – Multi-RRT*

the number of iteration where N is not decreased are constrained to be less than N

number of agents. Now, that at each step each agent checks if at least one of them

has improved his solution, we have guaranties of both termination of the algorithm

and Pareto optimality of the solution.

3.4 Conclusions about the algorithm

The core of the MULTI-RRT* scheme is the computation and the comparison of the

performance measures. They are relative values, with respect to the desired trajec-

tory of each agent, this means that an agent that has a very small desired trajectory,

will consider as too negative also small deviations. Instead an agent with a very long

desired trajectory (due to a physical obstacle in the middle, or simply because is far

from the goal) will accept easily small deviations. In example, if we have two ideal

trajectories of two different agents, the fist one has utility 10, and the second one has

utility 100, and both are worsen of 1. The performance index of the first agent will be

10/9 = 1.111, instead the performance index of the second one will be 100/99 = 1.0101.

Therefore is that second agent that has lower performance index, and it will be the

candidate to solve the problem, because will accept more easily the deviation.

The idea is to use a simple priority order method, solving the problem agent by

agent, but sorting them through the performance measure, instead that with a priory

defined order, therefore removing from the problem the agents that are less involved

in the collision, and solving at each iteration a smaller problem. This is a common

idea in engineering for problem solving, trying to bring back a problem to one that

has already been solved, or to a smaller and easier to solve one. The decentralization

has been achieved giving to each agent the due to compute his own trajectory, and

will rely on the soundness of the other trajectories. To distribute the workload means

that each agent will compute RRT* only (N + 1) times in the worst case, and only

once will compute a full tree, because in the other N computations the previous data

that have not been discarded will be used again.

The MULTI-RRT* is a simple and fast algorithm, that exploit a simple bargain

strategy, based upon the question ”who spend less to avoid the others?” to prioritize

19

3 – Multi-RRT*

the different agents, and find a Pareto optimal solution easily acceptable by all the

agents.

3.5 Pseudocode

1) Initialization.

2) Computation of individual trajectories for each agent.

3) Check if a collision is expected.

4) Check the active agents.

5) While (collisions are expected, and there are active agents).

6) Each agent compute a free-collision trajectory avoiding

all the other agents and connecting the goal to the start point.

Compute also the percentage, or merit figure

W.R.T the original path found in 2).

7) Creation of the vector containing the percentages.

8) Creation of the memory vector.

9) Creation of the New_Nodo.

10) Checking if the percentage have become worse.

11) if (the percentages are worse)

12) Backtrack to the previous node.

13 if (all the other paths have already been tried)

14) Accept the one with lower percentage.

15) else

16) accept the one with lower percentage that has not been

already tried.

17) end if

18) end if

19) updating variables.

20) end while

21) Compute the distance between the found paths.

22) Plots.

20

3 – Multi-RRT*

Pseudocode explanation

All the MATLAB functions cited in this explanation will be further analyzed in section

3.6

1 The first part provide the initialization of all the variables of the problem, in par-

ticular: Starting and goal point of all the agents, possible fixed obstacles in the

space, safety parameters for the agents, number of samples for each RRT* iter-

ation, velocity, starting orientation and maximum steering angle of the agents,

and the nodi vector, a data structure for the decision tree.

2 This first computation is preformed through the Find path tree noplot function.

3 The collision is checked through the function Check collision.

4 The active agents are checked through a vector call nodes, if an agent i is active,

then nodes(i)=1.

5 the while condition is checked through the Check_collision function, and the nodes[]vector.

6 The collision free trajectory connecting the start and the goal point, and avoiding

the other paths is computed in two steps. First the existing trees are pruned

through the function Removing nodes noplot, then all the removed nodes are

counted through the function Count missing nodes , and in the end a new

path is found using the function Continue rrt noplot. Then the percentages are

computed for each path as:

percentage = 100 ∗ (cost6)/(cost2); (3.2)

Where cost6 is the cost of the path computed at the step 6, and cost2 is the cost

of the path computed at the step 2.

7-8 simple creation of the vector p that contains all the percentages that show the

ratio between the new path found avoiding the other agents, and the previous

one found without considering them.

Creation of the Memory vector, vector that contains all the vector of percentages

of the search (needed for eventually backtracking, or for debugging).

21

3 – Multi-RRT*

9 Creation of the New Nodo, a struct variable that contains the active nodes at this

step, the vector of the percentages, an index and his predecessor.

10 The checking of the percentages is performed comparing the vector of percentages

of the node New Nodo with nodi(New Nodo.predecessor).percentages.

11 To activate this condition is necessary that all the percentages of New Nodo are

greater than the correspective element of the predecessors. In other words: is

necessary that the previous deactivation of the agent has not made the problem

easier, but, indeed, more complex.

∀i1 : number of agents nodi(New Nodo.predecessor).percentage(i) < New Nodo.percentage(i)

12 If the percentage have become worse, the percentage vector of the predecessor is

taken, the previous deactivated agent is reactivated, and also his percentage is

increased of 1000. 1

13 if all the percentages are more than 1000, this means that all the path have been

already tried and, therefore there is no choice that will not increment the per-

centage vector.

14-16 if condition 13 is true we will accept the agent with the lower percentage, that

is the first one that we have tried. Else we will accept always the agent with the

lower percentage, (that will be different from the previous one tried).

19 The Memory vector, and the nodi vector are updated with the new values.

21 the distance between the paths is computed using the Compara path function.

3.6 Main functions description

To fully understand the algorithms is necessary to precise some variable first:

SafetyDistance when i use this term i refer to the official safety distance required

between the two agents, for the ATM is 5 nautical miles (9260 meters).

1The meaning of increasing the percentage with 1000 is that, doing so, we can easily check whichactive agents have been already tried for each node, and at the same time the algorithm will alwaysactivate the one that worsen less the percentage. If all the percentages are greater than 1000, meansthat all the agents have been deactivated and reactivated again, and therefore the only choice is todeactivate again the first one, that will be again the one with lower percentage. (Becouse all of themhave been increased by 1000.)

22

3 – Multi-RRT*

CostCostraint when i use this term I am referring to a limit in cost to consider two

points of two different trees too near each other. The cost of each node represent

the time that the agent need to reach it, therefore if two nodes of different paths

(or trees) have the same cost, this means that they are passing in that nodes at

the same time.

Find path tree noplot

This function is used to find a free collision path connecting a start and a goal point

using the RRT* algorithm, at each step of the cycle a new sample (that does not

belongs to the tree) in the given range is extracted, and is given to the steernew2

function. Through this function a new point is found, and if the connection between

this point, and the nearest point belonging to the tree is collision free, it is added to

the tree, otherwise is discarded. To check if the connection is collision-free we use the

noColiision function. If this point is added, we check in a radius r other nodes of

the tree, trying to find another path connecting the new node to the tree with minor

cost. This operation is called ”Rewiring”, and was introduced in RRT*. If this path

is found the node is added definitely and his predecessor is updated with this last

result found. After that the tree is built with enough samples, we connect the goal

node with the nearest one belonging to the tree, and then backtrack from that node

to the start one, obtaining the desired path.

Check collision

This function will check if two paths collide to each other, it return 1 if there is

no collision, zero otherwise. It is composed by a double for cycle that explore all

the elements of the two paths, and check if there are two points too near both in

space than in time (SafetyDistance is referred to the distance between the points,

and Costconstraint is referred to the difference in cost between the two points (cost

means time)). Two points that cause a collision are called Critical Nodes.

Removing nodes noplot

This function will delete some nodes from a tree, the eliminated nodes will be set

with cost=-1, flagging them like ”Not valid nodes”, and will be discarded by all the

23

3 – Multi-RRT*

other algorithms. A node is removed if it is a Critical Node, or if his predecessor

has been removed. It is similar to the Check collision function, but instead of two

paths has like input a tree and a path, and if notice collision, automatically remove

the node. After the double for cycle it analyze again all the tree to remove the nodes

whom predecessor has been already removed. 2

Count missing nodes

this simple function will count through a for cycle the number of missing nodes (cost=-

1) in a tree.

Continue rrt noplot

This function allow the user to continue a previous RRT* simulation adding new nodes

to an existing tree, it is also possible to specify a safety distance parameter, and a

path to avoid, in such a way we can guarantee no collision with other trajectories.

It is very similar in structure to Find path tre noplot, but this function has more

input parameters, and when checking if the connection with the new node is feasible

use both the NoCollision function and the Check collision function. The first one is

used to check the collision with the obstacle, and the second one to check the collision

with the paths that must be avoided.

Compara path

This function is used to compute the vector representing point by point the distance

between two agents during the time. First it use the Interpola path function to

apply the Spline to the two trajectories. And then, through a for cycle computes the

distance between the two trajectories, point to point considering points with almost

the same cost.

steernew2

The steering function does not explicitly consider the dynamic of the robot, or differ-

ential constraints, but to keep light the computation of the trajectories it consider a

2It is important to precise that we are building a tree, not a road-map. Therefore removing anode means discard also all his successors.

24

3 – Multi-RRT*

simply constraint in the steering angle. Each time a new sample is taken the function

compute the orientation of the agent (knowing the actual node and his predecessor),

and compare this angle with the new one that should be created. If the difference

between these two angles is small enough, then we connect them directly, otherwise

we steer in the direction of the sample as much as we can. 3

noCollision

This function will simply check if a line, between two points intersects any of the four

edges of the given obstacle.

Interpola path

This function will use the common MATLAB function interp1(X,V,Xq,METHOD)

to interpolate the given path using as method the SPLINE one.

3.7 Other functions

Concatenate paths this function will concatenate more paths together. Adding one

path after the other creating a single vector. it is used in Multi-RRT* when

i want avoid collision with multiple paths, concatenating them is like if i was

giving as input a single path that comprehend all the concatenated paths. It is

done simply creating a longer vector with all the nodes of all the paths.

Miglia metri A simple function that convert the nautical miles into meters.

Plot path Function used to plot a path.

Plot tree Function used to plot a tree.

Reverse path This function will reverse the vector path, switching the first node with

the last and so on. This because the first element of a path is the goal, and is

easier to reverse it, than performing a for cycle from the last to the first element

of the path.

3If the distance between the sample and the tree is greater than eps, then we will not connectdirectly the tree with the sampled node, but will connect the node to a point lying to the lineconnecting the sample point and the tree, and far exactly eps from the tree.

25

3 – Multi-RRT*

Total cost This function will return the total cost of a path. (from start to goal).

3.8 Made by Say Vemprala

The following functions and script have been created by Sai Vemprala. This function

have been Modified and changed to fulfill my needs.

noCollision Previously described in 3.6.8, this is function was created by Sai Vem-

prala. Simply check using the CCW function if a segment connecting two nodes

intersect an edge of the obstacle.

CCW A function that given three points check he position of the first one w.r.t. the

edge connecting the other two.

dist Function that given two point return the Euclidean distance between them.

RRT* Script used to compute a trajectory through the RRT* algorithm. Has been

used as base for the Multi-RRT* algorithm.

26

Chapter 4

Simulation results

In this chapter some simulation results are reported to show the performance of multi-

RRT?. First a description of the setting used for the simulation, and the explanation

of the hypothesis assumed will be given in Section 4.1, then the data structure used

to implement the RRT* algorithm will be described in Section 4.2, and comments

about the single agent solution will be given in Section 4.3. Then, in the following

sections, will be analyzed two different settings of simulations, one with 3 agents in

Section 4.6, and the last one with 5 agents in Section 4.7.

4.1 Hypothesis and more

To test the algorithm, we have considered the aviation setting, and made some as-

sumptions:

In this setting the utility function used to evaluate the goodness of the trajectories

will be simply the reverse of the time time that they spend to reach the goal. ui(δ) =

1/T (δ, i) with T (δ, i) the time that the agent i will take to reach his goal following the

trajectory of the solution δ.

As known, a flight trip is composed by three phases: take-of, landing and cruise.

In this thesis we will consider, for the reasons explained below, only the cruise phase

as relevant for our study.

During the take-of, and the landing phases, that could be considered together for

their similarities, the trajectory planning is usually checked by the control tower of

the airport, and collision with other airplanes are not expected. Therefore we have

27

4 – Simulation results

no reason to apply the algorithm during these two phases.

The last phase remained, the cruise one, is the subject of our study. Usually, during

this phase the velocity of the airplane is kept constant, which allow to guarantee the

comfort of the passengers and maintain low the wastage of fuel; also, for the same

reason, too rude steering angles and vertical maneuvers are avoided.

The exclusion of the take-of and landing phases, plus the consideration about

the vertical maneuvers allow us to consider the motion of the plane only in a 2D

environment, and with constant speed during the flight. This choice allow us to

highly simplify the dynamic of the vehicle considering as steering function a simple

function that avoid too rude steering, and connect the point through linear segments,

which simplify the problem itself. This approximation is needed to keep fast the

execution of the algorithm, that is a fundamental requirement due to the need of

the MULTI-RRT* algorithm to execute a single RRT* many times. In the best

situation, without backtracking, with N agents the number of RRT* executed will be

(2N) + (N − 2) + (N − 3) + ....(1), but remember that this workload will be in theory

distributed between the agents, anyway in this testing phase will be executed all by

the same calculator.

The hypothesis of constant speed, applied in this thesis, could be removed in future

studies adding different and more complex dynamics. This can be easily implemented

into the code changing a single function, thank to the modularity of the implementa-

tion of the algorithm. In-fact the dynamic of the vehicle is entirely contained in the

steering function, discussed earlier in chapter 2.3, changing this function means to

completely change the problem that we are studying. Possible ideas and suggestions

on how to change this function can be found in [15] and [2].

A further hypothesis is that the agents are cooperating each other, and are able to

freely exchange information between them, without transmission errors. Each agent

will communicate to the other only the performance measure that define how much

an agent worse his trajectory to avoid all the other N − 1 agents, the ideal trajectory

that they would desire to follow, some flag that communicate to the other agents if

a collision is expected, or if an improvement in the solution has been reached, and

once a trajectory is fixed, the interested agent will send in broadcast the definitive

trajectory. Obviously necessary condition is that each agent has the same criteria

to measure distances and notice if a collision is expected, because the agents do not

28


check the soundness of the trajectories of the other agents, trusting the performance

measure that is transmitted, and that safety parameters are respected.

This is a cooperative setup, therefore we are implying that all the agents are coop-

erating to solve the problem, finding a Pareto optimal solution for everyone, no lies

or omission will be admitted in this setup.

By law, each agent is constrained to be at least α longitudinally distant from the

other agents. with α corresponding to 5 [NM]. As in general planning literature,

to be sure that this safety distance is respected, even after that the found path is

interpolated,and also in case of possible small deviations due to human error or the

wind, we just need to set this value to a α + ε with ε big enough. Doing so we can

assure the fulfillment of the constraint even under a certain degree of uncertainty.

4.2 Data structure

To completely implement the RRT* and the MULTI-RRT* algorithms we need to

define a data structure for the nodes that compose both the tree and the path.

A ”tree” will be composed by ”nodes”, each node is a struct that keep inside four

information: a coordinate along the x axis, a coordinate along the y axis, the time to

reach the node, starting from the starting point, and the ”predecessor” node. As can

be easily insight, each tree will born from a single node, corresponding exactly to the

starting point, with zero time to reach it, and no predecessor. 1

In such a way choosing a single node of a tree, we instantly find where it is,how

many time is needed to reach it, and from which nodes we have to pass through to

reach it. In a tree the starting point will be the ”root”, and the goal point will be

the ”leaf”. The presence of the time information inside each node allow us to check

if a collision between two paths is expected, considering both time and space. If two

agents are passing through the same coordinates, but one of them traverse that half

an our earlier of the other one, there is no need to expect a collision. The variable

that regulate this ”time-distance” is the Cost constraint, and can be used to tune

different level of safety in computing the trajectories.

1The predecessor node is simply the node of the tree, that has generated our node (or thatconnect our node to the root)

29


We have to precise that this data structure is a tree, and not a graph. Therefore

movement is allowed only from the roots toward the leaves. This is both because

we are building a tree, and not a road-map (see PRM in section 2.2 for that), both

because each agent will have his own dynamic, and usually an agent cannot proceed

backtracking easily. To make this concept clear, just think if it is the same to drive a

car with the fist gear inserted, or with the reverse one.

Each trajectory, will be then defined as a sequence of nodes of a tree, where each

node is the predecessor of the subsequent.

4.3 Solution found, weakness and strength

The quality of the trajectory found is due to different factors: The numbers of nodes

of the tree (cardinality of S), the function that connect each node of the tree (the

steering function), the time that we are willing to wait before obtaining a solution.

Indeed the first and the third parameter are strictly connected, because higher is the

number of nodes randomly extracted, better our solution will be, but more time we

will have to wait. The function that connect the nodes of the tree instead is a more

critical factor. It is called usually ”Steering function” and, as already precised, is

the function that must guarantee the constraints satisfaction, the feasibility and the

dynamic of the vehicle. More is complex, and precise, better and more realistic will

be our trajectory, but again higher will be the time to wait. What we should look for

is a reasonable ratio between the quality of the solution and the time needed to obtain

it. As explained above the speed of the agent will be considered constant, and the

only job of the steering function will be to check if a steering angle can be accepted

or not. more technical information about the steering function used will be given in

chapter 3.

In the next figure different comparisons will be shown, in 4.1 we can see, in light

green, a tree built with only 500 samples, not enough to completely explore the plane.

this Figure can be compared to 4.2 and to ?? where two trees formed by 11000 samples

and 50000 samples are depicted. in these two pictures we can see that both the trees

fully explore the whole space, allowing us to find better and feasible solutions. Also

30


the two path found with the corresponding tree are reported in 4.3, 4.4 and ??. We

can see that, obviously, trees with an higher cardinality cover better the whole plane.

Figure 4.1. Tree built with 500 samples (light green), avoiding a fixed obstacle (green)in the middle of the plane

Figure 4.2. A tree built with RRT* (light green), using 11000 samples, avoiding theobstacle in the center (green)

31


Figure 4.3. (green) Path found by the RRT* algorithm, using 500 samples. It is trivial to noticethat using a too low number of samples we have no guarantees about the soundness of the solution.

Figure 4.4. (green) Path found by the RRT* algorithm, using 11000 samples. The higher numberof samples guarantee a better, and feasible solution.

32


4.4 Some improvements

In order to make the algorithm more flexible, and to be able to both improve the

solution, and also test the effectiveness of Sample Based Planning methods, two aux-

iliary algorithms have been implemented that continue a preexisting tree, accordingly

with changing of the environment. Suppose that we want to add more samples to a

previous computed tree, or also that a new obstacle is discovered and therefore i must

recompute the entire tree. A possible solution could be to rebuilt again the tree from

the single root node. But we have implemented two functions that allow to build a

tree sampling more couple of coordinates, and checking the consistency of the tree

with respect to new obstacles, discarding only the branch of the tree that are not

feasible anymore. This is particularly useful in the multi-agent environment that is

described in chapter 3 saving time and memory, and avoiding useless computations.

An effect of this algorithm can be shown in figures 4.5 4.6 4.7, where we can see a

tree before a new path to avoid, (the thicker one) is added, after that is pruned, and

after that the tree grows up again with more samples, according to the new path to

avoid. The trees are represented with branches of different colors, depending by the

distance from the root node. This is necessary to report on a bi-dimensional plot

the time needed by the agent to reach each certain branch of the tree. To decide

if a certain branch must be removed, or if a collision is expected, we can use the

variable inside each nodes, that contains the time to reach it,graphically, the color, to

determine how near are two agents with respect to time.

33


Figure 4.5. Example of a normal tree, called alpha, avoiding a fixed obstacle (green), before thepruning and thickening operations, this tree has branch of different colors representing the cost toreach each branch. If to reach a branch from the starting point we need less than 15 minutes it isblue, if more than 15 minutes and less than 30 minutes it is green,if it is greater than 30 minutes,but less than 45 minutes it is red, otherwise if it is greater than 45 minutes it is black.

Figure 4.6. Example of the alpha tree (rainbow) that is pruned in order to guaranteeno collision against the new obstacle, the path that has been added that is representedwith a thicker rainbow line.

34


Figure 4.7. Example of the alpha tree (Rainbow) that grow up using new samples, andkeeping avoiding the central obstacle (green) and the other path (thick rainbow). We cansee that areas with similar color do never meet between the tree and the path, guaranteeingthe safety of the trajectory.

35


4.5 Interpolation

A further possible improvement of the solution can be the use of the SPLINE al-

gorithm. The SPLINE algorithm already implemented in MATLAB, is a cubical

interpolation of via points. We thought to use this interpolation to give continuity to

the trajectory. In other words all the algorithm will make the computations with the

trajectory composed by the conjunction of linear segments, and when the solution

has been found interpolation of these points occurs, in order to obtain a smoother so-

lution. The interpolated solution is not too much different from the not interpolated

one like can be seen in pictures, 4.8, 4.9, representing the comparison between the

interpolated, and the not interpolated path in detail and not. To make sure that the

interpolation does not compromise the feasibility of our solution a small increment of

the safety distance is enough, but, as will be showed in the following sections, we are

mostly far away from our safety distance limit, also we can see from the figures below

that the difference between the interpolated, and not interpolated paths is minimal.

The SPLINE algorithm has been chosen because already implemented, fast and able

to give a good solution, but any other interpolation function can be used, without

loss of generality.

Figure 4.8. In this picture an interpolated path (red), and a not interpolated one (blue) are plotted.As we can see they are so similar that in this scale no difference can be seen. In the red rectangleis highlighted the section that is zoomed in the next figure 4.9.

36


Figure 4.9. This figure is a zoomed area of the previous plot 4.8, where a path interpolated (red),and a not interpolated one (blue) are compared.

4.6 Configuration 1

This configuration is reported in the figure 4.10 , where to each agent is assigned a

color. The agent 1 is green, the agent 2 is blue and the agent 3 is magenta. The

starting point are marked with a ”x” sign, and the goal points are marked with a ”*”

sign. The green square shape in the plot is the obstacle that must be avoided.

37


Figure 4.10. Setup of configuration 1. We can see here the fixed obstacle in the middleof the plane (green), and the start and goal points of each agent. The starting points aredefined with a ”x” sign, and the goal points are defined with a ”*”. the agents are: agent 1(green), agent 2 (blue), agent 3 (magenta).

The starting point of the agent 1 is located in [460, 460], the starting point of the

agent 2 is located in [490, 220], the starting point of the agent 3 is located in [320,

300], the goal point of the agent 1 is located in [150, 50], the goal point of the agent 2

is located in [270, 20], the goal point of the agent 3 is located in [20, 20]. The obstacle

is a rectangle with center in [250, 270], and with sides long 100 and 300 kilometers.

The safety distance requested between each agent is 5 nautical miles, corresponding

to about 10 Kilometers. The maximum length of each branch of the tree is set as 2.5

kilometers. The number of samples needed to build each tree, (the cardinality of the

trees), is 11000 for all the agents. The range where new samples are taken is [500,

500]. The velocity of each agent, assumed constant is 15.216 km/min. The wiring

distance r discussed in chapter ?? is set equal to 5 kilometers. The maximum steering

angle of all the agents is pi/4. The starting angle of the first agent is set equal to pi/2,

the starting angle of the second agent is set equal to pi/2, the starting angle of the

third agent is set equal to −pi/2.

38


In the next figures we can see the evolution of the algorithm. In figure 4.11 we can

see that each single agent compute its own trajectory not being aware of the existence

of the other agents, after this step each agent will send in broadcast its own path,

and will check possible collision with other paths received. If a collision is expected

we proceed to step 2 reported in figure 4.12, where each agent will recompute its

path avoiding the paths received before. From now on at each step the performance

measure of each agent, reported in table 4.1, is checked to look for the agent that

worsen less its trajectory, from the table we can see that this agent is the agent 2,

therefore this agent will lock its trajectory that will be now on plotted as a slim

black line. At the next step, in figure 4.13 each agent except for agent 2 will try to

avoid the remaining paths, and the fixed obstacles (that are now both the rectangular

shape obstacle in the middle of the area, and the locked path of agent 2). After this

computation the performance measure of all the agents is computed, and again the

lower one is selected, in this case it is the agent 1 that will lock its trajectory. In the

last step, reported in figure 4.14 we can see that the last agent remaining try to avoid

all the fixed obstacles, and, remained the last one, will lock its path.

In the end the percentage results are here reported:

Agent1 + 15.51%

Agent2− 7.83%

Agent3− 7.25%

Table 4.1. This table represent the different value of the performance measure, Li(δ, δ′), of

each agent at each step of the algorithm in configuration 1. It is a percentage value, therefore110.8323 means that the path found worsen the original trajectory of about the 10%. Whena value is missing, does it means that the agent is not active at that moment, therefore itspath has been already locked.

——- step1 step2 step3agent1 114.79 115.5169 ———agent2 92.17 ——— ———agent3 139.84 139.63 92.75

In the end in figure 4.15 we plot the final set of trajectories found by the algorithm,

but interpolated with the SPLINE algorithm, and in figure 4.16 we plot the distances

between the agents: In black the distance between the agent 3 and 1, in yellow the

distance between the agents 2 and 3, and in blue the distance between the agent 1

39


Figure 4.11. Configuration 1, first iteration, 3 agents active. In this iteration each agentwill compute his own trajectory to reach its goal, without knowing the existence of theother agents, and avoiding the central obstacle (green). The agents are: agent 1 (green),agent 2 (blue) and agent 3 (magenta).

Figure 4.12. Configuration 1, second iteration, 3 agents active. In this iteration the agents havechecked if collisions are expected, and take aware of the existence of all the other agents, then willcompute new trajectories guaranteeing to avoid both the fixed obstacle (green), both all the otheragent’s paths seen in figure 4.11 (green, blue and magenta).

40


Figure 4.13. Configuration 1, third iteration, 2 agents active. In this iteration the agent x,that is the agent with lower performance measure Li(δ, δ

′), will lock its trajectory, that willbe now on plotted with a slim black line, and all the remaining agents compute a path ableto avoiding both the fixed obstacles (green middle obstacle and black line), both the otherremaining paths of figure 4.11.

Figure 4.14. Configuration 1, fourth iteration, 1 agent active. Similar to the fourth iteration,but now the agents that have locked their path are 2, which paths are represented throughblack lines, and only one agent is remaining.

and 2, also reporting with a red line the safety limit to guarantee. The chart reports

41


the distance between the agents at each single time instant.

Figure 4.15. Final paths found by the RRT* algorithm. Here the interpolated paths are reported,each with its own color: agent 1 (green), agent 2 (blue), agent 3 (magenta).

Figure 4.16. Distances computed point by point for each agent are plotted. The distancebetween the first and the third agent (black), the distance between the first and the secondagent (blue), and the distance between the second and the third agent (green). The red linedefine the safety limit of 5 nautical miles (red).

42


4.7 Configuration 2

This configuration is reported in the figure 4.17 , where to each agent is assigned a

color. The agent 1 is green, the agent 2 is yellow, the agent 3 is magenta, the agent

4 is blue and the agent 5 is black. The starting points are marked with a ”x” sign,

and the goal points are marked with a ”*” sign. There is no fixed obstacle in this

configuration.

Figure 4.17. Setup of configuration 2. We can see that there is no obstacle, and the start andgoal points of each agent. The starting points are defined with a ”x” sign , and the goal pointsare defined with a ”*”. the agents are: agent 1 (green), agent 2 (yellow), agent 3 (magenta),agent 4 (blue) and agent 5 (black).

The starting point of the agent 1 is located in [10, 10], the starting point of the

agent 2 is located in [480, 320], the starting point of the agent 3 is located in [10, 250],

the starting point of the agent 4 is located in [250, 10], the starting point of the agent

5 is located in [480, 10], the goal point of the agent 1 is located in [480, 480], the goal

point of the agent 2 is located in [10, 120], the goal point of the agent 3 is located in

[480, 250], the goal point of the agent 4 is located in [250, 480], the goal point of the

agent 5 is located in [10, 480]. The safety distance requested between each agent is 5

nautical miles, corresponding to about 10 Kilometers. The maximum length of each

43


branch of the tree is set as 2.5 kilometers. The number of samples needed to build

each tree, (the cardinality of the tree), is 20000 for all the agents. The range where

new samples are taken is [500, 500]. The velocity of each agent, assumed constant

is 15.216 km/min. The wiring distance r discussed in chapter ?? is set equal to 5

kilometers. The maximimum steering angle of all the agents is pi/4. The starting

angle of the first agent is set equal to 0, the starting angle of the second agent is set

equal to pi, the starting angle of the third agent is set equal to 0, the starting angle of

the fourth agent is set equal to 0, the starting angle of the fifth agent is set equal to pi,

In the next figures we can see the evolution of the algorithm. The procedure is

equal to the one of the previous configuration, but with more steps, we can see the

computation of the trajectories, without taking into account the other players, in

figure 4.18, and all the successive computations in figures 4.19 4.20 4.21 4.22 4.23.

the trajectories computed by each agent in each step is plotted coloured, instead the

locked ones, considered as fixed obstacle are plotted with slim black lines. In this

numerical example the order found by the agents was: agent 5, agent 2, agent 3,

agent 1, agent 4.

In the end in figure 4.24 we plot the final trajectories and the distances between

the agents: In 4.25, we can see the distance of the agent one from all the other agents,

In 4.26, we can see the distance of the agent two from all the other agents, in 4.27,

we can see the distance of the agent three from all the other agents, in 4.28, we can

see the distance of the agent four from all the other agents, in 4.29, we can see the

distance of the agent five from all the other agents.

In 4.2 is reported a table that reports the performance measure of all the agents

at each step of the algorithm. In this example the first agent that lock its trajectory

is the agent 5, worsening its original trajectory of 10.83%, then the agent 2 worsening

its trajectory of 19.1836%. In the end the percentage results are here reported:

Agent1 + 16.80%

Agent2 + 19.18%

Agent3 + 23.44%

Agent4− 18.39%

Agent5 + 10.83%

44


Figure 4.18. Configuration 2, first iteration, 5 agents active. In this iteration each agentwill compute his own trajectory to reach its goal, without knowing the existence of theother agents. The agents active are: agent 1 (green), agent 2 (yellow), agent 3 (magenta),agent 4 (blue) and the agent 5 (black).

Table 4.2. This table represent the different value of the performance measure, Li(δ, δ′), of

each agent at each step of the algorithm in configuration 2. It is a percentage value, therefore110.8323 means that the path found worsen the original trajectory of about the 10%. Whena value is missing, does it means that the agent is not active at that moment, therefore itspath has been already locked.

——- step1 step2 step3 step4 step5agent1 129.9810 125.5176 137.3119 116.8023 ———agent2 122.69 119.3836 ——— ——— ———agent3 128.8811 125.6951 123.4425 ——— ———agent4 148.0671 123.8983 141.3134 132.3652 81.6195agent5 110.8323 ——— ——— ——— ———

45


Figure 4.19. Configuration 2, second iteration, 5 agents active. In this iteration the agents havechecked if collisions are expected, and take aware of the existence of all the other agents, then willcompute new trajectories guaranteeing to avoid the paths of all the other agents seen in figure 4.18.

46


Figure 4.20. Configuration 2, third iteration, 4 agents active. In this iteration the agentwith lower performance measure lock its trajectory, that will be now on plotted with a slimblack line, and all the remaining agents compute a path able to avoid both the fixed obstacles(slim black line), and the other paths. In this case all the agents are active except for theagent 5 that has locked its trajectory.

47


Figure 4.21. Configuration 2, fourth iteration, 3 agents active. In this iteration the agent withlower performance measure lock its trajectory, that will be now on plotted with a slim black line,and all the remaining agents compute a path able to avoid both the fixed obstacles (slim blacklines), and the other paths. In this case all the agents are active except for the agent 5 and agent2, that have locked their trajectories.

48


Figure 4.22. Configuration 2, fifth iteration, 2 agents active. In this iteration the agent with lowerperformance measure lock its trajectory, that will be now on plotted with a slim black line, and allthe remaining agents compute a path able to avoid both the fixed obstacles (slim black lines), andthe other paths. In this case all the agents are active except for the agent 5, agent 3 and agent 2,that have locked their trajectories.

49


Figure 4.23. Configuration 2, sixth iteration, 1 agents active. In this iteration the agent withlower performance measure lock its trajectory, that will be now on plotted with a slim blackline, and all the remaining agents compute a path able to avoid both the fixed obstacles (slimblack lines), and the other paths. In this case the only agent active is agent 4 that will avoidall the fixed, slim black paths of the other agents.

Figure 4.24. Configuration 2: final solution. In this figure the final set of collision-free paths found is interpolated and plotted. agent 1 (green), agent 2 (yellow), agent 3(magenta), agent 4 (blue), agent 5 (black)

50


Figure 4.25. Distances computed point by point for each agent are plotted. The distance betweenthe first and the third agent (black), the distance between the first and the second agent (yellow),the distance between the first and the fourth agent (blue), and the distance between the first andthe fifth agent (green). The red line define the safety limit of 5 nautical miles (red).

51


Figure 4.26. Distances computed point by point for each agent are plotted. The distance betweenthe second and the first agent (yellow), the distance between the second and the third agent (black),the distance between the second and the fourth agent (blue), and the distance between the secondand the fifth agent (green). The red line define the safety limit of 5 nautical miles (red).

52


Figure 4.27. Distances computed point by point for each agent are plotted. The distance betweenthe third and the first agent (black), the distance between the third and the second agent (yellow),the distance between the third and the fourth agent (blue), and the distance between the third andthe fifth agent (green). The red line define the safety limit of 5 nautical miles (red).

53


Figure 4.28. Distances computed point by point for each agent are plotted. The distance betweenthe fourth and the first agent (black), the distance between the fourth and the second agent (yellow),the distance between the fourth and the third agent (blue), and the distance between the fourthand the fifth agent (green). The red line define the safety limit of 5 nautical miles (red).

54


Figure 4.29. Distances computed point by point for each agent are plotted. The distance betweenthe fifth and the first agent (black), the distance between the fifth and the second agent (yellow),the distance between the fifth and the third agent (blue), and the distance between the fifth andthe fourth agent (green). The red line define the safety limit of 5 nautical miles (red).

55

Chapter 5

Conclusions

In this master thesis we proposed a decentralized iterative scheme for a multi-agent

cooperative motion planning problem. The scheme that is able to find a Pareto

optimal solution in a finite number of iterations.

Interestingly, at each iteration, agents are required to plan their trajectory for a

given tentative solution of the other agents, which are considered as known (time-

varying) obstacles. This ease the single-iteration planning and allows to integrate in

the scheme standard methods developed in the litearture for collision-free single-agent

planning. We adopted RRT?, which allows to obtain optimal trajectory design.

The scheme was implemented in MATLAB code and tested on different 2-dimensional

configurations using RRT? as motion planner for the single agent. The approach is

simple but promising.

Possible future work includes the natural extension of the implemented algorithm

to a 3D environment; the implementation of a better, and more accurate steering

function; and the implementation of the communication protocol.

56

Bibliography

[1] European Commission-Transport-Transport modes-Air-Single European Sky.

https://ec.europa.eu/transport/modes/air/single european sky en

[2] Bascetta, L., Mendizabal Arrieta, I., & Prandini, M. (2017).Flat-RRT*: A

sampling-based optimal trajectory planner for differentially flat vehicles with con-

strained dynamics. In 20th World Congress of the International Federation of

Automatic Control (pp. 1-6).

[3] Braitenberg, V. (1986). Vehicles: Experiments in synthetic psychology. MIT

press.

[4] Corke, P. (2011).Robotics, vision and control: fundamental algorithms in MAT-

LAB (Vol. 73). Springer.

[5] Dimitrov, P., Piroddi, L., & Prandini, M. (2016, July). Distributed allocation of

a shared energy storage system in a microgrid. In American Control Conference

(ACC), 2016 (pp. 3551-3556). IEEE.

[6] European Commission, 29 July 2010, Commission Regulation (EU (EU) No

691/2010 of 29 July 2010 laying down a performance scheme for air navigation

services and network functions and amending Regulation (EC) No 2096/2005

laying down common requirements for the provision of air navigation services

,OJ L201/1 of 3 August 2010.

[7] European Commission, 3 May 2013, COMMISSION IMPLEMENTING REGU-

LATION (EU) No 390/2013 of 3 May 2013 laying down a performance scheme

for air navigation services and network functions, OJ L128/1 of 9 may 2013.

[8] European Commission, 27 June 2014, COMMISSION IMPLEMENTING REG-

ULATION (EU) No 716/2014 of 27 June 2014 on the establishment of the Pi-

lot Common Project supporting the implementation of the European Air Traffic

Management Master Plan, OJ L190/19 28 June 2014.

57

Bibliography

[9] Hansen, J. (2011). The economics of search engines.

[10] Hu, J., Prandini, M., & Sastry, S. (2002). Optimal coordinated maneuvers for

three-dimensional aircraft conflict resolution . Journal of Guidance, Control, and

Dynamics, 25(5), 888-900.

[11] Karaman, S., Frazzoli, E. (2011). Sampling-based algorithms for optimal motion

planning. The International Journal of Robotics Research, 30, 846–894

[12] Kavraki L., Svestka P., Latombe J.-C., & Overmars M. (1996). Probabilistic

roadmaps for path planning in high-dimensional configuration spaces, IEEE Tran.

Robot. Autom., vol. 12, no. 4, pp. 566–580, 1996.

[13] Kavraki E., Kolountzakis M., & Latombe J.-C. (1998). Analysis of probabilistic

roadmaps for path planning, IEEE Transactions on Robotics and Automation,

vol. 14, no. 1, pp. 166–171.

[14] S. M. LaValle & J. J. Kuffner (2001). Randomized kinodynamic planning, Int. J.

Robot. Res., vol. 20, no. 5, pp. 378–400.

[15] Lavalle Steven M. (2006) Planning Algorithms, Cambridge University Press

[16] Likhachev, M., & Ferguson, D. (2009).Planning long dynamically feasible maneu-

vers for autonomous vehicles. The International Journal of Robotics Research,

28(8), 933-945.

[17] Pivtoraiko, M., Knepper, R. A., & Kelly, A. (2009).Differentially constrained

mobile robot motion planning in state lattices. Journal of Field Robotics, 26(3),

308-333.

[18] Ragaglia, M., Prandini, M., & Bascetta, L. (2015, July). Poli-RRT*: optimal

RRT-based planning for constrained and feedback linearisable vehicle dynamics.

In Control Conference (ECC), 2015 European (pp. 2521-2526). IEEE.

[19] Ragaglia, M., Prandini, M., & Bascetta, L. (2016, June).Multi-agent Poli-RRT.

In International Workshop on Modelling and Simulation for Autonomous Sys-

tems (pp. 261-270). Springer International Publishing.

[20] Thrun, S., Burgard, W., & Fox, D. (2005).Probabilistic robotics. MIT press.

[21] Siciliano, B., Sciavicco, L., Villani, L., & Oriolo, G. (2008). Modellistica, Piani-

ficazione E Controllo. The Mcgraw-Hill.

[22] Vidal, J. M., & Vidal, J. M. (2006).Fundamentals of multiagent systems.

[23] Wooldridge, M. (2009).An introduction to multiagent systems. John Wiley &

Sons.

58

Bibliography

[24] Yang, Y., Zhang, J., Cai, K. Q., & Prandini, M. (2017).Multi-aircraft Conflict

Detection and Resolution Based on Probabilistic Reach Sets. IEEE Transactions

on Control Systems Technology, 25(1), 309-316.

59

Appendix A

Appendix: MATLAB code

Multi-RRT*

1 %% General s c r i p t f o r barga in ing

2 t i c

3 %% Var iab le i n i z i a l i z a t i o n

4 c l c

5 c l o s e a l l

6 c l c

7 f l a g =0; % Flag v a r i a b l e to check i f

the trend o f the

8 % Percentages i s monotonic

dec rea s ing .

9 contatore =0;

10 s t a r t 1 =[460 4 6 0 ] ; % Sta r t i ng po int o f the f i r s t

agent .

11 s t a r t 2 =[490 2 2 0 ] ; % Sta r t i ng po int o f the second

agent .

12 s t a r t 3 =[320 3 0 0 ] ; % Sta r t i ng po int o f the th i rd

agent .

13 goa l1 =[150 5 0 ] ; % Goal po int o f the f i r s t

agent .

60

A – Appendix: MATLAB code

14 goa l2 =[270 2 0 ] ; % Goal po int o f the second

agent .

15 goa l3 =[20 2 0 ] ; % Goal po int o f the th i rd

agent .

16

17 o b s t a c l e =[200 ,120 ,100 ,300 ] ; % D e f i n i t i o n o f a r e c tangu l a r

obs tac l e , the

18 % f i r s t two numbers i n d i c a t e

the coord inate

19 % of the down l e f t corner , the

other are the

20 % span in the h o r i z o n t a l and

v e r t i c a l

21 % d i r e c t i o n s .

22

23 Sa fe tyDi s tance=Mig l i a metr i (5 ) ;% Safe ty d i s t ance cons idered

by law to

24 % guarantee the t r a j e c t o r i e s

o f two

25 % a i r p l a n e s be s a f e .

26

27 CostCostra int =20; % How much i s the time

d i s t ance

28 % c o n s t r a i n t between the

a i r p l a n e s .

29

30 eps=Sa fe tyDi s tance /4 ; % Maximum step s i z e in case

the sampled

31 % point i s too f a r to reach ,

32 % in p r a c t i c e i t w i l l d e f i n e

the maximum

33 % length o f each segment o f

the t r e e .

61


34 % NB: Now with updated

s t e e r i n g func t i on i

35 % w i l l obta in that a l l the

segments o f the

36 % t r a j e c t o r i e s w i l l have

length eps .

37

38 numNodes1=11000; % Maximum number o f sample f o r

the t r e e o f

39 % the agent 1 .


the t r e e o f

41 % the agent 2 .


the t r e e o f

43 % the agent 3 .

44 Range=[500 5 0 0 ] ; % Range o f sampling .

45

46

47

48

49 % Sicurezza=Mig l i a metr i (5 ) ; % non dovrebbe s e r v i r e dopo

c o n t r o l l o .

50 %

51 % s i c u r e z z a 2=CostCostra int ; % non dovrebbe s e r v i r e dopo

c o n t r o l l o .

52 %

53 % Safe tyDi s tance=Mig l i a metr i (5 ) ;% s a f e t y d i s t ance cons idered

by law to

54 % % guarantee the t r a j e c t o r y

55 %

56

57

62


58

59 v e l o c i t y =15.2167; % Ve loc i ty o f the agents ,

r i g h t now i t has

60 % been cons idered cos tant and

equal

61 % f o r both the agents , i s a l s o

p o s s i b l e to

62 % implement in such a way to

have a d i f f e r e n t

63 % v e l o c i t y f o r each agent . I t

i s in km/min .

64

65

66 time =0.5; % I s the step s i z e when

d i s c r e t i z i n g the

67 % t r a j e c t o r y in sma l l e r po in t s

.

68 Wiring=2∗eps ; % The wir ing o f RRT∗ i t i s the

rad iu s i n s i d e

69 % which we look f o r sho r t e r

paths i n s i d e the

70 % tree .

71

72

73 m a x s t e e r i n g i n c o s =0.4 ; % Maximum s t e e r i n g c o n s t r a i n t

( used to d e f i n e

74 % i f a de f ined d i r e c t i o n i s

a c c e s s i b l e

75 % i n s i d e the s t e e r i n g func t i on

) .

76

77 max steer ing=pi /4 ; % Maximum stee r ing , in case a

change o f

63


78 % d i r e c t i o n i s not f e a s i b l e

the agent w i l l

79 % s t e e r o f ’maximum stee r ing ’

in the

80 % d i r e c t i o n o f the sample .

81

82 s t a r t a n g l e 1=pi /2 ; % Sta r t i ng ang le o f the f i r s t

agent

83 s t a r t a n g l e 2=pi /2 ; % Sta r t i ng ang le o f the second

agent

84 s t a r t a n g l e 3=−pi /2 ; % Sta r t i ng ang le o f the th i rd

agent

85

86

87 %% creaz i one d e l l a s t r u c t

88 % In t h i s s e c t i o n i am c r e a c t i n g a s t r u c t v a r i a b l e ’Nodo ’ ,

used l a t e r to

89 % keep tracked the prev ious d e a c t i v a t i o n s e f f e c t u a t e d .

90 % Each node w i l l have :

91

92 Nodo . i n d i c e =1; % Exc lus ive index that

c h a r a c t e r i z e each nodo

93 % element in an e x c l u s i v e way .

94

95 Nodo . a t t i v i =[1 1 1 ] ; % Vector o f the agents that are

a c t i v e in t h i s

96 % node .

97

98 Nodo . p e r c e n t u a l i =[0 0 0 ] ; % Vector o f the percentages o f the

paths found

99 % in t h i s node .

100

64


101 Nodo . predece s so re =0; % Index o f the predece s so r o f t h i s

node .

102

103 nodi = [ ] ; % This i s the t r e e that w i l l

c on ta in s a l l the

104 % nodes .

105 %% F i r s t agent

106 % The f i r s t agent w i l l compute h i s own t r a j e c t o r y knowing

only h i s s t a r t

107 % and h i s goa l po int

108

109 t i c

110 [ path1 t ree1 ]= F ind pa th t r e e nop l o t ( s ta r t1 , goal1 , Range ,

obs tac l e , eps . . .

111 , numNodes1 , v e l o c i t y , ’ g ’ , Wiring , max s t e e r ing in co s ,

max steer ing . . .

112 , s t a r t a n g l e 1 , 0 , ’b ’ , ’m’ ) ;

113 toc

114 path11=path1 ;

115

116

117 %% Second agent

118 % The second agent w i l l compute h i s own t r a j e c t o r y knowing

only h i s s t a r t

119 % and h i s goa l po int .

120

121 t i c



123 , numNodes2 , v e l o c i t y , ’ y ’ , Wiring , max s t e e r ing in co s ,

max steer ing . . .

124 , s t a r t a n g l e 2 , 0 , ’ g ’ , ’ y ’ ) ;

125 path22=path2 ;

65


126 toc

127

128 %% th i rd agent

129 % The th i rd agent w i l l compute h i s own t r a j e c t o r y knowing

only h i s s t a r t i n g

130 % and h i s goa l po int .

131

132 t i c



134 , numNodes3 , v e l o c i t y , ’m’ , Wiring , max s t e e r ing in co s ,

max steer ing . . .

135 , s t a r t a n g l e 3 , 0 , ’ k ’ , ’ r ’ ) ;

136 path33=path3 ;

137

138 % Save the f i r s t f i g u r e that conta in 3 paths ( green the f i r s t

, ye l low the

139 % second and purple the th i rd ) .

140

141 pr in t ( ’ C r o s s f i s t r 2 n 5 0 0 0 c o s t 2 0 p r i m i 2 ’ , ’−djpeg ’ ) ;

142 s a v e f i g ( ’ C r o s s f i s t r 2 n 5 0 0 0 c o s t 2 0 p r i m i 2 ’ ) ;

143 toc

144

145 memory=[0 0 0 ] ’ ; % The memory vector i s c reated and

s e t to [ 0 0 0 ] .

146

147 %% Active nodes

148

149 p a t h o r i g i n a l i =[path1 path2 path3 ] ; % Here i am sav ing f o r

l a t e r the

150 % path found u n t i l l now

, to use i t

66


151 % l a t e r to compute the

percentages .

152

153 i =1;

154 nodes =[1 1 1 ] ; % I n i t i a l i z a t i o n o f the

nodes vector .

155

156 c o l l i s i o n = . . .

157 C h e c k c o l l i s i o n ( path1 , Concatenate paths ( path2 , path3 , 0 , 0 , 0 , 2 ) ,

Sa f e tyDi s tance . . .

158 , CostCostra int ) ∗ . . .



160 , CostCostra int ) ∗ . . .



162 , CostCostra int )

163

164 %% While c y c l e

165 whi le ˜ c o l l i s i o n && ( nodes (1 )==1 | | nodes (2 )==1 | | nodes (3 )

==1)

166 f i g u r e

167 contatore =0;

168 max=0;

169 %% Computing the a l t e r n a t i v e s

170 % A s e r i e s o f i f and e l e s e , one f o r each agent w i l l

compute new paths

171 % i f needed and the corresponding percentages .

172

173 i f nodes (1 )==1 % I f the f i r s t agent i s actve .

174 t i c

175

67


176 t r e e1 mod i f i ed=Removing nodes noplot ( tree1 ,

Sa fe tyDi s tance . . .

177 , Concatenate paths ( path2 , path3 , 0 , 0 , 0 , 2 ) ,

CostCostra int ) ;

178

179 numMissing=Count miss ing nodes ( t r e e1 mod i f i ed ) ;

180

181 [ p a t h 1 a l t e r n a t i v e 1 t r e e 1 a l t e r n a t i v e 1 ]=

Cont inue r r t nop lo t ( s t a r t 1 . . .

182 , goal1 , t r ee1 mod i f i ed , Range , obs tac l e , eps , numMissing

. . .

183 , Sa fetyDistance , v e l o c i t y , Concatenate paths ( path2 ,

path3 , 0 , 0 , 0 , 2 ) . . .

184 , Wiring , ’ g ’ , CostCostra int , max s t e e r ing in co s ,

max steer ing . . .

185 , s t a r t a n g l e 1 , 0 , ’b ’ , ’m’ ) ;

186

187 toc

188 percentage1 =100∗ T o t a l c o s t ( p a t h 1 a l t e r n a t i v e 1 ) /

T o t a l c o s t ( path1 ) ;

189

190 e l s e

191 percentage1=10e5 ; % I f the node i s not a c t i v e the

corresponding

192 % percentage i s s e t to 10 e5 .

193 end

194 i f nodes (2 )==1 % I f the second agent i s a c t i v e .

195 t i c

196




CostCostra int ) ;

68


199


201

202 [ p a t h 2 a l t e r n a t i v e t r e e 2 a l t e r n a t i v e ]=



. . .


path3 , 0 , 0 , 0 , 2 ) . . .

205 , Wiring , ’ y ’ , CostCostra int , max s t e e r ing in co s ,

max steer ing . . .

206 , s t a r t a n g l e 2 , 0 , ’ g ’ , ’ y ’ ) ;

207

208 toc

209 percentage2 =100∗ T o t a l c o s t ( p a t h 2 a l t e r n a t i v e ) /


210

211 e l s e


corresponding


214 end

215

216 i f nodes (3 )==1 % I f agent three i s a c t i v e .

217 t i c

218




CostCostra int ) ;

221


223

69


224 [ p a t h 3 a l t e r n a t i v e t r e e 3 a l t e r n a t i v e ]=



. . .


path2 , 0 , 0 , 0 , 2 ) . . .

227 , Wiring , ’m’ , CostCostra int , max s t e e r ing in co s ,

max steer ing . . .

228 , s t a r t a n g l e 3 , 0 , ’ g ’ , ’ y ’ ) ;

229

230 toc

231 percentage3 =100∗ T o t a l c o s t ( p a t h 3 a l t e r n a t i v e ) /


232

233 e l s e


corresponding


236 end

237

238 vector p =[ percentage1 percentage2 percentage3 ] ; % Creat ion

o f the vector

239 %

conta in ing

the

ac tua l

240 %

percentages

.

241

242

243

244

70


245

246

247

248

249 memory=[memory vector p ’ ] ; % Updating the memory

vector .

250 %% Updating the New Nodo

251 New Nodo=Nodo ; % Creat ion o f a new node .

252

253 New Nodo . i n d i c e=i ; % ’ i ’ w i l l be the index

o f the node .

254 % NB: ’ i ’ i s the index o f

the whi le

255 % c y c l e .

256

257 New Nodo . a t t i v i=nodes ; % Updating the a c t i v e

nodes at t h i s

258 % moment .

259

260 New Nodo . p e r c e n t u a l i=vector p ; % Updating the percentage

vector o f the

261 % new nodo .

262

263 i f f l a g==1

264 New Nodo . predece s so re=e r e d i t a ;%

265 e l s e

266 New Nodo . predece s so re=i −1; %

267 end

268 New Nodo . paths =[ p a t h 1 a l t e r n a t i v e 1 p a t h 2 a l t e r n a t i v e

p a t h 3 a l t e r n a t i v e ] ;

269 % Updating the paths o f new nodo , with the ac tua l path

computed in t h i s

270 % c y c l e .

71


271

272 %% Checking i f the percentages are worse

273 % In t h i s s e c t i o n a f o r c y c l e w i l l check i f a l l the

percentages have

274 % become worse , and t h e r e f o r e i f backtrack ing must be

ac t i va t ed .

275

276 i f i ˜=1

277 f o r j =1:3

278

279 i f New Nodo . p e r c e n t u a l i ( j ) > . . .

280 nodi (New Nodo . predece s so re ) . p e r c e n t u a l i ( j )

&&.. .

281 New Nodo . a t t i v i ( j )==1

282

283 contatore=contatore+1

284 % I f the percentage i s worsen , and the

corresponding node i s

285 % i s a c t i v e i n c r e a s e the ’ contatore ’ v a r i a b l e by

1 .

286 end

287 max=max+New Nodo . a t t i v i ( j ) ; %I f the

corresponding node i s

288 % a c t i v e i n c r e a s e

the ’max ’

289 % v a i a b l e by 1 .

290 end

291 e l s e

292 max=3 % At the f i r s t c y c l e the max i s s e t to three ,

and there are

293 % no prev ious percentages to compare with .

294 end

295

72


296 i f contatore==max

297 f l a g =1;

298 di sp ( s p r i n t f ( ’ f l a g i s s e t =1 ’ ) )

299 % I f the number o f percentages that have become worse ,

the ’ contatore var i ab l e ’

300 % i s equal to the number o f a c t i v e nodes , the ’max ’

var i ab l e , f l a g i s

301 % s e t equal to 1 . Otherwise i s s e t equal to zero .

302 e l s e

303 f l a g =0;

304 end

305

306 i f f l a g==1 && . . .

307 ( nodi (New Nodo . predece s so re ) . p e r c e n t u a l i ( 1 )<10e2

| | . . .

308 nodi (New Nodo . predece s so re ) . p e r c e n t u a l i ( 2 )<10e2 | | . . .

309 nodi (New Nodo . predece s so re ) . p e r c e n t u a l i ( 3 )<10e2 )

310

311 % I f f l a g i s ac t ive , and at l e a s t one o f the percentages i s

l e s s than

312 % 1000 .

313 %% ind iv iduo l ’ u lt imo path a c c e t t a t o

314 v=nodi (New Nodo . predece s so re ) . a t t i v i −New Nodo . a t t i v i ;

315 % v i s the l a s t agent that have been deact ivated , i s found

subt ra c t i ng

316 % the ac tua l a c t i v e nodes to the prev ious one ( that o f the

predece s sor . )

317

318 % e r e d i t a=New Nodo . predece s so re ;

319

320 i f v (1 )==1 % the agent pr ev i ou s l y deac t iva ted i s a c t i va t e d

again , and h i s

321 % path i s s e t to the o r i g i n a l one .

73


322 nodes (1 ) =1;

323 path1=p a t h o r i g i n a l i ( 1 ) ;

324 di sp ( s p r i n t f ( ’ agent 1 has been a c t i v a t e d aga in ’ ) )

325

326 nodi (New Nodo . predece s so re ) . p e r c e n t u a l i ( 1 )= . . .

327 nodi (New Nodo . predece s so re ) . p e r c e n t u a l i ( 1 )+10e2 ;

328 end

329


again , and h i s


332 nodes (2 ) =1;



335

336 nodi (New Nodo . predece s so re ) . p e r c e n t u a l i ( 2 ) = . . .


338 end

339


again , and h i s


342 nodes (3 ) =1;



345 nodi (New Nodo . predece s so re ) . p e r c e n t u a l i ( 3 ) = . . .


347

348 end

349 [ f l a g idx ]=min ( nodi (New Nodo . predece s so re ) . p e r c e n t u a l i ) ;

350 % i d e n t i f y the minimum element o f the percentages vector o f

the

351 % pedeces sor .

74


352

353

354

355 e l s e % Flag i s NOT equal to 1 .

356 [ f l a g idx ]=min (New Nodo . p e r c e n t u a l i ) ; % The agent that

worsen l e s s h i s

357 % t r a j e c o t r y i s

i d e n t i f i e d .

358 idx

359 end

360 %% Deact ivat ion

361 % The agent that worsen l e s s h i s o r i g i n a l t r a j e c t o r y i s

deac t iva ted .

362 % h i s t r e e and path are d e f i n i t e l y updated , and i s

deac t iva ted .

363 i f idx==1

364 t r ee1=t r e e 1 a l t e r n a t i v e 1 ;

365 path1=p a t h 1 a l t e r n a t i v e 1 ;

366 nodes (1 ) =0;

367 di sp ( s p r i n t f ( ’ agent 1 deac t iva ted ’ ) )

368 end

369

370 i f idx==2

371 t r ee2=t r e e 2 a l t e r n a t i v e ;

372 path2=p a t h 2 a l t e r n a t i v e ;

373 nodes (2 ) =0;


375 end

376

377 i f idx==3

378 t r ee3=t r e e 3 a l t e r n a t i v e ;

379 path3=p a t h 3 a l t e r n a t i v e ;

380 nodes (3 ) =0;

75



382 end

383 nodi =[ nodi New Nodo ] ; % New nodo i s added to the ’ nodi vector

’ .

384

385 % i f i==1

386 % pr in t ( ’ middle s t ep 3 agents %d ’ , i , ’− djpeg ’ ) ;

387 % s a v e f i g ( ’ middle s t ep 3 agents ’ ) ;

388 % end

389

390 %% Saving the f i g u r e s

391 % Saving one f i g u r e f o r each i t e r a t i o n o f the loop .

392 i f i==1

393 pr in t ( ’ middle s t ep 3 agents 1 ’ , ’−djpeg ’ ) ;

394 s a v e f i g ( ’ middle s t ep 3 agents ’ ) ;

395 end

396

397 i f i==2



400 end

401

402 i f i==3



405 end

406

407 i f i==4



410 end

411

412

76


413 i=i +1;

414 nodes

415 end

416 %% Check i f the paths found r i s k c o l l i s i o n

417

418 c o l l i s i o n = . . .

419 C h e c k c o l l i s i o n ( path1 , Concatenate paths ( path2 , path3

, 0 , 0 , 0 , 2 ) . . .

420 , Sa fetyDistance , CostCostra int ) ∗ . . .


, 0 , 0 , 0 , 2 ) . . .

422 , Sa fetyDistance , CostCostra int ) ∗ . . .


, 0 , 0 , 0 , 2 ) . . .

424 , Sa fetyDistance , CostCostra int )

425

426

427 %% Distances

428 f i g u r e

429

430 er ror6=Compara path ( path1 , path3 , Range , v e l o c i t y ) ; % d i s t ance

po int to po int

431 % between

the

agent 1

and

432 % the agent

3 .

Black .

433


po int to po int

77


435 % between

the

agent 2

and

436 % the agent

3 .

Yellow .

437


po int to po int

439 % between

the

agent 1

and

440 % the agent

2 . Blue

.

441

442 pr in t ( ’ f i n a l paths ’ , ’−djpeg ’ ) ;

443 s a v e f i g ( ’ f i n a l paths ’ ) ;

444 toc

445

446 f i g u r e

447 p lo t ( error6 , ’ k ’ , ’ DisplayName ’ , . . .

448 ’ da mettere nome appropr iato [Km] , ( v i o l a vs verde ) ’ ) . . .

449 %f i n a l e a l t e r n a t i v o uno e due

450 s a v e f i g ( . . .

451 ’ d i s t anza path1 a l t e r n a t i v e g i a mod i f i c a to p a t h 3 a l t e r n a t i v e

’ ) ;

452 pr in t ( . . .

453 ’ d i s t anza path1 a l t e r n a t i v e g i a mod i f i c a to p a t h 3 a l t e r n a t i v e

’ , ’−djpeg ’ ) ;

454

78


455 f i g u r e

456 p lo t ( error7 , ’ y ’ , ’ DisplayName ’ , ’Nome da d e f i n i r e ’ )


458 s a v e f i g ( ’ d i s t anza path3 ( a l t e r n a t i v e g i a mod i f i c a to )

p a t h 2 a l t e r n a t i v e ’ ) ;

459 pr in t ( ’ d i s t anza path1 ( a l t e r n a t i v e g i a mod i f i ca to )

p a t h 2 a l t e r n a t i v e ’ . . .

460 , ’−djpeg ’ ) ;

461

462

463 f i g u r e

464 p lo t ( error8 , ’b ’ , ’ DisplayName ’ , ’Nome da d e f i n i r e ’ )


466 s a v e f i g ( ’ d i s t anza path1 ( a l t e r n a t i v e g i a mod i f i c a to )

p a t h 2 a l t e r n a t i v e ’ ) ;

467 pr in t ( ’ d i s t anza path1 ( a l t e r n a t i v e g i a mod i f i ca to )

p a t h 2 a l t e r n a t i v e ’ . . .

468 , ’−djpeg ’ ) ;

469

470

471

472 f i g u r e

473 hold on

474

475 p lo t ( error6 , ’ k ’ )

476 p lo t ( error7 , ’ y ’ )

477 p lo t ( error8 , ’b ’ )

478 s a v e f i g ( ’ d i s t anze in range d i tempo ’ ) ;

479 pr in t ( ’ d i s t anze in range d i tempo ’ , ’−djpeg ’ ) ;

480

481 %% Saving data and workspace

482 Cost1=path1 (1 ) . co s t ; %Salvo in un ve t to re i

t r e c o s t i

79


483 Cost2=path2 (1 ) . co s t ; %de i path 1 ,2 e 3 .

484 Cost3=path3 (1 ) . co s t ;

485 V e t t o r e c o s t i =[Cost1 ; Cost2 ; Cost3 ] ;

486 save ( ’ Workspace ’ ) ;

487 save ( ’ V e t t o r e c o s t i ’ , ’ V e t t o r e c o s t i ’ ) ;

488

489 f i d=fopen ( ’ V e t t o r e c o s t i . txt ’ , ’ a ’ ) ;

490 f p r i n t f ( f i d , ’%f %f %f ; \n ’ , [ Cost1 Cost2 Cost3 ] ) ;

491 f c l o s e ( f i d ) ;

492

493 toc

494

495 e x i t

80


Find path tree noplot

1 %% Find pa th t r e e nop l o t

2 % compute a tree , and f i n d a path p l o t t i n g only the r e s u l t

3

4 %s t a r t i s the s t a r t i n g po int o f our r r t ∗

5

6 % goa l i s the goa l po int o f our r r t ∗

7

8 % Range i s the range o f x and y where we take our samples .

9

10 % obs tac l e s , i s a vector [ x , y , z , k ] d e f i n i n g a re c tangu l a r

shape o b s t a c l e .

11

12 % eps repre sent the maximum span o f a segment o f the dynamic .

13

14 %numNodes i s the maximum number o f nodes that we want in our

t r e e .

15

16 %v e l o c i t y i s the cos tant v e l o c i t y o f the agent km/min used to

d e f i n e c o s t s

17 %as temporal c o e f f i c i e n t s .

18

19 %Wiring i s the max d i s t ance where rewind i s t r i e d u s u a l l y the

double o f

20 %eps

21

22 % co lo r e1 i s the c o l o r o f the l i n e s connect ing the node (

s t e e r i n g

23 % func t i on ) , c o l o r e2 i s the c o l o r o f the wir ing connect ions (

f i n a l t r e e ) , and

24 % co lo r e3 i s the c o l o r o f the f i n a l path .

25

81


26 % p l o t f l a g i s a boolean used to s p e c i f y i f we want to show

the f i g u r e or

27 % not .

28

29 f unc t i on [ path nodes ]= F ind pa th t r e e nop l o t ( s t a r t , goal , Range ,

o b s t a c l e s . . .

30 , eps , numNodes , v e l o c i t y , co lore3 , wir ing , m a x s t e e r i n g i n c o s

. . .

31 , max steer ing , s t a r t a n g l e , p l o t f l a g , co lore1 , c o l o r e2 )

32

33

34 x max = Range (1 ) ;

35 y max = Range (2 ) ;

36 o b s t a c l e = o b s t a c l e s ;

37 EPS = eps ;

38 numNodes = numNodes ;

39

40 q s t a r t . coord = [ s t a r t (1 ) s t a r t (2 ) ] ;

41 q s t a r t . c o s t = 0 ;

42 q s t a r t . parent = 0 ;

43 q goa l . coord = [ goa l (1 ) goa l (2 ) ] ;

44 q goa l . c o s t = 0 ;

45

46 nodes (1 ) = q s t a r t ;

47 f i g u r e (1 )

48 a x i s ( [ 0 x max 0 y max ] )

49 r e c t a n g l e ( ’ Po s i t i on ’ , obs tac l e , ’ FaceColor ’ , [ 0 . 5 . 5 ] )

50 hold on

51

52 f o r i = 1 : 1 : numNodes

53 q rand = [ f l o o r ( rand (1 ) ∗x max ) f l o o r ( rand (1 ) ∗y max ) ] ;

54 i f p l o t f l a g

82


55 p lo t ( q rand (1 ) , q rand (2 ) , ’ x ’ , ’ Color ’ , [ 0 0 .4470

0 . 7 4 1 0 ] )

56 end

57 % Break i f goa l node i s a l ready reached

58 f o r j = 1 : 1 : l ength ( nodes )

59 i f nodes ( j ) . coord == q goa l . coord

60 break

61 end

62 end

63

64 % Pick the c l o s e s t node from e x i s t i n g l i s t to branch out

from

65 nd i s t = [ ] ;


67 n = nodes ( j ) ;

68 tmp = d i s t (n . coord , q rand ) ;

69 nd i s t = [ nd i s t tmp ] ;

70 end

71 [ val , idx ] = min ( nd i s t ) ;

72 q near = nodes ( idx ) ;

73

74 q new . coord = steernew2 ( q rand , q near . coord , val , EPS,

nodes . . .

75 , q near . parent , s t a r t a n g l e , max steer ing ,

m a x s t e e r i n g i n c o s ) ;

76

77 i f n o C o l l i s i o n ( q rand , q near . coord , o b s t a c l e )

78


80

81 l i n e ( [ q near . coord (1 ) , q new . coord (1 ) ] , [ q near .

coord (2 ) . . .

83


82 , q new . coord (2 ) ] , ’ Color ’ , co lore1 , ’

LineWidth ’ , 2) ;

83

84 end

85

86 drawnow

87 hold on

88 q new . co s t = d i s t ( q new . coord , q near . coord ) / v e l o c i t y

+ q near . co s t ;

89

90 % Within a rad iu s o f r , f i n d a l l e x i s t i n g nodes

91 q nea re s t = [ ] ;

92 r = wir ing ;

93 ne ighbor count = 1 ;


95

96 i f n o C o l l i s i o n ( nodes ( j ) . coord , q new . coord ,

o b s t a c l e ) . . .

97 && d i s t ( nodes ( j ) . coord , q new . coord ) <= r

98

99 q nea re s t ( ne ighbor count ) . coord = nodes ( j ) .

coord ;

100 q nea re s t ( ne ighbor count ) . co s t = nodes ( j ) .

c o s t ;

101 q nea re s t ( ne ighbor count ) . parent=nodes ( j ) .

parent ;

102 ne ighbor count = neighbor count +1;

103 end

104 end

105

106 % I n i t i a l i z e co s t to cu r r en t l y known value

107 q min = q near ;

108 C min = q new . co s t ;

84


109

110 % I t e r a t e through a l l neare s t ne ighbors to f i n d

a l t e r n a t e lower

111 % cos t paths , a l s o keeping in c o n s i d e r a t i o n the

s t e e r i n g

112 % c o n s t r a i n t s .

113

114 f o r k = 1 : 1 : l ength ( q nea re s t )

115 [ x1 y1]= Crea vet tore ( q nea re s t ( k ) . coord , q new .

coord ) ;

116 i f q nea r e s t ( k ) . parent==0

117 angle1=a r c t a n m i g l i o r a t a ( x1 , y1 ) ;

118 c o s a l f a=cos ( angle1− s t a r t a n g l e ) ;

119 e l s e

120 [ x2 y2]= Crea vet tore . . .

121 ( nodes ( q nea re s t ( k ) . parent ) . coord , q nea re s t ( k

) . coord ) ;

122

123 c o s a l f a =(dot ( [ x1 y1 ] , [ x2 y2 ] ) ) . . .

124 /(norm ( [ x1 y1 ] ) ∗(norm ( [ x2 y2 ] ) ) ) ;

125 end

126

127 i f n o C o l l i s i o n ( q nea re s t ( k ) . coord , q new . coord ,

o b s t a c l e ) . . .

128 && q neare s t ( k ) . co s t . . .

129 + d i s t ( q nea re s t ( k ) . coord , q new . coord ) /

v e l o c i t y < . . .

130 C min && abs (1− c o s a l f a )<

m a x s t e e r i n g i n c o s

131

132 q min = q neare s t ( k ) ;

133 C min = q neare s t ( k ) . co s t . . .

85


134 + d i s t ( q nea re s t ( k ) . coord , q new . coord ) /

v e l o c i t y ;

135


137

138 l i n e ( [ q min . coord (1 ) , q new . coord (1 ) ] . . .

139 , [ q min . coord (2 ) , q new . coord (2 ) ] , ’

Color ’ , c o l o r e 2 ) ;

140

141 end

142 hold on

143 end

144 end

145

146 % Update parent to l e a s t cost−from node


148 i f nodes ( j ) . coord == q min . coord

149 q new . parent = j ;

150 end

151 end

152

153 % Append to nodes

154 nodes = [ nodes q new ] ;

155 end

156 end

157

158 D = [ ] ;


160 tmpdist = d i s t ( nodes ( j ) . coord , q goa l . coord ) ;

161 D = [D tmpdist ] ;

162 end

163

86


164 % Search backwards from goa l to s t a r t to f i n d the opt imal

l e a s t co s t path

165 [ val , idx ] = min (D) ;

166 q f i n a l = nodes ( idx ) ;

167 q goa l . parent = idx ;

168 q end = q goa l ;

169 nodes = [ nodes q goa l ] ;

170 k=1;

171 whi le q end . parent ˜= 0

172 s t a r t = q end . parent ;

173 path ( k )=q end ;

174 k=k+1;

175 l i n e ( [ q end . coord (1 ) , nodes ( s t a r t ) . coord (1 ) ] , [ q end .

coord (2 ) , nodes ( s t a r t ) . coord (2 ) ] , ’ Color ’ , co lore3 , ’


176 hold on

177 q end = nodes ( s t a r t ) ;

178 end

179 path (1 ) . co s t=d i s t ( path (1 ) . coord , path (2 ) . coord )+path (2 ) . co s t ;

180 path ( k )=q s t a r t ;

181 end

182

183

184

185 %% NOTES

186 % dimostraz ione che ogn nodo ha come parente un nodo

precedente .

187 % f o r i =1:1 : l ength ( nodes )

188 % i f nodes ( i ) . parent>i

189 % i

190 % e l s e

191 % end

192 % end

87


193 % s t a r t

88


Check collision

1 %% C h e c k c o l l i s i o n

2 % Check i f two paths are too near each other , return 1 i f

there i s no

3 % c o l l i s i o n , zero i f there i s c o l l i s i o n .

4

5 % Path1 i s the f i r s t path .

6

7 % Path2 i s the second path .

8

9 % Safe tyDi s tance i s the s a f e t y d i s t ance that we must

guarantee between the

10 % paths .

11

12 % CostCostra int i s the ’ time d i s tance ’ that we must guarantee

to c la im the

13 % absence o f c o l l i s i o n s .

14

15

16

17 f unc t i on i n t=C h e c k c o l l i s i o n ( path1 , path2 , Sa fetyDistance ,

CostCostra int )

18 i n t =1;

19 j =1;

20 i =1;

21 f o r j =1: l ength ( path1 )

22 f o r i =1: l ength ( path2 )

23 i f ( ( d i s t ( path1 ( j ) . coord , path2 ( i ) . coord )<

Sa fe tyDi s tance ) . . .

24 & ( abs ( path1 ( j ) . cost−path2 ( i ) . c o s t )<

CostCostra int ) )

25 i n t =0;

89


26 %i ; %j u s t check v a r i a b l e s uncomment to check the c o r r e c t

d i s t a n c e s

27 %j ;

28 end

29 end

30 end

31 end

90


Removing nodes noplot

1 %% Removing t r e e

2 % t h i s func t i on w i l l d e l e t e some nodes from a tree , the

e l im inated nodes

3 % w i l l be s e t with co s t=−1 and thanks to t h i s w i l l be

d i s carded by the next

4 % algor i thms . A node i s removed i f i t i s too near another

path , and i f the

5 % cos t to reach both the node o f the t r e e and the one o f the

path i s

6 % s imi l a r , or i f h i s predece s sor has been removed .

7

8 % tree i s the t r e e from which we remove the nodes .

9

10 % di s t ance i s one o f the parameter used to check i f a node

must be

11 % el iminated or not , in p a r t i c u l a r i t i s the minimum d i s t ance

a l lowed

12 %between the node and a given path .

13

14 % path i s the given path from which we compute the nodes to

remove .

15

16 % CostCostra int i s the minimum d i f f e r e n c e in c o s t s a l lowed to

do not

17 % e l i m in a t e the node . NB: Ful ly exp la ined in the t h e s i s .

18

19 f unc t i on t r e e=Removing nodes noplot ( tree , d i s tance , path ,

CostCostra int )

20 f o r l = 1 : l ength ( path ) ;

21 f o r j = 1 : l ength ( t r e e )

22

91


23 i f d i s t ( t r e e ( j ) . coord , path ( l ) . coord )<d i s t ance . . .

24 && ( abs ( path ( l ) . cost−t r e e ( j ) . c o s t )<

CostCostra int )

25

26 t r e e ( j ) . c o s t =−1;

27 % plo t ( t r e e ( j ) . coord (1 ) , t r e e ( j ) . coord (2 ) , ’ x ’ , ’

Color ’ , ’ r ’ )

28 end

29 end

30 end

31

32 f o r j = 2 : l ength ( t r e e )

33 i f t r e e ( t r e e ( j ) . parent ) . co s t == −1

34 t r e e ( j ) . c o s t =−1;

35 % plo t ( t r e e ( j ) . coord (1 ) , t r e e ( j ) . coord (2 ) , ’ x ’ , ’

Color ’ , ’ r ’ )

36 end

37 end

38 end

92


Count missing nodes

1 %% Count the number o f nodes that have been de l e t ed

2 % This func t i on w i l l s imply count the number o f nodes that

have been

3 % removed from a t r e e ( co s t==−1 as f l a g ) .

4

5 % Nodes i s the t r e e to ana lyze .

6

7 f unc t i on contatore=Count miss ing nodes ( nodes )

8

9 contatore =0;

10 f o r i =1: l ength ( nodes )

11 i f ( nodes ( i ) . c o s t==−1)

12 contatore=contatore +1;

13 end

14 end

93


Continue rrt noplot

1 %% Continue RRT∗

2 % This func t i on a l low the user to cont inue a prev ious RRT∗

s imu la t i on

3 % adding new nodes to the tree , i t i s a l s o p o s s i b l e to

s p e c i f y a s a f e t y

4 % di s t ance parameter , and a path to avoid , in such a way we

can guarantee

5 % no c o l l i s i o n with other t r a j e c t o r i e s .

6

7

8 % s t a r t i s the s t a r t i n g po int o f our r r t ∗ .

9

10 % goa l i s the goa l po int o f our r r t ∗

11

12 % Giventree i s the t r e e that we want to cont inue with our RRT

∗ a lgor i thm .

13


15

16 % obs tac l e s , i s a vector [ x , y , z , k ] d e f i n i n g a re c tangu l a r

shape o b s t a c l e .

17

18 % eps i s a v a r i a b l e r ep re s en t ing the maximum span o f a

segment o f the t r e e .

19

20 % numNodes i s the maximum number o f nodes that we want in our

t r e e .

21

22 % v e l o c i t y i s the cos tant v e l o c i t y o f the agent km/min used

to d e f i n e c o s t s

23 % as temporal c o e f f i c i e n t s .

94


24

25 % Wiring i s the max d i s t ance where rewind i s appl ied , u s u a l l y

the double o f

26 % eps .

27

28 % Avoiding path i s the path that must be avoided .

29

30 % Safe tyDi s tance i s the s a f e t y d i s t ance that we must

guarantee between our

31 % path and the path to avoid .

32

33 % CostCostra int i s the ’ time c o s t r a i n t ’ that we must

guarantee to avoid the

34 % t r a j e c t o r i e s .

35

36 % co lo r e1 i s the c o l o r o f the l i n e s connect ing the node (

s t e e r i n g

37 % func t i on ) , c o l o r e2 i s the c o l o r o f the wir ing connect ions (

f i n a l t r e e ) , and

38 % co lo r e3 i s the c o l o r o f the f i n a l path .

39

40 % p l o t f l a g i s a boolean used to s p e c i f y i f we want to show

the f i g u r e or

41 % not .

42

43 f unc t i on [ path nodes ]= Cont inue r r t nop lo t ( s t a r t , goal ,

Giventree , Range . . .

44 , ob s t a c l e s , eps , numNodes , s a f e t y d i s t a n c e , v e l o c i t y ,

AvoidingPath . . .

45 , wir ing , co lore3 , CostCostra int , max s t e e r ing in co s ,

max steer ing . . .

46 , s t a r t a n g l e , p l o t f l a g , co lore1 , c o l o r e2 )

47

95


48 x max = Range (1 ) ;

49 y max = Range (2 ) ;

50 q s t a r t . coord = [ s t a r t (1 ) s t a r t (2 ) ] ;

51 q s t a r t . c o s t = 0 ;


53 EPS = eps ;

54 q goa l . coord = [ goa l (1 ) goa l (2 ) ] ;

55 q goa l . c o s t = 0 ;

56 o b s t a c l e = o b s t a c l e s ;

57 a x i s ( [ 0 x max 0 y max ] )

58 nodes=Giventree ;




62 p lo t ( q rand (1 ) , q rand (2 ) , ’ x ’ , ’ Color ’ , [ 0 0 .4470

0 . 7 4 1 0 ] )

63


65 end



68 break

69 end

70 end

71


from

73 nd i s t = [ ] ;


75 i f nodes ( j ) . c o s t˜=−1 & nodes ( j ) . coord˜=q goa l . coord

76 n = nodes ( j ) ;



96


79 e l s e

80 tmp = 3000 ;

81 nd i s t =[ nd i s t tmp ] ;

82 end

83 end

84 [ val , idx ] = min ( nd i s t ) ;


86


nodes . . .

88 , q near . parent , s t a r t a n g l e , max steer ing ,


89

90 q new . co s t = d i s t ( q new . coord , q near . coord ) / v e l o c i t y +

q near . co s t ;

91 q new . parent=idx ;

92

93 f o r l = 1 : l ength ( AvoidingPath )

94

95 i f d i s t ( q new . coord , AvoidingPath ( l ) . coord )<

s a f e t y d i s t a n c e . . .

96 & ( abs ( AvoidingPath ( l ) . cost−q new . co s t )<

CostCostra int )

97

98 nodes ( j ) . c o s t =−1;

99 end

100 end

101 aux path (1 )=q near ;

102 aux path (2 )=q new ;

103

104 i f n o C o l l i s i o n ( q rand , q near . coord , o b s t a c l e ) && . . .

105 C h e c k c o l l i s i o n ( aux path , AvoidingPath ,

s a f e t y d i s t a n c e . . .

97


106 , CostCostra int )

107


109 l i n e ( [ q near . coord (1 ) , q new . coord (1 ) ] , [ q near .

coord (2 ) . . .

110 , q new . coord (2 ) ] , ’ Color ’ , co lore1 , ’


111 end

112

113 drawnow

114 hold on

115 q new . co s t = d i s t ( q new . coord , q near . coord ) / v e l o c i t y

+ q near . co s t ;

116


118 q nea re s t = [ ] ;

119 r = wir ing ;



122


o b s t a c l e ) . . .

124 & d i s t ( nodes ( j ) . coord , q new . coord ) <= r

. . .

125 & nodes ( j ) . c o s t˜=−1 . . .

126 & nodes ( j ) . coord˜=q goa l . coord

127


coord ;


c o s t ;


131 end

98


132 end

133


135 q min = q near ;


137



139 % cos t paths

140


142


o b s t a c l e ) . . .

144 && q neare s t ( k ) . co s t + . . .

145 d i s t ( q nea re s t ( k ) . coord , q new . coord ) /

v e l o c i t y < C min

146


148 C min = q neare s t ( k ) . co s t + . . .

149 d i s t ( q nea re s t ( k ) . coord , q new . coord ) /

v e l o c i t y ;

150


152

153 l i n e ( [ q min . coord (1 ) , q new . coord (1 ) ] , [

q min . coord (2 ) . . .

154 , q new . coord (2 ) ] , ’ Color ’ , c o l o r e2 ) ;

155

156 end

157 hold on

158 end

159 end

99


160





165 end

166 end

167



170 end

171 end

172

173 D = [ ] ;


175 i f nodes ( j ) . c o s t˜=−1 & nodes ( j ) . coord˜=q goa l . coord


177 e l s e

178 tmpdist = 3000 ;

179 end

180


182 end

183



185 [ val , idx ] = min (D) ;





190 k=1; %%modi f ied by paolo ve rbar i


100



193 path ( k )=q end ; %%modi f ied by paolo ve rbar i

194 k=k+1; %% modi f ied by paolo ve rbar i

195


coord (2 ) . . .

197 , nodes ( s t a r t ) . coord (2 ) ] , ’ Color ’ , co lore3 , ’


198

199 hold on


201 end

202 path (1 ) . co s t=d i s t ( path (1 ) . coord , path (2 ) . coord ) / v e l o c i t y+path

(2 ) . co s t ;

203 path ( k )=q s t a r t ;

204 end

101


Compara path

1 %% Compara path

2

3 % This func t i on w i l l compute the d i s t ance po int to po int

between two paths .

4 %F i r s t the two paths are i n t e r p o l a t e d through the

In t e rpo l a pa th

5 %funct ion , then the d i s t ance po int to po int ( more or l e s s ) i s

computed and

6 %stored i n s i d e a vector , c a l l e d e r ro r .

7

8 f unc t i on e r ro r=Compara path ( path1 , path2 , Range , v e l o c i t y )

9 e r ro r =0;

10 max=10000;

11 cmin=max ;

12 path1= i n t e r p o l a p a t h ( path1 , Range , v e l o c i t y ) ;

13 path2= i n t e r p o l a p a t h ( path2 , Range , v e l o c i t y ) ;

14

15 %% parte vecch ia che ho cambiato ( no s p l i n e )

16 % path1= Convert path ( path1 , v e l o c i t y , time ) ;

17 % path2= Convert path ( path2 , v e l o c i t y , time ) ;

18 % f o r j =1: l ength ( path1 )

19 % cmin=max ;

20 % f o r i =1: l ength ( path2 )

21 % i f abs ( path1 ( j ) . cost−path2 ( i ) . c o s t )<

CostCostra int

22 % newdist=d i s t ( path1 ( j ) . coord , path2 ( i ) .

coord ) ;

23 % end

24 % i f cmin>newdist

25 % cmin=newdist ;

26 % end

102


27 % end

28 % erro r =[ e r ro r cmin ] ;

29 % end

30 % end

31 % %% parte con s p l i n e

32 % error=d i s t ( path1 (1 ) . coord , path2 (1 ) . coord ) ;

33 % i f l ength ( path1 )>l ength ( path2 )

34 % j=length ( path1 )− l ength ( path2 ) ;

35 % maximum=length ( path1 ) ;

36 %

37 % f o r i=j : maximum

38 % erro r =[ e r ro r d i s t ( path1 ( i ) . coord , path2 ( i− j +1)

. coord ) ] ;

39 % end

40 % e l s e i f l ength ( path1 )<l ength ( path2 )

41 % j=length ( path2 )− l ength ( path1 ) ;



44 % erro r =[ e r ro r d i s t ( path1 ( i− j +1) . coord , path2 ( i ) .

coord ) ] ;

45 % end

46 % e l s e j =1;



49 % erro r =[ e r ro r d i s t ( path1 ( i ) . coord , path2 ( i ) .

coord ) ] ;

50 % end

51 % end

52 % end

53 %

54 % end

55

56

103


57 f o r j =1: l ength ( path1 )

58 cmin=max ;


60 i f abs ( path1 ( j ) . cost−path2 ( i ) . c o s t ) <0.5

61 newdist=d i s t ( path1 ( j ) . coord , path2 ( i ) .

coord ) ;

62 end

63 i f cmin>newdist

64 cmin=newdist ;

65 end

66 end

67 e r ro r =[ e r ro r cmin ] ;

68 end

69 end

104


steernew2

1 %% steernew2

2

3 % This i s a p o s s i b l e s t e e r i n g func t i on developed by me, does

not cons ide r

4 % any e x p l i c i t dynamic , only a c o n s t r a i n t in the maximum

s t e e r i n g .

5 % Can be e a s i l y changed with any kind o f s t e e r i n g funct ion ,

6 % that can a l s o cons ide r any kind o f d i f f e r e n t i a l c o n s t r a i n t .

7

8 %

9

10 f unc t i on new = steernew2 ( qr , qn , val , eps , tree , parent ,

s t a r t a n g l e . . .

11 , max steer ing , m a x s t e e r i n g i n c o s )

12

13 [ x1 y1]= Crea vet tore (qn , qr ) ;

14 i f parent==0 %f o r the s t a r t i n g node each other d i r e c t i o n

i s f i n e



17 e l s e

18 [ x2 y2]= Crea vet tore ( t r e e ( parent ) . coord , qn ) ;

19 c o s a l f a =(dot ( [ x1 y1 ] , [ x2 y2 ] ) ) /(norm ( [ x1 y1 ] ) ∗(norm ( [ x2

y2 ] ) ) ) ;

20 end

21 i f abs (1− c o s a l f a )<m a x s t e e r i n g i n c o s % I t i s an easy

way to eva luate

22 % the s t e e r ing ,

c o s a l f a i s the

23 % co s in e o f the

ang le between

105


24 % the two segments

,

25 % ( the new one and

the prev ious

26 % one ) , i f c o s a l f a

i s near to

27 % one , then the

s t e e r i n g i s

28 % reasonab le .

29

30 % i f va l >= eps

31 qnew (1) = qn (1) + ( ( qr (1 )−qn (1) ) ∗ eps ) / d i s t ( qr , qn ) ;

%( ( qr (1 )−qn (1) ) ) / d i s t ( qr , qn )=cos

theta ( angolo compreso f r a ca t e to e ipotenusa ,

o s s i a d i r e z i o n e de l ve i co l o , questa d i r e z i o n e

s c a l a t a per un maximum step s i z e che eps )

32 qnew (2) = qn (2) + ( ( qr (2 )−qn (2) ) ∗ eps ) / d i s t ( qr , qn ) ;

33 % e l s e

34 % qnew (1) = qr (1 ) ;

35 % qnew (2) = qr (2 ) ;

36 % end

37 new = [ qnew (1) , qnew (2) ] ;

38 e l s e % Case the s t e e r i n g i s not reasonable , we try to

s t e e r as much

39 % as we can toward the de s i r ed node .

40

41 i f parent ==0

42 angle2=s t a r t a n g l e ;


44 e l s e



47 end

106


48

49 i f ( angle1<angle2 ) % cond i t i on used to understand

the d i r e c t i o n

50 % of the s t e e r i n g .

51

52 qnew (1) = qn (1) +cos ( angle2−max steer ing ) ∗ eps ;

53 qnew (2) = qn (2) +s i n ( angle2−max steer ing ) ∗ eps ;

54 new = [ qnew (1) , qnew (2) ] ;

55 e l s e

56 qnew (1) = qn (1) + cos ( ang le2+max steer ing ) ∗ eps ;

57 qnew (2) = qn (2) + s i n ( ang le2+max steer ing ) ∗ eps ;

58 new = [ qnew (1) , qnew (2) ] ;

59 end

60

61

62 end

63 end

107


noCollision

1 f unc t i on nc = n o C o l l i s i o n ( n2 , n1 , o )

2 A = [ n1 (1 ) n1 (2 ) ] ;

3 B = [ n2 (1 ) n2 (2 ) ] ;

4 obs = [ o (1 ) o (2 ) o (1 )+o (3) o (2 )+o (4) ] ;

5

6 C1 = [ obs (1 ) , obs (2 ) ] ;

7 D1 = [ obs (1 ) , obs (4 ) ] ;

8 C2 = [ obs (1 ) , obs (2 ) ] ;

9 D2 = [ obs (3 ) , obs (2 ) ] ;

10 C3 = [ obs (3 ) , obs (4 ) ] ;

11 D3 = [ obs (3 ) , obs (2 ) ] ;

12 C4 = [ obs (3 ) , obs (4 ) ] ;

13 D4 = [ obs (1 ) , obs (4 ) ] ;

14

15 % Check i f path from n1 to n2 i n t e r s e c t s any o f the four

edges o f the

16 % o b s t a c l e

17

18 i n t s 1 = ccw (A, C1 , D1) ˜= ccw (B, C1 , D1) && ccw (A,B, C1) ˜=

ccw (A,B, D1) ;


ccw (A,B, D2) ;


ccw (A,B, D3) ;


ccw (A,B, D4) ;

22 i f i n t s 1==0 && i n t s 2==0 && i n t s 3==0 && i n t s 4==0

23 nc = 1 ;

24 e l s e

25 nc = 0 ;

26 end

108


27 end

109


Interpola path

1 %% G e s t i s c i path ( agg iunta dinamica )

2 % t h i s func t i on w i l l : d e f i n e waypoints and i n t e r p o l a t e them

through the

3 % SPLINE .

4

5 f unc t i on new path=i n t e r p o l a p a t h ( path given , Range , v e l o c i t y )

6 path=Reverse path ( path g iven ) ;

7 pathx=path ;

8 % d e f i n e waypoints

9 t = [ 1 : l ength ( pathx ) ] ;

10 xx = [ ] ;

11 yy = [ ] ;

12 f o r i =1: l ength ( pathx )

13 xx=[xx pathx ( i ) . coord (1 ) ] ;

14 yy=[yy pathx ( i ) . coord (2 ) ] ;

15 end

16 % c a l c u l a t e s p l i n e f o r way po in t s

17 tq = 0 : 0 . 1 : l ength ( pathx ) ;

18 xq = inte rp1 ( t , xx , tq , ’ s p l i n e ’ ) ;

19 yq = inte rp1 ( t , yy , tq , ’ s p l i n e ’ ) ;

20 Plot path ( pathx , Range , ’b ’ )

21 hold on

22 p lo t ( xq , yq , ’ r ’ )

23 new path (1 ) . coord =[xq (1 ) yq (1 ) ] ;

24 new path (1 ) . co s t =0;

25 f o r i =2: l ength ( xq )

26 new path ( i ) . coord =[xq ( i ) yq ( i ) ] ;

27 new path ( i ) . c o s t= . . .

28 d i s t ( new path ( i ) . coord , new path ( i −1) . coord )

/ v e l o c i t y . . .

29 + new path ( i −1) . co s t ; . . .

110


30 % attenz i one ancora una vo l t a c o n t r o l l o i l tempo

l i n e a r e ,

31 % non q u e l l o i n t e r p o l a t o

32 end

33 end

111


Concatenate path

1 %% CONCATENATE PATHS

2 % This func t i on w i l l concatenate more paths together . i t i s

used in the

3 % mult iagent environment to avoid mul t ip l e paths ,

concatenat ing

4 % them i can use them as input f o r RRT∗ f u n c t i o n s l i k e i f

they were a

5 % s i n g l e path .

6

7 % The v a r i a b l e number i s the v a r i a b l e that i n d i c a t e how many

paths are you

8 % concatenat ing .

9

10 % Up to now t h i s func t i on can concatenate t i l l 5 paths

together .

11

12 % Simply w i l l c r ea t e a newpath conta in ing the de s i r ed paths

one a f t e r the

13 % other .

14

15 f unc t i on newpath=Concatenate paths ( pathaux , path2 , path3 , path4 ,

path5 , number )

16 path1=pathaux ;

17 j=length ( path1 ) ;

18 i f number>=2


20 path1 ( j+i )=path2 ( i ) ;

21 end

22 end

23 i f number>=3


112


25 path1 ( j+length ( path2 )+i )=path3 ( i ) ;

26 end

27 end

28 i f number>=4


30 path1 ( j+length ( path2 )+length ( path3 )+i )=path4

( i ) ;

31 end

32 end

33 i f number>=5


35 path1 ( j+length ( path2 )+length ( path3 )+length (

path4 )+i )=path5 ( i ) ;

36 end

37 end

38 newpath=path1 ;

39

40 end

113


Miglia metri

1 %% Mig l i a metr i

2

3 % t h i s func t i on w i l l convert s NM in meters

4 f unc t i on i n t=Mig l i a metr i ( m ig l i a )

5 i n t=mig l i a ∗1 . 8 5 2 ;

6 end

Plot path

1 %% Plot path

2

3 %This func t i on w i l l p l o t a g iven path .

4

5 %nodes i s the path to p l o t

6

7 %c o l o r e i s the c o l o r o f the p l o t .

8


10

11 f unc t i on Plot path ( nodes , Range , c o l o r e )

12 %f i g u r e ;

13 x max=Range (1 ) ;

14 y max=Range (2 ) ;

15 a x i s ( [ 0 x max 0 y max ] )

16 f o r j =1: l ength ( nodes )−1

17

18 l i n e ( [ nodes ( j ) . coord (1 ) , nodes ( j +1) . coord (1 ) ] . . .

19 , [ nodes ( j ) . coord (2 ) , nodes ( j +1) . coord (2 ) ] . . .

20 , ’ Color ’ , co lore , ’ LineWidth ’ , 1) ;

21 end

22 end

114


Plot tree

1 %% P l o t t r e e

2 %w i l l p l o t the edges and the nodes o f a t r e e .

3

4 % nodes i s the t r e e to p l o t

5


7

8 % c o l o r e must be wr i t ten between ’ ’ or be a vector [ ] , and

i d e n t i f y the

9 % c o l o r o f the p l o t .

10

11

12 f unc t i on P l o t t r e e ( nodes , Range , c o l o r e )

13 f i g u r e ;

14 x max=Range (1 ) ;

15 y max=Range (2 ) ;

16 a x i s ( [ 0 x max 0 y max ] )

17 f o r j =1: l ength ( nodes )

18 i f ( nodes ( j ) . c o s t ˜=−1)

19 p lo t ( nodes ( j ) . coord (1 ) , nodes ( j ) . coord (2 )

. . .

20 , ’ . ’ , ’ c o l o r ’ , ’ k ’ )

21 end

22 end

23 j=length ( nodes ) ;

24 whi le ( j >1)

25 i f nodes ( j ) . c o s t˜=−1

26 l i n e ( [ nodes ( j ) . coord (1 ) . . .

27 , nodes ( nodes ( j ) . parent ) . coord (1 ) ] . . .

28 , [ nodes ( j ) . coord (2 ) , nodes ( nodes ( j ) .

parent ) . coord (2 ) ] . . .

115


29 , ’ Color ’ , co lore , ’ LineWidth ’ , 1) ;

30 end

31 j=j −1;

32 end

33 end

116


Reverse path

1 %% Reverse path

2

3 %This func t i on w i l l r eve r s e the vector path , sw i t ch ing the

f i r s t node with

4 %the l a s t and so on . This because the f i r s t element o f a path

i s the goal ,

5 %and i s e a s i e r to r eve r s e i t , than performing a f o r c y c l e

from the l a s t to

6 %the f i r s t element o f the path .

7

8 f unc t i on new path=Reverse path ( path )

9 i =1;

10 new path=path ( i ) ;

11 f o r i =2: l ength ( path )

12 new path=[path ( i ) new path ] ;

13 i=i +1;

14 end

15 end

Total cost

1 %% Total co s t

2

3 %This func t i on return the t o t a l co s t o f a path . ( from s t a r t

to goa l )

4

5 f unc t i on cont=T o t a l c o s t ( path )

6 cont=path ( l ength ( path ) ) . co s t ;

7 i f cont==0

8 cont=path (1 ) . co s t ;

9 end

10 end

117


CCW

1 f unc t i on va l = ccw (A,B,C)

2 va l = (C(2)−A(2) ) ∗ (B(1)−A(1) ) > (B(2)−A(2) ) ∗ (C(1)−A

(1) ) ;

3 end

dist

1 f unc t i on d = d i s t ( q1 , q2 )

2 d = sqr t ( ( q1 (1 )−q2 (1 ) ) ˆ2 + ( q1 (2 )−q2 (2 ) ) ˆ2) ;

3 end

118


RRT*

1 % RRT∗ a lgor i thm in 2D with c o l l i s i o n avoidance .

2 %

3 % Author : Sa i Vemprala

4 %

5 % nodes : Contains l i s t o f a l l explored nodes . Each node

conta in s i t s

6 % coord inates , c o s t to reach and i t s parent .

7 %

8 % B r i e f d e s c r i p t i o n o f a lgor i thm :

9 % 1 . Pick a random node q rand .

10 % 2 . Find the c l o s e s t node q near from explored nodes to

branch out from , towards

11 % q rand .

12 % 3 . Steer from q near towards q rand : i n t e r p o l a t e i f node i s

too f a r away , reach

13 % q new . Check that o b s t a c l e i s not h i t .

14 % 4 . Update co s t o f reach ing q new from q near , t r e a t i t as

Cmin . For now ,

15 % q near ac t s as the parent node o f q new .

16 % 5 . From the l i s t o f ’ v i s i t e d ’ nodes , check f o r neare s t

ne ighbors with a

17 % given radius , i n s e r t in a l i s t q nea re s t .

18 % 6 . In a l l members o f q neares t , check i f q new can be

reached from a

19 % d i f f e r e n t parent node with co s t lower than Cmin , and

without c o l l i d i n g

20 % with the o b s t a c l e . S e l e c t the node that r e s u l t s in the

l e a s t co s t and

21 % update the parent o f q new .

22 % 7 . Add q new to node l i s t .

119


23 % 8 . Continue u n t i l maximum number o f nodes i s reached or

goa l i s h i t .

24 c l c

25 c l e a r a l l

26 c l e a r v a r s

27 c l o s e a l l

28

29 x max = 1000 ;

30 y max = 1000 ;

31 o b s t a c l e = [ 5 0 0 , 1 5 0 , 2 0 0 , 2 0 0 ] ;

32 EPS = 20 ;

33 numNodes = 2000 ;

34

35

36 max steer ing=pi /4 ;

37 m a x s t e e r i n g i n c o s =0.4 ;

38 s t a r t a n g l e=pi /2 ;

39 q s t a r t . coord = [400 2 0 0 ] ;

40 q s t a r t . c o s t = 0 ;


42 q goa l . coord = [500 1 0 0 ] ;

43 q goa l . c o s t = 0 ;

44

45 nodes (1 ) = q s t a r t ;

46 f i g u r e (1 )

47 a x i s ( [ 0 x max 0 y max ] )

48 r e c t a n g l e ( ’ Po s i t i on ’ , obs tac l e , ’ FaceColor ’ , [ 0 . 5 . 5 ] )

49 hold on

50 %%



53 p lo t ( q rand (1 ) , q rand (2 ) , ’ x ’ , ’ Color ’ , ’ k ’ )

54

120





58 break

59 end

60 end

61


from

63 nd i s t = [ ] ;


65 n = nodes ( j ) ;



68 end

69 [ val , idx ] = min ( nd i s t ) ;


71


nodes , q near . parent , s t a r t a n g l e , max steer ing ,


73 i f n o C o l l i s i o n ( q new . coord , q near . coord , o b s t a c l e )

74 l i n e ( [ q near . coord (1 ) , q new . coord (1 ) ] , [ q near . coord

(2 ) , q new . coord (2 ) ] , ’ Color ’ , ’ k ’ , ’ LineWidth ’ ,

2) ;

75 drawnow

76 hold on

77 q new . co s t = d i s t ( q new . coord , q near . coord ) + q near

. co s t ;

78


80 q nea re s t = [ ] ;

81 r = 60 ;

121





o b s t a c l e ) && d i s t ( nodes ( j ) . coord , q new . coord )

<= r


coord ;


c o s t ;

87 q nea re s t ( ne ighbor count ) . parent=nodes ( j ) .

parent ;


89 end

90 end

91


93 q min = q near ;


95



97 % cos t paths

98 %%

99


101 [ x1 y1]= Crea vet tore ( q nea re s t ( k ) . coord , q new .

coord ) ;

102 i f q nea r e s t ( k ) . parent==0



105 e l s e

106 [ x2 y2]= Crea vet tore ( nodes ( q nea re s t ( k ) . parent ) .

coord , q nea re s t ( k ) . coord ) ;

122


107 c o s a l f a =(dot ( [ x1 y1 ] , [ x2 y2 ] ) ) /(norm ( [ x1 y1 ] ) ∗(

norm ( [ x2 y2 ] ) ) ) ;

108 end


o b s t a c l e ) && q neare s t ( k ) . co s t + d i s t (

q nea re s t ( k ) . coord , q new . coord ) < C min &&

abs (1− c o s a l f a )<m a x s t e e r i n g i n c o s


111 C min = q neare s t ( k ) . co s t + d i s t ( q nea re s t ( k )

. coord , q new . coord ) ;

112 l i n e ( [ q min . coord (1 ) , q new . coord (1 ) ] , [ q min

. coord (2 ) , q new . coord (2 ) ] , ’ Color ’ , ’ b ’ ) ;

113 hold on

114 end

115 end

116





121 end

122 end

123



126 end

127 end

128

129 D = [ ] ;




133 end

123


134



136 [ val , idx ] = min (D) ;





141 k=1; %%modi f ied by paolo ve rbar i



144 path ( k )=q end ; %%modi f ied by paolo ve rbar i

145 k=k+1; %% modi f ied by paolo ve rbar i


coord (2 ) , nodes ( s t a r t ) . coord (2 ) ] , ’ Color ’ , ’ r ’ , ’


147 hold on


149 end

150 path ( k )=q s t a r t ; %%modi f ied by paolo ve rbar i

151

152

153 %% NOTES

154 % dimostraz ione che ogn nodo ha come parente un nodo

precedente .

155 % f o r i =1:1 : l ength ( nodes )

156 % i f nodes ( i ) . parent>i

157 % i

158 % e l s e

159 % end

160 % end

161 % s t a r t

124

school of industrial and information engineering · tuata attraverso un metodo randomizzato noto in...

Documents