# Sampling-based optimal kinodynamic planning with motion primitives

This paper proposes a novel sampling-based motion planner, which integrates in RRT* (Rapidly exploring Random Tree star) a database of pre-computed motion primitives to alleviate its computational load and allow for motion planning in a dynamic or partially known environment. The database is built by considering a set of initial and final state pairs in some grid space, and determining for each pair an optimal trajectory that is compatible with the system dynamics and constraints, while minimizing a cost. Nodes are progressively added to the tree of feasible trajectories in the RRT* by extracting at random a sample in the gridded state space and selecting the best obstacle-free motion primitive in the database that joins it to an existing node. The tree is rewired if some nodes can be reached from the new sampled state through an obstacle-free motion primitive with lower cost. The computationally more intensive part of motion planning is thus moved to the preliminary offline phase of the database construction at the price of some performance degradation due to gridding. Grid resolution can be tuned so as to compromise between (sub)optimality and size of the database. The planner is shown to be asymptotically optimal as the grid resolution goes to zero and the number of sampled states grows to infinity.

## Authors

• 1 publication
• 1 publication
• 1 publication
• 1 publication
• ### Dispersion-Minimizing Motion Primitives for Search-Based Motion Planning

Search-based planning with motion primitives is a powerful motion planni...
03/26/2021 ∙ by Laura Jarin-Lipschitz, et al. ∙ 0

• ### Exploiting collisions for sampling-based multicopter motion planning

Multicopters with collision-resilient designs can operate with trajector...
11/08/2020 ∙ by Jiaming Zha, et al. ∙ 0

• ### Towards Learning Efficient Maneuver Sets for Kinodynamic Motion Planning

Planning for systems with dynamics is challenging as often there is no l...
07/18/2019 ∙ by Aravind Sivaramakrishnan, et al. ∙ 0

• ### RRT2.0 for Fast and Optimal Kinodynamic Sampling-Based Motion Planning

We present RRT2.0: a simple yet efficient tree-based planner for asympto...
09/12/2019 ∙ by Michal Kleinbort, et al. ∙ 0

• ### RRT-CoLearn: towards kinodynamic planning without numerical trajectory optimization

Sampling-based kinodynamic planners, such as Rapidly-exploring Random Tr...
10/27/2017 ∙ by Wouter Wolfslag, et al. ∙ 0

• ### T^⋆-Lite: A Fast Time-Risk Optimal Motion Planning Algorithm for Multi-Speed Autonomous Vehicles

In this paper, we develop a new algorithm, called T^⋆-Lite, that enables...
08/29/2020 ∙ by James P. Wilson, et al. ∙ 0

• ### Computing a Minimal Set of t-Spanning Motion Primitives for Lattice Planners

In this paper we consider the problem of computing an optimal set of mot...
03/25/2019 ∙ by Alexander Botros, et al. ∙ 0

##### This week in AI

Get the week's most popular data science and artificial intelligence research sent straight to your inbox every Saturday.

## 1 Introduction

Motion planning is one of the fundamental problems in robotics, and consists of guiding the robot from an initial state to a final one along a collision-free path.

For many robots, focusing only on kinematics could result in collision-free paths that are impossible to be executed by the actual system. In particular, for systems that are differentially constrained, such a decoupled approach makes it difficult to turn a collision-free path into a feasible trajectory. In order to tackle this problem, Donald et al (1993) proposed the idea of kinodynamic planning, which combines the search for a collision-free path with the underlying dynamics of the system, so that the resulting trajectory would be feasible.

For most robotic applications, the solution to the planning problem should not only be feasible and collision-free, but also satisfy some properties such as, e.g., reaching the goal in minimum time, minimizing the energy consumption, and maximizing safety. These additional requirements have shifted the focus from simply designing collision-free and feasible trajectories to finding optimal ones.

This paper deals with optimal kinodynamic motion planning for systems with complex dynamics and subject to constraints.

### 1.1 Literature review

A significant amount of work in the robotics community has then been dedicated to the problem of kinodynamic planning so as to determine a trajectory that fulfills the differential constraints arising from the dynamics of the robot. Solving this problem is complex, in general, since it requires a search in the state space of the robot, which often implies a higher-dimensional search space compared to a pure kinematic planning.

Most of the motion planners can be classified under the two categories of

