A Unified Formulation and Nonconvex Optimization Method for Mixed-Type Decision-Making of Robotic Systems

Mixed-type decision-making is ubiquitously required in robotic systems and has attracted significant research interests. Examples include, but not limited to, the integrated task and motion planning and optimal control of hybrid systems involving both continuous and discrete dynamic behaviors. For decision-making of robotic systems to improve operational efficiency, safety, and/or mission success rate, they involve both discrete variables representing task allocation or transitions between discrete modes and continuous variables representing trajectories of the planned motion or states governed by differential equations. This paper formulates a class of mixed-type decision-making problems with polynomial objective and constraints as quadratically constrained quadratic programming (QCQP) problems and a nonconvex optimization method based on alternating direction method of multipliers is proposed to solve the QCQP. The proposed optimization method consists of three sequential subproblems, all of which admit closed-form solutions. Moreover, convergence proof of the optimization algorithm is provided. Two representative problems, traveling salesman with obstacle avoidance and rendezvous and docking of a charging station with distinct phase constraints, are described and solved via the proposed method. Numerical simulations as well as experimental verification of both problems are presented and compared with a state-of-art method to validate the effectiveness, efficacy and robustness of the nonconvex optimization method.