Toward Certifiable Motion Planning for
Medical Steerable Needles

Early work studied planning and control for steerable needles in the 2D plane [4, 6, 10, 44]. To fully utilize the capability of steerable needles, later work began to focus more on needle steering in 3D environments. Duindam et al. [15] used inverse kinematics for planning but the planner was tested only with simple geometrically shaped obstacles and provides no theoretical guarantees.

Other planners built upon the probabilistic completeness guarantees of sampling-based methods such as the Rapidly-exploring Random Tree (RRT) [31]. Xu et al. [53] used an RRT variant for needle steering but showed low efficiency in computing time. Patil et al. [40] developed an RRT-based needle planner that guides the tree expansion by sampling in the 3D workspace (instead of the configuration space). The efficiency obtained by sampling in the workspace and not accounting for the needle’s orientation makes the planner extremely fast in practice. Unfortunately, this makes the completeness proofs of the original RRT inapplicable and probabilistic completeness is not guaranteed.

To avoid dealing with curvature constraints directly in the RRT algorithms, there are also hybrid methods that combine sampling and other techniques. Favaro et al. [16] proposed a method that uses RRT* [25] that builds a tree embedded in the 3D workspace to generate candidate plans of low cost, followed by a smoothing step to account for the curvature constraint. However, this decoupled approach does not provide any theoretical guarantees.

Liu et al. proposed the Adaptive Fractal Tree (AFT) [34] for needle steering and used a Graphics Processing Unit (GPU) to further speed up their algorithm. The method uses a greedy approach for path refinement—it iteratively uses the lowest-cost path in the previous iteration for plan refinement. However, expanding the best path of a coarse resolution does not necessarily lead to a best path of a finer resolution. Furthermore, the authors use a cost function consisting of three factors, only one of which is the distance to the goal, also known as the targeting error. Thus, when provided with a required targeting error, paths produced by the method are not guaranteed to adhere to this constraint since the targeting error may be sacrificed for a better cost for the other two terms. Pinzi et al. [41] later extended AFT to account for goal orientation constraints.

Other methods focus on accounting for uncertainty during needle insertion but do not account for completeness [20, 47, 51, 49]. To summarize, to the best of the authors’ knowledge, none of the existing steerable needle planners provide provable guarantees on the planner’s completeness.

Generally speaking, an algorithm is resolution complete if it generates a plan to the goal whenever a solution exists at the maximal resolution and returns failure otherwise [7]. This property guarantees that given a predefined maximal resolution, the algorithm terminates in finite time and provides a deterministic result.

Barraquand et al. [8] proposed a planner for single/multi-body mobile robots with nonholonomic constraints. They formally proved the planner is guaranteed to generate a solution path when the discretization of the search parameters is fine enough. This approach was later extended by Lindemann and LaValle [33] to suggest a multi-resolution approach for 2D car-like robots. Both these works [8, 33] serve as the algorithmic foundations to the planner we present in this paper.

Sampling-based planners (such as RRT) typically ensure probabilistic completeness (i.e., such a planner is guaranteed to find a solution, if one exists, with probability one when given infinite time). However, they can also be used to build resolution-complete planners given some mild assumptions on the minimal motion that the system can perform. Cheng et al. [11] proposed a resolution-complete version of RRT for systems that satisfy the Lipschitz condition. Yershov et al. [54] formally analyzed the system conditions for the existence of resolution-complete planners. Kleinbort et al. [27] later analyzed the assumptions for RRT’s probabilistic completeness in kinodynamic planning. However their analysis can be adapted to resolution-completeness guarantees.

Ljungqvist et al. [35] proposed a planner for a general two-trailer system in 2D. They used a two-point boundary value problem (2pBVP) solver to generate a set of motion primitives connecting 2D grid points. Their planner is resolution-complete and resolution-optimal with respect to the resolution in the configuration space, which means the planner generates a plan with minimal cost among all solutions that can be represented as a sequence of motion primitives. Most of the above-mentioned planners can be used to plan for 2D nonholonomic robots. However, none account explicitly for the challenges of planning with curvature constraints in 3D, where the dimension of the search space is higher and there is no efficient 2pBVP solver. In this work, we provide a planner for 3D needle steering that is both efficient in practice and is guaranteed to be resolution complete.

