Iterative optimal feedback control design under relaxed rigidity constraints for multi-robot cooperative manipulation

Cooperative manipulation of multiple robots presents an interesting control application scenario of coupled dynamical systems with a common goal. Here, we treat the problem of moving a formation of physically interconnected robots to a desired goal while maintaining the formation. This control problen is for example relevant in cooperative transport of an object from an initial to a final configuration by mobile robotic manipulators. To achieve the control goal we formulate an LQR-like optimal control problem that, in addition to goal regulation and minimization of input energy, includes the formation rigidity constraint in a relaxed form expressed as a biquadratic penalty term. The control problem is solved by two different iterative algorithms, a gradient descent using adjoint states and a quasi-Newton method, that determine a static linear state-feedback matrix. The proposed control design and the iterative algorithms are validated and compared in numerical simulations showing the efficacy of both approaches.