Autonomous Driving of Competition Robot

Navigation and Decision Making

Francisco Cotrim Varandas de Sousa

Thesis to obtain the Master of Science Degree in

Electrical and Computer Engineering

Supervisors: Prof. Pedro Manuel Urbano de Almeida Lima

Examination Committee

Chairperson: João Fernando Cardoso Silva SequeiraSupervisor: Prof. Pedro Manuel Urbano de Almeida Lima

Members of the Committee: Prof. Carlos Baptista Cardeira

October 2018

4

Acknowledgments

To Rúben Capitão, for the astonishing partnership both in this master thesis and in life; thank you for

the extraordinary support, mentoring and friendship since freshman. I would like to express my gratitude

towards my supervisor, Pedro Lima, for his great enthusiasm and guidance throughout this master thesis

and for inspiring curiosity and knowledge to all his students. Still in the academia, a big thank you to

Núcleo de Estudantes de Engenharia Electrónica do IST (N3E), which provided to be a great school of

growth and knowledge, through its unforgettable members, robotics experiments and soft skills.

A word of gratitude to Sofia, for her kindness and inspiration to strive and be better. She believes

that I’m up to great things and I genuinely thank her for her endless support and guidance in the recent

years. Also, to all my friends and colleagues, those who just crossed my path and those who still make

part of this incredible adventure, thank you for being part of my growth.

Last but not least, to my parents. Thank you for always providing everything I ever needed and for

the constant support and comprehension since young age. Everything I will achieve in life will come to

you. To my brother, a trouble maker who will always inspire me to maintain young and eager. Of course,

a big thank you to all my family, including those who are not with us anymore but specially to those who

are currently fighting for their life; my thoughts are with you.

To each and every one of you, I’m humbly grateful.

i

Abstract

The deployment of self-driving vehicles is underway which enables the research for novel autonomous

navigation techniques and the necessity of validation of current systems. Motion planning and decision-

making modules for an autonomous vehicle are proposed in this master thesis, which, from a robotics

background, evolve to valid applicability in a real car. As such, it is suggested a Robot Operating System

(ROS) implementation of three trajectory generation techniques based on lane corridors, clothoids and

splines, which are followed by a ROS local planner. This navigation stack is applied in a 1/8 scale

Remote Control (RC) car model with the objective of participating in the Portuguese Robotics Open

2018, organized by Sociedade Portuguesa de Robótica (SPR). The process of building the customized

robot platform, from scratch, is documented along this work, also focusing on porting it to ROS - which

does not support car-like robots by default. From Three Dimensional (3D) modeling, to simulation and

reality, this master thesis solution can handle all the proposed competing scenarios, with a high-speed

approach, by delivering a map based motion planning, given an absolute localization pose. Some tools

for rapid robot deployment are developed - such as trajectory editors - and suggested - such as graphical

state machines - so that the car can operate in distinct scenarios from the competition ones.

Keywords

Self-driving cars; Autonomous vehicles; Navigation; Decision-making; Robotics Operating System.

iii

Resumo

Assiste-se hoje a uma crescente adopção de veı́culos de condução autónoma que permite a investigação

de novas técnicas de navegação autosuficiente e gera a necessidade de validação dos sistemas exis-

tentes. São sugeridos nesta tese de mestrado, módulos de planeamento de trajectórias e de tomada de

decisão, que permitem a evolução do conhecimento prévio em robótica para uma aplicação num carro

real. Assim, é sugerida uma implementação, em ROS, de três técnicas de gerar trajectórias baseadas

em corredores, clotóides e splines, que são posteriormente seguidas por um planeador local do ROS.

Este sistema de navegação é aplicado num carro RC, de escala 1/8, com o objectivo de participar no

Festival Nacional de Robótica (FNR) 2018, organizado pela SPR. É documentado, ao longo deste tra-

balho, o processo de construção, de raı́z, do carro personalizado com foco na sua implementação em

ROS - que não suporta robôs tipo carro por omissão. Da modelação 3D, à simulação e realidade, a

solução desta dissertação consegue lidar com todos os cenários de competição propostos, com uma

abordagem de alta velocidade, utilizando um planeamento de trajectórias baseado em mapas, a partir

de uma posição absoluta de localização. Algumas ferramentas para implementação rápida do robô são

implementadas - como editores de trajectórias - e sugeridas - como um editor visual de máquinas de

estado - para que o carro possa operar em cenários diferentes do da competição.

Palavras Chave

Condução autónoma; Veı́culos autónomos; Navegação; Decisão; Robotics Operating System.

v

Contents

1 Introduction 1

1.1 Purpose and motivation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2

1.2 Problem statement . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

1.3 Goals and challenges . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4

1.4 Document organization . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4

2 Context and Related Work 5

2.1 Self-Driving Car System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6

2.2 Decision-Making Architectures . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7

2.3 Decision-Making Hierarchy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

2.3.1 Route Planning . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

2.3.2 Behavioral Layer . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9

2.3.3 Car Model and Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10

2.3.4 Trajectory Planning . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11

2.3.4.A Geometric Methods . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11

2.3.4.B Variational Methods . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12

2.3.4.C Graph-Search Methods . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12

2.3.4.D Sampling-Based Methods . . . . . . . . . . . . . . . . . . . . . . . . . . . 13

2.3.5 Feedback Control and Trajectory Tracking . . . . . . . . . . . . . . . . . . . . . . . 14

2.3.6 Reactive Layer . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15

3 Architecture and components 17

3.1 Competition Architecture . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18

3.2 Proposed Strategy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19

3.3 Hardware . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20

3.3.1 Sensors, Actuators and Other Components . . . . . . . . . . . . . . . . . . . . . . 20

3.3.2 Computing Platform . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21

3.3.3 Construction and Design . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21

3.4 Software . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22

vii

3.4.1 Overview of software architecture . . . . . . . . . . . . . . . . . . . . . . . . . . . 23

3.4.2 From 3D model to ROS environment . . . . . . . . . . . . . . . . . . . . . . . . . . 24

3.4.2.A TFs . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24

3.4.2.B Unified Robot Description Format (URDF) . . . . . . . . . . . . . . . . . 24

3.4.3 Gazebo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26

3.4.4 ROS Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26

3.4.5 Move Base . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28

3.4.5.A Planners . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28

3.4.5.B Maps and Costmaps . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29

3.4.6 Robot behavior and state machines . . . . . . . . . . . . . . . . . . . . . . . . . . 29

4 Implementation 31

4.1 Hardware . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32

4.1.1 3D Printing . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32

4.1.2 Assembly . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33

4.2 Software . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34

4.2.1 Gazebo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34

4.2.2 From 3D model to reality . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35

4.2.3 Mapping . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36

4.2.4 Costmaps and obstacle representation . . . . . . . . . . . . . . . . . . . . . . . . 37

4.2.5 Global Planning . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38

4.2.5.A Corridors approach . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38

4.2.5.B Custom global planner . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39

4.2.5.C Trajectory editor . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40

4.2.6 Local planning . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41

4.2.6.A teb local planner . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41

4.2.7 Joystick . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44

4.2.8 Control commands multiplexing . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45

4.2.9 Velocity and acceleration smoothing . . . . . . . . . . . . . . . . . . . . . . . . . . 45

4.2.10 State machines - FlexBE . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45

4.2.11 Operation flow of the robot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 48

5 Evaluation 49

5.1 Considerations from previous competition participations . . . . . . . . . . . . . . . . . . . 50

5.2 Other teams’ approaches . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50

5.3 Competition results and further work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51

5.4 Evaluation of performance . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52

viii

5.4.1 Task by task analysis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53

5.4.1.A Driving at pure speed . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53

5.4.1.B Driving with obstacles . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54

5.4.1.C Driving in a working area . . . . . . . . . . . . . . . . . . . . . . . . . . . 54

5.4.1.D Parallel parking without obstacles . . . . . . . . . . . . . . . . . . . . . . 55

5.4.1.E Parallel parking between obstacles . . . . . . . . . . . . . . . . . . . . . 56

5.4.1.F Bay parking with and without obstacle . . . . . . . . . . . . . . . . . . . . 57

