Extensions of sampling-based approaches to path planning in complex cost spaces: applications to robotics and structural biology

(1)

HAL Id: tel-01079963

https://tel.archives-ouvertes.fr/tel-01079963

Submitted on 4 Nov 2014

HAL is a multi-disciplinary open access

archive for the deposit and dissemination of sci-entific research documents, whether they are pub-lished or not. The documents may come from teaching and research institutions in France or abroad, or from public or private research centers.

L’archive ouverte pluridisciplinaire HAL, est destinée au dépôt et à la diffusion de documents scientifiques de niveau recherche, publiés ou non, émanant des établissements d’enseignement et de recherche français ou étrangers, des laboratoires publics ou privés.

planning in complex cost spaces: applications to robotics

and structural biology

Didier Devaurs

To cite this version:

Didier Devaurs. Extensions of sampling-based approaches to path planning in complex cost spaces: applications to robotics and structural biology. Robotics [cs.RO]. INP DE TOULOUSE, 2014. English. �tel-01079963�

(2)

&OWVFEFMPCUFOUJPOEV

**%0$503"5%&-6/7&345²%&506-064&**

%ÏMJWSÏQBS

1SÏTFOUÏFFUTPVUFOVFQBS

5JUSF

²DPMFEPDUPSBMF et discipline ou spécialité 6OJUÏEFSFDIFSDIF %JSFDUFVS TEFʾÒTF Jury:

5)µ4&

le

Institut National Polytechnique de Toulouse (INP Toulouse)

Didier Devaurs

10/10/2014

Extensions of Sampling-based Approaches to

Path Planning in Complex Cost Spaces:

Applications to Robotics and Structural Biology

ED MITT : Domaine STIC : Intelligence Artificielle

LAAS-CNRS

Lydia Kavraki, Rice University, rapporteuse Frédéric Cazals, INRIA Sophia-Antipolis, rapporteur

Stéphane Redon, INRIA Grenoble, examinateur Thierry Siméon, LAAS-CNRS, examinateur

Rachid Alami, LAAS-CNRS, examinateur Juan Cortés, LAAS-CNRS

(3)

(4)

3

Abstract

Planning a path for a robot in a complex environment is a crucial issue in robotics. So-called probabilistic algorithms for path planning are very successful at solving difficult problems and are applied in various domains, such as aerospace, computer animation, and structural biol-ogy. However, these methods have traditionally focused on finding paths avoiding collisions, without considering the quality of these paths. In recent years, new approaches have been developed to generate high-quality paths: in robotics, this can mean finding paths maximiz-ing safety or control; in biology, this means findmaximiz-ing motions minimizmaximiz-ing the energy variation of a molecule. In this thesis, we propose several extensions of these methods to improve their performance and allow them to solve ever more difficult problems. The applications we present stem from robotics (industrial inspection and aerial manipulation) and structural biology (simulation of molecular motions and exploration of energy landscapes).

Keywords

path planning · cost space · robotics · structural biology

Acknowledgments

The work presented in this thesis has been supported by:

• the European Community under Contract ICT 287617 “ARCAS”,

• the French National Agency for Research (ANR) under project GlucoDesign,

• the French National Agency for Research (ANR) under project ProtiCAD (project num-ber ANR-12-MONU-0015-01), and

• the HPC-EUROPA project (RII3-CT-2003-506079), with the support of the European Community - Research Infrastructure Action under the FP6 “Structuring the European Research Area” Program.

(5)

Summary

Planning a path for a mobile system in a complex environment is a crucial issue in robotics. In this context, sampling-based path planning algorithms have been very successful despite their conceptual simplicity. This is due to their ability to efficiently solve complex planning prob-lems involving mobile systems with numerous degrees of freedom, so-called high-dimensional problems. These algorithms have been successfully used in domains as diverse as robotics, aerospace, manufacturing, virtual prototyping, computer animation, computational structural biology, and medicine. Their underlying principle is to explore the space of the configurations of the mobile system (known as the configuration space) by sampling it, and to build a graph representing the topology of this space.

Sampling-based path planning has traditionally focused on finding feasible (i.e. collision-free) paths, without considering their quality. This is referred to as “feasible path planning”. However, in many applications it is important to compute high-quality (i.e. low-cost) paths with respect to a given cost criterion. In recent years, variants of classical sampling-based planners have been developed to take cost criteria into account during the space exploration. This can be referred to as “cost-space path planning”. Some methods even aim at finding the optimal path (i.e. the lowest-cost path) in a cost space, which is referred to as “optimal path planning”.

In this thesis, we propose several extensions of sampling-based path planning algorithms to efficiently solve ever more complex problems. Indeed, many application fields yield in-creasingly difficult, high-dimensional problems that existing methods cannot cope with, or only with difficulty. Our work focuses on robotics and structural biology applications. On the robotics side, we need planning algorithms for robots moving in large-scale workspaces, such as industrial installations. Beyond the classical computation of a path going from a point A to a point B, industrial inspection tasks require to compute a path going through several waypoints in an efficient manner. Another difficult problem we tackle is that of precise 6-dimensional manipulation performed by a towed-cable system involving three aerial robots. On the struc-tural biology side, all problems are inherently difficult because of their high-dimensionality, even when only small molecules are studied. Our work touches on two different issues: ex-ploring the energy landscape of a small peptide, and simulating the unbinding process of a protein-ligand complex.

The methods presented in this thesis are based on the Rapidly-exploring Random Tree (RRT), a popular algorithm that can solve the feasible path planning problem, as well as some of its variants: the Transition-based RRT (T-RRT), that can solve the cost-space path planning problem, and RRT*, that can solve the optimal path planning problem. This the-sis encompasses several algorithmic contributions. First, we present several extensions of T-RRT, allowing for a more efficient cost-space path planning. We improve the original mono-directional variant of T-RRT and propose a bidirectional variant that we generalize into a multiple-tree variant. Second, we combine the concepts underlying T-RRT and RRT* in two different ways, leading to two novel anytime algorithms for optimal path planning with improved results. We also show that this improvement is particularly significant when the topology of the space is complex and/or when its dimensionality is high. Third, we propose three parallel versions of RRT-like algorithms on distributed-memory architectures, which can improve feasible, cost-space and optimal path planning. We evaluate parallel versions of RRT and T-RRT, and we analyze the factors influencing their performance. Finally, we combine several of these approaches (such as the anytime and multiple-tree paradigms) to develop new algorithms able to solve challenging planning problems.

(6)

Chapter 1 Introduction

Planning a path for a mobile system, such as a robot, in a complex environment (typically, the physical world) is a crucial issue in robotics. Addressing this issue has potential repercussions reaching far beyond the field of robotics. We can start by mentioning the application domains that directly profit from technological developments in robotics, such as the aerospace and manufacturing industries. Other application domains that are less obviously connected to robotics can also benefit, as is the case for virtual prototyping using CAD/CAM software, for instance. Indeed, the (dis)assembly tests performed in this field can be modeled as path planning tasks. Another example is computer animation: if virtual actors are modeled as robots, path planning techniques become graphic animation tools. Furthermore, medical applications also exist: for example, finding a minimally-invasive path for a surgical tool, given a 3-dimensional model of a patient’s body, can be seen as a path planning problem. Finally, thanks to the similarities between structural models of robots and molecules, path planning techniques can be applied in computational structural biology.

