Cooperating Modular Goal Selection and Motion Planning for Autonomous Driving

We present a decision making approach for autonomous driving that concurrently determines the driving mode and the motion plan that achieves the driving mode goal. To do this, we develop two cooperating modules: a mode activator and a motion planner. Based on the current mode in a non-deterministic automaton, the mode activator determines all the feasible next modes, i.e., the modes for which there exists a trajectory that reaches the associated goal. Then, the motion planner generates trajectories achieving the goals of such feasible modes, selects the next mode and trajectory that result in the best performance, and updates the current mode in the automaton. To determine the feasibility, the mode activator uses robust forward and backward reachability that accounts for the discrepancy between the simplified model used in the reachability computation and the more precise model used by the motion planner. We prove that, under normal operation, the mode activator always returns a nonempty set of feasible modes, so that the decision making algorithm is recursively feasible. We validate the algorithm in simulations and experiments using car-like laboratory-scale robots.