5.4.2 Simulation versus reality - a fidelity comparison with increasing speed . . . . . . . 58

5.5 System assessment . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59

5.5.1 Advantages . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59

5.5.2 Limitations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59

6 Conclusions and Future Work 61

6.1 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62

6.2 Future Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64

6.3 Closing remarks . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64

A Competition results 73

B Operation instructions 79

ix

x

List of Figures

1.1 Competition track resembling several obstacles (Reprinted from [1]). . . . . . . . . . . . . 3

2.1 Simplified functional architecture of self-driving vehicles. Green boxes represent hardware

inputs and outputs. Grey boxes represent the problem of localization and perception,

which is being developed in parallel in another master thesis [2]. Yellow boxes represent

interactions between the ego vehicle and other vehicles, humans and the real world. Blue

boxes represent the scope of this document. . . . . . . . . . . . . . . . . . . . . . . . . . 6

2.2 Example representation of the decision-making hierarchy, whose description is on the

subsections of Section 2.3. The vehicle starts by planning a global route that will input

to the behavior selector according to the task to achieve. Then, a trajectory is generated

and followed, always considering a reactive layer that takes care of safety measures. . . . 8

2.3 Illustration of the kinematics of a bicycle model according to Equation 2.1. vf represents

the speed of the front wheel, while θ represents the heading of the vehicle regarding the

X axis and δ represents the angle of the steering wheel. d is the distance between the

ground contact of both wheels. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10

3.1 Representation of the proposed decision-making hierarchy. . . . . . . . . . . . . . . . . . 19

3.2 Exploded view of the SolidWorks robot model. . . . . . . . . . . . . . . . . . . . . . . . . 21

3.3 Overview of this master thesis software architecture. . . . . . . . . . . . . . . . . . . . . . 23

3.4 Transform (TF) general structure with localization frames marked in yellow, internal robot

frames marked in green and base link marked in blue because it connects both classes. . 24

3.5 ROS Visualization (RViz) visualization of the car’s 3D model, loaded using URDF with

transforms from both sensors and actuators to the base frame, base link. . . . . . . . . . 25

3.6 Different types of joints interfaces, which apply to the developing robot platform, marked

in orange. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27

4.1 Car-like robot platform developed for the master thesis. . . . . . . . . . . . . . . . . . . . 32

4.2 Hardware overview of the developed robot platform. . . . . . . . . . . . . . . . . . . . . . 33

xi

4.3 Gazebo simulation environment with URDF spawned robot. . . . . . . . . . . . . . . . . . 34

4.4 Data feed of the corner of an obstacle, the semaphore structure and the ground, from 3

distinct coupled sensors (Kinect in Red Green Blue and Depth (RGB-D), ZED in depth

heatmap and Hokuyo in red). RViz provides a visual clue of the calibration state of the

several sources. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36

4.5 A given map (g) is edited and converted to two occupancy grids, (h) and (c), which can

be interpreted in different ways for localization or trajectory generation. 4.5(d) is provided

by Rúben [2]. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37

4.6 Corridors approach with decaying inflation radius of 0,5 meters in blue where the half

lap trajectory in green is generated by the plugin global planner. Pink areas represent

obstacle zones where cyan ones represent the minimum allowed distance from the line. . 39

4.7 Trajectory editor for cubic spline interpolation of waypoints. . . . . . . . . . . . . . . . . . 40

4.8 FlexBE example of a competition behavior flow. . . . . . . . . . . . . . . . . . . . . . . . . 46

5.1 Two complete laps - as in competition - of a desired spline trajectory (in green) versus the

executed trajectory (in purple), navigating between obstacles and with a working area. . . 52

5.2 Autonomous driving tracks for testing and competing. . . . . . . . . . . . . . . . . . . . . 52

5.3 Driving at pure speed behavior. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53

5.4 Driving with obstacles behavior. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54

5.5 Driving in a working area behavior. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 55

5.6 Parallel parking without obstacles behavior. . . . . . . . . . . . . . . . . . . . . . . . . . . 55

5.7 Two step parallel parking behavior, between obstacles. . . . . . . . . . . . . . . . . . . . . 56

5.8 Two step bay parking behavior, with and without obstacle. . . . . . . . . . . . . . . . . . . 57

5.9 Side by side comparison of the execution of the same trajectory for 10 laps, both in simu-

lation and in reality, with increasing speeds and varying teb local planner parameters. . . 58

6.1 Final prototype participating in the Autonomous Driving competition at FNR. . . . . . . . . 63

xii

List of Tables

3.1 Table of tasks to fulfill in the competition, which are further characterized in [1]. . . . . . . 18

4.1 Cura slicer 3D printing settings. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33

4.2 Joystick commands. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44

5.1 Challenges completion checklist on FNR and after competing. . . . . . . . . . . . . . . . 51

5.2 Reliability testing of driving at pure speed - 50 repetitions. . . . . . . . . . . . . . . . . . . 53

5.3 Reliability testing of driving with obstacles - 50 repetitions. . . . . . . . . . . . . . . . . . . 54

5.4 Reliability testing of driving in a working area - 50 repetitions. . . . . . . . . . . . . . . . . 55

5.5 Reliability testing of parallel parking - 50 repetitions. . . . . . . . . . . . . . . . . . . . . . 56

5.6 Reliability testing of bay parking - 50 repetitions. . . . . . . . . . . . . . . . . . . . . . . . 57

xiii

xiv

Acronyms

2D Two Dimensional

3D Three Dimensional

ABS Anti-lock braking systems

ADAS Advanced Driver-Assistance Systems

AEB Automated Emergency Braking

AI Artificial Intelligence

API Application Programming Interface

DARPA Defense Advanced Research Projects Agency

DES Discrete Event System

ESC Electronic stability control

FDM Fused Deposition Modeling

FNR Festival Nacional de Robótica

FOV Field of View

GPU Graphics Processing Units

GUI Graphical User Interface

HMI Human-Machine Interface

IMU Inertial Measurement Unit

ISR Instituto de Sistemas e Robótica

IST Instituto Superior Técnico

xv

JSON JavaScript Object Notation

LTS Long Term Support

MPC Model Predictive Control

N3E Núcleo de Estudantes de Engenharia Electrónica do IST

NHTSA National Highway Traffic Safety Administration

ODE Open Dynamics Engine

PN Petri Nets

RC Remote Control

RGB Red Green Blue

RGB-D Red Green Blue and Depth

ROS Robot Operating System

RViz ROS Visualization

SAE Society of Automotive Engineers

SLAM Simultaneous Localization and Mapping

SPR Sociedade Portuguesa de Robótica

TF Transform

URDF Unified Robot Description Format

WHO World Health Organization

XML eXtensible Markup Language

xvi

1Introduction

Contents

1.1 Purpose and motivation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2

1.2 Problem statement . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

1.3 Goals and challenges . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4

1.4 Document organization . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4

1

Introduction

1.1 Purpose and motivation

We live in exciting times these days. For decades, humanity has been dreaming and conceiving a

bright future, in the form of science fiction, where robots and self-driving cars would be a daily reality.

To build cars that can drive themselves would imply a whole new set of advantages such as democ-

ratizing access to transportation, providing smaller commute times, less expensive traveling and safer

streets for all different participants. It could potentially transform the city shape as we know it, erasing

the vast parking lots and heavy traffic, leading to a tremendous economic and environmental impact.

According to the Global status report on road safety 2015 [3] of World Health Organization (WHO),

with data regarding 180 countries, the number of road traffic deaths is approximately 1.25 million per

year, worldwide. On the same year, National Highway Traffic Safety Administration (NHTSA) released a

report on the Critical Reasons for Crashes [4], which stated that 94% of car crashes in United States of

America, were due to driver error. These impressive figures reveal a high demand for better transporta-

tion systems and safety measures.

This problem has been partially addressed by several developments on driving systems and sensors

with the purpose of aiding the driver, which are known as Advanced Driver-Assistance Systems (ADAS).

Some of the well known systems comprise Automated Emergency Braking (AEB), Electronic stability

control (ESC), Anti-lock braking systems (ABS) or even systems as simple as seat belt reminders.

