Communication-based Decentralized Cooperative Object Transportation Using Nonlinear Model Predictive Control

# Communication-based Decentralized Cooperative Object Transportation Using Nonlinear Model Predictive Control

Christos K. Verginis, Alexandros Nikou and Dimos V. Dimarogonas The authors are with the ACCESS Linnaeus Center, School of Electrical Engineering, KTH Royal Institute of Technology, SE-100 44, Stockholm, Sweden and with the KTH Center for Autonomous Systems. Email: {cverginis, anikou, dimos}@kth.se. This work was supported by the H2020 ERC Starting Grant BUCOPHSYS, the EU H2020 Co4Robots Project, the EU H2020 AEROWORKS project, the swedish Foundation for Strategic Research (SSF), the Swedish Research Council (VR) and the Knut och Alice Wallenberg Foundation (KAW).
###### Abstract

This paper addresses the problem of cooperative transportation of an object rigidly grasped by robotic agents. We propose a Nonlinear Model Predictive Control (NMPC) scheme that guarantees the navigation of the object to a desired pose in a bounded workspace with obstacles, while complying with certain input saturations of the agents. The control scheme is based on inter-agent communication and is decentralized in the sense that each agent calculates its own control signal. Moreover, the proposed methodology ensures that the agents do not collide with each other or with the workspace obstacles as well as that they do not pass through singular configurations. The feasibility and convergence analysis of the NMPC are explicitly provided. Finally, simulation results illustrate the validity and efficiency of the proposed method.

Multi-Agent Systems, Cooperative control, Decentralized Control, Manipulation, Object Transportation, Nonlinear Model Predictive Control, Collision Avoidance, Singularity Avoidance. Leader-follower Control

## I Introduction

Over the last years, multi-agent systems have gained a significant amount of attention, due to the advantages they offer with respect to single-agent setups. Robotic manipulation is a field where the multi-agent formulation can play a critical role, since a single robot might not be able to perform manipulation tasks that involve heavy payloads and challenging maneuvers.

Regarding cooperative manipulation, the literature is rich with works that employ control architectures where the robotic agents communicate and share information with each other as well as completely decentralized schemes, where each agent uses only local information or observers [1, 2, 3, 4, 5, 6]. The most common methodology used in the related literature constitutes of impedance and force/motion control [1, 7, 8, 9, 10, 11, 12, 13, 14]. Most of the aforementioned works employ force/torque sensors to acquire knowledge of the manipulator-object contact forces/torques, which, however, may result to performance decline due to sensor noise.

Moreover, in manipulation tasks, such as pose/force or trajectory tracking, collision with obstacles of the environment has been dealt with only by exploiting the potential extra degrees of freedom of over-actuated agents, or by using potential field-based algorithms. These methodologies, however, may suffer from local minima, even in single-agent cases, and in many cases they yield high control inputs that do not comply with the saturation of actual motor inputs, especially close to collision configurations. In our previous works, [15, 16], we considered the problem of trajectory tracking for decentralized robust cooperative manipulation, without taking into account singularity- or collision avoidance.

Another important property that concerns robotic manipulators is the singularities of the Jacobian matrix, which maps the joint velocities of the agent to a D vector of generalized velocities. Such singular kinematic configurations, that indicate directions towards which the agent cannot move, must be always avoided, especially when dealing with task-space control in the end-effector [17]. In the same vein, representation singularities can also occur in the mapping from coordinate rates to angular velocities of a rigid body.

The main contribution of this work is to provide decentralized feedback control laws that guarantee the cooperative manipulation of an object in a bounded workspace with obstacles. In particular, given agents that rigidly grasp an object, we design decentralized control inputs for the navigation of the object to a final pose, while avoiding inter-agent collisions as well as collisions with obstacles. Moreover, we take into account constraints that emanate from control input saturation as well kinematic and representation singularities. The proposed approach to address this problem is the repeated solution of a Finite-Horizon Open-loop Optimal Control Problem (FHOCP) of each agent, by assigning a set of priorities. Control approaches using this strategy are referred to as Nonlinear Model Predictive Control (NMPC) (see e.g. [18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30]). A decentralized NMPC scheme has been considered in our submitted work [31], which concerns multi-agent navigation with inter-agent connectivity maintenance and collision avoidance.

