Numerical Approach to Reachability-Guided Sampling-Based Motion Planning Under Differential Constraints

Abstract

This paper presents a new method for motion planning under differential constraints by incorporating a numerically solved discretized representation of reachable state space for faster state sampling and nearest neighbor searching. The reachable state space is solved for offline and stored into a “reachable map” which can be efficiently applied in online planning. State sampling is performed only over states encompassed by the reachable map to reduce the number of unsuccessful motion validity checking queries. The nearest neighbor distance function is revised such that only reachable states are considered, with states which are unreachable or only reachable beyond a designated time horizon disregarded. This method is generalized for application to any control system, and thus can be used for vehicle models where analytical solutions cannot be found. Greater improvement is expected for more constrained systems where motion checking cost is relatively high. Simulation results are discussed for case studies on a holonomic model and a Dubins car model, both with maximum speed limitation and time included as a dimension in the configuration space, where planning speed (measured by tree growth rate) can be improved through reachability guidance in each system by at least a factor of 3 and 9, respectively.