exact and sampling-based methods, LaValle (2006). The former looks for a solution in the continuous state space, while the latter samples this space redefining it as a graph where nodes are connected via edges representing local trajectories between sampled states.

Exact methods are said to be complete, since they terminate in finite time with a solution, if one exists, and return the nonexistence otherwise. However, with the exact approaches even the simplest problem is PSPACE hard (Reif, 1979). Exact methods require that the obstacles are represented explicitly in the state space, dramatically increasing the problem complexity. However, they can provide practical solutions for problems that are characterized by a low dimensional state space, or for which a low dimensional obstacle representation can be adopted. Obstacles introduce non convex constraints in the admissible state space, and make the problem of computing an optimal trajectory hard. In particular, most of the algorithms, that are gradient based, can only find a solution in the same homotopy class of the initial guess (LaValle, 2006). There has been a significant progress to address this issue and, in particular, the ideas of dividing the global optimal trajectory planning problem into simpler subproblems and of using numerical optimization to compute locally optimal trajectories have been explored in, e.g., (Park et al, 2015; Kuderer et al, 2014).

Sampling-based approaches emerged to handle systems with high dimensional state spaces, and they became the most popular approaches in the planning literature techniques, representing the practical way to tackle the problem. The basic idea is to sample states (nodes) in the continuous state space and connect these nodes with trajectories in the collision-free space, building a roadmap in the form of a graph or a tree of feasible trajectories. These algorithms avoid an explicit representation of obstacles by using a collision check module that allows to determine the feasibility of a tentative trajectory. They are not complete, but they satisfy the probabilistic completeness

property, i.e., they return a solution with a probability converging to one as the number of samples grows to infinity, if such a solution exists.

Probabilistic Road Map (PRM), introduced by Kavraki et al (1996), and Rapidly exploring Random Trees (RRT), introduced by LaValle and Kuffner Jr (2001), were the first popular sampling-based planners. PRM first creates a graph in the free configuration space by randomly sampling nodes and connecting them to the already existing ones in the graph using a local planner. The graph can then be used to answer multiple queries, where in each query a start node and a goal node are added to the graph and a path connecting the two nodes is looked for. RRT, on the other hand, incrementally builds a tree starting from a given node, returning a solution as soon as the tree reaches the goal region, hence providing a fast on-line implementation. In all the different formulations of sampling-based planners, a steering function is required to design a trajectory (edge in the tree terminology) connecting two nodes of the tree.

Considering the quality of the solution, an important progress has been made with the introduction of RRT (Rapidly exploring Random Tree star) and PRM (Probabilistic Road Map star), which have been proven to be asymptotically optimal, i.e., the probability of finding an optimal solution, if there exists one, converges to 1 as the tree cardinality grows to infinity, (Karaman and Frazzoli, 2011). The main idea of these algorithms is to ensure that each node is connected to the graph optimally, possibly rewiring the graph by testing connections with pre-existing nodes that are in a suitably defined neighborhood. The same strategy applies to kinodynamic planning (Karaman and Frazzoli, 2010) as well, with the additional difficulty that when optimality is required, implementing the steering function involves solving a two point boundary value problem (TPBVP), which is computationally challenging especially when dealing with complex dynamics, such as for non-holonomic robots, in presence of actuation constraints. In the context of kinodynamic planning RRT and PRM cannot be considered in the same way. In fact, PRM is limited to symmetric costs and to those systems for which the cost associated to a TPBVP is conserved when the boundary pairs are swapped. Note that, nonholonomic systems do not belong to this class.
Considering instead RRT, it must be noticed that for various dynamical systems, such as non-holonomic vehicles, the presence of kinodynamic constraints makes the constrained optimization problem that the steering function has to solve extremely complex.
To deal with this computational complexity, some effort has been made towards developing effective steering functions for different types of dynamical systems. Webb and van den Berg (2013) have obtained the closed-form analytical solution for a minimum time minimum energy optimization problem for systems with linear dynamics, and extended it to non-linear dynamics using first-order Taylor approximation. Other works (Perez et al, 2012; Goretkin et al, 2013) have focused on approximating the solution for systems with linearizable dynamics, by locally linearizing the system and applying linear quadratic regulation (LQR).
Some recent attempts have been made towards optimality without formulating and solving a TPBVP, as well. For example, algorithms like Stable Sparse RRT (SST) (Li et al, 2016) have proved asymptotic optimality given access only to a forward propagation model. The idea is to iteratively sample a set of controls and final times instead of explicitly solving the BVP. Similarly, a variant of RRT (hwan Jeon et al, 2011) uses a shooting method without a steering function to improve the solution by pruning branches from the tree. If a sampled node has a lower cost compared to another one that is close by and that shares the same parent, the pre-existing node is pruned from the tree and its branches are connected to the newly sampled node, or they are pruned completely if they are not collision-free. This approach generates feasible but inherently suboptimal solutions. Other works on extending RRT to handle kinodynamic constraints include limiting the volume in the state space from which nodes are selected by tailoring it to the considered dynamical system in order to improve computational effectiveness (Karaman and Frazzoli, 2013).
Nevertheless, solving the TPBVP for an arbitrary nonlinear system remains challenging and typically calls for numerical solvers. The algorithms that account for a nonlinear optimization tool, like for example ACADO toolkit (Houska et al, 2011), GPOPS-II (Patterson and Rao, 2014), etc., commonly use Sequential Quadratic Programming (SQP) (Boggs and Tolle, 1995) for solving the TPBVPs numerically, and embed it as a subroutine in the sampling based planning framework such as in RRT* (Stoneman and Lampariello, 2014) or in Batch-Informed-Trees star (BIT) (Gammell et al, 2014).

