Localization aware sampling and connection strategies for incremental motion planning under uncertainty

Abstract

We present efficient localization aware sampling and connection strategies for incremental sampling-based stochastic motion planners. For sampling, we introduce a new measure of localization ability of a sample, one that is independent of the path taken to reach the sample and depends only on the sensor measurement at the sample. Using this measure, our sampling strategy puts more samples in regions where sensor data is able to achieve higher uncertainty reduction while maintaining adequate samples in regions where uncertainty reduction is poor. This leads to a less dense roadmap and hence results in significant time savings. We also show that a stochastic planner that uses our sampling strategy is probabilistically complete under some reasonable conditions on parameters. We then present a localization aware efficient connection strategy that uses an uncertainty aware approach in connecting the new sample to the neighbouring nodes, i.e., it uses an uncertainty measure (as opposed to distance) to connect the new sample to a neighboring node so that the new sample is reachable with least uncertainty (“the closest”), and furthermore, connections to other neighbouring nodes are made only if the new path to them (via the new sample) helps to reduce the uncertainty at those nodes. This is in contrast to current incremental stochastic motion planners that simply connect the new sample to all of the neighbouring nodes and therefore, take more search queue iterations to update the paths (i.e., uncertainty propagation). Hence, our efficient connection strategy, in addition to eliminating the inefficient edges that do not contribute to better localization, also reduces the number of search queue iterations. We provide simulation results that show that (a) our localization aware sampling strategy places less samples and find a well-localized path in shorter time with little compromise on the quality of path as compared to existing sampling techniques, (b) our localization aware connection strategy finds a well-localized path in shorter time with no compromise on the quality of path as compared to existing connection techniques, and finally (c) combined use of our sampling and connection strategies further reduces the planner run time.

Appendix: Probabilistic completeness proof for RRBT-LAS

In this section, we provide a formal proof that a planner with our localization aware sampling strategy is probabilistically complete for DistTH less than or equal to half of the inscribed radius of the robot.

The worst case situation that leads to probabilistic completeness issues with our approach is using RangeModel 1 where the sensors (beacons in our case) have limited range. In that case the heuristic used in our sampling strategy will limit the samples to only one (within ball of radius DistTH) for regions with low uncertainty reduction. This is where the completeness issue arises. If the value of DistTH is large then the planner that uses our sampling strategy may not be able to find a path. We show that if we keep DistTH below half of the inscribed radius of the robot (a reasonable assumption) then if there exists a collision-free path, a planner that uses our sampling strategy will also find one. For the proof we assume that the entire path passes through regions with low uncertainty reduction (a worst case scenario for our sampling strategy). Also note that our proof builds along the lines of Choset et al. (2005b), therefore, we follow most of their notations.

Suppose $q_{s}, q_{g} \in C_{free}$ (free region of C-space) are two robot configurations that can be connected by a path in $C_{free}$. RRBT-LAS is considered to be probabilistically complete, if for any given $(q_{s}, q_{g})$

$$\begin{aligned} \lim _{n \rightarrow \infty } Pr[(q_{s}, q_{g}) { FAILURE}] = 0, \end{aligned}$$

(10)

where $Pr[(q_{s}, q_{g}) { FAILURE}]$ denotes the probability that RRBT-LAS fails to answer the query $(q_{s}, q_{g})$ after a roadmap in $C_{free}$ with n samples has been constructed. The outline of the probabilistic completeness proof is as follows: First we assume that a path $\pi$ from $q_{s}$ to $q_{g}$ exists. We then tile the path with a set of carefully chosen balls such that generating a sample in each ball ensures that these samples can be connected with appropriate collision-free edges and hence a collision-free path, $\hat{\pi }$ between $q_{s}$ and $q_{g}$ will be found by RRBT-LAS and the probability of generating such samples approaches 1 with increasing n.

Assume a path $\pi$ (of length L) from $q_{s}$ to $q_{g}$ exists in d dimensional C-space. The clearance of $\pi$, denoted $\rho =clr(\pi )$, is the farthest distance away from the path at which a given point can be guaranteed to be collision-free. Note that $\rho \ge 2r$, where r is the inscribed radius of the robot. The measure $\mu$ denotes the volume of a region of space, e.g, $\mu (B_{\epsilon }(x))$ measures the volume of an open ball $B_{\epsilon }(x)$ of radius $\epsilon$ centered at x. If $A\subset C_{free}$ is a measurable subset and x is a random point chosen from $C_{free}$, then