In our previous work [28], a similar problem was considered in a centralized way. However, the computation burden is high, due to the fact that the number of states in the centralized case increases proportionally with the number of agents, causing exponential increase in the computational time and memory. In this work, we decouple the dynamic model among the object and the agents by using certain load-sharing coefficients and consider a communication-based leader agent formulation, where a leader agent determines the followed trajectory for the object and the follower agents comply with it through appropriate constraints. To the best of the authors’ knowledge, this is the first time that the problem of decentralized object transportation with singularity, obstacle and collision avoidance is addressed.

The remainder of the paper is structured as follows. Section II provides preliminary background. The system dynamics and the formal problem statement are given in Section III. Section IV discusses the technical details of the solution and Section V is devoted to a simulation example. Conclusions and future work are discussed in Section VI.

## Ii Notation and Preliminaries

The set of positive integers is denoted as and the real -coordinate space, with , as ; and are the sets of real -vectors with all elements nonnegative and positive, respectively. The notation and , with , stands for positive semi-definite and positive definite matrices, respectively. The notation is used for the Euclidean norm of a vector and for the induced norm of a matrix . Given a real symmetric matrix , and denote the minimum and the maximum absolute value of eigenvalues of , respectively. Its minimum and maximum singular values are denoted by and respectively; and are the identity matrix and the matrix with all entries zeros, respectively. Given a set , we denote by its cardinality and by its -fold Cartesian product.

The vector connecting the origins of coordinate frames and } expressed in frame coordinates in -D space is denoted as . Given a vector is the skew-symmetric matrix defined according to . We further denote by the -- Euler angles representing the orientation of frame with respect to frame , where ; Moreover, is the rotation matrix associated with the same orientation and is the -D rotation group. The angular velocity of frame with respect to , expressed in frame coordinates, is denoted as and it holds that [17]. Define also the sets , . We define also the set

 Oz\coloneqqO(cz,β1,z,β2,z,β3,z)={p∈R3:(p−cz)⊤P(p−cz)≤1},

as the set of an ellipsoid in 3D, where is the center of the ellipsoid, the lengths of its three semi-axes and is an index term. The eigenvectors of matrix define the principal axes of the ellipsoid, and the eigenvalues of are: and . For notational brevity, when a coordinate frame corresponds to an inertial frame of reference , we will omit its explicit notation (e.g., ). Finally, all vector and matrix differentiations will be with respect to an inertial frame , unless otherwise stated.

###### Definition 1.

[32] A continuous function belongs to class if it is strictly increasing and . If and , then function belongs to class .

###### Lemma 1.

[33] Let be a continuous, positive-definite function, and be an absolutely continuous function in . Suppose that: , and . Then, it holds: .

## Iii Problem Formulation

The formulation we adopt in this paper follows from the one from our previous work [28]. Consider a bounded and convex workspace , where is the workspace radius, consisting of robotic agents rigidly grasping an object, as shown in Fig. 1, and static obstacles described by the ellipsoids . The free space is denoted as . The agents are considered to be fully actuated and they consist of a base that is able to move around the workspace (e.g., mobile or aerial vehicle) and a robotic arm. The reference frames corresponding to the -th end-effector and the object’s center of mass are denoted with and , respectively, whereas corresponds to an inertial reference frame. The rigidity of the grasps implies that the agents can exert any forces/torques along every direction to the object. We consider that each agent knows the position and velocity only of its own state as well as its own and the object’s geometric parameters. Moreover, no interaction force/torque measurements or on-line communication is required.

### Iii-a System model

#### Iii-A1 Robotic Agents

We denote by the joint space variables of agent , with , , where is the position and Euler-angle orientation of the agent’s base, and , are the degrees of freedom of the robotic arm. The overall joint space configuration vector is denoted as , with .

The linear and angular velocities of the agents’ base are described by the functions , with and , with , where is the representation Jacobian matrix, with:

 JBi(ηBi)=⎡⎢ ⎢⎣10sin(θBi)0cos(ϕBi)−cos(θBi)sin(ϕBi)0sin(ϕBi)cos(θBi)cos(ϕBi)⎤⎥ ⎥⎦.