The path planning problem was originally formulated for a rigid-bodied robot having to move in an environment containing static obstacles, while only having to avoid collisions. This has usually been referred to as the “piano mover’s problem”. A geometrical formulation of the path planning problem was derived from the definition of the configuration space, i.e. the space of all the possible configurations of the mobile system. Based on this concept, the first methods proposed to solve the path planning problem were deterministic algorithms providing an exact solution. Nevertheless, these methods cannot cope with difficult problems, such as those mentioned above. The difficulty of a path planning problem stems from the complexity of the mobile system, which is mainly expressed through the number of its degrees of freedom, as well as the complexity of potential additional constraints. Path planning problems involving mobile systems characterized by numerous degrees of freedom are usually referred to as “high-dimensional problems”.

1.1 Sampling-based Path Planning

Contrary to deterministic algorithms, so-called “probabilistic” methods for path planning have been successful at efficiently solving complex, high-dimensional problems. Their underlying principle is to explore the configuration space of the mobile system by sampling it, and to build a graph representing the connectivity of this space. Despite their conceptual simplicity, sampling-based path planners have proven valuable to a wide range of applications, such as those mentioned above. As a result, they have benefited from a considerable research effort during the last 15 years. Several methods have been proposed, and have then been extended to deal with challenging issues, such as kinodynamic planning, loop-closure constraints, or dynamic environments. Among these methods, the Rapidly-exploring Random Tree (RRT) algorithm has become very popular.

(11)

optimal path planning cost-space path planning feasible path planning conﬁguration-cost function clearance energy ... path-quality criterion based on a conﬁguration-cost function integral of cost maximal cost ... path-quality criterion path duration path length ...

Figure 1.1: Schematic representation of the relationships that exist between the three path planning paradigms addressed in this thesis.

Sampling-based path planners such as RRT have traditionally been used to find feasible paths (i.e. collision-free paths) without considering the quality of these paths. This paradigm can be referred to as “feasible path planning” (cf. Fig. 1.1). However, in many application fields it is important to compute high-quality paths, with respect to a given quality criterion. Historically, the first quality criteria to be considered were path length and path duration, mainly to address the fact that the paths produced by sampling-based algorithms were usually “jerky”. Later on, the notion of clearance was introduced: the underlying idea was to generate paths along which the mobile system would remain as far as possible from the obstacles, to ensure its own safety. When using such quality criteria is desirable, after a solution path has been computed by a sampling-based path planner, it is most common to try and improve the quality of this path during a post-processing phase involving so-called “smoothing” methods. Nevertheless, such methods only allow to improve the path locally. We explain, later on, how better results can be achieved by taking the quality criteria into account during the exploration of the configuration space.

As already mentioned, it can be useful to generate high-quality paths, based on the use of specific quality criteria. In some application contexts, instead of considering a criterion, such as path length, that assesses the quality of a path as a whole, it might be more interesting to ensure that all configurations along the path have low costs, with respect to a given cost function. For instance, when high-clearance paths are desirable, the cost of a configuration can be based on the inverse of the distance between the mobile system and the closest obstacle. In structural biology, the cost of a conformation of a molecule is the molecular energy. When such a cost function is defined on the configuration space, we call the latter a “cost space”. This paradigm can thus be referred to as “cost-space path planning” (cf. Fig. 1.1). In recent years, variants of classical sampling-based algorithms have been developed to take cost functions into account during the exploration of the configuration space. One of these methods is a variant of RRT called the Transition-based RRT (T-RRT).

(12)

1.2. Algorithmic Contributions of the Thesis 11 Beyond the computation of a high-quality path, one might be looking for an optimal path, with respect to a given path-quality criterion. This paradigm is referred to as “optimal path planning” (cf. Fig. 1.1). When applied to this problem, classical grid-based methods, such as A* or D*, can compute resolution-optimal solution paths. However, these methods are limited to problems involving low-dimensional spaces that can be discretized without leading to a combinatorial explosion. As an alternative, some deterministic path planners implicitly compute the optimal path with respect to a specific criterion. For instance, the visibility diagramproduces the shortest path, and the Voronoi diagram generates the path with optimal clearance. Nevertheless, such methods are also limited to low-dimensional spaces and can only deal with polygonal obstacles. On the other hand, classical sampling-based path planners can cope with high-dimensional spaces, but they usually produce sub-optimal solutions. As a complement, the aforementioned smoothing methods can be used to improve path quality in a post-processing phase, but they offer no guarantee of converging toward the global optimum. The first RRT-like path planner providing such guarantee was RRT*.

Even though classical sampling-based path planners have achieved great success, their cost-space counterparts are still in their infancy and suffer from strong limitations. Indeed, many application fields yield increasingly difficult, high-dimensional problems that existing methods cannot really cope with, or only with difficulty. Besides, optimal path planning is even more challenging on such problems, as it amounts to solving a linear, non-convex, high-dimensional, global optimization problem. Additionally, the most commonly used path-quality criteria are still path length and path duration, and the most commonly used configuration-cost functions are discrete, coarse-grained ones. Our work aims at dealing with continuous configuration-cost functions, as well as path-quality criteria based on these functions (cf. Fig. 1.1), which is more challenging. Our goals are to improve the capabilities and the performance of sampling-based algorithms, mainly in the context of cost-space and optimal path planning. We also aim to study applications that require the creation of new path planning methods, in the fields of robotics and structural biology.

1.2 Algorithmic Contributions of the Thesis

In this thesis, we propose several extensions of sampling-based path planning algorithms to efficiently solve ever more difficult problems involving complex continuous cost functions. The methods we develop are based on RRT, which can solve the feasible path planning problem, as well as some of its variants: T-RRT, that can solve the cost-space path planning problem, and RRT*, that can solve the optimal path planning problem. The RRT, T-RRT and RRT* algorithms are reviewed in Chapter 2. Then, we present the various algorithmic contributions proposed in this thesis (cf. Fig. 1.2).

First, with the objective of achieving a more efficient cost-space path planning, we develop several extensions of T-RRT, starting from enhancements of its original mono-directional variant, then presenting a bidirectional variant, and finally generalizing it into a multiple-tree variant (cf. Fig. 1.2 and Chapter 3). More precisely, we suggest to improve the performance of the mono-directional T-RRT (that was originally proposed as an extension of the basic Extend RRT) by modifying the implementation of its transition test (cf. Section 3.1). We also show that using the Connect T-RRT or the Goal-biased T-RRT can generally provide improvements (cf. Section 3.1). Then, we present a bidirectional extension of T-RRT that reduces running time and sometimes increases (or otherwise maintains) solution-path quality (cf. Section 3.2). By generalizing this approach, we also develop a multiple-tree extension of T-RRT that can compute a path going through a set of waypoints, and we show that it outperforms path planners involving the Bidirectional T-RRT (cf. Section 3.3). Throughout Chapter 3, we apply these extensions of T-RRT to simulated yet practical robotic problems, such as industrial inspection tasks involving aerial robots.

