............................................................................................................ 3 3

................................................................................................... 5 .................................................. ................................................. 5

........................................................................................................ 9 9

.......................................................................................... 9 .................................................. ........................................ 9

1.1.1 Fonte di informazioni......................................................... 10 1.1.1 Source of information ............................................ ............. 10

1.1.2 Creazione della mappa....................................................... 11 1.1.2 Creating the map ............................................ ........... 11

1.1.3 Mantenimento della mappa.................................................12 1.1.3 Maintenance of the map ............................................ ..... 12

.............................................................................. 15 .................................................. ............................ 15

1.2.1 Definizione del problema................................................... 16 1.2.1 Problem definition ............................................ ....... 16

1.2.2 Soluzioni Geometriche...................................................... 17 1.2.2 Geometric Solutions ............................................. ......... 17

1.2.3 Soluzioni basate sul controllo............................................ 26 1.2.3 Solutions based on control ........................................... . 26

................................................................................... 27 .................................................. ................................. 27

...................................................................................................... 29 29

.................................................................................................. 29 .................................................. ................................................ 29

2.1.1 Concetti chiave.................................................................. 30 2.1.1 Key Concepts ............................................. ..................... 30

2.1.2 Funzionamento................................................................... 32 2.1.2 Operation .............................................. ..................... 32

.......................................................................................... 35 .................................................. ........................................ 35

2.2.1 Octomap............................................................................. 36 2.2.1 Octomap .............................................. ............................... 36

2.2.2 OMPL................................................................................ 38 2.2.2 OMPL .............................................. .................................. 38

2.2.3 MoveGroup........................................................................ 40 2.2.3 MoveGroup .............................................. .......................... 40

................................................................................... 43 .................................................. ................................. 43

...................................................................................................... 45 45

3.1 Gazebo & Hector Quadrotor 3.1 Gazebo & Hector quadrotor

.................................................... 45 .................................................. .. 45

............................................................................ 49 .................................................. .......................... 49

.................................................................................................. 53 .................................................. ................................................ 53

................................................................................... 57 .................................................. ................................. 57

...................................................................................................... 59 59

4.1 Integrazione dell'applicazione in ambiente reale 4.1 Integration of the application in real environment

........................................................................................ 61 .................................................. ...................................... 61

4.3 Possibili integrazioni in SHERPA 4.3 Possible additions in SHERPA

.......................................... 63 .......................................... 63

................................................................................... 66 .................................................. ................................. 66

.................................................................................................. 69 .................................................. ................................................ 69

Scopo di questa tesi è quello di realizzare un sistema in grado di risolvere il The purpose of this thesis is to provide a system capable of solving the

problema della “navigazione autonoma di un velivolo automatizzato all'interno problem of the "autonomous navigation of an aircraft in automated

di un ambiente a lui precedentemente sconosciuto, mappato dinamicamente of an environment previously unknown to him, mapped dynamically

mediante l'uso di sensori di profondità” come base di studi per future by the use of depth sensors "as the basis of studies for future

applicazioni all'interno del progetto SHERPA. applications within the SHERPA.

La trattazione si divide in una prima parte teorica in cui vengono descritti i The discussion is divided into a first theoretical part in which are described the

principali algoritmi disponibili a livello di ricerca per risolvere i problemi di main algorithms available in research to solve problems

ricostruzione della mappa e navigazione della stessa, seguita da una seconda in reconstruction of the map and navigation of the same, followed by a second in

cui verrà creato, sfruttando il framework Ros, un sistema in grado di ricostruire a which it will be created, using the framework Ros, a system able to reconstruct in

partire da dati provenienti da sensori rgbd la mappa di un luogo e di creare From data from sensors rgbd map of a place and create

traiettorie per un quadrirotore all'interno della stessa. trajectories for a quadcopter within the same. Infine il sistema così creato Finally, the system thus created

sarà testato mediante un simulatore per verificare le prestazioni reali dei vari It will be tested using a simulator to verify the real performance of the various

approcci proposti nella fase teorica. approaches proposed in the theoretical stage.

Se c'è qualcosa che caratterizza l'epoca in cui viviamo è l'ingresso di prepotenza If anything characterizes the era we live in is the input of arrogance

dell'informatica nella nostra vita quotidiana: tutti abbiamo a che fare ogni giorno information technology in our daily lives: we all have to do every day

con oggetti informatici, che sia per il lavoro o per svago viviamo circondati da with computational objects, whether for business or leisure we live surrounded by

computer, smartphone e sistemi di elaborazione e controllo più o meno potenti. computers, smartphones and processing systems and control more or less powerful.

Ma cos'è un computer? So what is a computer? Una macchina formidabile in grado di metterci in A formidable machine able to get in

contatto con persone all'altro capo del mondo, di darci accesso ad una fonte contact with people halfway around the world, to give us access to a source

inesauribile di conoscenza, di elaborare milioni se non miliardi di dati in un inexhaustible knowledge, process millions if not billions of data in a

battito di ciglia... Ma molto limitata per sua stessa natura. blink of an eye ... but very limited by its very nature.

Come ci dice il nome un computer non è altro che un sofisticatissimo sistema in As the name tells a computer is nothing more than a sophisticated system

grado di computare: fare dei calcoli per raggiungere un risultato seguendo una able to compute: make calculations to achieve a result following a

serie di istruzioni impartite. series of instructions. E' solo grazie all'abilità e alla genialità di chi li And 'only thanks to the skill and ingenuity of those who

programma se riusciamo a sfruttarli per fare cose che fino a pochi anni fa program if we can use them to do things which until a few years ago

sembravano fantascienza. They seemed science fiction.

Questa fondamentale limitatezza ne complica l'utilizzo quando si cerca di This fundamental limitation harder to use when trying to

emulare il comportamento di un essere umano: sono ancora tante le attività che emulate the behavior of a human being: there are still many activities

per un uomo sono istintive, mentre per una macchina sono impossibili o for a man they are instinctive, while for a machine are impossible or

realizzabili solo con costi computazionali altissimi. achievable only with high computational costs. Pensiamo ad esempio a tutti i Take, for example to all

casi in cui nella vita di tutti i giorni ci affidiamo all'istinto, all'intuizione, alle Where in the life of every day we rely on instinct, intuition, to

impressioni o ai nostri sensi per prendere decisioni in maniera rapidissima, tutte impressions on our senses to make decisions in a rapid, all

operazioni che un programma non è ancora stato in grado di emulare alla operations that a program has not yet been able to emulate the

Per capire quanto effettivamente un computer sia limitato nelle sue capacità To understand how a computer actually is limited in its capabilities

prendiamo come esempio il problema di muoversi verso una destinazione in una take as an example the issue of moving to a destination in a

stanza prima sconosciuta. room before unknown. Un uomo o un bambino non hanno neppure bisogno di A man or a child does not even need to

pensare per eseguire questo compito, è puramente istintivo, per un robot think to perform this task, it is purely instinctive, for a robot

automatizzato invece rappresenta un problema non banale. automated instead is a nontrivial problem. Quello che per noi è What for us is

un non-problema per la macchina può rappresentare un ostacolo difficile da a non-issue for the car can be a difficult obstacle to

sormontare per le diverse capacità a sua disposizione. overcome for the different capacity at his disposal.

Se pensiamo a tutto quello che inconsciamente il nostro cervello fa per condurci If we think about all that our brain is unconsciously to lead

ad una meta ci possiamo rendere conto del perché: prende coscienza del mondo a goal we can realize why: become aware of the world

che lo circonda fondendo informazioni provenienti da cinque fonti diverse, i surrounding it by merging information from five different sources, the

cinque sensi, si posiziona all'interno dell'ambiente osservato anche in five senses, is positioned within the environment observed in

movimento, si ricava a colpo d'occhio la strada più veloce, diretta e priva di movement, is obtained at a glance the road faster, direct and free of

ostacoli per raggiungere la metà e fa tutto ciò dinamicamente mentre comanda i obstacles to reach the middle and does everything while dynamically controls the

muscoli del corpo per muoversi, adattando le proprie decisioni ad eventuali body muscles to move, adapting their decisions on any

mutazioni dell'ambiente che lo circonda. mutations of the surrounding environment.

Realizzare tutto ciò in una macchina automatizzata è un operazione non semplice Achieve this in an automated machine is a task not easy

che coinvolge l'utilizzo obbligato di sensori e pesanti sforzi teorici, algoritmici e which involves the use of forced sensors and heavy theoretical efforts, and algorithmic

computazionali. computational. Per questo è stato possibile solo negli ultimi anni To this it has been possible only in recent years

commercializzare i primi semplici robot in grado di guidarsi da soli in ambienti commercialize the first simple robot that can drive themselves alone environments

sconosciuti evitando ostacoli e solo negli ultimissimi tempi si è iniziato a parlare unknown avoiding obstacles and only in very recent times has begun to talk

di veicoli automatizzati per il trasporto di persone. of automated vehicles for the transport of persons.

Scopo di questa tesi sarà quello di realizzare un sistema in grado di far navigare The purpose of this thesis is to provide a system able to navigate

un velivolo autonomo all'interno di un ambiente 3D in precedenza ricostruito an airplane autonomous within a 3D environment previously reconstructed

mediante dati provenienti da sensori rgbd montati a bordo. using data from sensors mounted on board rgbd.

La trattazione dal punto di vista teorico si concentrerà principalmente sullo stato The discussion from the theoretical point of view will focus primarily on the state

dell'arte degli algoritmi per la ricostruzione tridimensionale di un ambiente e la of the art algorithms for the reconstruction of a three-dimensional environment and

successiva navigazione all'interno di esso. next navigation within it. Dal punto di vista pratico, invece, From the practical point of view, instead,

scopo finale sarà la realizzazione di un sistema in grado di far volare un ultimate goal will be the realization of a system able to fly a

quadrirotore all'interno di un ambiente ricco di ostacoli di cui ha quadcopter in an environment full of obstacles that have

precedentemente ricostruito una mappa con lo scopo di confrontare all'interno di previously reconstructed a map with the purpose of comparing within

un simulatore le diverse soluzioni algoritmiche al problema presentate nella parte a simulator different algorithmic solutions to the problem presented in the

teorica. theoretical. Scopo della tesi non è quindi quello di produrre nuove soluzioni, ma di The purpose of the thesis is therefore not to produce new solutions, but of

realizzare un sistema che implementi il meglio di quelle disponibili come provide a system that implements the highlights of the ones available as

software open-source, in modo da poter servire come base di partenza e banco di open-source software, so it can serve as a starting point and bench

test per futuri algoritmi. test for future algorithms.

Questa applicazione servirà inoltre come base di studio per future applicazioni This application will also serve as the basis of study for future applications

all'interno del progetto SHERPA (Smart collaboration between Humans and within the SHERPA (Smart collaboration between Humans and

ground-aErial Robots for imProving rescuing activities in Alpine environments) ground-Aerial Robots for Improving rescuing activities in Alpine environments)

volto allo sviluppo di una piattaforma robotizzata eterogenea per il supporto aimed at the development of a robotic platform for heterogeneous support

all'attività di salvataggio umano in ambienti ostili. Human activity rescue in hostile environments.

Nel primo capitolo verranno descritte le basi teoriche ed i vari algoritmi che In the first chapter it describes the theoretical basis and the various algorithms

riguardano la ricostruzione di mappe tridimensionali di ambienti e la successiva concern the reconstruction of three-dimensional maps of environments and the subsequent

creazione di percorsi privi di ostacoli all'interno degli stessi. creating paths free of obstacles inside of the same.

Nel secondo verranno descritte le componenti software che forniscono The second will describe the software components that provide

un'implementazione reale di questi sfruttando il sistema “ROS” come base. a real implementation of these leveraging system "ROS" as a base.

Nel terzo si fornirà una descrizione del sistema creato sfruttando queste The third will provide a description of the system created using these

componenti e se ne darà una dimostrazione pratica facendo volare components and they give you a practical demonstration by flying

autonomamente un modello di quadrirotore all'interno del simulatore “Gazebo”. independently a model quadcopter within the simulator "Gazebo".

Nel quarto infine si parlerà di come questo sistema potrà essere integrato per In the fourth Finally we discuss how this system can be integrated

funzionare in situazioni reali ed in particolare all'interno del progetto SHERPA. work in real situations and in particular in the SHERPA.

In questo capitolo verranno descritte le basi teoriche e lo stato dell'arte degli This chapter describes the theoretical basis and the state of the art

algoritmi necessari per risolvere due dei problemi fondamentali algorithms necessary to solve two fundamental problems

