Mapping, Localization, and Naviga-

tion of an Autonomous Urban Shut-

tle

Tesi di Laurea Magistrale in

Computer and Science Engineering - Ingegneria In-
formatica

Author: Alessandro Gadola

Student ID: 952929

Advisor: Prof. Matteo Matteucci
Co-advisors: Dr. Simone Mentasti
Academic Year: 2021-22
 i

Abstract

Autonomous vehicles are a rising technology that has recently attracted much interest.
They have found many applications: from classic cars to industrial tools, but also shuttles
for closed environments, like hospitals, airports, and campuses. In this work we focus on
the last kind of vehicles. These operate much dierently from regular cars since, in this
environment, pedestrians can roam freely, and their movement is not easily predictable.
For this reason, shuttles have to move at a much lower speed and consider obstacles much
more carefully. Furthermore, in these environments, the GPS is absent or not always
present, so it can not be relied upon. For the same reasons, the proposed solution must
rely on dierent means. In this work, we develop multiple pipelines for mapping, local-
ization, and navigation of autonomous shuttles in unstructured environments. We then
test these on an urban shuttle inside a university campus. Using the available data, we
compare the various solutions to establish the best one for our setup. In particular, we
compare three state-of-the-art solutions based on a 2D model, with one that uses a 3D
approach, to analyze how this impacts the performance.

Keywords: mapping, localization, navigation, autonomous vehicle

Abstract in lingua italiana

I veicoli autonomi sono una tecnologia emergente che ha recentemente attirato molto in-
teresse. Hanno trovato molte applicazioni: dalle auto classiche agli strumenti industriali,
ma anche navette per ambienti chiusi, come ospedali, aeroporti e campus. In questo la-
voro ci concentriamo sull'ultimo tipo di veicoli. Questi funzionano in modo molto diverso
dalle normali auto poiché, in questo ambiente, i pedoni possono spostarsi liberamente
e il loro movimento non è facilmente prevedibile. Per questo motivo, le navette devono
muoversi a una velocità molto inferiore e considerare gli ostacoli con molta più attenzione.
Inoltre, in questi ambienti, il GPS è assente o non sempre presente, quindi non ci si può
dare. Per le stesse ragioni, la soluzione proposta deve basarsi su mezzi diversi. In questo
lavoro, sviluppiamo più pipeline per la mappatura, la localizzazione e la navigazione di
navette autonome in ambienti non strutturati. Quindi li testiamo su una navetta urbana
all'interno di un campus universitario. Utilizzando i dati disponibili, confrontiamo le varie
soluzioni per stabilire quella migliore per il nostro setup. In particolare, confrontiamo tre
soluzioni dello stato dell'arte basate su un modello 2D, con una che utilizza un approccio
3D, per analizzare come ciò inuisca sulle prestazioni.

Parole chiave: mappatura, localizzazione, navigazione, veicolo autonomo

 v

Contents

Abstract i

Abstract in lingua italiana iii

Contents v

Introduction 1

1 State of the art 3

1.1 Mapping . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
1.2 Localization . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7
1.3 Navigation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8
1.4 Thesis objectives . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9

2 Background 11
2.1 The ROS system . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
2.1.1 roscore . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
2.1.2 Topics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
2.1.3 Messages . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
2.1.4 LaserScan and PointCloud2 messages . . . . . . . . . . . . . . . . . 12
2.1.5 Publishers and subscribers . . . . . . . . . . . . . . . . . . . . . . . 12
2.1.6 Packages, nodes, and launch les . . . . . . . . . . . . . . . . . . . 13
2.1.7 Parameters . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
2.1.8 TFs . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
2.1.9 Rviz . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
2.1.10 move_base package . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
2.1.11 Bags . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
2.2 The shuttle . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16
2.2.1 Sensors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16
vi | Contents

2.2.2 Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22
2.3 Odometry . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22
2.4 The Ackermann model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24
2.5 SLAM . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25

3 Nodes 27
3.1 Common nodes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27
3.1.1 Odometry . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27
3.1.2 TF . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29
3.2 Mapping . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29
3.2.1 Gmapping . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30
3.2.2 Hector Slam . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31
3.2.3 Cartographer . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31
3.2.4 Slam Toolbox . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32
3.2.5 ARTSlam . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32
3.3 Localization . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33
3.3.1 AMCL . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33
3.3.2 Slam Toolbox . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34
3.3.3 ArtSlam . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34
3.4 Navigation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34

4 Results 39
4.1 The environment . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39
4.2 Mapping . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40
4.2.1 Gmapping . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40
4.2.2 HectorSlam . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41
4.2.3 Cartographer . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43
4.2.4 Slam Toolbox . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44
4.2.5 ART-Slam . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45
4.3 Localization . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45
4.3.1 AMCL . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46
4.3.2 Slam Toolbox . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49
4.3.3 ART-Slam . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49
4.4 Navigation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50

5 Conclusions and future developments 53

5.1 Future works . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53
Bibliography 55

List of Figures 59

List of Tables 61

Acknowledgements 63
 1

Introduction

Autonomous driving vehicles are a technology that has witnessed an impressive rise in
interest in the last decades. While the idea started to develop around the eighties, at
the time, the resources available were severely underpowered compared to today's tech-
nology, so it applied to very limited scenarios. The rst demonstration of the maturity
of this technology came from the DARPA challenge of 2005, where autonomous vehicles
compete. In this race, the participants receive the track to follow a few hours before the
event starts. They use this time to program their vehicle since, once it starts, the robot
can not contact humans in any way. For example, the 2005 edition was 212km long, with
the rst place nishing in a little under six hours. While this was the second year of the
event, it was the rst where some participants managed to complete the track successfully.
As shown in [2], the result is that today many companies and research centers started
working on such technology.
While the rst application that comes to mind is to replace traditional consumer cars,
autonomous vehicles nd use in many more elds. For example, logistics and heavy ma-
chinery could benet signicantly from such technology: if automated, everything from
agriculture to construction can become safer and more ecient. Moreover, returning to
transporting people, autonomous vehicles have found use in areas such as hospitals, air-
ports, and university campuses. Here we see vehicles of medium size designed to move
around a dozen of people. Due to their nature and design, they tend to drive at lower
speeds than traditional vehicles since the environment is usually more complex and dy-
namic than an urban road, so they require more tailored solutions. In particular, in this
scenario, pedestrians can freely move around, not limited to sidewalks, and there is no
clear separation between drivable and not-drivable areas. Therefore, the shuttle has to
navigate carefully, monitoring all obstacles that can appear at the last second. Also, the
GPS signal is not always present in such an environment, so the system cannot rely en-
tirely on it for localization and navigation.
This work focuses on the design and development of a mapping, localization and navi-
gation system for an autonomous urban shuttle. In particular, we rst aim to develop a
pipeline for mapping the environment. Using the generated map, we devise a localization
2 | Introduction

stack to track the shuttle's position. We then build the navigation pipeline on top of
it to test our setup in a real-world environment. While doing so, we confront multiple
algorithms for each task to determine the best one to accomplish the dierent tasks.
This thesis is structured as follows:

ˆ Chapter 1 describes the current state of the art for mapping, localization, and
navigation algorithms;

ˆ Chapter 2 is a collection of the key concepts that are required to understand our
thesis;

ˆ Chapter 3 explains the dierent pipelines we devised for our work;

ˆ Chapter 4 shows the results obtained on the various tasks we approached;

ˆ Chapter 5 sums up our work and lists some future developments that could be
interesting to pursue.
 3

1| State of the art

This Chapter presents the current state of the art for autonomous navigation, localization,
and mapping tasks in urban environments. In Section 1.1, we show the work done over
the years to nd solutions to map unknown areas. Then, in Section 1.2, we present the
algorithms developed to locate a robot inside a known map. In Section 1.3, we show the
currently available solutions for navigation. Finally, in Section 1.4, we will discuss the
objectives of our work in light of the current state of the art.

1.1. Mapping

Since the scope of our thesis is the autonomous navigation of a shuttle inside a univer-
sity campus, the rst step we had to take was to create a map of the area. The solution
proposed to solve the mapping task in the early 90s and still used today is Simultaneous
Localization And Mapping (SLAM). It is important to know that, as the name suggests,
SLAM accomplishes both mapping and localization. However, given that our shuttle is
designed to operate permanently in the same area, it is better to use it to output a map of
the environment. Then, use that to perform pure localization using a suitable algorithm.
So in this Section, we will discuss SLAM solutions, focusing on the resulting map.
One of the rst examples dates back to 1990 [14] when the computational power and the
available sensors were inferior to the ones available today. Here the authors explore the
possibility of creating a map using a sonar. They rst identify the problems of such a
sensor: it is impossible to create a map by hand, given that it "sees" a dierent world
from the one we experience. The proposed solution is to build a map by placing the sonar
in the various rooms that compose their environment to acquire data. The rst thing
the authors point out when making the map is that very few settings have entirely static
elements. This makes using any a priori map a delicate point to be treated carefully.
As explained earlier and presented in [21], a SLAM system can be divided into two code-
pendent blocks: localization and mapping. The former uses data from multiple sensors to
estimate in real-time the robot position. The latter instead builds a map of the surround-
ing environment, and using the information gathered by the other block, it can improve
4 1| State of the art

Figure 1.1: Image of the general architecture of a SLAM algorithm as in [21].

it through a feature-based matching system. Of course, at the same time, the localization
can improve its estimate using the map being produced. The two blocks form a contin-
uous loop, where one helps the other and vice versa. However, looking at the system's
logical architecture, we can identify what we could call a front-end and a back-end. The
rst is the one that estimates the robot's position and builds the map. This part is opti-
mized to work in real-time, and as such, it is subject to error. Here is where the back-end
comes into play. It performs loop-closure and global optimization of the map at a lower
frequency since it is heavier computationally and is not required to run at all times.
This architecture has stayed almost unchanged for the last thirty years. However, what
has changed is the approach to the SLAM problem. In particular, we can identify two
main categories of algorithms: the ones based on Bayes lters [18] and those using a
graph-based approach [8]. The former group is the rst type of SLAM solution developed
historically.
Looking for example to [7], Gmapping is one of the most used algorithms, thanks also to
its ROS implementation [16]. It uses laser data to reconstruct a 2D occupancy grid of the
environment. For the localization part, it employs a Rao-Blackwellized particle lter. As
the name implies, it works by having multiple particles on the map, each one representing
a potential trajectory of the robot. Using a probabilistic approach, at each iteration,
they are weighted against the currently available map to rene them and converge to the
correct location of the robot. Due to its light requirements in computational power and
hardware, requiring only a single-plane lidar, it has gained a lot of popularity among the
robotics community, making it the go-to solution for indoor robots that operate in not-
too-complex environments. However, its simplicity is its limiting factor since it struggles
in larger and more complex areas, where it tends to fail to do the loop closure.
1| State of the art 5