(13)

Optimal Path Planning Transition-based RRT* Anytime T-RRT Anytime Multi-T-RRT Cost-Space Path Planning mono-directional T-RRT Bidirectional T-RRT Multi-T-RRT Feasible Path Planning

Parallel Path Planning OR parallel RRT Distributed RRT Manager-worker RRT

Figure 1.2: Schematic organization of the principal algorithms proposed in this thesis, and representation of the relationships between them.

Second, to improve efficiency in the context of optimal path planning, we combine the beneficial, underlying concepts of T-RRT and RRT* in two different ways (cf. Chapter 4). We propose an extension to RRT* named Transition-based RRT* (T-RRT*) and an extension to T-RRT named Anytime T-RRT (AT-RRT). From their definitions, T-RRT* and AT-RRT feature both 1) the cost-based filtering properties of the transition test of T-RRT, favoring the creation of new nodes in low-cost regions of the configuration space, and 2) the quality-based management of edges of RRT*, allowing the quality of the solution path to increase with time (cf. Section 4.1). We show that both algorithms are probabilistically complete and asymptotically optimal (cf. Section 4.2). Then, we evaluate T-RRT* and AT-RRT on several optimal path-planning problems, and show that they converge toward the optimal solution-path faster than RRT* (cf. Section 4.3). We also show that the performance improvement they achieve is particularly significant when the topology of the configuration space is complex and/or when its dimensionality is high.

Third, to improve the efficiency of feasible, cost-space and optimal path planning, we propose three parallelization strategies for RRT-like algorithms (cf. Fig. 1.2 and Chapter 5). We focus on parallelizing RRT on large-scale distributed-memory architectures, which requires using the Message Passing Interface (MPI). More precisely, we develop three parallel versions of RRT, based on classical parallelization schemes: OR parallel RRT, Distributed RRT and Manager-worker RRT (cf. Section 5.1). Then, we evaluate the parallel versions of RRT and T-RRT on several path planning problems (cf. Section 5.2), and we analyze the various factors influencing their performance (cf. Section 5.3). Our evaluation results show that parallelizing RRT-like algorithms with MPI can provide substantial performance improvement in several cases that occur in numerous complex robotic problems and all structural biology problems. Additionally, we discuss how the RRT-like algorithms introduced in this thesis can be parallelized (cf. Section 5.4).

(14)

1.3. Applications 13 Finally, we combine several of the approaches introduced in this thesis to develop novel algorithms that can solve new kinds of challenging path-planning problems. Our motivation for presenting these methods separately in Chapters 3 to 5 was the will to evaluate and analyze them independently of each other. On the other hand, interesting new applications can be addressed when combining some of these methods together. For instance, we integrate the anytime and multiple-tree paradigms to create an Anytime Multi -T-RRT (cf. Fig. 1.2) that we use in robotics (cf. Chapter 6) and structural biology (cf. Chapter 7).

1.3 Applications

In addition to the aforementioned algorithmic contributions, this thesis also features several contributions on the application side. Indeed, we have applied several of the sampling-based planning techniques presented in this thesis to challenging (and sometimes new) path-planning problems encountered in robotics (cf. Chapter 6) and computational structural biology (cf. Chapter 7).

Robotics

On the robotics side, we need path-planning methods that can deal with robots moving in large-scale workspaces, such as industrial installations (oil platform, power plant, steel factory, etc). Beyond the classical computation of a path going from a point A to a point B (which we refer to as the “init-to-goal” problem), industrial inspection tasks require to compute a path going through a given set of waypoints in an efficient manner, which we refer to as the “ordering-and-pathfinding problem” (cf. Section 6.2). To solve this hybrid task-and-path planning problem, we propose a variant of T-RRT called Anytime Multi -T-RRT, based on the combination of two extensions of T-RRT presented in this thesis: the Multi -T-RRT and the Anytime T-RRT. Using the Anytime Multi -T-RRT, ordering-and-pathfinding problems can be solved from a purely geometrical perspective, without having to use a symbolic task planner. We demonstrate this method on a simulated industrial inspection problem involving an aerial robot.

Another difficult problem we tackle here is that of precise 6-dimensional manipulation performed by a towed-cable system involving cooperative aerial robots (cf. Section 6.1). For that, we propose a system, that we have called FlyCrane, and that consists of a platform attached to three flying robots using six fixed-length cables. In addition, the main element of our approach is an application-specific configuration-cost function that takes the constraints inherent to this robotic system into account. More precisely, this cost function is based on wrench-feasibility constraints (derived from the static analysis of cable-driven manipulators) and on thrust constraints. To validate our approach, we study two simulated manipulation problems. Our experiments show the superiority of cost-space path planning over feasible path planning when dealing with such complex robotic systems.

Most of the path-planning problems we address on the robotics side emanate from the ARCAS European project1_{. Some of the goals of this project are to develop robotic systems} involving cooperative aerial robots for the installation, the inspection, and the maintenance of industrial installations in places that are difficult to access for humans. An example of possible application is the construction of landing platforms in uneven terrains, for manned or unmanned aircrafts. Another example is the assembly of temporary structures for the evacuation of people in rescue operations. Note that, despite the interest of tackling such problems in real-life situations, the role of our team has been to develop and evaluate new path-planning methods within simulated environments. Real-life experiments are part of the ongoing work within the ARCAS project.

1

(15)

Computational Structural Biology

On the computational structural biology side, all problems are inherently difficult because of their high dimensionality, even when only small molecules are considered.

The first issue we address in this thesis is the exploration of the energy landscapes of small yet highly-flexible peptides (cf. Section 7.1). For that, we combine two complementary sampling-based methods. 1) We propose a simplified version of the Basin Hopping algorithm that can quickly reveal the meta-stable structural states of a peptide. In computational structural biology, Basin Hopping is a classical method that enables sampling local minima on the energy landscape of a molecule. 2) Then, we use the Multi -T-RRT and the Anytime Multi-T-RRT to quickly determine transition state and transition path ensembles, as well as transition probabilities between these meta-stable states. We validate this combined approach on the terminally-blocked alanine.

The second issue we address is the simulation of the unbinding process of a protein-ligand complex (cf. Section 7.2). We propose an approach that builds on a mechanistic representation of the molecular system and that currently considers only partial flexibility. Besides, at the moment, the approach is purely geometric, which means that no molecular energy is computed and that motions are validated only on the basis of collision avoidance. This simplification of the problem allows us to use a variant of RRT called Manhattan-like RRT (ML-RRT), whose exploratory efficiency leads to very short computing times. This was a requirement imposed by our decision to implement this method as an efficient web application. This tool yields ligand unbinding pathways that, as a first approximation, can provide useful information about protein-ligand interactions. We demonstrate this approach on the hexameric insulin-phenol complex. Finally, let us mention that integrating molecular energy computations into this approach is part of our ongoing research.