nell'automatizzazione di un robot: creare una mappa dell'ambiente in cui si trova in automation of a robot: create a map of the environment in which is located

( “mapping” ) e definire all'interno di esso un percorso privo di ostacoli per ("Mapping") and defining within it an obstacle-free route for

raggiungere una destinazione ( “path planning” ). reach a destination ("path planning").

Molte applicazioni di robotica richiedono una mappa tridimensionale Many robotics applications require a three-dimensional map

dall'ambiente che le circonda creata partendo dai dati registrati con appositi the environment that surrounds them created from data recorded with special

sensori. sensors. Creare e mantenere questa mole di dati in maniera efficiente non è Create and maintain this amount of data in an efficient manner is not

un'operazione banale, un buon metodo di gestione della mappa dovrebbe a trivial operation, a good management method should map

• Possibilità di modellare spazio libero, occupato e non ancora esplorato: • Ability to model space, occupied and not yet explored:

ad esempio per applicazioni di esplorazione è fondamentale differenziare for example, for applications of exploration it is imperative to differentiate

le aree non esplorate dalle altre. areas not explored by others.

• Rapidità e semplicità di creazione e aggiornamento: a seconda della • Quick and easy creation and modification: depending on

tecnica di memorizzazione scelta potrebbe essere più o meno complesso storage technique choice could be more or less complex

elaborare i dati in ingresso, spesso è infatti necessario discretizzarli in process the input data, often it is in fact necessary in discretizzarli

• Rappresentazione probabilistica delle informazioni: tipicamente i dati per • Probabilistic information: typically the data for

creare la mappa provengono da sensori 3D di svariati generi accomunati create map come from 3D sensors of various kinds in common

dalla presenza più o meno importante di rumori di misura e quindi di falsi by the presence of more or less important measurement noise, and then false-

dati. data. Un approccio probabilistico alla gestione della mappa permette di A probabilistic approach to the management of the map allows you to

fondere molte misure incerte per ottenerne una certa, in altre parole un merge many measures uncertain to obtain a certain, in other words a

punto è indicato come occupato o libero in base ad una stima point is indicated as busy or free according to an estimate

probabilistica sulle ultime misurazioni effettuate. Probabilistic on the latest measurements.

• Efficienza: sia in termini di tempi di accesso per ottenere informazioni su • Efficiency: both in terms of access time to obtain information on

di un singolo punto, sia in termini di memoria occupata per rappresentarla. of a single point, both in terms of memory occupied to represent it.

• Multi-resolution: potrebbe risultare comodo avere una mappa in grado di • Multi-resolution: it can be handy to have a map that can

visualizzare lo spazio a più risoluzioni in base alle esigenze del momento. display space for different resolutions according to the needs of the moment.

1.1.1 Fonte di informazioni 1.1.1 Source of information

I dati in ingresso possono provenire da svariate fonti, ma nell'ambito di questa The input data can come from a variety of sources, but in the context of this

tesi si sono presi in considerazione solamente sensori in grado di generare una thesis are taken into consideration only sensors able to generate a

nuvola di punti, o pointcloud , 3D. point cloud, or PointCloud 3D. Queste rappresentano la porzione di mondo These represent the portion of the world

osservato mediante una matrice di punti ciascuno caratterizzato da un valore observed by means of a matrix of dots each characterized by a value

numerico che ne indica la distanza dal sensore; number that indicates the distance from the sensor; la posizione nello spazio è data the position in space is given

quindi da questo valore e dalla posizione all'interno della matrice. then from this value and from the position within the matrix. Un'evoluzione Evolution

di questo concetto sono le immagini RGBD : ad ogni pixel sono associati quattro of this concept are the images RGBD: to each pixel are associated four

valori numerici, tre per i colori (rgb) e uno ( d ) per la distanza del punto rispetto numeric values, three colors (RGB) and one (d) to the distance of the point from

all'osservatore, nel nostro caso la distanza dal fuoco della camera. to the observer, in our case the distance from the focus of the room.

In entrambi i casi quello che otteniamo è una rappresentazione dell'oggetto In both cases, what we get is a representation of the object

osservato mediante migliaia di punti posizionati nello spazio, nel caso rgbd ad observed by means of thousands of points positioned in space, in the case rgbd to

ogni punto è anche associato un colore. each point is also associated with a color. Queste rappresentazioni sono considerate These representations are considered

lo standard per applicazioni di ricostruzione di ambienti 3D poiché permettono di the standard for reconstruction applications of 3D environments because they allow

unire in maniera semplice e computazionalmente efficiente informazioni join in a simple and computationally efficient information

riguardanti l'aspetto della scena osservata e la sua conformazione concerning the appearance of the observed scene and its conformation

E' possibile ottenere una pointcloud da fonti molto diverse, ad esempio proiettori You 'can get a PointCloud from very different sources, such as projectors

laser, telecamere stereo o sensori rgbd come Microsoft kinect o Asus prime. Gli laser, stereo cameras or sensors rgbd as Microsoft Kinect or Asus first. The

ultimi rappresentano l'ultima soluzione arrivata sul mercato ma anche quella che last represent the ultimate solution came on the market but also one that

si sta espandendo più velocemente in quanto coniugano alta risoluzione e It is expanding as fast as they combine high resolution and

precisione nella creazione della pointcloud ad un basso costo ed ingombro, precision in the creation of PointCloud at a low cost and dimensions,

limitando però la capacità di ricostruzione dell'ambiente a distanze piccole, restricting the ability of reconstruction of the environment at small distances,

nell'ordine di 5-6 m dalla camera. in the order of 5-6 m from the room.

La risoluzione dei dati in ingresso è spesso esagerata rispetto alle esigenze The resolution of the input data is often exaggerated compared to the needs

tipiche di un sistema robotizzato, è quindi prassi comune applicare una typical of a robotic system, it is therefore common practice to apply a

discretizzazione alla pointcloud mediante l'uso di voxel : cubi di dimensione fissa discretization to the PointCloud by the use of voxels: cubes of fixed size

che associano ad un singolo punto, il loro centro, e ad un valore numerico la combining in a single step, their center, and a numeric value the

rappresentazione della porzione di spazio che racchiudono. representation of the portion of space that is enclosed. Intuitivamente la Intuitively

nuvola di punti in ingresso viene suddivisa in volumi equivalenti , a ciascuno di cloud of input points is divided into equivalent volumes, to each of

essi viene associato un centro e un valore (occupato o libero) in base alla metrica they are associated with a center and a value (busy or idle) based on the metric

scelta per filtrare i punti in ingresso. choice to filter the input points.

1.1.2 Creazione della mappa 1.1.2 Creating the map

I dati provenienti dai sensori sono una sequenza di pointcloud legate fra di loro The data from the sensors are a sequence of PointCloud connected to them

solo da un ordine temporale, sono solamente un video tridimensionale di ciò che only by a temporal order, they are only a three-dimensional video of what

il sensore ha osservato, non creano un modello tridimensionale dell'oggetto the sensor observed, do not create a three-dimensional object

osservato senza ulteriori elaborazioni. observed without further processing.

Questo perché alla pointcloud manca un'informazione fondamentale: dove si This is because the missing information PointCloud fundamental: where

trovava il sensore all'interno dello spazio mentre acquisiva quell'immagine. was the sensor within the space while that image acquired. La There

conoscenza tridimensionale dell'ambiente ottenuta si limita alla posizione degli knowledge obtained three-dimensional environment is limited to the location of

oggetti osservati rispetto al sistema di riferimento che ha come origine il sensore. observed objects with respect to the reference system that has as its origin the sensor.

Non avendo nessuna informazione assoluta rispetto ad un sistema di riferimento Having absolutely no information with respect to a reference system

fisso, non siamo in grado di determinare la posizione relativa delle varie fixed, we are not able to determine the relative position of the various

pointcloud per poterle quindi posizionare nello spazio ed ottenere così una PointCloud for them then place in space and thus to obtain a

E' quindi necessario un sistema che tenga traccia degli spostamenti del sensore in E 'therefore need a system that keeps track of the movements of the sensor in

modo da poter posizionare ciascuna nuvola nel luogo in cui è stata acquisita, so you can position each cloud in the place where it was acquired,

ovvero un sistema che calcoli l' odometria del sensore: come si è mosso nello or a system that calculates the 'odometry sensor: as he moved in

spazio. space. Per far ciò tipicamente vengono unite informazioni provenienti da sensori To do this typically are merged information from sensors

indipendenti per garantire la maggior precisione possibile, ad esempio IMU, GPS independent to ensure the greatest possible precision, for example, IMU, GPS

o altri. or others. Particolarmente interessanti sono le applicazioni basate unicamente su Particularly interesting are the applications based solely on

dati provenienti da sensori video, dette visual odometry , che tenendo traccia di Data from video sensors, called visual odometry, that keeping track of

punti caratteristici, features, in immagini successive permettono di ricostruire lo landmarks, features in successive images allow us to reconstruct the

spostamento del sensore senza bisogno di altri fonti di informazioni. displacement of the sensor without the need for other sources of information. L'uso della The use of

sola odometria visuale per costruire mappe tridimensionali dei luoghi osservati only visual odometry to build three-dimensional maps of the places observed

prende il nome di VSLAM (visual simultaneous localization and mapping). It called VSLAM (visual simultaneous localization and mapping).

Questa sembrerebbe la soluzione ottimale sfruttando il solo stream di immagini This would seem to the optimal solution using only the stream of images

rgbd per odometria e mapping , ma richiede una notevole potenza rgbd for odometry and mapping, but requires considerable power

computazionale, non è applicabile con semplici pointcloud (necessita anche delle computational, is not applicable with simple PointCloud (needs both

informazioni sui colori) e non funziona in ambienti dotati di poche features . color information) and does not work in environments with few features.

Sapendo come il sensore si è mosso nello spazio è possibile posizionare Knowing how the sensor has moved in space you can place

correttamente le pointcloud in un sistema di riferimento assoluto e ottenere così correctly PointCloud in an absolute reference system and thus obtain

una rappresentazione consistente del mondo osservato. a consistent representation of the observed world.

1.1.3 Mantenimento della mappa 1.1.3 Maintenance of map

Una volta creata la mappa si pone il problema di come mantenerla in memoria in Once the map is created, there is the problem of how to keep it in memory

modo che sia efficiente da utilizzare e che occupi poche risorse. so that it is efficient to use and that it takes up very little resources.

Nel corso della storia sono state proposte varie soluzioni: Throughout history, various solutions have been proposed:

• Point-Cloud: • Point-Cloud:

Non si applica nessun tipo di trasformazione o Not apply any kind of transformation or

discretizzazione ma si salvano direttamente i discretization but are saved directly

dati che provengono dai sensori esterni: una data coming from external sensors: a

serie di coordinate (x,y,z) che indicano punti set of coordinates (x, y, z) which indicate points

occupati nello spazio. occupied space. Da un punto di vista From a point of view

computazionale è sicuramente il metodo computational method is definitely

migliore: non c'è nessuna trasformazione o best: there is no transformation or

elaborazione da effettuare, i dati sono salvati processing to be performed, the data is saved

così come sono. as they are. Non c'è però nessuna There is no particular

rappresentazione esplicita dello spazio libero o non esplorato e non c'è explicit representation of free space or not explored and there is

modo di gestire il rumore inevitabilmente presente sui dati non filtrati. how to handle the noise inevitably present on unfiltered data. La There

quantità di memoria necessaria a salvare i dati inoltre è eccessiva: i amount of memory needed to save the data also is excessive: the

moderni sensori rgbd sono in grado di generare nuvole dell'ordine dei rgbd modern sensors are capable of generating clouds of the order of

10000 punti per frame lavorando a frequenze di 30 Hz; 10000 points per frame working at frequencies of 30 Hz; è perciò evidente It is therefore clear

che memorizzare direttamente questi dati senza nessun tipo di that store this data directly without any

discretizzazione non può essere efficiente né per la gestione della discretization can not be efficient nor for the Management of

1.1 - Rappresentazione di 1.1 - Representation

memoria, né in termini di tempo di accesso ai dati e navigazione della memory, either in terms of access time to data and navigation of

mappa: non c'è nessun tipo di gerarchizzazione. map: there is no type of hierarchy.

• Voxel Grid [1]: • Voxel Grid [1]:

Viene applicata una discretizzazione dello spazio mediante l'utilizzo di It is applied to a discretization of the space through the use of

voxel. L'ambiente è rappresentato mediante una griglia 3D di cubi, voxel. The environment is represented by a 3D grid of cubes,

ciascuno rappresentante tutto lo spazio al suo interno mediante un singolo each representing the entire space in its interior by means of a single

