Embed Size (px)
Citation preview
Agent-aware mobile robot navigation in domesticenvironments
Inês Afonso Alexandre
Thesis to obtain the Master of Science Degree in
Aerospace Engineering
Supervisors: Prof. Rodrigo Martins de Matos VenturaM.Sc. Oscar Lima Carrion
Examination Committee
Chairperson: Prof. José Fernando Alves da SilvaSupervisor: Prof. Rodrigo Martins de Matos Ventura
Member of the Committee: Prof. Alexandre José Malheiro Bernardino
October 2018
ii
Declaration of Authorship
I declare that this document is an original work of my own authorship and that it fulfils all the require-
ments of the Code of Conduct and Good Practices of Universidade de Lisboa.
Declaracao de Autoria
Declaro que o presente documento e um trabalho original da minha autoria e que cumpre todos os
requisitos do Codigo de Conduta e Boas Praticas da Universidade de Lisboa.
iii
iv
Acknowledgments
Instituto Superior Tecnico provided me not only with technical knowledge but has also allowed me to
grow on a professional and personal level throughout this wonderful learning experience.
This thesis is the conclusion of all the hard work developed over the last few years. But I would not
be able to accomplish it all without the support and contribution of so many people. I want to thank
everyone that somehow helped me on this journey.
I would like to thank my supervisors Prof. Rodrigo Ventura and M.Sc. Oscar Lima for all the guidance
and help provided. The conclusion and success of this work would not be possible without them.
The opportunity to work on such an interesting project was incredible and I want to thank all members
of the SocRob@Home Team who have welcomed and helped me.
I want to thank all my incredible friends who have supported me along the way.
A special thanks to Margarida, Jorge and Bernardo. These years were amazing and I could not have
done it without such amazing people as you. Thank you for the support, friendship and affection.
Luıs, thank you for all the patience, support and love.
To my parents, I thank for all the opportunities you have given me throughout life. Thank you for the
unconditional support and love shown in all these years.
v
vi
Resumo
Com o avanco tecnologico e crescente ligacao das necessidades humanas a robotica, espera-se
que cada vez mais os ambientes domesticos sejam povoados com robots variados. A navegacao
autonoma e uma competencia essencial para robots de servico movel implementados nestes ambi-
entes. Uma quantidade consideravel de ferramentas e algoritmos foram e continuam a ser desenvolvi-
dos neste campo como consequencia de extensas pesquisas realizadas.
O trabalho desenvolvido aborda os desafios da navegacao autonoma num ambiente dinamico con-
siderando a presenca de humanos e robots. Considerando o desconhecimento da trajetoria dos agentes
e a ausencia de comunicacao entre eles, colisoes ou num bloqueios de passagem mutuos poderao
ocorrer.
O foco desta tese e obter predicoes, e respetivas incertezas associadas, de modo a melhorar a
navegacao. Com base em dados de posicao atual e velocidades anteriores, os dois metodos aplicados
para obtencao de previsoes sao Movimento Browniano (BM) e Algoritmo de Metropolis (MA). O calculo
da funcao de densidade de probabilidade usada no MA e realizado recorrendo ao metodo de Estimacao
de Densidade de Kernel.
Um mapa de predicoes e construıdo e enviado para o planeador local para que possa existir uma
resposta mais rapida a situacoes precarias.
Os resultados obtidos avaliam o tempo computacional e a probabilidade de sucesso de predicao de
ambas as abordagens. Concluiu-se que o metodo superior em rapidez e precisao e o BM.
Uma vez que um mapa de predicoes e construıdo para influenciar o planeador local, o sucesso
deste trabalho implica um melhoramento da sua resposta e consequentemente da navegacao.
Palavras-chave: Navegacao de Robots Moveis, Navegacao Consciente do Humano, Navegacao
Consciente do Robot, Navegacao Consciente do Agente
vii
viii
Abstract
Taking into consideration the technological progress and rising connection between human needs
and robotics, domestic environments are expected to be increasingly populated by varied robots. Au-
tonomous navigation is an essential competence for mobile service robots deployed in these environ-
ments. In fact, researches extensively studied it and, therefore, a considerate amount of tools and
algorithms have been and continue to be developed in this field.
The work developed focuses on addressing the challenge of autonomous navigation in a dynamic
environment populated with humans and robots. Considering in particular humans and non-cooperative
robots, a priori agent trajectory is unknown. The absence of coordination between agents may result in
deadlock or even collision.
This work focuses on attaining predictions, with associated uncertainty, to improve robot navigation.
Based on previous velocity data and current position, Brownian Motion (BM) and Metropolis Algorithm
(MA) are the applied methods for predicting the next state of the environment. Kernel Density Estimation
(KDE) is used to compute the probability density function used in MA.
A map of predictions is built and sent to the local planner resulting in a faster response to risky
situations.
The obtained results evaluate not only the computational time but also the prediction’s probability
success of both approaches. BM was concluded to deliver an overall more conservative, accurate and
faster prediction than MA combined with KDE.
Considering a prediction map is built to later influence the local planner, the success of the developed
work implies an enhanced planner response. Consequently, navigation is improved.
Keywords: Mobile Robot Navigation, Human-Aware Navigation, Robot-Aware Navigation, Agent-
Aware Navigation
ix
x
Contents
Declaration of Authorship . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . iii
Acknowledgments . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . v
Resumo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . vii
Abstract . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ix
List of Tables . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . xiii
List of Figures . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . xv
Nomenclature . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . xvii
1 Introduction 1
1.1 Motivation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1
1.2 Problem Overview . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2
1.3 Objectives . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
1.4 Thesis Outline . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
2 Background 5
2.1 Detection . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
2.2 Motion Planning . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
2.2.1 Global Planners . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7
2.2.2 Local Planners . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
2.3 Perception . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
2.4 Tracking and Predicting . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
2.4.1 Stochastic processes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16
3 Related Work 19
3.1 Human Acceptance . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20
3.2 Person–Acceptable Navigation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21
3.2.1 Accounting for Personal Space . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23
3.3 Navigating with dynamic obstacles . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23
3.4 Predicting with Uncertainty . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24
3.4.1 Uncertainty Margin . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25
xi
4 Approach and Implementation 27
4.1 Prediction Computation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27
4.1.1 Brownian Motion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28
4.1.2 Metropolis Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32
4.1.3 Uncertainty . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36
4.2 Simple Algorithm Integration . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42
4.2.1 Coordinate frames . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42
4.2.2 Prediction Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43
4.2.3 Relevant Time Interval . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43
4.2.4 Predictions’ Map . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44
4.3 Extending the Prediction Approach . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45
4.3.1 Scan Processing . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45
4.3.2 Multiple Obstacles . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 48
5 Evaluation 51
5.1 Simulations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52
5.1.1 Single Obstacle . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52
5.1.2 Multiple Obstacles . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53
5.2 Real Setting Tests . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 55
5.3 Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 56
5.3.1 Simulations’ Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57
5.3.2 Real Scenarios’ Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59
5.3.3 Predicted Costmaps . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61
6 Conclusions 63
6.1 Achievements . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64
6.2 Future Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64
Bibliography 65
xii
List of Tables
4.1 Zα/2 values for common CIs . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37
4.2 Quaternion multiplication guide table . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42
5.1 Obtained Results for Single Obstacle Simulations (5 seconds relevant time interval con-
sidered) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 58
5.2 Obtained Results for Multiple Obstacles Simulations (5 seconds relevant time interval
considered) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 58
5.3 Results obtained in Real Scenarios with defined Cluster Factor . . . . . . . . . . . . . . . 59
xiii
xiv
List of Figures
1.1 MBot, current service robot for the SocRob@Home Team . . . . . . . . . . . . . . . . . . 3
2.1 Laser functioning base parameters . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
2.2 Navigation flowchart considering Detection and Global Planning . . . . . . . . . . . . . . 8
2.3 Environment change and the need for Local Planners . . . . . . . . . . . . . . . . . . . . 9
2.4 Potential Field Local Planning example . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
2.5 Dynamic Window Approach Local Planning example . . . . . . . . . . . . . . . . . . . . . 10
2.6 Elastic Band Local Planning example . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
2.7 Navigation flowchart considering Detection, Global and Local Planning . . . . . . . . . . . 11
2.8 Monte Carlo Method used for area determination . . . . . . . . . . . . . . . . . . . . . . . 13
2.9 Markov Chain example . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
2.10 Basic Kalman Filter operation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
2.11 Navigation flowchart considering Detection, Global and Local Planning combined with
Prediction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16
3.1 Schematic of relevant functionalities in agent-aware navigation . . . . . . . . . . . . . . . 19
3.2 Behavior parameters and desired trajectory for passage maneuver . . . . . . . . . . . . . 21
4.1 Brownian Motion implementation flowchart . . . . . . . . . . . . . . . . . . . . . . . . . . 30
4.2 Predictions computed with BM . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31
4.3 Metropolis Algorithm with KDE implementation flowchart . . . . . . . . . . . . . . . . . . . 34
4.4 Predictions computed with KDE and MA . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35
4.5 Schematic of BM implementation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39
4.6 Computation of predictions with uncertainty ellipses . . . . . . . . . . . . . . . . . . . . . 40
4.7 Schematic of MA + KDE implementation . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41
4.8 Predictions’ Occupancy Grid Map . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44
4.9 Environment framework . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46
4.10 Prediction ellipses using clusters . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49
5.1 Ellipse with parameters definition . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51
5.2 Single obstacle simulations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54
5.3 Simulations with multiple obstacles . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 55
xv
5.4 Scenario 1 in 4 different time frames . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 56
5.5 Scenario 3 in 4 different time frames . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57
5.6 Points Validity in Simulations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 58
5.7 Likelihood ratio in Simulations with defined relevant time interval: 3, 5 and 7 seconds . . . 59
5.8 Likelihood ratio in Real Scenarios with defined Cluster Factor: 4 and 8 . . . . . . . . . . . 60
5.9 Points Validity in Real Scenarios . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 60
5.10 Scenario 1 Predicted Costmap in different time frames . . . . . . . . . . . . . . . . . . . . 61
5.11 Scenario 3 Predicted Costmap in different time frames . . . . . . . . . . . . . . . . . . . . 62
xvi
Nomenclature
Acronyms
2D Two Dimensional
3D Three Dimensional
AAN Agent-Aware Navigation
BCI Bayesian Credible Interval
BM Brownian Motion
CI Confidence Interval
DWA Dynamic Window Approach
FSA Finite-State Automata
GHMM Growing Hidden Markov Model
HAN Human-Aware Navigation
HMM Hidden Markov Model
ISR Institute for Systems and Robotics
IST Instituto Superior Tecnico
KDE Kernel Density Estimation
LQE Linear Quadratic Estimation
MA Metropolis Algorithm
MCM Monte Carlo Method
MCMC Markov Chain Monte Carlo
RAN Robot-Aware Navigation
ROS Robot Operating System
YOLO You Only Look Once
xvii
xviii
Chapter 1
Introduction
1.1 Motivation
As technology progresses, human being’s needs are increasingly linked to robotics. Consequently,
autonomous mobile service robots are nowadays becoming more and more present around humans in
varied environments.
In order to effectively operate, one of the essential aspects to consider is the robots’ ability to au-
tonomously navigate. Autonomous navigation can be defined as the freedom to act independently re-
garding the process of planning and executing a path, from start point to goal point [1]. A large amount
of uncertainty is inherent to real-world environments, where robots are expected to be utilized.
When deployed in domestic environments robots are expected to help carry out diverse tasks. For
such application, an awareness of humans or other robots that may be, and interact, in the same envi-
ronment is essential. In a group of interacting humans, each person is found to respect a virtual personal
space around every other intervenient. Proxemics is the concept of that virtual space, which may change
as the situations differ. Only by respecting the minimum proxemics both comfort and safety perception
as well as social acceptance from humans may occur [2].
Although autonomous mobile service robots are already present in varied human populated environ-
ments, their functions and challenges are boundless. This thesis considers the autonomous navigation
of a mobile robot in a domestic environment scenario which can be populated by humans as well as non-
cooperative robots. The presence of humans and/or robots in the environment involves considering, in
real-time, their safety and comfort thereby posing challenges in perception and prediction of behaviour.
These kind of challenges are also the focus of scientific competitions related to service robots in
domestic, healthcare and office environments as the ones SocRob@Home, a project at Institute for
Systems and Robotics (ISR) from Instituto Superior Tecnico (IST), participates in. This project focuses
on addressing challenges that arise when robots are deployed in real world environments, involving
a wide range of scientific knowledge in areas such as object and facial recognition, grasping objects,
natural language understanding and navigation.
1
Aerospace Motivation
Considering technological progress and the growing need for quick and easy traveling, airspace is
expanding in population.
In addition, the presence of non-cooperative agents in airspace is increasingly a concern given the
growing presence of unmanned aerial vehicles, such as drones.
Sense and avoid is a crucial capability in possible collision scenarios. The work here developed
can be extended with 3D lasers, while still considering absence of communication, and be used in such
scenarios to ensure proper separation between vehicles.
1.2 Problem Overview
The problem tackled by this work assumes that the algorithm will only be used in one robot, deployed
in a dynamic environment where static and dynamic obstacles can influence real-time navigation.
Autonomous navigation requires awareness of the surroundings. For such, cameras and various
sensors can be used. However, parts of the state can be missing from each sensors’ data and con-
sequently the environment is only partially observable. In addition, even with a correct and complete
detection of present humans and robots their perception as such is not flawless.
The method developed should ensure that the path chosen does not affect safety and/or comfort
of the agents, while still taking into consideration the probability of defective perception. To disregard
the problem of wrongful perception this thesis overlooks specific agent characterization. Instead, it
considers all detected points individually to later track and predict the overall surrounding change. A
correct perception of environment transitions should enable the prediction of next states.
The challenge concerning prediction arises from the stochastic property of the environment. Since
there is uncertainty present in the behavior of others, the environment state is not completely determined
by the prior state and the actions executed. In addition, non-cooperative agents are considered, there is
an absence of communication and a priori knowledge about trajectory. Consequently, coordination be-
tween agents is inexistent and collision or mutual blocking situations, deadlock, may occur. An accurate
prediction of environment changes enables a faster response to undesired situations.
A parametric, Brownian Motion (BM), and non-parametric, Metropolis Algorithm (MA), approach is
used considering previous velocity data and current position. The first is a simpler and faster method
that is known to successfully simulate human walking behavior. MA is used in combination with Kernel
Density Estimation (KDE), composing a promising but more complex method.
Taking into account reaction to a possible collision or deadlock situation is performed on a local level,
data solely collected from laser range finders is considered sufficient.
By constructing a map of predictions and sending it to the local planner a faster response to risky
situations is obtained.
In order to facilitate the task of conceiving complex robot applications, the Robot Operating Sys-
2
tem(ROS)1 running on Ubuntu is used by the SocRob@Home team, and also by this thesis, as it pro-
vides a base to create and run applications across varied robotic platforms.
Considering not only the possible contributions for the team but also the need to further develop and
test the algorithms adressed in this thesis, robots available at SocRob@Home, Figure 1.1, are used to
experiment and evaluate this work.
1.3 Objectives
The main objectives of this work contemplate:
• Development of an agent motion model that accounts for agent uncertainty;
• Formulation of an agent predictor valid for both static and dynamic obstacles;
• Integration of an agent-aware module in the SocRob@Home project;
• Experimental validation of the work developed.
Figure 1.1: MBot, current service robot for the SocRob@Home Team
1.4 Thesis Outline
The present research work was carried out inside IST’s facilities (Lisbon, Portugal) namely in the
Mobile Robotics Laboratory in collaboration with SocRob@Home Team.
Summarizing, the thesis is structured as follows:
1http://www.ros.org
3
• Chapter 1, Introduction, intends to set the scenario in which the developed work stands. It presents
the main challenges regarding the agent motion model development and also familiarizes the
reader with the research goals;
• Chapter 2, Background, presents the basic concepts of this thesis. A brief description of what
comprises detection, perception, motion planning, tracking and prediction is given as its considered
knowledge required to better understand the processes further developed;
• Chapter 3, Related Work, summarizes research previously carried out i.e. some state-of-the-art
regarding Agent-Aware Navigation (AAN). The challenges that concepts of Chapter 2 impose are
revisited by analyzing state-of-the art application methods envisioned to be applied;
• Chapter 4, Approach and Implementation, where a description is given of the two different ap-
proaches used to predict motion based on previous data: parametric approach considering BM
and non-parametric approach contemplating MA and the use of KDE. In addition, a detailed de-
scription on acquired data processing and implementation of the approaches is given. Moreover,
the interpretation of applied methods and adaptation to the used software, hardware and problem
is explored;
• Chapter 5, Evaluation, the methods are tested and compared in different simulations as well as
real situations. The analysis comprises not the only success of the prediction but concerns as well
computing time of both methods and uncertainty area. The obtained results are interpreted and
discussed;
• Chapter 6, Conclusions, presents the main conclusions of this thesis as well as some suggestions
for future work.
4
Chapter 2
Background
A variety of skills is needed to overcome the challenges imposed to a robot deployed in a populated
domestic environment scenario.
For the sake of performing required tasks, the robot has to autonomously navigate, meaning, to plan
and execute a path that leads him from start to end point.
In order to do so, it is essential for it to become aware of its surroundings, the static and dynamic
obstacles present in the environment, using sensors.
In addition, for a correct and efficient motion planning, considering a scenario populated by humans
and non-cooperative robots, the need not only to track but also to predict the motion of moving agents
arises.
This chapter intends to briefly present basic concepts needed to understand how to tackle the diffi-
culties that can arise in detection, perception, motion planning, tracking and predicting.
2.1 Detection
As defined in [1], detection is the activity of determining the presence of something.
The process of detecting the surroundings is essential to provide the robot with information about the
state of the environment and the changes that may occur. Therefore, robots are equipped with a variety
of sensors.
Commonly robots are provided with RGB (Red, Green, Blue) Cameras, laser range finders and scan-
ners, that yield point clouds.
Sensor fusion is the combination of output from at least two sensors.
Sensors are vulnerable to noise and measurement errors. This technique attains a more robust
and wide coverage of the surroundings. By using multiple sensors to determine the same property
uncertainty can be lowered and resolution increased [3] [4].
Furthermore, the environment is often only partially observable from each sensor, by fusing the mul-
tiple sensors data it is possible to obtain a more complete perception of the environment that otherwise
5
would be unavailable or challenging to obtain from a single sensor.
Even though the advantages of sensor fusion are clear, this thesis will only consider data from the
front and rear laser range finder.
The principle behind Laser Range Finders relies on the time-of-flight, time difference, between the
transmitted and received signal [5].
A laser signal is transmitted from the device, when faced with a target the signal is reflected, Figure
2.1.
Figure 2.1: Laser functioning base parametersWhere di represents the targeti to device distance and βi the angle of targeti detection
By determining the transit time of the laser signal, t, and considering its speed, c as speed of light in
the atmosphere, the distance between device and target, di, is obtained, equation 2.1.
di =ct
2(2.1)
By contemplating only the measures obtained with the lasers, the results of this thesis can be applied
to simple systems, only provided with the type of information attained from this type of sensors.
2.2 Motion Planning
Once the environment is characterized the robot is able to navigate. A plan of intended motion must
be determined to lead the robot to the desired location. Generating the action sequence that allows the
accomplishment of a given goal is called planning [6].
The environment’s state not only depends on previous actions and states but also influences future
ones. In order to achieve a feasible and efficient plan, the systems should account for constraint satis-
faction, carrying along and reasoning about the variation of world states with time, as different actions
take place.
6
2.2.1 Global Planners
Global planners design a plan to navigate through the environment based on a global model of it and
taking into account given constraints and goals [7].
Sampling-based methods are a class of motion planning algorithms that yield solutions from distri-
bution samples.
These planners obtain information about the environment by generating robot placing, checking if
collisions may occur and store the obtained information samples, not requiring the explicit constraints.
Distinct techniques to achieve the samples can be applied creating different sampling-based methods
[8], for instance Probabilistic RoadMaps and Rapidly-exploring Random Trees are widely used and
highlighted ones.
Open Motion Planning Library [9] is an open source software library for sampling-based motion plan-
ning that incorporates implementations of multiple modern algorithms. Since it is available for operations
in the ROS, it would be a convenient library to utilize in this work as it simplifies planning and optimization
of motion trajectories.
However, sampling-based algorithms are more likely to generate random and unnecessary naviga-
tion movements when compared to other methods. Even though it is feasible to enhance and optimize
the generated solutions, these methods are shown to be more adequate to use in arm motion planning.
Search-based algorithms can help tackle planning and constraint satisfaction problems.
Search is the mechanism of considering a arrangement of events to grasp a goal. Once a goal is
defined, and in order to solve the problem, the search algorithm recommends a sequence of actions that
lead to a solution [6].
In order to apply these algorithms it becomes essential not only to describe the world but also to do
so discretely.
In the context of graph and tree search1, nodes can be described as the intersection or branching
of pathways in a network. A state space can be defined as a network in which the points, nodes, are
states connected by a sequence of actions, involving all achievable states derived from the initial one
according to a transition model.
Among the various search methods A* and Dijkstra are the most used when it comes to robotic nav-
igation global planning as they provide the shortest path available.
Occupancy grid mapping
Occupancy grid mapping is a method from which a map of the environment can be built from noisy
and ambiguous data, with an established robot pose. The key idea is to partition the world into cells,
discrete regions, encode and store in each one the probability of being occupied by an obstacle.
1The main difference between tree and graph search is that tree search visits the same state several times if there are multipledirected paths to it and graph search avoids revisiting the same state by keeping track of the visited ones.
7
Bearing in mind that precision in obstacle avoidance is enhanced at lower speeds, in [10] the algo-
rithm is designed for a dynamic occupancy grid mapping.
Grids’ dimension changes with the robot’s velocity, widening or shrinking as speed increases or de-
creases, respectively. Thus achieving a time-based occupancy grid of the environment.
The simplest way to navigate would just comprise mapping the environment and globally plan a path
to the goal considering given constraints. Consequently, if an unexpected path obstructing obstacle
appeared, sensors would detect it and a new global plan would have to be computed with the new map
of the environment, Figure 2.2. If the obstacle is detected too late, meaning too close for the robot to
have time to recompute the plan, stopping may be required.
Figure 2.2: Navigation flowchart considering Detection and Global Planning
8
2.2.2 Local Planners
Since the environment can change while the robot is navigating, it is imperative to have a planner
that analyzes if there is any change at the local level that could impair navigation and, if so, adapt, Figure
2.3.
(a) Global Plan example
(b) Unexpected obstacle obstructs planned path
Figure 2.3: Environment change and the need for Local Planners
This reactive property of local planners enables the avoidance of unexpected obstacles while still
considering the convergence to the given goal [7].
Potential Fields are frequently applied in robotics associated to local navigation planning [11].
This approach is built upon virtual forces on a vector field. The presence of repulsive forces around
the obstacles pushes the robot away from them, avoiding collision. Attractive forces directed to the goal
draw the robot to the given target. The arrangement of the referred forces along a vector field spread
through the environment create the solution plan, the strongest field, Figure 2.4.
The Dynamic Window Approach (DWA) is a reactive approach to collision avoidance [12].
By incorporating the dynamics of the robot this method is able to reduce the search space consider-
ing only velocities and accelerations attainable within a brief time interval free from collisions. An optimal
solution is selected from the valid search space obtained, the heading and velocity that allows the robot
to reach the goal with maximum clearance from obstacles, Figure 2.5.
9
Elastic Bands rely on the idea of a deformable collision-free path.
A real-time deformation of a path, generated by the global planner intending to solve some task,
in order to handle surrounding local changes, that sensor identifies, thus avoiding collision [13]. The
robot is reacts properly to unpredicted uncertainties and obstacles as the elastic band deforms, thereby
obtaining a smooth and cleared path while preserving the overall nature of the outlined path, Figure 2.6.
Figure 2.4: Potential Field Local Planning example
Figure 2.5: Dynamic Window Approach Local Planning example
Figure 2.6: Elastic Band Local Planning example
10
With the aid of local planners, the process of navigating is improved. If sensors detect an unexpected
obstructing obstacle, instead of new global plan, a local plan would be computed thereby shortening the
computing time and complexity, Figure 2.7. Once again, the need to stop may be required if the obstacle
is detected too late.
Figure 2.7: Navigation flowchart considering Detection, Global and Local Planning
2.3 Perception
Perception can be defined as the ability to become aware of something through senses [1].
When navigating through crowded environments and in order to avoid collision the ability to percept
obstacles as static or dynamic becomes essential.
Since the environment this thesis considers can be populated with humans and robots, and consider-
ing that the latest can be presented in almost any shape and size, the perception of such with unknown
dimensions implies perceiving any moving obstacle.
Although this thesis will not be focusing on perception, but instead in real-time prediction and inte-
gration in planning and navigating, it is important to mention some concepts and aspects of this field.
11
Optical Flow is a motion detection method emerged to tackle the problem of recognizing movement
when processing image sequences [14].
By analyzing divergences in a chain of images a classification of moving obstacles is achievable.
Concerns with this method arise when it is applied to sequences of images retracted from a moving
camera, like one attached to a moving robot.
In [15], a moving camera, like the one that would be available for this thesis, was utilized and the
results obtained were shown to be impractical due to intermittent and high false rate detections.
Machine learning systems process raw data in order to obtain a convenient internal representation
from which becomes possible to detect, recognize and classify patterns [16].
Deep learning is a type of machine learning. It enables the perception of the universe as hierarchy
of concepts, building complex conceptions from simpler ones, as it gathers knowledge and learns from
experiences. An example of a deep learning algorithm is You Only Look Once (YOLO) [17]. YOLO
approaches object detection as a regression problem. A single neural network is responsible for fore-
seeing spatially detached bounding boxes and associated class probabilities directly from full images in
only one assessment. It only needs to ”look once” at an image to envision where and what the items are.
Although differentiable as individuals, human beings share a great deal of similarities. Thereby, and
when comparing to general moving agent perception, a more accurate detection of humans is achievable
since shared characteristics are more restrict.
Pedestrian detection is a fundamental issue in computer vision due to its importance in several
applications. Therefore this field has been the core of many recent researches.
Within the domains of pedestrian detectors emerge face, leg and upper body detection algorithms.
An overview of pedestrian detectors available is done by [18]. Highlights in this field go to The Fastest
Pedestrian Detector in the West [19] and to The face detection method aggregate channel features [20],
as the pedestrian detector with the most interesting aspects, achieving one of the best relative perfor-
mances and lowest log-average miss rate in detection, and as a powerful representation with fast feature
extraction, when focusing on the potential implementation into less capacitate devices, respectively.
2.4 Tracking and Predicting
As previously referred, the environment is expected to suffer changes even while the robot is navi-
gating.
This thesis acknowledges the usefulness of predicting the next state of the environment in the motion
planning process. In order to do so, obstacles must be not only perceived but also tracked enabling the
study and prediction of their motion.
There are varied methods capable of following the movement/progress of a point or a conglomerate
of points describing an obstacle, thus tracking.
12
K Nearest Neighbor, this method determines for each given point the k nearest elements of a group.
The most commonly utilized distance metric is Euclidean distance. When utilized between frames the
nearest neighbor algorithm returns the smoothest transition possible, minimum feasible distance and
velocity [21].
In addition to these a variety of prediction methods can be used for tracking.
The correspondence between points can also be determined by analyzing the previous behavior and
predicting the next point.
When using Statistical Methods to predict the next location of an obstacle, the tracking is com-
pleted by finding the point matching that maximizes probability. In other cases, the correspondence can
be obtained by minimizing the distance between the detections and previous predictions similarly to the
methods previously described [22] [23] [24].
When navigating through crowded environments and in order to avoid collision it is essential not only
to detect but also to track and predict motion of others. The challenge concerning prediction arises from
the stochastic property of the environment, there is uncertainty in the behavior of a mobile agent.
By discretizing the environment in time, the properties of each state can be analyzed and utilized to
predict the following states.
Markov Chain Monte Carlo (MCMC) is a commonly used process in prediction.
This method is a subclass of the Monte Carlo Method (MCM) whose main idea is the usage of
random experiments to solve deterministic problems [25]. By repeating enough experimentations, ei-
ther naturally or artificially, MCM obtains random or pseudorandom samples sufficient to effectively use
statistical inference methods and achieve valuable results.
An example of MCM application is area determination, Figure 2.8, equation 2.2.
(a) Blue area, Ablue, unknown (b) Randomly place points inside the rectangle
Figure 2.8: Monte Carlo Method used for area determination
Ablue = Arectanglenumber of points inside blue area
total number of points inside the rectangle(2.2)
13
Markov Chain is a way to model a sequence of conceivable events where the probability associated
to a certain event only depends on the state of the preceding event [26]. In Figure 2.9 a Markov Chain
with events A, B and C is represented. The associated probabilities are characterized by (2.3).
Figure 2.9: Markov Chain example
PAA = 1 − (PAB + PAC)
PBB = 1− (PBA + PBC)
PCC = 1− (PCA + PCB)
(2.3)
The essence of MCMC is to limit the distribution of a Markov chain to a target distribution from
which is desired to sample from [26]. Variance estimation has to account for the dependence between
samples, which arose from implementing a Markov chain, while estimates of predictions and errors are
achieved by balancing the samples [27].
A Hidden Markov Model (HMM) is a particular case of Finite-State Automata (FSA).
A FSA can be described as an abstract machine which can adopt at any moment a single state,
within a finite number of listed states. The conditions for transitions between states, that may occur in
response to external inputs, are defined as well as the initial state [28].
Markov Models emerge when the transition function, of FSA, is of the probabilistic type and the
relation between observation and state is unique. In HMMs the available output events are generated,
with some probability, given that the emission function is also of the probabilistic type, they are no longer
perceptible from the input.
• In [29] a distinct approach to HMM emerges, Growing Hidden Markov Model (GHMM).
Although similar to HMMs, GHMMs are able to adapt the transition structure as well as the number
of states as observation sequences are processed. In addition, and due to the learning algorithm
in this approach, GHMMs are capable of updating the model as they learn its parameters.
Linear Kalman Filters, or Linear Quadratic Estimations (LQEs), are projected to layer out unwanted
noise from data flow, increasing the accuracy from inaccurate data [30].
14
A joint probability distribution of unknown variables in each time period is estimated by the gathering
of several, corrupted, measurements over time. The algorithm is then able to produce more accurate
estimates of the current state of those variables and their uncertainties.
In LQE ”noise” vectors are added to account for uncertainties in the system’s dynamics that also
assumes linear dependence between consecutive states. Furthermore the noise is considered Gaus-
sian with zero mean. The algorithm is recursive and the probability distribution is updated at every new
observation [31], Figure 2.10.
Figure 2.10: Basic Kalman Filter operation
• Since the model used for the process is given by a linear dynamical system excited by white noise
adjustments may have to be applied in order to adjust the linear approximation at each filter update
in a non-linear system. This extension is named Extended Kalman Filter.
Finally, with prediction of the next states of the environment this thesis expects to improve even more
the navigation process. While sensors keep scanning the environment, the predictions of the next states
are updated. If the a close obstacle is predicted a local plan is computed, Figure 2.11.
If the prediction algorithm displays good results, the belief is that no obstructive obstacle will ever
come as close as it would come with the previous mention motion planning methods. Consequently, by
delivering a constructed predictions map to the local planner an early response would be obtained and
the robot wouldn’t need to stop.
15
Figure 2.11: Navigation flowchart considering Detection, Global and Local Planning combined withPrediction
2.4.1 Stochastic processes
Random events can occur in any natural environment.
Real physical processes such as speech, audio and video signals, blood pressure and temperature
may seem too random and consequently establishing a pattern or order for them encompasses a difficult
task.
Stochastic/random processes can be defined as compilations of random events, variables [32] [6].
In probability theory, stochastic processes are described as systems evolving with time represented
by some variable subject to random fluctuation.
Even with the randomness inherent to some systems, the sequence of random events characterizing
them can be described, in different settings, by stochastic processes such as the Bernoulli, Wiener and
Poisson process.
Random Motion
The Wiener process is a well-known continuous-time stochastic process with several mathematical
and physical applications. This process is commonly associated with BM theory, emerged in 1827 with
Robert Brown, as it can be used to study particles’ erratic motion analysis [33].
16
Particles move quickly and in random directions as they collide with each other. Statistical examina-
tions determined that considering a particle and its motion with time, a Gaussian distribution with mean
0 and variance α(t), α(t) ≥ 0, characterizes the distribution of x(t+s)−x(t), x(t) being the x-coordinate
random variable of that particle at time t and α(t) a function based on physically determined constants.
In addition, it was proved that x(t2)−x(t1), x(t3)−x(t2), ... , x(tn)−x(tn−1) are mutually independent
as long as t1 < t2 < ... < tn [34].
Even though BM can be extended to many other discussions, this work will be focusing on the
referred characteristics and adapt to the interpretation of a moving agent.
17
18
Chapter 3
Related Work
To the best of our knowledge the field of navigation with regards to general agent awareness hasn’t
received an extensive treatment by the literature.
Nonetheless, Human-Aware Navigation (HAN) has been the focus of many researches. The existing
approaches to this field include actions of understanding, deliberating and reacting in order to tackle
challenges in producing a natural motion while considering human comfort and social constraints [8].
These actions encompass a group of procedures we found applicable, not only to HAN but also to
Robot-Aware Navigation (RAN) and more generally, to AHN, Figure 3.1 where the connections in the
diagram show main data flow.
Figure 3.1: Schematic of relevant functionalities in agent-aware navigation(adapted from [8])
The Understanding portion of the schematic displayed in Figure 3.1 collects information about the
environment and self-location of the robot.
Considering the intention of this thesis, perception is divided into dynamic and static perceived obstacle.
An agent is now defined as any moving obstacle, thereby covering more than RAN and HAN, but a
19
general AAN. If the obstacle is considered an agent, it is tracked and its movement predicted, on the
other hand, if it is considered a static obstacle it is added to the environment’s map and considered
stationary.
The Deliberating group of functionalities is designed to consider not only the requested action but
also the final pose1 intended to better compute a path plan. On account of maintaining consciousness
regarding social constraints and human comfort, which includes desire for a more natural motion, while
navigating, behavior awareness is side-by-side with path planning.
Lastly, the Reacting portion of functionalities contemplates not only the action achieve by actuators
but also the plan to reacting on a local level i.e. local planning.
With the work developed we expect to aid motion planning by recognizing dynamical obstacles and
predicting its behaviors to later incorporate on the motion planning algorithm while guaranteeing hu-
mans’ safety and comfort. Therefore, this thesis will predominantly explore Understanding group of
functionalities but also the computation of safe navigation, a part of behavior awareness.
3.1 Human Acceptance
HAN is a well-approached topic in robotic related literature.
Hallway settings are one of the most challenging to human acceptance of robot navigation. The
arrangement of a long passage with doors leading to different rooms, a narrow corridor, implies a more
careful approach to human-robot interactions as it stresses the importance of motion behavior to ensure
humans’ safety and comfort.
This type of setting is explored in [35]. The paper intentions are to emulate human-human interac-
tions in human-robot ones.
The field of human interactions has been extensively studied. A concept arises to describe a virtual
personal space around humans, proxemics.
Spatial zones are defined as intimate, personal, social and public. Although personal space may vary
between different cultures, the intimate zone is conventionally defined to assume values up to 0.45m,
personal distance is socially appropriate for family and friends and ranges from 0.45m to 1.2m, social
zone is commonly used for casual interactions, appearing to be more indicated for human-robot interac-
tion, and extends from 1.2m to 3.5m and public distance comprises the remaining space, adequate for
one-way or no interaction at all adapting to situations of avoidance while navigating.
These zones are socially respected according to corresponding situations. Only by respecting the
minimum socially accepted distances both comfort and safety perception as well as social acceptance
from humans may occur [2].
In [35] the acceptance level, of people with limited experience among robots, is determined by ma-
nipulating robot’s velocity, lateral separation and start of maneuver distance between both agents, called
the signaling distance, that shows the robot’s intention of performing a maneuver, Figure 3.2.
1The pose of an object combines its position and orientation
20
Figure 3.2: Behavior parameters and desired trajectory for passage maneuver(adapted from [35])
For this user study, a velocity focused tracking module was implemented to determine the most
appropriate robot reaction. To cope with multiple agents a particle filter was utilized.
The Person Passage method, where the human-robot distance is maximized in the passing point,
was applied to navigate amongst dynamic agents as a local mapper maintained the list of closest obsta-
cles.
A better response was obtained with higher robot speed, bigger lateral and signaling distance. Lateral
separation emerges as the elected crucial parameter.
The conclusions acquired reinforce the relevance of this work. Robot navigation with prediction
of agent motion provide a faster response to a close encounter thereby enabling a larger lateral and
signaling distance. Consequently, there is an increase in the acceptance level but also in safety, which
is crucial not only to humans but all type of agents.
3.2 Person–Acceptable Navigation
Social conventions are granted soft constraints2 in addition to the required common hard constraints3
for robot’s navigation in the developed work [36]. Not only personal space but also conventional crossing
side, which may vary according to different cultures, are accounted in the planning phase.
As introduced and briefly characterized in Section 2.2.1, search-based methods are commonly uti-
lized as global planners. The referred work uses A*, a search-based method, for path planning. A
costmap of the static portion of environment is provided by previously scanning it.
2Soft constraints allow surpassing certain limits by associating determined costs3Hard constraints provide an absolute limit
21
A*
A* search algorithm not only considers the path cost, between nodes4, but it also includes an heuristic
function.
Heuristic functions are based on available information’s analysis to better rank the branches to follow,
shortening the search period. A cost estimate of the cheapest path from current to goal node, which
reaches zero in the latter, is returned by this function.
The sum of path cost, g(n), with the heuristic function, h(n), returns an evaluation function, f(n)
(equation 3.1).
f(n) = g(n) + h(n) (3.1)
Nodes’ expansion order is based on the lowest evaluation function and the algorithm is finished when
the goal node is selected for expansion [6].
In the referred case, path cost is increased when approaching obstacles. If a person is detected,
concerns with proxemics arise and the algorithm applied utilizes Gaussian functions to model an elliptical
shape, dependent of the relative human-robot velocity, indicating the personal space to be respected.
A ”pass on the right” approach is preferred when moving in opposite direction, which can be switched
to left if applied in cultures with different social models. This is enforced by increasing costs on the right
side of a person perceived in the environment which still enables the possibility of passing on left in need
cases.
Costs also account for robot’s direction and velocity, favoring constant speed and heading.
The person-tracker used is laser-based. An apriori map of the environment is utilized to isolate non
matching scan data then segmented into person’s shape. The shapes are tracked using particle filters.
The person’s next location prediction is achieved by using an estimation of velocity.
Dijkstra
Dijkstra is the used search-based global planning algorithm in SocRob@Home team robots. Similarly
to A*, this algorithm returns the shortest path available considering constraints, by adapting them in a
weighted graph.
In fact, Dijkstra can be described as an A* algorithm with null heuristic function, h(n) = 0. A good A*
heuristic prioritizes better nodes minimizing the explored paths when compared to Dijkstra. Nonetheless,
it requires more memory and more operations per node than Dijkstra.
As previously mentioned, it is possible to build a state space, a network with states(nodes) connected
by action’s sequences, starting from an initial state and involving all reachable ones according to a
transition model.
Dijkstra firstly assigns a tentative distance value, T , from the initial node, i, to each one of the other
nodes. For all nodes, A, this tentative distance is set to infinite, T (a) = ∞,∀a ∈ A , indicating there
wasn’t yet explored a path from i to a For the initial node, i, distance is set to zero, T (i) = 0
Since this is a graph search method, sets of visited and unvisited nodes are kept.4Nodes, in search methods, are defined as networks’ intersection or branching of paths.
22
In each iteration, if unvisited nodes exist, all neighbor unvisited nodes are considered and a new
distance is calculated, D, by adding the path cost from current node to neighbor, n, to the tentative
distance of the current node, c.
D(n) = T (c) + pathcostc−n (3.2)
If neighbor’s tentative distance value T (n) is bigger than D(n) the first is replaced by the latter,
T (n) = D(n) = T (c) + pathcostc−n.
Current node is marked as visited, and removed from the unvisited set to avoiding rechecking it
again, when all its unvisited neighbors are considered. The node with smaller tentative distance, T , in
the unvisited set is chosen to be the new current node.
The algorithm ends when the goal node is marked as visited or when all tentative distances of
unvisited nodes are infinity, denoting connection absence between initial and goal node.
3.2.1 Accounting for Personal Space
In order to globally plan navigation environment’s map is used. Global planners, like A* and Dijkstra,
analyze the possible paths based on those static maps.
When faced with an unexpected obstacle, human or other, its necessary to remap on a local level
and account for personal and safe space.
As briefly described in Section 2.2.2, local planners are responsible for enabling this reactive prop-
erty of motion planning. Sensors detect real time obstacles in the closer surroundings. If unexpected
obstacles are detected, local planners enable its avoidance while continuing to consider the given initial
goal [7].
DWA is the collision avoidance reactive approach used by the SocRob@Home team, [37].
Robot’s dynamics are incorporated by this method in order to reduce the valid search space. By
considering only attainable velocities and accelerations in brief time intervals, the possible next positions
are reduced to only feasible ones for the robot in use [12].
The referred method solution is the one that enables goal reaching with maximum clearance from
considered obstacles.
This thesis main goal is to predict the next states of the closer surroundings and provide it to the
local planner. With a response based on next states’ prediction, the obstacle avoidance process begins
earlier and is able to provide surrounding agents with more personal space, thereby increasing both
acceptance and safety.
3.3 Navigating with dynamic obstacles
MECO research group members deliver in [38] the culmination of several researches done by the
referred group regarding a spline-based motion planning method. In the work developed, planned tra-
jectories are described with splines 5 increasing complexity when compared to a straight-line approach.
5Splines are polynomial functions, linear combinations of B-spline basis functions.
23
Even though the referred work deals with cooperative robots, the communication between agents
doesn’t give away complete paths. A prediction model is applied using current position, pk , and velocity,
vk, to linearly compute the prediction in time, ppredk (t), for obstacle k (equation 3.3).
ppredk (t) = pk + t vk (3.3)
Both predicted position of obstacle k and robot’s self-location is returned as a point. Nonetheless,
the applied collision avoidance algorithm has to account for all agents, robot and obstacles, silhouette.
In order to avoid contact between agents, the condition obtained is for the robot’s shape not to overlay
any of the obstacles. In a dynamic environment with Nobstacles, consequently k = 1...N , the constraint
condition is described by equation 3.4 [38].
dist(veh(q(t)), obsk(ppredk (t))) ≥ εt , ∀t ∈ [0, T ] (3.4)
Where ε is a considered safety factor intended to assure a secure separation between robot and
dynamic obstacles. By minimizing T the time-optimal path is found.
Although this provides a good base for an obstacle avoidance method supported by prediction, the
safety factor appears unreliable when considering unknown agents’ shape. In addition, if the agents
change velocity with time, the simple linear prediction will fail.
This thesis intends to counteract the indicated fault. Computing k obstacle uncertainty for k = 1...N
based on set of previous velocities leads to a more accurate and directed safety radius, that will increase
for more hesitant agents and decrease for stabler ones consequently aiding the human acceptance level.
The problem with unknown shape of agents is disregarded by focusing this work in detected, tracked
and predicted points.
After computing all predictions a map is built and sent to the local planner responsible for an adequate
response.
3.4 Predicting with Uncertainty
Human motion and inherent large uncertainty were target of research done in [39].
A stochastic process model is used to compute probability distributions relative to humans’ presence
and motion in the environment.
As referred in Section 2.4, Markov chains model sequences of events by associating probabilities to
state transitions. The occurrence probability of an event only depends on the previous event state [26].
An extension to Markov chains are Markov decision processes. In the latter, state transitions depend
not only on current state but also on the actions that lead to the next possible states. In order to
determine the state transitions’ probabilities this type of process uses heuristic functions.
In the referred research, state transition’s probabilities are heuristically determined by considering
direction, speed, collision probability and site.
24
It is intuitive that maintaining direction and speed is preferred, human natural motion tends to follow
straight lines at constant speed. Nonetheless, the higher probability of changing direction is accounted
for when the agent is in a crossing area. In this situation, decreasing speed may have higher probability
as well.
Collision probability increases when agents approach obstacles, inherently the agent will most likely
deviate. The probability of moving towards a wall is lower than moving away from it.
Finally, certain places have higher probability of agent’s determined motions. For instance, near the
couch humans usually pause for sitting.
While recognizing the advantages of implementing a similar algorithm, the added needs considered
for adaptation to an AAN application are enormous. In addition to building a general agent perceiver a
more detailed characterization of the environment would have to be computed.
Nevertheless, the idea of using stochastic processes to model agent’s behavior is a good base for
an agent motion model.
3.4.1 Uncertainty Margin
As previously discussed, associating direction, speed and places with action’s probabilities can be
used to reduce motion prediction uncertainty.
In addition, a safe margin modeled by agent related uncertainty can help reduce the risk of collision.
Various developed researches in model predictive control, such as [40] and [41], examine estimations
of position and velocity based on Kalman filters and consider safety margins surrounding agents based
on uncertainty.
Ellipsoidal and Gaussian shapes are commonly recognized as good approximations for setting safe
margins surrounding uncertain agents. Both shapes can adapt knowledge regarding intuition of constant
direction and speed. The larger portions of the referred forms contemplate the space with greater
probability, same direction and speed, while still considering the uncertainty inherent to the remaining
surrounding space.
By computing a collision free path that recognizes not only the agent shape but also a surrounding
bound, uncertainty aware, robot motion becomes safer and more acceptable.
This thesis contemplates single points as agents and computed associated uncertainty.
To less uncertain agents a smaller safety margin is associated. This reveals to be particularly useful
when both static and dynamic obstacles are processed in the environment.
Static obstacles will have very small uncertainty, considering sensor noise, and consequently small
safe margins. Very uncertain agents will have bigger safe bounds to account for the inherent uncertainty.
25
26
Chapter 4
Approach and Implementation
Bearing in mind the concepts and state-of-the-art addressed in Chapters 2 and 3, this work focuses
primarily on developing a prediction method. The intended algorithm is based on agents’ current position
and velocity data.
A common approach to prediction algorithms is to use the referred required data obtained directly
from a tracker. Trackers return the tracked obstacle position in different time intervals, x(t1), x(t2), ... x(tn).
The velocity can be estimated from V = ∆x∆t = x(ti+1)−x(ti)
ti+1−ti .
4.1 Prediction Computation
Using obstacles’ current position and previous velocity knowledge the prediction of motion can be
computed. There are various manners to process data.
A parametric model is defined by a finite set of parameters as opposed to a non-parametric model.
Typically a non-parametric model is based on arbitrary sets of data. More data is expected at every step
and consequently this approach appears to be more promising in terms of results but computationally
more complex.
Two prediction models are developed, based on parametric and non-parametric approaches. Later
on, this work compares both and reasons about performance.
27
4.1.1 Brownian Motion
BM, briefly described in Section 2.4.1, is the chosen parametric approach in this work.
As previously mentioned, when analyzing human walking behavior conclusions were taken regarding
the preference to maintain both direction and speed. Since BM was originally used to characterize erratic
movements, which is not the case of common human motion, applying it to agent positions would just
result in a random and not very good prediction of movement.
While considering the constant speed and direction tendency in motion, variations in both are ac-
knowledged. A initially stationary agent has zero velocity that increases when motion is initiated. When
stopping at a goal, agent velocity decreases until it reaches zero. The concept behind BM can aid in the
characterization of motion fluctuations.
In two dimensional (2D) agent motion, BM is applied to describe velocity, v, in x and y coordinates,
vx and vy respectively. Nevertheless, computing a variance based on physical constants becomes an
even more complex problem when referring to humans walking. In addition, this work intends to still
consider the particularities referred in agent motion.
Therefore, and since there is access to previous data, the Gaussian distribution used to describe the
velocities adopts mean, µ, and variance, σ2 determined by it.
Vbx ∼ N (µvx , σ2vx) (4.1a)
Vby ∼ N (µvy , σ2vy ) (4.1b)
Equation 3.3 is adapted to (4.2) in order to obtain a prediction of the next positions, (x(t+ ∆t), y(t+
∆t)), based on the previous ones, (x(t), y(t)).
x(t+ ∆t) = x(t) + ∆t Vbx (4.2a)
y(t+ ∆t) = y(t) + ∆t Vby (4.2b)
Vbx and Vby are distributions and consequently the results obtained for both x(t) and y(t) are as well.
The expected value of a random variable, X, with cumulative distribution function FX(x) is computed
as shown below.
E(X) =
∫ 1
0
xdFX(x) (4.3)
In Gaussian distributions, X ∼ N (µ, σ2) , the expected value corresponds to the mean of the distri-
bution, E(X) = µ.
The deviation of a random variable from its mean variance is called the standard deviation, σ. The
28
variance is the square of the standard deviation, σ2, and can be determined by equation 4.4.
V ar(X) = E(X2)− E(X)2 (4.4)
Where the expected value of the squared random variable is computed by:
E(X2) =
∫ ∞−∞
x2dFX(x) (4.5)
When relating two random variables it is useful to account for the relation between them, this measure
is called covariance (4.6).
Cov(X,Y ) = E(XY )− E(X)E(Y ) (4.6)
Considering the advantage of not only analyzing both velocity components but also reasoning about
the variation between them, a prediction contemplating covariance, instead of only variance, measures
would result in better conclusions.
A covariance matrix,∑
, for k random variables, Xi with i = 1, ..., k, is computed as shown in (4.7)
[42].
∑=
Cov(X1, X1) Cov(X1, X2) · · · Cov(X1, Xk)
Cov(X2, X1) Cov(X2, X2) · · · Cov(X2, Xk)...
.... . .
...
Cov(Xk, X1) Cov(Xk, X2) · · · Cov(Xk, Xk)
(4.7)
For this work problem and considering the following covariance properties:
Cov(X,X) = V ar(X) (4.8)
Cov(X,Y ) = Cov(Y,X) (4.9)
The covariance matrix to be computed is defined as shown in (4.10).
∑=
σ2vx Cov(vx, vy)
Cov(vx, vy) σ2vy
(4.10)
To improve the solutions (4.2) would obtain, the prediction of next positions is computed consid-
ering the multivariate normal distribution dependent of the covariance matrix, Vb ∼ N (µv, Σ) with
µv =[µvx µvy
]T, equation 4.11.
x(t+ ∆t)
y(t+ ∆t)
=
x(t)
y(t)
+ ∆t Vb (4.11)
The application of this algorithm returns not only one predicted point but a set of possible next
positions, Figure 4.2. The ellipse shaped point cloud formed by this set of points is considered to give
29
safety margins from the most probable predicted point, the center of the ellipse (xc(t), yc(t)).xc(t)yc(t)
=
x(t)
y(t)
+ ∆t µv (4.12)
Figure 4.1: Brownian Motion implementation flowchart
Since the distribution is computed with the covariance matrix of previous data, the uncertainty inher-
ent to the agent is acknowledged by this margin. With a more uncertain agent, the variance is higher
and the points are more scattered. The covariance delivers information about how the data is spread.
30
Figure 4.2: Predictions computed with BMRed and green dots represent start and end point, respectively.
Blue points represent the remaining obstacle positions.Yellow dots represent the next expected points.
The strongest yellow being assigned to the last set.
31
4.1.2 Metropolis Algorithm
For the development of a non-parametric approach, MA is chosen algorithm to be used in combina-
tion with KDE.
The Metropolis-Hastings algorithm is a MCMC method, briefly described in Section 2.4. This algo-
rithm generates several sample values from a proposed distribution and an initial guess. An evaluation
based on a function f(x), proportional to the density of the desired distribution, is performed to deter-
mine the acceptance of new samples or the continuity of the previous one. After several iterations, the
burn-in phase, the distribution of the accepted values approximates the desired distribution [43] [44].
MA is the particular case of the Metropolis-Hastings algorithm where the proposal distribution is
symmetric.
As discussed in Section 4.1.1 and bearing in mind human motion preference for constant speed
and direction, the proposal distribution to apply MA in this work is a normal distribution with mean and
variance computed from previous velocity data.
Once again 2D agent motion is considered and the velocity, v, is decomposed in x and y coordinates,
vx and vy respectively.
The initial guesses, V0x and V0y, for computing the predicted next velocities are the last registered
ones.
New possible values, V1x and V1y, are obtained by sampling from a distribution N (µv, σ2v) .
The acceptability, β, of a new sample is evaluated based on a function proportional to the density
of the desired distribution. This work obtains the mentioned function by computing the KDE of previous
velocities, this process is addressed in Section 4.1.2.
β =KDE(V1)
KDE(V0)(4.13)
A value, γ, is generated from a uniform distribution with minimum and maximum values of 0 and 1,
U(0, 1). If γ is lower than β the new V0 becomes V1, otherwise V0 value is maintained.
After the burn-in phase, the fluctuations of V0 values reduce and are registered. When the cycle of
iterations ends the average of the registered values, V , is the estimated velocity that will then be used
to compute the predictions, equation 4.11.
Kernel Density Estimation
As referred, this thesis uses MA with probability density function obtained with KDE. Based on pro-
vided data samples, KDE estimates the given data underlying function [45].
An idea of a KDE can be perceived by visualizing a discrete histogram taking the form of a continu-
ous curve. The smoothness of a KDE resulting function curve is influenced by a bandwidth parameter.
Higher bandwidths result in less squiggly curves, shallower kernels, since wider regions of points con-
tribute to the weighting.
To compute KDE, distance between data samples, zi with i = 1, ... n considering n samples , and
desired point, z, is weighted, by function f(z) (4.14), resulting in the assignment of greater density
32
estimation to regions with higher concentration of samples.
f(z) =∑i
αiK (z − zi) (4.14)
Uniform weights are commonly considered, αi = 1/n.
Different kernel functions, K, can be used to compute the KDE, producing distinct estimates, and
avoid the storing of complete data. Bearing in mind all the mentioned considerations about normal
distributions and taking into account this is the typically used form of K function, the computed KDE in
this work uses a Gaussian form of K.
Recalling equation 4.13, β is obtained by computing:
β =f(V1)
f(V0)(4.15)
By applying the described algorithm, MA with KDE, a set of n probable next velocities is obtained,
Vi = [Vxi, Vyi]T for i = 1 ... n. The predicted positions are obtained with (4.16).
x(t+ ∆t)
y(t+ ∆t)
=
x(t)
y(t)
+ ∆t Vi (4.16)
Similarly to description given in Section 4.1.1, the predictions form an ellipse shape cloud of points,
Figure 4.4. The data spread center, (xc(t), yc(t)), is computed with an identical equation, (4.12), con-
sidering the velocity values obtained mean.
From the obtained data both variance and covariance are determined and deliver information about
orientation and spread of data.
33
Figure 4.3: Metropolis Algorithm with KDE implementation flowchart
34
Figure 4.4: Predictions computed with KDE and MAThe vertical scale bar represents the logarithmic scale applied to the velocities’ density function.
Regarding obstacle position, red and green dots represent start and end point, respectively.Blue points represent the remaining positions.
Yellow dots represent the next expected points. The strongest yellow being assigned to the last set.
35
4.1.3 Uncertainty
The computed set of predicted points gives the idea of an ellipse shaped confidence region of pre-
dictions around the center, (xc(t), yc(t)). Nonetheless, analyzing regions defined by set of points is
complex and computationally that is not the best way to determine safe margins.
To directly compute the region of uncertainty it is necessary to analyze the covariance matrix of this
problem (4.10).
Spread and orientation of data is described by the covariance matrix. Eigenvectors of Σ represent
the directions by which the data is scattered with variance equal to eigenvalues of the matrix.
The eigenvalues, λ of Σ are the solutions of equation 4.17.
|Σ− λI| = 0 (4.17)
Displayed below are the resulting equations when applied to this work.
det
σ2vx − λ Cov(vx, vy)
Cov(vx, vy) σ2vy − λ
= 0 (4.18a)
λ2 − λ(σ2vx + σ2
vy
)+ σ2
vxσ2vy − [Cov(vx, vy)]
2= 0 (4.18b)
Consequently, the eigenvalues obtained, λ1 and λ2, take the form of (4.18c).
λ1,2 =
(σ2vx + σ2
vy
)±√(
σ2vx + σ2
vy
)2
− 4(σ2vxσ
2vy − [Cov(vx, vy)]
2)
2(4.18c)
In order to compute corresponding eigenvectors, e, the results of equation 4.19 are determined.
(Σ− λI) e = 0 (4.19)
When applied to this thesis problem, it follows: σ2vx − λ Cov(vx, vy)
Cov(vx, vy) σ2vy − λ
e1
e2
=
0
0
(4.20a)
(σ2vx − λ
)e1 + Cov(vx, vy) e2 = 0 (4.20b)
Cov(vx, vy) e1 +(σ2vy − λ
)e2 = 0 (4.20c)
The eigenvectors, e = (e1, e2), are not unique solutions. Common practice is to find the solution that
enforces e21 + e2
2 = 1.
e2 =λ− σ2
vx
Cov(vx, vy)e1 (4.21a)
36
e21
(1 +
(λ− σ2
vx
)2[Cov(vx, vy)]
2
)= 1 (4.21b)
By replacing λ with the found eigenvalues, λ1 and λ2 from (4.18c), the respective eigenvectors are
determined.
As mentioned above the eigenvalues of Σ represent the variance of scattered data along the corre-
sponding eigenvector. In order to compute an uncertainty ellipse it is necessary to guarantee that the
majority of predictions would fall inside the chosen shape.
Confidence Interval
Confidence intervals (CIs) return ranges at which the desired proportion of samples, assigned by
setting a confidence level, is believed to be, [42].
In a normal distribution, as (4.1), with known standard deviation, σ, the CI for n observations is
computed by firstly manipulating the known distribution to match N (0, 1) , equation 4.22.
V − µσ√n
= Z ∼ N (0, 1) (4.22)
Where V , called the sample mean, is a point estimator of µ, the population mean.
The intended CI for a confidence level defined by (1− α)100% can be computed by determining the
values of Z1 and Z2 that validate equation 4.23.
P (Z1 ≤ Z ≤ Z2) = 1− α (4.23)
Considering Z ∼ N (0, 1) it is comprehended that Z2 = −Z1, commonly symbolized as Zα/2. By
manipulating equations 4.22 and 4.23 considering the properties of Z, the CI is obtained.
P (−zα/2 ≤V − µ
σ√n
≤ zα/2) = 1− α (4.24)
For the set confidence level, an appropriate z value is obtained from the standard normal distribution.
Since the confidence level is commonly set at 95% or 99%, in table 4.1 are displayed the corresponding
values for α = 0.05, 0.01 and 0.001.
Confidence Interval 95 99 99.9
Zα/2 1,960 2,576 3,291
Table 4.1: Zα/2 values for common CIs
P (V − zα/2σ√n≤ µ ≤ V + zα/2
σ√n
) = 1− α (4.25)
The obtained CI is[V − zα/2 σ√
n, V + zα/2
σ√n
]37
The axis extension of the uncertainty ellipse correspond to the CIs range, obtained with the eigen-
values, σ =√λ, multiplied by ∆t :
2 zα/2
√λ
n∆t (4.26)
Finally, the ellipse is placed centered in (xc(t), yc(t)) and rotated to match the eigenvectors.
The computed confidence interval returns the maximum expected spread of velocity data. Consider-
ing Vx = ∆x∆t and Vx = ∆x
∆t , the expected position limits are obtained by equations 4.27.
x+limit = xc(t) +
width∆t
2(4.27a)
x−limit = xc(t)−width∆t
2(4.27b)
y+limit = yc(t) +
height∆t
2(4.27c)
y−limit = yc(t)−height∆t
2(4.27d)
Bayesian Credible Interval
The distribution obtained from using MA and KDE is not a normal distribution and the samples
extracted from it are not independent, otherwise using Central Limit Theorem an approximation would
be possible. Consequently, CIs as described in Section 4.1.3 are not applicable to this approach.
Bayesian Credible Intervals (BCI) are analog to CI.
The first step on the Bayesian method for inference is to choose the prior distribution, a probability
density function, f(θ), expressing prior expectations about a parameter, θ, without considering any data,
Vi for i = 1 .. n (in a data set with n samples).
Expectations exist about v given parameter θ and a statistical model is adopted to match them,
f(v|θ).
The model is updated considering the observation of the previous data Vi for i = 1 .. n, V1, ..., Vn
Finally, a posterior density distribution, f(θ|V1, ..., Vn), is determined using the Bayes’ Theorem,
equation 4.28d.
By understanding axioms (4.28),
P (A ∩B) = P (A|B)P (B) (4.28a)
= P (B|A)P (A) (4.28b)
P (A|B) =P (A ∩B)
P (B)(4.28c)
P (A|B) =P (B|A)P (A)
P (B)(4.28d)
38
Figure 4.5: Schematic of BM implementation
The conditional probability of parameter Θ given V is obtained as (4.29).
P (Θ = θ|V = v) =P (V = v, Θ = θ)
P (V = v)(4.29a)
=P (V = v|Θ = θ)P (Θ = θ)∑θ P (V = v|Θ = θ)P (Θ = θ)
(4.29b)
Which for continuous variables is computed using density functions as follows:
f(θ|v) =f(v|θ)f(θ)∫f(v|θ)f(θ)dθ
(4.30)
39
As described in [42], for n independent and identically distributed random observations V1, V2, ..., Vn
(vn = v1, ..., vn):
f(θ|vn) =f(vn|θ)f(θ)∫f(vn|θ)f(θ)dθ
(4.31)
With,
f(vn|θ) =
n∏i=1
f(vi|θ) (4.32)
Bearing in mind the explored definitions, a BCI, [a, b], is estimated by computing a and b values that
validate equation 4.33. ∫ a
−∞f(θ|vn)dθ =
∫ ∞b
f(θ|vn)dθ =α
2(4.33)
By setting the desired credibility level, (1− α) 100% analog to the confidence level in CI, the credible
interval [a, b] is obtained.
P (θ ∈ [a, b] |vn) =
∫ b
a
f(θ|vn)dθ = 1− α (4.34)
(a) Predictions computed with BM and CIs (b) Predictions computed with MA, KDE and BCIs
Figure 4.6: Computation of predictions with uncertainty ellipsesRed dots represent starting points, blue represent the remaining obstacle positions.
Green ellipses represent the ellipse where expected points are predicted to be.The strongest green being assigned to the last computed ellipse.
40
Figure 4.7: Schematic of MA + KDE implementation
41
4.2 Simple Algorithm Integration
The developed algorithm is implemented in Python1, programming and available libraries, integrated
in a ROS framework.
Even though the developed algorithm is adequate for a wider range of applications, the main goal of
this thesis was to incorporate it on the robots used by the SocRob@Home Team, Figure 1.1.
This robot is equipped with a RGB Camera, a (13.5 cm above) ground sensor, laser range finder,
and scanners that provide point clouds. A map is previously built and stored using the referred sensors
in a preceding scan of the environment.
Bearing in mind that the robot is deployed in a domestic environment, the obstacle avoidance problem
can be tackled in 2D, disregarding the z-coordinate (perpendicular to the ground). All obstacles are
considered to rise from the ground, avoiding collision in 2D, x and y coordinates, still guaranteeing a
collision free path in three-dimensional (3D) space.
4.2.1 Coordinate frames
While deployed, robot transmits various types of data that are often relative to different referenced
coordinate frames. The tf library is used to track them.
Frame to frame transformations are provided and represented by a position vector and a quaternion.
Quaternions are commonly presented in the form q = a + b i + c j + dk or q = (a, b, c, d), where
scalar, a, and vector, (b, c, d), part are described.
This number system is particularly convenient in representing 3D orientation and rotation.
An extended explanation of quaternions is out of the work scope. Nonetheless, considering the
necessity for 3D data analysis in aerospace applications with respective algorithm adaptation, some
properties are referred bellow.
i2 = j2 = k2 = ijk = −1 (4.35)
× 1 i j k
1 1 i j k
i i -1 k -j
j j -k -1 i
k k j -i -1
Table 4.2: Quaternion multiplication guide table
In order to apply the described algorithms it is necessary to adapt all registered data into the same
coordinate system.
1Python Software Foundation, https://www.python.org/
42
A map fixed frame, map frame, is advantageous as base frame, it allows a more intuitive perception
of the environment state.
4.2.2 Prediction Algorithm
Both prediction algorithms described in Section 4.1 receive as input velocity and current position data.
An obstacle prediction model is built to estimate environment changes in the following time frames.
BM, Section 4.1.1, is implemented utilizing the stats from scipy Python library. When computing
several points stats.multivariate normal.rvs provided the multivariate normal considering the covariance
and mean of the available velocity data.
When computing directly the data distribution, parameters such as the covariance are determined
using numpy functions.
Regarding uncertainty, the method described in Section4.1.3 takes advantage of stats.norm.interval
to directly compute the required CIs.
In Section 4.1.2, an approach combining MA and KDE was described.
For the MA general implementation a simple cycle was implemented. This cycle starts with initial
velocity guess, V0, to which the last observed velocity is attributed.
• V1, possible new values, are sampled from a distribution N (µv, σ2v)
• β = KDE(V1)KDE(V0) , acceptability is determined
• γ is generated from U(0, 1)
– If γ < β : V0 = V1
– if γ ≥ β : V0 = V0
• After burn-in phase, V0 is continuously stored in a list, Vlist
V , the estimated next velocity, is computed by averaging the values inVlist.
KernelDensity from sklearn.neighbors Python library was implemented to facilitate KDE computation.
The prediction ellipses determined as described in Section 4.1.3 utilize stats.bayes mvs from scipy
for computation of BCIs.
4.2.3 Relevant Time Interval
Current positions and previous velocity data are used to predict forthcoming motion. The longer the
robot is deployed the greater data amount.
After some time, the extent of data to be processed is excessive resulting in a computationally slow
algorithm. In addition, the relevancy of past velocities in prediction calculation has to be taken into con-
sideration. For instance, if an agent stops and the data is never deleted the predictions will continuously
incorporate adverse data.
The longer it passes since a velocity is registered, the less it is relevant in modeling the next positions
of the agents. It is essential to disregard older data to enhance the model of behavior.
43
A relevant time interval of data is defined and data excluded accordingly.
4.2.4 Predictions’ Map
A predictions’ map is built so that the local planner receives environment estimated predictions. This
map describes the occupancy of the space by partitioning the environment into cells and attributing
occupancy percentages.
In this work, cells only receive values 0 or 100%, for unoccupied or occupied respectively.
A cell is assumed occupied if the center of the defined cell is contained in any of the ellipses com-
puted by the prediction algorithm, Figure 4.8.
(a) Prediction ellipses (b) Corresponding occupied grids
Figure 4.8: Predictions’ Occupancy Grid Map
44
4.3 Extending the Prediction Approach
Even thought using previously built trackers is an appealing solution for integrating the prediction
algorithm, this thesis considers both human and robots thereby discarding most common trackers. Since
robots can take a wide variety of shapes, tracking undefined moving forms would be the safer approach.
4.3.1 Scan Processing
Whether the robot is navigating or stationary, the surroundings are scanned with laser range finders.
The data received from scans is relative to a robot frame, dependent of robot’s position and orientation.
In addition to the transmission of range and angle of the scans, maximum and minimum values of
both are associated to data messages. Data is processed in order to only consider the valid scan values.
The position of obstacles is determined in the map frame by considering the required transformations.
Considering 2D motion and a robot centered frame, robot frame, robot’s position in the map frame,
RMF = (xMFR , yMF
R ), corresponds to the transform from map to robot frame, ~S in Figure 4.9(a), equation
4.36. This transform is necessary for detected obstacle position computation in the map frame, PMF =
(xMFP , yMF
P ), given its position in robot frame, PRF = (xRFP , yRFP ).
~S = OMFRF −OMF
MF (4.36a)
= RMF −OMFMF (4.36b)
= (x, y)MFR − (0, 0) (4.36c)
= (xMFR , yMF
R ) (4.36d)
(4.36e)
By determining ~U , the obstacle coordinates in the map frame, PMF , given a scan detection with
a determined Range, D, at an angle β, Figure 4.9(b), are obtained directly, ~UMF = PMF − OMFMF =
PMF − (0, 0) = PMF .
~UMF can be defined as the sum of vectors ~S and ~T . Evidently, vector addition requires vectors
defined the same coordinate frame.~UMF = ~SMF + ~TMF (4.37)
~TMF = PMF −OMFRF (4.38a)
= (x, y)MFP −
[RMF −OMF
MF
](4.38b)
To continue determining vector ~T it is important to acknowledge some relations between generic
45
(a) Frames and vectors’ definition. Map-Robot (~S), Robot-Obstacle (~T ), Map-Obstacle (~U )
(b) Positioning components in map and robot frames
Figure 4.9: Environment framework
components, equations 4.39 and 4.40 considering the rotation matrix R, Figure 4.9(b).
(x, y)MF ′= (x, y)RF R (4.39a)
= (x, y)RF
cos(α) − sin(α)
sin(α) cos(α)
(4.39b)
(xMF ′, yMF ′
) =(xRF cos(α)− yRF sin(α), xRF sin(α) + yRF cos(α)
)(4.39c)
46
(x, y)MF = (x, y)MF ′+ ~S (4.40a)
= (x, y)MF ′+ (xMF
R , yMFR ) (4.40b)
(xMF , yMF ) = (xMF ′+ xMF
R , yMF ′+ yMF
R ) (4.40c)
=(xRF cos(α)− yRF sin(α) + xMF
R , xRF sin(α) + yRF cos(α) + yMFR
)(4.40d)
Considering the distance from point P to robot frame center, D,
(xRFP , yRFP ) = (D cos(β), D sin(β)) (4.41)
Resuming ~T determination (4.38):
~TMF = ~TMF ′(4.42a)
= (xMF ′
P , yMF ′
P ) (4.42b)
=(xRF cos(α)− yRF sin(α), xRF sin(α) + yRF cos(α)
)(4.42c)
= (D cos(β) cos(α)−D sin(β) sin(α), D cos(β) sin(α) +D sin(β) cos(α)) (4.42d)
= (D cos(α+ β), D sin(α+ β)) (4.42e)
Finally, ~UMF is computed.
~UMF = ~SMF + ~TMF (4.43a)
= (xMFR , yMF
R ) + (D cos(α+ β), D sin(α+ β)) (4.43b)
=(D cos(α+ β) + xMF
R , D sin(α+ β) + yMFR
)(4.43c)
= (xMFP , yMF
P ) (4.43d)
(4.43e)
Resulting in the position of the robot in the map frame (4.44)
PMF = (xMFP , yMF
P ) (4.44a)
(xMFP , yMF
P ) =(D cos(α+ β) + xMF
R , D sin(α+ β) + yMFR
)(4.44b)
While the environment is scanned, scan data is stored in map frame coordinates and velocity is
47
computed (4.45).
~vx =∆x
∆t(4.45a)
~vy =∆y
∆t(4.45b)
Bearing in mind the position data is already in map frame, velocities computed are as well. Conse-
quently, the described approaches can utilize the directly computed velocity values.
4.3.2 Multiple Obstacles
The algorithms used consider all data scanned by the laser range finders. To compute the velocities,
obstacle correspondence is required between time frames.
In order to arrange obstacles and their correspondence in time, the K Nearest Neighbor method was
applied. As briefly explained in Section 2.4 the application of such algorithm return a defined number,k,
of nearest element of a group.
NearestNeighbors from sklearn.neighbors Python library was applied to follow the movement of laser
points, tracking.
Two different groups of points, in separated time frames, are inputed and the distance between them
is computed. Each point is labeled with the found corresponding nearest points from the opposite group.
Euclidean distance is used as the criteria for finding the closest points.
Considering p = (px, py) and q = (qx, qy), points in 2D space, the Euclidean distance between them
is given by (4.46)
d(p, q) =√
(px − qx)2 + (py − qy)2 (4.46)
Clustering
When implemented in a real environment deployed robot the amount of data and detected obstacles,
every point detected by the laser range finders, is immense. In order to reduce computational time, a
clustering algorithm was utilized.
The clusters were computed with all registered positions, already converted to the map frame, using
KMeans from sklearn.cluster Python library. Each cluster, considered an obstacle, it is tracked and the
next position is predicted, with adequate uncertainty.
The uncertainty margin given to each cluster uses the cluster prediction ellipses. The same ellipse
is the uncertain attributed to each of the points composing the corresponding cluster, Figure 4.10.
If a static obstacle represented by a set of points is considered by a single cluster, the prediction
ellipse will be represented by a single point, the cluster center. In this situation, by applying the prediction
to all points encompassed by the cluster, the static obstacle will appear ”perfectly” represented in the
next time step.
48
(a) Formed cluster (b) Cluster prediction (c) Points prediction
Figure 4.10: Prediction ellipses using clusters
49
50
Chapter 5
Evaluation
The analysis of algorithms performance is based on key factors such as computational time the pre-
diction success. Evaluation of prediction success considers not only the validity of predictions, meaning
the predicted points fall inside the predicted ellipse, but also the created ellipses’ area.
An ellipse centered in (c1, c2), with radius a and b for x and y coordinates respectively, Figure 5.1, is
described by equation 5.1.
Figure 5.1: Ellipse with parameters definition
(x− c1)2
a2+
(y − c2)2
b2= 1 (5.1)
The validity of inequality 5.2 implies that the new point (xn, yn) falls inside the predicted ellipse.
(xn − c1)2
a2+
(yn − c2)2
b2≤ 1 (5.2)
Nonetheless, the inequality veracity is not sufficient to evaluate the methods’ performance. Higher a
and b values result in larger ellipse’s area (equation 5.3). Consequently, the probability of a new point
falling inside the predicted ellipse increases as well.
Aellipse = π a b (5.3)
Probability density functions are considered to adequately evaluate the algorithms’. The probability
of the new point is computed taking into account the previous known distribution.
The likelihood of each new point is computed for both methods. Higher likelihoods imply better data
51
modeling. Consequently, considering equal data, the best algorithm is the one that displays higher
likelihood.
In addition, computational time should be taken into consideration. The methods applied are to be
utilized in real time navigation. A method that takes too much computing time, even if accurate, will not
be able to deliver a response in useful time.
To compute a fair evaluation of both algorithms, several simulations are performed and real setting
scenario tests are executed.
5.1 Simulations
Two sets of simulations were computed to better evaluate the results of both approaches, single and
multiple obstacle simulations.
5.1.1 Single Obstacle
Single obstacle simulations, Figure 5.2, consist of five different motion types.
Simulation 1
Simulation 1, Figure 5.2(a), reproduces an obstacle with uniformly described motion.
p = p0 + v t (5.4a)x = x0 + vx t
y = y0 + vy t(5.4b)
Simulation 2
Simulation 2, Figure 5.2(b), replicates the uniformly accelerated motion of a detected obstacle.
p = p0 + v t+a
2t2 (5.5a)
x = x0 + vx t+ ax2 t2
y = y0 + vy t +ay2 t2
(5.5b)
52
Simulation 3
Simulation 3, Figure 5.2(c), assumes a detected obstacle with uniformly described circular motion.
θ = θ0 + ω t
x = r cos(θ)
y = r sin(θ)
(5.6)
Simulation 4
Simulation 4, Figure 5.2(d), mimics the uniformly accelerated circular motion of an obstacle.
θ = θ0 + ω t+ α
2 t2
x = r cos(θ)
y = r sin(θ)
(5.7)
Simulation 5
Simulation 5 replicates an obstacle with random motion, Figure 5.2(e). Velocities are randomly gen-
erated resulting in erratic trajectories.
5.1.2 Multiple Obstacles
In order to extend the evaluation to tracking performance, using k-neighbor algorithm, motion simu-
lation of multiple obstacles is required.
• Simulation 6, Figure 5.3(a), replicates two crossing moving obstacles with uniformly described
motion, Equation 5.4;
• Simulation 7, Figure 5.3(b), mimics two uniformly accelerated, Equation 5.5, obstacles intersect-
ing;
• Simulation 8, Figure 5.3(c), reproduces the motion of two obstacle with uniformly describing cir-
cular motion side by side, Equation 5.6;
• Simulation 9, Figure 5.3(d), two uniformly accelerated circular motion, Equation 5.7, describing
obstacles assume a crossing situation.
53
(a) Simulation 1 (b) Simulation 2
(c) Simulation 3 (d) Simulation 4
(e) Simulation 5
Figure 5.2: Single obstacle simulationsRed dots represent starting points, and blue represent the remaining obstacle positions.
54
(a) Simulation 6 (b) Simulation 7
(c) Simulation 8 (d) Simulation 9
Figure 5.3: Simulations with multiple obstaclesRed dots represent starting points, and blue represent the remaining obstacle positions.
5.2 Real Setting Tests
In order to test the performance of the completed algorithms in more probable scenarios, several
dynamic environments are replicated.
• Scenario 1: The robot is static and an agent walks in its surroundings, Figure 5.4(a);
• Scenario 2: Both robot and agent move in the environment;
• Scenario 3: Two agents cross paths in the surrounding space of a stopped robot, Figure 5.4(c);
• Scenario 4: Two agents walk close to each other in the surrounding space of a stopped robot;
Scenario tests were performed on MBot, Figure 1.1, where the implemented laser range finder is at
13.5 cm above ground with an approximate maximum range of 6m.
55
(a) (b)
(c) (d)
Figure 5.4: Scenario 1 in 4 different time framesThe red dot represents the robot position while black ones represent detected obstacles’ positions.
5.3 Results
As previously mentioned, the performance evaluation of both approaches is based on the analysis
of both likelihood and computational time.
A new point position probability is considered by taking into account the previous known distribution.
The log-likelihood of each new point is computed for both methods. As previously mentioned, higher
likelihoods, and consequently higher log-likelihoods, imply better data modeling.
Considering log-likelihood values depend on data and the tests performed are very diverse, the
overall model comparison is achieved by using the likelihood ratio, RL.
RL =LBMLMA
= explog(LBM )−log(LMA) (5.8)
Equation 5.8 is used to compare the models in study, where log (LMX) is the computed log-likelihood
of Model X. Values of RL > 1 suggest the superiority of BM model when compared to MA.
In addition, computational time should be taken into consideration. The methods applied are to be
utilized in real time navigation. A method that takes too much computing time, even if accurate, will not
be able to deliver a response in useful time.
56
(a) (b)
(c) (d)
Figure 5.5: Scenario 3 in 4 different time framesThe red dot represents the robot position while black ones represent detected obstacles’ positions.
Nonetheless, the tests applied are for different time intervals of recorded data and consequently a
simple comparison of time will not suffice for applicability conclusions. Instead, 5.9 value is determined.
∆tMX
∆tsimulation(5.9)
Where ∆tMXis the computing time of method X when running the simulation data in ∆tsimulation.
A fair assessment of both algorithms is achieved by testing the several simulations and real setting
scenarios previously mentioned.
5.3.1 Simulations’ Results
The obtained results regarding single and multiple obstacle simulations are displayed in Tables 5.1
and 5.2, respectively.
57
Simulation 1 2 3 4 5
∆tBM
∆tsim1,0 1,0 1,1 1,0 1,1
∆tMA
∆tsim425,3 402,8 469,2 434,6 449,2
RL 1,3 0,4 1,3 1,3 1,1
Table 5.1: Obtained Results for Single Obstacle Simulations (5 seconds relevant time interval consid-ered)
Simulation 6 7 8 9
∆tBM
∆tsim1,3 1,4 1,9 1,3
∆tMA
∆tsim837,7 888,3 2608,6 644,5
RL 1,7 0,9 0,9 0,7
Table 5.2: Obtained Results for Multiple Obstacles Simulations (5 seconds relevant time interval consid-ered)
These results prove that, as expected, BM is a much faster method than MA. Nonetheless, the
likelihood ratio indicates similar quality of prediction results for both methods.
The overall prediction validity in simulations is evaluated by the veracity of inequality 5.2, represented
in Figure 5.6.
Figure 5.6: Points Validity in Simulations
The high valid points percentage is a good indicator that not only the prediction methods have similar
quality but also both obtain valid prediction results.
In addition, and in order to reason about the relevancy of data with time passage, obtained results
with three different time intervals of considered data are displayed in Figure 5.7.
58
Figure 5.7: Likelihood ratio in Simulations with defined relevant time interval: 3, 5 and 7 seconds
The different time intervals of considered data displayed were the ones that achieved the best overall
results. On one hand, an excessively big relevant time interval not only slowed computational time but
also considers non relevant data thereby hurting the predictions. On the other, and even though a smaller
time interval influences positively the algorithms computational time, it may also influence negatively the
results as it discards relevant data.
5.3.2 Real Scenarios’ Results
The obtained results regarding real scenarios are displayed in Table 5.3. Likelihood ratios are trans-
lated into Figure 5.8 for easier comprehension.
Cluster implementation is required due to the great amount of data collected by the laser on these
kind of dynamic scenarios. The cluster factor parameter designates the averaged number of points
contained in each cluster.
Scenario 1 2 3 4
Cluster Factor 4 8 4 8 4 8 4 8
∆tBM
∆tscenario5 2,5 5 2,5 5 2,5 5 2,5
∆tMA
∆tscenario2400 1230 1660 1480 2550 1260 2500 1370
RL 2,7 3,3 6,7 8,8 2,1 2,5 2,5 2,1
Table 5.3: Results obtained in Real Scenarios with defined Cluster Factor
59
Figure 5.8: Likelihood ratio in Real Scenarios with defined Cluster Factor: 4 and 8
As predicted, in a real scenario with higher amount of data both methods suffer in computational
time. Nonetheless, computational time is reduced when higher cluster factors are utilized.
In addition, the results indicate that BM can still be considered a possible approach in real time
utilization contrary to MA. Likelihood ratio indicates clear superior quality of prediction results using BM.
The overall prediction validity is evaluated by the veracity of inequality 5.2, represented in Figure 5.9.
Figure 5.9: Points Validity in Real Scenarios
The high valid points percentage indicates valid prediction results. As expected, a higher percentage
was achieved with a smaller cluster factor. Smaller sets of points are processed in each consideration,
consequently more accurate predictions are obtained.
60
5.3.3 Predicted Costmaps
The predicted costmaps computed for scenarios 1 and 3 are displayed in Figures 5.10 and 5.11,
respectively.
(a) (b) (c)
(d) (e) (f)
(g) (h) (i)
(j) (k) (l)
Figure 5.10: Scenario 1 Predicted Costmap in different time framesReal Scenario (left column); BM Predicted Costmap (middle column); MA Predicted Costmap (right
column)The red dot represents the robot position
61
(a) (b) (c)
(d) (e) (f)
(g) (h) (i)
(j) (k) (l)
Figure 5.11: Scenario 3 Predicted Costmap in different time framesReal Scenario (left column); BM Predicted Costmap (middle column); MA Predicted Costmap (right
column)The red dot represents the robot position
The obtained costmaps display both static and moving occupied areas, with higher dispersion of
points. The static areas are associated with fixed obstacles. Consequently, the prediction has little
uncertainty and the computed predictions will point to the real location of the obstacles. Areas with higher
dispersion of points are associated with motion, dynamic obstacles and the associated uncertainty.
62
Chapter 6
Conclusions
This thesis approaches AA autonomous robot navigation.
Non-cooperative moving agents, such as humans and non-cooperative robots, are considered through-
out this work. Communication absence rises challenges in environmental awareness and, consequently,
the main focus of this thesis is the prediction of changes in robot surrounding spaces.
Current position and previous velocity data of tracked obstacles, are used to the develop prediction
computation methods, BM and MA.
Prediction’s uncertainty is considered by analyzing agents motion covariance and adapting safety
margins accordingly. In addition, the implementation of both prediction algorithms contemplates relevant
time intervals of collected data.
Considering prediction computation relies on a previous track of the agent, whose shape and size
are unknown, the algorithm integration implies detecting, perceiving and tracking undifferentiated moving
obstacles. In order to do so, scan data from laser range finders is received and processed in real time.
The surrounding environment change is tracked by determining point correspondence between time
frames, that includes both static and dynamic obstacles.
Taking into account the need for a real time response and the great amount of points detected by a
laser scan on a real environment, a clustering method is implemented. The tracking is applied to clusters
of points and their velocity data is used to compute the prediction of each constituent points.
An occupancy grid map is built based on the computed predictions. This map is sent to the local
planner, resulting in an anticipated response to possible risk situations.
The results presented in Chapter 5 take into account not only the computational time but also the
prediction’s quality comparison between approaches. In addition, point prediction validity is considered.
The concluded superior method is the BM. Not only is this the method that displays superior predic-
tion success but also the faster response.
In real setting tests, MA algorithm takes, in the best case scenario, more than 1000× the simulation
time. Consequently, it would not be a useful method in real-time implementation as desired.
Considering a prediction map is built to later influence the local planner, the success of the developed
work implies an enhanced planner response. Consequently, navigation is improved.
63
6.1 Achievements
The major achievements of the present work include:
• The development of an agent motion model that accounts for uncertainty;
• The formulation of an agent predictor valid for both static and dynamic obstacles;
• Construction of an agent-aware module in the SocRob@Home project;
• Experimental validation of the work developed.
6.2 Future Work
As possible further development of this work it is suggested the experimental evaluation of the pre-
dicted costmap on the local planner response.
The integration with move base in the ROS framework and consequent complete integration of the
agent-aware module built in the SocRob@Home project.
In addition, an optimization of the coded algorithm would possibly result in an improvement on the
algorithms’ computational time.
An interesting evaluation suggested is to test the developed methods on a robot with higher laser
scan range. The agents would be detected earlier, consequently a better and faster response would be
possible.
64
Bibliography
[1] A. Stevenson. Oxford dictionary of English. Oxford University Press, USA, 2010.
[2] T. Kruse, A. K. Pandey, R. Alami, and A. Kirsch. Human-aware robot navigation: A survey. Robotics
and Autonomous Systems, 61(12):1726–1743, 2013.
[3] J. K. Hackett and M. Shah. Multi-sensor fusion: a perspective. In Robotics and Automation, 1990.
Proceedings., 1990 IEEE International Conference on, pages 1324–1330. IEEE, 1990.
[4] W. Elmenreich. Sensor fusion in time-triggered systems. Master’s thesis, Institut fur Technische
Informatik, 2002.
[5] Y.-H. Shyy. Laser range finder, Nov. 16 1993. US Patent 5,262,837.
[6] S. Russell, P. Norvig, and A. Intelligence. A modern approach. Artificial Intelligence. Prentice-Hall,
Egnlewood Cliffs, 25(27):79–80, 1995.
[7] E. Rimon, I. Kamon, and J. F. Canny. Local and Global Planning in Sensor Based Navigation of
Mobile Robots. Springer, 1998.
[8] A. Sanchez Lopez, R. Zapata, and M. A. Osorio Lama. Sampling-based motion planning: A survey.
Computacion y Sistemas, 12(1):5–24, 2008.
[9] I. A. Sucan, M. Moll, and L. E. Kavraki. The open motion planning library. IEEE Robotics &
Automation Magazine, 19:72–82, 2012.
[10] S. Marlow and J. Langelaan. Dynamically sized occupancy grids for obstacle avoidance. In AIAA
Guidance, Navigation, and Control Conference, page 7848, 2010.
[11] A. Howard, M. J. Mataric, and G. S. Sukhatme. Mobile sensor network deployment using potential
fields: A distributed, scalable solution to the area coverage problem. In Distributed Autonomous
Robotic Systems 5, pages 299–308. Springer, 2002.
[12] D. Fox, W. Burgard, and S. Thrun. The dynamic window approach to collision avoidance. IEEE
Robotics & Automation Magazine, 4(1):23–33, 1997.
[13] S. Quinlan and O. Khatib. Elastic bands: Connecting path planning and control. In Robotics and
Automation, 1993. Proceedings., 1993 IEEE International Conference on, pages 802–807. IEEE,
1993.
65
[14] A. Fernandez-Caballero, J. C. Castillo, J. Martınez-Cantos, and R. Martınez-Tomas. Optical flow
or image subtraction in human detection from infrared camera on mobile robot. Robotics and
Autonomous Systems, 58(12):1273–1281, 2010.
[15] S. G. Thoduka. Finding and following moving objects. Master’s thesis, University of Applied Sci-
ences Bonn-Rhein-Sieg, 2015.
[16] Y. LeCun, Y. Bengio, and G. Hinton. Deep learning. Nature, 521(7553):436, 2015.
[17] J. Redmon, S. Divvala, R. Girshick, and A. Farhadi. You only look once: Unified, real-time object
detection. In Proceedings of the IEEE conference on computer vision and pattern recognition,
pages 779–788, 2016.
[18] P. Dollar, C. Wojek, B. Schiele, and P. Perona. Pedestrian detection: An evaluation of the state of
the art. IEEE transactions on pattern analysis and machine intelligence, 34(4):743–761, 2012.
[19] P. Dollar, S. J. Belongie, and P. Perona. The fastest pedestrian detector in the west. In British
Machine Vision Conference, volume 2, page 7, 2010.
[20] B. Yang, J. Yan, Z. Lei, and S. Z. Li. Aggregate channel features for multi-view face detection. In
Biometrics (IJCB), 2014 IEEE International Joint Conference on, pages 1–8. IEEE, 2014.
[21] K. Fukunaga and P. M. Narendra. A branch and bound algorithm for computing k-nearest neighbors.
IEEE transactions on computers, 100(7):750–753, 1975.
[22] A. Yilmaz, O. Javed, and M. Shah. Object tracking: A survey. Acm computing surveys (CSUR), 38
(4):13, 2006.
[23] D. Reid et al. An algorithm for tracking multiple targets. IEEE transactions on Automatic Control,
24(6):843–854, 1979.
[24] G. Soysal and M. Efe. Performance comparison of tracking algorithms for a ground based radar.
In Target Tracking: Algorithms and Applications, 2006. The IEE Seminar on (Ref. No. 2006/11359),
pages 39–46. IET, 2006.
[25] D. P. Kroese, T. Brereton, T. Taimre, and Z. I. Botev. Why the monte carlo method is so important
today. Wiley Interdisciplinary Reviews: Computational Statistics, 6(6):386–392, 2014.
[26] D. P. Kroese and R. Y. Rubinstein. Monte carlo methods. Wiley Interdisciplinary Reviews: Compu-
tational Statistics, 4(1):48–58, 2012.
[27] C. J. Geyer. Practical markov chain monte carlo. Statistical science, pages 473–483, 1992.
[28] M. U. USwitzerland. Hidden markov models and other finite state automata for sequence process-
ing. Technical report, Dalle Molle Institute for Perceptual Artificial Intelligence, 2001.
[29] D. Vasquez, T. Fraichard, and C. Laugier. Growing hidden markov models: An incremental tool for
learning and predicting human and vehicle motion. The International Journal of Robotics Research,
28(11-12):1486–1506, 2009.
66
[30] B. Cipra. Engineers look to kalman filtering for guidance. SIAM News, 26(5):757–764, 1993.
[31] R. E. Kalman and R. S. Bucy. New results in linear filtering and prediction theory. Journal of basic
engineering, 83(1):95–108, 1961.
[32] S. M. Ross. Stochastic Processes. John Wiley & Sons, New York,, 1955.
[33] M. Kac. Random walk and the theory of brownian motion. The American Mathematical Monthly,
54(7):369–391, 1947.
[34] J. L. Doob. The brownian movement and stochastic equations. Annals of Mathematics, pages
351–369, 1942.
[35] E. Pacchierotti, H. I. Christensen, and P. Jensfelt. Human-robot embodied interaction in hallway
settings: a pilot user study. In Robot and Human Interactive Communication, 2005. ROMAN 2005.
IEEE International Workshop on, pages 164–171. IEEE, 2005.
[36] R. Kirby, R. Simmons, and J. Forlizzi. Companion: A constraint-optimizing method for person-
acceptable navigation. In Robot and Human Interactive Communication, 2009. RO-MAN 2009. The
18th IEEE International Symposium on, pages 607–612. IEEE, 2009.
[37] R. Ventura and A. Ahmad. Towards optimal robot navigation in domestic spaces. In Robot Soccer
World Cup, pages 318–331. Springer, 2014.
[38] T. Mercy, R. Van Parys, and G. Pipeleers. Spline-based motion planning for autonomous guided
vehicles in a dynamic environment. IEEE Transactions on Control Systems Technology, 2017.
[39] S. Tadokoro, M. Hayashi, Y. Manabe, Y. Nakami, and T. Takamori. On motion planning of mobile
robots which coexist and cooperate with human. In International Conference on Intelligent Robots
and Systems, page 2518. IEEE, 1995.
[40] M. Ono, B. C. Williams, and L. Blackmore. Probabilistic planning for continuous dynamic systems
under bounded risk. Journal of Artificial Intelligence Research, 46:511–577, 2013.
[41] M. Ono and B. C. Williams. Iterative risk allocation: A new approach to robust model predictive con-
trol with a joint chance constraint. In Decision and Control, 2008. CDC 2008. 47th IEEE Conference
on, pages 3427–3432. IEEE, 2008.
[42] L. Wasserman. All of statistics: a concise course in statistical inference. Springer Science &
Business Media, 2013.
[43] S. Chib and E. Greenberg. Understanding the metropolis-hastings algorithm. The american statis-
tician, 49(4):327–335, 1995.
[44] W. K. Hastings. Monte carlo sampling methods using markov chains and their applications.
Biometrika, 57(1):97–109, 1970.
67
[45] A. Elgammal, R. Duraiswami, D. Harwood, and L. S. Davis. Background and foreground modeling
using nonparametric kernel density estimation for visual surveillance. Proceedings of the IEEE, 90
(7):1151–1163, 2002.
68
