Towards Integrated Planning and Control of Autonomous Vehicles Using Nested MPCs

This paper presents a strategy to integrate the planning and control for autonomous vehicles. The aim of this work is to provide a method that can yield controller feasible reference paths, i.e. paths that are not only dynamically feasible but are feasible under the action of a low-level feedback controller. The method is designed to find a control feasible parameterization of a collision-free path provided by a path generation scheme, e.g. rapidly-exploring random trees or one of its many variants. This parameterization is found such that the vehicle under the action of the low-level controller will be able to follow that path within a specified tolerance. The design is based on a feedback strategy with nested MPCs for planning and control. The results presented here are preliminary but hint at the benefits of such a strategy and suggest avenues for future work.