valore: (occupato/libero/non esplorato). value: (busy / idle / not explored). Migliora l'efficienza nel consumo Improves efficiency in consumption

di memoria ma non la velocità di interrogazione, non c'è ancora una memory but not the speed of interrogation, there is still a

struttura gerarchica ben definita che lega i blocchi tra loro. well-defined hierarchical structure that links the blocks with one another. La dimensione The dimension

della griglia inoltre deve essere nota a priori per poter suddividere i punti the grid must also be known a priori to be able to divide the points

• Elevation Map [2]: • Elevation Map [2]:

Si sfrutta una griglia 2D per modellare lo It uses a 2D grid to model the

spazio, per ogni cella della griglia si salva space, for each cell of the grid is saved

l'elevazione massima raggiunta dagli ostacoli the highest elevation reached by obstacles

al suo interno; inside; spesso viene indicata anche often it is also shown

come “griglia 2,5D”. as "2.5D grid". Questa rappresentazione This representation

è considerata ottimale in termini di efficienza It is considered to be optimal in terms of efficiency

di memorizzazione e di interrogazione, ma ha storage and interrogation, but has

il grande difetto di non dare una the great disadvantage of not giving a

rappresentazione volumetrica dello spazio ma solo un discretizzazione volumetric representation of space but only a discretization

relativa ad un piano. for a plan. E' la rappresentazione ideale per tutti i robot che E 'the ideal representation for all the robots that

hanno la necessità di navigare su un piano. They need to navigate on one floor.

1.2 - Rappresentazione di 1.2 - Representation

• Multi Level Elevation Grid [3]: • Multi Level Elevation Grid [3]:

Si sfrutta una griglia 2D, per ogni cella It exploits a 2D grid, for each cell

vengono salvati tutti i voxel occupati che si They save all voxels that are occupied

trovano in quella porzione di spazio. located in that portion of space. C'è una There is a

rappresentazione volumetrica, ma non è volume rendering, but it is not

possibile distinguere tra spazio libero e spazio You can distinguish between free space and space

non esplorato (anche se sarebbe possibile not explored (although it would be possible

ovviare a questo salvando anche la lista di obviate this also saving the list of

voxel liberi per ogni cella). voxel free for each cell). L'aggiornamento The update

della mappa è ancora computazionalmente pesante perché non c'è nessun the map is still computationally heavy because there is no

tipo di gerarchia esplicita tra i voxel , inoltre non è ancora possibile type of explicit hierarchy between voxels, also not yet possible

ottenere risoluzione variabile della mappa. get variable resolution of the map.

• Octomap [4] [5]: • Octomap [4] [5]:

Lo spazio è rappresentato The space is represented

mediante un albero ottale: by a shaft octal:

tutto l'ambiente osservabile the whole environment observable

viene suddiviso in otto It is divided in eight

volumi, tipicamente cubi di volumes, typically cubes

ugual dimensione, a sua same size, at its

volta ciascun volume può essere ri- After each volume can be re-

suddiviso ricorsivamente in altri otto più recursively divided into eight more more

Sfruttando questa tecnica si può procedere a Using this technique you can proceed to

suddividere lo spazio fino alla precisione dividing the space up to the precision

voluta. desired. Ad ogni passaggio di ricorsione si At each step of recursion

creano nella zona considerata dei voxel più create that zone of the voxel

piccoli per rappresentare lo spazio. small to represent space. La There

ricorsione si ferma quando la dimensione recursion stops when the size

dei volumi raggiunge la misura minima stabilita a priori. volume reaches the minimum amount fixed at the outset.

1.3 - Rappresentazione di 1.3 - Representation

un albero mediante multi a tree by means of multi

level elevation grid level elevation grid

1.4 - Decomposizione ricorsiva di un cubo 1.4 - recursive decomposition of a cube

sfruttando larappresentazione ad albero ottale exploiting larappresentazione tree octal

1.5 - Rappresentazione di 1.5 - Representation

Il mondo così discretizzato può essere rappresentato mediante una So the world can be represented by a discretized

struttura ad albero dove ogni nodo ha otto figli e rappresenta una porzione tree where each node has eight children and is a portion

di spazio con una ben determinata risoluzione, data dalla dimensione del of space with a well determined resolution, given the size of the

lato. side. L'approccio è ottimale in quanto permette di lasciare parti di mappa a The approach is optimal since it allows to leave parts of the map in

diverse risoluzioni o addirittura non inizializzate senza intaccare la qualità different resolutions or even uninitialized without affecting the quality

complessiva della rappresentazione. overall representation. La struttura ad albero permette inoltre The tree also allows

di minimizzare il numero di informazioni da salvare per ogni nodo (la to minimize the amount of information saved for each node (the

posizione nello spazio è ricavabile a partire dal nodo radice), di position is obtainable from the root node), of

velocizzare l'interrogazione della mappa e di salvare informazioni nel caso speed querying of the map and save information in case

in cui quel particolare volume sia libero, occupato o inesplorato. that particular volume is free, busy or unexplored. Inoltre a In addition to

seconda della profondità a cui tagliamo l'albero è anche possibile ottenere Depending on the depth to which we cut the tree you can also get

mappe a risoluzioni differenti. maps at different resolutions.

Quest'ultima soluzione ha quindi tutte le caratteristiche per essere considerata la This latter solution therefore has all the characteristics to be considered the

soluzione ottimale per le esigenze di questa tesi e per tutti i campi in cui è optimal solution for the needs of this thesis and for all the fields in which it is

necessario creare una rappresentazione tridimensionale di un ambiente con un need to create a three-dimensional representation of an environment with a

consumo relativamente contenuto di risorse. a relatively low consumption of resources.

1.2 Path Planning 1.2 Path Planning

Se il sistema automatizzato dispone di una mappa dell'ambiente che lo circonda If the automated system has a map of the surrounding environment

può sfruttarla per pianificare il percorso migliore e privo di ostacoli per It can use it to plan the best route and free of obstacles to

raggiungere una meta o una configurazione specificata detta goal . Questa azione reach a goal or a configuration specified that goal. This action

prende il nome di path planning ed è oggetto di ricerca scientifica con ottimi It takes the name of path planning and is the subject of scientific research with excellent

risultati dagli anni '70 in quanto basilare per qualsiasi applicazione reale di results from the 70s as a basis for any real application of

Per poter sfruttare la mappa per generare un percorso il sistema deve To take advantage of the map to generate a path the system must

necessariamente conoscere altre due informazioni: una descrizione geometrica necessarily know two information: a geometric description

dello spazio occupato dal robot, in modo da poter verificare correttamente the space occupied by the robot, in order to complete verification

eventuali collisioni, e la posizione di partenza all'interno della mappa. collisions, and the starting position within the map. Nel caso In case

in cui sia il robot stesso a creare la mappa la seconda informazione è già ottenuta in which both the robot itself to create the second map information is already obtained

implicitamente, poiché è necessario che il robot sappia dove si trova per poter implicitly, since it is necessary that the robot knows where to

ricostruire la mappa di un luogo. reconstruct the map of a place.

Le traiettorie generate dagli algoritmi di planning non necessariamente tengono The trajectories generated by the algorithms of planning does not necessarily hold

conto dei limiti cinematici del robot e degli attuatori, molto spesso sono soluzioni account of the kinematic limits of the robot and actuators, are very often solutions

puramente geometriche, per questo di solito alla fase di determinazione della purely geometrical, for this is usually the step of determining the

traiettoria segue una di raffinamento che ha lo scopo di produrre la sequenza di trajectory follows a refinement which has the purpose of producing the sequence of

comandi necessari per condurre il sistema alla configurazione di goal; commands necessary to conduct the system configuration of goals; i n questo i n this

caso si parla di path planning geometrico . Esistono metodi che tengono già It is called a geometric path planning. There are methods that already take

conto delle dinamiche del robot e producono una serie di comandi già attuabili account the dynamics of the robot and producing a series of commands already feasible

per raggiungere il goal , si parla in questo caso di path planning orientato al to reach the goal, we speak in this case of path planning oriented

1.2.1 Definizione del problema [6] 1.2.1 Definition of the problem [6]

Da un punto di vista matematico il robot che deve essere pilotato e gli ostacoli From a mathematical point of view that the robot is to be controlled and the obstacles

che deve evitare possono essere schematizzati come figure geometriche: poligoni It must avoid can be summarized as geometric figures: polygons

se il robot si può muovere solo in un ambiente 2D o vincolato ad un piano, if the robot can only move in a 2D or constrained to a plane,

poliedri altrimenti. polyhedra otherwise. Data una mappa si definisce W l'insieme di tutti i punti che la Given a map defining W the set of all points that the

compongono, è quindi possibile definire due sottoinsiemi: O la regione in cui make up, you can then define two subsets: Or the region where

sono presenti ostacoli e il suo complementare F la regione libera di spazio. There are obstacles and its complement F region free space.

Parallelamente si definisce anche l'insieme C composto da tutte le In parallel we also defines the set C consists of all the

configurazioni, o posizioni, in cui si può trovare il robot all'interno dell'ambiente configurations, or positions, in which one can find the robot within the environment

e Cfree il suo sottoinsieme contenente tutte le configurazioni appartenenti a F. Cfree and its subset containing all configurations belonging to F.

Date una configurazione iniziale del robot ( start) e una finale ( goal) appartenenti Give an initial configuration of the robot (start) and an end (goal) belonging

a Cfree calcolare un percorso tra di esse equivale a cercare la sequenza di Cfree to calculate a route between them is equivalent to the search sequence

elementi appartenenti a Cfree che permettono di passare dalla partenza all'arrivo. elements belonging to Cfree that allow you to switch from start to finish.

1.2.2 Soluzioni Geometriche 1.2.2 Geometric Solutions

Questa serie di soluzioni è interessata ad ottenere solamente la traiettoria per This series of solutions is only interested in obtaining the trajectory for

passare da start a goal , senza nessuna informazione sui comandi da impartire al go to start in goal, without any information on the commands that shall be given to

robot, si parla quindi di path planning geometrico. Per poi muovere il robot sarà robot, is referred to as path planning geometric. To then move the robot will

necessario un successivo raffinamento della traiettoria e una conversione in necessary a subsequent refinement of the trajectory and a conversion into

comandi effettivi da impartirgli. D urante gli anni molte soluzioni sono state actual commands to impart. D uring the years many solutions have been

proposte a questo problema, per la maggior parte basate sulla creazione di grafi o proposed to this problem, for the most part based on the creation of graphs or

alberi che descrivano, schematizzandolo, il mondo in cui il robot si trova a trees that describe, schematizzandolo, the world in which the robot is

Una notevole eccezione a questo approccio è l'utilizzo di potential fields [7]: il A notable exception to this approach is the use of potential fields [7]: the

robot viene schematizzato come un punto soggetto ad un campo di forza, il goal robot is modeled as a point subject to a force field, the goal

genera forza attrattiva, mentre gli ostacoli forze repulsive. It generates attractive force, while obstacles repulsive forces. In questo modo per In this way for

ogni configurazione del robot è possibile calcolare il vettore forza che agisce su each configuration of the robot it is possible to compute the vector force acting on

esso, ovvero lo sforzo necessario ad avvicinarlo alla meta. it, that the effort needed to bring it closer to the goal. In questo modo non In this way not

solo ho determinato una traiettoria priva di ostacoli, ma per ogni punto della I have only given a path free of obstacles, but for every point

stessa è possibile ricavare il vettore forza da applicare al robot per condurlo alla you can get the same vector force applied to the robot to lead to

Sembra quindi una soluzione ideale: semplice da realizzare e in grado di It therefore seems an ideal solution: simple to implement and able to

determinare sia traiettoria che comandi per raggiungere il goal. Ha però un grave determining both trajectory commands to reach the goal. He, however, a serious

difetto: i “ punti di minimo ”, punti in cui la somma delle forze prodotte è 0, se il defect: the "minimum points", points in which the sum of the forces produced is 0, if the

robot si trova in uno di quei punti non è più in grado di uscirne e rimane quindi robot is in one of those points is no longer able to get out of it and then remains

bloccato. blocked. Una semplice soluzione potrebbe essere quella di immettere A simple solution would be to enter

casualmente del rumore nei comandi in modo che sia impossibile che il robot random noise in the command so that it is impossible that the robot

rimanga bloccato per un tempo indefinito in un punto. be blocked for an indefinite time at a point. Altro problema è che per Another problem is that for

ricavare la sola traiettoria è necessario integrare il vettore forza, operazione derive the single trajectory is necessary to integrate the force vector, operation