Our work in computational structural biology has been carried out within two research projects named GlucoDesign and ProtiCAD. The objective of the GlucoDesign project was the computer-aided design of enzymatic glycosylation tools for the synthesis of vaccines against endemic shigellosis (or bacillary dysentery). The goals of the ProtiCAD2 _{project are to yield} advances in a general methodology for protein design, and to develop computational tools for the synthesis of new proteins.

2

(16)

Chapter 2 Related Sampling-based Methods

for Path Planning

Sampling-based algorithms for path planning have been very successful at efficiently solving difficult planning problems involving mobile systems characterized by numerous degrees of freedom, so-called high-dimensional problems [33, 104]. They have proven valuable in a wide range of application domains, such as robotics, aerospace, manufacturing, virtual prototyping, computer animation, medicine, and computational structural biology. As a result, they have benefited from a considerable research effort during the last 15 years. Several approaches have been proposed (see, for example, [75,95,106]) and then extended to deal with challenging issues, such as kinodynamic planning [40,75,105], loop-closure constraints [38,160], or dynamic environments [59, 61, 67, 152, 166].

We now quickly review some of these existing methods, within the contexts of feasible, cost-space, and optimal path planning. We focus more specifically on the algorithms that we extend in this thesis or that we use for comparison with our approaches in experimental evaluations. We also review related work in the context of parallel path planning.

2.1 Feasible Path Planning

Traditionally, path planning has focused on computing feasible paths for a mobile system in an environment containing obstacles [102]. Informally speaking, for a path to be feasible, it has to avoid collisions with obstacles and collisions between articulated parts of the mobile system (so-called self-collisions).

2.1.1 Theoretical Framework

The classical formulation of the path planning problem relies on abstracting the workspace of a mobile system into a configuration space C, also called C-space [110]. A configuration q ∈ C describes the position and volume occupied by the mobile system in the workspace. The subset of C containing the configurations inducing self-collisions or collisions with some obstacles in the workspace is denoted by Cobst. Assuming that its complement in C is an open set, we denote by Cfree the set cl(C \ Cobst) of configurations producing no collision, where cl() denotes the closure of a set.

Given an initial configuration qinit ∈ Cfree and a goal configuration qgoal ∈ Cfree, a path planning problem can be defined as a triplet (C, qinit, qgoal). A path over the C-space is a continuous function π : [0, 1] → C. It is said to be collision-free if for all t ∈ [0, 1], π(t) ∈ Cfree, i.e. π : [0, 1] → Cfree. Let Π denote the set of all paths over the C-space, and let Πfree denote the set of collision-free paths in Π. The feasible path planning problem is classically defined as follows:

(17)

Definition 1(Feasible path planning problem). Given a path planning problem (C, qinit, qgoal), find a path π ∈ Πfree such that π(0) = qinit and π(1) = qgoal, if one exists, or report failure otherwise.

Let Πfeasdenote the set of feasible paths, i.e. the set of paths in Πfreesuch that π(0) = qinit and π(1) = qgoal. Among the path planning problems having a solution, the theoretical frame-work we rely on requires to focus on problems satisfying the robust feasibility property [93]. This property is based on the concept of strong clearance. Given λ ∈ R+, a path π ∈ Πfree is said to have strong λ-clearance if π lies entirely inside the λ-interior of Cfree. The λ-interior of Cfree is the set of configurations that are at least a distance λ away from any configuration in Cobst. From that, robust feasibility is defined as follows:

Definition 2(Robust feasibility). A path planning problem (C, qinit, qgoal) is said to be robustly feasible if there exists a path π ∈ Πfeas having strong λ-clearance, for some λ ∈ R+.

Based on the geometric formulation provided by the configuration space, several techniques have been proposed in the robotics community to solve the feasible path planning problem. The first ones were deterministic methods providing an exact solution [102]. These methods proved to be complete: they can terminate in finite time, returning a solution if one exists, or failure otherwise. However, they cannot cope with difficult problems, and are limited to low-dimensional spaces.

In this thesis, we focus on sampling-based approaches because they can solve the complex, high-dimensional problems we are dealing with. The underlying principle of these methods is to explore the configuration space of the mobile system by sampling it, and to build a graph representing the connectivity of this space. The most popular sampling-based path planners, that we present in the next sections, are the Rapidly-exploring Random Tree (RRT) and the Probabilistic Road-Map (PRM). As all sampling-based path planners, they are not complete, but they satisfy a property called probabilistic completeness, that can be interpreted as a notion of “almost-sure” success [33]:

Definition 3 (Probabilistic completeness). An algorithm A is said to be probabilistically complete if, for any robustly feasible path planning problem(C, qinit, qgoal), the probability that A fails to return a solution when one exists decays to zero as the running time of A approaches infinity.

2.1.2 Rapidly-exploring Random Tree (RRT)

The Rapidly-exploring Random Tree (RRT) is a prevalent sampling-based algorithm, usually applied to single-query path planning problems [100, 103, 106]. It is suited to solve robot path planning problems involving holonomic, nonholonomic, kinodynamic, or kinematic loop-closure constraints [38, 105, 106]. It is also applied to planning in discrete spaces or for hybrid systems [23, 56]. In computational biology, it is used to analyze genetic network dynamics [9] or protein-ligand interactions [39], for instance.

Let us consider the feasible path planning problem (C, qinit, qgoal). Starting from the initial configuration qinit, RRT iteratively builds a tree T that tends to rapidly expand over the C-space, thanks to the implicit enforcement of a Voronoi bias [106]. The nodes and edges of T correspond to configurations and local moves between configurations, respectively. The pseudo-code of the original variant of RRT is presented in Algorithm 1. At each iteration, a configuration qrand is randomly sampled in C. Then, an expansion toward qrand is attempted, starting from its nearest neighbor, qnear, in T . If the expansion succeeds, a new configuration qnew is added to T , and connected by an edge to qnear. The criteria on when to stop the exploration can be reaching the goal configuration qgoal, a given number of nodes in T , a given number of iterations, or a given running time.

(18)

2.1. Feasible Path Planning 17

Algorithm 1:RRT

input : the feasible path planning problem (C, qinit, qgoal) output: the tree T

1 T ← initTree(qinit)

2 while not stoppingCriteria(T ) do

3 q_rand ← sampleRandomConfiguration(C) 4 qnear← findNearestNeighbor(T , qrand) 5 qnew← extend(qnear, qrand)

6 if q_new6= null then 7 addNewNode(T , qnew) 8 addNewEdge(T , qnear, qnew) 9 return T

The expansion procedure involved in the main loop of the RRT algorithm (line 5) can be implemented in two different manners, called Extend and Connect. The Extend procedure consists of performing an interpolation between qnear and qrand, at a distance equal to the extension step-size, δ, from qnear(except if distance(qnear, qrand) < δ, in which case the result of the interpolation is qrand itself) [103]. The Connect procedure consists of iterating the Extend function until qrand is reached, or until an obstacle is encountered, in which case qnew is defined as the last collision-free configuration [100]. Note that, when using the Connect method, it is also possible to add all intermediate configurations (produced by the consecutive Extend procedures) as nodes in T .

Theorem 1 (Probabilistic completeness of RRT). The RRT algorithm is probabilistically complete [106].

