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

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.