Our needle planner builds a search tree $\mathcal{T}=(\mathcal{V},\mathcal{E})$ embedded in the C-space with $\mathbf{x}_{\rm start}$ as its root. Each node $v\in\mathcal{V}$ is associated with a configuration $\mathbf{x}_{v}\in\mathcal{X}$, and each edge $e=(v,u)\in\mathcal{E}$ represents the transition from $\mathbf{x}_{v}$ to $\mathbf{x}_{u}$. To expand a node $v\in\mathcal{V}$, we construct new nodes (children of $v$) with motion primitives (to be explained shortly in Sec. IV-B), which are pre-defined feasible motions. A child node $v_{\rm child}$ is accepted and added to the search tree if the trajectory from $v$ to $v_{\rm child}$ is collision-free and $v_{\rm child}$ is valid (will be detailed in Sec. IV-D). The search tree grows until there is some node $v$ with configuration $\mathbf{x}_{v}$ whose projection is inside the $\tau$-neighborhood of $p_{\rm goal}$ (condition (ii) in Problem 1).

A key aspect of our search method (which is similar in nature to other search-based planners [33]) is to use a set of motion primitives defined using multiple resolutions. Instead of expanding each node in our search tree using the entire set of motion primitives, we start with coarse motion primitives and use finer motion primitives as the search progresses. Thus, we start (Sec. IV-B) by describing the parameters required to define a motion primitive. After that, we continue (Sec. IV-C) to detail a hierarchy of motion primitives together with an ordering that will be used in our search algorithm. We then describe our search algorithm in detail (Sec. IV-D) and elaborate on the method we use to handle “similar” states, also known as duplicate detection [14] (Sec. IV-F). We conclude this section with some implementation details (Sec. IV-G).

Motion primitives, introduced by Frazzoli et al. [17], have been used in many motion planners [23, 24, 33, 42, 35]. In our setting, the motion primitives are a set of predefined kinematically feasible local motions. Roughly speaking, a motion primitive defines with what curvature the needle curves, how far the needle steers, and in what direction (see Fig. 3). Since for each motion primitive, the curvature $\kappa$ is explicitly defined, a motion primitive is guaranteed to be kinematically feasible as long as $\kappa\leq\kappa_{\max}$. As we will see in the proofs (Appendix A), our definition of motion primitives guarantees resolution completeness, and the experiments show that the definition also the enables computation efficiency of our algorithm.

More formally, to steer from configuration $\mathbf{x}_{v}$, a motion primitive is defined as a three-tuple $\mathcal{M}=(\kappa,\delta\ell,\delta\theta)$, where $\kappa\in[0,\kappa_{\max}]$ is the curvature, $\delta\ell>0$ is the length of the circular arc, and $\delta\theta\in[0,2\pi)$ is the angle between the curving plane and the XZ-plane of $\mathbf{x}_{v}$ (see Fig. 3). Thus the action space (or motion space) can be defined as $\mathcal{A}\subset\mathbb{R}^{3}$, which is the set of all motion primitives. We use $\mathbf{x}_{u}=\mathbf{x}_{v}\oplus\mathcal{M}$ to denote the operation of extending $\mathbf{x}_{v}$ with motion primitive $\mathcal{M}$ and obtaining the resultant configuration $\mathbf{x}_{u}$. See Fig. 3 for a step-by-step determination of $\mathbf{x}_{u}$. In the context of a search tree, by a slight abuse of notation, $u=v\oplus\mathcal{M}$ denotes the resultant node $u$, obtained by extending node $v$ with the motion primitive $\mathcal{M}$. We call $\mathcal{M}$ the extending primitive of node $u$.

Using motion primitives allows pre-computing intermediate configurations and thus saving computation efforts during planning by transforming these configurations to the frame defined by $\mathbf{x}_{v}$. Since the trajectory produced with one motion primitive is a circular arc, it is possible to densely interpolate the trajectory for collision-checking purposes.