However, according to the Society of Automotive Engineers (SAE) Levels of Driving Automation [5],

these methods only fit in levels 1 and 2 of automation, making us still some steps away from achieving

the full autonomy degree (level 5) and provide a better future.

Substantial technological progress has been made in the last years regarding environment sensing

devices as well as increased processing power per area unit, which now allows massive parallelization

of tasks, usually found on Graphics Processing Units (GPU). With these developments, notable work

has been made to achieve higher levels of automation, mainly by the academia, together with major

automotive brands and even other institutions such as Google, NVIDIA or Defense Advanced Research

Projects Agency (DARPA). Several competitions were held in the last decade such as DARPA Grand

2

Challenge (2004, 2005 and 2007) [6] or the Grand Cooperative Driving Challenge (2011 and 2016)

[7] which contributed with further knowledge about the subject. Google cars (now Waimo) already

drove 3 million km autonomously [8]. Yet another effort to achieve the reality of self-driving cars is the

Autonomous Driving competition, taking place, yearly, at Portuguese Robotics Open (FNR from now

on) [9] and promoted by SPR. This competition is the goal of this master thesis and has interesting

characteristics which will be described in the Section 1.2. However, there’s still work to be done [10].

1.2 Problem statement

The primary focus of this thesis is to conceive the navigation module of an entirely autonomous driv-

ing car model with exclusive onboard decision making. This navigation module will receive localization

and mapping data from another module, which is being developed in parallel in another master the-

sis [2], and provide decision-making and actuation for the robot. This robot should be able to participate

in the Autonomous Driving competition of Portuguese Robotics Open 2018, following all the proposed

rules [1].

This competition constitutes a medium complexity challenge in which a mobile, autonomous robot

must traverse a route along a closed track which resembles a conventional road. The track attempts to

replicate a real scenario, shown in Fig. 1.1, with a two-way road with a zebra-crossing, a pair of traffic

lights (one per direction), a tunnel, a working zone, traffic signs, two obstacles and parking areas.

Figure 1.1: Competition track resembling several obstacles (Reprinted from [1]).

Regarding tasks, it is composed by a broad set of challenges which start with a pure speed round

with start sign recognition, on top of which are added several other challenges, such as obstacle avoid-

ance, parallel and perpendicular parking with obstacles, directional semaphores detection, traffic signs

detection, low-light environments (tunnel) and a construction area. Speed is a crucial factor of per-

formance for every task, however, as another measure of performance there are penalties, which are

applied when the tasks are poorly accomplished or not done at all.

3

1.3 Goals and challenges

The primary goal of this competition is to perform all the tasks reliably and faster than the other

opponents without incurring penalties. This can be achieved with any robot that fulfills the requirements

of size and onboard decision making.

However, to address the real world problem and provide useful contributions to the field of self-

driving cars, we decided to design and build a non-holonomic car robot to resemble a real car and its

kinetic constraints. In this way, one can simulate some real-world scenarios and work on state-of-the-art

solutions.

After analyzing the current competition status, including competitors hardware and software, as well

as the overall performance, some requirements and guidelines were defined for this master thesis:

• Design and build a non-holonomic car-like robot under 3000 C and its software drivers;

• Use ROS as platform for all the algorithms implementation;

• Provide overall robot actuation which includes main motor, direction servo, brake servo, road lights

and traffic signs recognition feedback;

• Design the main car decision-making system to coordinate the competition tasks and internal

processes such as road lights and feedback on the traffic signs recognition;

• Design a motion planning module to generate and track useful trajectories for the designed car

based on the absolute pose provided by Rúben’s localization module [2];

• Provide a real-time solution software to meet the performance goals;

– Minimum top speed of 2 m · s−1;

– Minimum trajectory generation rate of 10 Hz.

1.4 Document organization

This document is organized as follows: Chapter 1 briefly describes the motivation and intended

goals for this thesis. Chapter 2 covers the related work methods that are used nowadays to solve the

stated problems, which will be analyzed, improved and implemented. Chapter 3 focuses on presenting

the general structure of the built system, where both hardware and software architectures are defined,

together with the selected components. Chapter 4 provides an in-depth description of the methodologies

used to tackle all the challenges in the competition. Chapter 5 exposes an assessment of the built system

performance, focused on motion planning and execution. Chapter 6 presents a general survey of this

master thesis and some thoughts on future work.

4

2Context and Related Work

Contents

2.1 Self-Driving Car System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6

2.2 Decision-Making Architectures . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7

2.3 Decision-Making Hierarchy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

5

Context and Related Work

In this chapter, it is presented an overview of the several approaches to solving the self-driving car

problem in a real-world scenario. It starts with a quick introduction to the whole car system, followed by

a deeper survey about the decision making processes involved. All the referred topics will be directly

applicable to the competition, which will serve as a base to my final work described in Chapter 3.

2.1 Self-Driving Car System

A standard vehicle is already a complex system with several distinct components. When the self-

driving capability is added, the complexity largely increases, namely regarding sensing, processing

power, connectivity and actuation [11].

Figure 2.1: Simplified functional architecture of self-driving vehicles. Green boxes represent hardware inputs andoutputs. Grey boxes represent the problem of localization and perception, which is being developed inparallel in another master thesis [2]. Yellow boxes represent interactions between the ego vehicle andother vehicles, humans and the real world. Blue boxes represent the scope of this document.

6

Current real implementations usually differ in specific hardware and software components but adopt

a common approach regarding its functional architecture [12], which is simplified in Fig. 2.1. One good

example of an identical architecture was applied in the recent project of Mercedes Benz, where a self-

driving vehicle traversed 103 km fully autonomously, in Germany [13].

As described in Chapter 1, the perception, and localization are out of the scope of this document due

to being developed in parallel in another master thesis. Human-Machine Interface (HMI) and vehicular

networks (connectivity) are also a broad area of study which are not covered by this document due to the

non-human and no-communication natures of the competition. Therefore, this document is focused on

the decision-making, motion planning and trajectory control which are further described subsequently.

2.2 Decision-Making Architectures

Self-driving vehicles must be able to reliably operate in complex and dynamic environments, with

multiple tasks requiring a real-time response from the robot. Even the most trivial decision made by

humans can be an extremely hard computing task for a robot, therefore, decision-making architectures

have been subject of study in the field of robotics for some decades. This survey [14] provides a good

overview regarding the fundamental approaches of these experiments, resulting in 4 main architectures:

Deliberative architectures: Some pioneers in this architecture, such as Albus [15], introduced a hierar-

chical approach heavily based on Artificial Intelligence (AI) in terms of knowledge representation.

This provides symbolic models, for both the environment and the agents, resulting in some cogni-

tive properties. Although there are practical applications of this model, such a high-level structure

has severe limitations in uncertain environments and also requires a high computing power to

predict behaviors in complex models.

Reactive architectures: Reactive systems usually make use of several predefined behaviors which

are triggered by a specific set of measurements from sensors, generating an action with minimal

computation time. A notable work by Brooks [16] combined several asynchronous layers of simple

tasks to successfully implement a robust reactive system. This architecture doesn’t need to pro-

cess complex behavior models, thus, resulting in a simpler computation complexity that provides

a much faster response.

Hybrid architectures: The hybrid architecture makes use of both deliberative (in a vertical hierarchical

way) and reactive architectures (in a horizontal shared structure). Some research [17] suggests

that, in a higher level of abstraction, the deliberative layer is used for perceiving the environment

and conceiving plans while on the bottom level, the reactive layer is responsible for modules such

as sensor processing or basic control generation.

7

Cognitive architectures: [14] introduces a new academic approach that tries to achieve human-level

cognition at a high level task such as perception, reasoning, and learning.

It’s important to notice that high computing power is currently available in small forms thanks to the

advance of mobile computing and GPU’s [18], which incentives the use of more complex algorithms

[19] and extensive parallelization [20], largely contributing towards a more deliberative self-driving car.

Recent self-driving projects [13, 21] are adopting Hybrid Architectures, to coordinate a high level of

abstraction with low-level tasks or safety measures.

2.3 Decision-Making Hierarchy

