Shared invariance control for constraint satisfaction in multi-robot systems

In systems involving multiple intelligent agents, e.g. multi-robot systems, the satisfaction of environmental, inter-agent, and task constraints is essential to ensure safe and successful task execution. This requires a constraint enforcing control scheme, which is able to allocate and distribute the required evasive control actions adequately among the agents, ideally according to the role of the agents or the importance of the executed tasks. In this work, we propose a shared invariance control scheme in combination with a suitable agent prioritization to control multiple agents safely and reliably. Based on the projection of the constraints into the input spaces of the individual agents using input-output linearization, shared invariance control determines constraint enforcing control inputs and facilitates implementation in a distributed manner. In order to allow for shared evasive actions, the control approach introduces weighting factors derived from a two-stage prioritization scheme, which allots the weights according to a variety of factors such as a fixed task priority, the number of constraints affecting each agent or a manipulability measure. The proposed control scheme is proven to guarantee constraint satisfaction. The approach is illustrated in simulations and an experimental evaluation on a dual-arm robotic platform.