In the following sections, we show that $\delta\ell$ and $\delta\theta$ are gradually refined in the algorithm. In contrast, we keep a fixed set of curvatures, $\{0,\kappa_{\max}\}$, for all motion primitives. As we will see (Sec. V and Appendix A) this does not hinder the guarantees provided by our approach. Moreover, as we demonstrate in our experiments (Sec. VI), these primitives, coupled with our planner allow us to efficiently compute paths for non-trivial instances where other planners fail.

Our algorithm uses a sequence of motion primitives, whose resolution changes from coarse to fine. The coarsest motion primitives are defined by some parameters $\delta\ell_{\max}$ and $\delta\theta_{\max}$. In our implementation and examples (e.g., Fig. 4) we have that $\delta\theta_{\max}=\frac{\pi}{2}$ and $\delta\ell_{\max}>0$ is a user-given parameter.

Since $\delta\theta\in[0,2\pi)$ and $\delta\theta_{\max}=\frac{\pi}{2}$, there exist four orientations ($\delta\theta\in\{0,0.5\pi,\pi,1.5\pi\}$) that have the coarsest orientation (see Fig. 4). There exists only one coarsest length, which is $\delta\ell_{\max}$, since path length is accumulated when we expand a node. To characterize how fine the resolution of a motion primitive $\mathcal{M}=(\kappa,\delta\ell,\delta\theta)$ is, we define the notions of length level $l_{\ell}$ and angle level $l_{\theta}$. More formally,

where ${\rm MOD}(\cdot)$ is the modulo operation.

For a motion primitive $\mathcal{M}=(\kappa,\delta\ell,\delta\theta)$, we refine the resolution of both the insertion $\delta\ell$ and the orientation $\delta\theta$. The new motion primitives constructed by refining $\delta\ell$ are:

Similarly, the motion primitives constructed by refining $\delta\theta$ are:

It is straight-forward to see that the refined motion primitives $\mathcal{M}_{\ell-}$ and $\mathcal{M}_{\ell+}$ both have a length level of $l_{\ell}(\mathcal{M})+1$ and the refined motion primitives $\mathcal{M}_{\theta-}$ and $\mathcal{M}_{\theta+}$ both have an angle level of $l_{\theta}(\mathcal{M})+1$ (see Fig. 4).

Note that when refining a motion primitive with $l_{\ell}(\mathcal{M})=0$ (resp. $l_{\theta}(\mathcal{M})=0$), we ignore $\mathcal{M}_{\ell+}$ (resp. $\mathcal{M}_{\theta-}$) as they both exceed the range of exploration.

Similar to Lindemann and LaValle [33], our search algorithm expands nodes according to a node’s rank. Rank captures both the depth of a node in the search tree and the fineness of resolution along the branch connecting the node from the root. We define the rank of the root node to be zero, the rank of any other node $v$ is recursively defined as:

For a visualization, see Figs. 4 and 5.

We run an A*-like search where nodes are ordered according to their rank (Eq. 3). A distinctive feature from (vanilla) A* is that when we expand a node, we also increase the resolution of the motion primitives used to expand its parent and add nodes using these finer motion primitives to the search’s priority queue. The rest of this section formalizes this idea.

Alg. 1 shows the pseudocode of our needle planner. We first initialize the coarsest orientations and the curvature set (line 1), then initialize the OPEN list and CLOSED set (line 3). The search algorithm then iteratively extracts nodes from the OPEN list (line 5), where nodes are ordered in a monotonically non-decreasing order according to their rank.

Only at this point (line 6) the extracted node is validated (also known as lazy validation [21, 36]). Validation of node $v$ involves ensuring that: (i) the accumulated trajectory length should not exceed the maximum insertion length $\ell_{\max}$; (ii) the goal point should be inside or close to the reachable region of $\mathbf{x}_{v}$ (Sec. IV-G); (iii) $v$ should not be a duplicated node (Sec. IV-G); and that (iv) the circular arc connecting $v.{\rm parent}$ and $v$ should be collision-free. An invalid node will be rejected and discarded. For a valid node $v$, we further check if there exists any similar configuration in the CLOSED set in order to avoid considering highly similar configurations (Sec. IV-F and Appendix A). A valid node without a similar configuration is accepted, expanded, and added to the CLOSED set (lines 10-12). The search terminates if the associated configuration of the accepted node satisfies the goal tolerance.