A different class of algorithms, aiming at optimality, is based on graph search and adopt a gridding approach. The main idea is to discretize the state space, building a grid, and compute a graph. The motion planning problem is then recast into finding the best sequence of motions by traversing this graph with an optimal search algorithm like A (Pearl, 1984). This graph is often represented by a state lattice, a set of states distributed in a regular pattern, where the connections between states are provided by feasible/optimal trajectories (Pivtoraiko et al, 2009). Likhachev and Ferguson (2009) improved the idea of state lattice by using a multi-resolution lattice such that the portion of the graph that is close to either the initial or the goal state has a higher resolution than the other parts. These approaches have been successfully applied to several robotic systems and found to be effective for dynamic environments (Likhachev and Ferguson, 2009; Dolgov et al, 2010). However, these algorithms are resolution optimal, such that the optimality is guaranteed up to the grid resolution. Furthermore, their computational effectiveness is highly related to the resolution: the finer is the grid, the higher the branching factor, and thus the computational time and the required memory to execute a graph traversal algorithm.

### 1.2 Contribution of the paper

The main contribution of this paper is proposing an algorithm, called , which extends RRT by introducing a database of pre-computed motion primitives in order to avoid the online solution of a constrained TPBVP for the edge computation. The database is composed of a set of trajectories, each one connecting an initial state to a final one in a suitably defined grid. By sampling in the gridded state space, the implementation of the steering function adopted for growing and rewiring the RRT tree reduces to the search of a motion primitive in a pre-computed Look Up Table (LUT).
The proposed approach is applicable to any dynamical system described by differential equations and subject to analytical constraints, for which edge design can be formulated as a TPBVP. Notably, when a model of the robot is not available, the database can be derived directly from experimental trajectories.

The main difference of the proposed approach, with respect to existing algorithms that use a database of pre-computed trajectories, e.g., search based approaches as in (Pivtoraiko et al, 2009), is that it leverages on a dynamic tree whose size depends only on the number of samples, but not on the number of motion primitives that affects the accuracy in the approximation of the robot kinematic and dynamic characteristics. As a consequence, memory consumption to store the tree and computation time to determine a solution on a given tree can be bounded selecting an appropriate maximum number of samples, without introducing undesired constraints in the robot action space.
Search based approaches, instead, have to strongly limit the action space, keeping the number of motion primitives low, as the branching factor of the graph, i.e., the number of edges generated expanding each node, depends on the size of the database.

The effectiveness of   has been validated in simulation, showing that the time required to build the tree is greatly reduced with the introduction of a LUT. This represents a promising result for online applications, especially in dynamic environments where the planner has to generate a new trajectory in response to changes in the obstacle-free state space.

An analysis of the probabilistic completeness and optimality properties of   is also provided. This involves a two-step procedure where we assess how close the proposed sample-based solution gets to the optimal one in the gridded state space as the number of samples grows to infinity, and how it gets close to the optimal solution in the continuous state space as the gridding gets finer and finer.

