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.
Similar content being viewed by others
References
Agha-mohammadi, A., Chakravorty, S., & Amato, N. (2014). FIRM: Sampling-based feedback motion planning under motion uncertainty and imperfect measurements. The International Journal of Robotics Research, 33(2), 268–304.
Bai, H., Hsu, D., & Lee, W. S. (2014). Integrated perception and planning in the continuous space: A POMDP approach. The International Journal of Robotics Research, 33(9), 1288–1302.
Bai, H., Cai, S., Ye, N., Hsu, D. & Lee, W.S. (2015). Intention-aware online POMDP planning for autonomous driving in a crowd. In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA) (pp. 454–460).
Bouilly, B., Simeon, T. & Alami, R. (1995). A numerical technique for planning motion strategies of a mobile robot in presence of uncertainty. In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA) (pp. 1327–1332).
Bry, A. & Roy, N. (2011). Rapidly-exploring random belief trees for motion planning under uncertainty. In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA) (pp. 723–730).
Choset, H., Lynch, K. M., Hutchinson, S., Kantor, G. A., Burgard, W., Kavraki, L. E., et al. (2005). Principles of robot motion: theory, algorithms, and implementations. Cambridge, MA: MIT Press.
Choset, H., Lynch, K.M., Hutchinson, S., Kantor, G.A., Burgard, W., Kavraki, L.E. & Thrun, S. (2005). Principles of Robot Motion: Theory, Algorithms, and Implementations. title, MIT Press, Cambridge, MA, chap 7, pp. 242–246.
Fox, D., Burgard, W., Dellaert, F. & Thrun, S. (1999). Monte carlo localization: Efficient position estimation for mobile robots. In Proceedings of the National Conference on Artificial Intelligence (pp. 343–349).
Fraichard, T. & Mermond, R. (1998). Path planning with uncertainty for car-like robots. In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA) (pp. 27–32).
Hsu, D., Latombe, J. C., & Kurniawati, H. (2006). On the probabilistic foundations of probabilistic roadmap planning. International Journal of Robotics Research (IJRR), 25(7), 627–643.
Huang, Y. & Gupta, K. (2008). RRT-SLAM for motion planning with motion and map uncertainty for robot exploration. In: Proceedings of the IEEE International Conference on Intelligent Robots and Systems (IROS) (pp. 22–26).
Huang, Y. & Gupta, K. (2009). Collision-probability constrained PRM for a manipulator with base pose uncertainty. In: Proceedings of the IEEE International Conference on Intelligent Robots and Systems (IROS) (pp. 1426–1432).
Kaelbling, L., Littman, M., & Cassandra, A. (1998). Planning and acting in partially observable stochastic domains. Artificial Intelligence, 101, 99–134.
Karaman, S. & Frazzoli, E. (2010). Incremental sampling-based optimal motion planning. In Proceedings of the Robotics: Science and Systems (RSS).
Karaman, S., & Frazzoli, E. (2011). Sampling-based algorithms for optimal motion planning. International Journal of Robotics Research (IJRR), 30(7), 846–894.
Karaman, S., Walter, M., Perez, A., Frazzoli, E., & Teller, S. (2011). Anytime motion planning using the RRT*. In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA).
Knepper, R. A., & Mason, M. T. (2012). Real-time informed path sampling for motion planning search. International Journal of Robotics Research (IJRR), 31(11), 1231–1250.
Kurniawati, H., Du, Y., Hsu, D. & Lee, W.S. (2009). Motion planning under uncertainty for robotic tasks with long time horizons. In Proceedings of the International Symposium on Robotics Research.
Kurniawati, H., Bandyopadhyay, T., & Patrikalakis, N. (2012). Global motion planning under uncertain motion, sensing, and environment map. Autonomous Robots, 33(3), 255–272.
Lambert, A. & Gruyer, D. (2003). Safe path planning in an uncertain-configuration space. In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA) (pp. 4185–4190), Roma, Italy.
LaValle, S. M. (2006). Planning algorithms. Cambridge, UK: Cambridge University Press.
Lazanas, A., & Latombe, J. C. (1995). Motion planning with uncertainty: A landmark approach. Artificial Intelligence, 76(1–2), 285–317.
Melchior N.A. & Simmons, R. (2007). Particle RRT for path planning with uncertainty. In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA) (pp. 1617–1624), Roma, Italy.
Missiuro, P.E. & Roy, N. (2006). Adapting probabilistic roadmaps to handle uncertain maps. In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA) (pp. 1261–1267).
Pineau, J., Gordon, G. & Thrun, S. (2003). Point-based value iteration: An anytime algorithm for POMDPs. In International Joint Conferences on Artificial Intelligence (pp. 1025–1032).
Prentice, S., & Roy, N. (2009). The belief roadmap: Efficient planning in belief space by factoring the covariance. The International Journal of Robotics Research, 28(11–12), 1448–1465.
Ribeiro, M. I. (2004). Gaussian probability density functions: Properties and error characterization. Institute for Systems and Robotics: Technical report.
Stachniss, C., Grisetti, G. & Burgard, W. (2005) Information gain-based exploration using rao-blackwellized particle filters. In Proceedings of the Robotics: Science and Systems (RSS) (pp. 65–72).
van den Berg, J., Abbeel, P., & Goldberg, K. (2011). LQG-MP: Optimized path planning for robots with motion uncertainty and imperfect state information. The International Journal of Robotics Research, 30(7), 895–913.
Wikipedia (2015). Kullback-Leibler divergence. https://en.wikipedia.org/wiki/Kullback-Leibler_divergence. Retrieved September 25, 2015.
Author information
Authors and Affiliations
Corresponding author
Appendix: Probabilistic completeness proof for RRBT-LAS
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})\)
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
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
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})\).
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
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
and for \(n^{th}\) sample the expression (for \(Pr[I_{i}^{n}=0]\)) is
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
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
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).
Rights and permissions
About this article
Cite this article
Pilania, V., Gupta, K. Localization aware sampling and connection strategies for incremental motion planning under uncertainty. Auton Robot 41, 111–132 (2017). https://doi.org/10.1007/s10514-015-9536-y
Received:
Accepted:
Published:
Issue Date:
DOI: https://doi.org/10.1007/s10514-015-9536-y