In our search algorithm, only the coarsest child nodes are added to the OPEN list during the initial expansion of a node (lines 10-11). But additional child nodes, created with finer motion primitives, are added when the coarse child nodes are extracted from the OPEN list (line 15). More specifically, when node $v$ is extracted, we refine its extending motion primitive $\mathcal{M}_{v}$ following Eq. 1 and 2 (line 14), and use the refined motion primitives $\mathcal{M}_{\ell\pm}$ and $\mathcal{M}_{\theta\pm}$ to expand $v.{\rm parent}$.

As specified in Sec. III, for a physical needle-steering robot there exists some smallest interval or precision of the achievable motions, which induces the minimal insertion and axial rotation $\delta\ell_{\min}$ and $\delta\theta_{\min}$, respectively. We term $\delta\ell_{\min}$ and $\delta\theta_{\min}$ as the cutoff resolution and stop adding refined nodes when the extending motion primitive $\mathcal{M}$ satisfies $2^{-l_{\ell}(\mathcal{M})}\cdot\delta\ell_{\max}<\delta\ell_{\min}$ or $2^{-l_{\theta}(\mathcal{M})}\cdot\delta\theta_{\max}<\delta\theta_{\min}$.

To avoid re-expanding the same or highly similar nodes multiple times, search-based planners often employ duplicate detection [14] that prunes so-called “duplicate” nodes. To prune duplicate nodes and enable the planner to rapidly explore the entire C-space, we reject a node if there already exists a similar configuration in the search tree (line 7). More formally, we reject node $v$ with configuration $\mathbf{x}_{v}$ if $\exists u\in\mathcal{V},\rho(\mathbf{x}_{u},\mathbf{x}_{v})<d_{\rm sim}$, where $d_{\rm sim}>0$ is a radius we use to identify similar configurations. Here, $\rho(\cdot)$ is a distance metric defined on $\mathcal{X}$ which in our work is defined as

where $\alpha>0$ is a weighting parameter and ${\rm dist}_{\sphericalangle}()$ is the angular distance between two orientations. Note that to guarantee resolution completeness, the value of  $d_{\rm sim}$ depends on other system parameters detailed in Sec. V and Appendix A.

We now describe several implementation details used to further speed up our approach. To distinguish between different implementations of our approach we refer to the basic version of our Resolution-Complete Search (i.e., without the implementation details described below) as RCS_BASIC and to the (basic) version that does not use similar-node rejection (i.e., when not performing the test in line 7) as RCS_NR. The versions that use all the following implementation details without and with parallelization (explained shortly) will be referred to as RCS and RCS_PARA, respectively.

Clearly the above definition is more a general intuition than a precise definition. We need to define what a “qualified solution is” and what “running $\mathcal{P}$ with resolution $R_{\min}$ on $\Delta$” means. These notions together with our main theoretic result (Thm. 2) are formalized in Appendix A.

As a first step we need to state how Def. 1 is instantiated in our setting. Here, the resolution is a pair $R=\{r_{\ell},r_{\theta}\}$ that characterizes the action space (namely, the insertion $\delta\ell$ and rotation $\delta\theta$ of the needle). However, this geometric characterization of the needle motion is a simplification of the way we control a needle in practice—via insertion and rotation velocity. This difference is important as the relative insertion and rotation velocity creates paths that have curvatures ranging between zero and $\kappa_{\max}$. In contrast, our motion primitives either follow a straight line or a path of maximal curvature $\kappa_{\max}$. Thus, the first part of our proof shows that considering these two fixed curvatures allows us to approximate any path arbitrarily well. This is done using the notion of duty cycling [37] and is detailed in Sec. B of Appendix A. The original idea in [37] is designed specifically for bevel-tip needles. We look at the problem from a geometric perspective and decouple the guarantees and the needle mechanism, thus making it valid for needles with different designs.

The second part of our proof, detailed in Sec. C of Appendix A, states that any path that adheres to some mild assumptions can be approximated arbitrarily well by a very specific set of motion primitives—those with some fixed resolution. This is a somewhat technical but important step—it will allow us to argue that as long as the cutoff resolution (defined in Sec. IV-E) is fine enough, our algorithm RCS_NR is guaranteed to find a solution in finite time (when no node rejection is applied). Here we adapt the original proof by Barraquand et al. [7, Appendix A] that considers paths in a two-dimensional workspace. As our needle moves in a 3D workspace, we cannot use the proof as-is and detail some required adaptations.