### 1.3 Paper structure

The paper is organized as follows. Section 2 introduces a formal description of the problem. In Section 3 the proposed algorithm is explained in detail. An analysis of its probabilistic completeness and optimality properties is presented in Section 4. A numerical validation of   is provided in Section 5. Finally, some concluding remarks are drawn in Section 6.

## 2 Problem Statement

In this work, dynamical systems with state vector

and control input , governed by

 ˙q(t)=f(q(t),u(t)) (1)

where is continuously differentiable as a function of both arguments, are considered. The control input is subject to actuation constraints, and the admissible control space is denoted as . The state is constrained in the set , and initialized with . Both and are assumed to be compact. An open subset of represents the goal region that the state has to reach.

A trajectory of system (1) is defined by the tuple , where is the duration of the trajectory, and and define the state and control input evolution along the time interval , satisfying the differential equation (1) for , the initial condition , and the final condition .

Obstacles are represented via an open subset of . The free space is then defined as , and the assumption is enforced.
A trajectory of system (1) is said to be collision free, if it avoids collisions with obstacles, i.e., , .
The set of collision free trajectories is denoted as .

An optimal kinodynamic motion planning problem can then be formalized as finding a feasible and collision free trajectory that is optimal according to a cost criterion that is expressed as

 J(z)=τ∫0g(q(t),u(t))dt

where is an instantaneous cost function. We assume that trajectories joining two different states have a non-zero cost. This is for instance the case in minimum time optimization where and the trajectory duration is one of the optimization variables of the problem.

###### Remark 1 (translation invariance property).

In the context of motion planning, system (1) represents the robot equations of motion and, consequently, its state vector includes the robot position with respect to a given absolute reference frame. In the following, a translation invariance property is supposed to hold. This means that, if obstacles are neglected and a pair of initial and final states and the associated optimal trajectory are considered, by shifting the origin of the coordinate system and rewriting all relevant quantities – including system dynamics (1), and initial and final states – in the new coordinate system and applying input , the optimal robot trajectory is obtained, which is rewritten in the new coordinates.

## 3 Rrt⋆ With Motion Primitives

The approach here proposed is based on previous works on search-based (Pivtoraiko et al, 2009; Likhachev and Ferguson, 2009), and sampling-based (Karaman and Frazzoli, 2011, 2010) methods, and combines them in a novel way.
In particular, it relies on a uniform discretization of the state space, and on the computation of a finite set of motion primitives by solving a constrained optimization problem with boundary conditions on the grid points of a smaller (uniform) grid. The motion primitives are then embedded in the RRT algorithm, where they are used to connect the randomly generated nodes to the tree, thus eliminating the need of solving online challenging and time consuming TPBVPs.

### 3.1 Database of Motion Primitives

The database of motion primitives is built by gridding the continuous state space in order to obtain a finite set of boundary conditions (initial and final states), and by solving offline a constrained boundary value optimization problem for each pair. The resulting set of optimal trajectories is then used repeatedly online, implementing a procedure that, when an edge connecting two nodes is requested by the planner, simply picks up from the database a suitable trajectory.

Given a state tuple , , including the robot position , motion primitives are computed for each pair of initial and final states with . Given a boundary value pair , a motion primitive is computed by solving the following optimization problem

 mininimizeu(⋅),τ τ∫0g(q(t),u(t))dt (2) subject to ˙q(t)=f(q(t),u(t)) u(t)∈U,t∈[0,τ] q(t)∈Qfree,t∈[0,τ] q(0)=¯q0,q(τ)=¯qf

Finally, the database is generated by considering distinct pairs of initial and final states , , and computing the corresponding set of motion primitives with the associated costs .

Note that, thanks to the translation invariance property introduced in Remark 1, the size of the database can be kept small while covering a wide range of the space where the robot is moving. Indeed, one can, without loss of generality, set the initial position to when building the database, and recover the optimal trajectory for an arbitrary initial position by simply centering the coordinate system in .

###### Example 1.