The decision making architectures for self-driving vehicles can be fairly diffuse, as many research

groups and companies keep their investigation on closed source. However, a very comprehensive sur-

vey [22] about motion planning and control techniques suggest a convergence on the decision-making

structure as expressed in Fig. 2.2. This survey can also be handy to understand the following sub-

sections of the current chapter.

Figure 2.2: Example representation of the decision-making hierarchy, whose description is on the subsections ofSection 2.3. The vehicle starts by planning a global route that will input to the behavior selector accord-ing to the task to achieve. Then, a trajectory is generated and followed, always considering a reactivelayer that takes care of safety measures.

2.3.1 Route Planning

Route planning is the problem of retrieving the best route for a vehicle to traverse from the initial

location towards the goal [23]. The result should be a set of way points through the road-network, for

the vehicle to follow. In a real-world scenario, where millions of roads and nodes may exist, per country,

this creates a very complex problem which can be revised in-depth on [24].

However, in the competition environment, the number of possible paths and nodes is limited and low

thus, traditional methods such as Dijkstra [25] or A* [26] can solve the problem with high-performance

[27]. Dijkstra algorithm (uninformed search) works by iteratively expanding the node with the smallest

8

cumulative cost, within the list of unvisited nodes, while A* (informed search), makes use of a heuristic

function to select the next node to expand, based on an estimate of the total cost to the goal. It is worth

noticing that Dijkstra search is complete whereas A* is optimally efficient, while both assuredly return

the optimal solution.

For those algorithms to work, a topological map of the road intersections must be built in the form

of a directed graph. This representation is quite common in the literature, and each edge is usually

associated with a high detailed map of the correspondent street or path. Although mapping is not

included in the ambit of this work, creating a topological graph is essential for the route planning. Thus,

there are several techniques for converting satellite images [28] or car sensor data [29] to topological

maps, even with parking lots included [30].

Route planning should usually involve the input of the user (HMI) to select the final destination of

the vehicle. However, in the competition, a goal should be automatically selected based on the current

challenge which, consequently, triggers the route planning module.

2.3.2 Behavioral Layer

After having a route plan as a set of waypoints, the vehicle must coordinate several tasks to ac-

complish the desired goals. In the competition context, the necessary functions include handling of the

semaphore recognition, changing lanes, avoid obstacles, parking and others. Each of these tasks has

specific requirements and must comply with the traffic rules and restrictions such as, not crossing the

track borders or not crossing the zebra.

This problem resembles a more generic system known as Discrete Event System (DES), which can

be defined as an event-driven system where asynchronous discrete events trigger the transitions from

state to state [31]. There are several approaches [32] for task coordination of a mobile platform such as

finite-state machines [33] or Petri Nets (PN) [34].

Finite-state machines and variants are fairly common and have been applied in several well-known

projects, namely during DARPA Urban Challenge [35–37]. They consist in an abstract machine with a

set of pre-defined connected states, where the current state of the machine is selected by the transitions

between states. Modeling complex behaviors (such as autonomous driving) to finite-state machines can

be cumbersome due to the high number of states and connections, especially in urban environments.

Some exciting approaches [38,39] circumvent this issue through pattern recognition, genetic algorithms

and semi-supervised learning of the vehicle behavior.

PNs are another modeling language for describing DESs, composed of places, transitions, arcs, and

tokens. Transitions represent trigger events that redirect tokens from the input places to the respective

output places through the arcs. The token positions in the net describe the current state of the PN -

known as marking - and the weigh of each directed arc between places and transitions is compiled in

9

the net incidence matrix. [40] divides an intelligent robot into organization, coordination and execution

levels and suggests the implementation of planning and control tasks using PNs, on the coordination

level. Furthermore, [41,42] present navigation selection methods according to specific situations or lack

of particular sensor data (such as GPS inside a tunnel) in contrast to [43], where the PN behaves directly

as a supervisor of the robot motions.

2.3.3 Car Model and Kinematics

Before introducing the trajectory planning and control, it’s essential to define the motion of a car-like

robot as a mathematical model, known as vehicle differential kinematics. This model should describe

the geometry of the system as well as relate the wheel speeds with the vehicle speed, which will allow

to calculate the future configurations of the robot in the world coordinate frame, based on the control

commands applied - a technique called forward kinematics [44].

The car-like robot, in its most simplistic form, can be described by the bicycle kinematic model (Eq.

2.1) which is composed by two wheels connected by a rigid link, where the wheels do not slip. While

both of the wheels do have one degree of freedom, where they can rotate about their axes of revolution,

the front wheel has one extra degree of freedom which allows it to rotate about an axis, normal to the

plane of motion, which models the steering of the vehicle.

ẋf = vf cos (θ + δ),

ẏf = vf sin (θ + δ),

θ̇ =vfd

sin (δ).

(2.1)

Figure 2.3: Illustration of the kinematics of a bicycle model according to Equation 2.1. vf represents the speed ofthe front wheel, while θ represents the heading of the vehicle regarding the X axis and δ represents theangle of the steering wheel. d is the distance between the ground contact of both wheels.

10

This model has an explicit limitation in maneuverability and, therefore, mimics well the behavior

of a real vehicle, under certain conditions. When driving slow, where no slippage exists on the tires

or the inertial forces are low, the kinematic models are suitable for path planning and generate low

error trajectories. However, these models also assume instantaneous steering rotation or instant speed

changes, which is just not physically feasible in a real system.

When higher speeds are applied, these restrictions can severely limit a system performance, and

those constraints can’t be ignored anymore. Thus, in the pursuit of a better accuracy, these kinematic

models can be expanded into dynamic models [45,46], imposing torque values and acceleration or jerk

rates. Moreover, the inertial effects can be added - according to the mass of the vehicle - and slippage -

caused by the wheel-ground contact - can be taken into account. All these features can be modeled, but

with the shortcoming of largely increasing calculations dimension (especially with non-linear constraints),

leading to higher complexity and computation time penalties on trajectory planning and control.

2.3.4 Trajectory Planning

With a global plan defined and subsequently a selected behavior, the car needs to compute the

trajectory it must traverse through the sequence of waypoints. Based on the vehicle model defined in

Subsection 2.3.3, this trajectory goes down to the fundamental core of the robot motion and consists

of a function, that describes the evolution of the vehicle pose in time, and includes longitudinal and

steering commands to the mobile platform. [47] presented the first path planner to take into account

non-holonomic robots such as cars and, since then, this envolved to a broader topic of study that can

be clustered in four main areas, described in the next subsections.

2.3.4.A Geometric Methods

As stated before, robotics developments over the years had a considerable contribution to the reality

of autonomous cars. Several classic techniques (which are intimately related with the map represen-

tation of the free space) can be applied to the context of autonomous driving such as the geometric

algorithms, for example, visibility graphs or cell decompositions. Despite being discontinuous and, thus,

impractical for trajectory planning of a car-like robot, voronoi diagrams have been successfully com-

bined with potential fields to achieve a safe path cost function on the Stanford entry to Darpa Urban

Challenge [48]. Another good example of the use of artificial potential fields is the design of a lane

keeping trajectory [49], which attracts the vehicle to move forward in the lane while being repulsed from

the road borders.

11

2.3.4.B Variational Methods

Variational methods consist in the continuous optimization of a non-linear path function, which has

the advantage of quickly converging (by minimization) into locally optimal paths, although the global so-

lution is usually sub-optimal due to the local minima convergence. The Carnegie Mellon Team applied

an example of these methods on their entrance to the Darpa Urban Challenge, where they implemented

a Model Predictive Control (MPC) to generate a valid trajectory according to their vehicle model [50].

An MPC [51] allows for the iterative optimization of a dynamic model in a finite horizon. From itera-

tion to iteration, the algorithm takes into account the previous control steps and increases the horizon

length (an event called receding horizon) which allows to better predict future events and, therefore,

generate trajectories. There are endless applications of MPCs in several areas of expertise and also

numerous variations of the underlying theory, for instance, the NMPC, which stands for Nonlinear MPC,

and employs nonlinear models in the prediction. An interesting comparison between the accuracy of the