These parts are summarized in the following theorem (stated informally to avoid using notations defined in Appendix A).

The third part of our proof, described in Sec. E of Appendix A, shows that even with similar node rejection, the basic version of our algorithm RCS_BASIC still finds a solution when several conditions are satisfied. Here we adapt the proofs provided by Cheng and LaValle [11, Thm. 5.2]. In their proof, a fixed control period is assumed for every motion primitive. Thus, we need to incorporate the machinery developed in the second part of our proof (Sec. C of Appendix A) and obtain the following result (again, stated informally to avoid using notations defined only in Appendix A).

In the final part of the proof (Sec. F of Appendix A), we show that none of the implementation details we use to improve the algorithm’s efficiency hinder the theoretical guarantees of RCS_BASIC.

Before we state the different parts of our proof, we introduce some definitions. Recall that a steerable needle motion planning problem is a tuple $\Delta=(\mathcal{X},\mathcal{W}_{\rm obs},\mathbf{x}_{\rm start},p_{\rm goal},\tau,\ell_{\max},\kappa_{\max})$ and that $\rho(\cdot)$ is a distance metric defined on $\mathcal{X}$ (Eq. 4). Finally, recall that $\mathcal{A}$ is the action space, which is the set of all valid motion primitives. Throughout the proof, for some sequence of motion primitives $M$, we will use $\mathbf{x}\oplus M$ to denote the resultant trajectory obtained by sequentially applying elements in $M$ to $\mathbf{x}$.

Note that in the definitions of decomposable and traceable trajectories we allow any arbitrary set of motion primitives. This allows us to decouple the planner’s ability of finding a path using a given set of motion primitives with the expressiveness of the motion primitives.

Finally, as we will see, it will be convenient to introduce the notion of a finest set of motion primitives.

When a bevel-tip needle is inserted only, it follows a trajectory with curvature $\kappa_{\max}$. When the needle is inserted while applying axial rotational velocity that is relatively larger than the insertion velocity, it follows a straight line (i.e., of curvature zero). Minhans et al. [37] introduced the notion of duty-cycling to approximate any curvature for bevel-tip steerable needles. Roughly speaking, combining periods of needle spinning (i.e., zero-curvature trajectories) with periods of non-spinning (i.e., maximal-curvature trajectories) enables the needle to achieve any curvature up to the maximum needle curvature. This idea is formalized in the following lemma.

Proof sketch. Here, to explicitly show how the approximation factor is used. And to provide a more general discussion, we provide a proof from a geometric perspective (and not control-based as in the original work by Minhas et al. [37]).

The trajectory $\sigma$ is decomposable, thus there exists a sequence of motion primitives $M_{\sigma}=\{\mathcal{M}_{1},\dots,\mathcal{M}_{n}\}$ such that $\sigma=\sigma(0)\oplus M_{\sigma}$ and each motion primitive $\mathcal{M}_{i}$ has arbitrary curvature $\kappa_{i}\in[0,\kappa_{\max}]$. To approximate $\mathcal{M}_{i}$, we construct a sequence of motion primitives $M_{i}=\{\mathcal{M}_{i}^{(1)},\dots,\mathcal{M}_{i}^{(n_{i})}\}$ that satisfies

Namely, the first motion primitive $\mathcal{M}_{i}^{(1)}$ ensures that both trajectories use the same curving plane (see Fig. 3) and the the rest of the sequence stays within this curving plane and approximates the (arbitrary) curvature $\kappa_{i}$.

We then decompose $\mathcal{M}_{i}$ into small equal-length segments of length $\ell_{i}$ (except possibly the last segment) where the specific value of $\ell_{i}$ is chosen according to the value of $\varepsilon_{d}$. We then use three motion primitives to approximate each of these segments as illustrated in Fig. 9. It is not hard to see that (i) the start and end configurations of $\mathcal{M}_{i}$ and $M_{i}$ are identical, and (ii) the one-way Hausdorff distance between $\mathcal{M}_{i}$ and each $\mathcal{M}_{i}^{(j)}$ is less than $\varepsilon_{d}$ if $\ell_{i}$ is carefully chosen.