A similar approach comes from HectorSlam [12]. It is designed for even simpler robots,
usually low-power ones. As such, it relies entirely on lidar data, ignoring odometry data.
It assumes that the high operating frequency of such devices is enough to do both local-
ization and mapping through only scan matching. To do this, it employs a 3D Extended
Kalman Filter (EKF), which estimates the vehicle's state. One core feature missing from
HectorSlam is the loop closure, which, while it is not necessary for smaller spaces, is a
must in bigger ones. While this approach is denitely valid in specic scenarios, similarly
to Gmapping, it is not suited for large and complex environments.
More recently, the graph-based approach to SLAM started gaining popularity. The rst
examples are KartoSlam [13] and Cartographer [9]. These algorithms work similarly to
the ones previously described. Still, instead of keeping track of the robot's trajectory at
all times, they simplify the mechanism using graph theory. They create a graph where
the nodes are some of the robot's poses, and the edges are constraints between them.
Also, the algorithm associates notable features extracted from the lidars with each node.
This paradigm shift renders the whole architecture lighter, allowing us to obtain better
performance without high computation costs. Looking at Cartographer, it implements
KartoSlam with ulterior code built on top of it to enhance its performance. It works by
inserting new laser scans in the most recent submap. This way, scan matching happens
only against the last one available. To make the system lighter, Cartographer rejects
using a particle lter. Instead, when a submap is considered complete, it is scan-matched
against the others to perform loop closure. A constraint is added to the optimization
problem if a good match is found. This approach makes it much faster to close the loop
once the robot returns to an already-visited location.
In the following years, another graph-based algorithm surfaced, SLAM-Toolbox [15]. It
ditches the submaps approach from Cartographer: its pose-graph contains all the raw
data collected during the run. While this choice implies a more computationally intensive
algorithm, it oers several advantages. The authors' objective was to develop a system
capable of mapping vast areas, up to 100, 000f t2 . It also provides some features never seen
before in SLAM algorithms. For instance, we can run the algorithms in synchronous or
asynchronous mode. The rst will process all incoming data, regardless if it falls behind.
The second one will use only what it can to keep up and stay in real-time. While this
will produce a worse map, it is helpful for less powerful robots. Here also comes into play
another feature of it: multi-session mapping. Since, as stated before, the algorithm keeps
all row data, we can load a previously produced pose graph and run the robot through the
environment again to capture new and further information. Another groundbreaking and
never-seen-before feature is lifelong mapping. While it is still a prototype, this function
is designed to have a map we can update over time. So, instead of just adding data when
6 1| State of the art

Figure 1.2: Architecture of ART-SLAM [6]. Core modules are the mandatory blocks
of the pipeline, used to estimate an accurate trajectory and to build a 3D map of the
environment, given a sequence of input clouds.

running again, it can detect changes in the environment and update the map accordingly.
Despite these enticing features and signicant improvements, SLAM-Toolbox experiences
performance drops in large and open areas, such as our testing area. Instead, it appears
to be optimized for indeed large but indoor areas, where it can capture many features.
As such, it struggles on outdoor vehicles, where the surroundings are less characterized
and hard to tell apart.
Finally, the most recent approach is ART-Slam [6]. This algorithm distances itself from
the previous ones due to its use of 3D data. While this approach requires more data to
work, the results are outstanding, and it outperforms the other algorithms where they
struggled the most, large open areas with few features. Another core dierence lies in
the architecture itself. While dierent algorithms are usually monolithic pieces of code,
ART-Slam nds its strength in its modularity. Its components can be divided into two
groups: mandatory, representing the system's core, and optional. Since it is divided into
modules, each one can run independently and parallel to one another, allowing for better
performance on modern processors with many cores. To exploit this, ART-Slam struc-
tures its modules using the register-and-dispatch technique. Each one has many observers
that allow capturing relevant information as soon as it is available. Once it arrives, it is
put into one or more dispatch queues to be processed accordingly. After the module's core
has processed this data, it will advertise the results to others using notiers that signal
1| State of the art 7

to the concerned observers that new information is available. Furthermore, ART-Slam is

a zero-copy software, meaning no data is duplicated. As such, it allows processing large
amounts of data while keeping it in memory only once to save computational time and
space. Other than point clouds, it can accept data from IMUs, GPSs, and odometry
sources, to further improve its performance. Another critical feature of ART-Slam is how
it does loop closure. As we have seen in previous algorithms, once the size of the area
starts to grow, it becomes hard to perform loop closure. To lighten such a task, it employs
a method called Scan Context [11]. It is a technique that creates a matrix that describes
the structure of a point cloud. This approach makes the loop detection invariant to the
lidar viewpoint changes. As such, it becomes simpler to close loops, obtaining better
performance than pure scan matching. All these features make ART-Slam stand out from
the competition in our testing environment.

1.2. Localization

Once we created a map, we had to localize our shuttle to continue our work. To do so,
we explored some of the available localization algorithms compatible with ROS.
The rst one that comes by default and is used as the standard for mobile robots is
AMCL. It stands for Adaptive Monte Carlo Localization. There are many proposed
Monte Carlo approaches [3][19] in the literature. In particular, AMCL refers to the work
of [5]. Similarly to the rst type of SLAM algorithms, it employs a particle lter to track
the robot's pose against a known map. These particles simulate the posterior probability
of the estimated state and rene the lter through state prediction, weight update, and
resampling. MCL-based algorithms, however, suer some problems: particle degradation
and the kidnapped robot. The rst happens when the size of the area is vast: here, the
weight of most particles becomes negligible, while only a few gain a signicant one. The
latter happens when the robot is suddenly moved to a dierent arbitrary location. This
operation can cause many problems for an MCL-based algorithm if it has no particles
where the robot has been moved. To solve these issues, AMCL allows the user to use
selective resampling [7]. This technique reduces the resampling rate, making it happen
only when the amount of eective particles is less than half the total, countering the
particle degradation problem. Instead, it uses augmented Monte Carlo to counteract the
kidnapped robot issue. While it performs well, we could achieve some improvements in
our setup. In particular, its performance tends to suer where the map has few features.
So, we looked for alternatives.
Looking at the graph-based SLAM algorithms analyzed in Section 1.1, they can strip
down the structure and oer a localization mode with a few minor tweaks. Cartographer
8 1| State of the art

keeps its architecture the same but changes how the mapping part operates. Instead of
producing a whole map, it can be congured to keep in memory only a few of the most
recent submaps. At the same time, it scan-matches them to the previously created pose
graph and relative map to estimate the robot's position.
Similarly, SLAM-Toolbox provides a localization mode. It keeps a rolling buer of recent
scans and compares them to the already-built pose graph to add new temporary nodes
for on-the-y localization. This approach allows the underlying map to stay unchanged.
Lastly, ART-Slam uses a module called ART-SLAM LOC. It is developed as an Unscented
Kalman Filter [20]. It works by collecting linear acceleration and angular velocity data
from an IMU sensor and using it to predict the robot's motion. Then, it employs 3D lidar
data to correct the estimation and update the lter. This operation involves aligning
the lidar scan with the global map produced earlier using scan-to-map registration. This
approach makes subsequent correction steps independent of each other. This technique
makes the overall error depend only on the error distribution in the scan matching between
the current scan and the map, i.e., it depends only on the accuracy of the map itself. As
seen in Section 1.1, this is very precise and, as could be expected, translates to excellent
performance in the localization phase.

1.3. Navigation

Autonomous vehicle navigation can be performed in multiple ways. Given the complex-
ity of the system and the highly dynamic environment they travel, dierent approaches
have been developed. These dier based on the environment in which the target vehicle
will operate. In structured ones, a common practice is to compute the ideal trajectory
of the car on the road and try to maintain this course [1]. Here, obstacles, such as other
vehicles, are represented as points or bounding boxes together with the distance from the
trajectory. If necessary, the control system adjusts it to avoid collisions. This approach
works best, for example, when driving a vehicle on open roads and streets, where we can
assume that the other cars will move in the same direction. On the contrary, it does
not fare well in environments like university campuses, such as our own, where obstacles
are much less predictable. So, for unstructured environments, the occupancy-grid-based
approach is the most popular [10]. It works by using a grid-like structure, where each cell
represents a small area, for example, 5x5cm, and its value says if it is free, occupied, or
unknown.
An example of this approach is move_base, the de facto standard navigation tool in ROS.
It rst takes an existing map to understand the static elements present and plot trajec-
tories around them. Then, while the vehicle moves, it uses the available lidars to add
1| State of the art 9

newly found obstacles to the occupancy grid. It uses a particular implementation called
Costmap2D. Here, it assigns each cell a cost to traverse it, usually dependent on the
distance from an obstacle, where the more we get near, the higher it grows. The planners
then use this information to create a trajectory using algorithms such as A* [22] or Di-
jkstra [4] to nd the optimal path from start to nish. Also, move_base uses a modular
architecture; for example, users can change the planners freely. This feature allowed us
to include a local planner specically designed to control vehicles like ours. All in all, this
solution proved to be adequate for our use case.

1.4. Thesis objectives

In light of what we discussed in the previous sections, in this thesis, we will rst try to
produce a good map of the environment we are using for testing. Once this is done, we will
test the presented localization algorithms to nd the best for our objective. Finally, we
will deploy the chosen one to do autonomous navigation tests using the available vehicle.
 11

2| Background

In this Chapter, we describe the tools and concepts necessary to comprehend the work
done in this thesis. In Section 2.1, we present the ROS system and its main components.
Then, in Section 2.2, we present the vehicle used in our work, the sensors it mounts, and
how to control it. In Section 2.3, we explain the concept of odometry and how it relates
to our tests. Next, in Section 2.4, we describe the particular kinematics that characterizes
our vehicle. Lastly, in Section 2.5, we explain the concept of SLAM and its purpose in
our work.