Despite its successes, when applied to complex problems, the growth of an RRT can become computationally expensive [29, 36, 84, 162]. Some techniques have been proposed to improve the efficiency of RRT, by dynamically controlling its sampling domains [84], reducing the dispersion of the samples drawn in C [109], or reducing the complexity of the nearest neighbor search [162]. Performance can also be improved by relaxing the constraints used to validate local motions and using gap reduction techniques perturbing the solution path in a post-processing phase [29]. A well-known weakness of RRT is its sensitivity to the metric defined on the C-space; this issue has been addressed in [30]. Furthermore, a resolution complete variant of RRT has been developed [31]. In this thesis, we present an additional enhancement of RRT, which consists of parallelizing it on large-scale distributed-memory architectures (cf. Chapter 5).

2.1.3 Probabilistic Road-Map (PRM)

The Probabilistic Road-Map (PRM) is another well-known sampling-based algorithm, usually applied to multiple-query path planning problems [95]. There exist numerous variants of PRM. The pseudo-code of the one used in this thesis is presented in Algorithm 2. Given the feasible path planning problem (C, qinit, qgoal), starting from the initial configuration qinit, this version of PRM iteratively builds a graph G over the C-space. The most standard versions of PRM differ from this variant in that they consist of two distinct phases: 1) a set of configurations is randomly sampled in Cfree, and 2) connections are created between these configurations. However, interleaving these two phases and building the graph iteratively, as is done in Algorithm 2, allows us to benefit from an anytime behavior that is required by the experimental evaluation in which PRM is involved here (cf. Section 3.3).

(19)

Algorithm 2:PRM

input : the feasible path planning problem (C, qinit, qgoal) the connection radius R

output: the graph G 1 G ← initGraph(qinit)

2 while not stoppingCriteria(G) do

3 qnew ← sampleRandomConfiguration(Cfree) 4 addNewNode(G, qnew)

5 Q_near← findNodesInBall(G, q_new, R) 6 foreach qnear∈ Qnear do

7 if isCollisionFree(path(qnew, qnear)) then 8 addNewEdge(G, q_new, q_near)

9 return G

The variant of PRM we use works as follows (see Algorithm 2): At each iteration, a new configuration qnew is randomly sampled in Cfree, and a new node is added to G. Then, all the nodes in G that are within a distance R of qnew (where R is called the connection radius) are considered for edge creation. For each candidate qnear, if the path between qnew and qnear is collision-free, a new edge is added to G. The criteria on when to stop the exploration are the same are those defined for RRT.

Theorem 2 (Probabilistic completeness of PRM). The PRM algorithm is probabilistically complete [95].

2.2 Cost-Space Path Planning

Instead of building paths that are only feasible, it can be important to generate “high-quality” paths, with respect to some quality criteria. Historically, because the paths produced by sampling-based algorithms were usually “jerky”, the first quality criteria to be used were path length and path duration, which assess the quality of a path as a whole [69]. However, it may be more interesting to ensure that all configurations along the path have low costs, with respect to a given cost function. When such a cost function is defined on the C-space, we call the latter a “cost space”, and we talk about “cost-space path planning”.

Early work in cost-space path planning only involved discrete, coarse-grained cost func-tions [60, 93]. Our work focuses on continuous cost funcfunc-tions, which is more challenging. As an example, in outdoor navigation problems, the cost of a configuration can be the elevation of the position of a robot within a 2-D terrain. In planning problems where high-clearance paths are desirable, the cost of a configuration can be based on the inverse of the distance between the mobile system and the closest obstacle [49, 83]. More complex cost functions can appear in robotic problems [12, 116] and structural biology problems [82].

Let c : C → R+ denote a cost function associating to each configuration of the C-space a positive cost value. The cost-space path planning problem is denoted by a quadruplet (C, qinit, qgoal, c). Solving this problem consists of solving the feasible path planning prob-lem (C, qinit, qgoal) while taking the cost function c into account during the exploration of the C-space. This amounts to performing a rejection sampling of configurations in C, by filtering configurations on the basis of their costs. More precisely, each method aimed at solving the cost-space path planning problem imposes a specific cost constraint evaluating each configu-ration, based on its cost alone, or on the cost variation associated with the local move between two configurations.

(20)

2.2. Cost-Space Path Planning 19

Algorithm 3:RRTobst

input : the cost-space path planning problem (C, qinit, qgoal, c) the cost threshold cmax

the increase rate of the cost threshold crate output: the tree T

1 T ← initTree(q_init) 2 cmax← c(qinit)

4 q_rand ← sampleRandomConfiguration(C) 5 qnear← findNearestNeighbor(T , qrand) 6 qnew← extend(qnear, qrand)

7 if q_new6= null and c(q_new) < c_max then 8 addNewNode(T , qnew)

9 addNewEdge(T , qnear, qnew) 10 c_max← c_max+ c_rate

11 return T

The first approaches dealing with cost-space path planning were based on RRT. Un-fortunately, they were all focused on specific applications in the area of 2D robot naviga-tion [57, 58, 60, 61, 107, 151], and some of them were evaluated only on configuranaviga-tion spaces involving very coarse-grained, discrete cost functions [60,61, 151]. More importantly, all these methods suffer from different practical drawbacks [83]. For example, some of them rely on the estimated cost-to-goal, which tends to bias the search straight toward the goal at the expense of higher-quality paths [60, 61, 151]. Also, the threshold-based method presented in [57, 58] suffers from the non-decreasing nature of its threshold and from its high sensitivity to the increase rate of the threshold [83]. As it is involved in some of our experiments, we now present in greater details this threshold-based method. After that, we present a variant of RRT called the Transition-based RRT (T-RRT) that has been more successful for cost-space path planning than the aforementioned methods.

2.2.1 Threshold-based RRT (RRTobst)

The RRTobst algorithm is an extension of RRT devised specifically for cost-space path plan-ning. It was proposed in the context of rough terrain navigation [58]. The cost function introduced in that work aimed to assess the level of difficulty (the so-called “obstacleness”) corresponding to attaining a given configuration of the robot. The pseudo-code of RRTobst is presented in Algorithm 3. The idea is to accept or reject new configurations created by the RRT expansion based on their costs: a configuration is accepted if its cost is below a given threshold, denoted by cmax, and rejected otherwise. This cost threshold is a dynamic parameter of the algorithm: it is initialized to a low value (typically the cost of the initial configuration) and iteratively increased during the space search, using the cost increment crate. As a result, the search performed by RRTobst is biased toward low-cost regions of the config-uration space. Higher-cost regions of the space are explored only when inevitable. Therefore, paths produced by RRTobst follow low-cost regions of the space, and their maximal cost is kept relatively low.

In our experimental evaluations (cf. Section 3.3), we use a multiple-tree variant of RRTobst, called RRTobst way [57]. This method grows several trees rooted at various waypoints on the C-space, and regularly tries to connect them. The idea is to explore significant low-cost basins of the cost landscape before connections are made between them.

(21)

Algorithm 4:Transition-based RRT

input : the cost-space path planning problem (C, qinit, qgoal, c) output: the tree T

1 T ← initTree(qinit)