kinematic bicycle model and the dynamic bicycle model with nonlinear constraints is presented in [52].

A very comprehensive survey about other variational methods techniques can be found in [53].

2.3.4.C Graph-Search Methods

Motivated by the name, these methods discretize the space in the trajectory generation problems

using traditional computer science algorithms for graph searching such as the already explained Dijkstra

and A* (Section 2.3.1) and other new approaches such as D* or anytime algorithms. D* is a variant

of A*, created by Anthony Stentz [54], where arc costs can dynamically change during runtime. This

method is especially useful in autonomous vehicle navigation problems, where new obstacles show up

frequently, forcing to quickly replan trajectories and update arc costs. Anytime based algorithms operate

by returning a suboptimal path first, which is then refined over time to find better solutions. The presented

search path methods can be applied to distinct forms of graphs.

• The route plan presented in Section 2.3.1 may serve as a possible trajectory as it is composed

of the lane and intersections network. However, this presents some drawbacks such as the lack

of capacity to deal with unexpected obstacles or temporary lane changes (construction area, for

example) and the necessity to drive in the inner sides of the curves (as opposed to navigation in

the lane center) to cut time in the competition.

• Probabilistic roadmaps (PRM) [55] start generating a graph by randomly sampling the configura-

tion space and then removing all the samples outside the free space. Then, these samples are

connected through feasible arcs, avoiding the existing obstacles, and the process is repeated until

a specified path density is achieved. Terminated the learning phase, it is initiated the query phase

that makes use of a search algorithm to select the best path. This kind of trajectories are best

12

applied to holonomic or unicycle robots, needing further processing of the paths to eliminate the

discontinuities and allow a car-like robot to navigate. This approach is probabilistically complete

and generates asymptotically optimal navigation trajectories [56].

• A lattice graph [57] is usually a regular tree whose drawing forms a regular tiling, covering the free

space in a uniform way. This graph is generated recursively by applying steering angles to the

kinematic models resulting in always feasible paths between the arcs of the graph. Furthermore,

the continuity between arcs in a node can be assured if the heading is taken into account in the

tree generation, meaning that discontinuous paths result in illegal branches. This technique was

successfully applied in the Darpa Urban Challenge [58] using Anytime Dynamic A* for the graph

search. In its basic definition, this approach discretizes the free space while the MPC discretizes

time [59] however; spatiotemporal state lattices were suggested [60], combining space and time

into a manifold, resulting in a reparametrization (deformation) of the cartesian plane and allowing

planning for high-speed tasks.

2.3.4.D Sampling-Based Methods

Sampling-Based methods continue to rely on the already mentioned graph search algorithms in

Subsection 2.3.4.C however, they do differ in the approach of building the graph. A big advantage of

these methods is the fact that they are designed for high dimensional dynamic models, providing speed

and real-time response the trajectory generation.

• RRT stands for Rapidly-exploring Random Trees and was firstly suggested by LaValle [61] con-

sisting in incrementally building a tree, starting from the current robot pose, with random samples

on the free space area. Once again, the new samples only connect with the nearest neighbor in

the tree if they are feasible, meaning that they cannot collide with obstacles and should satisfy the

robot constraints. Note that this method may not stop if a solution does not exist, thus, to better

restrict the growth of the tree, some limits are applied such as the growth factor - which typically

impose limits in the length of the arcs - or probabilistic restrictions of growing into a determined

area. [56] shows that RRT may converge to a suboptimal solution, which leads to the creation of

RTT*, a variation that, in each iteration, explores the neighbor vertexes of each node and rewires

their paths if it results in minimizing the cost path between them and the starting node.

• Rapidly-exploring Random Graphs (RRG) [62] enhance the RRT as they provide optimal solutions

and a graph representation, instead of a tree. These characteristics allow better control of the

generated graph, providing an assessment if the graph model is rich enough to comply with the

requirements for the robot constraints and the trajectory path.

13

2.3.5 Feedback Control and Trajectory Tracking

A sequence of actions is generated by the trajectory planning module, however, these methods

can’t usually predict model errors or uncertainty caused by external factors such as road conditions or

extreme weather influence. Therefore, there is the need for feedback control to stabilize and compensate

the deviation from the planned trajectory - in a closed-loop schema - or to quickly replan an entirely

new sequence of actions - open-loop. Several approaches will be presented for the trajectory tracking

approach.

The most basic process of trajectory following can be a Proportional–Integral–Derivative (PID) con-

troller which continuously calculate the error value between the robot pose and the desired trajectory,

and generates a corrective command according to the previously defined PID values. Although this

comes from the classical control theory and is widely applied in real linear systems, this approach does

not fit an autonomous car because the kinematics and dynamics of nonholonomic robots are not linear.

A second method, still very simple, and yet robust, is the pure pursuit controller. It works by having

a look-ahead distance point which is coincident with the previously planned trajectory and will always

be the target pose of the robot. This look-ahead distance must be adjusted to avoid big oscillations

and abrupt reactions. Concerning a self-driving car application, this method behaves particularly fine in

following a long road but can be unstable in urban environments, where maneuvers are required.

Other group of controllers tracks the wheels positions regarding the target trajectory by assuming

that the wheel should always be over the path. Accordingly, the error rate between the pose and the

trajectory serves as input to a correctional function, which will output the appropriate steering angle to

achieve convergence. With increasing complexity, a control design based on the Lyapunov function is

suggested in [63] for non-holonomic vehicles. In this approach, it is defined a tracking error in the car

coordinate frame. Then, the controls are generated by a change of basis from the inertial coordinate

frame to the reference trajectory and velocity.

All the previous tracking methods are fine for low-speed rigid bodies, and some were even used in

Darpa Grand or Urban Challenges, however, in extreme driving conditions with high speeds and dynamic

behaviors they don’t provide accurate and predictable actuation. MPC was introduced in Section 2.3.4.B

as a trajectory generation method and, through the same technique, it can also apply the desired control

commands directly to the robot platform in real time. This is achieved through an open loop design,

where the trajectory planning task must be solved multiple times per second. As soon as it receives

information about its pose, the MPC module replans the trajectory (with a short horizon to keep low

computation times) and applies the result in the following iteration.

14

2.3.6 Reactive Layer

It’s important to note that a reactive system is not the same as a reflexive system mainly because the

former entails a direct relation between sensors and actuators, while in the latter can be applied some

degree of perception/processing to the sensor data to provide a simple feedback actuation and fast re-

sponse. Even though there are purely reactive robots and hybrid robots mainly reactive [64] with robust

implementations, their behavior can be challenging to plan and predict and, thus, not desirable for au-

tonomous driving purposes. Nonetheless, when applied to a hybrid system, these reactive components

can be beneficial because they usually operate in parallel with the deliberative layer and if a reactive

situation is triggered, its output overrides the deliberative layer commands. As stated in 2.2, deliberative

responses are getting faster, leading to some discussion about the necessity of reactive layers.

In autonomous vehicles, this concept can be applied in several distinct ways for instance, Team An-

nieWAY [36] in Darpa Urban Challenge, integrated a reactive layer to modify planned trajectories based

on GPS waypoints, to take into account more accurate data about the terrain (such as small rocks or ter-

rain edges), that the primary obstacle tracker could not cover, eventually. [13], for example, implemented

brake functions as safety features in their reactive layer. Another similar case already applied in some

cars, the AEB, continually monitors several sensors to predict possible collisions, triggering deceleration

instructions overriding the human driver explicit commands [21].

15

16

3Architecture and components

Contents

3.1 Competition Architecture . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18

3.2 Proposed Strategy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19

3.3 Hardware . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20

3.4 Software . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22

17

Architecture and components

In this section, the hardware and software architectures are explained in detail. The competition

provides many close-to-real scenarios, therefore, it is presented a mimic of a real car system (Section

2.1) for the sake of applicability in the real world. Although the hardware is a collaboration with another

student [2], this document will focus on the work developed by its author, however, some decisions,

which will be duly noted, are the outcome of that partnership. All the software structure is original.

3.1 Competition Architecture

According to the competition guidelines, at the beginning of each challenge, the robot is positioned

at the start mark (zebra), and the specific task code is displayed in the semaphore, which corresponds