2.1. The ROS system

The Robot Operating System (ROS) is a set of libraries and tools designed to help
when working with robots. It is based on UNIX systems and can be used in multiple
languages, the most used are C++ and Python. Here we describe the main component
we used in our work.

2.1.1. roscore
To start the ROS system, we have to call the roscore command in a terminal. This
input spawns three processes, a ROS master, the parameter server, and the rosout node.
The master is responsible for providing names, keeping track of all nodes, and knowing
all publishers and subscribers present. The parameters server collects and distributes
parameters to all nodes. We will discuss it better in Section 2.1.7. Lastly, the rosout
node handles all the logs produced by the various nodes. Without having called roscore,
it is impossible to launch any node, and closing it will shut down all the ones running.

2.1.2. Topics
The communication in ROS is based on the concept of topics: they are buses where
messages are exchanged. They are anonymous and work on the publisher-subscriber
paradigm: the communication is designed to be unidirectional, so for each topic, anyone
12 2| Background

interested in a particular piece of information will subscribe to it; at the same time, those
with such data will publish on it. It is then essential to standardize the messages.

2.1.3. Messages
In ROS, there are dierent kinds of messages designed for all sorts of situations. Each
topic will work with a single type of message, so those who publish must use this kind,
so subscribers know that data will always come in the same form. ROS is also very
exible, allowing you to dene your messages according to your necessities. An example
of a message we use is the Pose message: it is made of two other messages, a Point called
position and a Quaternion called orientation. position contains the x, y, and z coordinates
of the robot, while orientation denes where it is looking as a quaternion.

2.1.4. LaserScan and PointCloud2 messages

Two particular kinds of messages that are featured prominently in our work are Laser-
scan and PointCloud2. Light Detecting And Ranging (LIDAR) sensors use these kinds
of messages. They work similarly to the most known radars. While a radar works by
sending sound waves and analyzing the time it takes to return to the sensor, a lidar
does the same using a laser. Its advantage is that employing light is much more precise
and has more resolution than sound. Laserscan messages dene a single plane of points.
They are structured as such: there are three elds called angle_min, angle_max, and
angle_increment, which describe the amplitude of measurement and the precision. Then
in the array ranges, we nd for each point the distance measured. The PointCloud2 mes-
sage is designed for sensors with multiple scanning planes and has a more straightforward
structure. Two elds, height and width, dene the dimension and the number of points
in the cloud. Then we have the array data, which contains the XYZ coordinated of each
measured point, plus eventual extra information provided by the sensor.

2.1.5. Publishers and subscribers

As stated in Section 2.1.2, ROS is based on the publisher-subscriber paradigm. These
two building blocks are essential for all communication. As such, almost any node present
in ROS will have at least one of each, if not more. A subscriber works by rst setting
the topic we want it to listen to and then choosing the length of its queue. This setting
tells the subscriber how many messages to keep if one arrives while it is still processing
an old one. Of course, this is an undesired case since once the queue is complete, it will
throw away older messages in favor of newer ones. At the same time, we must tell the
2| Background 13

subscriber what to do once a message is received. We do this by giving it a callback. It is

a function that the subscriber will execute each time it has a new message. This callback
takes as a parameter a ROS message. This implies that it can only receive one type of
message, so the topic it is subscribed to must be of that kind. Here we will put most
of the logic of the node. While processing the information we receive, we may want to
broadcast what we obtain to other nodes. This is the role of the publishers. A publisher
is declared similarly to a subscriber by setting the topic it will publish on and the length
of its queue. Here it is used in case we send too many messages quickly and the publisher
can not keep up. Again, this is an undesired case and should be avoided since it can lead
to data loss. Then, instead of setting the callback, we have to set the kind of message it
will publish. This is done to check that the chosen topic already works with this type of
message since there cannot be dierent kinds in one. Also, when using this publisher, we
must ensure that we give it the same type we declared at the start.

2.1.6. Packages, nodes, and launch les

In ROS, each piece of software is a node, and they are organized in packages. This
structure is handy since to run a node; it is as simple as opening the terminal and using
the rosrun command, followed by the name of the package that contains it, plus its name.
ROS also oers launch les. These are text documents written in XML designed to run
multiple nodes by calling them. Furthermore, it is possible to set their parameters inside
here for each one. Since writing many parameters for each node can still be tedious in
XML form, these can be written in external .yaml les and imported here. These les are
handy since they are just lines of parameter-value pairs. Launch les are collected inside
packages, as nodes do. Similarly, to run one, we have to open the terminal and call the
roslaunch command, followed by the package's name and the launch le's name.

2.1.7. Parameters
Parameters are another staple of ROS. This feature allows nodes to be very exible.
One of the modules of the ROS master is the parameter server. It acts as a shared dictio-
nary available to all nodes present on the ROS network. Of course, as any introductory
programming course teaches, having global variables and parameter is a bad practice.
Here parameters are stored in a folder-like system, accessed by name. As a standard
practice, parameters are kept under the name of each node. This way, there is no risk of
dierent nodes overwriting each other's parameters. For example, if I had a node called
controller with a parameter called max_speed, I would access it by requesting /con-
troller/max_speed. Furthermore, the exibility of parameters does not stop here. There
14 2| Background

is another tool available in ROS called dynamic_recongure. Usually, parameters are read
once when the node is initialized and then stored in memory. The dynamic_recongure,
as the name suggests, allows changing parameters at runtime through a handy interface
without stopping the node to edit some le. While at rst, this may seem just a com-
modity to save time when changing a few numbers, it is much more. When using more
complex algorithms, these can not be expected to work straight out of the box, so some
tuning is always required. In this case, the number of parameters reaches the tens or even
the hundreds. So, having to recompile each time we change a single number, when it may
take even minutes, would be too much of a waste of time and make its use impractical.

2.1.8. TFs
One of the core components of ROS is the tf system. When working with robots,
there are many sensors and moving parts, and we want to know where they are in space
relative to each other. The tf package handles just that. To dene the link between
two coordinate systems, we can use a TransformStamped message: it contains a header
with the timestamp and the frame_id, a string called child_frame_id, and a Transform
message. The latter has the actual rotation and translation from one system to the other.
frame_id says from where to apply the transformation, and child_frame_id which system
is at the resulting location. For example, when doing autonomous navigation, if a lidar
sees an obstacle, the system must know the tf between that sensor and the base of the
vehicle. It then can calculate where it is relative to the center of the shuttle and plan
accordingly to avoid it. We can broadcast TransformStamped messages on the /tf topic
to publish such information. We can do it in two ways: manually, when we write our own
nodes, or automatically, running tf_static_publisher nodes. These are designed to be
run from a launch le, and they are static. This method is usually used when expressing
static transformations, such as the ones between dierent components of our shuttle. In
ROS, the tf is structured as a tree. As such, there are some rules to follow when using this
system. First of all, no frame can have more than one parent. Secondly, there cannot be
loops inside the tree. These rules are in place to avoid impossible and conicting scenarios.
An adequately structured tree guarantees that ROS works correctly when displaying data
or moving from one frame to another.

2.1.9. Rviz
Rviz is a tool available in ROS to visualize data. It is essential in our work since it
can display all the dierent data we are working with, like LaserScan and PointCloud2
messages. Of course, it also implements the tf library, allowing us to see all data from
2| Background 15

Figure 2.1: Image of the architecture of move_base taken from http://wiki.ros.org/

move_base.

the dierent available frames.

2.1.10. move_base package

move_base is a package oered by the standard ROS library and is part of the nav-
igation stack. It is the go-to tool when autonomously driving a robot. It comprises
ve modules: local_costmap, global_costmap, local_planner, global_planner, and recov-
ery_behaviors. The global_costma p takes data on obstacles from a preconstructed map,
while local_costmap takes it from the sensors available. As the name implies, the two
planners are responsible for plotting the track to the destination: the global one makes
a plan from start to nish, while the local one produces smaller plans to follow the main
one. The recovery_behaviors module takes control when the robot is under challenging
spots, and the planners fail. The outstanding feature of move_base is its modularity: the
planners are easily swappable with custom ones, as long as they are written according to
spec. This feature is handy since most local planners are designed with dierential-drive
or omnidirectional robots. Still, our shuttle is car-like and, as such, has a more com-
plicated geometry, which is incompatible with the ones mentioned before. However, the
solution is to use a planner designed for this architecture.

2.1.11. Bags
Another handy feature of ROS is bags: you can freely record messages sent on some
or all topics and play them back at a dierent time. In our case, it allowed us to record
all relevant data while driving around with the shuttle. Then, at a later time, we could
work and analyze such data without the need to be on the shuttle at all times.
16 2| Background

Figure 2.2: Image of the experimental vehicle employed for the data acquisition and the
real test.

2.2. The shuttle

The EZ10 shuttle, shown in Figure 2.2, is an electric vehicle designed to operate in
various environments like campuses, parks, and universities. By default, it comes with its
own self-driving system. However, we decided to use it as a tool to implement a dierent
version of autonomous navigation based on ROS. It hasvarious sensors designed to help
in this task, mainly laser scanners, lidars, a GPS, and cameras.

2.2.1. Sensors
All the sensors mounted on the shuttle are connected through a network switch to the
central computer on board. They all have drivers to communicate their data directly on
the ROS network.

IMU
The Inertial Measurement Unit (IMU) is a fundamental sensor that can be found almost
anywhere, to the point that all phones nowadays have one. It relays data on the linear
accelerations and the angular velocities it is subject to at all times, with a very high
frequency. Some also provide additional information, like ours, on the electromagnetic
elds they perceive. Our model is a Xsens MTI-30AHRS.
2| Background 17

(a) Image of the IMU sensor mounted on the shut- (b) Image of the GPS sensor mounted on the
tle. shuttle.

GPS
Another sensor found on many devices is the GPS. Of course, it is a handy sensor in
most scenarios where the task is robot localization. However, as we will explain later in
the Chapter 4, it is not reliable in our environment, so our navigation system will not
be based on it. Still, we will use it in the experimental phase to confront the dierent
localizations with the ground truth given by the GPS when it is accurate enough. Our
model is a Swiftanv Pixi.
18 2| Background