3 q_rand ← sampleRandomConfiguration(C) 4 qnear ← findNearestNeighbor(T , qrand) 5 if refinementControl(T , qnear, qrand) then 6 q_new← extend(q_near, q_rand)

7 if qnew6= null and transitionTest(T , c(qnear), c(qnew)) then

8 addNewNode(T , qnew)

9 addNewEdge(T , q_near, q_new) 10 return T

2.2.2 Transition-based RRT (T-RRT)

The Transition-based RRT (T-RRT) algorithm is a general sampling-based approach to cost-space path planning that can deal with any configuration-cost function [83]. Being an exten-sion of RRT, T-RRT combines the exploratory strength of RRT with a stochastic optimization mechanism. It has been successfully applied to various cost-space path-planning problems in robotics [12, 79, 83, 114] (some examples even involving human–robot interactions [114]) and in computational structural biology [79, 82]. When compared to methods developed ear-lier [58, 151], T-RRT produced better-quality paths [83].

T-RRT extends RRT by integrating a stochastic transition test enabling it to favor the ex-ploration of low-cost regions of the C-space [83]. This transition test is based on the Metropolis criterion typically used in Monte Carlo optimization methods [65]. These techniques aim at finding global minima in complex spaces and involve randomness as a way to avoid being trapped in local minima. Similarly, T-RRT uses a transition test to accept or reject a new candidate configuration, based on the cost variation associated with the local motion whose target is the new configuration and whose source is its nearest neighbor in the tree. The pseudo-code of T-RRT (shown in Algorithm 4) is analogous to that of the Extend RRT, with the addition of the transitionTest and refinementControl functions.

The transitionTest presented in Algorithm 5 is used to accept or reject the move between two configurations on the basis of their costs ci and cj. Note that ci refers to the cost of the source configuration, and cj refers to the cost of the target configuration. Three cases are possible: 1) A new configuration whose cost is higher than the maximal value cmax1 is automatically rejected. 2) A transition corresponding to a downhill move in the cost landscape (cj≤ ci) is always accepted. 3) An uphill move is accepted or rejected based on the probability exp(−(cj− ci) / T ) that decreases exponentially with the cost variation cj− ci, in a way similar to the Metropolis criterion. In that case, the level of selectivity of the transition test is controlled by the adaptive parameter T , called temperature only by analogy to statistical physics. Low temperatures limit the expansion to gentle slopes of the cost landscape, and high temperatures enable to climb steep slopes. In T-RRT, the temperature is dynamically tuned during the search process, based on the current number of consecutive rejections nF ail and on the maximal number of consecutive rejections nF ailmax. 1) After each accepted uphill move, T is decreased to avoid over-exploring high-cost regions. More precisely, T is divided by the temperature adjustment factor α, and nF ail is reset to 0. 2) After each rejected uphill transition, different actions are possible, depending on the value of nF ail. If nF ail is less than

1

(22)

2.2. Cost-Space Path Planning 21

Algorithm 5:transitionTest (G, ci, cj) input : the maximal cost cmax

the current temperature T

the temperature adjustment factor α

the current number of consecutive rejections nF ail the maximum number of consecutive rejections nF ailmax output: true if the transition is accepted, false otherwise

1 if c_j> c_max then 2 returnFalse 3 else if c_j≤ c_i then 4 returnTrue

5 else if rand(0, 1) < exp(−(c_j− c_i) / T ) then

6 T ← T / α

7 nF ail ← 0 8 returnTrue 9 else

10 if nF ail > nF ail_maxthen

11 T ← T · α

12 nF ail ← 0

13 else

14 nF ail ← nF ail + 1 15 returnFalse

Algorithm 6:refinementControl (G, qnear, qrand) input : the extension step-size δ

the refinement ratio ρ

output: true if refinement is not too high, false otherwise

1 if distance(qnear, qrand) < δ and nbRefinementNodes(G) > ρ · nbNodes(G) then 2 returnFalse

3 else

4 returnTrue

its maximal value nF ailmax, the temperature remains unchanged, but nF ail is incremented by 1. If nF ail is greater than nF ailmax, T is increased to facilitate exploration and avoid being trapped in a local minimum. More precisely, T is multiplied by the temperature adjustment factor α, and nF ail is reset to 0. According to experiments reported in [83], a value of 2 seems to be a good choice for the α parameter. Furthermore, note that the adaptive tuning of the temperature allows T-RRT to automatically balance its bias toward low-cost regions with the Voronoi bias of RRT. This can be controlled by changing the value of the nF ailmax parameter, which thus determines a trade-off between low computation time and high quality of the produced paths. A small value (e.g. 10) leads to a greedy search, and a greater value (e.g. 100) enables to produce higher-quality paths [83].

The adaptive tuning of the temperature ensures a given success rate for uphill transitions, but it can also produce an unwanted side-effect: T may be reduced by the acceptance of new states close to states already contained in the tree, whereas increasing T may be required to go over a local cost barrier and explore new regions of the space. Accepting such states only contributes to refining the exploration of low-cost areas already reached by the tree.

(23)

The objective of the refinementControl procedure (shown in Algorithm 6) is to limit this refinement and facilitate tree expansion toward unexplored regions of the space. The idea is to reject an expansion that would lead to more refinement if the number of refinement nodes already present in the tree is greater than a certain ratio ρ of the total number of nodes. A refinement node is defined as a node whose distance to its parent is less than the extension step-size δ. Another benefit of the refinement control is to limit the number of nodes in the tree, and thus to reduce the computational cost of the nearest-neighbor search. According to experiments performed in [83], a value of 0.1 seems to be a good choice for the ρ parameter. It has been shown that this refinement-control mechanism is beneficial mainly when dealing with low-dimensional spaces. However, its impact seems to be negligible on high-dimensional problems. Therefore, for simplicity’s sake, we do not include this procedure in the T-RRT-like algorithms we present in the rest of this thesis.

Theorem 3(Probabilistic completeness of T-RRT). In the space where configurations whose cost is greater thancmaxare regarded as part of Cobst, the T-RRT algorithm is probabilistically complete [83].

2.3 Optimal Path Planning

In some application contexts, beyond the computation of high-quality paths, it might be interesting to produce an optimal path, with respect to a given path quality criterion. This paradigm is referred to as “optimal path planning” [93]. This can mean, for instance, finding the shortest path, as is often done in robotics. However, in this thesis, we do not wish to restrict ourselves to such criteria that assess the quality of the path as a whole and ignore the costs of the configurations along the path. Our objective is to deal with a more general formulation of the optimal path planning problem, where the path-quality criterion is defined based on the configuration-cost function characterizing the cost space (as we describe later in this section). As examples, in robotics this kind of optimal path planning can result into looking for the path maximizing safety; in biology this means finding the motion minimizing the energy variation of a molecule.