to a specific challenge. When the team is ready, the challenge is triggered by showing direction arrows

or parking commands. The base structure of all challenges is to fulfill two full laps to the track with an

hypothetical parking at the end. Challenges are described succinctly in Table 3.1.

Code DescriptionD1 Driving at pure speed challengeD2 Driving with signs challengeD3 Driving with signs plus tunnel plus obstacles challengeD4 Driving with all plus a road working area challengeP1 Parallel parking without obstaclesP2 Parallel parking between obstaclesB1 Bay parking without obstacleB2 Bay parking with obstacleV1 Vertical signs detection challenge

Table 3.1: Table of tasks to fulfill in the competition, which are further characterized in [1].

It is especially important for the robot to take the task into account in both route planning and be-

havioral modules as some challenges involve pure speed and a pre-defined route, while others involve

random changes of direction (through semaphores) or parking maneuvers. Accordingly, both modules

must react differently to each task, applying a different set of rules and constraints.

18

3.2 Proposed Strategy

Due to the tiny amount of waypoints and intersections, it is suggested the integration between the

route planning layer and the behavioral module, which brings simplicity to the decision making process.

As such, the use of a tested and functional approach can achieve the desired results with minimum effort,

thus, a finite state machine is suggested. Another point in favor of this choice is that the environment

has a very definite structure where the unpredictable situations can be tackled by the motion planner, as

opposed to a real-world stochastic situations.

The motion planning and local feedback control are intimately bounded with the proposed goals in

Section 1.3 as they will define the performance goals, namely the top speed. As previously discussed,

the trajectory can be generated once and followed through a closed-loop control system - which requires

a very accurate robot model that can handle the dynamics of our robot platform - or using an open-loop

approach - that generates a new trajectory for each new pose through time with the potential to generate

more adaptive and smooth motions. Is then suggested the use a mix of previously recorded global

trajectory with an open-loop approach for local trajectory generation.

In the bottom layer of the control hierarchy, two reactive algorithms are proposed: the prediction

and detection of collision - through direct analysis of depth data from the available sensors - and loss

prevention based on the pose accuracy provided by the localization module. Both these underlying

modules should trigger an emergency stop of the vehicle (obstacle avoidance should not be addressed

on this layer as it is covered by the deliberative trajectory planning).

It is assumed that all the perception and pose components will be addressed by the localization

module, developed by Rúben [2], and that the finite state machine will cover all the possible situations,

including recovery procedures and time/performance optimizations during the challenge.

Figure 3.1: Representation of the proposed decision-making hierarchy.

19

3.3 Hardware

3.3.1 Sensors, Actuators and Other Components

Sensors:

• Image sensors comprise a Microsoft Kinect One and a Stereolabs ZED (RGB-D) and an IDS

global shutter (Red Green Blue (RGB)), coupled with a wide Field of View (FOV) lens, for

high frame rate capture. The RGB-D points forward for obstacle and lane detection, while the

RGB points upwards for semaphores and traffic signs recognition.

• Odometry is provided by sensor fusion of an optical encoder on the motor axle and a 9 DOF

Inertial Measurement Unit (IMU).

• Three infrared range sensors (Sharp GP2Y0A41SK0F) were placed in the rear bumper to re-

semble some car’s parking sensors and will serve that purpose during the parking challenges.

• A lidar range sensor (Hokuyo URG-04LX) was also placed during the competition due to

noise issues affecting the Microsoft Kinect One point cloud.

Actuators:

• The team replaced the original gas motor of the chassis by a high-speed electric motor for

better control. The motor (RS-555 Motor 7750rpm 12V) was chosen to achieve high speeds

during the competition and fulfill the proposed goals mentioned in Section 1.3.

• It was installed a mechanical brake on the transmission axle to provide precise stop control

and reduce motor stress on decelerations. This brake is actuated by an original servo motor.

• The other original servo motor handles the vehicle direction, which discretizes all the possible

orientation angles in a specific set of defined intervals.

Other Components:

• An Arduino Mega 2560 will provide the software interface between the computing platform

and the robot, through USB serial interface.

• Over the microcontroller, a custom made PCB (designed by the parallel master thesis [2]) will

provide an interface with the sensors and actuators, plus power management of the system.

• The motor driver (Cytron 13A) will be connected to the Arduino board, and it’s characteristics

are important for the control response of the motor.

• A Bluetooth joystick (Sony Dualshock 3) is a separate component which is fundamental for

safety reasons and workflow as it provides teleoperation of the robot and wireless safety stop.

• Two LI-PO batteries: 11.1 V, 5 Ah (for computing) and 22.2 V, 3.3 AH (for electronics).

20

3.3.2 Computing Platform

Instead of the traditional competition approach of having a laptop over the robot, the team adapted

a desktop computer, on-board for purely autonomous behavior. This approach provides faster code

deployment, debugging and testing through a high-speed Wi-Fi link (IEEE 802.11ac), with the computer

behaving as a server. It was designed with high performance in mind, making use of a powerful CPU

(Intel Core i7-7700 Quad-Core 3.6GHz) and a GPU for parallelization (NVIDIA GeForce GTX 1050 Ti).

An additional fake HDMI adapter was acquired to run the platform headless.

3.3.3 Construction and Design

Figure 3.2: Exploded view of the SolidWorks robot model.

With the specified components in mind, the team opted to build a robot from scratch to better resem-

ble a real-world scenario of an actual vehicle and due to previous experience in the competition with a

specific chassis, which has car-like kinematics, wheel suspensions and one mechanic differential per

axle. This chassis was part of a commercial car model, the Kyosho Inferno GT Subaru Impreza, with a

gas motor and RC.

The gas motor and RC systems were removed, while keeping the direction system (based on servos)

and all the other components. From this base, an extensive work of 3D Modeling was done by the author

21

to adapt every single component (and their required positions) mentioned in the Section 3.3.1, to the

chassis, which is depicted in the Fig. 3.2. Although these parts are later produced and made real,

mechanical characteristics and studies are out of the scope of this master thesis.

Analyzing the final model, there’s a prominent tower in the back of the vehicle with several sensors,

which places the center of mass quite distant from the ground. Although this choice decreases stability

and increases the dynamic forces over suspensions, it was critical for the cameras and laser to properly

sense the environment as they have limited FOV. To compensate the weight from this tower, both

batteries were placed in the front of the vehicle.

Lights were also placed high and behind cameras, due to the lack of other support links with the

chassis and to prevent image brightness aberration. The placement of the other components was de-

cided in function of the RC model case, which was maintained for aesthetic and protection reasons. One

consequence of this decision was the need of creating a second big surface above the chassis to make

space for the electronics and computing platform, above the actuators.

One can also notice that the modeled chassis is a simplification of the actual RC model (where the

main sizes are maintained) because there was no need to materialize that part. Conversely, there was

the need to have a complete functional 3D model for software simulation and visualization, which will be

specified in Section 3.4.2.

3.4 Software

ROS [65] aims to provide a framework and tools for the development of robotic software systems on

top of an Ubuntu Linux distribution. With the multiple packages available in its library, ROS brings a whole

set of useful features such as common devices drivers, specific robots drivers, visualizers, simulators

and some common algorithms implementations. Although it is not a real-time operating system, ROS

has a message-passing protocol through TCP/IP that optimizes the communication between multiple

nodes (sensing, actuation, planning, etc.) with diverse data frequencies in a fast and efficient manner.

The decentralized architecture of ROS, where each feature can be a separate node, makes the code

modular and easy to maintain. Hence, one can have different nodes written in distinct programming

languages, that run in parallel without concerns over concurrency and parallelism. This also implies

that, if some node fails, the system can keep working, even in networked setups with multiple machines.

The team also decided to adopt ROS for being open source, well documented and well known in the

robotics community. This master thesis is developed using ROS Kinetic Kame on top of Ubuntu 16.04.3

LTS because both are the recommended releases for Long Term Support (LTS), at the time of writing.

22

3.4.1 Overview of software architecture

All the components presented in this chapter are combined in Fig. 3.3 which characterizes the overall