computazionalmente costosa. computationally expensive. Spesso risulterebbe utile poter separare la Often be useful to separate the

traiettoria dalla sua attuazione. trajectory from its implementation.

Per questo nascono tutti gli algoritmi basati sull' uso di grafi in cui il problema For this arise all algorithms based on 'use of graphs in which the problem

viene risolto in due passaggi, prima si crea un grafo rappresentante una is solved in two steps, first creating a graph representing a

discretizzazione di tutte le configurazioni possibili nello spazio in cui il robot si discretization of all possible configurations in the space in which the robot

muove e si connettono start e goal alla struttura generata che prende quindi il move and connect the start and goal to the structure generated which then takes the

nome di roadmap . name roadmap. A questo punto il problema di planning è stato semplificato At this point the problem of planning has been simplified

notevolmente, diventando un problema di routing su un grafo già ampiamente greatly, becoming a routing problem on a graph already widely

studiato e ottimamente risolto. well studied and solved.

Gli algoritmi di generazione di grafi sono vari: The algorithms of generation of graphs are various:

Scomposizione in celle dell'intera mappa: Break it down into cells of the entire map:

tutta la mappa viene It is all over the map

suddivisa in celle, poligonali nel caso la mappa sia in due dimensioni, divided into cells, polygonal in case the map is in two dimensions,

poliedriche altrimenti e si crea un grafo rappresentante lo spazio libero otherwise multifaceted and creating a graph representing the free space

considerando come nodi i centri delle celle libere e connettendo tra di loro Whereas nodes as the centers of the free cells and connecting between them

solo quelle adiacenti. only adjacent ones. A questo punto rimane solo da determinare in quale Now it only remains to determine how

celle si trovano start e goal per ottenere una roadmap consistente. cells are start and goal to achieve a consistent roadmap. Questo This

metodo permette di raggiungere in tempi finiti la soluzione al problema, method allows you to reach in a finite time the solution to the problem,

se questa esiste, o di indicare con sicurezza che non può esistere; if one exists, or to state with confidence that it can not exist; si dice they say

quindi completo . then complete. Necessita però di un'analisi esaustiva di tutta la mappa, However, it requires a comprehensive analysis of the whole map,

per questo queste soluzioni vengono dette combinatorie . for this these solutions are said combinatorial.

Esistono molti algoritmi per la suddivisione in celle della mappa: There are many algorithms for the division into cells of the map:

1. Celle omogenee: 1. Cells homogeneous:

Tutta la mappa viene suddivisa in celle di ugual dimensione, la The whole map is divided into cells of equal size,

risoluzione della mappa è unica e stabilita a priori, così come le resolution of the map is unique and established a priori, as well as

dimensioni del grafo risultante. size of the resulting graph.

2. Celle poligonali [6]: 2. Cells polygonal [6]:

Gli ostacoli sono schematizzati The obstacles are summarized

come poligoni o poliedri, da ogni as polygons or polyhedrons, from any

vertice si tracciano linee rette Summit will draw straight lines

parallele ad uno degli assi, a due parallel to one axis, two

se in 3D, fino a quando non if 3D, until

intercettano un altro ostacolo o il intercept another obstacle or

1.6 - Decomposizione in celle 1.6 - decomposition in cells

poligonali e grafo ottenuto polygonal and graph obtained

bordo della mappa. edge of the map. Come nodi vengono presi un punto interno ad As nodes are taken to an interior point

ogni cella e uno nelle rette, il punto interno sarà collegato tramite each cell and each other's lines, the interior point is connected via

rami a quelli che stanno nelle linee che delimitano la sua cella. branches to those who sit in lines defining his cell. Così So

facendo si ottiene una suddivisione ottimale in celle della mappa che making you get optimal split in the cells of the map which

permette di minimizzare le dimensioni del grafo, ma con costi allows to minimize the size of the graph, but with costs

computazionali molto alti in fase di creazione. very high computational being created.

3. QuadTree/Octree [8]: 3. quadtree / octree [8]:

ricorsivamente in quadrati, se 2D, recursively into squares, whether 2D,

o in cubi, se 3D, in modo che or in blocks, if 3D, so that

rispettivamente quaternario o respectively or quaternary

ottale, la ricorsione viene bloccata octal, recursion is blocked

desiderata per quella particolare desired for that particular

area. area. In questo modo la dimensione delle celle non è fissa, ma si può In this way the size of the cells is not fixed, but can be

adattare dinamicamente alla zona che deve rappresentare, dynamically adapt to the area that has to represent,

intuitivamente vicino agli ostacoli le celle saranno più piccole, intuitively close to obstacles cells will be smaller,

mentre nelle zone libere più grandi. while in the free areas larger.

4. Diagramma di Voronoi [9]: 4. Diagram of Voronoi [9]:

Si suddivide la mappa in aree It divides the map into areas

identificate ciascuna da un proprio each identified by its own

punto caratteristico detto “centro”. characteristic point called "the center".

Un punto per appartenere a quella A point to belong to that

casella deve essere più vicino al Box to be closer to

suo centro che a qualsiasi altro its center than to any other

punto nell'insieme dei centri. point in all the centers.

Ponendo i centri all'interno degli Placing the centers within the

ostacoli, si ottiene una roadmap ottimale considerando come nodi i obstacles, you get a roadmap optimal considering how the nodes

1.8 - Decomposizione in zone di 1.8 - decomposition in areas of

Voronoi di una mappa Voronoi of a map

1.7 - Decomposizione di una 1.7 - decomposition of a

mappa bidimensionale mediante two-dimensional map by means of

vertici delle celle e come rami i bordi. top of the cells and branches as the edges. Ottimale perché per Optimal because

definizione i percorsi da essa ottenuti si trovano alla massima defining the routes that have been obtained are the maximum

distanza da essi e permettono quindi passaggi a distanza di sicurezza away from them and then allow passages from a safe distance

dagli ostacoli. obstacles. La fase di definizione delle celle è però lunga e The definition phase of the cells, however, is long and

5. Visibility Graph [10]: 5. Visibility Graph [10]:

schematizzati come poligoni o summarized as polygons or

considerato un nodo del grafo ed è considered a node of the graph and is

connesso a tutti gli altri vertici connected to all other vertices

“visibili” dalla sua posizione, cioè "Visible" from its position, ie

quelli a cui è connesso da uno those to which it is connected by a

spigolo o per cui è possibile tracciare una linea retta di congiunzione edge or for which it is possible to draw a straight line of conjunction

che non intersechi ostacoli. that does not intersect obstacles. Il risultato è una roadmap che permette di The result is a roadmap that allows

calcolare i percorsi più brevi possibili per aggirare ostacoli, ma calculate the shortest routes possible to get around obstacles, but

difficile da usare perché prevede passaggi radenti ad essi. difficult to use because it provides sliding passages to them.

• Probabilistic roadmap (PRM) [11]: • Probabilistic Roadmap (PRM) [11]:

creazione di grafi rispetto agli creating graphs than

scomposizione in celle, che sono decomposition in cells, which are

efficaci ma richiedono una potenza effective, but require a power

analizzano sempre l'intera mappa, Always analyze the entire map,

anche quando sarebbe sufficiente una even when one would suffice

conoscenza molto più sommaria dell'ambiente. much more cursory knowledge of the environment. Per questo diventano Why become

importanti gli approcci probabilistici alla risoluzione del problema: i important probabilistic approaches to the resolution of the problem:

1.9 - visibility graph ottenuto 1.9 - visibility graph obtained

partendo da una mappa starting from a map

1.10 - Probabilistic roadmap 10.01 - Probabilistic roadmap

ottenuta su una mappa obtained on a map

nodi del grafo non sono più scelti con un criterio specifico, ma sono nodes of the graph are no longer chosen with a specific criterion, but are

selezionati a caso tra le possibili configurazioni del robot appartenenti a randomly selected among the possible configurations of the robots belonging to

Cfree, la parte computazionalmente più complessa diventa creare le Cfree, the part becomes more computationally complex create

connessioni all'interno del grafo. connections within the graph. Ogni volta che viene aggiunto un nodo Each time you add a node

questo viene connesso a tutti quelli contenuti in una palla di raggio n this is connected to all those contained in a ball of radius n

centrata sul nodo, con n definito a priori, e raggiungibili attraverso un centered on the node, with n defined a priori and can be reached through a

percorso retto privo di ostacoli. straight path without obstacles. Un approccio alternativo è quello di An alternative approach is to

congiungerlo ai k nodi più vicini indipendentemente dalla distanza a cui tie it to k nodes closer regardless of distance to which

L'aggiunta di nodi termina quando la roadmap è considerata abbastanza The addition of nodes is completed when the roadmap is considered enough

densa, a questo punto inizia l'utilizzo vero e proprio, ogni volta che voglio thick at this point begins using true, whenever I want

creare un percorso dovrò solamente aggiungere le configurazioni di start e create a path I'll just add configurations and start

goal alla roadmap connettendole ai nodi più vicini e sfruttare il grafo per goals by connecting them to the roadmap nodes closer and exploit the graph for

Questa soluzione non è più completa : non è possibile stabilire con This solution is not complete: it is not possible to determine

certezza se sia possibile o meno raggiungere un goal, ne trovare sempre in certainty whether it is possible or not to reach a goal, you always find in

tempi finiti una soluzione poiché l'algoritmo di ricerca ha una componente Rates finished a solution because the search algorithm has a component

casuale. random. E' però probabilisticamente completa : la probabilità di non E 'but probabilistically complete: the probability of not

risolvere un problema con questo approccio è talmente bassa che per solve a problem with this approach is so low that for

applicazioni pratiche è una delle soluzioni più usate, anche perché ha un practical applications is one of the most used, because it has a

altro vantaggio: una volta creata la roadmap può essere sfruttata per molte Another advantage: once created the roadmap can be used for many

query , di conseguenza il costo della fase di creazione viene ammortizzato . queries, accordingly the cost of being created is amortized.

Esistono varianti ottimizzate di questo algoritmo: There are optimized variants of this algorithm:

1. PRM* [12]: il valore del parametro n, definito nell'algoritmo 1. PRM * [12]: the value of the parameter n, defined in the algorithm

standard, varia dinamicamente, intuitivamente diminuendolo standards, varies dynamically, intuitively decreasing

all'aumentare delle dimensioni della mappa. as the size of the map.

2. LazyPRM [13]: Funziona come il PRM normale, ma l'assenza di 2. LazyPRM [13]: Functions as the PRM normal, but the absence of

collisioni nel cammino che congiunge start e goal viene verificata collisions on the road that connects the start and goal is verified

solamente al momento del bisogno. only when needed. The phase of creating the map

It is speeded up, but the single query is slower.

• Rapidly exploring random trees [14]:

Another approach to the creation of graphs

in the configuration of start , at each

Cfree looking for the nearest node and if it is

without obstacles that connects the node is added to the configuration

the shaft, otherwise it is discarded. After a predetermined number of

iterations you try to connect to the shaft configuration goal, if

the cast succeeds, the problem is solved and it is sufficient to apply a

search algorithm on trees to get the path, otherwise

shaft expansion continues. The feature of this technique is that

He manages to "explore", ie make accessible areas of the map, in

very quickly: it is proved that already in very few interactions

(In the hundreds) the algorithm is able to cover a large part of the

Unlike the roadmap in this case the data structure is created single-

query : whenever a request is made to Schedule I is

created a new one. This may seem like a limitation, but the cost

Computational given by the creation is much lower than that of

a roadmap : the number of nodes and especially of branches is less and the

The heart of the system is the local planners, the component that has the

task to connect nodes and create the branches. If the latter is able

not only to see if the nodes are connected via free routes

obstacles, but also what commands to give the robot to make it move from

first to the second is possible to realize a zero-cost algorithm that

also takes into account the dynamics of the system; in that case a node

It would be added only if actually achieved by the robot from

There are some variations of this algorithm:

It provides for the simultaneous expansion of two trees with one root in

start , the other in the goal . The expansion is alternating, after a certain number

iterations of trying to connect the two structures, if the connection

2. RRG ( Rapidly exploring random graph ) [12]:

Provides for the possibility of cycles in the structure, is a cross between

RRT and roadmap : the expansion in the algorithm proceeds as the basis, but to

each additional node is looking into a ball of predetermined radius around

to it in search of possible nodes to connect. The structure becomes

then a graph, but the cost of creation is always less than the

Proceeds as the RRG, but the creation of cycles is avoided by removing

redundant branches, that is not part of the shortest path from the

root of the tree to a vertex, the end is obtained a tree