We consider that each agent has access to its own state , and can compute, therefore the terms .

In addition, we denote as the position and Euler-angle orientation of agent ’s end-effector. More specifically, it holds that:

 pEi(qi) =pBi+RBi(ηBi)kpi(αi), ηEi(qi) =ηBi+kηi(αi),

where are the forward kinematics of the robotic arm [17], and is the rotation matrix of the agent ’s base.

Let also denote a function that represents the generalized velocity of agent ’s end-effector, with being the angular velocity. Then, can be computed as:

 vi(qi,˙qi) =[˙pEi(qi)ωi(qi,˙qi)] =⎡⎣˙pBi−S(RBi(ηBi)kpi(αi))ωBi(qi,˙qi)+RBi(ηBi)∂kpi(αi)∂αiωBi(qi,˙qi)+RBi(ηBi)JAi(qi)˙αi⎤⎦, (1)

where is the angular Jacobian of the robotic arm with respect to the agent’s base [17]. The differential kinematics (1) can be written as:

 vi(qi,˙qi)=Ji(qi)˙qi, (2)

where the Jacobian matrix is given by:

 Ji(qi)\coloneqq⎡⎣I3−S(RBi(ηBi)kpi(αi))JBi(ηBi)RBi(ηBi)∂kpi(αi)∂αi03×3JBi(ηBi)RBi(ηBi)JAi(qi)⎤⎦.
###### Remark 1.

Note that becomes singular at representation singularities 111Other representations, such as rotation matrices or unit quaternions, do not exhibit such singularities. The prevent, however, global stabilization by continuous controllers due to topological obstructions., when and becomes singular at kinematic singularities defined by the set

 Qi={qi∈Rni:det(Ji(qi)[Ji(qi)]⊤)=0},i∈N.

In the following, we will aim at guaranteeing that will always be in the closed set:

 ˜Qi={qi∈Rni:|det(Ji(qi)[Ji(qi)]⊤)|≥ε>0},i∈N,

for a small positive constant .

The joint-space dynamics for agent can be computed using the Lagrangian formulation:

 Bi(qi)¨qi+Ni(qi,˙qi)˙qi+gqi(qi)=τi−[Ji(qi)]⊤λi, (3)

where is the joint-space positive definite inertia matrix, represents the joint-space Coriolis matrix, is the joint-space gravity vector, is the generalized force vector that agent exerts on the object and is the vector of generalized joint-space inputs, with , where is the generalized force vector on the center of mass of the agent’s base and is the torque inputs of the robotic arms’ joints. By differentiating (2) we obtain:

 ˙vi(qi,˙qi)=Ji(qi)¨qi+˙Ji(qi)˙qi. (4)

By inverting (3) and using (2) and (4), we can obtain the task-space agent dynamics [17]:

 Mi(qi)vdi(qi,˙qi,¨qi)+Ci(qi,˙qi)vi(qi,˙qi)+gi(qi)=ui−λi, (5)

with the corresponding task-space terms , , :

 Mi(qi) =[Ji(qi)B−1i(qi)J⊤i(qi)]−1, Ci(qi,˙qi)Ji(qi)˙qi =Mi(qi)[Ji(qi)B−1i(qi)Ni−˙Ji(qi)]˙qi, gi(qi) =Mi(qi)Ji(qi)B−1i(qi)gqi(qi).

The task-space input wrench can be translated to the joint space inputs via

 τi=[Ji(qi)]⊤ui+¯τi(qi), (6)

where belongs to the nullspace of and concerns over-actuated agents (see [17]). The term concerns over-actuated agents and does not contribute to end-effector forces.

We define by , the union of the ellipsoids that bound the -th agent’s volume, i.e., which is essentially the union of the ellipsoids that bound the volume of the agents’ links.

#### Iii-A2 Object Dynamics

Regarding the object, we denote its state as , , representing the pose and velocity of the object’s center of mass, with , , . The second order Newton-Euler dynamics of the object are given by:

 ˙xO =[JOr(ηO)]−1vO, (7a) λO =MO(xO)˙vO+CO(xO,vO)vO+gO(xO), (7b)

where is the positive definite inertia matrix, is the Coriolis matrix, and is the gravity vector. In addition, is the object representation Jacobian , with