structure of this master thesis approach. The flow start is triggered by an human order through FlexBe,

which will mainly call move base goals, semaphores recognition and parking challenges. The remaining

challenges are handled by the move base stack and its ability to overcome the multiple obstacles through

the costmaps, whose performance will heavily depend on the defined configuration. move base will be

configured to generate trajectories based on a global pose on the map, through absolute positioning

(received as a TF by the localization module [2]), with logical barriers (road lines) that are enforced via

costmaps. Static maps are loaded through map server package, while the costmap 2d package allows

some specific sensor messages to be directly transposed to the map, such as pointcloud or laserscan

data. Regarding control, the local planner should output cmd vel commands to the robot electronics or

Gazebo simulation for actuation, while it may or may not receive odometry messages for a closed loop

approach.

Figure 3.3: Overview of this master thesis software architecture.

23

3.4.2 From 3D model to ROS environment

3.4.2.A TFs

ROS materializes the concept of transforms between multiple coordinate frames in a very practical

way, making use of a 3D vector for translation (x,y,z) and quaternions for rotation (x,y,z,w). A quaternion

is a complex number based rotation representation which is a well-known alternative to Euler angles

(avoiding the gimbal lock issue) or affine matrices (simplifying representation).

Through the ROS tf2 stack, all the components of the robot are linked to a central point known

as base link, which is then connected to the odom and map frames. All this is organized in a tree

structure that can change format over time, where the odom to base link transform is provided by relative

localization (as it is calculated by odometry sources) whereas the map to odom transform makes the

rectification of the odometry frame error to the real map coordinates, generating an absolute positioning.

The world frame is also relevant for this scenario as several maps - that represent a portion of the

real-world - can be interconnected.

Figure 3.4: TF general structure with localization frames marked in yellow, internal robot frames marked in greenand base link marked in blue because it connects both classes.

3.4.2.B URDF

With the conceived parts introduced in 3.3.3 and the need of having accurate transforms between

components, the complete 3D model of the car was converted to URDF, which constitutes an eXtensible

Markup Language (XML) file with the representation details of a specified robot, including its geometry,

mass, inertial values or materials (as presented in Listing 3.1). This was achieved through the use of the

SolidWorks to URDF Exporter package [66] and accomplishes the goal of making the correspondence

between sensors (and its data), actuators, and the map.

24

Figure 3.5: RViz visualization of the car’s 3D model, loaded using URDF with transforms from both sensors andactuators to the base frame, base link.

Listing 3.1: Example of a URDF file.1 2 3 4 5 6 7 8 9

10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31

25

RViz [67] is a 3D visualization tool that supports the multiple streams of ROS topics, which include

sensor messages, camera feeds, occupancy grids, trajectories and several others, such as the stated

URDF model that can be visualized in Fig. 3.5. In this figure, one can also see the established trans-

forms, whose connections can also be relate as a tree.

This is an important step, as the team decided to invent a new robot platform which is not supported

by ROS, and leads to the possibility creating a simulation environment, where the transforms are asso-

ciated with a mesh file (structure of vertices, edges, and faces that make up a 3D shape), which can be

used for physic simulations, as one can see in Section 3.4.3 and additional collision avoidance during

motion planning.

3.4.3 Gazebo

Gazebo [68] is a realistic simulation engine that allows testing robot algorithms and geometries in

virtual scenarios. It offers a comprehensive graphical interface with essential features like a physics

engine, robot control, and sensors simulation. The default physics engine of Gazebo, Open Dynamics

Engine (ODE), comprises a rigid body dynamics simulation engine and a collision detection engine that

support different functioning joint types, such as continuous spinning wheels versus revolute steering

joints, and friction between bodies.

To achieve this full simulation environment, that allows preliminary tests of trajectories and behaviors

with a perfect pose estimation (ground truth), Gazebo makes use of the robot model meshes generated

in Section 3.4.2 which must be connected through joints and virtual actuators. The connection between

URDF and these virtual motors can be implemented through ros control [69].

3.4.4 ROS Control

ROS Control is a control framework that allows the software implementation of transmissions,

hardware interfaces and custom controllers, which is particularly convenient for custom hardware im-

plementations as it connects the measured joint states, from the real robot, with the URDF model which

is then read by a custom controller that generates the control commands sent to the actuators. It is

important to note that this framework is flexible enough to be applied in a range of robotics applications

such as differential robots, car-like robots, robot arms and end effectors. For instance, the controller

manager could load several different controllers for the same robot.

This master thesis implements the ros control package in simulation in order to accept controls gen-

erated by the planners and materialize simulated motions in Gazebo, thus, providing a preliminary test to

the controller. For that purpose, one will need the controller manager to spawn the ackermann controller

which needs to be correctly mapped to the URDF joints of the robot platform through a configuration file.

26

The ackermann controller package implements car-like geometry by applying the concepts exposed in

Section 2.3.3, which makes it the perfect fit for the developing robot platform. Therefore, one must define

the distance between axles, d, the linear and angular velocity limits and an associated covariance to the

motion model, which will produce actuation based on the conversion of cmd vel commands.

(a) Continuous joints. (b) Revolute joints.

Figure 3.6: Different types of joints interfaces, which apply to the developing robot platform, marked in orange.

While the controller spawn is configured on a ROS launch file, transmissions and hardware interfaces

are implemented directly in the URDF file - through the use of a transmission XML tag - and linked with

its specific component. As such, the developing robot has two distinct types of hardware interfaces, both

of them simple transmissions with a mechanical reduction of 1:1 as stated in Listing 3.2:

• Velocity Joint Interface - which control is done through the angular speed of that joint. Applies to

continuous joints and fit the rotation of the wheels.

• Position Joint Interface - which control is done through the desired position of that joint. Applies to

revolute joints and fit the car steering.

Wheels speed commands are applied directly to the velocity joints and steering angle commands

directly to the position joints. This doesn’t match the reality of the vehicle due to the 4x4 linkage between

wheels, however, it achieves satisfactory results for simulation and reduces the complexity of the model.

After the controller commands are sent, joint limits are verified and, if infringed, overwritten by the

reached limit. Accordingly, each joint has limits of revolution, speed or effort defined in the URDF which

are enforced by ros control.

27

Listing 3.2: Example of a transmission tag in URDF file.1 2 3 transmission interface/SimpleTransmission4 5 hardware interface/VelocityJointInterface6 7 8 hardware interface/VelocityJointInterface9 1

10 11

3.4.5 Move Base

move base is the core constituent of the ROS navigation stack and connects with several other

components to make an attempt of moving a mobile robot, in a known or dynamic map, towards reaching

a given destination in the world. The internal structure of move base comprises a global and local

planner, including with their respective costmaps which are commonly known as occupancy grids. The

interaction with the external components will be discussed in Section 3.4.1.

This node is implemented via actionlib, an interface for tracking the status of preemptable tasks, with

the possibility of reporting progress or canceling goals. Actionlibs are implemented through ROS topics

and provide non-blocking calls to functions that may require some time to be completed (or may never

give an output) contrary to ROS services, which block code execution and, as a consequence, should

be only used for immediate execution tasks.

3.4.5.A Planners

ROS global and local planners provide similar functionality to the concepts introduced in 2.3 where

the global planner launches a trajectory which is then followed by the local planner. There are several

readily available planners in ROS that implement well-known algorithms such as sbpl lattice planner for

global lattice-based trajectories or global planner which implements A* or Dijkstra’s to generate a global

path for navigation between obstacles. All the approaches will be discussed further in Chapter 4.

As suggested in Section 3.2 it would be useful to have a planner with capabilities of generating free

trajectories based on the track borders and paths, for flexibility, as well as launching up pre-recorded

trajectories so that the robot can follow strict paths. For the competition purposes, it would also be

useful to define constraints for each path, such as speed or accelerations, in a dynamic way. This can

be achieved by ROS dynamic reconfiguration capability, in which some nodes can be re-parameterized

during runtime provoking a change in behavior. This kind of dynamic reconfiguration can be triggered

by the state machines when receiving some clues such as semaphores (to allow backward motion for

parking, for instance) or obstacles detected (to slow down for maneuvering, for example).