optimized. In this way it combines the efficiency in determining the

paths of graphs to the practicality of management of the trees.

• EST (Expansive Search Tree) [19]:

A further alternative to the geometric solution of the problem is this

approach that also provides for the creation of a tree, the expansion

is no longer completely random as nell'RRT but companion, so as to

not expand into areas already accessible. It uses a grid of cells for

discretize Cfree and track configurations already explored: cells

reachable from the root by a path in the tree; the points

with which to expand the tree will be taken from free cells close to the edge

the portion of the grid already explored. At each iteration is guaranteed

the expansion of the tree in areas previously unattainable, but the function of

point selection with which to expand the tree becomes more complex.

Even this algorithm provides for the simultaneous creation of two shafts,

one rooted at the start in the other goal . There are variants posticipates

verification of the validity of the path at the time of need, for example

SBL (single-query bi-directional lazy collision checking) [16]. In this

algorithm in the expansion phase connect only nodes according to the

relative proximity, when you have created a path between start and goal are tested

all branches to check for collisions, if there are not the

problem is solved, otherwise they delete branches invalid and proceed

Search algorithms on trees and graphs:

Once you have a schematic of the environment which also includes

configurations start and goal, come in search algorithms on graphs or

trees, object of research for many years as fundamental for many

applications so that you have achieved optimal standards of

The algorithm most widely used in various applications is A * [17] . The idea

base is to examine the nodes of the tree or the graph according to some

order starting from the nodes directly accessible from the start , until

visiting the knot goal . Then you can determine a path

through nodes previously visited links departure and arrival.

The order in which they carry out the visits is determined by a special

heuristic function, this associates to each node a score based on its

distance from the start , just calculated, and the goals , obtainable only

using mathematical estimates in a precisely heuristic. Are

constantly maintained two lists: one of visited nodes and one of those still to be

visit, at each iteration: you make a visit to a node by adding it to

list visited, expands the list with other new nodes reachable, are calculated

the scores for the new items and you choose to continue with the next node

the expansion according to these. This algorithm is thus able to find the

the least costly path given good heuristic function, which can

establish with a low degree of error estimation of the distance from the goal .

open = list of accessible nodes to visit;

closed = list of nodes already visited;

x = estraiNodoConScorePiùBasso ( open ) ;

aggiornamentoScoreDeiNodi ( closed );

A different approach is to model already in the path planning the

kinematic and dynamic characteristic of the controlled robot. In this way

with only one algorithm it is possible to obtain a trajectory actually

executable by the robot and controls required to implement it, avoiding rework

next. The computational burden required to resolve each query ,

however, increases and you also need to be able to accurately model

Intuitively, one must discretize somehow inputs that you can

send the robot from the outside. The simplest approach is to do this: to establish a

minimum unit of time t, starting from the configuration of start apply all the

possible combinations of input for a time t , and calculate all the configurations

reached belonging to Cfree , repeat the procedure for each state so

reached. The structure thus obtained will be a rooted tree in the start,

in the branches will be saved commands needed to move from one node to the next.

This procedure should then be repeated for each new node so obtained up

An optimized version of this process is represented by the algorithm

KPIECE (kinodinamic plannign by interior-exterior cell exploration) [18].

resolution that keeps track of the space already

explored. The focus is no longer placed on

mapped physical space, as on the states that the robot can take inside

This is made possible by the differentiation of cells outside the cells and internal : the

grid is initialized at the time of the call of the algorithm, but the cells no,

They will be created only when visited by a tree branch during a phase

expansion. It defines the internal cell with 2n neighboring cells initialized,

n size of the state space, cells outside all the others.

Choosing the point from which to expand the tree is not random, but established through

a score associated with each cell so as to direct it to an external cell

as a starting point, so the tree is able to cover the space available in

very quickly. The different resolutions of the grid allow

accelerate the selection of the cell from which: first comparing the cells of more

high level, the larger, then, found one with the most points, you

reapplies the steps within it recursively up to the

minimum resolution, decreasing the number of comparisons required

compared to directly use the grid with smaller cells.

This algorithm has proved able to obtain excellent results in riescendo

reach the goal within a reasonable time and with a data structure very light if

compared to those obtained by other algorithms. Although it presented as

algorithm based on the control, is simply used for the simple

Schedule geometric considering how the state space

commands to be given all possible rototranslations in three dimensions.

The solutions proposed both for the creation of maps that for the determination of

paths are many, each characterized by its strengths and weaknesses, but if as

concerns the maintenance of the map 's octomap it appears as the solution

better for the purposes of this thesis, in the world of planning routes

there is an algorithm clearly superior to the other in any situation. Own

for this in the following system it will be developed so as to adapt

dynamically to meet demand by choosing the most appropriate algorithm

Based on the start, goal and situation of the space around the robot.

To test and compare the algorithms described in the previous chapter, it was decided

not to produce new solutions from zero, where possible, but to attain

desired functionality by combining components already developed, tested and available as

In particular, it is used as a linking element for a dialogue between

the various components of their system ROS; the role of planning and mapping was

instead it made using the component MoveIt !, based on many libraries between

which OMPL for path planning and octomap for mapping .

ROS , which stands for "robot operating system", is one of the solutions open source

more widespread within the frameworks that facilitate the development of software

robot. The system is defined as "meta-operating system" because it gives

disposal of the developer a number of features and services

traditionally offered by an operating system, although relying on a

other OS, typically ubuntu . It also allows you to facilitate the management of systems

complexes also consist of very different machines between them, providing a

unifying layer to facilitate the exchange of information.

The main services are made available to developers:

• mechanism of synchronous communication between processes with model

client / server, similar to ' rpc , through the use of services .

• Mechanism of asynchronous communication between processes by implementing a

• Mechanism shared memory communication between processes

leveraging a centralized server said parameter server .

• Possibility of structuring the application, or more generally the code,

stacks or package easily distributable and reusable, so as to

The system is able to handle independent processes weakly bound to each

to the other, easily relocatable or even replaceable to run-time even in

distributed and heterogeneous systems and structured on the package or stack for easy

It is not tied to a particular programming language: offers interfaces

to its services in C ++, Python, Lisp and experimentally also Java and Lua.

What ROS realize is the "ROS Computation Graph" that is a peer network

to-peer processes ROS that process data together, communicating through a

• Nodes: the processes that run within the framework ROS .Sono I'm

standard executable that make up the graph of processes and exploit the

• Master: ( or ROS-master ) is the naming system integrated in ROS that associates

logical names to the physical location of the nodes or generally components

of the system. It acts as a DNS making it possible to get the

resources that interest us only by their logical name without

• Parameter Server: currently integrated in the master, is a system

centralized to save data like a database simplified. All the

nodes running on the framework may at any time and read

save data on the server, changing according to them their behavior.

• Messages: Format through which the nodes communicate with each other, are

data structures composed of fields typed. The data are supported: types

primitive common to almost all programming languages, arrays, and

arbitrarily nested structures. They are described by special files with

extension ".msg" saved inside the packet of applications ROS.

Topics: Mechanism through which it makes possible communication between nodes

with semantics pub / sub : a node produces messages and makes them available

outside "posting" on a topic ( publish), those interested in those

messages you "subscribe" to that topic ( subscribe) whenever there is a

Message ready to read the subscribers are notified by the framework in

so that they can "consume" that is read. In this way more

processes are in communication with each other without knowing each other

explicitly. Each topic must be strongly typed, ie

contain messages in a very specific format is known to the manufacturer that the

consumer, and it must be identified by a unique name. More nodes

can subscribe to the same topic , as well as more nodes can

publish on the same or more topics : each is a separate entity, a

communication channel independent of the processes that use them.

• Services: Mechanism through which communication is made between

knots with semantics Client / Server synchronous blocker. A node is able

exposing outside a function, those who want to invoke it can do

using a mechanism similar to that of remote procedure call .Il The

clients will have to know what features are available and how to invoke them,

then the signature of the functions are stored in special files descriptive

" .srv " and the list of possible functions is maintained in the Master.

• Bags: I am a format used to store and play back at a later

ROS also provides a set of tools for the management of all

resources associated with the execution of the application, creating a system of

distribution of software packages similar to what you can find in many

Linux distributions. The resources are differentiated:

• Package: the main unit for the organization of the software, within

a package is all that is required to carry: knots, libraries,

configuration files and in general all that is related to

• Stack: is a set of packages logically related in some way between

• Manifest: it is a file xml in each package, which provides a

description together with a series of metadata, such as dependencies

• Stack Manifest: realizes the function of the manifest at the level of the stack .

• Message Type: description file messages ROS used by nodes

the package , typically found in the " msg "within the

package itself or other external. The file format is purely textual, you

indicate the fields and data structures that make up the message.

• Service Type: File description of the services provided by the nodes,

They are located in the " srv " inside the package . The file format is

textual, is indicate the input parameters and the output of the service.

In addition to these features ROS provides a convenient full suite

functions and applications invoked from the command line to manage resources,

perform nodes, invoke services, testing of applications and in general for

everything related to the management framework . In the latest versions all

monitoring tools at runtime were grouped into

a single application with a graphical interface, " rqt ".

Each node before being put into effect, if use of topic, must

be "remapped": you must indicate whether and on what topic should subscribe

He must publish. This information typically is not wired, but are

let parameterized so you have reusable nodes with different topics .Per For

carry out the re-mapping , that is, give a significant value to these parameters,

can specify command line values on invocation ( re-

mapping arguments ) or alternatively use a launch files : a file with syntax

like xml that lets you specify many of the properties of a node before

If the node supports it, a part of the parameters of the launch file may be

saved on the server parameter so that it can be modified dynamically

run-time ; typically the mapping is not among them: Once a node is

tied to a topic as a publisher or subscriber is not able to detach and

At this point the system is ready, every time there is a new entry in a

topics in the framework will inform the various subscriber so that

can invoke the function callback associated with the management of messages

the topic . Similarly if there is a change of the parameters in the parameter

server will always be the task of the framework to inform the nodes of the new value.

As regards the services instead the system behaves with a semantics of

type client / server : the master provides a list of available services,

doing also by service names for the same; the client after a lookup on

server for the 'URI of who implements the service will connect

the same and make a call to the remote procedure similar to a PRC .

From a more technical point of view the semantics pub / sub is obtained by exploiting the

simple socket direct between publishers and subscribers with variants of protocols TCP or

Udp. The Master intervenes only at the stage of establishing a connection

by doing service names, once this is created no longer it interacts in

any way with the nodes involved. Even the semantics Client / Server of services is

achieved by using a connection established between the two processes by

socket to exchange requests and responses; again the Master does only

service names when establishing a connection.

The nodes are made available to the developer community in packages that

contain all that is necessary to its operation, is then provided

of the navigation commands and file system change that extend those

standard unix to exploit this hierarchical structure. The system for the

Software distribution is similar to that of many Linux distributions: here

the framework is able to solve only the dependencies of a package

software (shown in manifest.xml ) and then proceed to installation. As

Many packages are distributed in source format, they are made

available as tools to compile two versions of the command make :

ROSmake and catkin-make . As the command from which these also

based on a configuration file ( makefile ) present within each

package , which specifies where to find the libraries and sources to compile.

Moveit! is a framework software developed by ROS and created with the idea

to make it simple and immediate the definition of trajectories without obstacles for

a robot within an environment and their subsequent execution. Scopo Purpose

the main system is to make available within the framework

ROS multiple external libraries that implement state of the art

algorithms for solving problems of path planning and mapping, coordinating the

operation, masking the complexity of the system and promoting

the immediacy of use; so that the developer does not need to worry

of general issues have already been studied and resolved, but can focus on

characteristics of your system. The framework was developed in

very modular way, it is therefore easy to expand it by replacing some parts

2.1 - Diagram of a high level of the various components of the

framework, the elements in gray are external components to

with other custom to be able to easily adapt to your needs. All is

controlled entirely by external configuration files, generated good

It starts automatically from a description of the robot to be controlled.

The system exploits the framework octomap for the part of reconstruction maps, the

library OMPL (Motion Planing Open Library) for the part of path planing a

set of libraries for the detection of collisions and alternative methods of planning .Il The

everything is coordinated by a single executable: the node ROS "Move Group" that

It manages the representation of the map (Planning Scenes), the system

Navigation within it (Pipeline Planning) and controller (Trajectory

Execution Manager) in charge of implementing the defined trajectories. The Move

Group also has the task of receiving the commands given by users,

therefore it represents the heart of the whole system.

To generate a map MoveIt! uses an implementation of costumizzata

Octomap framework, which implements a representation of the map

environment through Octree and uses probabilistic functions to update it in