Let $M_{\sigma}^{\varepsilon_{d}}=M_{1}\cdot M_{2}\cdot\ldots\cdot M_{n}$ be this sequence of all the newly constructed motion primitives. Then it is straightforward that $\sigma(0)\oplus M_{\sigma}^{\varepsilon_{d}}$ is an $\varepsilon_{d}$-approximation of $\sigma$. $\hfill\blacksquare$

Proof sketch (adapted from [7, Appendix A]). The trajectory $\sigma$ is decomposable, thus there exists a finite sequence of motion primitives $M_{\sigma}=\{\mathcal{M}_{1},\dots,\mathcal{M}_{n}\}$ such that $\sigma=\sigma(0)\oplus M_{\sigma}$. Set $K_{\sigma}=\bigcup_{i}\mathcal{M}_{i}.\kappa$ to be the set of all curvatures that appear in $M_{\sigma}$.

To approximate each motion primitive $\mathcal{M}_{i}$ using primitives from the finest set of motion primitives $\mathcal{M}_{\rm fs}(R(\sigma,\varepsilon_{r}),K_{\sigma})$ (Def. 9), we construct a sequence motion primitive $M_{i}=\{\mathcal{M}_{i}^{(1)},\dots\mathcal{M}_{i}^{(n_{i})}\}$, where

Similar to the sequence constructed for Lemma 1, the first motion primitive $\mathcal{M}_{i}^{(1)}$ accounts for the curving plane (though here it can only be approximated) and the the rest of the sequence stays within this curving plane and accounts for the length of the circular arc the trajectory follows in this plane. Applying the sequence $M_{i}$ is equivalent to applying one motion primitive $\tilde{\mathcal{M}}_{i}=(\mathcal{M}_{i}.\kappa,n_{i}\cdot r_{\ell},k_{i}\cdot r_{\theta})$. Thus, by carefully choosing $r_{\ell}$ and $r_{\theta}$, distance between $\mathcal{M}_{i}$ and $\tilde{\mathcal{M}}_{i}$ (see Def. 7) can be arbitrarily small.

Since the system is Lipschitz continuous,

Thus, to ensure that $\sigma(0)\oplus\{\tilde{\mathcal{M}}_{1},\dots,\tilde{\mathcal{M}}_{n}\}$ is an $\varepsilon_{r}$-approximation of $\sigma$, we only need to ensure that $\varepsilon\leq\frac{\varepsilon_{r}(L_{s}-1)}{L_{s}(L_{s}^{n}-1)}$. As both $n$ and $L_{s}$ are fixed, we can choose $\varepsilon$ to be as small as needed thus the desired fine resolution exists which concludes the proof. $\hfill\blacksquare$

Proof sketch. Set $\varepsilon_{t}=\varepsilon_{d}=\varepsilon_{r}=\varepsilon/3$. According to Def. 6, there exists a decomposable trajectory $\sigma_{t}$ that is an $\varepsilon_{t}$-approximation of $\sigma$. Moreover, according to Lemma 1, there exists a finite sequence of motion primitives $M_{D}$ in which every element has curvature $\kappa\in\{0,\kappa_{\max}\}$ such that the trajectory $\sigma_{d}=\sigma(0)\oplus M_{D}$ is an $\varepsilon_{d}$-approximation of $\sigma_{t}$.

Note that by construction $\sigma_{d}$ is decomposable. Thus, according to Lemma 2, there exists a fine resolution $R(\sigma,\varepsilon_{r})=\{r_{\ell},r_{\theta}\}$ and a finite sequence of motion primitives $M_{R(\sigma,\varepsilon_{r})}$ such that $\sigma_{r}=\sigma(0)\oplus M_{R(\sigma,\varepsilon_{r})}$ is an $\varepsilon_{r}$-approximation of $\sigma_{d}$. Moreover, $M_{R(\sigma,\varepsilon_{r})}\subseteq\mathcal{M}_{\rm fs}(R(\sigma,\varepsilon_{r}),\{0,\kappa_{\max}\})$ as the construction in the proof of Lemma 2 does not add new curvatures.