28

3.4.5.B Maps and Costmaps

Another essential component of move base is costmaps. Costmaps, or occupancy grids, are ma-

trices in which each cell represents a portion of a space and its probability of being occupied in the

real environment. This concept is fundamental for the planning layers as it distinguishes the free zones,

for navigation, from the occupied or forbidden zones. Therefore, the common move base architecture

makes use of a static map for the global planner planning, whereas the local planner uses a dynamic

costmap built by the sensor measurements, which are overlaid on the static map through the transforms

from the sensors to base link and from base link to map.

The referred static map is usually the same as the localization map as it is obtained through some

Simultaneous Localization and Mapping (SLAM) technique by packages such as gmapping, hector-

mapping or cartographer. It is important to state, however, that road lines are not real obstacles, but

logical ones thus needing a special perception algorithm to make them obstacles and then using that

data to create logical maps for defining barriers over that lines. This means that, for our purposes, lines

should be seen as walls by the navigation stack, however, this module makes part of the perception

layer, which is out of the scope of this document.

Although the competition track is bi-dimensional, which fits in the concept of occupancy grid, there

is another type of representation known as voxel grid that can be useful in a real-world application of

autonomous driving as it implements an occupancy grid in a three-dimensional space. Nonetheless, the

Two Dimensional (2D) occupancy grid is enough for our scenario and can be completed with other fea-

tures from costmap 2d such as the inflation layer around obstacles, which acts as a potential repealing

force, avoiding paths to be created nearby track limits. costmap 2d also allows having other occupancy

grid layers, on top of the static layer map, that can be useful to segment obstacles (for instance, dis-

tinguishing green obstacles from working areas cones in the competition context) which can then, be

handled differently.

3.4.6 Robot behavior and state machines

Once again, several packages exist for ROS that provide state machine and behavior control for a

robot. One of the most recent approaches in this field is FlexBE [70] which consists of a Graphical

User Interface (GUI) based on a well known ROS library named SMACH. SMACH is a robust library

that allows creating complex behaviors as a connection of several states resulting in a hierarchical state

machine. A significant advantage of this is the modularity of code, where the same functionality can

be used in different parts of the task in hand, as well as an easy inspection of the state transitions and

flows.

FlexBE implements a drag and drop interface on its GUI, where one has a list of available states

29

with generic and parametric functionality that can be connected between each other. The SMACH

hierarchical interface is implemented on FlexBE through joining one of several possible outputs of a

state to the input of the next state and so forth. Moreover, these connected states are triggered by an

initial point of execution and terminate in an ending state, which allow behaviors to be containerized as a

state in higher level behaviors. This can be useful on the parking cases, where parking involves several

smaller steps that are always contained in the end of a bigger flow (according to the competition rules).

Each state works by event callbacks and follows a defined life-cycle which run pieces of code ac-

cording to the status of the state for example, the on start state runs when the whole code begins, while

the on enter runs when the state is reached by the behavior. Several other life-cycle event callbacks

are available. FlexBE can run totally autonomously or monitored by an operator, who has live access

to the execution status and can even receive a request for help in a certain tasks, in a concept named

collaborative autonomy. Although this is forbidden by the rules, it would make sense for some real world

implementations before reaching the full autonomy (level 5).

30

4Implementation

Contents

4.1 Hardware . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32

4.2 Software . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34

31

Implementation

Chapter 3 describes the high-level structure of this master thesis, covering packages and tools

needed for its implementation. With the global scope defined, one needs to extract conclusions from

failed trials and to describe the applied methodologies of the final solution, which will be scrutinized

in the present chapter. Experimental results of the implemented solution will be exposed and properly

evaluated in Chapter 5. Used packages will marked in italic whereas parameters will be marked in

math notation. The final source, bill of materials and documentation are available on GitHub [71].

4.1 Hardware

4.1.1 3D Printing

To simplify the complex transformation of an RC car to a robotic platform, 3D printing was heavily

used to adapt the different components to the chassis. During the multiple iterations of the vehicle

construction, 36 models were designed through the use of SolidWorks - Fig. 5.8(c) - and there are

currently 23 3D printed parts deployed on the vehicle - Fig. 5.8(d).

(a) Solidworks 3D model (b) Real model

Figure 4.1: Car-like robot platform developed for the master thesis.

32

Setting Value DescriptionLayer Height 0,2 mm Impacts printing quality and speedInfill Density 20 % Defines robustness of the objectPrinting Temperature 215 ºC Melting temperature of filamentHeated plate Temperature 60 ºC Impacts the material adhesion to the platePrint speed 80 mm/s Impacts quality and printing temperature

Table 4.1: Cura slicer 3D printing settings.

The designed parts were printed on a BQ Prusa i3 Hephestos with a heated plate, that materializes

3D models through a technique named Fused Deposition Modeling (FDM) which is an additive manu-

facturing process where the object is built by depositing melted thermoplastic filament, layer-by-layer, in

a pre-computed path. Through the iterations, some knowledge was acquired on the required margins

for fittings and also on the material expansion of the printing process and its results on produced parts

(dilatation, warping and others). All the objects were printed with Black PLA (polylactic acid which is a

biodegradable thermoplastic derived from renewable resources, such as corn or sugarcane) from BQ,

with the relevant printing settings of the slicer software (Ultimaker Cura) presented in Table 4.1.

4.1.2 Assembly

(a) Actuators and motor con-nection with chassis.

(b) Computing modules and mi-cro controller interfaces.

(c) Sensing modules and lights.

Figure 4.2: Hardware overview of the developed robot platform.

The robot was assembled with an effort for facilitating the disassembly for eventual maintenance

interventions. For this motive, cables were selected and customized to have small footprints and were

grouped by modules, using proper connectors. Vibrations were also taken into account by choosing an

SSD disk and gluing all the screws and bolts. Multiple external fans were installed to ensure the air

circulation and keep temperature low within the inner case.

33

4.2 Software

4.2.1 Gazebo

Figure 4.3: Gazebo simulation environment with URDF spawned robot.

Gazebo has a small list of pre-defined buildings and fixtures which allows the user to deploy freely

in the provided scenarios. Even though this permits to create some complex environments, the real

advantage of Gazebo is the possibility to load the user-customized models in the form of SDF (an XML

formatted file similar to URDF). As such, this master thesis implements its environment by making use

of the Autonomous Driving Simulator for the Portuguese Robotics Open [72, 73], a gazebo environ-

ment developed by another competitor, that resembles the competition track with useful utilities such as

spawning obstacles or direction semaphores. After the environment setup, the developed robot can be

spawn through the URDF.

Some adjustments were made to the generated file from SolidWorks on each of the URDF links,

namely on the inertia parameters (ixx,ixy,ixz,iyy,iyz,izz). The default computed parameters resulted in

either explosion or contraction of the spawned model, rendering it unusable. These parameters define

a 3x3 symmetrical matrix (4.1) that represents the mass moment of inertia that characterizes each

component resistance to rotation acceleration about an axis.

34

ixx ixy ixzixy iyy iyzixz iyz izz

(4.1)SolidWorks also calculates other relevant parameters such as the center of mass and mass, accord-

ing to the object materials. Unfortunately, the latter was proven to be inaccurate thus, it was manually

corrected to the weight of the real objects. To accomplish a more realistic simulation Gazebo supports

other pertinent URDF parameters, listed below.

• Contact coefficients - for interaction between links

– mu - friction coefficient

– kp - stiffness coefficient

– kd - dampening coefficient

• Joint Dynamics - interaction between joints and links

– friction - physical static friction. For prismatic joints, the units are Newtons. For revolving

joints, the units are Newton meters.

– damping - physical damping value. For prismatic joints, the units are Newton seconds per

meter. For revolving joints, Newton meter seconds per radian.

Ultimately, Gazebo permits sensor simulations, which are convenient for a full robot simulation, where

all the real sensors’ data is generated from sensing the simulated Gazebo environment. To enable this,

Gazebo offers multiple plugins which must be defined on the gazebo XML tag on the URDF file. The

main plugins imported for this master thesis concerning navigation, obstacle detection and control were:

• gazebo ros control - for l