Multiquery Motion Planning in Uncertain Spaces: Incremental Adaptive Randomized Roadmaps

Sampling-based motion planning is a powerful tool in solving the motion planning problem for a variety of different robotic platforms. As its application domains grow, more complicated planning problems arise that challenge the functionality of these planners. One of the main challenges in the implementation of a sampling-based planner is its weak performance when reacting to uncertainty in robot motion, obstacles motion, and sensing noise. In this paper, a multi-query sampling-based planner is presented based on the optimal probabilistic roadmaps algorithm that employs a hybrid sample classification and graph adjustment strategy to handle diverse types of planning uncertainty such as sensing noise, unknown static and dynamic obstacles and an inaccurate environment map in a discrete-time system. The proposed method starts by storing the collision-free generated samples in a matrix-grid structure. Using the resulting grid structure makes it computationally cheap to search and find samples in a specific region. As soon as the robot senses an obstacle during the execution of the initial plan, the occupied grid cells are detected, relevant samples are selected, and in-collision vertices are removed within the vision range of the robot. Furthermore, a second layer of nodes connected to the current direct neighbors are checked against collision, which gives the planner more time to react to uncertainty before getting too close to an obstacle. The simulation results for problems with various sources of uncertainty show a significant improvement compared with similar algorithms in terms of the failure rate, the processing time and the minimum distance from obstacles. The planner is also successfully implemented and tested on a TurtleBot in four different scenarios with uncertainty.