which is singular when . Finally, is the force vector acting on the object’s center of mass. Also, similarly to the robotic agents, we define by the bounding ellipsoid of the object.

#### Iii-A3 Coupled Dynamics

Consider robotic agents rigidly grasping an object. Then, the coupled system object-agents behaves like a closed-chain robot and we can express the object’s pose and velocity as a function of and , . Hence, In view of Fig. 1, we conclude that:

 pO=pOi(qi) \coloneqqpEi(qi)+pO/Ei(qi) \coloneqqpEi(qi)+REi(qi)pEiO/Ei, (8a) ηO=ηOi(qi) =ηEi(qi)+ηO/Ei, (8b)

for every , where are local functions of the agents that provide the object’s pose, represents the constant distance and the relative orientation offset between the th agent’s end-effector and the object’s center of mass, which are considered known. The grasp rigidity implies that , . Therefore, by differentiating (8a), we can also express as a function of as

 vO=vOi(qi,˙qi)\coloneqqJiO(qi)vi(qi,˙qi), (9)

from which, we obtain:

 ˙vOi(qi,˙qi)=JiO(qi)vdi(qi,˙qi,¨qi)+˙JiO(qi)vi(qi,˙qi), (10)

where is a smooth mapping representing the Jacobian from the object to the -th agent:

 JiO(qi)=[I3S(pEi/O(qi))03×3I3], (11)

and is always full rank due to the grasp rigidity.

###### Remark 2.

Since the geometric object parameters and are known, each agent can compute and by (8) and (9), respectively, without employing any sensory data. In the same vein, all agents can also compute the object’s bounding ellipsoid .

The Kineto-statics duality [17] along with the grasp rigidity suggest that the force acting on the object center of mass and the generalized forces , exerted by the agents at the contact points are related through:

 λO=[G(q)]⊤λ, (12)

where and is the grasp matrix, with and the matrix inverse of , .

Consider now the constants , with and , that play the role of load sharing coefficients for the agents. Then (7b) can be written as:

 ∑i∈Nci{MO(xOi(qi))vdOi(qi,˙qi,¨qi)+gO(xOi(qi))CO(xOi(qi),vOi(qi,˙qi))vOi(qi,˙qi)} =∑i∈N[JOi(qi)]⊤λi,

from which, by employing (12), (2), (4), (9) and (10), and after straightforward algebraic manipulations, we obtain the coupled dynamics:

 ∑i∈N{˜Mi(qi)¨qi+˜Ci(qi,˙qi)˙qi+˜gi(qi)}=∑i∈N[JOi(qi)]⊤ui, (13)

where:

 ˜Mi(qi) \coloneqqciMO(xOi(qi))JiO(qi)Ji(qi)+[JOi(qi)]⊤Mi(qi)Ji(qi), ˜Ci(qi,˙qi) \coloneqq[JOi(qi)]⊤(Mi(qi)˙Ji(qi)+Ci(qi,˙qi)Ji(qi))+ciMO(xOi(qi))JiO(qi)˙Ji(qi), +ciMO(xOi(qi))˙JiO(qi)Ji(qi)+ciCO(xOi(qi),vOi(qi,˙qi)), ˜gi(qi) \coloneqqcigO(xOi(qi))+[JOi(qi)]⊤gi(qi),

where and .

###### Remark 3.

Note that the agents dynamics under consideration hold for generic robotic agents comprising of a moving base and a robotic arm. Hence, the considered framework can be applied for mobile, aerial, or underwater manipulators.

###### Problem 1.

Consider robotic agents, rigidly grasping an object, governed by the coupled dynamics (13). Given a desired pose for the object, design the control inputs such that , while ensuring the satisfaction of the following collision avoidance and singularity properties:

1. ,

2. ,

3. ,

4. ,

5. , ,

6. , ,

, for , as well as the velocity and input constraints: , for some positive constants .

Regarding the joint velocity constraints, we impose for the arm joint velocities the constraint , which maximizes the manipulability ellipsoid of the arms [17], and hence increases robotic manipulability. Specifications in the aforementioned problem stand for collision avoidance between the agents, the objects, and the workspace obstacles and specifications stand for representation and kinematic singularities.

