Abstract

It is hard to find the global optimum to general nonlinear, nonconvex optimization problems in reasonable time. This paper 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 paper allows to solve them in real-time. We proof 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 as well as in 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.