From a PointCloud and dall'odometria robot.

The framework can provide a very detailed map of the places observed

without an exaggerated employment in the memory and with the ability to model both

the free space that is not yet explored. The efficiency in the management of

memory is guaranteed not only by particular choices embodiments, from the structure

data tree: if the children of a node all have the same value you can

remove them and leave only the parent node to describe that portion of

space. In this way the quantity of information to be stored is the

minimum and subsequent queries on the map are speeded up. The

Queries are also facilitated by the hierarchical structure that stores the

data: if we consider as minimum resolution such as a cube of 5cm

side will be enough only 16-level tree to represent an area enclosed in a

cube 327m from the side, more than enough for the majority of applications.

The update employment to every new input is always at the level

of the leaf nodes by means of a probabilistic function so as to have

an implementation robust even in the presence of possible noise on the data

coming from the sensors. Starting from the old value associated with employment

node and the new one coming from the sensors, by applying a function defined

a priori, the new value is obtained; the node is considered busy if the

result of this transaction exceeds a certain threshold, otherwise free. If it's not

yet it has been initialized the node is considered unexplored.

The basic structure of the single node is very simple: it contains the data value

associated employment and a pointer to an array of eight pointers to nodes

It is representing the eight possible children. This structure allows to obtain the minimum

memory footprint without loss of information, for example is not

necessary to store the coordinates at which is located a node or its size

as obtainable from the data structure. Despite the amount of data to be processed

is huge, there is talk of tens of thousands of points for each new measurement, the

system fails to update the map in reduced times, in the order of tenths of a

according to. The only action required when receiving the input is quantized

points in suitable voxel and go to update the value of employment

that part of the space. The framework must update the value of employment

even in case of free space, to differentiate it from what is not explored, of

Therefore, you must also quantify all the free space that is

between the observer and the points of the cloud so you can properly update

Each node can be associated additional information, eg

RGBD using sensors can be coupled to each voxel color.Ogni Each

added property needs of special blending modes to make

updating from the new input data, just as it is updated

The implementation exploited by MoveIt! has the objective the obtaining of

high performance, for this the resolution of the map is relatively large and

We do not consider the data rgb relating thereto. Furthermore, to ensure modularity is

You can be specified via the configuration file which plugin to use

perform the map update at each new input, so as to be able

use of personalized based on other data in place of the default based on

2.2.2 OMPL (Open Motion Planning Library) [23]

For the determination of the trajectories is used OMPL, a library open

source that implements many algorithms based on sampling for the motion

planning. The library was established for use in both research environment, both as

means to be used in industry robots, it must then combine ease of

expansion modularity and high performance. This is made possible by simplifying the

Library components and making them completely independent from each

other, so as to make it easily extensible, but also efficient.

What the library offers are simply a series of implementations

optimized algorithms of the main path planning based on sampling:

RRT, PRM, KPIECE, EST .... It is not explicitly represented geometry

robot or the environment in which is navigating, this allows to use these

algorithms and this library in fields outside those, industry-

eg in medicine. What is able to do is, given a set of elements and

a validation function of the same, to find a path between two points

belonging to such set; together and function will be defined at the time

the library call. In the case examined by the thesis together will be the

configuration space that the robot can occupy

Validation will verify the absence of collisions with the world and respect for

constraints of self-collision . The validity of the path between two nodes neighbors occurred

by a special component, the Motion Validator, also only defined as

interface to be flexible in the implementation. It comes

Default validator that checks based on the interpolation of a discrete number

intermediate states between the two to connect. From these premises it follows that this

Seller can work both for the geometric planning for both the one based on

control simply by changing the set from which the sampling takes place and

function that checks the validity of the states.

• Easy expansion: a new algorithm is Extra those

available simply by providing an implementation that befits

• Capable of benchmark comparison it is possible to compare the

implementations of algorithms to determine performance

• Adaptability: data start , goals and state space, the system can determine

dynamically which algorithm is best to use to solve the

problem, in order to have always an optimal solution.

• High performance: to be as efficient as possible the library is written in

C ++ and makes extensive use of multi-threading for faster operation

Interfacing with MoveIt! it got through the package ROS

"Ompl_ROS" adapting the API library in the world ROS. Through MoveIt! You

You can have access to all features such as choice of OMPL

the algorithm to be used or the benchmark , without having to worry about defining

the set of elements to be sampled, the validator of them or the motion validator .

What the system will be sampled points of octomap that will be

validated using libraries for collision checking , FCL and the PCD

Specifically, using the three-dimensional description of the robot contained in the files of

configuration and three-dimensionally to establish

the validity or otherwise of a point and the configuration of the robot associated with it.

The Move Group is the main process of the application MoveIt !, is responsible

to coordinate the operation of all the libraries involved and to manage the

user requests, thus represents the core real application.

• A description of the robot which specific joint, links and sizes in

format .urdf [24] standard in ROS .

A process you post the odometry of the robot, in the format tf

[25] of ROS, in which for each link is indicated which is the transformation

to be made to bring a given taken in the reference system

centered in the link to an absolute reference system fixed.In questo In this

so it is possible to derive the position of the components of the robot in

space and bring to an absolute reference system any

• A process can publish a PointCloud from rooms

External to be able to reconstruct the working environment. This requirement is

optional as it is not always necessary to take into account the environment

The user interaction consists of three phases:

1. Creation of the configuration file from the file system ".urdf"

Description of the robot using a simple application to

GUI, the setup assistant , which creates an automated

all files needed by the application to work . A robot is

described as a series of links, that is, static elements, connected by the joint ,

movable joints, for each of them are described dimensions and degrees of

freedom as well as a description of the geometry and visual through mesh .In In

this phase also specifies the groups on which will go to act

MoveIt !, that is, sets of links and joint for which the system will plan

trajectories, as well as the robot is connected to a link that static

It represents the terrain; in other words if it is fixed, it moves on a plane, or

2. Configuration files previously produced are finished

by the user with the addition of the indications on the controllers that will have

implement trajectories and sensors to acquire information about the world.

From the previous stage you are also being created a series of launch file for

start the application itself, it will be responsibile

control and perception of the external environment.

3. At this point everything is ready for the actual use, the user has three

primary means for interacting with the application: API via code in C ++,

command line and related API in python or via an interface

graphics created by a plugin in the viewer ROS "Rviz" ;

This allows you to issue the query of planning by specifying start and

Goals graphically and to get a representation of planning

scenes , that is, the map reconstructed from the system with the sensor data.Per For

the needs of this thesis the last option is the best as

It allows to obtain a visual representation of both the map

maintained by the system is calculated trajectories. In situations of

Use the real use of the API via code in C ++ remains the option

preferable because beg to have total control over all

facets of the application without masking the complexity as

Once selected start and goal application calculates the distance to be traversed,

expressed as a series of positions in the space for the link and joint that

make the robot. This will then be sent to a manager controller

specified in the launch file , the trajectory execution manager that is responsible for

communicate the path to the process that materially change to

commands to execute and supervise its operation through feedback

by the executor and MoveIt !. In the case where there are problems, such as a

new obstacle appears in the path, the execution is canceled and

The controller itself can not be a simple process, should

granatire a certain degree of interaction and control. To this must implement

a ROS Action : a particular interface and library ROS, made

entirely by the use of topic, which allows to obtain a semantics of

Communication Client / Server with non-blocking ability to monitor

the trend and possibly cancel the request by the client. In practice

the node servers will exhibit a range of topics on which it will be possible to send requests

by the client to perform trajectories, get feedback or cancel

execution. To get all these features the controller must

obviously be structured in a parallel manner, since while the trajectory

is performed must also be listening on various topics for any new

All necessary information during execution are enclosed in

external configuration files generated, in good part, automatically. Ciò That

makes the application very flexible and reusable, for example it can be

also use to control robotic arms, while remaining very

The choice of platform ROS is derived from the large spread that it had

in the research community in robotics and automation and the consequent

plenty of software solutions based on it. The choice of MoveIt! as

application of reference, however, was given by the double purpose of getting in

a single executable application that can map and make path planning ;

in addition to having a system usable in real contexts high performance, but which

at the same time it can also be an interesting case study because of

possibility to benchmark between different algorithms.

In addition, although still in alpha test , MoveIt! represents the component that

the future will help to solve any problem related to planning within the

To test the validity of the proposed software components has carried out a

series of tests using a simulator integrated in Ros for the simulation of

quadcopter and the surrounding environment. In this way it was possible

verify the effectiveness of the system and to compare the performance of various algorithms

proposed in operating situations realistic.

To test the operation of the component MoveIt! he was chosen for the simulator

open source Gazebo [26], which began as official simulator integrated with the system

Ros , if they have recently posted becoming an independent component, but

however, it remains the reference point for testing applications developed

within Ros both for ease of integration, and especially for the large

number of Package for the simulation of the robot and sensors of various kinds are already created

and made available to the developer community .

The basic simulator is able by means of a physics engine to faithfully reproduce

the dynamics of a system, taking account of gravity, contact forces and friction.

It may be further extended to simulate also other factors, such as the

wind or propulsion in the case FVO, through a simple system based on

plugin. What we can do is to simulate the behavior of a robot

within an environment consisting of geometric elements. Structure

robots, as well as that of the elements that make up the world of work, is

contained in text in external files, but is represented in the

simulator by the use of mesh . The robot behaves exactly like that

using real physics engine inside to interact with the world more

any plugins for other functions. This way you can test the full

application operation Ros ; at the time of interfacing with the real-

the only changes to make robots will replace the topic of communication with

the simulator with those of communication with the real robot.

FVO simulation was used the package " Hector_quadrotor " [27] ,

created to provide within the world Ros a complete simulation of a

quadcopter, both as regards the controls low-level, managed by

PID cascade, that for those of high-level managed directly by means of

software Ros. Besides FVO itself (structure and engines), they are simulated

also a considerable number of sensors: IMU, barometers, magnetometers, sensors

ultrasonic and GPS. All information from the sensors are merged

by means of an EKF ( Extended Kalman Filter ) to obtain a single

representation of the state of the vehicle and to decrease the error inevitably

This information will then be utilized by the controller to stabilize the

quadcopter. From the outside is instead to speed control: for imparting a

flight control will need to provide a vector representing the speed at

which should move the quadcopter; in the case where the magnitude of the vector is 0,

aircraft will simply fluctuate. The controller is implemented as a series of

cascades of PID, with the ' inner loop for controlling the attitude, speed

angular and vertical, while the ' outer loop controls the speed

horizontal, altitude and direction. Also these are simulated, in particular

by integration of a module matlab outside, so that it is possible

for users to go to customize or replace the controller with a

their implementation. The control approach based on the use of two loop

to coordinate two actions different control has been widely studied in

many scientific publications, for example [28] .

Very useful features of this package are the presence in the model

quadcopter sensor RGBD simulated (a Kinect ) chosen

source PointCloud in the final design and two nodes for the issue

odometry aircraft format tf [25] is required for the low level

control quadcopter is later for the rest of the application . The medium is

described, according to the common format in the world Ros, mainly through three

link : the base_link that represents the body of quadcopter, the camera_link that

represents the reference system of the room rgbd and the base_footprint that

It represents the projection of the aircraft on the ground, indicated by the frame fixed

Publish the odometry of quadcopter within Ros means publishing in

each instant in the topic tf the transformation required to convert a given by

reference system centered in one of these links to a reference system

considered fixed, that inside the project takes the name of odom_combined.

For convenience the links are categorized in a hierarchical tree structure with the

root in the link fixed, so you need to publish only the

transformation that links a link to its parent. For each link just then

posting a single transformation, while all the others will be retrievable in

order to simplify and lighten the system. This task is carried out by

3.1 - Tree tf relative to simulated quadcopter

two nodes contained in the appropriate package simulation, so that

the end user should not worry about how it obtained the odometry. Over

what MoveIt! also it needs a topic in which they are explicitly published

positions of the link as coordinates in space relative to the fixed frame, this is

It obtained in the simulation by means of a suitable plug- in gazebo.

These features combine to provide MoveIt! all the data it needs

for a correct operation: the description of the robot is contained in the package

Hector_Quadrotor in the right format urdf , the odometry is published by

components mentioned above and the PointCloud is generated by the sensor simulated

3.2 - Simulation of a quadcopter equipped with a Kinect inside

Defined simulation environment you have to create the configuration file for

MoveIt !; these can be created through a simple graphical interface,

the SetupAssistant, starting from the file ".urdf" descriptor of the robot.

Through this application you can add joint virtual so

define how the robot is connected to the ground, which is the fixed frame