Figure 2.3: Image of the cameras mounted on the shuttle.

Cameras
The shuttle mounts two cameras, one on the front and the other back. The model is
the Point Grey Blackies, a black and white camera. We did not use them in our work,
but in the last years, there have been some Visual SLAM solutions, so they could be
helpful for future work.

Lidars
There are eight lidars on the shuttle, of three dierent kinds.
2| Background 19

(a) The SICK LD-MRS sensor.

(b) What the sensor sees.

Figure 2.4: This is the SICK LD-MRS sensor and an example of what it sees.

ˆ SICK LD-MRS: There are two SICK LD-MRS lidars mounted on the sunroof. They
are sensors designed for long-range, up to 250m. It has four scanning planes, with a
horizontal eld of view (FOV) of 110° and a vertical of 3.2°. Their placement makes
them suitable for the localization task since, from their mounting point, they mainly
see buildings and static obstacles while not seeing people and vehicles. They use
the PointCloud2 message to transmit their readings.
20 2| Background

(a) The SICK LMS-151-10100

sensor.

(b) What the four combined sensors see.

Figure 2.5: This is the SICK LMS-151-10100 sensor and an example of what they see.

ˆ SICK LMS-151-10100: There is a SICK LMS lidar in each of the four corners of the
shuttle. It is a short-range sensor, up to 50m, with a horizontal FOV of 270°. It also
has a single scanning plane. All these features make it more suitable for obstacle
detection than for localization. Being a single scanning plane sensor, it uses the
LaserScan message to relay its data.
2| Background 21

(a) The Velodyne VLP-16 sensor.

(b) What the sensor sees.

Figure 2.6: This is the Velodyne VLP-16 sensor and an example of what it sees.

ˆ Velodyne VLP-16: There are two VLP-16s mounted each on the front and the back,
near the two cameras. This sensor has a potential horizontal FOV of 360°, although
its placement restricts it to 180°; the vertical FOV is 30°. It sees up to 100m and
has 16 scanning planes. It is a very versatile sensor used for obstacle detection and
localization after some data preprocessing. Similarly to the SICK LD, it uses the
PointCloud2 message.
22 2| Background

2.2.2. Control
The shuttle has two operating modes: manual, where it is commanded through a
joystick present inside the vehicle, or automatic, where it is controlled by sending messages
through the onboard PC. We are interested in the second mode. In particular, the shuttle
has a complete control and feedback system that works on the Controller Area Network
(CAN) protocol. It is only essential to know that it works based on the exchange of
frames (i.e., messages) on a serial bus. The shuttle knows the information we send based
on each message's identier (ID). For example, to send a command to drive, we have to
put information on the wheels' desired speed and steering angle in a message with ID
0x193.

2.3. Odometry

Odometry is the use of sensor data to estimate the change in the position and orien-
tation of a robot in space. This process works by taking subsequent data from motion
sensors, like wheel encoders, and integrating it to reconstruct the road traveled. This
operation, of course, is not perfect. For example, a wheel encoder at certain intervals
sends information about the speed at which it is spinning. So, if the last message received
states the velocity is 2m/s, and the previous one came 0.5s earlier, the result is that the
robot has traveled 1m between the two. Of course, this operation assumes that the wheels
kept this speed constant the entire time between the two messages, which is not always
this way. Moreover, encoders only measure the number of rotations. Then, knowing the
radius of the wheels, they can compute the speed. Of course, they are subject to usury,
so the radius of a wheel may change after a while, so the encoders' estimate can drift.
Knowing the speed of the wheels is not enough, however. Dierent vehicles have dierent
kinds of geometry, so knowing the type of robot we are working on is necessary. Here is
a list of the main kinematics:

ˆ Dierential drive: this is a robot with only two wheels, each has its motor. It
can move back and forth by spinning both simultaneously, while it can turn by
actuating only one of the two. If both spin in opposite directions, it will rotate in
place. This one is the simplest and most common model due to its versatility and
ease of controlling it.

ˆ Synchronous drive: this kind of robot has at least three wheels but always two
motors. One is responsible for turning all the wheels while the other rolls them. It
can move in any direction without having to rotate in place since the wheels can
2| Background 23

just align in the desired orientation.

ˆ Omnidirectional drive: this robot uses a particular kind of wheel called the Swedish
wheel. It has rollers on its surface, allowing it to roll sideways. This robot has at
least three of them, with a motor each, capable of moving back and forth, sideways,
and rotating in place. It is the most versatile kind of kinematics available.

ˆ Tricycle drive: these robots are typical of industrial and warehouse machines. It
has one central actuated wheel and two additional passive ones.

ˆ Skid steering drive: these robots are the ones with tracks. They have a motor
for each side, and the wheels cannot steer. This constraint translates to having to
actuate the two sides at dierent speeds to obtain a rotation. They need proper
calibration and complex models, with parameters that change depending on the
kind of surface the robot is moving, so they are hard to control.

ˆ Ackermann drive: this model describes vehicles with four wheels, such as cars. As
a consequence, it is rarely found on robots. We will explain it more accurately in
Section 2.4 since it is the one of our shuttle.
24 2| Background

Figure 2.7: Image of the Ackermann model.

2.4. The Ackermann model

Most four-wheeled vehicles on the planet, including our shuttle, use the Ackermann
steering model. It was designed to avoid slippage of the wheels when turning by applying
a dierent rotation to each wheel. The formulas that describe how it works are:

d
R= +b (2.1a)
tan αR
ωd
= VF R (2.1b)
sin αR
ωd
= VF L (2.1c)
sin αL
d
αL = arctan( ) (2.1d)
R+b
ω(R + b) = VBL (2.1e)
ω(R − b) = VBR (2.1f)
2| Background 25

Figure 2.8: Image of the Ackermann model with the bicycle approximation.

A simpler model, called the bicycle approximation, considers only a virtual central wheel.
This way, the formulas become:
d
R= (2.2a)
tan α
ωd
= VF (2.2b)
sin α
ωR = V (2.2c)
sin α
ω = VF (2.2d)
d

2.5. SLAM

Simultaneous Localization And Mapping (SLAM) is the process of mapping an un-

known area while keeping track of the robot's pose inside of it. It works by producing
a map slice, then iteratively doing the localization and updating the map with each new
piece of data. Of course, this technique has many dierent implementations, from particle
lters to Kalman lters to the most recent graph-based approaches. We used many dier-
ent versions to produce a map of our environment in our work. There are dierent kinds
26 2| Background

of SLAM also based on the type of sensors used. While our thesis used ones that depend
on lidars, some solutions also work with cameras. Some versions allow the use of known
landmarks to ease the SLAM process. As the name suggests, such algorithms have two
main components: localization and mapping. The rst collects data from sensors such as
lidars and extracts notable features. It then tries to match them with already available
ones to estimate the robot's position. At the same time, the mapping part takes the
features that were not recognized and processes them to update the map with new infor-
mation. The two systems exchange data to achieve better results, as seen in Figure 1.1,
where there is an example of a SLAM system architecture.
 27

3| Nodes

This Chapter will explain what ROS nodes we used for each part of our work. In Sec-
tion 3.1, we list the nodes necessary for every operation in our thesis, and as such, are
always present, whatever the task. Then, in Section 3.2, we present the nodes used in
the mapping task, divided by algorithm. In Section 3.3, the ones used for localization
are again organized by the method. Finally, in Section 3.4, we list the nodes used in the
navigation task.

3.1. Common nodes

3.1.1. Odometry
The rst step to any operation we will do is to calculate the odometry of our shuttle,
which means keeping track continuously of where it is and where it's headed. We can do
this by using the information on the steering angle of the wheels and their linear speed.
For our vehicle, we use the Ackermann steering model with the bicycle approximation,
which describes the relationships between linear and angular velocity and the steering
angle of wheels for car-like vehicles. Given the information we have, we can rst calculate
the radius of the rotation using Equation (2.2a). With that, we can now nd the angular
speed of the vehicle using Equation (2.2d). The odom_gen node is responsible for this
task. It listens to the topic /speed_steer for the data about speed and steering. This
information comes as a Vector3 message, with speed expressed in m/s in the x eld
and steering angle in the y eld in radians. The z eld contains the total kilometers
traveled by the shuttle, which is useless to us. Each time a message is received, we have
to integrate this information to update the pose of the shuttle. We can do this in three
ways: using either Euler's or Runge-Kutta's approximation or doing the exact integration,
28 3| Nodes

Figure 3.1: A sample trajectory produced by the EKF.

shown respectively in Equations (3.1), (3.2) and (3.3).



 xk+1 = xk + vk TS cos θk


 yk+1 = yk + vk TS sin θk

(3.1)


 θk+1 = θk + ωk TS


TS = tk+1 − tk


ωk TS


 xk+1 = xk + vk TS cos(θk + )


 2
ωk TS


yk+1 = yk + vk TS sin(θk + )

2 (3.2)

θk+1 = θk + ωk TS






TS = tk+1 − tk


 vk
 x k+1 = x k + (sin θk+1 − sin θk )
ωk



vk


k+1 = yk − (cos θk+1 − cos θk )

y
ωk (3.3)

θk+1 = θk + ωk TS






 TS = tk+1 − tk
We can choose which method to use by setting a parameter before launching the node.
Once we calculate the new position and orientation of the shuttle, the node publishes such
state on the topic /odom, in the form of an Odometry message. However, this odometry is
subject to error due to sensors drifting over time. We rst try to minimize it by using an
3| Nodes 29

Extended Kalman Filter (EKF) [17]: it is a state estimation tool that can fuse dierent
data sources to make a more accurate estimate of the system's state. It rst makes a
prediction using the following formulas:

µ̄t = At µt−1 + Bt ut
(3.4)
Σ̄t = At Σt−1 ATt + Rt

The result then gets corrected using the following formulas:

Kt = Σ̄t CtT (Ct Σ̄t CtT + Qt )−1

µt = µ̄t + Kt (zt − Ct µ̄t ) (3.5)
Σt = (I − Kt Ct )Σ̄t

