“We’re fascinated with robots because they are reflections of ourselves.”
— Ken Goldberg
Universidade de Aveiro Departamento deElectr´onica, Telecomunica¸c˜oes e Inform´atica, 2017
Andr´
e
S´
a
Navega¸
c˜
ao de Robˆ
os M´
oveis e Inteligentes
Navigation of Autonomous Mobile Robots
Universidade de Aveiro Departamento deElectr´onica, Telecomunica¸c˜oes e Inform´atica, 2017
Andr´
e
S´
a
Navega¸
c˜
ao de Robˆ
os M´
oveis e Inteligentes
Navigation of Autonomous Mobile Robots
Disserta¸c˜ao apresentada `a Universidade de Aveiro para cumprimento dos requisitos necess´arios `a obten¸c˜ao do grau de Mestre em Engenharia em Electr´onica e Telecomunica¸c˜oes, realizada sob a orienta¸c˜ao cient´ıfica de Prof. Doutor Nuno Lau e Prof. Doutor Artur Pereira, professores auxil-iares do Departamento de Electr´onica, Telecomunica¸c˜oes e Inform´atica da Universidade de Aveiro
o j´uri / the jury
presidente / president Professor Doutor Lu´ıs de Seabra Lopes Professor Associado da Universidade de Aveiro
vogais / examiners committee Professor Doutor Lu´ıs Paulo Gon¸calves dos Reis
Professor Associado da Escola de Engenharia da Universidade do Minho
Professor Doutor Jos´e Nuno Panelas Nunes Lau Professor Auxiliar da Universidade de Aveiro (orientador)
Agradecimentos Acknowledgements
Tese dedicada `a minha fam´ılia pelo apoio nos 23 anos passados e aos meus amigos por estarem l´a nos meus altos e baixos. Aos meus orientadores, professores Nuno Lau e Artur Pereira, um obrigado especial pela disponibil-idade constante e por todos os conselhos, deveras importantes para terminar este trabalho. Por fim deixo uma men¸c˜ao a um grande amigo meu e colega de trabalho Miguel Horta, pelo trabalho desenvolvido lado a lado durante os nossos 5 anos de curso, incluindo obviamente as participa¸c˜oes cheias de sucesso no Microrato e IEEExtreme.
Resumo Automa¸c˜ao, na mais simples das designa¸c˜oes, ´e a arte de criar vida na m´aquina, possibilitando certas a¸c˜oes sem controlo directo por parte de um utilizador. Esta ´area de estudo permite que certas atividades que consider-amos aborrecidas ou perigosas possam ser executadas por m´aquinas. Nesta tese, um estudo do estado da arte no campo de robˆos m´oveis e in-teligentes foi realizado, apresentando um focus especial em algoritmos de navega¸c˜ao baseados em procura e amostragem. Uma simula¸c˜ao foi desen-volvida, na qual um modelo do robˆo Wiserobot foi criado, utilizado como ambiente de teste um ed´ıficio conhecido no campo da rob´otica, o laborat´orio da Willow Garage. Nesta simula¸c˜ao foram realizados testes aos algoritmos explorados anteriormente, nomeadamente Dijkstra, PRM e RRT. Para tes-tar os algoritmos por amostragem, um plug-in foi desenvolvido para utilizar a Open Motion Planning Library para avaliar resultados dos mesmos. Por fim, c´odigo foi desenvolvido, usando e tendo por base bibliotecas existentes no ROS, de modo a dar ao nosso modelo do robˆo capacidades de navega¸c˜ao no ambiente simulado, inicialmente est´atico seguido de testes com objectos n˜ao declarados. Os resultados dos v´arios planeadores foram comparados para avaliar a presta¸c˜ao nos casos de testes definidos, utilizando m´etricas escolhidas previamente.
Abstract Automation, in the simplest of designations, is the art of creating life in the machine, allowing the performance of certain actions without the need of direct control by an user. This area of study allows for certain activities that we deem as tedious or dangerous to be executed by machines.
In this thesis, a study of the state of the art in the field of mobile and autonomous robotics is made, focusing in navigation algorithms based on search and sampling. A simulation was developed, in which a model of the robot was created, to be used with an environment well know by roboti-cist, Willow Garage. In this simulation, tests were made to the algorithms explored earlier, namely Dijkstra, PRM and RRT. To test multiple sample-based planners, a plug-in was developed to use the Open Motion Planning Library for benchmarking purposes. Finally code is developed, based and using existing ROS packages, to give a model cargo robot navigation ca-pabilities in a simulated indoor environment, initially static then with un-declared obstacles. The results were compared from multiple planners to evaluate the performance in the test cases defined, using pre-established metrics.
Contents
Contents i
List of Figures iii
List of Tables v 1 Introduction 1 1.1 Motivation . . . 1 1.2 Objectives . . . 2 1.3 Wiseware . . . 2 1.4 Wiserobot . . . 2 1.5 Thesis Outline . . . 3
2 Exploring the State of the Art 5 2.1 Basics for Motion Planning . . . 7
2.1.1 Properties of the Robot . . . 7
2.1.2 Properties of the Algorithm . . . 7
2.1.3 Properties of the Task . . . 8
2.2 Navigation planning algorithms . . . 11
2.2.1 Search-based planners . . . 12
2.2.2 Sampling-based planners . . . 15
3 Software 19 3.1 Gazebo . . . 19
3.2 Robot Operating System . . . 20
3.3 The Open Motion Planning Library . . . 21
4 Creating the simulation 25 4.1 Robot model . . . 25
4.2 Test environment . . . 26
4.3 Offline planner testing . . . 26
4.3.1 Metrics of evaluation . . . 28
4.3.2 Test cases . . . 28
4.4 Online simulation . . . 29
4.4.1 Static environment . . . 29
5 Results 31
5.1 Planner testing . . . 31
5.2 Robot motion . . . 37
5.2.1 Static environment . . . 37
5.2.2 Undeclared obstacles . . . 38
6 Conclusions and future work 39 6.1 Conclusions . . . 39
6.2 Future Work . . . 39
List of Figures
1.1 The planned path, while valid, may be not optimal (by Robot Comics) . . . . 1
1.2 Wiserobot early design (left) and disposition of the wheels (right) . . . 2
2.1 Roomba vacuuming robot . . . 5
2.2 Stanley car, winner of 2015 DARPA Challenge . . . 6
2.3 Example of state prediction using MCL. . . 9
2.4 Example of the map generated post SLAM. . . 10
2.5 Example of Bug 1 algorithm. . . 11
2.6 Node search order of the graph in Depth-first versus Breadth-first planners . 12 2.7 Task performed by A* (left) and Dijkstra’s (right) algorithms (examples from http://theory.stanford.edu/ amitp/GameProgramming/AStarComparison.html) 13 2.8 Example of a motion planning query on a PRM [1] . . . 16
2.9 Example of a motion planning query on a RRT [1] . . . 17
3.1 Gazebo environment. . . 19
3.2 ROS communication infrastructure. . . 20
3.3 Rviz environment. . . 21
3.4 Overview of OMPL API structure. . . 22
4.1 Wiserobot model displayed in Gazebo. . . 25
4.2 Willow Garage displayed in Gazebo. . . 26
4.3 Basic code for benchmarking in OMPL. . . 27
4.4 Visual representation of goal configurations for each test case. . . 29
5.1 Memory usage for test case A, B and C. . . 33
5.2 Status of solution for test case A, B and C. . . 33
5.3 Time elapsed for test case A, B and C. . . 34
5.4 Simplification time elapsed for test case A, B and C. . . 34
5.5 Non-simplified (left) and simplified (right) length results for test case A. . . . 35
5.6 Smoothness of non-simplified (left) and simplified (right) paths for test case A (closer to zero the better). . . 35
5.7 Non-simplified (left) and simplified (right) length results for test case B. . . . 35
5.8 Smoothness of non-simplified (left) and simplified (right) paths for test case B (closer to zero the better). . . 36
5.9 Non-simplified (left) and simplified (right) length results for test case C. . . . 36
5.10 Smoothness of non-simplified (left) and simplified (right) paths for test case C (closer to zero the better). . . 36
5.11 Movement from configuration B to A (left) and configuration B to C (right). 37 5.12 Movement from configuration C to S (left) and configuration S to B (right). . 37 5.13 Path before detecting the undeclared obstacle. . . 38 5.14 Path correction upon detecting the undeclared obstacle. . . 38
List of Tables
2.1 Comparison between multi-query methods and single-query methods . . . 15 4.1 Description of each testcase. . . 28 5.1 Results for tests using Dijkstra . . . 31
Chapter 1
Introduction
1.1
Motivation
Besides being one of the ultimate science fiction dreams for our daily life (i.e. self driv-ing cars, automated houses, robot butlers), automation has great roots and interested third parties in the area of Industry. The ability to fully automate a production line, often deemed as tedious or in some cases dangerous work, can be both positive in terms of quality of life, production efficiency and maintenance costs. This dissertation context appears in the interest of making use of a mobile robot platform to perform the tasks of transport and logistics. The idea of a cargo delivery capable of motion planning in an industrial environment (character-ized by its dynamics and the presence of humans - potential non static obstacles) represents a huge leap considering some of the most common examples are limited by fixed hard coded paths. A good example of this is following existing line paths on the ground, a technique that has seen less popularity in the past years in industry since these systems require the previous installation of a navigation infrastructure that comes with installation and maintenance costs and reduces the flexibility of the industrial environment. There are already existing methods to plan a path, that can be grouped in different classes based on execution, that allow a robot to plan a path between its actual position and a goal position. As expected, each class of methods or even each individual method presents their own advantages and disadvantages given the context of its application.
1.2
Objectives
The main objective of this project is to perform a comparative study of different navigation algorithms to determine which are the best given the application in question. These algorithms must be developed into code to be applied to the real robot, named ’Wiserobot’. Since ’Wiserobot’ was not accessible during the realization of this thesis, a simulation had to be created of an indoor environment and a model of the robot was created to perform tests regarding the performance of the algorithms. Test cases were defined, followed by gathering of the resulting data from such tests. Conclusions are to be made of the results from the comparison of the algorithms tested. Secondary objective is that the results of this project should also allow for a functional early prototype of the navigation system of the ’Wiserobot’ in a dynamic environment.
1.3
Wiseware
Wiseware is a Portuguese research and development (R&D) company based in Aveiro. Their focus is in finding solutions in technological problems in a wide array of areas like robotics, micro-electronics, artificial intelligence and wireless communications, among others. Besides developing their own products (like a 3D Measurement Table), they also offer services to interested clients, from PCB manufacturing to 3D laser cutting. Further information on their portfolio can be found on their website [2].
1.4
Wiserobot
The proposed design of the ’Wiserobot’ is given below in Figure 1.2. The base is ex-pected to be a 0.52 meters side square, with a height of approximately 1.20 meters. It has a compartment to store cargo and an onboard display to easily access information. In terms of locomotion, the model used is a differential three wheeled robot [3], with the free turning wheel in front of the robot and and able to compute and feed independent velocities for both the left and right wheels. The sensoring is provided by a Laser Range Finder (LRF), with a cover radius of 270 degrees wide, centered in the front of the robot.
1.5
Thesis Outline
This dissertation is structured as follows:
In Chapter 2, a brief state of the art is exposed, focusing in explaining the algorithms chosen for testing in this work.
Chapter 3 gives the reader a general introduction to the used software (Gazebo, ROS and OMPL).
Chapter 4 provides a description of strategies of approach for algorithm testing and im-plementation of a functional model of the robot, with references to both model creation and algorithm development.
Chapter 5 exposes the various results obtained throughout this work and provides discus-sion and interpretation of the latter.
Finally, at Chapter 6 some final thoughts are given with, with some ideas for future improvements upon the work.
Chapter 2
Exploring the State of the Art
Before diving into basic motion planning concepts, it can be of interest to take a look at existing examples of applied autonomous movement. This quick look will serve of introduc-tion to some approach strategies (some examples of algorithms used with success) in a more loosely way, before characterizing them in more detail later in this chapter. The variety of applications (using either early strategies, current approaches and projects in development) found today range from simple household items to space exploration vehicles. Some specific examples that can be considered relevant to our work (problem similarity or interesting ap-proach) will now be briefly presented.
Roomba
A simple example of an automatic mobile robot already available to the general public with the capability of interacting with our daily life is the well-known ’Roomba’ (Figure 2.1), from the iRobot [4] company. These small robots are easily their most known product, but its idea is simple: it is a vacuum cleaner that, making use of sensors, automatically moves around the house cleaning the floors, being able to detect areas of debris around it. The navigation software itself allows the robot to follow walls, make corners, move around obstacles or even navigate under furniture. The main limitation of the initial release was the lack of proper Localization and Mapping algorithms [5] (and as result, the ability of performing a SLAM of the user’s house) which depraved the robot of the capacity to identify the environment. Newer versions solved this issue [4], being now possible to, for example, check a map of the house’s areas already cleaned using their mobile app.
DARPA Grand Challenge
The DARPA Grand Challenge [6] is a prize competition for American autonomous vehicles, funded by the Defense Advanced Research Projects Agency (DARPA). The initial edition, the ”2004 DARPA Challenge”, was created to ignite the development of technologies needed to create the first fully autonomous ground vehicle capable of completing a substantial off-road course (Mojave Desert, United States of America) within a limited time. In this edition no team was able to complete the course, so a new edition of this challenge was scheduled for the next year. This time, the Stanley car (Figure 2.2) was able to reach the finish line first (two other cars also completed the challenge). The 2007 DARPA Urban Challenge followed up, now requiring the navigation to be made in situation replicas of real world urban challenges, characterized by their challenging dynamics. Search-based and sample-based algorithms were used successfully in this competition, as several winning entrants to the 60 mile autonomous urban driving race used motion planning as their primary guidance logic, including CMU’s winning Boss car with Anytime-D, Stanford’s 2nd-place Junior car with hybrid A, and MIT’s 4th-place Talos car with Rapidly-exploring Random Trees (RRT).
Figure 2.2: Stanley car, winner of 2015 DARPA Challenge
Waymo
Originally known as The Google self-driving car project, the Waymo [7] is a self-driving technology company with the mission of making roads safer by fully automatize the car in-dustry. Their car fleet, with some vehicles like the ’Firefly’ or the ’Chrysler Pacifica hybrid minivan’, represent some of the most advanced autonomous vehicle available, capable of de-tecting variables like pedestrians, cyclists, vehicles or road work and act accordingly to the situation, safety being its primary directive. With over 3 million miles driven by May 2017, the early rider program was launched in the same year, allowing for residents of designated testing areas in the USA to join a public trail of the vehicles.
With these examples given to spark some interest in the topic and to also absorb an abstract idea of the multitude of both approaches and challenges motion planning faces, this
2.1
Basics for Motion Planning
Defining basic concepts to understand the necessities for Motion Planning is important but a wide number of ways to do so are available. In order to standardize the following work, the basic concepts will be those utilized on [8]. The characterization of our motion planner can then be made according to the Task addressed, the Properties of the Robot executing the task (in our case a single robot, ’Wiserobot’, so these characteristics will not be changed once described bellow) and finally the Properties of the Algorithm, defining how the task is to be completed with the robot available. These concepts are important to expose, as the designations and approaches mentioned in this Chapter will be the bases for our case study and the code implemented (Chapter 4) and respective interpretation of results (Chapter 5).
2.1.1 Properties of the Robot
An effective motion planner will depend heavily on the properties of the robot executing the task. Both the robot and the environment determine the number of degrees of freedom and the shape of the Configuration Space. The Configuration Space of a robot system is the space of all possible configurations of the system. The number of degrees of freedom is the dimension of the Configuration Space (or the minimum number of parameters needed to specify a configuration). Without going into much mathematical detail (available in chapter 3 of [8]), since our problem can be defined by a mobile robot translating and rotating in a plane, 3 degrees of freedom will be available (x and y for translation and theta for rotation), making the Configuration Space defined by the special Euclidean group for 2 dimensions, or SE(2). These spacial concepts are important as many are used for definition of existing algorithm libraries.
Defined the Configuration Space, it is important to know the velocity constraints of the robot. If it is able to move instantaneously in any direction in its set of configuration spaces that avoid collisions with obstacles (denoted as Free Space), the robot is considered omni-directional. Examples of this are robots equipped with ’Omni wheels’, wheels capable of movement in any direction in a plane. Of course, this is not always the case. Cars, for ex-ample, cannot translate sideways, and movement constraints also exist in the ’Wiserobot’ as mentioned before due to its differential movement type. In these cases, the robot is defined as nonholonomic.
Lastly, the robot could be modeled using Kinematic equations (velocities as controls) or using Dynamic equations of motion (forces as controls). In this work the model used is the kinematic one, as only positions and velocity constraints will be used (not forces).
2.1.2 Properties of the Algorithm
Given a robot and a task, it is important to have some metrics to compare the many algorithms available to execute the task. It is possible to choose an algorithm based on how they solve the problem. This means choosing the planner that find motions optimal in some way (optimization), such as length or execution time, among others (more examples given in Chapter 3 exploring the benchmarking capabilities of OMPL). This is of great importance for this work as the motion of the robot from a configuration A to a configuration B needs not only to be the shortest possible, but also computed as fast as possible to avoid long waiting times and executed with the highest smoothness possible to avoid sharp motions.
The problem of finding the most optimal planner comes with the computational complex-ity, since memory requirements or running time of the algorithms can grow in a constant, polynomial or even exponential way with the amount of degrees of freedom of the robot. Usually a planner is only considered practical if it runs in time polynomial or better. Spe-cial interest will be taken in this characteristic, namely the increment of running time, as a balance between the computational complexity and optimization must be found to avoid long computing times on one hand, and completely non-optimal paths on other.
Perhaps the most basic question for the task: does the algorithm always find a solution to the motion planning problem when it exists and, in case it does not, does the planner indicate failure in a finite time? If yes, the algorithm is considered complete. While completeness is a very desirable property, when the degrees of freedom increase, again the correspondent computational complexity can result in the planner being computationally intractable. So weaker, more specific forms of completeness can be used, in order to achieve the perfect balance with the optimization and computational complexity.
If a plan creation is based on a known model of the environment/world (referred to as costmaps from now on) prior to the execution, the planner is said to be offline. The planner is online if it incrementally constructs the plan while the robot is executing, being sensor-based. This distinction is considered not exact, as specific cases from both types can behave the same in the extremes, but there are differences in computation time that should be taken into consideration during testing. For example, an offline planner that runs quickly enough can be used in feedback loop to re-plan as new data is sensed in the environment. Since the initial focus of this work will be to compare different navigation planning algorithms, this study will be made based in an offline approach to compute the global path, but in a second part an hybrid implementation combining online and offline planning will be developed to make use of the continuous sensor data streaming.
2.1.3 Properties of the Task
Before an algorithm is chosen, it is first necessary to establish what is the task that the planner needs to address. The different type of tasks can be resumed to four concepts, as established in [8]: Coverage, Localization, Mapping and Navigation.
Coverage
Depicts the way the robot ”sees the world”, how it senses external information. This task is a bit more generic than the remaining, as the algorithms here reflect the different means to perform coverage, each one with their own advantages and disadvantages. Laser Range Finders (LRF) while very precise and easy to compute, are expensive and, in case of aquatic robots, have the light distorted (wrong readings). Sonar, on the other hand, are very cheap and can perform underwater readings, but overall the readings are of poorer quality compared to laser (requires wider angle of minimum reading). Vision coverage is the most advanced coverage implementation as it can be used to detect elements by shape, colors, among other qualities. The downside is a greater computational complexity and susceptibility to light variations on the environment. These are just some examples and a proper choice of sensors is imperative for proper buffer of information to perform the remaining tasks, as non valid information of the environment can result in catastrophic outcomes (collisions being the most common). As mentioned in the description of the robot in Chapter 1, the way ’Wiserobot’
Localization
It is the problem of computing the pose of a robot relative to a given map of the envi-ronment (also called position estimation or position tracking). It can be seen as a problem of coordinate transformation, of establishing a correspondence between the map coordinate system (using landmarks - easy to recognize points of the environment) and the robot’s local coordinate system. This is the main problem as the pose can’t usually be sensed directly due to noise in the measuring pose’s sensor, so the position must be inferred from data.
This problem is most commonly solved using algorithm based on filters, being Monte Carlo Localization [9] the state of the art localization algorithm nowadays. Also known as particle filter localization, it is an algorithm for robots to localize using a particle filter. Using the map of the environment as input, the algorithm estimates the position and orientation of a robot as it moves and senses the environment. The algorithm uses a particle filter to represent the distribution of likely states, with each particle representing a possible state. It typically starts with a uniform random distribution of particles over the configuration space, meaning the robot has no information about where it is and assumes it is equally likely to be at any point in space. Whenever the robot moves, it shifts the particles to predict its new state after the movement (Figure 2.3). If the robot senses something, the particles are resampled based on recursive Bayesian estimation (calculates how well the actual sensed data correlate with the predicted state). The end goal is that the particles should converge towards the actual position of the robot. [10]
Figure 2.3: Example of state prediction using MCL.
In this work a different approach to localization is used using the work developed at IRIS, explained in the paper [11]. The abstract idea is that ”the likelihood field plays a central role in the approach, as it avoids the necessity to establish direct correspondences; it is the connection link between scan matching and robotic localization, and it provides a reduced computational complexity. Scan matching is formulated as a non-linear least squares prob-lem and solved by the Gauss-Newton and Levenberg-Marquardt methods. Furthermore, to reduce the influences of outliers during optimization, a loss function is introduced” [11]. Due to the faster and more accurate results showed in the paper when comparing datasets to ones generated by aMCL, this proposal was chosen for our work. These results were validated during the testing of our simulation by noticing a more robust localization in the extreme case of a collision (aMCL would become lost if the robot collided often and while this is an extreme non-desirable case, it was a metric of choosing between the two).
Mapping
Can be described as the representation of the environment to the robot by the means of data. This allows the navigation algorithms to evaluate the geometry of the environment and act accordingly. As such, this is highly linked to Localization. In fact, while the Mapping information can be given in advance (by a plant of the level, converted to a costmap, for ex-ample), the same can be created or altered. For this, a process is used called Simultaneous localization and mapping (SLAM), which is the computational problem of constructing (Figure 2.4) or updating a map of an unknown environment while simultaneously keeping track of an agent’s location within it.
Figure 2.4: Example of the map generated post SLAM.
Navigation
The most interesting task to this work, it can be described as the problem of finding a collision-free motion from one configuration/state to another and, if required, to execute that path with success (reaching goal without collisions, usually also following an optimization pa-rameter like time, distance, consume, among others). In other words, given certain physical constraints of the robot towards the world, plans a valid path between a start position and a goal position. The next section will give a deeper look into existing approaches to solve the planning problem.
These tasks, while defined by different needs for optimal function, rely heavily on each other for the whole system to operate as desired. For example, a proper path planning algo-rithm from Navigation can only be executed if the position values are correctly calculated by the Localization algorithm, the later being by itself dependent of a proper Coverage (readings from the sensoring of the environment) and successfully correlated with the landmarks of the
2.2
Navigation planning algorithms
To navigate with success between start and goal configurations, only two things can be truly required for a robot: to actually reach the goal location and to avoid at all costs a collision. The simplest algorithms to perform such objectives are Bug 1 (Figure 2.5) or Bug 2 [8], navigation algorithms that simply move towards the goal and move around any obstacles they find. These reactive algorithms can be implemented into unknown environments since the only info they require is the present and goal location (or even only the direction of the goal) and a method to sense obstacles in the way. While simple to implement and useful in a local workspace, it is of interest to study algorithms that make use of the global map information if it is available, to avoid unnecessary search time or memory use for example.
Figure 2.5: Example of Bug 1 algorithm [8]. Robot moves towards the goal and upon contact with an obstacle, a full lap of the obstacle is made, with the robot then proceeding to the closest point in a straight line to the goal around the obstacle and moving towards the goal.
Before exploring search-based and sampling-base algorithms, a brief look will be made into Potential Fields. Conceptually a simple and intuitive idea, the classical potential field involves computing a vector at each point in the workspace by calculating the sum of an attractive force emanating from the goal, and a repulsive force from all of the obstacles. The point robot can then navigate using gradient descent to follow the potentials to the goal. Potential fields have been generalized to systems with a rigid body by considering multiple points on the robot, and can operate in real-time. Navigating a system using a potential field, however, can fail due to local minima in the field itself, which stem from the heuristic combination of the forces in the workspace. Ideally the field would be constructed in the state space of the system, but this is equivalent to solving the original problem. Some approaches consider a navigation function where the potential field is guaranteed to have a single minimum, but computing such a function is possible only in low-dimensional spaces and is non-trivial. While mentioned, this method was discarded early on during our work and the focus diverged exclusively to search-based and sampling-based algorithms.
2.2.1 Search-based planners
Let’s start by looking at some basic concepts. A graph is a collection of nodes and edges, a node being able to represent a certain configuration, and an edge the connection between 2 configurations, but these configurations are not exclusive to one another, so cycles are allowed. The graphs can be either directed (meaning you can, for example, go from node A to node B but not necessarily the opposite) or undirected. A special type of directed graph, called tree, has no cycles. Using a parent-child analogy, a parent node has nodes below it called children. There is a special node called root which does not have a parent node and on the opposite any node without children is called a leaf. No non-leaf node can be removed or the tree loses connectivity.
Let’s now resume the comparison above with our planning situation - the root node can be our start configuration and a desired node x can be our goal configuration. Any edge can be the representation of a motion between the 2 nodes making it. There are 2 basic ways to search the goal configuration (node x) from the start configuration (root). Depth-first search will start at the root, choose a child, then that node’s child, and so on until it finds the desired node x or a leaf. If a leaf is found, the search backs up a level in the tree and chooses the next unvisited child, continuing the search from there until the desired node x or a leaf is found; this process is repeated until the node x is found or all nodes are visited in the graph. Breadth-first search changes the priority of the search, so instead of searching a child of the current node visited, it searches the next unvisited node in the same level (first the root, then the children of the root, then the grandchildren of the root, and so on).
We can now define a grid as a four-point connectivity graph (can also be eight-point con-nectivity, but for simplification purposes only the four case is considered), a node being a cell and an edge the connection of a cell to one of it’s neighbours (north, south, east or west). While this graph representation may not be a tree, the search techniques described above can still apply, having now a link length describing the number of edges in a path of a graph (not to be confused with path length since the later takes the weight of each edge into account). The Breadth-first search places the children of the current node in a queue (FIFO, first-in first-out), forcing the nodes with the same link length to be visited first; Depth-first search places the current node children in a stack (LIFO, last-in first-out) prioritizing always a child from the current node.
A* Algorithm
Both Breadth-first search and Depth-first search are uninformed algorithms, as the search is conducted without any preference or optimization. Optimization metrics like shortest-path length, time, safety, can all be used. For example, if the goal node coordinates are known, we can apply an heuristic to estimate the cost from the current node to the desired goal node. One of the most intuitive actions is to base the heuristic on the Euclidean distance to the goal, since the best case scenario is a straightforward line to the goal. The heuristic is the base for the A* Algorithm as it allows for an efficient search, if the heuristic is ”good”. The algorithm functions in a very similar way to the Breadth-first search, as the current node adds its children to a priority queue, but before moving to the next node the queue is sorted by an astarcost, the latter being the sum of the cost from the start node to the actual node and the estimated cost remaining to the goal (our heuristic). If it encounters the goal node, the astarcost is saved and the search is considered complete. In the case that no nodes remain in the queue before the goal is reached, the algorithm indicates a failure as it finds no valid path to the goal. If we are searching a grid with obstacles, to avoid choosing a path with collision, an easy way is to immediately move on to the next node in the queue if a node representing an obstacle is found. Example of pseudocode to perform a basic A* search can be found in the next page.
Since A* generates a search tree, it performs search using only a finite number of acyclic paths, so the search is also finite (may not search all paths). This means A* will always terminate if the graph is finite (no infinite maps in a mobile navigation perspective), even if no goal was found, so completeness is assured. The fact that node search is restricted to lower astarcost than the saved astarcost to goal, the search is always continued to find a shorter path, so the algorithm is also optimal (assuming that the heuristic used is valid, not overestimating the real distance) regarding the shorter path metric used.
Dijkstra’s Algorithm
A specific case of A* is the Dijkstra’s Algorithm, where the heuristic is null, so the sorting of the priority queue takes only into account the link length of the path to the start for sorting. It basically propagates as a wave in all possible directions instead of directing the search to the goal. It still finds the shortest path available and maintains completeness.
Figure 2.7: Task performed by A* (left) and Dijkstra’s (right) algorithms (examples from http://theory.stanford.edu/ amitp/GameProgramming/AStarComparison.html)
Algorithm 1 Pseudocode to perform basic A*
Data: Start/Goal Configurations {start,goal }, Costmap information Result: Planned path totalpath or failure indication
closedset ← {} openset ← {start} origset ← {} gscore[start] ← 0
f score[start] ← gscore[start] + heuristic(start, goal) while openset is not empty do
current ← node in openset with lowest fscore if current = goal then
totalpath ← [current] while current in origset do
current ← origset[current] totalpath.append(current) end
return totalpath end
remove current from openset add current to closeset
for each neighbor in neighbor nodes(current) do if neighbor in closedset then
continue end
temp gscore ← gscore[current] + distance(current, neighbor) if neighbor not in openset or temp gscore < gscore[neighbor] then
origset[neighbor] ← current gscore[neighbor] ← temp gscore
f score[neighbor] ← gscore[neighbor] + heuristic(neighbor, goal) if neighbor not in openset then
add neighbor to openset end
end end end
return failure
Above we have the basic required steps to create an A* planning algorithm. If we wish to adapt this to Dijkstra’s Algorithm, the only change would be to eliminate the weight of the heuristic (giving it zero for all configuration spaces). The problem with exact motion planners like search-based is that in high-dimension systems under complex constraints they become computationally intractable, due to the immense size and number of the search trees generated. Therefore, the usage of these planners is normally restricted to low-dimensional systems, as sampling-based planners perform much better and are currently considered state
2.2.2 Sampling-based planners
These planners are based on a powerful method that employs sampling of the state space of the robot in order to quickly and effectively solve planning problems, instead of a full and successive search of neighbouring states until the goal is found as described above. The main advantage of these planners lies in a state space with higher degrees of freedom, due to the reduced computational complexity compared to other approaches. Another interesting feature is that these planners can achieve probabilistic completeness: if a solution path exists, the planner will find it as the probability of finding it converges to one as the number of samples goes to infinite. The problem with this approach is that the solution found may not always be the best one available. Multiple sample based algorithms have been developed in the past years, with some algorithms getting various adaptations over the years. For this work, with a multitude of choices in this area, the focus of work during this study of the state of the art was given to the Probabilistic Roadmap Method (PRM) and Rapidly-exploring Random Trees (RRT) algorithms.
Sampling-based methods can be divided into two categories: multi-query methods and single-query methods. A comparison is given in Table 2.1.
Multi-query Single-query
Phases 1. Roadmap construction 2. Searching
Roadmap construction and searching online
Typical algorithm Probabilistic Roadmap Method (PRM)
Rapidly-exploring Random Trees (RRT)
Pros Fast searching No preproccessing
Cons Inability to deal with envi-ronment changes
No memory
Table 2.1: Comparison between multi-query methods and single-query methods For multi-query methods, a probabilistic roadmap is built during the preprocessing phase and stored inside the robot. After given the a start and a goal configuration, the robot will search the roadmap for a path joining the two nodes. As the name suggests, once the roadmap is ready, the robot can query it multiple times to plan a path connecting any two nodes. The biggest advantage for multi-query methods is that searching is really fast. However, this kind of methods works only when the environment does not change too much. For single-query methods, no roadmaps is built ahead of the task. The robot constructs the roadmap online during the searching, with sampling conducted in the process to avoid being stuck in the local minima due to any deterministic approach. This type of methods is capable of dealing with dynamic environment, but it has no memory of past experience.
Probabilistic Roadmap Method
PRM planners are able to compute collision-free paths for robots of virtually any type moving among stationary obstacles (static workspaces). This method, when it was first presented, was of great interest for robots with high degrees of freedom (namely 5 or more) as very few motion planners existed at the time capable of that task. This method consists in two phases: a learning phase and a query phase. In the learning or construction phase a probabilistic roadmap is created. In a probabilistic roadmap, a group of states have been sampled. After sorting those that are collision-free, we proceed to establish a connection
between some sampled states, the criterion used in this kind of algorithms is to connect a state with all those nodes that are inside a given radius, leading to the construction of the roadmap. In the query phase, the roadmap is used to answer requests for connections between given start and goal configurations. Given the start and goal configurations, this step attempts to connect these configurations in the roadmap and searches for an existing valid sequence of local paths linking them. If the start and goal configurations are not among the configurations stored in the roadmap, the first step is to connect them to nodes within the roadmap. In order to do this, nodes are chosen from the roadmap in ascending order of distance to our start configuration. If a link is possible with a node from the roadmap, the same is attempted for the goal configuration (in the case that no connection nodes are found, random-bounce walks may be attempted - if this fails, the whole query fails). After connection nodes in the roadmap to the start and goal configurations are found, the next step is to connect those nodes within the graph. This will generate a sequence of way points and this can be used to generate our desired path.
If the roadmap created in the construction phase is adequate, queries will be answered quickly and reliably. If this is not the case, queries will fail frequently and it is necessary to improve the roadmap. It is not necessary to regenerate the whole roadmap. As the graph is an incremental structure, new nodes (preferably in difficult regions) can just be added to the old roadmap. As a final note, this is a multi-query planner, designation given because these two phases do not have to be executed sequentially. Instead, they can be mixed to limit or adapt the size of the roadmap to difficulties found during the path creation phase. This can be observed in the implementation used by the OMPL further in this thesis. Further information can be found in [12].
Figure 2.8: Example of a motion planning query on a PRM [1] Rapidly-exploring Random Trees
The concept of a RRT is a randomized data structure that is designed for a broad class of path planning problems, being specifically designed to handle nonholomic constraints and high degrees of freedom. It works by growing a tree from the start configuration by using random samples from the search space. As each sample is drawn, a connection is attempted between it and the nearest state in the tree; if possible (as in no collision and constraints respected), this sample is added to the tree. The search for these states are usually limited by a growth constant. If the random sample is further from its nearest state in the tree than this limit allows, a new state at the maximum distance from the tree along the line to the random sample is used instead of the random sample itself. The random samples can then be viewed as controlling the direction of the tree growth while the growth factor determines its rate.
growth. It is possible to apply some sort of heuristic to lure the planner to search a specific direction/area, for example by forcing the planner to search towards the goal. The basic pseudocode required for a basic implementation of a RRT algorithm can be found bellow. Deeper mathematical look can be found at [13].
Figure 2.9: Example of a motion planning query on a RRT [1]
Algorithm 2 Pseudocode to perform basic RRT
Data: Start configuration qstart, number of vertices in RRT K, incremental distance ∆q Result: RRT graph S
S.init(qstart) k ← 1
while k ≤ K do
qrand ← rand conf ig
qnear ← closest vertex(qrand, S)
qnew ← compute conf ig(qnear, qrand, ∆q) S.add vertex(qnew)
S.add edge(qnear, qnew) k ← k + 1
end return S
Chapter 3
Software
In this chapter the software used for the simulation will be briefly presented. Brief mention to OMPL capabilities made, followed by a description of experiments performed. All the work was made on Ubuntu, initially using the version 14.04 to work with the Indigo version of ROS, but the project was later on exported to the version 16.04 to make use of the more recent ROS version Kinetic. The main programming languages used for the project were C++ and Python.
3.1
Gazebo
The base simulator used for this project was Gazebo. Perhaps the most popular among the roboticist’s community, it is an open source multi robot 3D simulator with an active com-munity, developed in C++, that allows to rapidly test algorithms, make custom robot designs and efficiently simulate populations of robots in complex indoor and outdoor environments (Figure 3.1 shows an empty environment ready to be build upon), making use of a robust physics engine, high-quality graphics and convenient programmatic and graphical interfaces. Gazebo uses a distributed architecture with separated libraries for physics simulation, ren-dering, user interface, communication, and sensor generation. Its API is used by the own simulator giving the user the ability to act or obtain data related to the state of the world.
Figure 3.1: Gazebo environment.
plat-form, and due to the experience the people at IRIS Lab have with this tool, making it easier to eliminate any doubt originated during the work.
3.2
Robot Operating System
The Robot Operating System (ROS), using the official description, ”is a flexible framework for writing robot software. It is a collection of tools, libraries, and conventions that aim to simplify the task of creating complex and robust robot behavior across a wide variety of robotic platforms. From the robot’s perspective, problems that seem trivial to humans often vary wildly between instances of tasks and environments. Dealing with these variations is hard, and as a result, ROS was built from the ground up to encourage collaborative robotics software development. For example, one laboratory might have experts in mapping indoor environments, and could contribute a world-class system for producing maps. Another group might have experts at using maps to navigate, and yet another group might have discovered a computer vision approach that works well for recognizing small objects in clutter. ROS was designed specifically for groups like these to collaborate and build upon each others work, as it was designed to be as distributed and modular as possible, so that users can use as much or as little of ROS as they desire. The distributed nature of ROS also fosters a large community of user-contributed packages that add a lot of value on top of the core ROS system. These packages range in fidelity, covering everything from proof-of-concept implementations of new algorithms to industrial-quality drivers and capabilities. The ROS user community builds on top of a common infrastructure to provide an integration point that offers access to hardware drivers, generic robot capabilities, development tools, useful external libraries, and more. The core of ROS is licensed under the standard three-clause BSD license. This is a very permissive open license that allows for reuse in commercial and closed source products” [14].
Essentially, ROS is an open-source, meta-operating system for your robot. It provides the services you would expect from an operating system, including hardware abstraction, low-level device control, implementation of commonly-used functionality, message-passing between pro-cesses, and package management. It also provides tools and libraries for obtaining, building, writing, and running code across multiple computers. The communication infrastructure (Figure 3.2) is based on nodes, messages and topics. The nodes are executables that com-municate between themselves by sending and receiving messages, using topics to storage for the later. All the nodes have to be declared as sender the a special node called ROS Master before transmitting.
Also, as mentioned, ROS has a wide collection of libraries, called packages, that implement functions very useful for robotic applications. Three worth mentioning that were used during this project were tf (transformation tree between multiple referentials), amcl (localization algorithm used early on based on Monte Carlo Localization) and joy (ROS driver to allow the usage of any game controller - used early on to control the robot without any planning, to test the model and later on the localization algorithms). Finally, two very used visualization and reading tools used that are deployed by ROS were rqt graph (graphical tool that allows to see the architecture of the application - which topics a node has a subscription of for example) and rviz (Figure 3.3, is a 3D visualization tool with a wide choice of features, some of the most used in this work being the visualization of the modeled robot, costmaps, sensor data, referential position estimated by the robot (or by any other referential used by the tf package) and even input a goal configuration directly in the environment map.
Figure 3.3: Rviz environment.
The choice of this middleware was made by advice from both the mentors of this work and based on already existing work by the side of Wiseware, that already had started using ROS, although the main author had no previous knowledge of the tool, so an initial learning was required.
3.3
The Open Motion Planning Library
The Open Motion Planning Library (OMPL) [15][16][1] is an open-source C++ a library composed by state of the art sampling-based motion planning algorithms and the core low-level data structures that are commonly used. It provides a way to represent all the important concepts of motion planning, such as the state space, control space, state validity, sampling, goal representations, and planners. One thing worth mention is that OMPL does not have a default collision checker, this parameter left in charge of the user. Python bindings are also included which expose almost all functionality to Python users.
The library itself is aimed to a variety of members of the roboticist community, from mo-tion planning researchers, educators to the end users. The creamo-tion of this library had the main objective of establishing a metric for comparing and benchmarking newly developed planners
to already established and tested ones, sharing the implementations of low-data structures and subroutines and focusing in evaluating the differences at the high-level algorithm.
The foundations of OMPL are based on the idea that many sampling-based motion plan-ners require a few similar components to solve a planning query: a sampler to compute valid configurations of the robot, a state validity checker to quickly evaluate a specific robot con-figuration, and a local planner to connect two samples along a collision free path. OMPL provides most of these components in similarly named classes. The following OMPL classes are analogous to ideas in traditional sampling-based motion planners: StateSampler, Near-estNeighbors, StateValidityChecker, MotionValidator, OptimizationObjective and ProblemDefinition.
Figure 3.4: Overview of OMPL API structure. Class names correspond to well understood concepts in sampling-based motion planning. Detailed documentation is available in [16].
Figure 3.4 shows the hierarchy of the major components of OMPL and how they are interrelated. Due to the object oriented nature of OMPL, the user is able to inherit currently existing components or create band new components for the planning problem at hand. In fact, when solving a query, not all objects require initialization as most already have default implementations sufficient for general planning.
Two methods of planning are available. With Geometric planning the user must define and instantiate the state space object for the robot, and provide a start and goal configura-tion in that space to define the moconfigura-tion planning query. Any planner defined in the geometric namespace can then be employed to solve the motion planning query. Some of these planners (as indicated by the planner specification) also follow optimization objectives. When
Plan-provide a start and goal configuration. In addition, the user must define valid control inputs via a ControlSpace object, and provide a method for computing how the system state changes when applying valid controls using a StatePropagator instance. The user can use any planner in the control namespace to compute a solution.
SimpleSetup provides a way to encapsulate the various objects necessary to solve a geometric or control query in OMPL. When using SimpleSetup, the user only supplies the state space, start state, and goal state (and state propagator for planning with controls). Specifically, SimpleSetup instantiates the SpaceInformation, ProblemDefinition, and Goal objects. Additionally, SimpleSetup allows for the retrieval of all of these sub-components for further customization. This means that SimpleSetup does not inhibit any native functionality of OMPL, and ensures that all objects are properly created before planning takes place. Moreover, SimpleSetup exposes fundamental function calls for ease of use. For example, the function “solve” from the Planner class is available in SimpleSetup, and it is not necessary for the user to extract the Planner from the SimpleSetup instance to do so.
To finalize this introduction to OMPL, it is important to mention OMPL.app as it provides a graphical front-end to the core OMPL library, and allows a user to see many of the ideas from sampling-based motion planning in action. The GUI is written using Python and PyQt. OMPL.app comes bundled with the core OMPL library. When OMPL received attention for this work, it was the initial intent to perform the tests in this application, but after multiple attempts at installation, the later kept failing in the computer used to perform the tests due to problems with the Python bindings. To avoid losing more time with the problem, an hard code approach was decided and the library used for testing directly through the ROS project, using Gazebo for the graphical display.
Chapter 4
Creating the simulation
In this chapter a description of the implementation made will be presented, from the mod-eling of both the robot and environment to the tests performed. Since ROS and OMPL were used, mention of packages used will be made during the next pages with a brief description of the same. Further information can be found in the documentation in each respective website [14][16].
4.1
Robot model
A model is a virtual representation of the characteristics of the physical object that we wish to simulate, like position, orientation or mass, to name a few. A model can be the representation from either a cube or a complex robot, being the later the result of a variety of rigid bodies connected by joints.
For this work, the modeling language used was the Unified Robot Description Format (URDF). It is a XML dialect used specially by ROS for the description of models, namely for the robot description in the ’rviz’ tool. It will allow the description of the physical structure of the robot, the collision model and to define the inertia moment values.
For testing purposes, only the navigation related part of the robot will be represented in the simulation. While it can be visually appealing to have a full model of the robot, it was irrelevant for this work as it had no impact in the navigation of the robot in a plane (location in height of the center of mass irrelevant to our 2D navigation but this is not valid for human movement for example). The model of the ’Wiserobot’ (see Figure 4.1) is composed of three components - base, laser and single wheel - each one modeled for visual representation (using meshes), collision and inertia. The connection between the components is given by joints, with two single wheels connected to the base on the sides and the laser connected to the base above the latter.
Still regarding the model, a plugin by the name of gazebo ros diff drive, by Piyush Khandelwal, was used in this work. It is based in the plugin found in the ROS package er-ratic gazebo plugins, with the purpose of this custom plugin is to build the ROS functionality directly into a proper Gazebo plugin, rather than a node that communicates with Gazebo.
4.2
Test environment
The world used will be a known environment in the roboticist environment, the map of Willow Garage laboratory [17] (see Figure 4.2). The office floor is a perfect case study for our algorithms, as it incorporates rooms raging from wide open spaces to small, almost maze-like rooms and sharp corners. Most rooms are connected by multiple possible routes, making it easier to evaluate the algorithm performance. The use of another world was not deemed necessary, so this will be the single test environment.
Figure 4.2: Willow Garage displayed in Gazebo.
4.3
Offline planner testing
For the performance tests of generating a path by the planners studied before, both search-based and sampling-search-based algorithms will be used. For the search-based the Dijkstra’s
sadly does not have a default package for A*. This package publishes the last plan computed, which our developed code measures and outputs its length.
For the sample-based algorithms, a different approach had to be made. No default/official package in ROS uses these type of algorithms, let alone one that allowed a proper bench-marking of the results, greatly appreciated to have in robot simulation and in special with sample-based algorithms due to the randomness factor existing in sampling. Initially the idea was to implement manually the algorithms, but this approach was expected to be hugely time consuming. With further research, it was possible to conclude that the OMPL library not only had implementations of the desired algorithms ready to be used, but also had a benchmark tool to present the results in a standardized manner across multiple planners. It was decided then that a plug-in was to be developed to use the OMPL libraries for this work. The plug-in was based on the tutorials available on OMPL’s website [16]. For a successful benchmarking, first its necessary to configure the benchmark problem as geometric or control based (geometric was used in our work as we use a local planner with the global planner, so the differential movement is defined outside of OMPL). After that, we create a ompl:Benchmark object that takes our problem as input (we assume the state space and goal/start configura-tion are already defined). We can now add the planners we desire to test to the benchmarking list. Finally, we define the limits for the tests, these being memory usage, maximum search time and number of attempts. The results are then saved to a log file of our choice, with the results capable if being transformed into either database file or plots. Example of the code is given in Figure 4.3 below. Many more options can be defined in the code, but this code is already capable of performing basic benchmarking.
Figure 4.3: Basic code for benchmarking in OMPL.
The algorithms chosen to perform the sample-based tests were the same studied in the previous chapter, PRM and RRT. Adaptations of both algorithms will also be evaluated. While regular PRM attempts to connect states to a fixed number of neighbors, PRM* gradually increases the number of connection attempts as the roadmap grows in a way that provides convergence to the optimal path. LazyPRM is a planner that constructs a roadmap of milestones that approximate the connectivity of the state space, just like PRM does. The difference is that the planner uses lazy collision checking. The difference between RRT and LazyRRT is that when moving towards a new state, LazyRRT does not check to make
sure the path is valid. Instead, it is optimistic and attempts to find a path as soon as possible. Once a path is found, it is checked for collision. If collisions are found, the invalid path segments are removed and the search process is continued. Finally, RRT* (optimal RRT) is an asymptotically-optimal incremental sampling-based motion planning algorithm. RRT* algorithm is guaranteed to converge to an optimal solution, while its running time is guaranteed to be a constant factor of the running time of the RRT. The notion of optimality is with respect to the distance function defined on the state space we are operating on.
One thing to note is that no optimization parameter will be used with OMPL except for RRT*, that demands one to be used (uses path length as default). This means that all the other planners should terminate as soon as a valid path is found towards the goal state. In the case of RRT*, the planner will use the maximum time available to continuously search for the shortest path.
4.3.1 Metrics of evaluation
The parameters for comparing the algorithms will be the path length given in meters, execution time given in seconds, memory used given in MB and the smoothness of the computed path (this means the closer to zero, the smoother the path). The choice of this metrics was pretty straightforward. The primary objective when computing a path is to find the shorter one (assuming that a uniform velocity is established), so this metric was an obvious choice. It is also in our interest to remove the maximum dead time possible in the robot, so faster computations are welcome. Possible limitations in hardware, while not specified at the time of this work, may occur, so leaving the values of memory consumed may be of interest for future work. Lastly, since we are working with a differential robot (nonholonomic) it is important to avoid paths with sharp corners, although this can be solved by using a secondary local planner to perform trajectory adjusting (further explored in the next section).
4.3.2 Test cases
With the objective of gathering results for each algorithm, test cases were defined for pairs of configurations with geometrically different expected paths, both in length and shape. All the configurations are located inside the known map, inside the region defined by the free space (no locations chosen from inside the walls for example). Certain locations of the ’Willow garage’, through measurement, were found not possible to access for our robot from a physical dimension point of view. All the various goal configurations for the test cases are described in the Table 4.1 and can be visually seen in the Figure 4.4 below (the red point cloud are the obstacles detected by the modeled LRF). For the practical reason of resetting the tests, the start configuration was always the spawn configuration of the robot in the simulation (x = 0.0, y = 0.0, θ = 0).
Test case x (m) y (m) θ (radians)
S to A 21.0 0.0 0
S to B 17.0 14.0 0
S to C -17.0 12.5 3.14
Figure 4.4: Visual representation of goal configurations for each test case.
4.4
Online simulation
After comparing the various algorithms during the offline simulation, a second group of tests was realized, this time using the model of the robot created to navigate in the Willow Garage 4.2. The objective here was to develop a capable movement model of the robot that could navigate in real time.
4.4.1 Static environment
The robot has knowledge of the map and needs to navigate the tests cases defined above. A single offline planner was chosen here with Dijkstra, as during this part of the work it was of more interest to develop a capable motion system independent of the type of planner (results for these were already tested in the previous section). A local planner will be used (given by the ROS package ”TrajectoryROS”) to apply a Proportional-Integral-Derivative (PID) controller to the defined global trajectory in order to simplify the movement while the same is performed, while maintaining the necessary constrains for a nonholomic robot. This controller will be adjusted by the desired weights for three different desired objectives (each one referring to one of the three costs of a PID): how much do we want to reach the objective, how much do we want to find a local shorter path and how much do we want to follow the given global planner. With these weights, a local planner will be continuously recalculated to help smooth the path of the robot. The implementation of computed actions is performed by the ”move base” package (here the global and local plans are used as an input) while the map information is given using the ”map server” package.
4.4.2 Undeclared obstacles
In a dynamic environment like a factory, with boxes, people or instruments sometimes being placed in non expected places, that temporary obstacles may appear in the robot trajectory. To simulate this, non declared obstacles will appear in the path generated by the global planner. The robot is always updating the global costmap accordingly to the values of the local costmap, but the global plan is always made with the values of the global costmap. If an obstacle that impairs the movement is found, the robot is stopped (the stop condition being a collision cost detected in a point belonging to the generated path) and the global costmap is updated. Then, a new global path from the present configuration to the goal configuration is computed (with the new version of the global costmap) that should allow the robot to navigate around the obstacle. This was all implemented in a new plugin, named DiffLocalPlanner, used to control the command velocities while updating the path when required, while the global plan is still given as before by the navFn. When an obstacle is detected mid course, the robot is stopped and a python script of A* is called to generate a new path with the updated costmap information. The search is performed having the obstacle detection state as the start configuration. Another strategy of approach that was a contemplated possibility was the usage of the D* algorithm, a search-based planner that contrasts to A* by searching from the goal to the start instead, that takes obstacle handling into consideration (a ROS package base planner cu does this but it was not available for our version, so this option eventually was discarded). This new local planner also is constantly updating the costmaps with the local environment. Upon reaching the goal destination, when a new goal is declared, the planner will take into account previous detected obstacles. On the other hand, if the robot passes through an are with a previous detected obstacle that no longer exists, it’s value will be updated in the costmap (it will no longer recognize that configuration area as an obstacle) but this update will only occur after calculating a new path.
Chapter 5
Results
5.1
Planner testing
The results using Dijkstra (Table 5.1) were given by console output, following the metrics of time and path length also defined for the sample-based algorithms. The smoothness was not taken into account in this situation, since the search based algorithms create paths by searching each node neighbors, so the resulting path is always expected to be smooth assum-ing a proper resolution is used.
Test case Path Length (m) Average time (s)
A 28 0.2
B 43 0.4
C 34 0.3
Table 5.1: Results for tests using Dijkstra
Next the results for all the test cases using the sampling algorithms RRT, PRM, RRT*, PRM*, LazyRRT and LazyPRM will be displayed, using OMPL benchmarking generated graphics, according to the defined metrics of evaluation above. The number of average runs for each algorithm will be of 50, with time limit per run of 10 seconds and memory limit of 500 MB. This time limit is a very generous one and mainly aimed at RRT*, since the shorter path length default optimization will make the planner use the maximum time available to find the shortest path available. The results will also be presented post simplification process. This is an OMPL functionality that evaluates each path after being generated and eliminates unnecessary nodes (in example, if we have nodes A, B and C, with node A connecting to node B and node B connecting to node C, if the connection between node A and C is collision free, then node B is eliminated from the path). An expected implication of this is the reduction of smoothness as the path will have less points.
In terms of memory (Figure 5.1) it is possible to see that both PRM and RRT consumed similar values, PRM having better results for test case C. The remaining planners consumed similar values between themselves. For RRT* and PRM* this is expected as both planners keep searching for the optimal path, and by using all the time available (Figure 5.3), they end up consuming more memory in total. For the Lazy versions of the algorithms, the memory
consumption is explained by the fact that both planners had timeout error in most of their runs (except once in test case A) as seen in Figure 5.2 by the run status. Like the star versions, they ran for the maximum time available until they timed out, consuming larger amounts of memory. Looking at the status data again, it is possible to conclude that both PRM and PRM* are robust planners, PRM* presenting minimal timed out attempts in test case C. RRT and RRT* had some problems in the latter test case, where in about half the tests only an approximate solution could be found. The time values, as mentioned above, averaged at 10 seconds (limit time) for both star and Lazy approaches of PRM and RRT. The basic versions had different values. PRM presented a mode of 0.5 seconds for test case A, 0.46 seconds for test case B and 1.15 seconds for test case C. RRT presented a mode of 1.43 seconds for test case A, 2.85 seconds for test case B and 10 seconds for test case C. Not only PRM showed a lower mode in all cases, but it also showed less dispersed values. All the simplification times (Figure 5.4) are low enough to not be untactful (less than 0.01 seconds). Moving on to path lengths results, with mode values showed being normal/simplified path lengths.
For test case A (Figure 5.5) we have a mode of 83/71 meters for PRM, 46/31 meters for RRT, 44/30 meters for LazyPRM, 37/33 meters for LazyRRT (this being the only successful run of the lazy planners out of all 150 runs - 50 for each test case), 31/29 meters for PRM* and 29/28 meters for RRT*. Both RRT and PRM present wide dispersion of the readings regarding the most showed values for each planner, with RRT presenting shorter paths overall between the two.
For test case B (Figure 5.7) we have a mode of 88/66 meters for PRM, 93/58 meters for RRT, 47/44 meters for PRM* and 44/42 meters for RRT*. Something worth mention in these results is that post simplification RRT is shorter than PRM, which does not happen before. No data for the lazy planners is displayed as none returned a valid path during benchmarking. For test case C (Figure 5.9) we have a mode of 45/36 meters for PRM, 87/59 meters for RRT, 38/36 meters for PRM* and 38/36 meters for RRT*. No data for the lazy planners is displayed as none returned a valid path during benchmarking.
Finally, in terms of smoothness all planners showcased similar results in all the three test cases (Figures 5.6, 5.8 and 5.10). As expected, the simplification process has an heavy cost in the smoothness of the resulting path, due to the fewer points used.
Figure 5.1: Memory usage for test case A, B and C.
Figure 5.3: Time elapsed for test case A, B and C.