Consider, as an example, a planning problem for a unicycle robot characterised by a 3D search space , including the position and the orientation , and by a 2D actuation space , constituted by the linear velocity and the angular velocity .
Motion primitives are computed solving the following TPBVP

 mininimizev(⋅),ω(⋅),τ τ∫0(1+0.5v(t)2+0.5ω(t)2)dt subject to ˙x(t)=v(t)cos(θ(t)) ˙y(t)=v(t)sin(θ(t)) ˙θ(t)=ω(t) v(t)∈[0,2],t∈[0,τ] ω(t)∈[−2,2],t∈[0,τ] x(0)=¯x0,y(0)=¯y0,θ(0)=¯θ0 x(τ)=¯xf,y(τ)=¯yf,θ(τ)=¯θf

for different initial and final poses.
Figure 1 shows a subset of these motion primitives, characterized by trajectories starting from .
As can be seen in Figure 1, with the dynamical system and cost function considered in this example, motion primitives, corresponding to boundary conditions that are symmetric with respect to the -axis, are symmetric. A further analysis reveals that the same property holds for the -axis as well, and that symmetric primitives are characterized by the same cost.

###### Remark 2.

When for the considered dynamical system and cost criterion stronger invariance properties hold, like the axis symmetry in Example 1, the size of the database can be further reduced by storing only a few “reference trajectories”, and generating all the others using the invariance transformation.
Figure 2 shows an example, referred to the TPBVP considered in Example 1, where the trajectories represented by the pairs , , are characterised by the same cost and can be easily mapped to a “reference trajectory” corresponding to the boundary value pair . In this case, the size of the database can be further reduced storing only the “reference trajectory”.

### 3.2 Search Space design

In order to take advantage of the pre-computed database of motion primitives in sampling-based planning, the search space of the planner has to be defined appropriately so that every time the planner needs to connect two nodes, the corresponding optimal trajectory can be found in the database. To guarantee that this is indeed the case, the search space of the planner is uniformly gridded as the region where motion primitives are built. The translation invariance property111This approach can be easily extended in case stronger invariance properties hold. can then be exploited, as any optimal trajectory which connects a pair of initial and final states (in the discretized search space) can be computed by first shifting the initial and final states so that the initial robot position corresponds to the zero position, then picking a suitable motion primitive in the database, and finally shifting the motion primitive so as to get back to the original reference coordinate system. Translation invariance, jointly with uniform gridding, allow a reduced number of motion primitives to cover the entire (discretized) search space. Note that, as the resolution of the database and the uniform grid size of the search space are coupled, we often use these two terms interchangeably.

Determining optimal state space discretization depends on the specific application, and is out of the scope of this work. We shall assume here that the state space grid should include the initial state , at least a grid point in the goal region, and few points in the free space . Moreover, in order to find a solution that reaches the goal region, the algorithm should be able to search within all homotopy classes that are feasible given the robot footprint. In other words, one should be able to represent in the grid space all sets of trajectories in the continuous state space that can be obtained applying a smooth transformation and lead to the goal region. Missing a homotopy class could highly deteriorate performance in terms of achieved cost.
In Section 5 some numerical examples are provided, in which different grids are used for solving the same case study and results are compared.

### 3.3 Motion Planning

This section introduces   (Algorithm 1), the proposed variant of RRT integrating the database of motion primitives for the computation of a collision-free optimal trajectory (cf. Section 2).

As in RRT,   is based on the construction of a random tree where is the set of nodes, and is the set of edges. Nodes are states and edges are optimal trajectories, each one connecting a pair of origin and destination nodes and solving the TPBVP in (2).
The tree is expanded for a maximum number of iterations , defined by the user, starting from , where is the initial state, and , as described in Algorithm 1.
Every node is connected to via a single sequence of intermediate nodes , , , and associated edges , , with .
One can then associate to this sequence a cost given by

 C(→qn)=n−1∑i=0C(ei)

where denotes the cost associated with edge and computed as in (2).

Tree growing is based on four main steps – random sampling, finding near nodes, extending the tree, and rewiring – that are described in the following.

#### Random sampling

A random state is sampled from the free state space

according to a uniform distribution by

. Unlike the original RRT algorithm, however, the node is not sampled from the continuous state space, but from its discretization according to a uniform grid. For this reason, there is also a non zero probability that the same state is sampled again in the next iterations of the algorithm.

#### Near nodes (Algorithm 2)

In RRT a random state can be connected only to a node that is within the set of its near nodes.
For Euclidean cost metrics, the set of near nodes is defined as a -dimensional ball centered at of radius

 γball=γRRT⋆(log(n)/n)1/d

