Optimal, Smooth, Nonholonomic Mobile Robot Motion Planning in State Lattices

Abstract

We present an approach to the problem of mobile robot motion planning in arbitrary cost fields subject to differential constraints. Given a model of vehicle maneuverability, a trajectory generator solves the two point boundary value problem of connecting two points in state space with a feasible motion. We use this capacity to compute a control set which connects any state to its reachable neighbors in a limited neighborhood. Equivalence classes of paths are used to implement a path sampling policy which preserves expressiveness while eliminating redundancy. The implicit repetition of the resulting minimal control set throughout state space produces a reachability graph that encodes all feasible motions consistent with this sampling policy. The graph encodes only feasible motions by construction and, by appropriate choice of state space dimension, can permit full configuration space collision detection while imposing heading and curvature continuity constraints at nodes. Nonholonomic constraints are satisfied by construction in the trajectory generator. We also use the trajectory generator to compute an ideal admissible heuristic and significantly improve planning efficiency. Comparisons to classical grid search and nonholonomic motion planners show the planner provides better plans or provides them faster or both. Applications to planetary rovers and terrestrial unmanned ground vehicles are illustrated.