Finally, as $\varepsilon_{d}=\varepsilon_{d}=\varepsilon_{r}=\varepsilon/3$. Then the trajectory $\sigma_{r}$ is an $\varepsilon$-approximation of $\sigma$. $\hfill\blacksquare$

Proof sketch. Let $\sigma$ be a traceable solution with clearance $\gamma$. Following Cor. 1, there exists a fine resolution $R(\sigma,\varepsilon)=\{r_{\ell},r_{\theta}\}$ and a finite sequence of motion primitives $M_{R(\sigma,\varepsilon)}\subseteq M_{\rm fs}(R(\sigma,\varepsilon_{r}),\{0,\kappa_{\max}\})$ such that $\sigma(0)\oplus M_{R(\sigma,\varepsilon)}$ is an $\varepsilon$-approximation of $\sigma$. In our algorithm, the resolutions are divided by half as the length level $l_{\ell}$ and angle level $l_{\theta}$ increase. Thus, there exists a fine-enough resolution $\tilde{R}=\{2^{-l_{\ell}}\cdot\delta\ell_{\max},2^{-l_{\theta}}\cdot\delta\theta_{\max}\}$ that satisfies $2^{-k_{\ell}}\cdot\delta\ell_{\max}<r_{\ell}$, $2^{-k_{\theta}}\cdot\delta\theta_{\max}<r_{\theta}$. Setting the cutoff resolution $R_{\min}$ to be finer (both with respect to the insertion as well as rotation) than $\tilde{R}$ ensures that $M_{R(\sigma,\varepsilon)}$ can be approximated arbitrarily well.²²2To be more precise, one needs to account for the cases where $R(\sigma,\varepsilon_{r})$ is not in the sequence of resolutions considered by the algorithm and we may introduce additional error when approximating $R(\sigma,\varepsilon_{r})$ with $\tilde{R}$. However, using the techniques we previously used this can be easily accounted for. We omit this in our proof sketch.

The search tree built with RCS_NR is a subtree of a dense tree in which each node is expanded with every element in $\mathcal{M}_{\rm fs}(\tilde{R},\{0,\kappa_{\max}\})$. This is because every coarse motion primitive used in RCS_NR can be decomposed into a sequence of motion primitives in $\mathcal{M}_{\rm fs}(\tilde{R},\{0,\kappa_{\max}\})$. Additionally, if we allow the algorithm run until the OPEN list is exhausted, every node in the dense tree (except for those that are in collision) will be explored by RCS_NR. Since the dense tree encodes all possible trajectories that can be decomposed with $\mathcal{M}_{\rm fs}(\tilde{R},\{0,\kappa_{\max}\})$, when the solution $\sigma$ is traceable, has $\gamma$-clearance, and the system is Lipschitz continuous, an $\varepsilon$-approximation (with $\varepsilon<\gamma$) of $\sigma$ will be encoded in the dense tree and thus will be explored by RCS_NR. $\hfill\blacksquare$

We are now ready to show that even with similar node rejection, our algorithm is still resolution complete

The proof of Thm. 2 uses Thm. 1 and then follows Cheng and LaValle [11, Thm. 5.2]. We include it here for completeness.

Proof. According to Thm. 1, RCS_NR terminates in finite time, thus RCS_BASIC also terminates in finite time since more nodes are rejected. We now prove that RCS_BASIC can find a solution plan if conditions (C1)-(C4) are satisfied.

Since $\sigma$ is traceable, there exists some fine resolution $R(\sigma,\varepsilon)$ can be explored by RCS_BASIC (as discussed in Thm. 1), with which we can construct an $\varepsilon$-approximation of $\sigma$. Denote the decomposable approximation $\sigma^{\prime}$, and the sequence of motion primitives to compose it $M_{\sigma^{\prime}}=\{\mathcal{M}_{1},\dots,\mathcal{M}_{n}\}$. When $M_{\sigma^{\prime}}$ is sequentially applied to $\mathbf{x}_{\rm start}$, we obtain a sequence of configurations $\{\mathbf{x}_{0},\mathbf{x}_{1},\dots,\mathbf{x}_{n}\}$, where $\mathbf{x}_{0}=\mathbf{x}_{\rm start},\mathbf{x}_{i}=\mathbf{x}_{i-1}\oplus\mathcal{M}_{i},i\in[1,n]$. For the rest of the proof, we use $M_{\sigma}[i,j]=\{\mathcal{M}_{i},\dots,\mathcal{M}_{j}\}$ to denote a subsequence of $M_{\sigma^{\prime}}$. We also use $\mathbf{x}+M_{\sigma^{\prime}}[i,j]$ to denote the configuration after sequentially applying $\{\mathcal{M}_{i},\dots,\mathcal{M}_{j}\}$ to $\mathbf{x}$.

