Optimal Sampling-Based Motion Planning under Differential Constraints: the Driftless Case  Edward Schmerling, Lucas Janson, Marco Pavone  Abstract — Motion planning under differential constraints is a classic problem in robotics. To date, the state of the art is represented by sampling-based techniques, with the Rapidly- exploring Random Tree algorithm as a leading example. Yet, the problem is still open in many aspects, including guarantees on the quality of the obtained solution. In this paper we provide a thorough theoretical framework to assess optimality guarantees of sampling-based algorithms for planning under differential constraints. We exploit this framework to design and analyze two novel sampling-based algorithms that are guaranteed to converge, as the number of samples increases, to an optimal solution (namely, the Differential Probabilistic RoadMap al- gorithm and the Differential Fast Marching Tree algorithm). Our focus is on driftless control-affine dynamical models, which accurately model a large class of robotic systems. In this paper we use the notion of convergence in probability (as opposed to convergence almost surely): the extra mathematical flexibility of this approach yields convergence rate bounds — a first in the field of optimal sampling-based motion planning under differential constraints. Numerical experiments corroborating our theoretical results are presented and discussed.  I. I NTRODUCTION  Motion planning is a fundamental problem in robotics. It involves the computation of a sequence of actions that drives a robot from an initial condition to a terminal condition while   avoiding   obstacles,   respecting   kinematic/dynamical constraints, and possibly optimizing an objective function [1]. The basic problem, where a robot does not have any constraints on its motion and only an obstacle-free solution is required, is well-understood and solved for a large number of practical scenarios [2]. On the other hand, robots do usually have stringent kinematic/dynamical (in short, differential) constraints on their motion, which in most settings need to be properly taken into account. There are two main approaches [2]: (i) a decoupling approach, in which the problem is decomposed in steps of computing a geometric collision-free path (neglecting the differential constraints), smoothing the path to satisfy the motion constraints, and finally reparam- eterizing the trajectory so that the robot can execute it, or (ii) a direct approach, in which the differentially-constrained motion planning problem (henceforth referred to as DMP problem) is solved in one shot. The first approach, while fairly common in practice, has several drawbacks, including  Edward   Schmerling   is   with   the   Institute   for   Computational   & Mathematical   Engineering,   Stanford   University,   Stanford,   CA   94305,  schmrlng@stanford.edu . Lucas Janson is with the Department of Statistics, Stanford University, Stanford, CA 94305,   ljanson@stanford.edu . Marco Pavone is with the Department of Aeronautics and Astronautics, Stanford University, Stanford, CA 94305,   pavone@stanford.edu . This work was supported by an Early Career Faculty grant from NASA’s Space Technology Research Grants Program (Grant NNX12AQ43G).  the computation of very inefficient trajectories, failure in finding a trajectory due to the decoupling scheme itself, and inflated information requirements [2]. This motivates a quest for efficient algorithms that   directly   solve the DMP problem. However, directly finding a feasible, let alone optimal, solution to the DMP problem is difficult (note that the basic version without differential constraints is already PSPACE- hard [3, 1], which implies NP-hard). Early work on this topic dates back to more than two decades ago [4], but the problem, especially when optimality is taken into account, is still open in many aspects [5, 2], including algorithms with practical convergence rates, guarantees on the quality of the obtained solution, and class of dynamical systems that can be addressed. To date, the state of the art is represented by sampling-based techniques, where an   explicit   construction of the configuration space is avoided and the configuration space is probabilistically “probed” with a sampling scheme. Arguably, the most successful algorithm for DMP is the sampling-based rapidly-exploring tree algorithm (RRT) [6], which incrementally builds a tree of trajectories by randomly sampling points in the configuration space. Lately, several variations of the RRT algorithm, referred to as RRT ∗ , stem- ming from [7] and its kinodynamic extension [8], have been considered to ensure that the cost of the computed trajectory converges to the optimal cost as the number of sampled points goes to infinity [8, 9, 10, 11, 12]. These works, while providing strong experimental validation, only provide proof sketches that do not fully address many of the complications that arise in extending asymptotic optimality arguments from the geometric case to differentially constrained paths. For example, rewiring the RRT tree within a local volume con- taining (in expectation) a log fraction of previous samples is not sufficient in itself to claim optimality, as in [10, 11]. Ad- ditional assumptions on trajectory approximability must be stated and verified for the differential constraints in question. Such requirements are discussed in [8], but it is not clear how assuming the existence of forward-reachable trajectory ap- proximations is sufficient for a “ball-to-ball” proof technique that requires backward approximations as well. A different approach to asymptotically optimal planning has recently been proposed by STABLE SPARSE RRT which achieves optimality through random control propagation instead of connecting existing samples using a steering subroutine [13]. This paper, like the RRT ∗   variations, follows a steering approach, although it may be considered less general, as it is our view that leveraging as much knowledge as possible of the differential constraints while planning is necessary in order to have a hope of planning in real-time.  arXiv:1403.2483v2 [cs.RO] 2 Mar 2015
Statement of Contributions : The objective of this paper is to provide a theoretical framework to study optimality guar- antees of sampling-based algorithms for the DMP problem, and to exploit this framework to design efficient sampling- based algorithms that are guaranteed to asymptotically con- verge to an optimal solution. The focus of this paper is on driftless control-affine (DCA) dynamical systems of the form   ̇ x ( t ) =  m ∑  i =1  g i ( x ( t )) u i ( t ) ,  where the available motions of trajectories   x ( t )   are linear combinations given by input control functions   u i ( t )   and their corresponding actions at each point in space   g i ( x ) . Our work is written from the perspective of optimizing trajectory arc length, but applies also to cost metrics satisfying similar properties, which we discuss. This model is often the result of nonholonomic constraints that the kinematic variables of the system must satisfy [14]. A large class of robotic systems can be modeled as DCA, including mobile robots with wheels that roll without slipping [1], multi-fingered robotic hands [15], rigid bodies with zero angular momentum un- dergoing re-orientation [16], and systems with nonholonomic actuators [14]. The DCA model, however, rules out the pres- ence of dynamics with drift, which in many important cases (e.g., spacecraft control) can not be neglected. Specifically, the contribution of this paper is threefold. First, we show that  any   trajectory of a DCA system may be “traced” arbitrarily well by connecting randomly distributed points from a suf- ficiently large sample set covering the configuration space. We will refer to this property as   probabilistic exhaustivity , as opposed to probabilistic completeness [1], where the requirement is that   at least   one trajectory is traced with a suf- ficiently large sample set. Probabilistic exhaustivity is a key tool in proving asymptotic optimality, and is of independent interest. Second, we introduce two novel sampling-based algorithms for the solution to the DMP with DCA systems, namely the Differential Probabilistic RoadMap algorithm (DPRM ∗ ) and the Differential Fast Marching Trees algorithm (DFMT ∗ ). Third, by leveraging the property of probabilistic exhaustivity for DCA systems, we rigorously show that both DPRM ∗   and DFMT ∗   are asymptotically optimal. We note that in this paper we use the notion of convergence in probability (as opposed to convergence almost surely, as in, e.g., [11]): the extra mathematical flexibility of this approach yields convergence rate bounds — a first in the field of optimal sampling-based motion planning under differential constraints. Our approach is inspired by [11] and [17].  Organization :   This   paper   is   structured   as   follows.   In Section II we provide a review of some key results in differential geometry that will be extensively used in the paper. In Section III we formally define the problem we wish to solve, while in Section IV we prove the aforementioned probabilistic exhaustivity property for DCA systems. In Sec- tion V we present the DPRM ∗   and DFMT ∗   algorithms, and in Section VI we prove their asymptotic optimality (together with a convergence rate characterization). In Section VII we provide implementation details for the proposed algorithms, and we study them via numerical experiments. Finally, in Section VIII, we draw some conclusions and we discuss directions for future work. II. B ACKGROUND   M ATERIAL  In this section we provide some definitions and a brief review of key results in differential geometry, on which we will rely extensively later in the paper. Let   M ⊂   R n   be the manifold defining a configuration space. Within this space let us consider driftless control-affine (DCA) dynamical systems of the form   ̇ x ( t ) =  m ∑  i =1  g i ( x ( t )) u i ( t ) ,   x   ∈ M , u   ∈   U,   (1) where the available motions of trajectories   x ( t )   are linear combinations given by input control functions   u i ( t )   and their corresponding actions at each point in space   g i ( x ) . We shall assume in this paper that   g 1 , . . . , g m   are smooth vector fields on   M , and that the control set   U   ⊂   R m  is closed and bounded. We also assume   U   is symmetric about the origin so that the system is time-reversible and  0   is in the interior of the convex hull of   U   . This last condition ensures that the local possibilities for motion at each point appear as a linear space spanned by the   g i , a fact essential to the forthcoming controllability discussion [18]. We denote the driftless control-affine system of equation (1) as   Σ = ( M , g, U   ) . A function   x   : [0 , T   ]   → M   is called a   dynamically feasible   trajectory, alternatively path, if there exists a corresponding control function   u ( t )   with which it satisfies   Σ . All trajectories discussed in this paper are dynamically feasible unless otherwise noted.  A. Arc Length and Sub-Riemannian Distance  The   arc length   of a path   x ( t )   is defined as  ` ( x ) :=  ∫   T  0  ‖    ̇ x ( t ) ‖ dt,  where   ‖    ̇ x ( t ) ‖   =   ‖    ̇ x ( t ) ‖ 2   =   √ 〈    ̇ x ( t ) ,    ̇ x ( t ) 〉   is computed using the standard Euclidean inner product on the tangent spaces of   M . The arc length function induces a   sub-Riemannian distance   d   on   M , defined for   x 1 , x 2   ∈ M   as   d ( x 1 , x 2 ) := inf x   ` ( x ) , where the infimum is taken over dynamically feasible trajectories   x ( t )   connecting   x 1   and   x 2 . Note that for driftless control-affine systems, time-reversibility implies that   d   is symmetric and indeed a metric. The   sub-Riemannian ball   may be defined in analogy to the standard Euclidean ball (i.e.,   B e ( x, ε ) =   { y   ∈ M   :   ‖ x   −   y ‖ ≤   ε } ) according to  B ( x, ε ) :=   { y   ∈ M   :   d ( x, y )   ≤   ε } . Note that by definition  B ( x, ε )   ⊂   B e ( x, ε ) .  B. Controllability and Reachable Sets  We   now   present   a   series   of   results   regarding   system controllability following the discussion in [19] and [20]. As noted above, the vector fields   g 1 , . . . , g m   characterizing the system   Σ   represent a set of possible motions for a trajectory within   M . More precisely at each point   p   ∈ M   the vectors
g 1 ( p ) , . . . , g m ( p )   span a linear subspace of local directions within the tangent space   T p M   . For a vector field   Y   on   M  let   Φ Y,t   :   M → M   denote its local flow, the function that maps an initial state to the state obtained by following the vector field   Y   for time   t . That is,   Φ Y,t ( p ) :=   y ( t )   where  y   : [0 , t ]   → M   is a solution to the initial value problem   ̇ y ( τ   ) =   Y   ( y ( τ   )) ,   y (0) =   p . Commutators of flows, akin to control switching in (1), allow for local motions transverse to the   g i   to be achieved while satisfying the differential constraints. Given two vector fields   Y   ,   Z   on   M   and a starting point   p   ∈ M , we have the approximation for small   t :  p   +   t 2 [ Y, Z ]( p ) = Φ Y,t   ◦   Φ Z,t   ◦   Φ Y, − t   ◦   Φ Z, − t ( p ) +   O ( t 3 ) ,  where the Lie bracket   [ Y, Z ]   is a third vector field which may be computed with respect to a coordinate system as  [ Y, Z ] =   J Z   X   −   J X   Z , where   J Y   and   J Z   are the Jacobian matrices of   Y   and   Z   respectively. Computing Lie brackets allows us to characterize all directions that are possible from each   p , in addition to those given by   g 1 ( p ) , . . . , g m ( p ) . Let   L   =   L ( g 1 , . . . , g m )   be the distribution, equivalently the set of local vector subspaces, generated by the vector fields   g 1 , . . . , g m . We define recursively   L 1   =   L ,  ] L k +1   =   L k   + [ L 1 ,   L k ]  where   [ L 1 ,   L k ] = Span { [ Y, Z ] :   Y   ∈ L 1 , Z   ∈ L k } . Then   L k  is the distribution generated by the iterated Lie brackets of the   g i   with   k   terms or fewer. The Lie hull of   L   is Lie ( L ) :=  ∪ k ≥ 1 L k . Let   L k ( x )   denote the vector space corresponding to   x   ∈ M   in   L k . The vector fields   g 1 , . . . , g m   are said to be   bracket gener- ating   if Lie ( L )( x ) =   T x M   for all   x   ∈ M . This requirement is also referred to as Chow’s condition, or the linear algebra rank condition, and means that arbitrary local motion may be achieved by composing motions along the control directions  g i . In fact, provided that the   g i   are bracket generating, any two points   x 1 , x 2   ∈ M   may be connected by a trajectory satisfying   Σ , that is   d ( x 1 , x 2 )   <   ∞ . The remainder of this section develops tighter bounds on   d ( x 1 , x 2 )   that will be used in our asymptotic optimality proofs of planning algorithms. Chow’s condition implies that for all   x   ∈ M , there exists a smallest integer   s   =   s ( x )   such that   L s ( x )   =   T x M . Indeed, we have   L 1 ( x )   ⊂   L 2 ( x )   ⊂ · · · ⊂   L s ( x ) ( x ) =   T x M .   Set  n k ( x ) = dim   L k ( x ) . The integer list   ( n 1 ( x ) , . . . , n s ( x ) ( x ))  is called the growth vector of   L   at   x . A point   x   is called a   regular point   if there exists an open neighborhood of   M  around   x   such that the growth vector is constant; otherwise  x   is said to be a   singular point . We now further assume that every   x   ∈ M   is a regular point, so that the growth vector   ( n 1 , . . . , n s   =   n )   is constant over the whole configuration manifold. Fix a base point   x 0   ∈ M . Using the bracket-generating assumption we select a lo- cal orthonormal frame for   T x 0   M   of vector fields   Y 1 , . . . , Y n  as follows: the set   { Y 1   =   g 1 , . . . , Y n 1   =   g m }   spans   L  near   x 0 ;   { Y 1 , . . . , Y n 2   }   spans   L 2   near   x 0 ;   { Y 1 , . . . , Y n 3   }  spans   L 3   near   x 0 ; and so on. Define the weights   w i   =   k  if   Y i ( x 0 )   ∈   L k ( x 0 )   and   Y i ( x 0 )   / ∈   L k +1 ( x 0 ) . Applying a procedure developed in [19], the coordinate system   y i  corresponding to this local frame may be transformed into a  privileged   coordinate system   z i   by a polynomial change of coordinates of the form  z 1   =   y 1 , z 2   =   y 2   +   poly 2 ( y 1 ) , z 3   =   y 3   +   poly 3 ( y 1 , y 2 ) ,  . . .  z n   =   y n   +   poly n ( y 1 , . . . , y n − 1 ) ,  (2) where poly i ( · ) ,   i   = 2 , . . . , n , denotes a polynomial function that includes only terms of degree   ≥   2   and   < w i . From the triangular structure of (2), it is clear that the inverse transformation   y   =   z   +   poly ′ ( z )   is of the same form. Given privileged coordinates   z i , define the   pseudonorm   at   x 0   as  ‖ z ‖ x 0   := max {| z 1 | 1 /w 1   , . . . ,   | z n | 1 /w n   } .  Using this pseudonorm we define the   w-weighted box   of size  ε   at   x 0   as the point set Box w ( ε ) :=   { z   ∈   R n   :   ‖ z ‖ x 0   ≤   ε } .  We use the notation Box w ( x 0 , ε )   for the corresponding locus of points in   M   given by the coordinates   z i .  Theorem II.1   (Ball-box theorem [19]) .   Fix a point   x 0   ∈ M   and a system of privileged coordinates   z 1 , . . . , z n   at   x 0 . Then there exist positive constants   a ( x 0 ) , A ( x 0 )   >   0 ,   and  σ ( x 0 )   >   0   such that for all   x   with   d ( x 0 , x )   < σ ( x 0 ) ,  a ( x 0 ) ‖ z ( x ) ‖ x 0   ≤   d ( x 0 , x )   ≤   A ( x 0 ) ‖ z ( x ) ‖ x 0   .   (3) Constructing the coordinate system   z   thus gives structure to how sub-Riemannian distance behaves locally; this struc- ture may be used for steering, e.g. [21]. It can be shown that there exists a continuously varying system of privileged coordinates on   M   so that the inequality (3) holds at all  x 0   for continuous positive functions   a ( · ) , A ( · ) ,   and   σ ( · )   on  M . Let us assume that the system   Σ   is sufficiently regular such that there exist bounds   0   < a min   ≤   a ( x )   ≤   A ( x )   ≤  A max   <   ∞   and   σ ( x )   ≥   σ min   >   0   for all   x   ∈ M . We state a pair of lemmas (whose proofs are provided in the Appendix) concerning how privileged coordinates relate to the Euclidean notions of volume and distance.  Lemma II.2   (Box volume) .   Fix   x   ∈ M . The volume of Box w ( x, r )   is given by   μ ( Box w ( x, r )) =   r D   where   D   = ∑ n i =1   w i .  Lemma II.3   (Distance comparison) .   Fix a point   x 0   ∈ M  and a system of privileged coordinates   z 1 , . . . , z n   at   x 0 . Then there exists a positive constant   θ ( x 0 )   >   0   such that for all  x   with   ‖ x 0   −   x ‖ ≤   θ ( x 0 ) ,  ‖ x 0   −   x ‖ ≤   d ( x 0 , x )   ≤   2 A max ‖ x 0   −   x ‖ 1 /s .  III. P ROBLEM   F ORMULATION  Let   M ⊂   R n   be the manifold defining a configuration space of a robotic system. Let   M obs   ⊂ M   be the obstacle region, such that   M \ M obs   is an open set, and denote the obstacle-free space as   M free   = cl( M \ M obs ) . The starting configuration   x init   is an element of   M free , and the goal region
M goal   is an open subset of   M free . The trajectory planning problem   is   denoted   by   the   tuple   (Σ ,   M free , x init ,   M goal ) , where   Σ , as discussed in Section II, denotes a driftless control-affine system. A dynamically feasible trajectory   x   is  collision-free   if   x ( t )   ∈ M free   for all   t   ∈   [0 , T   ] . A trajectory  x   is said to be   feasible   for the trajectory planning problem  (Σ ,   M free , x init ,   M goal )   if it is dynamically feasible, collision- free,   x (0) =   x init , and   x ( T   )   ∈   cl( M goal ) . Let   X   be the set of all feasible paths. A   cost function   for  (Σ ,   M free , x init ,   M goal )   is a function   c   :   X →   R ≥ 0   from the set of paths to the nonnegative real numbers; in this paper we consider the cost function   c ( x ) =   ` ( x )   defined as the arc length of   x   with respect to the Euclidean metric in   M . The objective is to find the feasible path with minimum associated cost; this minimum is achieved as long as   U   is closed and bounded [1]. The optimal trajectory planning problem is then defined as follows:  Optimal   motion   planning   for   driftless   sys- tems :   Given   a   trajectory   planning   problem  (Σ ,   M free , x init ,   M goal )   and an arc length function  c   :   X →   R ≥ 0 , find a feasible path   x ∗   such that  c ( x ∗ ) = min { c ( x ) :   x   is feasible } . If no such path exists, report failure. Our analysis will rely on two key sets of assumptions, relating, respectively, to the underlying system   Σ   and the problem-specific parameters   M free , x init ,   M goal .  1) Assumptions on system:   As in Section II, we require from   Σ   that a) the vector fields   g 1 , . . . , g m   are bracket generating, b) the configuration space   M   contains only regular points, c) there exist constants   a min , A max , σ min   such that Theorem II.1 holds with these values at all   x 0   ∈ M , and d) there exists a bounding constant   θ min   such that  0   < θ min   < θ ( x 0 )   for all   x 0   ∈ M , where   θ ( x 0 )   is as defined in Lemma II.3. Assumption (a) is a basic requirement for the system to be controllable, let alone optimally controlled, while (b), (c), and (d) ensure that there are no extreme regions of the configuration space which would require an unbounded sample density to capture their geometry. These assumptions will be collectively referred to as   A Σ .  2) Assumptions on problem parameters:   We require that the goal region   M goal   has   regular boundary , that is there exists   ξ >   0   such that   ∀ y   ∈   ∂ M goal , there exists   z   ∈ M goal  with   B e ( z, ξ )   ⊆ M goal   and   y   ∈   ∂B e ( z, ξ ) . This requirement that the boundary of the goal region has bounded curvature ensures that a point sampling procedure may expect to select points in the goal region near any point on its boundary. We also make requirements on the   clearance   of a trajec- tory, i.e., its “distance” from   M obs , standard for sampling- based methods [22]. For a given   δ >   0 , the   δ -interior of  M free   is defined as the set of all states that are at least a Euclidean distance   δ   away from any point in   M obs . A collision-free path   x   is said to have strong   δ -clearance if it lies entirely inside the   δ -interior of   M free . A collision-free path   x   is said to have weak   δ -clearance if there exists a path  x ′   that has strong   δ -clearance and there exists a homotopy  ψ , with   ψ (0) =   x   and   ψ (1) =   x ′   that satisfies the following three properties: (a)   ψ ( α )   is a dynamically feasible path for all   α   ∈   (0 ,   1] , (b)   lim α → 0   c ( ψ ( α ))   =   c ( x ) , and (c) for all   α   ∈   (0 ,   1] ,   ψ ( α )   has strong   δ α -clearance for some  δ α   >   0 . Properties (a) and (b) are required since pathological obstacle sets may be constructed that squeeze all optimum- approximating homotopies into undesirable motion. In prac- tice, however, as long as   M free   does not contain any passages of infinitesimal width, the fact that   Σ   is bracket-generating will allow every trajectory to be weak   δ -clear. IV. P ROBABILISTIC   E XHAUSTIVITY OF   S AMPLING  S CHEMES UNDER   D RIFTLESS   C ONSTRAINTS  In this section we prove a key result characterizing ran- dom sampling schemes for motion planning under drift- less differential constraints: any feasible trajectory through the configuration space   M   is “traced” arbitrarily well by connecting randomly distributed points from a sufficiently large sample set covering the configuration space. We will refer   to   this   property   as   probabilistic   exhaustivity .   Note that this notion is much stronger than the usual notion of probabilistic completeness in motion planning, where the requirement is that   at least one   feasible trajectory is traced. The notion of probabilistic exhaustivity, besides being a result of independent interest, is a strong tool in proving asymptotic optimality of sampling-based motion planning algorithms, as will be shown in Section V (specifically, we focus on differential variants of PRM ∗   [22] and FMT ∗   [17]). Let   x   : [0 , T   ]   → M   satisfy the system (1). Given a set of waypoints   { y m } M m =1   ⊂ M , we associate a dynamically feasible trajectory   y ∗   :   [0 , S ]   →   M   that connects the nodes   y 1 , . . . , y M   in order so that each connection is locally optimal, i.e. each path segment connecting   y m   to   y m +1   has length   d ( y m , y m +1 ) . We consider the waypoints   { y m }   to  ( ε, r ) -trace   the trajectory   x   if: a)   d ( y m , y m +1 )   ≤   r   for all  m , b) the cost of   y ∗   is bounded as   c ( y ∗ )   ≤   (1 +   ε ) c ( x ) , and c) the distance from any point of   y ∗   to   x   is no more than   r , i.e.   min t ∈ [0 ,T   ]   d ( y ( s ) , x ( t ))   ≤   r   for all   s   ∈   [0 , S ] . In the context of sampling-based motion planning, we may expect to find closely tracing   { y m }   as a subset of the sampled points, provided the sample size is large. We formalize this notion in Theorem IV.5, the proof of which requires three technical lemmas (the proofs of these lemmas are provided in the Appendix).  Lemma IV.1.   Let   x   be a dynamically feasible trajectory and consider a partition of the time interval   0 =   τ 1   < τ 2   <  · · ·   < τ M   =   T   . Suppose that   { y m } ⊂ M   satisfy a)   y m   ∈  B ( x ( τ m ) , ρ )   for all   m   ∈ { 1 , . . . , M   } , and b) more than a  (1   −   α )   fraction of the   y m   satisfy   y m   ∈   B ( x ( τ m ) , βρ )   for a parameter   β   ∈   (0 ,   1) . Then the cost   c ( y ∗ )   of the trajectory  y ∗   sequentially connecting the nodes   y 1 , . . . , y M   is upper bounded as  c ( y ∗ )   ≤   c ( x ) + 2 M ρ ( β   +   α   −   αβ ) .  Remark IV.2.   If we further assume that   y 1   =   x (0) , then the bound   c ( y ∗ )   ≤   c ( x ) + 2( M   −   1) ρ ( β   +   α   −   αβ )   holds.  Let   SampleFree ( n )   denote a set of   n   points sampled independently and identically from the uniform distribution
on   M free .  Lemma IV.3.   Fix   n   ∈   N ,   α   ∈   (0 ,   1) , and let   S 1 , . . . , S M   be disjoint subsets of   M free   with  μ ( S m ) =   μ ( S 1 )   ≥  (   2 + log(1 /α )  n  )  e 2 μ ( M free ) ,  for each   m . Let   V   =   SampleFree ( n )   and define  K n   := # { m   ∈ { 1 , . . . , M   }   :   S m   ∩   V   =   ∅} .  Then   P   ( K n   ≥   αM   )   ≤   e − αM  1 − e − n   .  Lemma IV.4.   Fix   n   ∈   N   and let   T 1 , . . . , T M   be subsets of  M free , possibly overlapping, with  μ ( T m ) =   μ ( T 1 )   ≥   κ   (log   n/n )   μ ( M free )  for   each   m   and   some   constant   κ   >   0 .   Let   V   =  SampleFree ( n )   and denote by   E m   the event that   T m ∩ V   =  ∅   for each   m . Then  P  (∨ M m =1   E m  )  ≤   M n − κ .  Before stating the theorem and proof in full, we sketch our approach for proving probabilistic exhaustivity. Given a path to be traced with waypoints from a sample set, we tile the span of the path with two sequences of concentric sub-Riemannian balls – a sequence of “small” balls and a sequence of “large” balls. With high probability, all but a tiny   α   fraction of the small balls will contain a point from the sample set (Lemma IV.3), and for any small balls that don’t contain such a point we ensure that the concentric large ball does (Lemma IV.4). We take these points as a sequence of waypoints which tightly follows the reference path with few exceptions, and never has a gap over any section of the reference path when it deviates. We then use the metric inequality for   d   to bound the total cost of the waypoint trajectory (Lemma IV.1).  Theorem   IV.5   (Probabilistic   exhaustivity) .   Let   Σ   be   a DCA system satisfying the assumptions   A Σ   and suppose  x   :   [0 , T   ]   →   M free   is a dynamically feasible trajectory with   strong   δ -clearance,   δ   >   0 .   Let   V   =   { x init } ∪  SampleFree ( n ) ,   ε >   0 , and for fixed   n   consider the event  A n   that there exist   { y m } M m =1   ⊂   V   which   ( ε, r n ) -trace   x , where  r n   = 4 A max (1 +   η ) 1 /D  (   μ ( M free )  D  ) 1 /D   (   log   n n  ) 1 /D  for a parameter   η   ≥   0 . Then, as   n   → ∞ , the probability that  A n   does not occur is asymptotically bounded as   P   ( A c n ) =  O ( n − η/D   log − 1 /D   n ) . Proof.   Note that in the case   c ( x ) = 0   we may pick   y 1   =   x (0)  to be the only waypoint and the result is trivial. Therefore assume   c ( x )   >   0 . Fix   n   sufficiently large so that   r n / 2   ≤  σ min . Take   x ( τ n,m )   to be points spaced along   x   at sub- Riemannian distances   r n / 2 ; more precisely let   τ 1   = 0 , and for   m   = 2 ,   3 , . . .   consider  τ n,m   = min ( { τ   ∈   ( τ n,m − 1 ,   1) :   d ( x ( τ   ) , x ( τ n,m − 1 ))   ≥   r n / 2 } )   .  Let   M n   be the first   m   for which the set is empty; take  τ n,M n   =   T   . Note that since the distance   d   is an infimum over all feasible trajectories, one of which is the segment of   x   between   τ n,m   and   τ n,m +1 , we have the bound   M n   ≤ d 2 c ( x ) /r n e . We now make the identification   α   =   β   =   ε/ 2   anticipating the application of Lemma IV.1. Take   ρ n   =   r n / 4   and define a sequence of sub-Riemannian balls   B n, 1 , . . . , B n,M n   centered along the trajectory   x   by   B n,m   =   B ( x m , ρ n ) , where   x m   =  x ( τ n,m )   for   m   ∈ { 1 , . . . , M n } . Within these balls define a concentric sequence of smaller, disjoint balls   B β n,m   =  B ( x m , βρ n )   for each   m   ∈ { 1 , . . . , M n } . Note that since Box w ( x m , ρ n /A max )   ⊂   B n,m , we have the volume lower bound   μ ( B n,m )   ≥   ( ρ n /A max ) D   and similarly   μ ( B β n,m )   ≥  ( βρ n /A max ) D   .   Denote   K β n   :=   # { m   ∈   { 1 , . . . , M n }   :  B β n,m   ∩   V   =   ∅} . We consider the event    ̃ A n   that every large ball   B n,m , as well as at least a   (1   −   α )   fraction of the small balls   B β n,m , contains at least one point of   V   :   ̃ A n   =   { K β n   < αM n  }   ∧  M n ∧  m =1  { B n,m   ∩   V   6   =   ∅} .  We claim that    ̃ A n   implies the event   A n   that there exist  ( ε, r n ) -tracing   { y m }   ⊂   V   . If    ̃ A n   holds, then we select waypoints   { y m } M n  m =1   ⊂   V   such that   y m   ∈   B n,m   for every point, as well as   y m   ∈   B β n,m   for at least a   (1   −   α )   fraction of the points. In particular let us select   y 1   =   x (0) . First note that   y m   ∈   B n,m   for each   m   implies  d ( y m , y m +1 )   ≤   d ( y m , x m ) +   d ( x m , x m +1 ) +   d ( x m +1 , y m +1 )  ≤   r n / 4 +   r n / 2 +   r n / 4 =   r n .  Next, applying Remark IV.2 we have  c ( y ∗ )   ≤   c ( x ) + 2( M n   −   1) ρ n ( β   +   α   −   αβ )  ≤   c ( x ) + 2(2 c ( x ) /r n )( r n / 4) ε  ≤   (1 +   ε ) c ( x ) .  Finally we check that any point   y ∗ ( t )   is within distance   r n  from a point on   x . To see this suppose that   y ∗ ( t )   lies on the shortest path connecting   y m   to   y m +1 . Note that  d ( y m , y ∗ ( t )) +   d ( y ∗ ( t ) , y m +1 ) =   d ( y m , y m +1 )   ≤   r n .  Then we may write  d ( x m , y ∗ ( t )) +   d ( x m +1 , y ∗ ( t ))   ≤   d ( x m , y m ) +   d ( x m +1 , y m +1 ) +   r n  ≤   (3 / 2) r n ,  so that   min { d ( x m , y ∗ ( t )) , d ( x m +1 , y ∗ ( t )) } ≤   (3 / 4) r n   < r n . Thus the waypoints   { y m }   ( ε, r n ) -trace   x . Now making the identifications   S m   =   B β n,m   and   T m   =  B n,m   in Lemmas IV.3 and IV.4 respectively, we see that by considering   n   ≥   N 1   sufficiently large so that   log   N 1   ≥
Dβ − D   (2 + log(1 /α )) e 2   (satisfying the volume assumption of Lemma IV.3), we compute the union bound  P   ( A c n )   ≤   P  (    ̃ A c n  )  ≤   P   ( K β n   ≥   αM n  )   +   P  (∨ M n  m =1 { B n,m   ∩   V   =   ∅}  )  ≤   e − αM n  1   −   e − n   +   M n n − (1+ η ) /D   .  Now,   c ( x )   >   0   and   r n   = Θ((log   n/n ) 1 /D   )   together imply that   M n   = Θ(( n/   log   n ) 1 /D   ) . The second term in the bound dominates as   n   → ∞ , and   P   ( A c n ) =   O ( n − η/D   log − 1 /D   n ) . V. O PTIMAL   S AMPLING - BASED   A LGORITHMS FOR  D RIFTLESS   C ONTROL -A FFINE   S YSTEMS  In this section we present two algorithms for the mo- tion   planning   problem   with   driftless   control-affine   sys- tems. The first algorithm, named the Differential Probabilis- tic RoadMap algorithm (DPRM ∗ ), is a derivation of the PRM ∗   algorithm presented in [22], while the second algo- rithm, named the Differential Fast Marching Tree algorithm (DFMT ∗ ), is a derivation of the FMT ∗   algorithm presented in [17]. This section provides a description of both algorithms, while the next section focuses on their theoretical character- ization (chiefly, their asymptotic optimality property). As in Section IV, let   SampleFree ( k )   be a function that returns a set of   k   ∈   N   states sampled independently and identically from the uniform distribution on   M free . These sampled states are connected as vertices in a graph from which a solution trajectory will be computed. Given two ver- tices   x 1   and   x 2 , we denote with the edge   ( x 1 , x 2 )   an optimal cost trajectory from   x 1   to   x 2   neglecting   obstacle constraints. Let   CollisionFree ( x 1 , x 2 )   denote the boolean function which returns true if and only if the edge   ( x 1 , x 2 )   does not intersect an obstacle. Given a set of vertices   V   , a state   x   ∈ M , and a threshold   r >   0 , let   Near ( V, x, r )   be a function that returns the set of states   { v   ∈   V   :   ‖ v ‖ x   < r/a min } . Given a graph   G   = ( V, E ) , where   V   is the vertex set and  E   is the edge set, and a vertex   x   ∈   V   , let   Cost ( x, G )   be the function that returns the cost of the shortest path in the graph   G   between the vertices   x init   and   x . Let   Path ( x, G )  be the function that returns the path achieving that cost. The DPRM ∗   algorithm is given in Algorithm 1, while the DFMT ∗   algorithm is given in Algorithm 2. DPRM ∗   works by sampling a set of points within   M free   and connecting each state to every state in a local neighborhood around it, provided that the connection is collision free. The resulting graph spans   M free , with the local connections combining to yield a global “roadmap” for traveling between any two states, not just   x init   and the goal. The least cost path in the graph between   x init   and   M goal , computed, e.g., using the Dijkstra’s algorithm, is output by DPRM ∗ . The DFMT ∗   algorithm essentially implements a stream- lined version of DPRM ∗   by performing a “lazy” dynamic programming recursion, this time during the state connection phase instead of as a last step, to grow a   tree   of trajectories  Algorithm 1   Differential Probabilistic RoadMap (DPRM ∗ )  1   V   ← { x init } ∪   SampleFree ( n ) ;   E   ← ∅  2   for each   v   ∈   V   do  3   for each   u   ∈   Near ( V   \{ v } , v, r n )   do  4   if   CollisionFree ( u, v )   then  5   E   ←   E   ∪ {{ u, v }}  6   end if  7   end for  8   end for  9   V   ∗   ←   argmin v ∈ V   ∩M goal   Cost ( v, G   = ( V, E ))  10   if   V   ∗   6   =   ∅   then  11   v ∗   ←   random vertex in   V   ∗  12   return   Path ( v ∗ , G   = ( V, E ))  13   else  14   return   Failure  15   end if  which moves steadily outward in cost-to-come space. In the Algorithm 2 outline, the set   W   consists of all of the nodes that have not yet been added into the tree, while   H  is comprised only of nodes that are in the tree. In particular, while   H   keeps track of nodes which have already been added to the tree, nodes are removed from   H   if they are not near enough to the edge of the expanding tree to actually have any new connections made with   W   (see [17] for further details). At each iteration, DFMT ∗   examines the neighborhood of a state in   H   and   only   considers locally-optimal (assuming no obstacles) connections for potential inclusion in the tree (see line (10)). By only checking for collision on the locally- optimal (assuming   no obstacles) connection,   as   opposed to every possible connection (essentially what is done in DPRM ∗ ), DFMT ∗   saves a large (indeed unbounded as the number of vertices increases) number of collision-check computations. A more detailed explanation of the meaning of the dual sets   H   and   W   and of the philosophy behind DFMT ∗   can be found in [17]. The main differences of these algorithms with respect to their geometric counterparts (i.e., PRM ∗   and FMT ∗ ) are that (i) near vertices lie within the privileged coordinate box Box w ( x, r/a min )   rather than within the Euclidean ball, and (ii) the edges connecting vertices are optimal   trajectories  rather than straight lines. Indeed, attempting connections within the sub-Riemannian ball   B ( x, r )   would be the best analogy to the geometric planning case, and is preferable for efficiency if possible. The structure of this set is difficult to compute exactly, however, which motivates the choice Box w ( x, r/a min )   as a tractable approximation (as also sug- gested in [11]). Checking connections within this superset of   B ( x, r )   induces an extra run time factor of at most  ( A max /a min ) D   , a bound on the volume ratio between the two sets. VI. A SYMPTOTIC   O PTIMALITY OF   DFMT ∗   AND   DPRM ∗  In this section, we prove the asymptotic optimality of DFMT ∗   and DPRM ∗   (obtained as a simple corollary). We conclude the section by providing a discussion of the results.
Algorithm 2   Differential Fast Marching Tree (DFMT ∗ )  1   V   ← { x init } ∪   SampleFree ( n ) ;   E   ← ∅  2   W   ←   V   \{ x init } ;   H   ← { x init }  3   z   ←   x init  4   while   z / ∈ M goal   do  5   H new   ← ∅  6   X near   =   Near ( V   \{ z } , z, r n )   ∩   W  7   for   x   ∈   X near   do  8   Y near   ←   Near ( V   \{ x } , x, r n )   ∩   H  9   y min   ←   arg min y ∈ Y near   { Cost ( y, T   =   ( V, E )) +  d ( y, x ) }  10   if   CollisionFree ( y min , x )   then  11   E   ←   E   ∪ {{ y min , x }}  12   H new   ←   H new   ∪ { x }  13   W   ←   W   \{ x }  14   end if  15   end for  16   H   ←   ( H   ∪   H new ) \{ z }  17   if   H   =   ∅   then  18   return   Failure  19   end if  20   z   ←   arg min y ∈ H   { Cost ( y, T   = ( V, E )) }  21   end while  22   return   Path ( z, T   = ( V, E ))  A. Asymptotic Optimality of DFMT ∗   and DPRM ∗  The following theorem presents a result comparing the output cost of DFMT ∗   to the cost of any feasible trajectory.  Theorem   VI.1   (DFMT ∗   cost   comparison) .   Let  (Σ ,   M free , x init ,   M goal )   be   a   trajectory   planning   problem satisfying the assumptions   A Σ   and suppose   x   : [0 , T   ]   → M  is a feasible path with strong   δ -clearance,   δ >   0 . Assume further that   x   extends into the interior of   M goal , i.e. there exists   γ >   0   such that   B ( x ( T   ) , γ )   ⊂ M goal . Let   c n   denote the cost of the path returned by DFMT ∗   with   n   vertices using a radius  r n   = 4 A max (1 +   η ) 1 /D  (   μ ( M free )  D  ) 1 /D   (   log   n n  ) 1 /D  for a parameter   η   ≥   0 . Then for fixed   ε >   0  P   ( c n   >   (1 +   ε ) c ( x )) =   O ( n − η/D   log − 1 /D   n ) .  Proof.   The proof of this theorem is conceptually similar to the one of Theorem 1 in [17]. The details are provided in the Appendix. We are now in a position to state the optimality result for DFMT ∗   (note that the result also provides a   convergence rate  bound). The optimality of DPRM ∗   will follow as a corollary.  Theorem   VI.2   (DFMT ∗   asymptotic   optimality) .   Let  (Σ ,   M free , x init ,   M goal )   be a trajectory planning problem, satisfying the assumptions   A Σ   and with   M goal   ξ -regular, such that there exists an optimal path   x ∗   : [0 , T   ∗ ]   → M  with weak   δ -clearance for some   δ >   0 . Let   c ∗   denote the arc length of   x ∗ , and let   c n   denote the cost of the path returned by DFMT ∗   with   n   vertices using the radius  r n   = 4 A max (1 +   η ) 1 /D  (   μ ( M free )  D  ) 1 /D   (   log   n n  ) 1 /D  for a parameter   η   ≥   0 . Then for fixed   ε >   0  P   ( c n   >   (1 +   ε ) c ∗ ) =   O ( n − η/D   log − 1 /D   n ) .  Proof.   Note that   c ∗   =   0   implies   x init   ∈   cl ( M goal ) , and DFMT ∗   will terminate immediately and optimally. In this case the result is trivial, therefore assume   c ∗   >   0 . Let  ψ   denote   the   homotopy   given   by   the   weak   δ -clearance definition for   x ∗ . Using the fact that   lim α → 0   c ( ψ ( α ))   =  c ∗ ,   we   pick   an   α 0   >   0   such   that   c ( ψ ( α 0 ))   ≤   (1 +  ε/ 4) c ∗ . Denote   x   :=   ψ ( α 0 ) . We now extend   x   into the interior of   M goal   so that we may apply Theorem VI.1. Since   x ∗   is   optimal,   x ( T   )   =   x ∗ ( T   ∗ )   ∈   ∂ M goal   and the   ξ -regularity of   M goal   means there exists   z   ∈   M goal  with   B e ( z, ξ )   ⊆   M goal   and   y   ∈   ∂B e ( x ( T   ) , ξ ) .   Pick  ρ   = min  {  ξ,   δ α 0  2   ,   1 2 A max  (   εc ∗  4  ) s  , θ min  }  ,   and take   z ′   on the straight line segment between   x ( T   )   and   z   with   ‖ x ( T   ) − z ′ ‖ ≤  ρ . Then   B ( z ′ , ρ )   ⊂   B e ( z ′ , ρ )   ⊂   B e ( z, ξ )   ⊂ M goal . The last two terms in the minimum above ensure, from Lemma II.3, that   d ( x ( T   ) , z ′ )   ≤   ( ε/ 4) c ∗ . Consider the extension   x ′   of  x   constructed by concatenating   x   with the shortest sub- Riemannian path between   x ( T   )   and   z ′ . The cost of   x ′   is bounded as  c ( x ′ )   ≤   c ( x ′ ) + ( ε/ 4) c ∗   ≤   (1 +   ε/ 2) c ∗ .  The strong   ( δ α 0   / 2) -clearance of   x ′   is established by noting for any   p   along the path between   x ( T   )   and   z ′ :  inf  a ∈M obs  ‖ p   −   a ‖ ≥   inf  a ∈M obs  ‖ x ( T   )   −   a ‖ − ‖ p   −   x ( T   ) ‖ ≥   δ α 0   −   δ α 0   / 2 =   δ α 0   / 2 .  Then by Theorem VI.1, with the approximation factor   ε/ 4 , we have for   ε   ∈   (0 ,   1)   that  P   ( c n   >   (1 +   ε ) c ∗ )   ≥   P   ( c n   >   (1 +   ε/ 4)(1 +   ε/ 2) c ∗ ) =   O ( n − η/D   log − 1 /D   n ) .  For   ε   ≥   1   the desired result follows from the monotonicity in   ε   of the above probability.  Remark   VI.3.   There   is   an   implicit   dependence   on   the problem parameters and approximation factor   ε   in the con- vergence rate bound given above in Theorem VI.2; these fixed parameters influence the threshold   n > N   after which the stated asymptotic bound holds.  We conclude this section with the asymptotic optimality result for DPRM ∗ .  Corollary   VI.4   (DPRM ∗   asymptotic   optimality) .   Let  (Σ ,   M free , x init ,   M goal )   be a trajectory planning problem, satisfying the assumptions   A Σ   and with   M goal   ξ -regular, such that there exists an optimal path   x ∗   : [0 , T   ∗ ]   → M  with weak   δ -clearance for some   δ >   0 . Let   c ∗   denote the arc
length of   x ∗ , and let   c n   denote the cost of the path returned by DPRM ∗   with   n   vertices using the radius  r n   = 4 A max (1 +   η ) 1 /D  (   μ ( M free )  D  ) 1 /D   (   log   n n  ) 1 /D  for a parameter   η   ≥   0 . Then for fixed   ε >   0  P   ( c n   >   (1 +   ε ) c ∗ ) =   O ( n − η/D   log − 1 /D   n ) .  Proof.   Given the same sample set   V   , the trajectory tree constructed by DFMT ∗   is a subgraph of the roadmap graph constructed by DPRM ∗ . Hence the cost of the path returned by DPRM ∗   is upper bounded by that of DFMT ∗ . The desired result then follows immediately from Theorem VI.2.  B. Discussion  We note that there is room for improvement in the choice of constants for the theorems and lemmas above (particularly in the connection radius   r n ), which for the sake of clarity were not pushed as tight as possible. As a consequence, the asymptotic rate bound   O ( n − η/D   log − 1 /D   n )   may be achieved with a connection radius smaller by a constant fac- tor. The convergence rate bounds for DFMT ∗   and DPRM ∗ , stated in terms of the sample size, are also tunable to specific implementations and planning problems. A greater local connection radius achieves a faster asymptotic convergence rate, but such a choice may ultimately result in greater algorithm execution time as more connections are checked. The essential property of the system   Σ   underpinning the asymptotic optimality results above is the ball-box Theo- rem II.1. Indeed, although the results we present nominally apply only to arc length, the proofs hold for any system with a cost metric   d   on   M   satisfying the ball-box inequality (3). For example, if the ball-box inequality holds for arc length on a subset of dimensions of   M , DFMT ∗   and DPRM ∗  asymptotic optimality hold with respect to that subspace metric, a fact we make use of in our simulations. The constant   a min   ensures that the sub-Riemannian ball   B ( x, r )  is covered by the connection neighborhood of   x   (i.e. the algorithms search far enough in each direction), while the constant   A max   provides a lower bound on   μ ( B ( x, r ))   that ensures the sample set will intersect the neighborhood with high probability (i.e. the algorithms examine enough volume along an optimal path). If checking state membership in balls  B ( x, r )   is computationally efficient, then algorithmic perfor- mance gains may be achieved by using   Near ( V, x, r ) =   V   ∩  B ( x, r ) , while maintaining the guarantees of Theorems VI.1 and VI.2. Another consequence of this ball-box analysis foundation is the possibility of accommodating   approximate  steering techniques (as in [21]) for computing local, obstacle- free state connections (one of the bottlenecks for DPRM ∗   and DFMT ∗ ). As long as DFMT ∗   and DPRM ∗   have access to a local planner that can connect two states exactly, but possibly with some approximation factor to the optimal distance, then the global optimality theorems hold up to that same factor. Á  Á  Á  Á   Á   Á   Á  Á   Á  Á Á  Á   Á   Á   Á   Á  Á  Á  Á  Á  Á   Á   Á   Á   Á  Á  Á  Á  Á  Á  Á   Á  Á   Á  Á  Á  Á  Á   Á   Á  Á   Á   Á  Á  Á  Á   Á   Á   Á  Á   Á  Á  Á  Á  Á  Á  Á   Á  Á   Á  Á  Á  Á   Á  Á   Á   Á   Á   Á  Á  Á  Á  Á   Á  Á   Á   Á   Á  Á  Á  Á   Á  Á   Á   Á   Á  Á  Á  Á   Á  Á   Á  Á  Á   Á  Á  Á  Á  Á   Á   Á   Á  Á   Á  Á  Á  Á  Á  Á   Á  Á   Á   Á  Á  Á  Á   Á   Á  Á  Á  Á   Á  Á  Á  Á   Á  Á  Á  Á   Á  Á  Á  Á  Á   Á  Á   Á  Á   Á  Á  Á  Á  Á  Á   Á  Á   Á   Á  Á  Á  Á  Á   Á   Á   Á   Á  Á  Á  Á  Á   Á   Á   Á   Á   Á  Á  Á  Á  Á  Á  Á   Á   Á   Á  Á  Á  Á  Á  Á   Á  Á   Á  Á  Á  Á  Á  Á   Á   Á  Á   Á   Á  Á  Á   Á  Á   Á  Á   Á   Á  Á  Á  Á  Á   Á  Á   Á   Á  Á  Á  Á  Á   Á  Á   Á   Á  Á  Á  Á  Á  Á   Á   Á   Á   Á  Á  Á  Á  Á  Á   Á  Á   Á   Á  Á  Á  Á  Á  Á  Á  Á   Á   Á  Á  Á Á  Á   Á  Á  Á   Á  Á  Á  Á  Á  Á   Á   Á  Á   Á   Á  Á  Á  Á  Á   Á   Á  Á   Á   Á  Á  Á  Á  Á  Á   Á  Á   Á   Á  Á  Á Á  Á  Á  Á   Á   Á   Á  Á  Á  Á  Á   Á   Á  Á   Á   Á  Á  Á  Á  Á  Á   Á   Á   Á   Á  Á  Á  Á  Á  Á   Á   Á   Á   Á  Á  Á  Á  Á  Á   Á  Á  Á   Á  Á  Á  Á  Á  Á   Á  Á   Á   Á  Á  Á  Á  Á   Á   Á  Á   Á   Á  Á  Á Á   Á  Á  Á  Á   Á   Á  Á  Á  Á  Á  Á  Á   Á   Á  Á  Á   Á  Á   Á   Á   Á   Á  Á  Á  Á  Á  Á  Á   Á  Á  Á  Á Á  Á  Á   Á  Á  Á   Á  Á  Á  Á  Á   Á  Á  Á  Á   Á  Á  Á  Á  Á  Á   Á  Á  Á   Á  Á  Á  Á  Á   Á  Á  Á   Á   Á  Á  Á  Á  Á  Á   Á  Á   Á   Á  Á  Á  Á  Á  Á  Á  Á  Á   Á  Á  Á  Á   Á  Á   Á  Á   Á   Á  ·  ·  ·  ·   ·   ·   ·  ·   ·  · ·  ·   ·   ·   ·   ·  ·  ·  ·  ·  ·   ·   ·   ·   ·  ·  ·  ·  ·  ·  ·   ·  ·   ·  ·  ·  ·  ·   ·   ·  ·   ·   ·  ·  ·  ·   ·   ·   ·  ·   ·  ·  ·  ·  ·  ·  ·   ·  ·   ·  ·  ·  ·   ·  ·   ·   ·   ·   ·  ·  ·  ·  ·   ·  ·   ·   ·   ·  ·  ·  ·   ·  ·   ·   ·   ·  ·  ·  ·   ·  ·   ·  ·  ·   ·  ·  ·  ·  ·   ·   ·   ·  ·   ·  ·  ·  ·  ·  ·   ·  ·   ·   ·  ·  ·  ·   ·   ·  ·  ·  ·   ·  ·  ·  ·   ·  ·  ·  ·   ·  ·  · ·  ·   ·  ·   ·  ·   ·  ·  ·  ·  ·  ·   ·  ·   ·   ·  ·  ·  ·  ·   ·   ·   ·   ·  ·  ·  ·  ·   ·   ·   ·   ·   ·  ·  ·  ·  ·  ·  ·   ·   ·   ·  ·  ·  ·  ·  ·   ·  ·   ·  ·  ·  ·  ·  ·   ·   ·  ·   ·   ·  ·  ·   ·  ·   ·  ·   ·   ·  ·  ·  ·  ·   ·  ·   ·   ·  ·  ·  ·  ·   ·  ·   ·   ·  ·  ·  ·  ·  ·   ·   ·   ·   ·  ·  ·  ·  ·  ·   ·  ·   ·   ·  ·  ·  ·  ·  ·  ·  ·   ·   ·  ·  · ·  ·   ·  ·  ·   ·  ·  ·  ·  ·  ·   ·   ·  ·   ·   ·  ·  ·  ·  ·   ·   ·  ·   ·   ·  ·  ·  ·  ·  ·   ·  ·   ·   ·  ·  · ·  ·  ·  ·   ·   ·   ·  ·  ·  ·  ·   ·   ·  ·   ·   ·  ·  ·  ·  ·  ·   ·   ·   ·   ·  ·  ·  ·  ·  ·   ·   ·   ·   ·  ·  ·  ·  ·  ·   ·  ·  ·   ·  ·  ·  ·  ·  ·   ·  ·   ·   ·  ·  ·  ·  ·   ·   ·  ·   ·   ·  ·  · ·   ·  ·  ·  ·   ·   ·  ·  ·  ·  ·  ·  ·   ·   ·  · ·   ·  ·   ·   ·   ·   ·  ·  ·  ·  ·  ·  ·   ·  ·  ·  · ·  ·  ·   ·  ·  ·   ·  ·  ·  ·  ·   ·  ·  ·  ·   ·  ·  ·  ·  ·  ·   ·  ·  ·   ·  ·  ·  ·  ·   ·  ·  ·   ·   ·  ·  ·  ·  ·  ·   ·  ·   ·   ·  ·  ·  ·  ·  ·  ·  ·  ·   ·  ·  ·  ·   ·  ·   ·  ·   ·   ·  Û  Û  Û  Û   Û   Û  Û   Û  Û   Û   Û  Û  Û  Û  Û   Û  Û  Û  Û  Û  Û  Û  Û  Û  Û  Û   Û   Û  Û  Û  Û   Û   Û  Û  Û  Û  Û  Û  Û  Û  Û  Û   Û  Û   Û  Û  Û  Û  Û   Û  Û  Û  Û  Û   Û  Û  Û  Û  Û   Û  Û   Û  Û  Û  Û  Û   Û   Û  Û  Û  Û  Û  Û   Û  Û  Û  Û   Û  Û  Û  Û   Û  Û   Û  Û  Û  Û   Û  Û   Û  Û  Û  Û  Û  Û   Û   Û  Û  Û  Û  Û   Û  Û  Û  Û  Û   Û   Û  Û  Û   Û  Û  Û  Û  Û  Û  Û  Û  Û   Û  Û  Û  Û  Û   Û   Û  Û  Û  Û  Û   Û  Û  Û  Û  Û   Û  Û  Û  Û   Û   Û  Û  Û  Û  Û  Û  Û  Û  Û  Û  Û  Û  Û  Û  Û  Û  Û  Û   Û  Û   Û  Û  Û  Û  Û   Û  Û  Û   Û  Û   Û  Û  Û   Û  Û  Û  Û  Û  Û  Û  Û  Û  Û   Û  Û   Û  Û  Û   Û  Û  Û  Û  Û  Û  Û  Û  Û  Û  Û  Û  Û  Û  Û  Û  Û  Û  Û  Û  Û   Û   Û  Û  Û   Û  Û  Û  Û   Û  Û  Û  Û  Û   Û   Û  Û  Û  Û  Û  Û   Û  Û   Û  Û  Û  Û  Û  Û   Û   Û  Û  Û  Û  Û   Û  Û  Û  Û   Û   Û  Û  Û  Û  Û  Û  Û  Û  Û  Û  Û  Û  Û  Û  Û  Û  Á   DFMT *  ·   DFMT *   H no precomputation L  Û   DPRM *  0   5   10   15   20  1.2  1.4  1.6  1.8  2.0  Execution Time   H s L  Solution Cost  Reeds - Shepp Car Á  Á Á Á   Á   Á   Á   Á   Á  ·  · ·   ·   ·   ·   ·   ·   ·  Û  Û   Û   Û   Û  Á   DFMT *  ·   DFMT *   H no precomputation L  Û   DPRM *  0   5   10   15   20  0.0  0.2  0.4  0.6  0.8  1.0  Execution Time   H s L  Success Rate  Reeds - Shepp Car DFMT   ∗   Tree, N = 1000 Samples  Fig. 1.   Top: Simulation results for the Reeds-Shepp car system with a maze obstacle set. The error bars in each axis represent plus and minus one standard error of the mean for a fixed sample size   n . Bottom: Example DFMT ∗   tree for   n   = 1000 . The feasible path returned is highlighted in blue with car direction at each sample state denoted by an arrow.
VII. I MPLEMENTATION AND NUMERICAL EXPERIMENTS  The DFMT ∗   and DPRM ∗   algorithms were implemented in Julia and run using a Unix operating system with a 2.0 GHz processor and 8 GB of RAM. Near neighbor sets were precomputed and cached at algorithm initialization after the sample set was selected. Note that for batch-processing (as opposed to “anytime”) algorithms such as DFMT ∗   and DPRM ∗ , one can precompute both near neighbor sets and sub-Riemannian distance ahead of time, as they do not depend on the obstacle configuration — the price to pay is a moderate increase in memory requirements. We tested DFMT ∗   and DPRM ∗   on the Reeds-Shepp car system [23], a well-known example of a regular driftless control-affine   system.   Within   the   configuration   manifold  M   =   R 2   ×   S 1 , trajectories   x ( t ) = ( x 1 ( t ) , x 2 ( t ) , θ ( t ))   are subject to equation (1) with   g 1 ( x ) = (cos   θ,   sin   θ,   0)   and  g 2 ( x )   =   (0 ,   0 ,   1) . With the car constrained to move at unit speed with turning radius   R , the control set is   U   =  {− 1 ,   1 } ×   [ − 1 /R,   1 /R ] . The privileged coordinate system at each point   x   is defined by the vectors   g 1 ( x ) ,   g 2 ( x ) , and  [ g 1 ( x ) , g 2 ( x )] = (sin   θ,   −   cos   θ,   0)   with associated weight vector   (1 ,   1 ,   2) . We aim to optimize planar arc length in the first two dimensions of   M , in which case the constants for the ball-box Theorem II.1 are computed as   a min   =   √ 2 R   and  A max   = 2 √ 2 R   for   σ min   =   R . An analytic solution to the Reeds-Shepp optimal steering problem in the absence of obstacles is known [23], and consists of checking forty-six candidate curves for the one of minimum length. We precomputed and cached the optimal curve type between a state at the origin and any general state (using spatial symmetry this gives the optimal steering curve type between any two states in Reeds-Shepp space); the time for this operation was not included in our accounting of algorithm execution time. We used this data to implement the variants of DFMT ∗   and DPRM ∗   discussed in Section VI- B where the exact sub-Riemannian ball is used for the  Near   function instead of the privileged coordinate box. Collisions with obstacles were detected by approximating trajectories by piecewise linear interpolations, for which polytope intersection is simple to compute. The simulation results are summarized in Figure 1. A maze was used for   M obs , and our DFMT ∗   and DPRM ∗  implementations were run 50 times each on sample sizes up to   n   = 4000   in the case of DFMT ∗   and   n   = 1000   in the case of DPRM ∗ . We plot results for DFMT ∗   run both with and without the precomputed neighbor sets and distance cache; the solutions returned are identical and differ only in their timing data. For the Reeds-Shepp system, local connection computation and collision checking takes the majority of the computation time, and the speed increase from the cache is modest. For a greater speedup at the cost of more memory usage, all local   collision-free   connections could be cached ahead of time as well, leaving only collision checking with  M obs   (the only detail specific to the problem instance) to be computed online. VIII. C ONCLUSIONS  In this paper we presented a thorough theoretical frame- work   to   assess   optimality   guarantees   of   sampling-based algorithms for planning with driftless control-affine   sys- tems. We exploited this framework to design and analyze two novel sampling-based algorithms that are guaranteed to converge, as the number of samples increases, to an optimal solution. Our theoretical framework, which relies on the key notion of probabilistic exhaustivity, separates the probabilistic analysis of random sampling from the discrete analysis of combinatorial optimization algorithms, the two main elements in determining the performance of batch- processing motion planners (such as DFMT ∗   and DPRM ∗ ). We hope that this analysis framework can prove useful to analyze other types of algorithms for this class of problems. We note that our analysis approach yields convergence rate guarantees,   which   shed   additional   light   on   the   behavior of sampling-based algorithms for differentially-constrained motion planning problems. This paper leaves numerous important extensions open for further research. One extension that appears quite tractable is generalizing the cost function from arc length to functions that may depend on state-space position and/or control effort. For example, the techniques established in this paper may be used, with a few modifications, to prove DFMT ∗   and DPRM ∗   variants for positionally weighted arc length costs, in which certain spatial regions incur higher or lower costs for traversal. A second extension of great interest is the design and analysis of algorithms that address systems with  drift   (of critical importance, for example, for spacecraft motion planning). The specific analysis techniques used in this paper rely heavily on the fact that driftless control- affine systems can be viewed as metric spaces, but asym- metric adaptations may be imagined within the same proof framework. Third, we plan to study bidirectional versions of DFMT ∗   and DPRM ∗ . Finally, determining how algorithm tuning parameters affect asymptotic convergence rates and, ultimately, run time merits more extensive theoretical and experimental analysis, especially for DCA systems beyond the basic proof-of-concept Reeds-Shepp example presented in this paper. R EFERENCES [1]   S. Lavalle,   Planning Algorithms .   Cambridge University Press, 2006. [2]   S. M. LaValle, “Motion planning,”   IEEE Robotics Automation Maga- zine , vol. 18, no. 2, pp. 108–118, 2011. [3]   J. H. Reif, “Complexity of the mover’s problem and generalizations,” in   20th Annual Symposium on Foundations of Computer Science , 1979, pp. 421 –427. [4]   B. Donald, P. Xavier, J. Canny, and J. Reif, “Kinodynamic motion planning,”   Journal   of   the   Association   for   Computing   Machinery , vol. 40, no. 5, pp. 1048–1066, 1993. [5]   K. I. Tsianos, I. A. Sucan, and L. E. Kavraki, “Sampling-based robot motion planning: Towards realistic applications,”   Computer Science Review , vol. 1, no. 1, pp. 2 – 11, 2007. [6]   S. M. LaValle and J. J. Kuffner, “Randomized kinodynamic planning,”  International Journal of Robotics Research , vol. 20, no. 5, pp. 378– 400, 2001. [7]   S. Karaman and E. Frazzoli, “Sampling-based algorithms for optimal motion planning,”   International Journal of Robotics Research , vol. 30, no. 7, pp. 846–894, 2011.
[8]   ——,   “Optimal   kinodynamic   motion   planning   using   incremental sampling-based   methods,”   in   Proc.   IEEE   Conf.   on   Decision   and Control , 2010, pp. 7681–7687. [9]   A.   Perez,   R.   Platt,   G.   Konidaris,   L.   Kaelbling,   and   T.   Lozano- Perez, “LQR-RRT ∗ : Optimal sampling-based motion planning with automatically derived extension heuristics,” in   Proc. IEEE Conf. on Robotics and Automation , 2012, pp. 2537–2542. [10]   G.   Goretkin,   A.   Perez,   r.   Platt   Jr.,   and   G.   Konidaris,   “Optimal sampling-based planning for linear-quadratic kinodynamic systems,” in   Proc. IEEE Conf. on Robotics and Automation , 2013. [11]   S. Karaman and E. Frazzoli, “Sampling-based optimal motion planning for   non-holonomic   dynamical   systems,”   in   Proc.   IEEE   Conf.   on Robotics and Automation , 2013. [12]   D. J. Webb and J. van den Berg, “Kinodynamic RRT*: Optimal motion planning for systems with linear differential constraints,” in   Proc. IEEE Conf. on Robotics and Automation , 2013. [13]   Y. Li, Z. Littlefield, and K. Bekris, “Sparse methods for efficient asymptotically optimal kinodynamic planning,”   Workshop on Algo- rithmic Foundations of Robotics , 2014, available at http://arxiv.org/ pdf/1407.2896v2.pdf. [14]   R. T. M’Closkey III, “Exponential stabilization of driftless nonlinear control systems,” Ph.D. dissertation, 1995. [15]   R. M. Murray and S. Sastry,   A mathematical introduction to robotic manipulation .   CRC press, 1994. [16]   N. E. Leonard and P. Krishnaprasad, “Averaging for attitude control and motion planning,” in   Proc. IEEE Conf. on Decision and Control , 1993, pp. 3098–3104. [17]   L. Janson, E. Schmerling, A. Clark, and M. Pavone, “Fast marching tree: a fast marching sampling-based method for optimal motion planning in many dimensions,”   International Journal of Robotics Research , 2015. [18]   F. Bullo and A. Lewis,   Geometric control of mechanical systems , ser. Texts in Applied Mathematics.   Springer, 2005, vol. 49. [19]   A. Bella ̈ ıche, “The tangent space in sub-riemannian geometry,” in   Sub- Riemannian Geometry , A. Bella ̈ ıche and J. Risler, Eds.   Birkhauser, 1996, pp. 1–78. [20]   R. Montgomery,   A tour of subriemannian geometries, their geodesics, and applications , ser. Mathematical Surveys and Monographs.   Amer- ican Mathematical Society, 2002, vol. 91. [21]   F. Jean, G. Oriolo, and M. Vendittelli, “A globally convergent steering algorithm for regular nonholonomic systems,” in   Proc. IEEE Conf. on Decision and Control , 2005, pp. 7514–7519. [22]   S. Karaman and E. Frazzoli, “Sampling-based algorithms for optimal motion planning,”   International Journal of Robotics Research , vol. 30, no. 7, pp. 846–894, 2011. [23]   J. Reeds and R. Shepp, “Optimal paths for a car that goes both forwards and backwards,”   Pacific Journal of Mathematics , vol. 145, no. 2, 1990. [24]   M. Penrose,   Random Geometric Graphs .   Oxford University Press, 2003.  A PPENDIX  Lemma II.2   (Box volume) .   Fix   x   ∈ M . The volume of Box w ( x, r )   is given by   μ ( Box w ( x, r )) =   r D   where   D   = ∑ n i =1   w i . Proof.   From   the   formula   (2)   we   see   that   the   Jacobian matrix   J Z   for the change of coordinates between the lo- cal frame   y 1 , ..., y n   and privileged coordinates   z 1 , . . . , z n  is lower-triangular with all diagonal elements equal to   1 ; thus the Jacobian determinant   | J Z   |   is identically   1 . Since  Y 1 ( x ) , . . . , Y n ( x )   are orthonormal, the change of coordi- nates from the Euclidean standard basis to the coordinates  y 1 , . . . , y n , with Jacobian   J Y   , satisfies   | J Y   |   =   1 . Then  μ ( Box w ( x, r )) =   ∫  ‖ z ‖ x ≤ r   | J − 1  Z   || J − 1  Y   | dz   =   ∫  ‖ z ‖ x ≤ r   1   dz   =  r ∑ n i =1   w i   .  Lemma II.3   (Distance comparison) .   Fix a point   x 0   ∈ M  and a system of privileged coordinates   z 1 , . . . , z n   at   x 0 . Then there exists a positive constant   θ ( x 0 )   >   0   such that for all  x   with   ‖ x 0   −   x ‖ ≤   θ ( x 0 ) ,  ‖ x 0   −   x ‖ ≤   d ( x 0 , x )   ≤   2 A max ‖ x 0   −   x ‖ 1 /s .  Proof.   The left-hand inequality is a consequence of the definition of   d . For the right-hand inequality, we note for each coordinate that   | z i ( x ) 1 /w i   |   =   | y i ( x ) | 1 /w i   + O ( ‖ y ( x ) ‖ 2 /w i   )   ≤ ‖ x 0   −   x ‖ 1 /w i   +   O ( ‖ x 0   −   x ‖ 2 /w i   ) ,   so   then   d ( x 0 , x )   ≤  A max ‖ z ( x ) ‖ x 0   ≤   2 A max ‖ x 0   − x ‖ 1 /s   for small   ‖ x 0   − x ‖ .  Lemma IV.1.   Let   x   be a dynamically feasible trajectory and consider a partition of the time interval   0 =   τ 1   < τ 2   <  · · ·   < τ M   =   T   . Suppose that   { y m } ⊂ M   satisfy a)   y m   ∈  B ( x ( τ m ) , ρ )   for all   m   ∈ { 1 , . . . , M   } , and b) more than a  (1   −   α )   fraction of the   y m   satisfy   y m   ∈   B ( x ( τ m ) , βρ )   for a parameter   β   ∈   (0 ,   1) . Then the cost   c ( y ∗ )   of the trajectory  y ∗   sequentially connecting the nodes   y 1 , . . . , y M   is upper bounded as  c ( y ∗ )   ≤   c ( x ) + 2 M ρ ( β   +   α   −   αβ ) .  Proof.   Using the triangle inequality for the metric   d   we compare:  c ( y ∗ ) =  M   − 1 ∑  m =1  d ( y m , y m +1 )   ≤  M   − 1 ∑  m =1  [ d ( y m , x ( τ m )) +   d ( x ( τ m ) , x ( τ m +1 )) +   d ( x ( τ m +1 ) , y m +1 ) ]  =  M   − 1 ∑  m =1  d ( x ( τ m ) , x ( τ m +1 )) + 2  M ∑  m =1  d ( x ( τ m ) , y m )  −   d ( x 1 , y 1 )   −   d ( x M   , y M   )  ≤   c ( x )+2 ( M βρ   +   αM   (1 − β ) ρ )   ≤   c ( x )+2 M ρ ( β   +   α   −   αβ ) .  Lemma IV.3.   Fix   n   ∈   N ,   α   ∈   (0 ,   1) , and let   S 1 , . . . , S M   be disjoint subsets of   M free   with  μ ( S m ) =   μ ( S 1 )   ≥  (   2 + log(1 /α )  n  )  e 2 μ ( M free ) ,  for each   m . Let   V   =   SampleFree ( n )   and define  K n   := # { m   ∈ { 1 , . . . , M   }   :   S m   ∩   V   =   ∅} .  Then   P   ( K n   ≥   αM   )   ≤   e − αM  1 − e − n   . Proof.   The proof relies on a Poissonization argument. Let   ̃ n   be a random variable drawn from a Poisson distribution with parameter   n/e 2   (denoted as Poisson ( n/e 2 ) ). Consider the set of nodes    ̃ V   :=   SampleFree ( ̃ n ) . Let    ̃ K n   be the Poissonized analogue of   K n , namely   ̃ K n   := #  {  m   ∈ { 1 , . . . , M   }   :   S m   ∩    ̃ V   =   ∅  }  .  From the definition of    ̃ V   , we can see that   P   ( K n   ≥   αM   )) =
P  (  ̃ K n   ≥   αM   |    ̃ n   =   n  )  . Thus, we have  P  (  ̃ K n   ≥   αM  )  =  ∞ ∑  j =0  P  (  ̃ K n   ≥   αM   |    ̃ n   =   j  )  P   ( ̃ n   =   j )  ≥  n ∑  j =0  P  (  ̃ K n   ≥   αM   |    ̃ n   =   j  )  P   ( ̃ n   =   j )  ≥  n ∑  j =0  P  (  ̃ K n   ≥   αM   |    ̃ n   =   n  )  P   ( ̃ n   =   j ) =   P  (  ̃ K n   ≥   αM   |    ̃ n   =   n  )  P   ( ̃ n   ≤   n ) =   P   ( K n   ≥   αM   )   P   ( ̃ n   ≤   n )  ≥   (1   −   e − n ) P   ( K n   ≥   αM   ) ,  (4)  The third line follows from the fact that   P (    ̃ K n   ≥   αM   |  ̃ n   =  j )   is nonincreasing in   j , and the last line follows from a tail approximation of the Poisson distribution [24, p. 17] and the fact that   E [ ̃ n ] =   n/e 2 . The locations of the nodes in    ̃ V   are distributed as a spatial Poisson process with intensity   n/ ( e 2 μ ( M free )) . This means that for a Lebesgue-measurable region   R   ⊆ M free , the number of nodes in   R   is distributed as a Poisson random variable with distribution Poisson  (  ( n/e 2 ) μ ( R ) /μ ( M free )  )  , independent of the number of nodes in any region disjoint with   R   [22, Lemma 11]. Since by assumption the   S m   are all disjoint, we get that for   m   ∈ { 1 , . . . , M   } ,  P  (  S m   ∩    ̃ V   =   ∅  )  =   e − ( n/e 2 ) μ ( S 1 ) /μ ( M free ) .  Therefore,    ̃ K n   is distributed according to a binomial distri- bution, in particular according to a distribution Binomial( M   ,  e − ( n/e 2   ) μ ( S 1 ) /μ ( M free ) ). Noting that   e − ( n/e 2 ) μ ( S 1   ) /μ ( M free )   ≤  α/e 2   by assumption, we have that   E [    ̃ K n ]   ≤   αM/e 2 . So from a tail approximation to the Binomial distribution [24, p. 16],  P (    ̃ K n   ≥   αM   )   ≤   e − αM   and thus plugging into inequality (4)  P ( K n   ≥   αM   )   ≤   e − αM  1   −   e − n   .  Lemma IV.4.   Fix   n   ∈   N   and let   T 1 , . . . , T M   be subsets of  M free , possibly overlapping, with  μ ( T m ) =   μ ( T 1 )   ≥   κ   (log   n/n )   μ ( M free )  for   each   m   and   some   constant   κ   >   0 .   Let   V   =  SampleFree ( n )   and denote by   E m   the event that   T m ∩ V   =  ∅   for each   m . Then  P  (∨ M m =1   E m  )  ≤   M n − κ .  Proof.   We union bound the probability of any   E m   occurring:  P  (∨ M m =1   E m  )  ≤  M ∑  m =1  P   ( T m   ∩   V   =   ∅ ) =  M ∑  m =1  (  1   −   μ ( T m )  μ ( M free )  ) n  ≤   M e −  (   μ ( Tm )  μ ( M free )  )  n   ≤   M n − κ .  Theorem   VI.1   (DFMT ∗   cost   comparison) .   Let  (Σ ,   M free , x init ,   M goal )   be   a   trajectory   planning   problem satisfying the assumptions   A Σ   and suppose   x   : [0 , T   ]   → M  is a feasible path with strong   δ -clearance,   δ >   0 . Assume further that   x   extends into the interior of   M goal , i.e. there exists   γ >   0   such that   B ( x ( T   ) , γ )   ⊂ M goal . Let   c n   denote the cost of the path returned by DFMT ∗   with   n   vertices using a radius  r n   = 4 A max (1 +   η ) 1 /D  (   μ ( M free )  D  ) 1 /D   (   log   n n  ) 1 /D  for a parameter   η   ≥   0 . Then for fixed   ε >   0  P   ( c n   >   (1 +   ε ) c ( x )) =   O ( n − η/D   log − 1 /D   n ) .  Proof.   Consider   n   so that   r n   ≤   min { γ, δa min / 2 A max } , and apply Theorem IV.5 to produce, with probability at least   1   −  O ( n − η/D   log − 1 /D   n ) , a sequence of waypoints   { y m } M m =1   ⊂  V   which   ( ε, r n ) -trace   x . We claim that in the event that such   { y m }   exist, the DFMT ∗   algorithm will return a path with cost upper bounded as   c n   ≤   c ( y ∗ )   ≤   (1 +   ε ) c ( x ) . It is clear that the desired result follows from this claim. Assume the existence of   ( ε, r n ) -tracing   { y m } . Note that our upper bound on   r n   implies that Box w ( y m , r n /a min )  intersects   no   obstacles.   This   follows   from   the   inclu- sion   Box w ( y m , r n /a min )   ⊂   B ( y m , A max r n /a min )   ⊂  B e ( y m , A max r n /a min ) , and the Euclidean distance bound  inf  a ∈M obs  ‖ y m   −   a ‖ ≥   inf  a ∈M obs  ‖ x m   −   a ‖ − ‖ y m   −   x m ‖ ≥   2  (   A max  a min  )  r n   −   r n   ≥  (   A max  a min  )  r n .  Consider running DFMT ∗   to completion and for each   y m , let   c ( y m )   denote the cost-to-come of   y m   in the generated tree. If   y m   is not contained in any edge of the tree, we set  c ( y m ) =   ∞ . We show by induction that  min( c ( y m ) , c n )   ≤  m − 1 ∑  k =1  d ( y k , y k +1 ) ,   (5) for all   m   ∈ { 2 , . . . , M   } . The base case   m   =   2   is trivial, since the first step in the DFMT ∗   algorithm is to make every collision-free connection between   x init   =   y 1   and the nodes contained in Box w ( x, r n /a min ) , which will include   y 2   and, thus,   c ( y 2 ) =  d ( y 1 , y 2 ) . Now suppose (5) holds for   m   −   1 ; that means that one of the following four statements must hold. 1.   c n   ≤   ∑ m − 2  k =1   d ( y k , y k +1 ) , 2.   c ( y m − 1 )   ≤   ∑ m − 2  k =1   d ( y k , y k +1 )   and DFMT ∗   ends before considering   y m , 3.   c ( y m − 1 )   ≤   ∑ m − 2  k =1   d ( y k , y k +1 )   and   y m − 1   ∈   H   when  y m   is first considered, 4.   c ( y m − 1 )   ≤   ∑ m − 2  k =1   d ( y k , y k +1 )   and   y m − 1   / ∈   H   when  y m   is first considered. Case 1:   c n   ≤   ∑ m − 2  k =1   d ( y k , y k +1 )   ≤   ∑ m − 1  k =1   d ( y k , y k +1 ) .
Case 2:   c ( y m − 1 )   <   ∞   implies that   y m − 1   enters   H   at some point during DFMT ∗ . The case that   y m   goes uncon- sidered means that the algorithm terminates before   x m − 1  is ever minimum-cost element of   H . Since the end-node of the solution returned must have been the minimum-cost element of   H ,   c n   ≤   c ( y m − 1 )   ≤   ∑ m − 2  k =1   d ( y k , y k +1 )   ≤  ∑ m − 1  k =1   d ( y k , y k +1 ) . Case 3: Since Box w ( y m , r n /a min )   intersects no obstacles,  y m   must   be   connected   to   some   parent   when   it   is   first considered.   y m − 1   is a candidate, so   c ( y m )   ≤   c ( y m − 1 ) +  d ( y m − 1 , y m )   ≤   ∑ m − 1  k =1   d ( y k , y k +1 ) . Case 4: When   y m   is first considered, it is because there exists   z   ∈   Box w ( y m , r n /a min )   such that   z   is the minimum- cost element of   H . Again since Box w ( y m , r n /a min )   inter- sects no obstacles and contains at least one node in   H ,  y m   must   be   connected   to   some   parent   when   it   is   first considered. Since   c ( y m − 1 )   <   ∞ , there is a well-defined path   P   =   { v 1   =   x init , v 2 , . . . , v q   =   y m − 1 }   giving its cost-to- come in the DFMT ∗   tree at termination. Let   w   =   v j   , where  j   = max i ∈{ 1 ,...,q } { i   :   v i   ∈   H   when   y m   is first considered } . Then there are two subcases, either   w   ∈   B ( y m , r n )   or  w / ∈   B ( y m , r n ) . If   w   ∈   B ( y m , r n ) , then,  c ( y m )   ≤   c ( w ) +   d ( w, y m )  ≤   c ( w ) +   d ( w, y m − 1 ) +   d ( y m − 1 , y m )  ≤   c ( y m − 1 ) +   d ( y m − 1 , y m )   ≤  m − 1 ∑  k =1  d ( y k , y k +1 ) .  If   w / ∈   B ( y m , r n ) , then   r n   ≤   d ( w, y m )   ≤   d ( w, y m − 1 ) +  d ( y m − 1 , y m ) , so we have  c ( y m )   ≤   c ( z ) +   d ( z, y m )   ≤   c ( w ) +   r n  ≤   c ( w ) +   d ( w, y m − 1 ) +   d ( y m − 1 , y m )  ≤   c ( y m − 1 ) +   d ( y m − 1 , y m )   ≤  m − 1 ∑  k =1  d ( y k , y k +1 ) .  Thus the inductive step holds in all cases and therefore (5) holds for all   m . In particular taking   m   =   M   , and noting that   r n   ≤   γ   implies that   y M   ∈ M goal , this means that   c n   ≤  c ( y M   )   ≤   ∑ M   − 1  k =1   d ( y k , y k +1 ) =   c ( y ∗ )   as desired.