Hierarchical Task-Space Optimal Covariance Control With Chance Constraints

Abstract

This letter presents a new control paradigm applicable to nonlinear systems such as robots subject to chance and covariance assignment constraints which we refer to as hierarchical optimal covariance control. To the best of our knowledge, this is the first study to formulate the hierarchical optimal covariance control problem involving multiple operational tasks. The framework is defined as a multi-stage optimization problem considering multiple hierarchical tasks specified in lexicographic order. Towards this goal, we first approximate the nonlinear dynamic model of a robot into multiple linear stochastic systems by linearizing the model along given trajectories. We then project these stochastic models onto the null-space of the previous task models for efficiently solving lexicographical optimization. In addition, we specify probability functions to account for chance constraints using the Whittacker M function. We formulate the chance constraints as a positive semi-definite matrix constraint and solve the hierarchical optimal covariance control problem using sequential semi-definite programming. We demonstrate that this procedure yields higher accuracy for multiple hierarchical tasks than employing deterministic operational space control models.