Receding Horizon Control Using Graph Search for Multi-Agent Trajectory Planning

It is hard to find the global optimum of general nonlinear and nonconvex optimization problems in a reasonable time. This article presents a method to transfer the receding horizon control approach, where nonlinear, nonconvex optimization problems are considered, into graph-search problems. Specifically, systems with symmetries are considered to transfer system dynamics into a finite-state automaton. In contrast to traditional graph-search approaches where the search continues until the goal vertex is found, the transfer of a receding horizon control approach to graph-search problems presented in this article allows to solve them in real time. We prove that the solutions are recursively feasible by restricting the graph search to end in accepting states of the underlying finite-state automaton. The approach is applied to trajectory planning for multiple networked and autonomous vehicles. We evaluate its effectiveness in simulation and experiments in the Cyber-Physical Mobility Lab, an open-source platform for networked and autonomous vehicles. We show real-time capable trajectory planning with collision avoidance in experiments on off-the-shelf hardware and code in MATLAB for two vehicles.