$$\begin{aligned} Pr(x\in A)= \dfrac{\mu (A)}{\mu (C_{free})}. \end{aligned}$$

(11)

We now tile the path $\pi$ with balls each of radius $\frac{\rho }{2}$. Let $m=\lceil \frac{2L}{\rho } \rceil$ and observe that there are m points (centers of balls) on the path such that $dist(q_{i},q_{i+1})<\frac{\rho }{2}$, where dist is a Euclidean metric on $\mathbb {R}^{d}$. Let $y_{i} \in B_{\rho /2}(q_{i})$ and $y_{i+1} \in B_{\rho /2}(q_{i+1})$. Then the line segment $\overline{y_{i}y_{i+1}}$ must lie inside $C_{free}$ since both endpoints lie in the ball $B_{\rho }(q_{i})$. An illustration of this basic fact is given in Figure 7.17 of Choset et al. (2005b). Let $V\subset C_{free}$ be a set of n configurations generated by our localization aware sampling strategy. If there is a subset of configurations $\lbrace y_{1},\ldots ,y_{m} \rbrace \subset V$ such that $y_{i} \in B_{\rho /2}(q_{i})$, then each ball will get a sample and a path from $q_{s}$ to $q_{g}$ will be found. Let $I_{1},\ldots ,I_{m}$ be a set of indicator variables such that each $I_{i}$ witnesses the event that there is a $y \in V$ and $y \in B_{\rho /2}(q_{i})$. It follows that RRBT-LAS succeeds in answering the query $(q_{s}, q_{g})$ if $I_{i}=1$ for all $1\le i \le m$. If at least one of the indicator variables is 0 then RRBT-LAS would fail. Therefore, the probability of failure (Eq. 10) then can be written as

$$\begin{aligned} Pr[(q_{s}, q_{g}) { FAILURE}]\le & {} Pr \Bigg (\bigvee _{i=1}^{m} I_{i}= 0 \Bigg ) \end{aligned}$$

(12)

$$\begin{aligned}\le & {} \sum _{i=1}^{m} Pr[I_{i}=0], \end{aligned}$$

(13)

where the last inequality follows from the union bound. We now mainly focus on the computation of $Pr[I_{i}=0]$ for $i^{th}$ ball, i.e., after placing n samples by our localization aware sampling strategy what is the probability that none of these samples lie in a ball $B_{\rho /2}(q_{i})$.

Fig. 23

[Case $DistTH\le \frac{\rho }{2}$]—black colour (bold) circle denotes one of the balls $B_{\frac{\rho }{2}}(q_{i})$ that is used to tile a path, red colour dots represent randomly placed samples, and hatched region (of radius DistTH=$\frac{\rho }{2}$) around each sample denotes the restricted region where samples can not be placed according to heuristic used in localization aware sampling. This figure shows the situation (excluding b) where none of the samples has yet been placed inside the black ball. a neighbouring samples around $B_{\frac{\rho }{2}}(\cdot )$ restrict some region (hatched areas inside the black ball) inside the black ball where samples can not be placed, b neighbouring samples totally covered $B_{\frac{\rho }{2}}(\cdot )$ but in that case samples lie on the periphery (closed set), c samples are placed at a distance d such that $\frac{\rho }{2} < d < \frac{\rho }{2}+\epsilon$, even in this worst case scenario the probability of generating a sample in $B_{\frac{\rho }{2}}(\cdot )$ is greater than 0 (see text for explanation) (Color figure online)

For RangeModel 1, in regions outside the sensor range where there is no sensor information, hence no uncertainty reduction, our localization aware sampling strategy does not allow another sample within the vicinity (DistTH) of an already placed sample point (see Fig. 23). Therefore, the probability of failure to generate a second sample in a ball $B_{\rho /2}(q_{i})$ depends on where the first sample was placed and so on. Let $I_{i}^{1},\ldots ,I_{i}^{n}$ be a set of indicator variables for the $i^{th}$ ball such that each $I_{i}^{k}$, for all $1\le k \le n$, witnesses the event that the $k^{th}$ sample does not lie in ball $B_{\rho /2}(q_{i})$. These events are dependent on each other. Below we provide the expressions to compute $Pr[I_{i}^{k}=0]$ that will lead us to the computation of $Pr[I_{i}=0]$. For the first two samples, the probability of failure to generate a sample inside ball $B_{\rho /2}(q_{i})$ can be written as