In order to solve the aforementioned problem, we need the following assumption regarding the workspace:

###### Assumption 1.

(Problem Feasibility Assumption) The set , is connected.

###### Assumption 2.

(Sensing and communication capabilities) Each agent is able to continuously measure the other agents’ state , . Moreover, each agent is able to communicate with the other agents without any delays.

Note that the aforementioned sensing assumption is reasonable, since in cooperative manipulation tasks, the agents are sufficiently close to each other, and therefore potential sensing radii formed by realistic sensors are large enough to cover them. Moreover, each agent can construct at every time instant the set-valued functions , , whose structure can be transmitted off-line to all agents. Let us define also the sets:

 Si,O \coloneqq{qi∈Rni:Ai(qi)∩Oz≠∅,∀z∈Z}, Si,A \coloneqq{q∈Rn:Ai(qi)∩Aj(qℓ)≠∅,∀ℓ∈N∖{i}},
 ˜Si,A([qℓ]ℓ∈N∖{i}) \coloneqq{qi∈Rni:q∈Si,A} SOi \coloneqq{qi∈Rni:CO(xOi(qi))∩Oz≠∅,∀z∈Z},

for every , associated with the desired collision-avoidance properties, where the notation stands for the stack vector of all , . Moreover, define the projection sets for agent as the set-valued functions:

 ˜Si,A([qℓ]ℓ∈N∖{i})\coloneqq{qi∈Rni:q∈Si,A},i∈N,

where the notation stands for the stack vector of all .

## Iv Main Results

In this section, a systematic solution to Problem 1 is introduced. Our overall approach builds on designing a NMPC scheme for the system of the manipulators and the object. The proposed methodology is decentralized, since we do not consider a centralized system that calculates all the control signals and transmits them to the agents, like in our previous work [28]. As expected, this relaxes greatly the computational burden of the NMPC approach, which is also verified by the simulation results. To achieve that, we employ a leader-follower perspective. More specifically, as will be explained in the sequel, at each sampling time, a leader agent solves part of the coupled dynamics (13) via an NMPC scheme, and transmits its predicted variables to the rest of the agents. Assume, without loss of generality, that the leader corresponds to agent . Loosely speaking, the proposed solution proceeds as follows: agent solves, at each sampling time step, the receding horizon model predictive control subject to the forward dynamics:

 ˜M1(q1)¨q1+˜C1(q1,˙q1)˙q1+˜g(q1)=[JO1(q1)]⊤u1, (14)

and a number of inequality constraints, as will be clarified later. After obtaining a control input sequence and a set of predicted variables for , denoted as , it transmits the corresponding predicted state for the object for the control horizon to the other agents . Then, the followers solve the receding horizon NMPC subject to the forward dynamics:

 ˜Mi(qi)¨qi+˜Ci(qi,˙qi)˙qi+˜g(qi)=[JOi(qi)]⊤ui, (15)

and the state equality constraints:

 xOi(qi)=xO1(^q1),vOi(qi,˙qi)=vO1(^q1,^˙q1), (16)

as well as a number of inequality constraints that incorporate obstacle and inter-agent collision avoidance. More specifically, we consider that there is a priority sequence among the agents, which we assume, without loss of generality, that is defined by , and can be transmitted off-line to the agents. Each agent, after solving its optimization problem, transmits its calculated predicted variables to the agents of lower priority, which take them into account for collision avoidance. Note that the coupled object-agent dynamics are implicitly taken into account in equations (14), (15) in the following sense. Although the coupled model (13) does not imply that each one of these equations is satisfied, by forcing each agent to comply with the specific dynamics through the optimization procedure, we guarantee that (13) is satisfied, since it’s the result of the addition of (14) and (15), for every and , respectively. Intuitively, the leader agent is the one that determines the path that the object will navigate through, and the rest of the agents are the followers that contribute to the transportation. Moreover, the equality constraints (16) guarantee that the predicted variables of the agents will comply with the rigidity at the grasping points through the equality constraints (16).

By using the notation , , the nonlinear dynamics of each agent can be written as:

 ˙xi=˜fi(xi,ui)\coloneqq[˜fi1(xi)˜fi2(xi,ui)], (17)

where