where is the tree cardinality at the current iteration of the algorithm and is a suitable constant selected as

 γRRT⋆>2(1+1/d)1/d(μ(Qfree)/ζd)1/d

and denoting the volume of the free configuration space and of the unit ball, respectively, in a -dimensional Euclidean space.
For non-Euclidean cost metrics, the distance between two states is represented by the optimal cost of the trajectory that connects them, and near nodes are selected from a set of reachable states, , defined as the set of states that can be reached from or that can reach with a cost that satisfies some threshold value. More specifically,

 Qreach={q∈Q:C(eqrand,q)≤l(n)∨C(eq,qrand)≤l(n)} (3)

where denotes the edge from to , and is a cost threshold that decreases over the iterations of the algorithm as such that a ball of volume is contained within , where and are suitable constants (see Karaman and Frazzoli, 2010, for further details).

In , however, the set has to be further constrained to ensure that there is a pre-computed trajectory in the database for each connection in the set of near nodes, that is thus redefined as follows

 Qnear=Qreach∩BoundingBox(πrand)

where denotes the box of grid points in the state space adopted for the database construction, with the origin of the reference coordinate system shifted from to , which is the robot position associated to state and obtained by using .
If occurs to be an empty set, the algorithm continues to the next iteration selecting a new .

#### Extend (Algorithm 3)

The tree is extended to include by selecting the node such that the edge connects with a minimum cost collision free trajectory.
is determined as follows

 qbest=argminq∈QfeasibleC(→q)+C(eq,qrand)

where is the set of nodes that belong to and such that the trajectory connecting to is collision free, i.e.,

 Qfeasible={q∈Qnear|CollisionFree(eq,qrand)=1}

where is a function that returns 1 for an edge that is collision free, 0 otherwise.
The selection of an edge from the database, connecting to , is a peculiarity of , and is performed by the function as follows:

1. a translation is applied to the pair of initial and final states (Figure 3(a)), obtaining the normalized pair , such that the resulting has the position corresponding to the null vector (Figure 3(b));

2. a query is executed on the database to look for the trajectory and the cost ;

3. the inverse of the previous translation is applied in order to recover the trajectory connecting the actual pair of boundary values , determining the edge (Figure 3(c)).

At the end of this procedure, if is not already in the tree, then it is added to the tree together with the minimum cost edge, i.e., is replaced by and by (see steps 7 and 8 in Algorithm 1). If is already in the tree and if the computed is different from the current parent node, , of , given by , then the previous edge, , is replaced by the new edge, (see step 1 in Algorithm 1).

#### Rewiring (Algorithm 4)

In order to ensure that all node pairs are connected by an optimal sequence of edges, every time a new node is added to the tree, a check is performed to verify if an already existing node can be reached from this newly added node with a smaller cost.
Therefore, if is collision free, and the following conditions hold

 C(eqrand,q)≤l(n) C(→qrand)+C(eqrand,q)≤C(→q)

the tree is rewired, i.e.,

 ET←{ET∖eprev}∪{eqrand,q}

where is the previous edge connecting the node to the tree.

#### Termination and best sequence selection

After the maximum number of iterations is reached the procedure to build the tree terminates.
The best trajectory is selected as the node sequence reaching the goal region with the minimum cumulative cost .
Note that, using a discretized search space limits the number of nodes that can be sampled, once all of them have been sampled the tree cardinality does not increase any more, but the algorithm can still continue updating the edges to ensure that each node is connected with the best possible parent node.

## 4 Completeness and Optimality Analysis

In this section, probabilistic completeness of the proposed planning algorithm and optimality of the solution are discussed. Furthermore, some results to assess how close the solution obtained using a discretized state space and motion primitives is to the optimal trajectory computed considering a continuous state space are provided.

Let define the set of grid points that represent the discretized state space222In this section, a superscript is used to denote all variables that are associated with the grid state space, so as to distinguish them from their continuous state space counterpart. and similarly represents the free discrete state space. Assuming that the discretization step size is chosen properly, then, the collection of all grid points that can be reached from by concatenating a sequence of motion primitives in is a non empty set. We shall denote this set as and its cardinality as . Note that the end points of the concatenated motion primitives are grid points in and hence they belong to .

