Energy-optimal motion planning for multiple robotic vehicles with collision avoidance

We propose a numerical algorithm for multiple-vehicle motion planning that explicitly takes into account the vehicle dynamics, temporal and spatial specifications, and energy-related requirements. As a motivating example, we consider the case where a group of vehicles is tasked to reach a number of target points at the same time (simultaneous arrival problem) without colliding among themselves and with obstacles, subject to the requirement that the overall energy required for vehicle motion be minimized. With the theoretical setup adopted, the vehicle dynamics are explicitly taken into account at the planning level. This paper formulates the problem of multiple-vehicle motion planning in a rigorous mathematical setting, describes the optimization algorithm used to solve it, and discusses the key implementation details. The efficacy of the method is illustrated through numerical examples for the simultaneous arrival problem. The initial guess to start the optimization procedure is obtained from simple geometrical considerations, e.g., by joining the desired initial and final positions of the vehicles via straight lines. Even though the initial trajectories thus obtained may result in intervehicle and vehicle/obstacle collisions, we show that the optimization procedure that we employ in this paper will generate collision-free trajectories that also minimize the overall energy spent by each vehicle and meet the required temporal and spatial constraints. The method developed applies to a very general class of vehicles; however, for clarity of exposition, we adopt as an illustrative example the case of wheeled robots.