the tree of tf; in the case of a joint quadcopter was added

Virtual fluctuating that connects the airplane to the ground, thus defining the aircraft

as capable of moving with six degrees of freedom in space. Sempre Always

interface should also be defined MoveGroup , ie groups of links and joint for

the system will issue trajectories, in our case, one group

composed of all aircraft. MoveIt! it was created for the management of any

type of robots, including models with automatic arms or parts can

to move independently from each other, so it is possible to refine

MoveGroup separate within the same robot to generate trajectories

The result of this first phase are a number of automated file

configuration and launch files sufficient to start the application in "mode

demo ", ie not connected to a real robot or simulated, but already able to

produce trajectories valid for it. To complete the configuration, you must

specify, going to manually edit the configuration file, like

retrieve the data from the external sensors, a room rgbd in our case, and

for each j oint which controller is in charge of implementing the trajectory produced

Within the project of this thesis the sensor data are already

issued by the simulator through appropriate plugin , so it was necessary

only specify the application in which the topics are published. In caso di In case of

actual application instead would be necessary to place a driver that allows

application Ros to dialogue with the room rgbd such OpenNI and its

wrapper Ros , who deal with retrieving data from the chamber and publish

appropriate topic, just like in the simulator with the room and the simulated

In order to function for a large number of robots the application MoveIt! provides

a structure of the robot controller at two levels:

• A manager of controller, designed as a plug- in MOVEit !, which dialogues

directly with the application of planning and receives from it the trajectories

issued for MoveGroup . According to the definition of them and any

other configuration files get the list of joint to be controlled and forwards

requests to the controller in charge of implementing them. Also monitors

the execution of the trajectory and in case of problems, for example obstacles

first not received orders to cancel the current execution and

• The controller real receiving trajectories to perform and

produce commands that shall be given to the physical controllers of the various joint .Per For

a total control on the part of the manager, for example,

cancel the run in progress, they should be structured

exploiting actionlib and a file action costum that allow to obtain

semantics client / server based on the use of non-blocking topic. In

especially in the file action is called a kind of contract of function,

ie what are the message formats to be issued on specific topics for

require the conduct of consultation to ' actionServer that implements the

action. They are normally defined formats topic of goal, result,

The controller of the quadcopter has therefore been made in two parts: a

SimpleControllerManager, designed as plug- in

ActionController, realized as independent node. For the realization

dell ' ActionController was necessary to define an action because those custum

already in the system provided for the control of the joint with only one degree of

MultiDofFollowJointTrajectoryAction and provides for the issue as a goal of a

trajectory composed of a series of points in space.

Since the purpose of this thesis was mainly to test the validity of the algorithms

of mapping and path planning, the ' ActionController was made in a manner

simplified as pure filter that given an input trajectory converts it into

commands to be performed for the quadcopter. The realization otherwise exploits

actionlib, so everything is pre-configured for a realization competitor

usable in real situations outside of a simulator. The only change

make it a multi-threaded management controller so that while a

thread emits the series of commands another remains awaiting possible

Information on the change of course by the plugin MoveIt! .

The system is now ready to fly UAV simulated, for the sake of convenience is also

It defined a simple knot to tele-operate the robot via keyboard by

User. In this way, at first the aircraft will be flown

by the user so as to obtain a consistent representation of the environment

around him, then control will pass the application of planning

and it will be up to the user only specify the goal . Thanks to the system entirely

based on topics in the manual and automated control can coexist

3.3 - Diagram final simplified communication between the nodes of the application,

ovals represent independent processes, arrows topics through which

The application as a whole is then composed by a series of executable

independent of each other; / move_group , node generated by MoveIt! ,

is the core of the application, receives the odometry produced by

/ Robot_state_publisher and / ground_truth_to_tf on the topic / tf, and according to them

It emits trajectories to be communicated to / my_controller_node , action controller

commissioned to translate them into commands to be issued in speed on topics / cmd_vel. Finally

/ gazebo is the process that is running the simulation itself, while

/ Quadrotor_teleop controller is the handling of the airplane. All this is visible in

Figure 3.3 represents a simplified view of all communications in

act in the application, such as the lack of topics related to data from

The end-user interaction with the system of planning is via a

handy plugin viewer Rviz that allows you to select graphically

the goals that the robot has to reach and which algorithm to use planning as well

to offer a real-time representation of the map reconstructed from

3.4 - Graphical interface for query generation of planning and visualization

The application thus described allows to easily reconstruct the map

of the simulated and choose which algorithm to use to resolve the query

planning among those available: Kpiece, Bidirectional Kpiece, Lazy Bidirectional

Kpiece, EST, PRM, PRM *, RRT, RRT-Connect, RRT *, SBL. Alternatively,

You can delegate the choice of the planner application, leaving the option of

default "undefined": in this case will be the program itself to instantiate the

solver deemed most suitable data start, goal and situation of the planning stage.

All of the algorithms in each case are limited to the planning geometry, are not

I never considered approaches based on the control.

The ability to decide which algorithm to use can make

direct comparisons and comparisons between the various solution strategies proposed, both by a

the point of view of timing resolution of the query, both from the point of view of

memory footprint and breadth of the data structures created.

A simple test provides for the definition of an environment in the simulator compound

by a series of cylinders that surround the UAV. The purpose of the test is to verify the correct

the map creation and operation of the various algorithms to bring the paintings

Rotor out of an area dotted with obstacles, to increase the difficulty of the test

the work area of the application has been limited so that the airplane

can simply climb over obstacles, but needs to work around them through steps

close together, so simulating the presence of a ceiling.

3.5 - Working environment in the simulator on the left and its reconstruction

within the plugin MoveIt! on the right

In a first phase the frameworks rotor has been tele-operated by the user so as to

guide him to the observation of the surrounding environment to create a map, in

Following the control passes to the application that will try to determine a

way out of this area, the user will only have to indicate the goal by

As regards the part of the reconstruction of the map the test has failed

positive: thanks to the perfect odometry produced by the simulator map obtained

It is very accurate and precise, and the building and updating take place in

In reality the odometry of the medium can never be so accurate, as

deduced from measurements of sensors inevitably subject to noise and disturbances.

A common solution is to merge data from multiple sensors using

an EKF as is nell'uav simulated for the control of established. In this way

you can join the measuring systems of high-level as Visual odometry and

GPS with other more low level as IMU and magnetometers to obtain non-

only exact, but also a system capable of operating in the

different environments and hostile as it comes with a number of independent

For the part of path planning library instead it has proved capable of

solve the problem posed no particular problems, but times

execution vary widely from execution to execution, since all the

algorithms require a random sampling of space then a

non-deterministic component in the calculation of the trajectory. Anyhow

on average the solutions are found with times of less than a tenth of

second and with a data structure composed of at most a few hundreds of states.

The output of the various algorithms is a trajectory formed by a series of points in

space, task controller will translate it into commands for the quadrotore. In In

practice will simply calculate the velocity vector need to move to the middle

from one station to the next and apply it to a unit of time. Per For

As simple as the controller has proved to be able to perform quite

faithfully trajectories, with very small errors due to sudden changes

3.6 - Example of queries submitted to the system on the left (the goal is

represented by the marker orange) and trajectory of solution to the right

3.7 - Another example of a query submitted to the system and resulting path, this time

been limited work area so as not to allow the overcoming of obstacles

direction and small instability in flight. It is not applied to any type of

simplification of the trajectory or optimization of commands, for this

the execution takes on average a long time and the flight is not fluid.

The tests have shown problems with certain algorithms, namely:

BKPIECE, SBL and LBKPIECE that, in this implementation, have problems

in the management of bodies and trajectories to six degrees of freedom, with the result of non-

to obtain optimum performance or even not be able to create a valid trajectory

even after the creation of structures with thousands of states. Probably these

and other problems will be solved in the future with the passage of MoveIt! versions

The timing of implementation of the other algorithms, despite the diversity with which

approach the problem, they are quite similar exception that in this RRT *

implementation has been created to ensure consistent execution times, but

very high (about 5 seconds on the test configuration) but try

produce the trajectory more simple and straightforward as possible to reach the goal. The

trajectories produced by the other algorithms instead are not necessarily

optimal, indeed very often sharp or too long, while

while remaining viable. This in spite of each algorithm to a

first phase of resolving the query will follow a simplification of

trajectory time to make it as linear as possible.

The number of states created each execution is highly unlikely, since it is based

the random sample of the state space, so not very relevant to

analyze. Tend all algorithms do not require more than a few

hundred were before creating a valid path. Again, exception

RRT * which in this implementation also produces thousands of stages for

obtaining a solution as optimally as possible.

This analysis shows that even algorithms oldest definition as

PRM or RRT manage to achieve more than valid, resulting sometimes

even superior to most modern and sophisticated algorithms. This probably

because from the point of view of the quadcopter is seen as a cube

able to move freely in space, that is, the basic definition, as well as one

the simplest cases of path planning. There being no particular restrictions

Structural than to not collide with the environment in the generation of

direction, even algorithms based on brute force can make conversions

Optimal also thanks to the ease with which they can be parallelized for

The application is then able to effectively carry out the tasks for which

It was created, even if not yet ready for use in a real environment. Per For

As regards the part of mapping everything works fine provided you have

un'odometria perfect for the medium, for the part of path planning instead is

necessary to have a choice if you are looking for solutions in a short time we have a

wide selection of algorithms, but the path may need to be

softening before it is implemented; if instead we prefer to obtain already at this

phase optimal solutions the most reliable configuration is RRT *.

This thesis is aimed at a preliminary study for the integration of a

framework for mapping and planning in an automated system

operating in real environments, specifically within the SHERPA.

The application developed and tested previously could provide a basis for

which to implement a number of features within the project.

4.1 Integration of the application in real environment

The application is already realized theoretically capable of operating in a

real, provided you make some small changes:

• Create a file ".urdf" description of the robot: The first step is created

a descriptive file of the real robot in "urdf" so that the system

You may know the structure of the robot.

• Create and edit the odometry: In a real context of the odometry

means is no longer generated by something external as in the simulator, but

It must be obtained starting from the observations of the onboard sensors. Per For

reduce errors due to wrong measurements or noise

typically they combine several sources of information using an EKF,

have independent systems for calculating odometry is indeed appropriate

is to increase the accuracy of the system and to ensure the

operation even in hostile environments. In the context of

It will be necessary to write a node that performs the dual task of

publish the changes on the appropriate topic / tf and posting

position of the joint of the robot on another.

• Get the data from the sensors of depth: In a real need to

a node that interfaces with the physical sensor by means of special drivers,

fetch data act and to publish them in special topics in a format

recognized by Ros and MoveIt !, typically a PointCloud.

• Interface with controllers: The system no longer needs to connect to

controller simulated, but to the real ones of the half. Regardless of the

controller actual communication with MoveIt! must always be

realized by means of a knot structured as ActionServer, which will have the

responsibility to communicate with controllers or even lower level

directly with the actuators. It will be given the trajectories

generated by the application, then it will be his task to transform them into commands or

pass to control lower level. The association joint controller

Anyway it remains specified in the configuration file.

• Simplification of trajectories: The trajectories generated are not always

optimal, even in some cases it is too tortuous to be

implemented in real situations. Serves then a component which monitors the

submission of queries to the system and possibly rerun until

when the result is not considered to be optimal. Then optionally

it is possible to apply a process of smoothing to make the trajectory

more simple to perform. An alternative is to use always

RRT * that in this implementation, as already said, seeks to obtain

once the optimal trajectory of the solution, not the tip speed

In addition to these changes we need to consider the problems arising from the

computational power required: to manage real-time application

it would require a hardware least comparable to that of a

notebook midrange, but the physical weight of this architecture can represent

a not negligible problem in many situations, for example in the UAV. Una A

solution is to not perform all processing on board of the system by

check, but to delegate parts computationally heavier a

separate system, managing on-board only controls low-level and the

direct communication with the sensors. In other words, in the case of a real UAV

by the processor board and we will be extrapolated from the data coming from

sensors, actuators controlled physical and eventually calculated the odometry there

the reconstruction of the map and path planning could instead be

turning on a remote machine independent. This type of processing

distributed and coordination between systems is made possible by the very structure of the

system Ros, in particular by the possibility it offers to put into

communication, through the system of names managed by RosMaster centralized,

processes that are also running in different machines, provided connected to the same

Smart collaboration between Humans and ground-Aerial Robots for

Improving rescuing activities in Alpine environments

The SHERPA project promoted by the European Union provides for cooperation

from seven universities, two companies and one association for the development of a

robotic platform for assisting human operators in the search operation