Let be a graph where the set of nodes is given by defined before and the set of edges is the collection of all the (possibly translated) motion primitives iteratively built as follows: starting from consider all the (translated) motion primitives that lie in and connect to all possible grid points in , and, then, continue with the same strategy for all of the newly reached grid points iteratively until it is not possible to further expand the graph.

Finally, denotes the set of those grid points of that belong to . The motion planning problem using the grid representation admits a solution if is not empty since this means that there exists a way of reaching a state in starting from with the available motion primitives. In the following derivations we assume that .

Similarly to RRT,   generates a tree based on the random samples extracted from . However, unlike RRT, the nodes and edges added to the tree belong respectively to and , so that the obtained tree is a sub-graph of , i.e., . The subscript is used to denote the generated tree and the cost of the lowest cost trajectory represented in that tree after -th iterations, i.e., and , respectively.

Since , then, there exists at least an optimal branch of composed of the ordered sequence of nodes

 S⋆:={q⋆0,q⋆1,…,q⋆k},

which represents a resolution optimal -trajectory, a minimum cost trajectory that starts at the initial state and ends in the goal region, i.e., , such that and , for any . Let denote the cost of this optimal trajectory, i.e., , which is named resolution optimal -cost.

The goal of   can then be reformulated as that of generating a tree that contains an optimal branch to reach that is represented in . Note that, one could in principle build and apply an exhaustive search on it. However, this can still be an issue due to the combinatorial nature of the problem, in particular due to the branching caused by the dimensionality of the state space and the number of nodes contained in the graph.

In this section the quality of the solution obtained by   is analyzed by addressing the following questions:

1. resolution optimality: if there exists a resolution optimal -trajectory in , then, is it possible to obtain such a trajectory?

2. asymptotic optimality: how close is the resolution optimal -cost to the cost of the optimal trajectory, as the grid resolution increases and the grid converges to the continuous state space?

###### Theorem 4.1.

As the number of iterations goes to infinity the cost of the trajectory returned by   converges to the resolution optimal -cost with a probability equal to 1, i.e.,

 P({limi→∞ci=c⋆Δ})=1.

Moreover, the expected number of iterations required to converge to is upper bounded by , where is the length of an optimal branch of .

###### Proof.

returns the resolution optimal solution if it discovers an optimal branch, , defined on . Assuming that the constant defining the cost threshold for selecting nearby nodes, , is selected large enough (e.g. can be chosen so that defined in (3) contains the   for ), as soon as is a branch in the tree, , then the algorithm adds the edge to the tree in one of the following two ways:

1. any time is sampled it is connected to by the EXTEND procedure. In fact, as is part of the optimal sequence, there is no better way of reaching other than .

2. if is sampled before and connected to some node , such that and , the edge is selected any time is sampled, thanks to the REWIRE procedure.

This property allows to model the process of determining the sequence as an

absorbing Markov chain

initialized at with as absorbing state and all intermediate , that are transient states. There is a positive probability of advancing in the sequence, i.e., moving from to , and a probability of staying at the same state. Considering that at each iteration a new grid point is sampled from independently and according to a uniform distribution, each one has a probability of being extracted. Therefore, there is a probability of advancing in the Markov chain and, since all states are transient states apart from which is the absorbing state, the probability that the process is absorbed by tends to 1 as tends to infinity (Bertsekas and Tsitsiklis, 2002). Moreover, there is a finite number of expected iterations, , before the process is absorbed, i.e., before   returns the resolution optimal solution. ∎

Clearly, the number of expected iterations increases with the depth of the solution, , and the number of states represented in the grid. However, one advantage of , as RRT is the possibility of obtaining a solution rapidly and possibly improving its quality within the allowed computing time.

###### Corollary 1.

is probabilistically resolution complete, as the number of iterations goes to infinity the algorithm will return a solution to the motion planning problem, if there exists one in , with a probability 1.

The remaining of this section deals with the relation between the resolution optimal -trajectory returned by   as the number of iterations grows to infinity and the truly optimal trajectory in the continuous state space. To this purpose, we shall focus with the case when there are no actuation constraints and enforce the following assumptions regarding the properties of the dynamical system (1) and the existence of a solution.

###### Assumption 1.

The following properties hold for the dynamical system in (1)