In our case, we feed it with the odometry we just calculated together with data from
the IMU for angular speed and linear acceleration. Doing this can't completely x the
drifting since the IMU is also subject to it, but it should still reduce it. In Figure 3.1, we
can see an example of the trajectory computed by the EKF. Clearly, this result, which
should be a complete lap, is far from it.

3.1.2. TF
As stated before in Section 2.1.8, TFs are a core part of ROS, and so in our thesis.
Our TF tree is structured as follows: the center of the shuttle is base_link, then there is
a frame for each part connected to it. It contains transformations to indicate the position
of all the dierent sensors relative to the center of the shuttle. Viewed in 3D, it appears
as in Figure 3.2. Beyond these static TFs, our work often presents two frames: map
and odom. The latter is used to keep track of the robot's position. Usually, the EKF
publishes the transformation from odom to base_link. Instead, the localization algorithms
are responsible for the TF from map to odom. This transformation is used to correct the
drift accumulated over time in the position estimate.

3.2. Mapping

Most mapping algorithms operate in 2D, meaning they project everything on the XY
plane for simplicity, which works well in most cases. We tried many algorithms with
varying degrees of success.
30 3| Nodes

Figure 3.2: Image of the TF tree of the shuttle.

3.2.1. Gmapping
The rst one we used was Gmapping. It uses a Rao-Blackwellized particle lter to
solve the SLAM problem.It works by taking in input from the topic /scan laser scan
data and using it to produce a 2D map of the surrounding environment iteratively, which
it outputs on the /map topic. However, this is not directly compatible with our setup,
where eight lidars are available. At rst, we tried to fuse all the sensors to produce a single
laser scan. We rst have to convert point cloud messages from the VLP-16 and SICK-LD
sensors to laser scans, which means projecting them on a single scanning plane. So we
used the laserscan_virtualizer node from the IRA_laser_tools package, which takes point
clouds from a topic and outputs them as laser scans on another one, with the possibility
of placing them in a dierent frame; we exploit this to put them in the same frame
as the center of the robot. We then use the other node available in IRA_laser_tools,
laserscan_multi_merger, to fuse them into a single one. This approach, however, proved
not to be the best one. Since the four sensors on the corners and the two in the front
and the back see all obstacles at road level, the resulting map tends to vary greatly. In
turn, this means that when it comes to localization, if done on the same dataset used
in making the map, the performance would be good, but when done on dierent data,
it would handle poorly. To solve this, we decided that instead of fusing all the sensors,
3| Nodes 31

(a) Map produced using all lidars. (b) Map produced using only the top
mounted lidars.

Figure 3.3: In the image on the left the silhouttes of cars parked around the area are
visible, while they don't appear on the right. Also the east section has a completely
dierent aspect.

it would be best only to utilize the two sensors mounted on the sunroof. Since they see
obstacles only from around 2.5m and upwards, they tend to capture only the surrounding
buildings, not the people and other vehicles. With this approach, as expected, the map
produced would only contain static elements such as buildings, making it more suitable to
be used over dierent datasets. Gmapping also uses information on odometry by reading
the /tf topic, so the odom_gen node and the local EKF are running and publishing such
information as a TF from odom to base_link.

3.2.2. Hector Slam

Hector Slam functions the same way as Gmapping, by reading laser scan data from the
/scan topic. So we used the same setup as before: we fused only the data from the two
lidars on top. One thing to note is that it is the only algorithm to work without odometry
information. As such, it does not require running the odom_gen node and the EKF.

3.2.3. Cartographer
Cartographer is the rst node to provide support for multiple kinds and multiple
sources of data. However, this still did not change our setup since it had some di-
culty synchronizing the data when using the two sources, so we opted to keep fusing them
as laser scans.
32 3| Nodes

Figure 3.4: Architecture of the nodes used for mapping and localization with ART-Slam.
Red rectangles represent odometry sources, blue ones lidar data, green are nodes that do
data preprocessing, and orange are the core of the setup.

3.2.4. Slam Toolbox

The same situation still applies to Slam Toolbox, which again oers the possibility to
use multiple sources, but also has some timing issues, so our setup stayed the same.

3.2.5. ARTSlam
We had to make a few changes when switching to using ART-SLAM. Since it operates
on 3D data, unlike the precedent algorithms, using only the two sensors on the sunroof
would not provide enough points. So, we decided to fuse data from all available lidars,
this time producing a single point cloud, not a laser scan as before. To do this, we
rst joined the four laser scan messages from the corner sensors. At the same time,
we ltered for points nearer than 5m since some of them were not tuned correctly and
recorded the shuttle itself as an obstacle; we did this whole operation again with the
laserscan_multi_merger node. Following this, we had to convert it into a point cloud
message. We wrote a custom node that handles this process, called laser_converter.
This node uses the laser_geometry library oered by ROS to create a LaserProjection
object, which has a method to do what we need. At this point, the last thing left to
do is fuse the now ve sources of data into a single one. Since the sensors operate at
dierent frequencies, it was opportune to sync them before the fusion. To do this, we
wrote another node called pcl_assembler. It works by creating a Synchronizer object
from the message_lters library. It is designed to use a policy of choice to synchronize
messages coming from dierent topics and group them to pass along to a callback. This
3| Nodes 33

callback takes each point cloud, transforms it to a common frame using the available tfs,
and nally combines them into a single one and publishes it on the /cloud topic.

3.3. Localization

Localization uses similar setups to the ones used in the earlier mapping stage. The
main dierence is that we always have to add the gps_to_enu and comparator nodes we
wrote. The rst one takes the raw readings from the GPS on the /swiftnav/front/gps_pose
topic, expressed as latitude, longitude, and altitude coordinates. These are rst l-
tered to remove invalid and wrong readings depending on their covariance. After this,
using a xed location we chose as a zero, it converts it into local coordinates using
the xEast, yNorth, zUp (ENU) convention. It then publishes it to the topic /swift-
nav/front/gps_pose/converted as a Pose message. The comparator node then listens for
both this and the /odometry/global or /pose topic, where the EKF or the localization
algorithms will publish the estimated location of the shuttle. We use this data in Chap-
ter 4 to show the result of this task. Also, it is necessary to add a new TF between the
frame map and a new one that we call truth. We do this since the map used in some
algorithms is not aligned with the real world, and using this, we can correct this issue.
We calculated the correct value of this TF by graphically trying to overlap the GPS track
with the shuttle's trajectory. We wrote a node called dynamic_tf_publisher to change at
runtime the published TF and nd the correct value.

3.3.1. AMCL
AMCL is an algorithm that uses the adaptive Monte Carlo localization approach. It
uses the same setup as Gmapping but with two additions. The rst is another EKF node,
which takes the same input as the rst one, plus the pose that AMCL produces. It corrects
the estimate of the shuttle's position by publishing the corrective TF from map to odom.
We did this because the algorithm sometimes may jump to the wrong location. To handle
this, we modied the code for the robot localization and implemented a chi-squared test
on the input to lter out this behavior. The second addition is the map_server node
from the package of the same name, responsible for publishing the map on which AMCL
will then make the localization. The primary AMCL node then functions similarly to
Gmapping. It reads the available odometry information from the /tf topic, particularly
the transformation from odom to base_link. At the same time, it expects lidar data
on the /scan topic. It then has a few dierent outputs. It can publish the corrective
TF from the map frame to odom. However, we disabled such feature since, as stated
34 3| Nodes

before, we let the second EKF handle this task. AMCL also outputs on the /particlecloud
topic the position of each particle it is using in the lter on the map. Finally, on the
/amcl_pose topic, it sends the information about the estimated position of the shuttle as
a PoseWithCovarianceStamped message, which is the input we forward to the EKF.

3.3.2. Slam Toolbox

Slam Toolbox also oers a localization feature. It uses the pose-graph produced in the
mapping stage to estimate the shuttle's position. The setup is the same as before since
the algorithm is the same. It reads the laser scan data from the /scan topic. Its only
output is the corrective TF from map to odom. To be able to confront it with the GPS,
we wrote a node called pose_from_tf, which, reading the /tf topic, outputs the position
of the base_link frame relative to map as a Pose message.

3.3.3. ArtSlam
ART-SLAM similarly has a localization mode that uses the map produced earlier
instead of the pose graph, which we saved as a single point cloud in a .pcd le. As for
Slam Toolbox, here we have to add the pose_from_tf node, to have a Pose message we
can use for the comparator.

3.4. Navigation

The navigation pipeline we developed to test the algorithm is built on top of the
AMCL localization stack. We used the move_base package oered by ROS. To interact
correctly with move_base, we rst had to apply some preprocessing to our data. The
rst one concerns the point clouds coming from our lidars. In particular, the two VLP-
16s intersect the ground plane due to their mounting position, which is considered an
obstacle. To do this, we pass the data through a node we wrote called pcl_normal_lter.
It uses a dierence of normals lter to remove the ground without losing the sidewalks.
We have to do this since a simpler height-based lter does not suce in our setup. The
road in our environment is uneven, so there are a few humps. These create two problems:
the rst is that when approaching them, they show up as obstacles, and the second is
that when going over them, the shuttle tilts enough to see the ground in front as an
obstacle. Our approach works by rst organizing the incoming cloud: this is crucial since,
in the next step, we will have to look for the neighboring points for each one, and having
them spatially sorted drastically reduces the computational time required to do so. Once
3| Nodes 35

Figure 3.5: Architecture of the nodes used for navigation. Red rectangles represent odom-
etry sources, blue ones lidar data, green are nodes that do data processing, purple broad-
cast the maps, and orange are the core of the setup.

the cloud is organized, we use the NormalEstimation class from the PCL to estimate
the normal vector to each point of the cloud. Then we instantiate an object from the
DierenceOfNormals class to estimate the surfaces captured by the cloud. At this point,
we can apply a conditional removal of all points that belong to an upward-facing surface.
This technique may lter out other horizontal surfaces beyond the ground. However,
this is not a problem in our setup, and it never presents, given the placement of the two
sensors. Indeed, they are at a height of 55 cm from the ground: at this level, there are
few to no obstacles in the university campus environment. So, the probability of having
an obstacle below this level, which has a at horizontal surface, is almost zero and has
never shown up in our testing. The result of this operation can be seen in Figure 3.6. The
following preprocessing we had to do was to address this problem again, but for the four
angular sensors. Since they are all single-scanning plane ones, we can not use the same
approach, but instead, a simple distance-based lter works great. We lter out all points
further than 5m. These sensors work to provide close-by obstacle detection, so beyond
such distances, they do not oer valuable information. From our testing, this did not
cause any safety concerns, particularly in the smaller corridors, where they still managed
to identify the presence of walls on the side of the shuttle, where the other sensors could
36 3| Nodes