When applied to the optimal path planning problem, classical grid-based methods, such as A* or D*, can compute resolution-optimal solution paths [148]. However, these methods are limited to problems involving low-dimensional spaces that can be discretized without leading to a combinatorial explosion. As an alternative, some deterministic path planners implicitly compute the optimal path with respect to a specific quality criterion. For instance, the visi-bility diagramallows obtaining the shortest path, and the Voronoi diagram allows generating the path with optimal clearance [102]. Nevertheless, such methods are also limited to low-dimensional spaces, and can only deal with polygonal obstacles. On the other hand, classical sampling-based path planners can cope with high-dimensional spaces, but usually produce sub-optimal solutions because they focus on feasible path planning. As a complementary technique, after a solution path has been computed, it is common to improve the quality of this path during a post-processing phase involving so-called “smoothing” methods [69]. Nevertheless, such methods only allow to improve the path locally, and offer no guarantee of converging toward the global optimum. The first sampling-based path planner providing such guarantee was RRT* [90], that we present later in this section.

2.3.1 Theoretical Framework

Let cp: Πfree → R+denote a quality criterion, associating to each collision-free path a positive cost value, and whose definition is based on the configuration-cost function c : C → R+. Note that cp can be defined in different ways (as the integral of the cost, the mechanical work, the

(24)

2.3. Optimal Path Planning 23 maximal cost, etc) that we describe in the next section. The optimal path planning problem can be defined as follows:

Definition 4(Optimal path planning problem). Given a path planning problem (C, qinit, qgoal), a configuration-cost function c : C → R+, and a monotonic, bounded path-quality criterion cp : Πfree → R+, find a path π∗ ∈ Πfeas such that cp(π∗) = min{cp(π) | π ∈ Πfeas} if one exists, or report failure otherwise.

Based on these notations, an optimal path planning problem is denoted by a quintuplet (C, qinit, qgoal, c, cp). If it admits a solution path π∗, then π∗ is called the optimal path. Among the optimal path planning problems having an optimal solution path, the theoretical frame-work we rely on requires to focus on problems admitting a robustly optimal solution [93]. This concept is based on the notion of weak clearance. Given λ ∈ R+, a path π ∈ Πfree is said to have weak λ-clearance if there exists a path π0 ∈ Πfree having strong λ-clearance and a homotopy ψ : [0, 1] → Πfree such that ψ(0) = π, ψ(1) = π0, and ∀ α ∈ (0, 1), ψ(α) has strong λα-clearance for some λα∈ R+. Note that, if a path has strong λ-clearance, it obviously has weak λ-clearance. From that, robust optimality is defined as follows:

Definition 5 (Robust optimality). An optimal solution path π∗ to an optimal path planning problem (C, qinit, qgoal, c, cp) is robustly optimal if it has weak λ-clearance and if any sequence of paths {πm}m∈N in Πfree such that limm→+∞πm= π∗ satisfieslimm→+∞cp(πm) = cp(π∗).

In the context of optimal path planning, the evaluation of a planning algorithm should be based not only on the concept of probabilistic completeness, but also on the concept of asymptotic optimality [93]. A similar concept that has emerged in recent work, but that we do not consider in this thesis is asymptotic near-optimality [54, 118]. Asymptotic optimality can be interpreted as a notion of “almost-sure” convergence toward the optimal path, and has been defined as follows [93]:

Definition 6 (Asymptotic optimality). An algorithm A is asymptotically optimal if, for any optimal path planning problem (C, qinit, qgoal, c, cp) admitting a robustly optimal solution path with finite cost c∗ ∈ R+, the cost of the current solution path that can be returned by A (this cost being infinite if no solution is available yet) decreases toward c∗ _{as the running time of} A approaches infinity.

Note that it has been shown that RRT is not asymptotically optimal [93]. In other words, RRT cannot solve the optimal path planning problem. As a consequence, T-RRT is not asymptotically optimal either. Even though it yields high-quality paths when solving the cost-space path planning problem, T-RRT cannot solve the optimal path planning problem because it does not include any mechanism to improve its solution.

2.3.2 Path-Quality Criteria

The path-quality criterion cp : Πfree → R+ can be defined in several ways. As already mentioned, the first criterion to be utilized was path length. In the context of cost-space path planning, using path length would mean either 1) that the costs of configurations along the path are ignored or 2) that the configuration-cost function associate to every configuration the cost value 1. It is obviously more interesting to consider a path-quality criterion that takes a more complex configuration-cost function into account. We now present the various criteria that we consider in this thesis.

The most common criterion is the integral of the cost along a path. As a discrete approx-imation of the integral of the cost (IC) with constant step size δ = _n1 (where n is the number of subdivisions of the path), the cost of a path π can be defined as

cp(π) = length(π) n n X k=1 c π k n . (IC)

(25)

As an alternative, the mechanical work of a path can be defined as the sum of the positive cost variations along the path. This can be interpreted as summing the “forces” acting against the motion. It has been shown that the mechanical work can assess path quality better than the integral of the cost in many situations [83]. As a discrete approximation of the mechanical work (MW) with constant step size δ = 1

n, the cost of a path π can be defined as cp(π) = n X k=1 max 0 , c π k n − c π k − 1 n . (MW)

Additionally, we consider other, simpler cost criteria to evaluate path quality, such as the maximal cost along the path (maxC), and the average cost (avgC). As discrete approximations with n subdivisions of the path π, these costs are defined as follows:

cp(π) = max c π k n ; k = 0..n (maxC) cp(π) = 1 n n X k=0 c π k n . (avgC)

Which criterion is the most suited depends on the planning problem and on the character-istics of its expected optimal solution. Comparing quality criteria is out of the scope of this thesis. In our experiments, we use several of these criteria not to limit ourselves to a single criterion, which could bias the interpretation of the results. Finally, note that when using such quality criterion, as a slight abuse of language, we interchangeably utilize the expressions “high-quality path” and “low-cost path”.

2.3.3 The RRT* Algorithm

The RRT* algorithm was specifically proposed to solve the optimal path planning prob-lem [93]. Indeed, it has been shown that RRT* offers asymptotic-optimality guarantees. This path planner has been successfully applied to various robotic problems [87, 93, 94] (some even involving kinodynamic planning [91] or manipulation tasks [131]) and to pursuit-evasion games [92]. However, RRT* has been most often used to optimize path length (without any configuration-cost function involved), and has only rarely been applied to more sophisticated quality criteria. In these rare cases, it has been evaluated only on cost-space path-planning problems involving coarse-grained, discrete configuration-cost functions. Furthermore, even though RRT* performs very well in low-dimensional spaces, it appears to converge slowly toward the optimum in high-dimensional spaces [49, 79].

RRT* was developed as an extension to RRT. The two algorithms differ mainly in the way connections are created between nodes of the tree. In RRT*, instead of being linked to qnear, qnew is linked to the configuration (among its neighbors in C) maximizing the quality of the path in T between qinit and qnew. Furthermore, if, as a parent in T , qnew allows one of its neighbors in C to be connected to qinit via a higher-quality path than the one currently available, some rewiring is performed in T . By deciding how to create and remove edges of T based on the quality of the paths between qinit and every node in T , RRT* enables the quality of the solution extracted from T to increase with time.