• the system is small-time locally attainable (STLA)333A system is STLA from a state if the reachable set of states from in time , contains a d-dimensional subset of , where denotes the set of neighborhood states in terms of Euclidean distance, (Choset, 2005).;

• function , representing the system dynamics, is Lipschitz continuous with Lipschitz constant ;

• function , assigning a cost to an edge, satisfies the following Lipschitz-like continuity condition with Lipschitz constant :

 ∣∣C(eq0,q1)−C(e~q0,~q1)∣∣≤Kc∥∥∥∣∣∣q0q1∣∣∣−∣∣∣~q0~q1∣∣∣∥∥∥

for each pair of edges and .

Derivations in the rest of this section apply straightforwardly to state spaces that are Euclidean, and can be generalized to state spaces that are manifolds if the following assumption holds.

###### Assumption 2.

The state space manifold of system (1) with state variables is a subspace of the -dimensional Euclidean space, , therefore can be locally treated as .

With a slight abuse of the previously introduced notation, in the rest of this section we use the term “trajectory” for the state space component of the tuple defined in Section 2. In order to compare the trajectory returned by   and the optimal trajectory in the continuous state space, firstly, trajectories whose points are all away from obstacles by a certain distance are considered. For this reason, the definition of obstacle clearance of a trajectory, i.e., the minimum distance between obstacles and points belonging to the trajectory, has to be introduced.

###### Definition 1 (ϵ-obstacle clearance).

Given a trajectory , , if the ball of radius and centred at is strictly inside , for any , then, the -obstacle clearance property holds for .

###### Definition 2 (ϵ-free space).

Let be a trajectory which has -obstacle clearance, the -free space along is given by

 Qϵσ:=⋃t∈[0,T]Bϵ(σ(t))

Then, a set of trajectories that are called to be -similar to can be introduced.

###### Definition 3 (ϵ-similarity).

Any trajectory is said to be -similar to if it lies in the free-space, i.e., if

 ~σ(t)∈Qϵσ,t∈[0,~T].

Figure 4 shows an example of a trajectory that is -similar to another one.

Note that having an -free space along a trajectory is not sufficient to guarantee the existence of -similar trajectories (-dynamic clearance), as this existence depends also on the properties of the dynamical system (1). The following definition relates the -similarity with the -free space through the system dynamics.

###### Definition 4 (ϵ-dynamic clearance).

Given a trajectory which has obstacle clearance, if for any pair of time instants , such that , there exists a set of states inside a ball of radius , with , centered at that are reachable from according to dynamics (1) without leaving the -free space around , then has -dynamic clearance.

Let denote all the trajectories that solve the motion planning problem and have at least -obstacle and dynamic clearance. Let denote the minimum cost over all , which corresponds to the -optimal trajectory, . Note that, as tends to zero converges to the truly optimal trajectory in the continuous state space. Due to the discretized nature of   it is not possible to converge to the optimal trajectory in the continuous state space, however in the following it is proven that the graph, , contains an -similar trajectory, , to the optimal trajectory with a particular -clearance. Consequently, this result will be used to show that as the resolution of the grid increases, and as converges to zero, the resolution optimal -cost will converge to the truly optimal cost, i.e., .

###### Theorem 4.2.

Let be smaller than half of the shortest side of the bounding box and such that the system (1) admits an -optimal trajectory for any , then,

• for a sufficiently fine gridding, contains an -similar trajectory to , ,

• as the grid resolution increases, the resolution optimal -cost converges to .

###### Proof.

Let us fix , with , and sample a set of states, , along the -optimal trajectory, , starting from the initial state and ending in the final state, in such a way that a ball of radius centered at sample would be contained in the BoundingBox centered at the preceding sample and touching its boundary for (Figure 6), till the final state is included within BoundingBox.

As has -dynamic clearance, it is true that for any sample along the trajectory there exists a set of states within a ball of radius centered at that is reachable without leaving , which is the -free space along . From Assumption 1, in particular from the Lipschitz continuity of the system dynamics, it follows that a sequence of non-overlapping balls, , centered at the samples along and characterized by radius where

 δ=αϵ2Kf (4)

can be determined such that any state within can reach any state in without leaving444It is assumed that . (see Khalil, 1996; Karaman and Frazzoli, 2010). Assuming that the discretization is fine enough so that none of the