(a) The point cloud before ltering.

(b) The point cloud after ltering.

Figure 3.6: This is an example point cloud before and after applying our the dierence of
normals lter.

not provide information. With all this done, we can feed all the data to move_base,
making sure to set the two top sensors to only mark obstacles and not to remove them
since they don not see obstacles below 3m, but still provide helpful information on distant
elements. For the global and local planners, we use GlobalPlanner, one of the default ones
from ROS, and RSBand, a custom one designed with car-likes vehicles in mind. Since it
respects the move_base standards, it outputs commands as linear and angular speeds,
but it is incompatible with our setup. For the global costmap, we provide move_base the
map produced by Slam Toolbox. We had to tweak the map since the original had many
sections with open borders, and the planner would try to pass through those openings.
So, we drew some black lines to close such spaces. As stated in Section 2.2.2, the shuttle
is controlled through a message containing the wheels' linear speed and steering angle.
To calculate it, we use the Ackermann model as before, so from Equation (2.2d), we can
3| Nodes 37

obtain:
ωd
α = arcsin (3.6)
VF
This calculation is done in the speed_controller node. Moreover, the shuttle requires
driving commands to be sent rigorously at 50Hz. It also limits the linear acceleration
and the speed at which the steering wheels are moved. Again, move_base does not
consider such things, and it outputs commands at 10Hz. So, our speed_controller node
also handles such issues by keeping track of the last message sent and the last one received
and carefully adjusting the next one that it will send to respect these limits.
 39

4| Results

This Chapter presents the results obtained from the dierent tasks we approached. First
of all, in Section 4.1, we introduce the operating scenario of the shuttle and the relative
challenges. In Section 4.2, we confront the dierent mapping algorithms while showing the
maps produced. Then, in Section 4.3, we analyze the performance of multiple localization
algorithms in our setup. Lastly, in Section 4.4, we show a navigation test we conducted
in the same environment.

4.1. The environment

We deployed the shuttle in the Politecnico di Milano, Bovisa campus to test our dif-
ferent pipelines for mapping and localization. The site has many challenging sections,
both for mapping and localization. The area resembles a squared 8-shape which mea-
sures around 180x70m. The rst problem is the campus's sheer size, which is taxing on
simpler algorithms, such as Gmapping and HectorSlam. A problem common to mapping
and localization is the environment: it comprises narrow one-way corridors with very few
features and large and wide open areas. These problems make it hard to reconstruct
such sections correctly and localize the shuttle while keeping a low error. Examples of
such places are the east corridor, which has near to no features, and the west section,
where the lidar has no returns. A problem that mainly aicts the localization task is
the many overpassing arches throughout the area. As shown in Figure 4.2a, these make
the GPS lose its signal, so localization cannot rely solely on it. Moreover, after passing
these sections, the GPS also takes some time to reconnect to the RTK corrections. This
delay creates areas where the GPS is indeed present but not accurate. Taking a closer
look at Figure 4.2, we can see that around the overpasses, the trajectory is irregular and
not close to reality. Figure 4.2b shows the part of the trajectory where the GPS has a
stable RTK connection. Of course, if we consider almost constant the time to reacquire
the connection, it is clear that driving at higher speeds would reduce even more the area
covered by the RTK corrections. This problem also slightly impairs the tests we can do
to evaluate the performance of the algorithms.
40 4| Results

Figure 4.1: Image of the Bovisa campus of Politecnico di Milano.

The combination of lidars we used gives us a 70° blindspot on each side of the shuttle.
This fact makes it hard on simpler algorithms when doing scan matching in corners or
while turning, where most data comes from the sides of the vehicle.

4.2. Mapping

For mapping, we evaluated most of the available solutions in ROS. As a means of

comparison, we both confronted the map produced with the satellite data from Google
Maps and measured the distance from the GPS while the algorithm was running. As
stated before, the GPS only covers some sections of the campus, so the error calculated
in such areas is not meaningful.

4.2.1. Gmapping
Gmapping is the rst solution we deployed. It is a simple tool that works well in
most common applications but has diculties in more complex environments like ours.
As shown in Figure 4.4a, it can handle creating a map for a subsection of the campus.
However, this result was not easy to accomplish and is not consistent, requiring a lucky
run to get it. This problem is partly due to the fact that the odometry we produce is
not very accurate. In Figure 3.1, we show an example of the trajectory calculated by
the EKF. This result shows a lot of accumulated error and many meters of oset from
4| Results 41

(a) Image of the part of trajectory where the (b) Image of the part of the trajectory where the
GPS has an accuracy between 0.2m and 2m. GPS has an accuracy below 0.2m.

Figure 4.2: Images showing the how the accuracy of the GPS changes along the track.

Position error of tested SLAM algorithms during the mapping phase

14
Cartographer
12 slam-toolbox
ART-SLAM
10
Distance (m)

0
0 100 200 300 400 500 600 700 800 900
Time (s)

Figure 4.3: Position error of three graph-based slam algorithms during the map creation
phase. Due to the challenging scenario and the limited number of possible loop closure
Cartographer and slam-toolbox accumulate large error, while ART-SLAM maintain a low
position error.

the starting position. Unfortunately, it is impossible to improve without using GPS or a

localization algorithm. As such, the loop-closing algorithm used by AMCl most times is
not sucient to x such a drift. This fact also reects when trying to map the entire area:
in this case, Gmapping fails to produce a map. As shown in Figure 4.4b, at a certain
point, due to the size of the environment and the lack of features, new scans are placed on
top of each other since it struggles to do scan matching and the odometry is not accurate
enough to supply for it.

4.2.2. HectorSlam
Seeing how Gmapping only half-succeeded in the task, we envisioned HectorSLAM
only to perform worse. As stated in Section 3.2.2, this algorithm does not use odometry
42 4| Results

(a) The map produced on a subsection of the (b) The map produced when trying to map the
area. entire area.

Figure 4.4: The maps produced by Gmapping on a subsection and on the entire area.

Figure 4.5: Image of the map produced by HectorSlam.

information, relying only on subsequent scans. While very eective in enclosed spaces
rich with features, this approach is counterproductive in our scenario. As can be seen
in Figure 4.5, since corridors on the campus are not very distinctive, the algorithm fails
to estimate their length and produces artifacts by compressing them. If we confront this
map with any of the others, it is clear that the road that connects what is supposed to
be the middle and the eastern corridor should be much longer. Furthermore, it lacks
any loop-closing feature, which given our environment and setup, is necessary for any
mapping algorithm. These two things combined, paired with the blindspots on the sides
of the vehicle, make the algorithm fail. We also tried to combine data from all sensors,
not only from the two on the top, but this was not enough to improve HectorSLAM's
performance.
4| Results 43

(a) Image of the map produced by Cartographer on a

subsection of the area.

(b) One detail of the south sec- (c) One detail of the south section
tion in Cartographer. in Slam Toolbox.

(d) Another detail of the south (e) Another detail of the south
section in Cartographer. section in Slam Toolbox.

4.2.3. Cartographer
Cartographer, the rst algorithm to use a graph-based approach to the SLAM problem,
can produce an acceptable map even when running around the whole environment and not
just a subsection. The map produced, shown in Figure ??, could be better: the borders are
foggy and some details are not clearly dened. Even when confronted with the Gmapping
one, it is evident that the map is not very detailed on the edges. Cartographer uses a
dierent approach from most algorithms when deciding if a cell is free, requiring multiple
scans to be sure of it. However, this technique proves unsuitable for our setup since our
vehicle moves at higher speeds than most robots, producing insucient scans to iron out
44 4| Results

(a) Image of the map produced by Slam Toolbox.

(b) The map produced overlapped to the (c) Detail of the Slam Toolbox map. The
satellite view from Google Maps. containers and the buildings are clearly un-
aligned with the map.

Figure 4.6: The map produced by Slam toolbox and overlapped to Google Maps.

the details on the edges of the map. This problem also shows when plotting the error to
the GPS, as in Figure 4.3. Here we can see that it manages to do the loop closure, with
the distance suddenly dropping around 400, 700, and 860 seconds, but it still fails to get
close to reality, having a mean error well above 2m.

4.2.4. Slam Toolbox

Slam Toolbox, the current state-of-the-art solution, performs well in our setup. The
map produced, shown in Figure 4.6, is very detailed and nely matches the real-world
track. However, this result took a lot of work to obtain. The algorithm is optimized for
enclosed spaces rich with features and loop-closing opportunities, not for large outdoor
environments. As such, it required a lot of tuning of the algorithm's parameters. This
4| Results 45

happens because we had to set the algorithm to search for loops in a meters-wide area to
compensate for the signicant build-up of sensor drift. At the same time, it had to use a
small enough precision to produce good results. This combination of settings proved to
be very resource-intensive and inconsistent, with the algorithm failing a few times. This
diculty can be seen in Figure 4.3, where, while it has a lower error than Cartographer, it
takes much longer to do the loop closure, with the distance building up a lot around 400
seconds, dropping down much later instead. Still, looking carefully, it is evident that the
western section of the map does not match reality as accurately as it does on the eastern
side. This detail will worsen the performance of the algorithms used in the localization
phase in Section 4.3, which already struggle in this section.

4.2.5. ART-Slam
The latest algorithm we employed is ART-SLAM. It still uses a graph-based approach
to the problem, but the main dierence is it works in 3D, unlike the previous ones. As
stated in Section 3.2.5, here we fuse all lidar data. This method allows us to collect much
more data points. While the two top sensors produced around 200,000 points per second,
this number jumps to approximately a million a second when using all sensors, a vefold
increase. This approach yields far better results than its counterpart, and while it still
requires loop closure to x the drift, it holds a much lower error than the other algorithms.
The produced map can be seen in Figure 4.7, where it is evident how much more detailed
it is, especially in the northwest section of the campus, where other algorithms can not
produce anything meaningful.

4.3. Localization