If we run RCS_NR, every $\mathbf{x}_{i}$ will be explored and $\sigma$ will be constructed when the search terminates. However, if we run RCS_BASIC, we prune nodes using duplicate detection (Sec. IV-F). Thus, we need to show that even with pruning, RCS_BASIC will still find a plan. This will be done by showing that the same sequence of motion primitives can be applied to configurations that are “similar” to $\mathbf{x}_{0}\ldots\mathbf{x}_{n}$ and the resultant plan $\tilde{\sigma}$ exists using the fact that $\tilde{\sigma}$ is “similar” to $\sigma$ and that $\sigma$ has $\gamma$-clearance. The rest of this proof formalizes this idea.

Recall that (C3) ensures that $d_{\rm sim}<\delta\ell_{\min}$ which guarantees that any motion primitive will end up at a non-similar configuration. Now, let $\mathbf{x}_{i}$ be the first configuration that is pruned because of a similar configuration (see Alg. 1, line 7). We will say that $\mathbf{x}_{i}$ is replaced by the similar configuration $\mathbf{x}^{\prime}_{i}$. As $i\geq 1$, in the worst case we have $i=1$. We then apply $M_{\sigma^{\prime}}[2,n]$ to $\mathbf{x}^{\prime}_{1}$. According to (C1), the maximal error accumulated to $\mathbf{x}^{\prime}_{n}=\mathbf{x}^{\prime}_{1}+M_{\sigma^{\prime}}[2,n]$ is $\varepsilon_{1}=\rho(\mathbf{x}^{\prime}_{n},\mathbf{x}_{n})=L_{s}^{n-1}\cdot d_{\rm sim}$. Similarly, when $\mathbf{x}^{\prime}_{2}$ is replaced by $\mathbf{x}^{\prime\prime}_{2}$, we apply $M_{\sigma^{\prime}}[3,n]$ to $\mathbf{x}^{\prime\prime}_{2}$ and for $\mathbf{x}^{\prime\prime}_{n}=\mathbf{x}^{\prime\prime}_{2}+M_{\sigma^{\prime}}[3,n]$, the accumulated error is $\varepsilon_{2}=\rho(\mathbf{x}^{\prime\prime}_{n},\mathbf{x}^{\prime}_{n})=L_{s}^{n-2}\cdot d_{\rm res}$. The same analysis applies for $\{\mathbf{x}_{3},\dots,\mathbf{x}_{n}\}$. According to (C3), the total accumulated error then becomes:

According to (C4), we have that $\|{\rm Prog}(\mathbf{x}_{n})-g_{\rm goal}\|_{2}\leq\frac{\tau}{2}$. Thus,

This implies that even in the worst case where all possible replacements happen, the final configuration $\mathbf{x}^{(n)}_{n}$ still satisfies the required goal tolerance (see Fig. 11).

Additionally, we prove that when pruning happens for $\mathbf{x}^{(j)}_{i}$, the motion plan constructed with $M_{\sigma^{\prime}}[i,n]$ is still collision-free. We have shown above that $\rho(\mathbf{x}^{(n)}_{n},\mathbf{x}_{n})<\frac{\tau}{2}$. Moreover, we have that $\forall i\in[0,n],\rho(\mathbf{x}^{(n)}_{i},\mathbf{x}_{i})<\frac{\tau}{2}$ since less error is accumulated for $i<n$. Thus we have that

And for any configuration $\tilde{\mathbf{x}}$ along edge $(\mathbf{x}^{(j)}_{k},\mathbf{x}^{(j)}_{k+1})$, we have that