Optimal kinodynamic motion planning in environments with unexpected obstacles

This paper presents and analyzes a new algorithm, the Goal Tree (GT) algorithm, for motion planning in dynamic environments where new, unexpected obstacles appear sporadically. The GT builds on the RRT* algorithm by employing an initial RRT* tree rooted at the goal. When finding new obstacle information, O, the GT quickly constructs a new tree rooted at the current location of the robot, xI', by sampling in a strict subset of the free space. The new tree then reuses branches from the original tree so that it can produce paths to the goal. Compared to running the RRT*, the GT reduces, on average, the time needed to produce a path of equal cost. We prove that, generically, there exists a region, which is a strict subset of the free space, which can be used with the GT algorithm to produce an asymptotically globally optimal path. This region is theoretically characterized for planning problems in d dimensional environments. An alternative region is provided for robots with Dubins' vehicle dynamics and a vehicle with no dynamics both under a Euclidean distance cost function. Simulations for a Dubins' vehicle robot verify our results.