and rescue climbers missing, distressed or overwhelmed by avalanches.

Ultimate goal of the project is to develop a robotic system consists of three

independent entities: the small UAV (rotary wing), a high-altitude aircraft and a

land vehicle, acting in coordination under the direction of an operator

Human to support it in the search and rescue of missing persons in the alpine scenery.

The system will in fact be able to quickly scan large areas thanks to

audio-video sensors mounted on all the robots, the visual data from aircraft

They will also be combined to produce a three dimensional map of the area of

search. Everything is fully automated, operator's task will only give

simple commands, for example, which area to explore, then the implementation will be

entirely delegated to the intelligence board.

The three robots that compose the system take the name of " animals "and have

• Land Vehicle ( Inteligent Donkey ): It serves as a base for operations

computationally heavy data processing and a vehicle of

transport and charging base for quadrirotori. It follows the human operator

freeing it from having to hand carry the necessary equipment to

coordinating the operation of the system contained in a form

independent of the Sherpas box , also it allows you to charge on-site aircraft

at low altitude, increasing the autonomy otherwise poor.

• Aircraft at low altitude (Trained Wasps) : Probably made

small quadrirotori, are able to carry only small loads and with little

autonomy, but they are very agile and necessary for the reconstruction of the place

by means of observations from privileged positions close to the ground. Sono I'm

automated as regards the stabilization of the flight, take-off and

the landing, but not able to perform complex tasks if not

coordinated by an external component. They are designed to fly in

nearby human operator equipped with sensors and depth

audio-video, coordinated by Sherpas box . They represent the "eyes

• Aircraft at high altitude (Patrolling hawks) : Increase range

information available to the system through observations of the environment

high altitude. Both have the task of providing for panoramic

creating the map of the environment is to provide useful information for

coordination of the other components. They are available in two versions

both have great autonomy: a fixed-wing aircraft powered

by solar panels, energetically autonomous but not able to bring

very payload, and a helicopter able to replace the terrestrial vehicle

carrying the sherpa box in situation where access to the area

The Sherpas box performs the tasks of: contain all the necessary hardware to

communication and remote control of various animals, contain all

tools and equipment necessary to the human rescuer in its activities and

act as docking / recharge / deployment station for aircraft at low altitude.

It must be designed so as to be easily transportable from both the vehicle

Earth is from the aircraft at high altitude is the human operator, thus combining

lightness and strength of the structure. The interaction of the system with the user

Human occurs through wearable devices and devices for the reality

increased, so as to provide the additional information produced, without

distract the operator from their duties.

So the system can operate all the animals need to communicate to

other and with the sherpa box both to obtain the directives on the trajectories to be carried out

or commands to be performed, both for transmitting the data to the processing unit

perceived by the sensors. This communication is achieved where possible

leveraging technology wifi , while for communication with the components more

away are provided for other solutions, such as the use of radio waves.

From this high-level description can be obtained a series of

problems solved with the components software presented in this thesis:

• Communication: the final system must allow a large group

and heterogeneous of automated mechanisms to communicate with each other, we

You will therefore need a layer unifying between machines; if this

already foresaw a number of connection options and advanced features

It could greatly facilitate the work of developers software. Use

the ROS framework appears as a possible solution to the

carrying out communication between the various media, not only to allow

exchange simple messages between machines connected on the same network

leveraging technology standard socket, but also makes

all the functionality of high-level described above.

It 'also been developed to work even in configurations with

low hardware capacity, consequently could be executed by all

• Creation of the map from several sources: the system Sherpas each

robot is equipped with a video sensor or depth which he uses to perceive

the environment that surrounds it. All these streams are then gathered to

obtain a unique representation of the environment in the form of map

three-dimensional. Make a single map for each robot and merge them into

Following is a computationally appears counterproductive, both as a mole

of data to send. Consequently there would be no need of a system that

centralizing the management of the map and its updating since the

stream of data coming from the sensors. The framework octomap allows

do so, in particular in the implementation of MoveIt! considered, the

map management is centralized, while for the update is

defined a plugin for each source of information. Publishing data

from sensors on different topics it is therefore possible not only to

update a single map with more data sources, but also manage

different types of sensors simultaneously defining the plugin appropriate

to associate the update. This would make it possible

exploit to update the same map several technologies:

rgbd rooms, laser projectors or room stereo.

• Producing trajectories for different robot configurations: The system is

composed of robot with motor skills very different, a choice

design may be to want a single system capable of

produce trajectories for each of them, rather than having many solutions

different customized. MOVEit! thanks to its high configurability

It allows to do so, provided to model adequately the three types of robots.

This is made possible by the use of the library OMPL that abstracting

completely from the representation of the problem outside of the

space of states, can be used for virtually any vehicle

automated, as long as it provided a validator been appropriate;

if this application is automatically generated by MoveIt!

Disadvantage of this approach is that you do not take advantage of unifying the

peculiarities of individual vehicles, for example in the case of the aircraft high-

a quota system as MoveIt! overly complex: view

the absence of obstacles almost always the trajectory can be a line

straight line joining the start and goal. On the other hand there is only one component

software to control and one starts from a basic system already realized,

Possible additions proposals do not represent a final and

usable in all the problems of the project, but a solid foundation from which you can

Starting and possibly adapt with customizations. Surely these

solutions will be less accurate than others created ad hoc for the system using

functionality lowest level, but already widely developed and tested by a

large community of developers and users.

All the components used, also, are open source with the possibility of

analyze and modify the source code to suit your needs.

MoveIt! has also been developed to adapt to situations and

many different configurations allowing extensive customization thanks

widespread use of plugins : program components independent

implement a specific interface and achieve a certain functionality. Tutte All

key features (map creation, planning and control) s s

implemented through plug external indicated in the appropriate file

configuration, you can then integrate their custom parts in the system

go hence without changing the core of which will continue to exploit all the

functionality. You would, for example, you can insert a plug of planning with a

custom algorithm, while continuing to leverage the map rebuilt

by the system, the libraries kinematic and those for the detection of collisions or

bring together even more plugins of planning, so you can compare the

own customized solution with the other proposals.

The only component of the system is not replaceable Ros as everything else

the application is based on it, especially on the use of widespread topic

to make all communications between the various components running.

The developed application has performed remarkably well in the

simulator, fully succeeding in the purpose for which it was developed.

But if we move to consider a scenario of the actual application

difficulties they face are many, in addition to the necessary changes

previously enumerated, for example, there is the problem of complexity

Computational . With the technology currently available is very

difficult to perform all the necessary processing to a chip mounted

directly onboard the quadcopter, a part of the processing

They will have to be carried out on a separate workstation. This is evidently

a problem since the aircraft will never be really autonomous until

be bound to another processing unit, it is, however, likely that within

few years also solutions Mobile computing will achieve

the power required to operate a system structured.

The major problems and limitations of the proposed approach are in phase

Schedule : If you want to get immediate answers to queries you

riaschiano trajectories to get too edgy to be implemented,

thereby making a step of simplification and validation

the trajectory before the execution. In the case of quadcopter would

probably already considered more effective algorithms that take into account

the dynamics of the medium or use optimized algorithms for

calculation of the optimal trajectory as RRT * at the cost of a decay of

As regards the step of mapping , however, the proposed algorithm

It works well with the data from the sensor rgbd succeeding to

update the map with an accuracy of centimeters in time

Real provided, however, to have a perfect for the odometry means.

Although there are problems to be solved before actual use, the base

the system is sound and the possibilities for development are many especially

thanks to the great capacity of customization that permits to exploit

only parts adapting the behavior of the system to your

need. The same community of users of Ros and in particular of

MoveIt! is constantly increasing, and with it the number of solutions and plugins

They can go to integrate and improve the project.

With this in mind you can think of many future developments for the project with

the ultimate goal of making fully self a real

quadcopter. For example you might test the effectiveness of the system with

the odometry calculated by the onboard sensors, in place of that generated by the

simulator, or may be integrated: algorithms planning

custom optimized for the control of this specific means, algorithms in

able to carry out automatic exploration of an environment controllers and more

sophisticated able to simplify the trajectories to perform.

[1] Tabak Roth-Y, R Jain - "Building an environment model using depth information." (1989).

[2] M Hebert, Caillas C, Krotkov And, Kweon IS, Kanade T - "First Results for Terrain Mapping

for a Roving Planetary Explorer "(1989). Robotics Institute.Paper 433.

[3] J Ryde, Hu H - "3D mapping with multi-resolution voxel occupied lists." (2010) Autonomus

[4] D Meagher - "Geometric modeling using octree encoding." (1982)

Image Processing , Vol. 19, No. 2. pp. 129-147

[5] Armin Hornung, Kai M. Wurm, Maren Bennewitz, Cyrill Stachniss, Wolfram Burgard -

"OctoMap: an efficient probabilistic 3D mapping framework based on octress." (2013)

Authonomus Robots Volume 34 Issue 3 pp. 189-206

[6] Steven M. LaValle - "Motion Planning: The Essentials" (2011) Robotics & Automation

IEEE Magazine Volume 18 Issue 1 pp. 79-89

[7] J. Barraquand, B. Langlois, and JC Latombe - "Numerical techniques for potential field

robot path planning "(1992) Systems, Man and Cybernetics, IEEE Transactions on pp. 224-241

[8] Alex Yahja, Antony (Tony) Stentz, Sanjiv Singh and Barry Brumit - "Framed-Quadtree Path

Planning for Mobile Robots in Sparse Operating Environments. "(1998) Proceedings of the IEEE

[9] Priyadarshi Bhattacharya, Marina L. Gavrilova - "Voronoi diagram in optimal path

planning "(2007) Voronoi Diagrams in Science and Engineering pp. 38-47

[10] Tomás Lozano-Pérez; Wesley Michael A. - "An algorithm for planning collision-free paths

among polyhedral obstacles "(1979) Comunications of the ACM Volume 22, Issue 10, pp.560-

[11] THE Kavraki, Svestka P., Latombe JC, MH Overmars - "Probabilistic roadmaps for path

planning in high-dimensional configuration spaces "(1996) Robotics and Automation, IEEE

[12] S. Karaman and E. Frazzoli - "Sampling-based Algorithms for Optimal Motion Planning"

(2012) American Control Conference, pp.735-742

[13] R. Bohlin and LE Kavraki - "Path Planning Using Lazy PRM" (2000), and Robotics

[14] SM LaValle - "Rapidly-exploring random trees: A new tool for path planning." (1998)

Computer Science Dept., Iowa State University

[15] J. Kuffner and SM LaValle - "RRT-Connect: An efficient approach to single-query path

planning "(2000) Robotics and Automation; pp. pp. 995-1001

[16] G. Sánchez and JC Latombe - "A single-query bi-directional probabilistic roadmap

planner with lazy collision checking "(2003) Springer Tracts in Advanced Robotics Volume 6

[17] Hart, PE; Nilsson, NJ; Raphael, B. - "A Formal Basis for the Heuristic Determination of

Minimum Cost Paths "(1968) Cybernetics and Systems Science Volume 4, pp. 100-107

[18] AI Şucan and LE Kavraki - "Kinodynamic motion planning by interior-exterior cell

exploration "(2009) Algorithmic Foundation of Robotics VIII (Proceedings of the Workshop on the

Algorithmic Foundations of Robotics). 57: 449-464

D. Hsu, JC Latombe, and R. Motwani - "Path planning in expansive configuration spaces"

(1997) Robotics and Automation Volume 3 pp. 2719-2726

[21] A. Ioan Sucan and Sachin Chitta, "MoveIt!" - [Online] Available:

Armin Hornung, Kai M. Wurm, Maren Bennewitz, Cyrill Stachniss, Wolfram Burgard -

"OctoMap: an efficient probabilistic 3D mapping framework based on octress." (2013)

Authonomus Robots Volume 34 Issue 3 pp. 189-206

[23] A. Ioan Şucan, Mark Moll, Lydia E. Kavraki - "The Open Motion Planning

Library "(2012) Robotics & Automation Magazione Volume 19; pp.72-82

[27] http://wiki.ros.org/hector_quadrotor , Johannes Meyer and Alexander and Sendobry

Stefan and Uwe Kohlbrecher Klingauf and Oskar von Stryk - "Comprehensive

Simulation of quadrotor UAVs using ROS and Gazebo "(2012) Lecture Notes

Computer Science Volume 7628 pp. 400-411

[28] Naldi, Furci, Sanfelice, Marconi "Global Trajectory Tracking for Underactuated

VTOL Aerial Vehicles using Cascade Control Paradigms "(2013)

Original text

Contribute a better translation