$$\begin{aligned}&Pr\Big [I_{i}^{1}=0\Big ] = \Bigg \lbrace 1-\dfrac{\mu (B_{\rho /2}(q_{i}))}{\mu (C_{free})} \Bigg \rbrace \end{aligned}$$

(14)

$$\begin{aligned}&Pr\Big [I_{i}^{2}=0\Big ] = \int Pr \Big (I_{i}^{2}=0 \mid x^{1} \Big ) Pr\Big (x^{1} \Big ) dx^{1}. \end{aligned}$$

(15)

In above expression $x^{1}$ denotes the position of first sample. Above expression is just the marginalization over the position of first sample. Similarly, the expression for the third sample is

$$\begin{aligned} Pr \Big [I_{i}^{3}=0 \Big ]= & {} \iint Pr\Big (I_{i}^{3}=0 \mid x^{1},x^{2}\Big ) Pr\Big (x^{2} \mid x^{1}\Big )\nonumber \\&Pr\Big (x^{1}\Big ) dx^{2} dx^{1} \end{aligned}$$

(16)

and for $n^{th}$ sample the expression (for $Pr[I_{i}^{n}=0]$) is

$$\begin{aligned}&=\idotsint Pr \left( I_{i}^{n}\!=\!0 \mid x^{1},\ldots ,x^{n-1}\right) ,\ldots ,Pr \Big (x^{2} \mid x^{1} \Big )\nonumber \\&Pr \Big (x^{1} \Big ) dx^{n-1} dx^{n-2},\ldots ,dx^{2} dx^{1}. \end{aligned}$$

(17)

Clearly, parameters $\frac{\rho }{2}$ (radius of ball B) and DistTH (restricted region around a sample) are embedded in above expressions. Using Eqs. 14–17, Eq. 13 can now be written as

$$\begin{aligned} Pr[(q_{s}, q_{g}) { FAILURE}] \le \bigg \lceil \frac{2L}{\rho } \bigg \rceil \Bigg (\prod _{k=1}^{n} Pr[I_{i}^{k}=0]\Bigg ) \end{aligned}$$

(18)

Fig. 24

[Case $DistTH > \frac{\rho }{2}$]—This figure shows that if $DistTH > \frac{\rho }{2}$ then the restricted regions of neighbouring samples may completely block the ball $B_{\rho /2}(q_{i})$ and will prevent generation of a sample inside it. This will lead to the failure of a planner that uses our localization aware sampling strategy

Note that $Pr(I_{i}^{n}=1 \mid x^{1},\ldots ,x^{n-1})$ denotes the probability of generating the $n^{th}$ sample inside ball $B_{\rho /2}(q_{i})$ given that $n-1$ samples have been placed. This is nothing but the ratio of volume of white region inside the black ball (after excluding the hatched region) and total volume of white region (with reference to Fig. 23). In general, this can be written as

$$\begin{aligned} Pr \Big (I_{i}^{n}=1 \mid x^{1},\ldots ,x^{n-1} \Big ) = \dfrac{\mu \Big (^{B}C_{free}^{R} \Big )}{\mu \Big (C_{free}^{R} \Big )}, \end{aligned}$$

(19)

where $C_{free}^{R}$ denotes the $C_{free}$ left after excluding the restricted regions around already placed samples and $^{B}C_{free}^{R}$ denotes the same but inside ball $B_{\rho /2}(q_{i})$. This ratio approaches one as more and more samples are placed outside the black ball. That implies that $Pr(I_{i}^{n}=0 \mid x^{1},\ldots ,x^{n-1})$ approaches zero. The expression in Eq. 17 is one of the product terms in RHS of inequality 18. Convergence of $Pr[I_{i}^{n}=0]$ (to 0) will lead to the convergence of RHS of inequality 18. Therefore, $Pr[(q_{s}, q_{g}) { FAILURE}]$ converges to 0 as the number of samples increases, hence showing the completeness of RRBT-LAS. The same completeness can not be guaranteed for $DistTH > \frac{\rho }{2}$ (see Fig. 24).