Using a similar approach to the one in Section 4.2, here we confront three dierent
algorithms for localization: AMCL, Slam Toolbox, and ART-Slam. Similarly to mapping,
we compute the distance from each GPS message to compare the various techniques.
Again, we consider only the open area sections where the GNSS signal is stable. In
Figure 4.8, we can see the error graph in such areas. Furthermore, it is informative
to compute and show for each algorithm the mean and maximum error and standard
deviation.
46 4| Results

(a) Image of the map produced by ART-Slam.

(b) Detail of the northwest section.

Figure 4.7: Images of the map produced by ART-Slam.

4.3.1. AMCL
For localization, AMCL uses the map produced by Slam Toolbox. While it is still
inferior to the one by ART-Slam, it is the best available as an OccupancyGrid, the only
format supported by it. The results obtained using this algorithm are acceptable but
leave much to be desired. While its mean absolute error (MAE) is the second best,
AMCL struggles particularly in the most challenging section, the open area on the west
side. Looking at Figure 4.9, it is clear that the algorithm diverges signicantly from the
GPS track, making turns broader than the ground truth. In general, AMCL handles
poorly the sections where the vehicle is turning. This problem is probably due to the fact
that the algorithm requires setting as a parameter the odometry that describes the robot:
4| Results 47

Figure 4.8: Graph of the error of the three localization algorithms in the open sky sections.

mean error (m) max. error (m) std. deviation (m)

amcl 0.6721 2.4636 0.4909
slam-toolbox 0.8101 2.0302 0.4796
ART-SLAM 0.3838 1.1271 0.1890

Table 4.1: Localization error of the dierent tested algorithms

omnidirectional or dierential drive. However, the shuttle has neither of these, so we had
to choose the former. We made this choice since the other would apply some optimizations
when turning, resulting in even worse performance. In particular, dierential drives can
rotate in place. So, AMCL optimizes its calculations so that when it detects that the
robot is turning, if its linear speed is low, it assumes that it is rotating in place and sets
to zero its translation. This optimization is done since dierential drive robots are usually
controlled in the following way. First, it turns in place to point to the objective, then
drives forward to it, and nally, if necessary, it spins again to reach the desired orientation.
This strategy, of course, is highly incompatible with an Ackermann drive such as ours,
so in this mode, AMCL performs poorly when the shuttle is turning at low speeds. Still,
by choosing the other option, AMCL assumes the shuttle is an omnidirectional drive,
which has an entirely dierent range of motion from the Ackermann model. While an
omnidirectional robot can rotate in place, it does not need to since it can spin all its
wheels in such a way as to move in the desired direction. This option, however, performs
better than the other one. Of course, a solution could be to modify AMCL and implement
an Ackermann drive odometry model. While this approach would yield better results, it
would not be worth the eort. While in localization it would reduce the error we obtain,
48 4| Results

(a) Image of the entire track.

(b) Detail of the northwest section of the track. (c) Detail of the south section of the track. Here
When turning AMCL follows a dierent trajec- the algorithm realized it was localizing wrong and
tory from the GPS. corrected.

Figure 4.9: Image of the track computed with AMCL compared with the GPS.

it would have less of an impact in navigation. This is because the shuttle was operated
manually when capturing the data for localization testing. Instead, with autonomous
navigation, we impose much lower speeds for safety reasons. As a result, the algorithm
has more opportunities to correct the wrong estimates made from the incorrect odometry
model using lidar data. All in all, the localization error tends to be lower when navigating
autonomously. Furthermore, its accuracy is limited by the map used, so implementing
this change would not guarantee that it could outperform ART-Slam. Lastly, the relative
part of the code is complex and it would require much eort and knowledge to implement
a new odometry model.
4| Results 49

(a) Image of the entire track. (b) Detail of the northwest section of the track.
Here Slam Toolbox performs worse than AMCL
in Figure 4.9b.

Figure 4.10: Image of the track computed with Slam Toolbox compared with the GPS.

4.3.2. Slam Toolbox

Slam Toolbox uses the pose graph produced in the mapping phase to oer a localization
mode. Since it uses the same map as AMCL but with a more complex approach, we would
expect similar or better results. This reasoning is partly true, but there are some surprises.
The same conclusion we reached in the mapping phase emerges here: the algorithm is
more optimized for enclosed spaces, rich with features. For example, when in corridors or
more narrow sections, for instance, in the eastern part of the track, Slam Toolbox performs
better than AMCL, reaching even lows of 20cm of error. On the contrary, in the west
section, the performance is way worse, with peaks well over 2m, making it unreliable and
unt for the outdoor navigation task. This problem weighs on the general performance
to the point where its MAE is the third overall.

4.3.3. ART-Slam
Similarly to Slam Toolbox, ART-Slam has a localization mode that uses the pose graph
produced in the mapping phase. In the narrow sections, its performance has no notable
dierences from the previous algorithms; sometimes, it even loses to AMCL only by a few
centimeters. Where ART-Slam shines is the open sections. Here, using a 3D algorithm
signicantly boosts the performance, outperforming the other algorithms by a signicant
margin. This result is mainly because the higher number of data points helps ART-Slam
extract more information than the opposing algorithms, providing sound localization even
in areas with no signicant features in 2D. As such, ART-Slam achieves a slightly higher
localization error in these areas compared to the narrow sections. Looking at the data, it
50 4| Results

Figure 4.11: Image of the track computed with ART-Slam compared with the GPS.

has peaks below 70cm, while it stands under 20cm in easier sections. This result makes
it the best overall, with an MAE almost half of AMCl, the second best.

4.4. Navigation

We designed an autonomous driving test inside the Bovisa campus to validate our
pipeline. Here we present a left cornering test we did in the central area of the campus.
We chose this particular segment since the GPS signal is good and the localization works
well enough not to impair navigation, unlike, for example, the west side. In Figure 4.12a,
the vehicle starts with a slight shift from the GPS, giving a high initial error, but slowly
converges to the correct position. This result shows in Figure 4.12b, where we plotted
the distance from the GPS during the movement. It is important to note that we gave
the vehicle multiple intermediate goals, not just the nal one. If we had done as such,
the global planner would have plotted a straight line directly to the destination. This
approach, however, is not suited for the shuttle since such a trajectory is designed for
robots that can move dierently. The global planners available in ROS are all designed
with dierential and omnidirectional drives. So, similarly to how AMCL works, explained
in Section 4.3.1, these planners assume that robots can follow any trajectory made of
segments by rotating in place or turning the wheels to match the desired orientation.
4| Results 51

(a) Image of the trajectory (blue) and the GPS signal (green).

(b) The error graph during the navigation test.

Figure 4.12: The trajectory followed during the navigation test and the relative error.

Under this assumption, global planners plan courses designed to reach the goal by taking
the shortest path available. This behavior also shows in Figure 4.12a, where the plan is a
series of segments. However, the local planner considers the geometry of our vehicle and
proposes suitable trajectories that, while in motion, may dier from the global one but,
in the end, reach the desired location. This behavior also explains the arches we see in
Figure 4.12b, which are apparent from the 50s mark and onwards.
 53

5| Conclusions and future

developments

In this work, we build a complete mapping, localization, and navigation pipeline for a
car-like vehicle that operates in an open private environment. For mapping the area, we
prepared a setup that combines the data from multiple sensors to produce a map using
dierent algorithms. The best results came from ART-Slam, which manages to create
an accurate map even in the most challenging sections, keeping the error below 50cm in
the process. However, due to its incompatibility with other localization and navigation
algorithms, we had to use the one produced by Slam Toolbox. For localization, we saw
ART-Slam again performing the best, with AMCL coming in second, with an MAE of
0.38m and 0.67m respectively. This vast dierence in performance is due to their handling
of the west section of the area, which is the most challenging. Here, AMCL and Slam
Toolbox struggle to nd features and, as a result, reach peaks of 2m of error. Instead,
ART-Slam, with its 3D approach, handles it nely, peaking only in a small section at 1.1m
of oset. Lastly, for navigation, we used the stack built for AMCl paired with move_base,
using planners designed for car-like vehicles.

5.1. Future works

Due to compatibility reasons, we were forced to test the navigation only with the
AMCL localization since ART-Slam is not directly compatible with move_base. There
are two main incompatibilities: the rst is that the map produced by it is a 3D point
cloud, and, given the sensors involved, it is not very straightforward to convert it into an
occupancy grid for the navigation algorithm. This problem arises since, rst of all, the
Velodyne sensors capture the ground, which marks everything as occupied. While this
is very easily xable by ltering based on the height, the sensors also register vehicles
passing by the shuttle. Removing these obstacles from the resulting map is more dicult
and not so straightforward. The second problem is tied instead to the hardware. Up to
now, we have performed all navigation tests through a laptop connected to the onboard
54 5| Conclusions and future developments

PC. While this is enough for using AMCL, we run into power-saving measures adopted
by the computer that limit the computational power when trying to use ART-Slam with
move_base. This results in the combination being unusable for testing. This happens
because there are no power outlets inside the shuttle, so the laptop has to operate on
battery power. However, we had no trouble testing it oine and connected it to a power
outlet. So, mounting an inverter would allow us to do further testing of more complex
setups.
Another possible development could rely on the cameras. As mentioned in Section 2.2.1,
the vehicle mounts two cameras on the front and the back of the vehicle. While they
are simple black-and-white ones, there have been some studies regarding Visual SLAM
or odometry solutions in the literature. While it is not easily deployable, it could greatly
benet our setup, where the GPS is not always available or reliable.
A further improvement could be to change move_base to suit better our use case, notably
the costmap used to track obstacles. The condition to remove old impediments from the
map relies on the vehicle's speed. The problem is that sometimes passing pedestrians
would obstruct the passage, so the shuttle would stop, seeing no possible trajectory.
However, at this point, since it could not move, the costmap would not remove the
obstacles registered, leading to a block status that could only be solved by manually
resetting the map. This outcome, of course, is undesirable, so changing the condition
that governs the costmap could improve the usability of the navigation system.
Lastly, the mechanical engineering department of the Politecnico di Milano has developed
a navigation system that relies on GPS to follow a predetermined track. It is based on
Matlab and Simulink, and, as such, it can interface with ROS. While we have discussed
before that the GNSS signal is absent in some sections, making this system not ideal
in our setup, we could convert the output of our localization to a GPS message to be
fed to that system. This would allow it to operate on the entire track. Of course, this
approach does not use lidar data to detect obstacles, so it can not be the nal solution in
an environment such as a university campus. However, it could be exported to dierent
scenarios where, for example, pedestrians are not present, so having a vehicle move based
only on a precomputed path would not be a safety concern. As such, experimenting with
it could still prove interesting.
 55

