Optimal Cooperative Maneuver Planning for Multiple Nonholonomic Robots in a Tiny Environment via Adaptive-Scaling Constrained Optimization

This letter is focused on the time-optimal Multi-Vehicle Trajectory Planning (MVTP) problem for multiple car-like robots when they travel in a tiny indoor scenario occupied by static obstacles. Herein, the complexity of the concerned MVTP task includes i) the non-convexity and narrowness of the environment, ii) the nonholonomy and nonlinearity of the vehicle kinematics, iii) the pursuit for a time-optimal solution, and iv) the absence of predefined homotopic routes for the vehicles. The aforementioned factors, when mixed together, are beyond the capability of the prevalent coupled or decoupled MVTP methods. This work proposes an adaptive-scaling constrained optimization (ASCO) approach, aiming to find the optimum of the nominally intractable MVTP problem in a decoupled way. Concretely, an iterative computation framework is built, wherein each intermediate subproblem contains only risky collision avoidance constraints within a certain range, thus being tractable in the scale. During the iteration, the constraint activation scale can change adaptively, thereby enabling to promote the convergence rate, to recover from an intermediate failure, and to get rid of a poor initial guess. ASCO is compared versus the state-of-the-art MVTP methods and is validated in real experiments conducted by a team of three car-like robots.