The pseudo-code of RRT* is shown in Algorithm 7. The part that differs from RRT concerns what is done after a new configuration qnew is built. First, a new node is created in T to store qnew (line 7). Then, a search in T is performed to compute the set Qnear of configurations contained in a neighborhood of qnew of radius γ (log(n) / n)1 / d (line 9). This radius depends on the dimension d of C, on a constant γ derived from the volume of Cfree, and on the number n of nodes in T . This dependency on n ensures that the radius decreases as T grows. The next step consists of finding the configuration qpar in Qnear ∪ {qnear} to

(26)

2.3. Optimal Path Planning 25

Algorithm 7:RRT*

input : the optimal path planning problem (C, qinit, qgoal, c, cp) the dimension d of the C-space

the γ constant derived from the volume of Cfree [93] output: the tree T

1 T ← initTree(q_init)

3 q_rand ← sampleRandomConfiguration(C) 4 q_near← findNearestNeighbor(T , q_rand) 5 qnew← extend(qnear, qrand)

6 if qnew6= null then 7 addNewNode(T , q_new)

8 n ← numberOfNodes(T )

9 Qnear← findNodesInBall(T , qnew, γ (log(n) / n)1 / d)

10 q_par ← findParentMinimizingCostFromInit(q_new, q_near, Q_near, c_p) 11 addNewEdge(T , qpar, qnew)

12 foreachqn ∈ Qnear do

13 π ← pathInSpace(q_new, q_n)

14 if costFromInit(qnew) + cp(π) < costFromInit(qn) and isCollisionFree(π) then

15 removeEdge(T , parent(q_n), q_n)

16 addNewEdge(T , qnew, qn)

17 return T

which qnew should be connected (line 10): the parent of qnew is chosen as the configuration via which the path between qinit and qnew has maximal quality (i.e. minimal cost). This is done by computing, for all qn ∈ Qnear∪ {qnear}, the cost cp(πnT) + cp(πnC), where πTn is the path between qinit and qn in T , and πnC is the path between qn and qnew in C. Finally, since the addition of a new node in T potentially leads to the apparition of new paths having lower costs than those currently in T , some rewiring might be performed (lines 12–16). For each qn ∈ Qnear, if the cost of the path going from qinit to qn via qnew is lower than the cost of the current path between qinit and qn in T , qnew becomes the new parent of qn in T .

Theorem 4 (Probabilistic completeness of RRT*). The RRT* algorithm is probabilistically complete [93].

Let us assume that the γ constant involved in RRT* satisfies γ > 2 1 +1 d 1_d µ(Cfree) ζd 1_d , (2.1)

where d is the dimension of the C-space, ζdis the volume of the unit ball in the d-dimensional Euclidean space, and µ() is an operator measuring volumes. Under this assumption, RRT* has proven to be asymptotically optimal [93]. The lower bound on γ expressed in (4.1) is the minimal value allowing RRT* to be asymptotically optimal. Keeping in mind that increasing the value of γ raises the computational cost of an RRT* iteration (because of the increased number of neighbors to consider), this lower bound represents the optimal tradeoff between efficiency and asymptotic optimality.

Theorem 5 (Asymptotic optimality of RRT*). If the γ constant satisfies (4.1), the RRT* algorithm is asymptotically optimal [93].

(27)

2.4 Parallel Path Planning

2.4.1 General Parallel Approaches

The idea of improving path planning performance using parallel computation has been ex-plored for several decades. A survey of some early work suggests a classification scheme to review various path planning approaches and related parallel processing methods [74]. A more recent trend is to exploit the multi-core technology available on many of today’s PCs, which allows having multiple threads collaboratively solving a problem [40]. Another re-cent trend consists of using shared-memory models on many-core Graphics Processing Units (GPUs) [15, 17, 129, 130].

Among classical approaches, the embarrassingly parallel paradigm exploits the fact that some randomized algorithms, such as PRM, are what is termed “embarrassingly parallel” [6]. The massive inherent parallelism of the basic PRM algorithm allows reaching a significant speedup, even with simple parallelization strategies, especially on shared-memory architec-tures. In this approach, computation time is minimized by having several processes coopera-tively building the road-map.

Another simple approach is known as the OR parallel paradigm. It was first applied to theorem proving, before providing a parallel formulation for the Randomized Path Planner (RPP) [27]. Its principle is to have several processes running the same sequential randomized algorithm, each one trying to build its own solution. The first process to reach a solution reports it and broadcasts a termination message. The idea is to minimize computing time by finding a small-sized solution. Despite its simplicity, this paradigm has been successfully applied to other randomized algorithms [26].

More sophisticated approaches are the scheduler-processor scheme and the master-slave scheme developed to distribute the computation of the Sampling-based Roadmap of Trees (SRT) algorithm [134, 135]. In a first step, several trees (called milestones, and that can be RRTs or ESTs) are computed in parallel by all processes. In a second step, a scheduler evenly distributes the computation of edges linking these trees among the other processes [134]. As an improvement to this, several masters can cooperate to distribute the workload among their respective slave processes [135]. More generally, an approach based on growing several independent trees, such as the Rapidly exploring Random Forest of Trees (RRFT) [9, 56] or RRTLocTrees [149], can lead to a straightforward parallelization.

2.4.2 Parallel RRT

Only little work relates to parallelizing RRT [2, 25, 44, 142]. The first approach applied the simple OR parallel and embarrassingly parallel paradigms, and a combination of both [25]. To benefit from the simplicity of the shared-memory model, the embarrassingly parallel algorithm is run on a single Symmetrical Multi-Processor (SMP) node of a multi-nodes parallel computer. The only communication involved is a termination message that is broadcast when a solution is reached, and some coordination is required to avoid concurrent modifications of the tree. This scheme does not make use of the full computational power of the parallel platform, contrary to the OR parallel algorithm, which is run on all processors of all nodes. The same paradigms are also applied on a dual-core CPU in [2], where they are renamed OR and AND implementations. In the Open Motion Planning Library (OMPL), the AND paradigm is implemented via multi-threading, and thus for shared memory2_{. In [142], the OR paradigm is} used on shared memory. A more recent and very successful approach for parallelizing RRT and RRT* on shared-memory architectures is presented in [78]. It is primarily based on lock-free shared data structures that are updated using atomic compare-and-swap (CAS) operations,

2

(28)

2.4. Parallel Path Planning 27 and on a partition-based sampling (in other words, each thread is assigned a distinct region of the space).

In Chapter 5 of this thesis, we focus on the less studied issue of parallelizing RRT on distributed-memory architectures, using the Message Passing Interface (MPI). To the best of our knowledge, there has been only one attempt to develop a parallel version of RRT on distributed memory. In [44], the construction of the tree is distributed among several autonomous agents, using a message passing model. However, no explanation is given on how the computation is distributed, and how the tree is reconstructed from the parts built by the agents.

The objective of our work was to provide parallel versions of the basic (single-tree) RRT algorithm. This work was not about parallelizing subroutines of RRT, such as is done for collision detection in [15], nor about parallelizing specific variants of RRT, such as is done for the anytime RRT in [127]. In Chapter 5 we present three parallel versions of RRT based on classical parallelization schemes: OR parallel RRT, Distributed RRT and Manager-worker RRT [48, 50]. Since this work was published, two extensions of our Distributed RRT have been proposed [81].

(29)