Bibliography

[1] M. Bersani, S. Mentasti, P. Dahal, S. Arrigoni, M. Vignati, F. Cheli, and M. Mat-

teucci. An integrated algorithm for ego-vehicle and obstacles state estimation for
autonomous driving. Robotics and Autonomous Systems, 139:103662, 2021.

[2] K. Bimbraw. Autonomous cars: Past, present and future a review of the developments
in the last century, the present scenario and the expected future of autonomous
vehicle technology. In 2015 12th international conference on informatics in control,
automation and robotics (ICINCO), volume 1, pages 191198. IEEE, 2015.
[3] F. Dellaert, D. Fox, W. Burgard, and S. Thrun. Monte carlo localization for mo-
bile robots. In Proceedings 1999 IEEE International Conference on Robotics and
Automation (Cat. No.99CH36288C), volume 2, pages 13221328 vol.2, 1999. doi:
10.1109/ROBOT.1999.772544.

[4] E. Dijkstra. A note on two problems in connexion with graphs. Numerische Mathe-
matik, 1:269271, 1959.
[5] D. Fox, W. Burgard, F. Dellaert, and S. Thrun. Monte carlo localization: Ecient
position estimation for mobile robots. pages 343349, 01 1999.

[6] M. Frosi and M. Matteucci. Art-slam: accurate real-time 6dof lidar slam. IEEE
Robotics and Automation Letters, 7(2):26922699, 2022.
[7] G. Grisetti, C. Stachniss, and W. Burgard. Improved techniques for grid mapping
with rao-blackwellized particle lters. IEEE transactions on Robotics, 23(1):3446,
2007.

[8] G. Grisetti, R. Kümmerle, C. Stachniss, and W. Burgard. A tutorial on graph-based

slam. IEEE Intelligent Transportation Systems Magazine, 2(4):3143, 2010.

[9] W. Hess, D. Kohler, H. Rapp, and D. Andor. Real-time loop closure in 2d lidar slam.
In 2016 IEEE international conference on robotics and automation (ICRA), pages
12711278. IEEE, 2016.

[10] S. Hoermann, M. Bach, and K. Dietmayer. Dynamic occupancy grid prediction for
56 | Bibliography

urban autonomous driving: A deep learning approach with fully automatic labeling.
In 2018 IEEE International Conference on Robotics and Automation (ICRA), pages
20562063. IEEE, 2018.

[11] G. Kim and A. Kim. Scan context: Egocentric spatial descriptor for place
recognition within 3d point cloud map. In 2018 IEEE/RSJ International Con-
ference on Intelligent Robots and Systems (IROS), pages 48024809, 2018. doi:
10.1109/IROS.2018.8593953.

[12] S. Kohlbrecher, O. Von Stryk, J. Meyer, and U. Klingauf. A exible and scalable
slam system with full 3d motion estimation. In 2011 IEEE international symposium
on safety, security, and rescue robotics, pages 155160. IEEE, 2011.
[13] K. Konolige, G. Grisetti, R. Kümmerle, W. Burgard, B. Limketkai, and R. Vincent.
Ecient sparse pose adjustment for 2d mapping. In 2010 IEEE/RSJ International
Conference on Intelligent Robots and Systems, pages 2229. IEEE, 2010.
[14] J. J. Leonard and H. F. Durrant-Whyte. Directed sonar sensing for mobile robot
navigation, volume 175. Springer Science & Business Media, 2012.
[15] S. Macenski and I. Jambrecic. Slam toolbox: Slam for the dynamic world. Journal
of Open Source Software, 6(61):2783, 2021.
[16] M. Quigley, K. Conley, B. Gerkey, J. Faust, T. Foote, J. Leibs, R. Wheeler, A. Y.
Ng, et al. Ros: an open-source robot operating system. In ICRA workshop on open
source software, volume 3, page 5. Kobe, Japan, 2009.
[17] M. Ribeiro and I. Ribeiro. Kalman and extended kalman lters: Concept, derivation
and properties, 04 2004.

[18] S. Thrun. Probabilistic robotics. Communications of the ACM, 45(3):5257, 2002.

[19] S. Thrun, D. Fox, W. Burgard, and F. Dellaert. Robust monte carlo localization
for mobile robots. Articial Intelligence, 128(1):99141, 2001. ISSN 0004-3702. doi:
https://doi.org/10.1016/S0004-3702(01)00069-8.

[20] E. A. Wan and R. Van Der Merwe. The unscented kalman lter for nonlinear es-
timation. In Proceedings of the IEEE 2000 Adaptive Systems for Signal Processing,
Communications, and Control Symposium (Cat. No. 00EX373), pages 153158. Ieee,
2000.

[21] X. Xu, L. Zhang, J. Yang, C. Cao, W. Wang, Y. Ran, Z. Tan, and M. Luo. A review
5| BIBLIOGRAPHY 57

of multi-sensor fusion slam systems based on 3d lidar. Remote Sensing, 14(12):2835,

2022.

[22] W. Zeng and R. L. Church. Finding shortest paths on real road networks: the case
for A*, Apr. 2009.
 59

List of Figures

1.1 Image of the general architecture of a SLAM algorithm as in [21]. . . . . . 4

1.2 Architecture of ART-SLAM [6]. Core modules are the mandatory blocks
of the pipeline, used to estimate an accurate trajectory and to build a 3D
map of the environment, given a sequence of input clouds. . . . . . . . . . 6

2.1 Image of the architecture of move_base taken from http://wiki.ros.

org/move_base. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
2.2 Image of the experimental vehicle employed for the data acquisition and
the real test. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16
2.3 Image of the cameras mounted on the shuttle. . . . . . . . . . . . . . . . . 18
2.4 The SICK LD-MRS sensor and view. . . . . . . . . . . . . . . . . . . . . . 19
2.5 The SICK LMS-151-10100 sensor and view. . . . . . . . . . . . . . . . . . 20
2.6 The Velodyne VLP-16 sensor and view. . . . . . . . . . . . . . . . . . . . . 21
2.7 Image of the Ackermann model. . . . . . . . . . . . . . . . . . . . . . . . . 24
2.8 Image of the Ackermann model with the bicycle approximation. . . . . . . 25

3.1 A sample trajectory produced by the EKF. . . . . . . . . . . . . . . . . . . 28

3.2 Image of the TF tree of the shuttle. . . . . . . . . . . . . . . . . . . . . . . 30
3.3 In the image on the left the silhouttes of cars parked around the area are
visible, while they don't appear on the right. Also the east section has a
completely dierent aspect. . . . . . . . . . . . . . . . . . . . . . . . . . . 31
3.4 Architecture of the nodes used for mapping and localization with ART-
Slam. Red rectangles represent odometry sources, blue ones lidar data,
green are nodes that do data preprocessing, and orange are the core of the
setup. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32
3.5 Architecture of the nodes used for navigation. Red rectangles represent
odometry sources, blue ones lidar data, green are nodes that do data pro-
cessing, purple broadcast the maps, and orange are the core of the setup. . 35
3.6 Point cloud before and after ltering. . . . . . . . . . . . . . . . . . . . . . 36

4.1 Image of the Bovisa campus of Politecnico di Milano. . . . . . . . . . . . . 40

60 | List of Figures

4.2 Images showing the how the accuracy of the GPS changes along the track. 41
4.3 Position error of three graph-based slam algorithms during the map cre-
ation phase. Due to the challenging scenario and the limited number of
possible loop closure Cartographer and slam-toolbox accumulate large er-
ror, while ART-SLAM maintain a low position error. . . . . . . . . . . . . 41
4.4 Maps produced by Gmapping. . . . . . . . . . . . . . . . . . . . . . . . . . 42
4.5 Image of the map produced by HectorSlam. . . . . . . . . . . . . . . . . . 42
4.6 Map produced by Slam Toolbox. . . . . . . . . . . . . . . . . . . . . . . . 44
4.7 Images of the map produced by ART-Slam. . . . . . . . . . . . . . . . . . 46
4.8 Graph of the error of the three localization algorithms in the open sky
sections. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 47
4.9 Image of the track computed with AMCL compared with the GPS. . . . . 48
4.10 Image of the track computed with Slam Toolbox compared with the GPS. 49
4.11 Image of the track computed with ART-Slam compared with the GPS. . . 50
4.12 Navigation test. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51
 61

List of Tables

4.1 Localization error of the dierent tested algorithms . . . . . . . . . . . . . 47

 63

Acknowledgements

Vorrei ringraziare il professor Matteucci per avermi dato la possibilità di svolgere questa
tesi e di lavorare su un argomento che mi appassiona a fondo. Ringrazio inoltre Simone,
per avermi seguito ed aiutato in tutti questi mesi di lavoro.

Il ringraziamento più grande va a Veronica. Questa laurea non sarebbe stata possibile
senza di te. Quella che ho trovato è più di una compagna di studio, ma anche una amica
fantastica, che nonostante i litigi e le dicoltà, è ancora qui. Sono stato davvero fortunato
ad averti al mio anco in questo percorso, grazie di tutto.

Devo ringraziare i miei amici Caterina, Massimo, Valentina e Vittoria, che sono stati i
miei compagni in questa esperienza di vita a Milano. Devo anche ringraziare Luca, il mio
compagno di stanza, probabilmente il miglior coinquilino che avrei mai potuto trovare,
che è riuscito ad alleggerire la mia vita da fuorisede.

Ringrazio poi tutti i miei amici di Brescia, che nonostante la distanza continuano ad
esserci per me in ogni occasione e senza i quali non sarei arrivato a questo punto della
mia vita.

Ringrazio inne la mia famiglia. Grazie ai miei genitori che mi hanno dato le oppor-
tunità e i mezzi per raggiungere questo traguardo. Grazie a mia nonna, che ha sempre
fatto il tifo per me in attesa di questa occasione.