1

A Nonlinear Model Predictive Control Scheme
for Cooperative Manipulation with Singularity
and Collision Avoidance
arXiv:1705.01426v2 [cs.RO] 12 Nov 2017

Alexandros Nikou, Christos Verginis, Shahab Heshmati-alamdari and Dimos V.
Dimarogonas

Abstract
This paper addresses the problem of cooperative transportation of an object rigidly grasped by N
robotic agents. In particular, 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. 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.

Index Terms
Multi-Agent Systems, Cooperative control, Cooperative Manipulation, Nonlinear Model Predictive
Control, Collision Avoidance.
Alexandros Nikou, Christos Verginis and Dimos V. Dimarogonas 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: {anikou, cverginis, dimos}@kth.se. Shahab Heshmati-alamdari is with the Control Systems
Lab, Department of Mechanical Engineering, National Technical University of Athens, 9 Heroon Polytechniou Street, Zografou
15780, Athens, Greece. Email: {shahab}@mail.ntua.gr. This work was supported by the H2020 ERC Starting Grant
BUCOPHSYS, the EU H2020 AEROWORKS project, the EU H2020 Co4Robots project, the Swedish Foundation for Strategic
Research (SSF), the Swedish Research Council (VR) and the Knut och Alice Wallenberg Foundation (KAW).

November 15, 2017

DRAFT

2

I. I NTRODUCTION
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. In the case of robotic manipulation and object transportation, difficult tasks involving heavy payloads as well as challenging
maneuvers necessitate the employment of multiple robots. Fig. 1 depicts a system of two robotic
mobile manipulators (KUKA youBots), each comprising of a moving base and a robotic arm of
5 Degrees of Freedom (DOF).
Early works related to cooperative manipulation develop 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, avoiding potential communication delays [1]–[7]. Impedance and force/motion control constitutes the most common
methodology used in the related literature [1], [8]–[16]. However, 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 or mounting difficulties. Recent technological advances allow to manipulator grippers to grasp rigidly
certain objects (see e.g., [17]), which, as shown in this work, can render the use of force/torque
sensors unnecessary.
Furthermore, in manipulation tasks, such as pose/force or trajectory tracking, collision with
obstacles of the environment has been dealt with only by exploiting the extra degrees of freedom
that appear in over-actuated robotic agents. Potential field-based algorithms may suffer from local
minima and navigation functions [18] cannot be extended to multi-agent second order dynamical
systems in a trivial way. Moreover, these methods usually result in high control input values
near obstacles that need to be avoided, which might conflict the saturation of the actual motor
inputs.
Another important property that concerns robotic manipulators is the singularities of the
Jacobian matrix, which maps the joint velocities of the agent to a 6D 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 [19]. In the same vein, representation singularities can also occur in the mapping
from coordinate rates to angular velocities of a rigid body.
In this work, we aim to address the problem of cooperative manipulation of an object in a

November 15, 2017

DRAFT

3

Fig. 1. Two ground vehicles (KUKA youBots) consisting of a moving base and a attached manipulator with 5 DOF.

bounded workspace with obstacles. In particular, given N agents that rigidly grasp an object, we
design 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.
For the design of a stabilizing feedback control law for each robot, such that the desired
specifications are met, while satisfying constraints on the controls and the states, one would
ideally look for a closed loop solution for the feedback law satisfying the constraints while
optimizing the performance. However, typically the optimal feedback law cannot be found
analytically, even in the unconstrained case, since it involves the solution of the corresponding
Hamilton-Jacobi-Bellman partial differential equations. One approach to circumvent this problem
is the repeated solution of an open-loop optimal control problem for a given state. The first part
of the resulting open-loop input signal is implemented and the whole process is repeated. Control
approaches using this strategy are referred to as Nonlinear Model Predictive Control (NMPC) (see
e.g. [20]–[34]) which we aim to use in this work for the problem of the constraint cooperative
manipulation of an object which is rigidly grasped by N agents. To the best of the authors’
knowledge, this problem has not been addressed in the related literature.
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. Finally, conclusions and future work are discussed in Section VI.

November 15, 2017

DRAFT

4

II. N OTATION AND P RELIMINARIES
The set of positive integers is denoted as N and the real n-coordinate space, with n ∈ N,
as Rn ; Rn≥0 and Rn>0 are the sets of real n-vectors with all elements nonnegative and positive,
n×n
respectively. The notation Rn×n
≥0 and R>0 , with n ∈ N, stands for positive semi-definite and

positive definite matrices, respectively. Moreover, kxk is the Euclidean norm of a vector x ∈ Rn .
Given a set S, we denote by |S| its cardinality and by S N = S × · · · × S its N -fold Cartesian
product. Given the sets S1 , S2 , the set difference and the Minkowski addition are denoted by \, ⊕,
respectively, and are defined by S1 \S2 = {s : s ∈ S1 and s2 ∈
/ S2 } and S1 ⊕ S2 = {s1 + s2 : s1 ∈
S1 , s2 ∈ S2 }, respectively. The n × n identity matrix and the n × m matrix with zero entries, are
denoted by In , 0n×m and 1n , respectively, with n, m ∈ N. The largest singular value of matrix
A ∈ Rn×m is denoted as σmax (A).
The vector connecting the origins of coordinate frames {A} and {B} expressed in frame {C}
coordinates in 3-D space is denoted as pCB/A = [xB/A , yB/A , zB/A ]> ∈ R3 . Given a ∈ R3 , S(a) is
the skew-symmetric matrix defined according to S(a)b = a × b. We further denote as ηA/B =
[φA/B , θA/B , ψA/B ]> ∈ T3 ⊆ R3 the x-y-z Euler angles representing the orientation of frame {A}
with respect to frame {B}, with φA/B , ψA/B ∈ [−π, π] and θA/B ∈ [− π2 , π2 ], where T3 is the
3-D torus; Moreover, RAB ∈ SO(3) is the rotation matrix associated with the same orientation
and SO(3) is the 3-D rotation group. The angular velocity of frame {B} with respect to {A},
C
A
expressed in frame {C} coordinates, is denoted as ωB/A
∈ R3 and it holds that ṘAB = S(ωB/A
)RAB .

We further define the sets M = R3 × T3 , N = {1, . . . , N }. We define also the set
Oz , O(cz , β1,z , β2,z , β3,z )

= p ∈ R3 : (p − cz )> P (p − cz ) ≤ 1 ,
as the set of an ellipsoid in 3D, where cz ∈ R3 is the center of the ellipsoid, β1,z , β2,z , β3,z ∈ R>0
the lengths of its three semi-axes and z ≥ 1 is an index term. The eigenvector of matrix P ∈ R3
−2
−2
−2
and β3,z
. For
define the principal axes of the ellipsoid, and the eigenvalues of P are: β1,z
, β2,z

notational brevity, when a coordinate frame corresponds to an inertial frame of reference {I},
I
we will omit its explicit notation (e.g., pB = pIB/I , ωB = ωB/I
, RA = RAI , etc.). Finally, all vector

and matrix differentiations will be with respect to an inertial frame {I}, unless otherwise stated.
Definition 1. ([35]) A continuous function f : [0, α] → R≥0 , α ∈ R>0 is said to belong to class
K, if is strictly increasing and f (0) = 0.

November 15, 2017

DRAFT

5

Fig. 2. Two robotic arms rigidly grasping an object with the corresponding frames.

Lemma 1. ([36]) Let γ be a continuous, positive definite function and x be an absolutely
continuous function on R. If the following holds:
•
•

kx(·)k
Z t< ∞, kẋ(·)k < ∞,
lim
γ(x(s))ds < ∞.
t→∞

0

Then, it holds that: limt→∞ kx(t)k = 0.
III. P ROBLEM F ORMULATION
Consider a bounded and convex workspace W ⊆ R3 consisting of N robotic agents rigidly
grasping an object, as shown in Fig. 2, and Z obstacles described by the ellipsoids Oz , z ∈ Z =
S
{1, . . . , Z}. The free space is denoted as Wfree = W\ z∈Z Oz . 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 i-th end-effector
and the object’s center of mass are denoted with {Ei } and {O}, respectively, whereas {I}
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 i 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.
A. System model
1) Robotic Agents: We denote by qi : R≥0 → Rni the joint space variables of agent i ∈ N ,
>
>
>
>
with ni = nαi +6, qi (t) = [p>
: R≥0 → R3 , ηBi =
Bi (t), ηBi (t), αi (t)] , where pBi = [xBi , yBi , zBi ]

[φBi , θBi , ψBi ]> : R≥0 → T3 ⊆ R3 is the position and Euler-angle orientation of the agent’s base,

November 15, 2017

DRAFT

6

and αi : R≥0 → Rnαi , nαi > 0, are the degrees of freedom of the robotic arm. The overall
P
> >
joint space configuration vector is denoted as q = [q1> , . . . , qN
] ∈ Rn , with n = i∈N ni . In
addition, we denote as pEi : Rni → R3 , ηEi : Rni → T3 ⊆ R3 the position and Euler-angle
orientation of agent i’s end-effector. Let also vi : Rni × Rni → R6 denote the velocity of agent
> >
ni
i’s end-effector, with vi (qi , q̇i ) = [ṗ>
× Rni → R3 are the linear
Ei , ωEi ] , whereas ṗBi , ωBi : R

and angular velocity, respectively, of the agent’s base.
B

B

We consider that each agent i ∈ N has access to its own state qi as well as ṗBii , ωBii , and α̇i
B

B

via on-board sensors. Then, ṗBi , ωBi can be obtained via ṗBi = RBi (ηBi )ṗBii , ωBi = RBi (ηBi )ωBii ,
where RBi : T3 → SO(3) is the rotation matrix of the agent i’s base. Moreover, η̇Bi is related
to ωBi via ωBi = JBi (ηBi )η̇Bi , where JBi : T3 → R3×3 , with


0
sin(θBi )
1



JBi (ηBi ) = 0 cos(φBi ) − cos(θBi ) sin(φBi ) .


0 sin(φBi ) cos(θBi ) cos(φBi )
The pose of the ith end-effector can be computed via
pEi (qi ) = pBi + RBi (ηBi )kpi (αi ),
ηEi (qi ) = kηi (ηBi , αi ),
where kpi : Rnαi → R3 , kηi : T3 × Rnαi → T3 are the forward kinematics of the robotic arm
[19]. Then, vi can be computed as




ṗE (qi , q̇i )

vi (qi , q̇i ) =  i
ωEi (qi , q̇i )


∂kpi
ṗB − S(RBi kpi )ωBi + RBi ∂αi
,
= i
ωBi + RBi JAi α̇i

(1)

where JAi : Rnαi → R3×nαi is the angular Jacobian of the robotic arm with respect to the agent’s
base. The differential kinematics (1) can be written as


ṗE (qi , q̇i )
 = Ji (qi )q̇i ,
vi (qi , q̇i ) =  i
ωEi (qi , q̇i )

(2)

where Ji : Rni → R6×ni is the agent Jacobian matrix, with


∂kpi (αi )
I3 −S(RBi (ηBi )kpi (αi ))JBi (ηBi ) RBi (ηBi ) ∂αi
.
Ji (qi ) = 
03×3
JBi (ηBi )
RBi (ηBi )JAi (qi )

November 15, 2017

DRAFT

7

Remark 1. Note that JBi becomes singular at representation singularities, when θBi = ± π2 and
Ji becomes singular at kinematic singularities defined by the set
Qi = {qi ∈ Rni : det(Ji> Ji ) = 0}, i ∈ N .
In the following, we will aim at guaranteeing that qi will always be in the closed set:
ei = {qi ∈ Rni : |det(J > Ji )| ≥ ε > 0}, i ∈ N ,
Q
i
for a small positive constant ε.
The joint-space dynamics for agent i ∈ N can be computed using the Lagrangian formulation:
Bi (qi )q̈i + Ni (qi , q̇i )q̇i + gqi (qi ) = τi − Ji> λi ,

(3)

where Bi : Rni → Rni ×ni is the joint-space positive definite inertia matrix, Ni : Rni × Rni →
Rni ×ni represents the joint-space Coriolis matrix, gqi : Rni → Rni is the joint-space gravity
vector, λi ∈ R6 is the generalized force vector that agent i exerts on the object and τi ∈ Rni is
> >
>
> >
∈ R6
the vector of generalized joint-space inputs, with τi = [λ>
Bi , ταi ] , where λBi = [fBi , µBi ]

is the generalized force vector on the center of mass of the agent’s base and ταi ∈ Rnαi is the
torque inputs of the robotic arms’ joints. By inverting (3) and using (2) and its derivative, we
can obtain the task-space agent dynamics [19]:
Mi (qi )v̇i + Ci (qi , q̇i )vi + gi (qi ) = ui − λi ,

(4)

with the corresponding task-space terms:

−1
Mi (qi ) = Ji (qi )Bi−1 (qi )Ji> (qi )
,
h
i
Ci (qi , q̇i )Ji (qi )q̇i = Mi (qi ) Ji (qi )Bi−1 (qi )Ni − J˙i (qi ) q̇i ,
gi (qi ) = Mi (qi )Ji (qi )Bi−1 (qi )gqi (qi ).
The task-space input wrench ui can be translated to the joint space inputs τi ∈ Rni via
τi = Ji> (qi )ui + (Ini − Ji> (qi )J¯i> (qi ))τi0 , where J¯i is a generalized inverse of Ji [19]. The term
τi0 concerns over-actuated agents and does not contribute to end-effector forces.
We define by Ai (qi ) , Oi , i ∈ N , the ellipsoid that bounds the i th agent’s volume with the
corresponding centers ci and semi-axes βi,1 , βi,2 , βi,3 , i.e., the workspace of the arm of agent i
[19] enlarged so that it includes the ith base. Note that Ai depends on qi and can be explicitly
found.

November 15, 2017

DRAFT

8

2) Object Dynamics: Regarding the object, we denote as xO : R≥0 → M, vO : R≥0 → R6
>
>
the pose and velocity of the object’s center of mass, with xO (t) = [p>
O (t), ηO (t)] , pO (t) =
>
>
[xO (t), yO (t), zO (t)]> , ηO (t) = [φO (t), θO (t), ψO (t)]> and vO (t) = [ṗ>
O (t), ωO (t)] . The second

order dynamics of the object are given by:
(xO )vO (t),
ẋO (t) = JO−1
r

(5a)

λO = MO (xO )v̇O (t) + CO (xO , vO )vO (t) + gO (xO ),

(5b)

where MO : M → R6×6 is the positive definite inertia matrix, CO : M×R6 → R6×6 is the Coriolis
matrix, gO : M → R6 is the gravity vector, which are derived from the Newton-Euler formulation.
In addition, JOr : M → R6×6 is the object representation Jacobian JOr (xO ) = diag{I3 , JOr,θ (xO )},
with



0
sin(θO )
1



JOr,θ (xO ) = 0 cos(φO ) − cos(θO ) sin(φO ) ,


0 sin(φO ) cos(θO ) cos(φO )

which is singular when θO = ± π2 . Finally, λO ∈ R6 is the force vector acting on the object’s
center of mass. Also, similarly to the robotic agents, we define by CO (xO ) , OO , as the bounding
ellipsoid of the object.
3) Coupled Dynamics: Consider N 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 qi and q̇i , ∀i ∈ N . In view of Fig. 2, we have that
pEi (qi (t)) = pO (t) + pEi /O (qi )
E

= pO (t) + REi (t)pEii /O ,
ηEi (qi (t)) = ηO (t) + ηEi /O ,

(6a)
(6b)

E

∀i ∈ N , where pEii /O represents the constant distance and ηEi /O the relative orientation offset
between the ith agent’s end-effector and the object’s center of mass, which are considered known.
The grasp rigidity implies that ωEi = ωO , ∀i ∈ N . Therefore, by differentiating (6a), we obtain
vi (qi , q̇i (t)) = JOi (qi )vO (t),

(7)

which, by time differentiation, yields
v̇i (t) = JOi (qi )v̇O (t) + J˙Oi (qi )vO (t),

November 15, 2017

(8)

DRAFT

9

where JOi : Rn → R6×6 is a smooth mapping representing the Jacobian from the object to the
i-th agent:

JOi (qi ) = 

I3

S(pO/Ei (qi ))

03×3

I3


,

and is always full rank due to the grasp rigidity.
E

Remark 2. Since the geometric object parameters pEii /O and ηEi /O are known, each agent can
compute pO , ηO and vO simply by inverting (6) and (7), respectively, without employing any
sensory data. In the same vein, all agents can also compute the object’s bounding ellipsoid CO ,
which depends on q.
The Kineto-statics duality [19] along with the grasp rigidity suggest that the force λO acting
on the object center of mass and the generalized forces λi , i ∈ N , exerted by the agents at the
contact points are related through
λO = G> (q)λ̄,

(9)

> >
where λ̄ = [λ>
∈ R6N and G : Rn → R6N ×6 is the grasp matrix, with G(q) =
1 , · · · , λN ]

[JO>1 , · · · , JO>N ]> .
Next, we substitute (7) and (8) in (4) and we obtain in vector form after rearranging terms:
λ̄ = u − M̄ (q)G(q)v̇O − (M̄ (q)Ġ(q, q̇) + C̄(q, q̇)G(q))vO − ḡ(q),

(10)

> >
where we have used the stack forms M̄ = diag{[Mi ]i∈N }, C̄ = diag{[Ci ]i∈N }, ḡ = [g1> , . . . , gN
] ,
> >
and u = [u>
1 , . . . , uN ] . By substituting (10) and (5) in (9) and by noticing from (6) that xO

depends on q owing to the grasp rigidity, we obtain the coupled dynamics:
f(q)v̇O + C(q,
e q̇)vO + ge(q) = G> (q)u,
M

(11)

where:
f(q) = MO (q) + G> (q)M̄ (q)G(q),
M
e q̇) = CO (q) + G> (q)M̄ (q)Ġ(q, q̇) + G> (q)C̄(q)G(q),
C(q,
ge(q) = gO (q) + G> (q)ḡ(q),

(12a)
(12b)
(12c)

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.

November 15, 2017

DRAFT

10

We can now formulate the problem considered in this work:
Problem 1. Consider N robotic agents rigidly grasping an object, governed by the coupled
dynamics (11). Given the desired pose xO,des , design the control input u : R≥0 → R6N such
that lim xO (t) = xO,des , while ensuring the satisfaction of the following collision avoidance and
t→∞

singularity properties:
1) Ai (qi ) ∩ Oz = ∅, ∀i ∈ N , z ∈ Z,
2) CO (xO ) ∩ Oz = ∅, ∀z ∈ Z,
3) Ai (qi ) ∩ Aj (qj ) = ∅, ∀i, j ∈ N , i 6= j,
4) − π2 < −θ̄ ≤ θO ≤ θ̄ < π2 ,
5) − π2 < −θ̄ ≤ θBi ≤ θ̄ < π2 ,
ei .
6) qi ∈ Q
for a 0 < θ̄ <

π
,
2

as well as the input and velocity magnitude and input constraints: |τik | ≤

τ̄i , |q̇ik | ≤ q̇¯i , ∀k ∈ {1, . . . , ni }, i ∈ N , for some positive constants τ̄i , q̇¯i , i ∈ N .
The aforementioned constraints correspond to the following specifications:
•

1) stands for collision avoidance between the agents and the obstacles.

•

2) stands for collision avoidance between the object and the obstacles.

•

3) stands for collision avoidance between the agents.

•

4) stands for representation singularity avoidance of the object.

•

5) stands for representation singularity avoidance of the agents’ bases.

•

6) stands for kinematic singularity avoidance of the agents.

In order to solve the aforementioned problem, we need the following reasonable assumption
regarding the workspace:
Assumption 1. (Problem Feasibility Assumption) The distance between any pair of obstacles is
sufficiently large such that the coupled system object-agents can navigate among them without
collisions.
We also define the following sets for every i ∈ N :
Si,O (q) = {qi ∈ Rni : Ai (qi ) ∩ Oz 6= ∅, ∀z ∈ Z},
Si,A (q) = {qi ∈ Rni : Ai (qi ) ∩ Aj (qj ) 6= ∅, ∀j ∈ N \{i}},
SO (xO ) = {xO ∈ M : CO (xO ) ∩ Oz 6= ∅}.

November 15, 2017

DRAFT

11

associated with the desired collision-avoidance properties.
IV. P ROBLEM S OLUTION
In this section, a systematic solution to Problem 1 is introduced. Our overall approach builds
on designing a Nonlinear Model Predictive control scheme the system of the manipulators and
the object. Nonlinear Model Predictive Control (see e.g. [20]–[28]) have been proven suitable
for dealing with nonlinearities and state and input constraints.
The coupled agents-object nonlinear dynamics can be written in compact form as follows:


f1 (x, u)


ẋ = f (x, u) = f2 (x, u) , x(0) = x0 ,


f3 (x, u)

(13)

> > >
∈ Rn+12 , u ∈ R6N and
where x = [x>
O , vO , q ]

(xO )vO ,
f1 (x, u) = JO−1
r
h
i
−1
>
f
e
f2 (x, u) = M (q) G (q)u − C(q, q̇)vO − ge(q) ,
ˆ
e
f3 (x, u) = J(q)J
O (q)IvO ,
where we have also used that:
n

 o
(Ji> Ji )−1 Ji> i∈N ∈ Rn×6N ,
n  o
JO (q) = diag JOi i∈N ∈ R6N ×6N ,
h
i>
Ie = I6 , · · · , I6 ∈ R6N ×6 .
ˆ = diag
J(q)

(14)

The expression for f3 (x, u) is derived by employing (8) and (2). Note that f is locally Lipschitz
continuous in its domain since it is continuously differentiable in its domain. Next, we define
the respective errors:


e(t) = x(t) − xdes




 

xO (t) xO,des  xO (t) − xO,des 

 
 

=  vO (t)  − ẋO,des  = 
 ∈ Rn+12 ,
vO (t)

 
 

q(t)
qdes
q(t) − qdes

(15)

where qdes = [q1,des , . . . , qN,des ]> is appropriately chosen such that xO (t) = xO,des , ∀t s.t. q(t) =
qdes (see (6)), and ẋO,des = q̇des = 0. The error dynamics are then ė(t) = f (x(t), u(t)), which can
be appropriately transformed to be written as:
ė(t) = fe (e(t), u(t)), e(0) = e0 = x(0) − xdes .
November 15, 2017

(16)

DRAFT

12

where fe (t) , f (e(t) + xdes , u(t). By ignoring over-actuated input terms, we have that τi =
Ji> (qi )ui , which becomes
kτi k ≤ τ̄i ⇔ σmin,i kui k ≤ τ̄i ,

(17)

where we have employed the property σmin (Ji> )kui k ≤ kJi> ui k, with σmin (Ji> ) denoting the
ei is always
minimum singular value of J > , which is strictly positive, if the constraint qi ∈ Q
i

satisfied. Hence, the constraint |τik | ≤ τ̄i is equivalent to
kui k ≤

τ̄i
, ∀i ∈ N .
σmin (Ji> )

Let us now define the following set U ⊆ R6N :

U = u ∈ R6N : kui k ≤

τ̄i
, ∀i ∈ N
σmin (Ji> )

(18)


,

(19)

as the set that captures the control input constraints of the error dynamics system (16). Define
also the set X ⊆ Rn+12 :
n
X = x ∈ Rn+12 : θO (t) ∈ [−θ̄, θ̄], θBi (t) ∈ [−θ̄, θ̄],
ei \ (Si,O (qi ) ∪ Si,A (qi )) ,
|q̇ki | ≤ q̇¯i , qi ∈ Q
o
3
xO ∈ R \SO (xO ), ∀t ∈ R≥0 .
The set X captures all the state constraint of the system dynamics (13). In view of (15), we
define the set E ⊆ Rn+12 as:
E = {e ∈ Rn+12 : e ∈ X ⊕ (−xdes )},
as the set that captures all the constraints of the error dynamics system (16).
The problem in hand is the design of a control input u(t) ∈ U such that limt→∞ ke(t)k = 0
while ensuring e(t) ∈ E, ∀t ∈ R≥0 . In order to solve the aforementioned problem, we propose
a Nonlinear Model Predictive scheme, that is presented hereafter.
Consider a sequence of sampling times {ti }i≥0 with a constant sampling period 0 < h < Tp ,
where is Tp is the prediction horizon, such that:
ti+1 = ti + h, ∀ i ≥ 0.

(20)

In the sampling-data NMPC, a finite-horizon open-loop optimal control problem (OCP) is solved
at discrete sampling time instants ti based on the current state error information e(ti ). The solution
is an optimal control signal û(t), for t ∈ [ti , ti + Tp ]. For more details, the reader is referred
November 15, 2017

DRAFT

13

to [21]. The open-loop input signal applied in between the sampling instants is given by the
solution of the following Optimal Control Problem (OCP):
min J(e(ti ), û(·))
û(·)



Z

ti +Tp

= min V (ê(ti + Tp )) +
û(·)

h

i 
F (ê(s), û(s)) ds

(21a)

ti

subject to:
˙
ê(s)
= fe (ê(s), û(s)), ê(ti ) = e(ti ),

(21b)

ê(s) ∈ E, û(s) ∈ U, s ∈ [ti , ti + Tp ],

(21c)

ê(ti + Tp ) ∈ Ef ,

(21d)

where the hat ˆ· denotes the predicted variables (internal to the controller), i.e. ê(·) is the solution
of (21b) driven by the control input û(·) : [ti , ti + Tp ] → U with initial condition e(ti ). Note that
the predicted values are not necessarily the same with the actual closed-loop values (see [21]).
The term F : E × U → R≥0 , is the running cost, and is chosen as:
F (e, u) = e> Qe + u> Ru.

(22)

The terms V : E → R>0 and Ef are the terminal penalty cost and terminal set, respectively, and
are used to enforce the stability of the system (see Section 4.2). The terminal cost is given by
(n+12)×(n+12)

V (e) = e> P e. The terms Q ∈ R≥0

(n+12)×(n+12)

, P ∈ R>0

6N ×6N
and R ∈ R>0
are chosen

as:
Q = diag{e
q1 , . . . , qen+12 }, P = diag{e
p1 , . . . , pen+12 }, R = diag{e
r1 , . . . , re6N }.
where qei ∈ R≥0 , pei ∈ R>0 , ∀i ∈ {1, . . . , n + 12} and rej ∈ R>0 , ∀j ∈ {1, . . . , 6N } are constant
weights.
Lemma 2. There exist functions α1 , α2 ∈ K∞ such that:



α1 kzk ≤ F e, u ≤ α2 kzk ,

>
for every z , e> , u> ∈ E × U.
Proof. The proof can be found in Appendix A.

November 15, 2017

DRAFT

14

The solution of the OCP (21a)-(21d) at time ti provides an optimal control input denoted by
û? (t; e(ti )), for t ∈ [ti , ti + Tp ]. It defines the open-loop input that is applied to the system until
the next sampling instant ti+1 :
u(t; e(ti )) = û? (ti ; e(ti )), t ∈ [ti , ti+1 ).

(23)

The corresponding optimal value function is given by:
J ? (e(ti )) , J ? (e(ti ), û? (·; e(ti ))).

(24)

where J(·) as is given in (21a). The control input u(t; e(ti )) is a feedback, since it is recalculated
at each sampling instant using the new state information. The solution of (16) starting at time
t1 from an initial condition e(t1 ), applying a control input u : [t1 , t2 ] → U is denoted by
e(s; u(·), e(t1 )), s ∈ [t1 , t2 ]. The predicted state of the system (16) at time ti + s, s > 0 is
denoted by ê(ti + s; u(·), e(ti )) and it is based on the measurement of the state e(ti ) at time ti ,
when a control input u(·; e(ti )) is applied to the system (16) for the time period [ti , ti + s]. Thus,
it holds that:
e(ti ) = ê(ti ; u(·), e(ti )).

(25)

We define an admissible control input as:
Definition 2. A control input u : [0, Tp ] → R6N for a state e0 is called admissible, if all the
following hold:
1) u(·) is piecewise continuous;
2) u(s) ∈ U, ∀ s ∈ [0, Tp ];
3) e(s; u(·), e0 ) ∈ E, ∀ s ∈ [0, Tp ];
4) e(Tp ; u(·), e0 ) ∈ Ef ;
Lemma 3. The terminal penalty function V (·) is Lipschitz continues in Ef , with Lipschitz constant
LV = 2ε0 σmax (P ), for all e(t) ∈ Ef .
Proof. The proof can be found in Appendix B.
Through the following theorem, we guarantee the stability of the system which is the solution
to Problem 1.
Theorem 1. Consider the Assumptions 1,2. Suppose also that:

November 15, 2017

DRAFT

15

Errors of the Object
10

xO (t) − xO,des
yO (t) − yO,des
zO (t) − zO,des
φO (t) − φO,des

8

exo (t), eyo (t), ezo (t), eφo (t)

6
4
2
0
-2
-4
-6
-8
-10
-12
0

10

20

30

40

50

60

70

80

Time [sec]

Fig. 3. The errors of the object.

1) The OCP (21a)-(21d) is feasible for the initial time t = 0.
2) The terminal set Ef ⊆ E is closed, with 0n+12 ∈ Ef .
3) The terminal set Ef is chosen such that there exists an admissible control input uf :
[0, h] → U such that for all e(s) ∈ Ef it holds that:
a) e(s) ∈ Ef , ∀ s ∈ [0, h].
∂V
fe (e(s), uf (s)) + F (e(s), uf (s)) ≤ 0, ∀ s ∈ [0, h].
b)
∂e
Then, the closed loop trajectories of the system (16), converges to the set Ef , as t → ∞.
Proof. As usual in predictive control the proof consists of two parts: in the first part it is
established that initial feasibility implies feasibility afterwards. Based on this result it is then
shown that the error e(t) converges to the terminal set Ef . The feasibility analysis can be found
in Appendix C. The convergence analysis can be found in D.
V. S IMULATION R ESULTS
To demonstrate the efficiency of the proposed control protocol, we consider two simulation
scenarios.
Scenario 1: Consider N = 2 ground vehicles equipped with 2 DOF manipulators, rigidly
grasping an object with n1 = n2 = 4, n = n1 + n2 = 8. From (13) we have that x =
November 15, 2017

DRAFT

16

Velocities of the Object
7

ẋo (t)
ẏo (t)
żo (t)
φ̇o (t)

6

ẋo (t), ẏo (t), żo (t), φ̇o (t)

5

4

3

2

1

0

-1

-2
0

10

20

30

40

50

60

70

Time [sec]

Fig. 4. The velocities of the object.

> > >
>
>
[x>
∈ R16 , u ∈ R8 , with xO = [p>
∈ R4 , vO = [ṗ>
∈ R4 , pO =
O , vO , q ]
O , φO ]
O , ωxO ]
> >
∈ R4 , pBi = [xBi , yBi ]> ∈ R2 , αi =
[xO , yO , zO ]> ∈ R3 , q = [q1> , q2> ]> ∈ R8 , qi = [p>
Bi , αi ]

[αi1 , αi2 ]> ∈ R2 , i ∈ {1, 2}. The manipulators become singular when sin(αi1 ) = 0}, i ∈ {1, 2},
thus the state constraints for the manipulators are set to:
π
π
π
− ε, − + ε < α12 < − ε,
2
2
2
π
π
π
− + ε < α21 < −ε, − + ε < α22 < − ε.
2
2
2
ε < α11 <

We also consider the input constraints:
−10 ≤ ui,j (t) ≤ 10, i ∈ {1, 2}, j ∈ {1, . . . , 4}.
The initial conditions are set to:
h
π i>
, vO (0) = [0, 0, 0, 0]> ,
xO (0) = 0, −2.2071, 0.9071,
2
h
h
π π i>
π π i>
q1 (0) = 0, 0, ,
, q2 (0) = 0, −4.4142, − , −
.
4 4
4
4
The desired goal states are set to:
h
π i>
xO,des = 10, 10, 0.9071,
, vO,des = [0, 0, 0, 0]> ,
2
h
h
π π i>
π π i>
q1,des = 10, 12.2071, ,
, q2,des = 10, 7.7929, − , −
.
4 4
4
4

November 15, 2017

DRAFT

17

Errors of Agent 1
10

xB1 (t) − xB1 ,des
yB1 (t) − yB1 ,des
α11 (t) − α11 ,des
α12 (t) − α12 ,des

8
6
4

q1 (t) − q1,des

2
0
-2
-4
-6
-8
-10
-12
0

10

20

30

40

50

60

70

80

Time [sec]

Fig. 5. The errors of vehicle 1 as well as the errors of the manipulator.

We set an obstacle between the initial and the desired pose of the object. the obstacle is spherical
with center [5, 5, 1] and radius 2. The sampling time is h = 0.1 sec, the horizon is set to Tp =
0.3 sec, and the total simulation time is 80 sec; The matrices P, Q, R are set to:
P = Q = 10I16×16 , R = 2I8×8 .
The simulation results are depicted in Fig. 3- Fig. 8, which shows that the states of the agents as
well as the states of the object converge to the desired ones while guaranteeing that the obstacle
is avoided and all state and input constraints are met.
Scenario 2: Consider N = 3 ground vehicles equipped with 2 DOF manipulators, rigidly
grasping an object with n1 = n2 = n3 = 4, n = n1 + n2 + n3 = 12. From (13) we have
> > >
>
>
that x = [x>
∈ R20 , u ∈ R12 , with xO = [p>
∈ R4 , vO = [ṗ>
∈ R4 ,
O , vO , q ]
O , φO ]
O , ωxO ]
> >
pO = [xO , yO , zO ]> ∈ R3 , q = [q1> , q2> , q3> ]> ∈ R12 , qi = [p>
∈ R4 , pBi = [xBi , yBi ]> ∈ R2 ,
Bi , αi ]

αi = [αi1 , αi2 ]> ∈ R2 , i ∈ {1, 2}. The manipulators become singular when sin(αi1 ) = 0}, i ∈
{1, 2, 3}, thus the state constraints for the manipulators are set to:
π
π
π
− ε, − + ε < α12 < − ε,
2
2
2
π
π
π
− + ε < α21 < −ε, − + ε < α22 < − ε.
2
2
2
ε < α11 <

November 15, 2017

DRAFT

18

Errors of Agent 2
10

xB2 (t) − xB2 ,des
yB2 (t) − yB2 ,des
α21 (t) − α21 ,des
α12 (t) − α12 ,des

8
6
4

q2 (t) − q2,des

2
0
-2
-4
-6
-8
-10
-12
0

10

20

30

40

50

60

70

80

Time [sec]

Fig. 6. The errors of vehicle 2 as well as the errors of the manipulator.

Control Inputs of Agent 1

u1,j (t), j ∈ {1, 2, 3, 4}

10

5

0

u1,1 (t)
u1,2 (t)
u1,3 (t)
u1,4 (t)

-5

-10

0

10

20

30

40

50

60

70

80

Time [sec]

Fig. 7. The control inputs of the actuators of agent 1.

We also consider the input constraints:
−10 ≤ ui,j (t) ≤ 10, i ∈ {1, 2}, j ∈ {1, . . . , 4}.

November 15, 2017

DRAFT

19

Control Inputs of Agent 2

u2,j (t), j ∈ {1, 2, 3, 4}

10

5

0

u2,1 (t)
u2,2 (t)
u2,3 (t)
u2,4 (t)

-5

-10

0

10

20

30

40

50

60

70

80

Time [sec]

Fig. 8. The control inputs of the actuators of agent 2.

The initial conditions are set to:
h
π i>
, vO (0) = [0, 0, 0, 0]> ,
xO (0) = 0, −2.2071, 0.9071,
2
h
h
π π i>
π π i>
, q2 (0) = 0, −4.4142, − , −
,
q1 (0) = 0.5, 0, ,
4 4
4
4
h
π π i>
q3 (0) = −0.5, 0, ,
.
4 4
The desired goal states are set to:
h
π i>
, vO,des = [0, 0, 0, 0]> ,
xO,des = 5, −2.2071, 0.9071,
2
h
h
π π i>
π π i>
, q2,des = 5, −4.4142, − , −
,
q1,des = 5.5, 0, ,
4 4
4
4
h
π π i>
q3,des = 4.5, 0.0, ,
.
4 4
The sampling time is h = 0.1 sec, the horizon is set to Tp = 0.5 sec, and the total simulation
time is 100 sec; The matrices P, Q, R are set to:
P = Q = 0.5I20×20 , R = 0.5I12×12 .
The simulation results are depicted in Fig. 9- Fig. 16, which shows that the states of the agents as
well as the states of the object converge to the desired ones while guaranteeing that all state and

November 15, 2017

DRAFT

20

Errors of the Object
2

1

exo (t), eyo (t), ezo (t), eφo (t)

0

-1

xO (t) − xO,des
yO (t) − yO,des
zO (t) − zO,des
φO (t) − φO,des

-2

-3

-4

-5

-6
0

10

20

30

40

50

60

70

80

90

100

Time [sec]

Fig. 9. The errors of the object.

input constraints are met. The simulation scenarios were carried out by using the NMPC toolbox
given in [25] and they took 23500 sec, 45547 sec for Scenario 1 and Scenario 2, respectively, in
MATLAB Environment on a desktop with 8 cores, 3.60 GHz SPU and 16GB of RAM.
VI. C ONCLUSIONS AND F UTURE W ORK
In this work we proposed a NMPC scheme for the cooperative transportation of an object
rigidly grasped by N robotic agents. The proposed control scheme deals with singularities of
the agents, inter-agent collision avoidance as well as collision avoidance between the agents and
the object with the workspace obstacles. We proved the feasibility and convergence analysis of
the proposed methodology and simulation results verified the efficiency of the approach. Future
efforts will be devoted towards including load sharing coefficients, internal force regulation, and
complete decentralization of the proposed method. Finally, we will try to decrease the overall
complexity and carry out real-time experiments.

November 15, 2017

DRAFT

21

Velocities of the Object
1

ẋo (t)
ẏo (t)
żo (t)
φ̇o (t)

0.8

ẋo (t), ẏo (t), żo (t), φ̇o (t)

0.6

0.4

0.2

0

-0.2

-0.4

-0.6

-0.8

-1
0

10

20

30

40

50

60

70

80

90

Time [sec]

Fig. 10. The velocities of the object.

Errors of Agent 1
2

1

q1 (t) − q1,des

0

-1

xB1 (t) − xB1 ,des
yB1 (t) − yB1 ,des
α11 (t) − α11 ,des
α12 (t) − α12 ,des

-2

-3

-4

-5

-6
0

10

20

30

40

50

60

70

80

90

100

Time [sec]

Fig. 11. The errors of vehicle 1 as well as the errors of the manipulator.

A PPENDIX A
P ROOF OF L EMMA 2
By invoking the fact that:
λmin (P )kyk2 ≤ y > P y ≤ λmax (P )kyk2 , ∀y ∈ Rn , P ∈ Rn×n , P = P > > 0,
November 15, 2017

(26)
DRAFT

22

Errors of Agent 2
2

1

q2 (t) − q2,des

0

-1

xB2 (t) − xB2 ,des
yB2 (t) − yB2 ,des
α21 (t) − α21 ,des
α12 (t) − α12 ,des

-2

-3

-4

-5

-6
0

10

20

30

40

50

60

70

80

90

100

Time [sec]

Fig. 12. The errors of vehicle 2 as well as the errors of the manipulator.

Errors of Agent 3
2

1

q3 (t) − q3,des

0

-1

xB3 (t) − xB3 ,des
yB3 (t) − yB3 ,des
α31 (t) − α31 ,des
α32 (t) − α32 ,des

-2

-3

-4

-5

-6
0

10

20

30

40

50

60

70

80

90

100

Time [sec]

Fig. 13. The errors of vehicle 3 as well as the errors of the manipulator.

we have:
e> Qe + u> Ru ≤ λmax (Q)kek2 + λmax (R)kuk2 = max{λmax (Q), λmax (R)}kzk2 ,

November 15, 2017

DRAFT

23

Control Inputs of Agent 1
9

8

u1,1 (t)
u1,2 (t)
u1,3 (t)
u1,4 (t)

7

u1,j (t), j ∈ {1, 2, 3, 4}

6

5

4

3

2

1

0

-1

-2
0

10

20

30

40

50

60

70

80

90

100

Time [sec]

Fig. 14. The control inputs of the actuators of agent 1.

Control Inputs of Agent 2
9

8

7

u2,j (t), j ∈ {1, 2, 3, 4}

6

u2,1 (t)
u2,2 (t)
u2,3 (t)
u2,4 (t)

5

4

3

2

1

0

-1

-2
0

10

20

30

40

50

60

70

80

90

100

Time [sec]

Fig. 15. The control inputs of the actuators of agent 3.

and:
e> Qe + u> Ru ≥ λmin (Q)kek2 + λmin (R)kuk2
= min{λmin (Q), λmin (R)}kzk2 ,

November 15, 2017

DRAFT

24

Control Inputs of Agent 3
10

u3,j (t), j ∈ {1, 2, 3, 4}

8

u3,1 (t)
u3,2 (t)
u3,3 (t)
u3,4 (t)

6

4

2

0

-2
0

10

20

30

40

50

60

70

80

90

100

Time [sec]

Fig. 16. The control inputs of the actuators of agent 3.


>
where z = e> , u> . Thus, we get:
min{λmin (Q), λmin (R)}kzk2 ≤ e> Qe + u> Ru ≤ max{λmax (Q), λmax (R)}kzk2 .
By defining the K∞ functions α1 , α2 : R≥0 → R≥0 :
α1 (y) , mkyk2 , α2 (y) , max{λmax (Q), λmax (R)}kyk2 ,
and the parameter m ∈ R>0 by:
m , min{λmin (Q), λmin (R)},

(27)




α1 kzk ≤ F e, u ≤ α2 kzk ,

(28)

we get:

which leads to the conclusion of the proof.

November 15, 2017

DRAFT

25

A PPENDIX B
P ROOF OF L EMMA 2
Proof. For every e(t) ∈ Ef , the following holds:
>
|V (e1 ) − V (e2 )| = |e>
1 P e1 − e2 P e2 |
>
>
>
= |e>
1 P e1 + e1 P e2 − e1 P e2 − e2 P e2 |
>
= |e>
1 P (e1 − e2 ) − e2 P (e1 − e2 )|
>
≤ |e>
1 P (e1 − e2 )| + |e2 P (e1 − e2 )|.

(29)

By employing the property that:
|x> Ay| ≤ σmax (A)kxkkyk, ∀ x, y ∈ Rn , A ∈ Rn×n ,
(29) is written as:
|V (e1 ) − V (e2 )| ≤ σmax (P )ke1 kke1 − e2 k + σmax (P )ke2 kke1 − e2 k
= σmax (P )(ke1 k + ke2 k)ke1 − e2 k
≤ σmax (P )(ε0 + ε0 )ke1 − e2 k
= [2ε0 σmax (P )] ke1 − e2 k.
which completes the proof.
A PPENDIX C
F EASIBILITY A NALYSIS
Consider any sampling time instant ti for which a solution exists. In between ti and ti+1 , the
optimal control input û? (s; e(ti )), s ∈ [ti , ti+1 ) is implemented. According to (25), it holds that:
e(ti+1 ) = ê(ti+1 ; û? (·; e(ti )), e(ti )).
The remaining piece of the optimal control input û? (s; e(ti )), s ∈ [ti+1 , ti + Tp ] satisfies the state
and input constraints E, U , respectively. Furthermore,
ê(ti + Tp ; û? (·; e(ti )), e(ti )) ∈ Ef ,

November 15, 2017

DRAFT

26

and we know from Assumption 2b of Theorem 1 that for all e(t) ∈ Ef , there exists at least one
control input uf (·) that renders the set Ef invariant over h. Picking any such input, a feasible
control input ū(·; e(ti+1 )), at time instant ti+1 , may be the following:


û? (s; e(ti )),
s ∈ [ti+1 , ti + Tp ],
ū(s; e(ti+1 )) =

uf (ê(ti + Tp ; u? (·), e(ti ))), s ∈ [ti + Tp , ti+1 + Tp ].

(30)

Thus, from feasibility of û? (s, e(ti )) and the fact that uf (e(t)) ∈ U , for all e(t) ∈ Ef , it follows
that:
ū(s; e(ti+1 )) ∈ U, ∀ s ∈ [ti+1 , ti + Tp ].
Hence, the feasibility at time ti implies feasibility at time ti+1 . Therefore, if the OCP (21a) (21d) is feasible at time t = 0, it remains feasible for every t ≥ 0.
A PPENDIX D
C ONVERGENCE A NALYSIS
The second part involves proving convergence of the state e in the terminal set Ef . In order
to prove this, it must be shown that a proper value function is decreasing along the solution
trajectories starting at a sampling time ti . Consider the optimal value function J ? (e(ti )), as is
given in (24). Consider also the cost of the feasible control input, indicated by:
¯ i+1 )) , J(e(t
¯ i+1 ), ū(·; e(ti+1 ))),
J(e(t

(31)

where ti+1 = ti + h, as is given in (20). Define:
u1 (s) = ū(s; e(ti+1 )),

(32)

e1 (s) = ē(s; u1 (s), e(ti+1 )), s > ti+1 .
e1 (s) stands for the predicted state e at time s, based on the measurement of the state e at
time ti+1 , while using the feasible control input ū(s; e(ti+1 )). Let us also define th the following
terms:
u2 (s) = û? (s; e(ti )),

(33)

e2 (s) = ê(s; u2 (s), e(ti )), s > ti+1 .
(32), (33) form convenient notations for the readability of the proof hereafter.

November 15, 2017

DRAFT

27

By employing (21a), (24) and (31), the difference between the optimal and feasible cost is
given by:
¯ i+1 )) − J ? (e(ti )) = V (e1 (ti+1 + Tp )) +
J(e(t

ti+1 +Tp

Z

h
i
F (e1 (s), u1 (s)) ds

ti+1

Z

ti +Tp

− V (e2 (ti + Tp )) −
Z

ti
ti +Tp

= V (e1 (ti+1 + Tp )) +

h

h

i
F (e2 (s), u2 (s)) ds

i
F (e1 (s), u1 (s)) ds

ti+1

Z

ti+1 +Tp

+

h

i
F (e1 (s), u1 (s)) ds − V (e2 (ti + Tp ))

ti +Tp

Z

ti+1

−

h

Z
i
F (e2 (s), u2 (s)) ds −

ti +Tp

h

i
F (e2 (s), u2 (s)) ds.

(34)

ti+1

ti

Note that, from (30), the following holds:
ū(s; e(ti+1 )) = û? (s; e(ti )), ∀ s ∈ [ti+1 , ti + Tp ].

(35)

By combining (32), (33) and (35), it yields that:
u1 (s) = u2 (s) = ū(s), ∀ s ∈ [ti+1 , ti + Tp ],

(36)

e1 (s) = e2 (s), ∀ s ∈ [ti+1 , ti + Tp ].

(37)

which implies that:

The combination of (36) and (37) implies that:
F (e1 (s), u1 (s)) = F (e1 (s), u1 (s)), ∀ s ∈ [ti+1 , ti + Tp ].
which implies that:
Z

ti +Tp

h

Z
i
F (e1 (s), u1 (s)) ds =

ti+1

ti +Tp

h

i
F (e2 (s), u2 (s)) ds.

(38)

ti+1

By employing (38), (34) becomes:
¯ i+1 )) − J (e(ti )) = V (e1 (ti+1 + Tp )) +
J(e(t
?

ti+1 +Tp

Z

h

i
F (e1 (s), u1 (s)) ds

ti +Tp

Z

ti+1

− V (e2 (ti + Tp )) −

h

i
F (e2 (s), u2 (s)) ds.

(39)

ti

November 15, 2017

DRAFT

28

Due to the fact that ti+1 + Tp − (ti + Tp ) = ti+1 − ti = h, and the Assumption 2b of Theorem
1 holds for one sampling period h, by integrating this inequality from ti + Tp to ti+1 + Tp and
we get the following:
Z ti+1 +Tp h

i
∂V
fe (e1 (s), u1 (s)) + F (e1 (s), u1 (s)) ds ≤ 0
∂e
ti +Tp
Z ti+1 +Tp h
Z ti+1 +Tp h
i
i
F (e1 (s), u1 (s)) ds ≤ 0
⇔
V̇ (e1 (s)) ds +
ti +Tp

ti +Tp

Z

ti+1 +Tp

⇔V (e1 (ti+1 + Tp )) − V (e1 (ti + Tp )) +

h

i
F (e1 (s), u1 (s)) ds ≤ 0

h

i
F (e1 (s), u1 (s)) ds ≤

ti +Tp

Z

ti+1 +Tp

⇔V (e1 (ti+1 + Tp )) − V (e1 (ti + Tp )) +
ti +Tp

V (e2 (ti + Tp )) − V (e2 (ti + Tp ))
Z ti+1 +Tp h
i
F (e1 (s), u1 (s)) ds − V (e2 (ti + Tp )) ≤
⇔V (e1 (ti+1 + Tp )) +
ti +Tp

V (e1 (ti + Tp )) − V (e2 (ti + Tp )).
By employing the property y ≤ |y|, ∀y ∈ R, we get:
Z ti+1 +Tp h
i
F (e1 (s), u1 (s)) ds − V (e2 (ti + Tp )) ≤
V (e1 (ti+1 + Tp )) +
ti +Tp

|V (e1 (ti + Tp )) − V (e2 (ti + Tp ))| .

(40)

By employing Lemma 2, we have that:
|V (e1 (ti + Tp )) − V (e2 (ti + Tp ))| ≤ LV ke1 (ti + Tp ) − e2 (ti + Tp )k.

(41)

By combining (40) and (41) we get:
Z ti+1 +Tp h
i
V (e1 (ti+1 + Tp )) +
F (e1 (s), u1 (s)) ds − V (e2 (ti + Tp )) ≤
ti +Tp

LV ke1 (ti + Tp ) − e2 (ti + Tp )k

(42)

For s = ti + Tp , (37) gives:
e1 (ti + Tp ) = e2 (ti + Tp ).

(43)

By combining (43) and (42) we have:
V (e1 (ti+1 + Tp ))+
Z ti+1 +Tp h
i
F (e1 (s), u1 (s)) ds − V (e2 (ti + Tp )) ≤ 0.

(44)

ti +Tp

November 15, 2017

DRAFT

29

By combining (39) with (44), the following holds:
Z ti+1 h
i
?
¯
J(e(ti+1 )) − J (e(ti )) ≤ −
F (e2 (s), u2 (s)) ds.

(45)

ti

By substituting e = e2 (s), u = u2 (s) in (28) we get:
F (e2 (s), u2 (s)) ≥ mkz2 (s)k2 ,
where z2 , [e2 , u2 ]> . The latter is equivalent to:
Z ti+1 h
Z
i
F (e2 (s), u2 (s)) ds ≥ m
ti

ti+1

kz2 (s)k2 ds

ti
ti+1

Z
⇔−

Z
h
i
F (e2 (s), u2 (s)) ds ≤ −m

ti

ti+1

kz2 (s)k2 ds.

(46)

ti

By combining (45) and (46) we finally get:
¯ i+1 )) − J ? (e(ti )) ≤ −m
J(e(t

ti+1

Z

kz2 (s)k2 ds.

(47)

ti

It is clear that the optimal solution at time ti+1 i.e., J ? (e(ti+1 )) will not be worse than the
¯ i+1 )). Therefore, (47) implies:
feasible one at the same time i.e. J(e(t
Z ti+1
?
?
kz2 (s)k2 s ≤ 0,
(48)
J (e(ti+1 )) − J (e(ti )) ≤ −m
ti
i−1 Z
X

ti

Z

2

kz2 (s)k ds =

or, by using the fact that
t0

?

j=0

Z

?

tj+1

kz2 (s)k2 ds, equivalently, we obtain:

tj

ti+1

J (e(ti+1 )) − J (e(ti )) ≤ −m

2

kz2 (s)k ds + m
t0

i−1 Z
X
j=0

tj+1

kz2 (s)k2 ds.

(49)

tj

By using induction and the fact that ti = h · i, ti+1 = h · (i + 1), ∀i ≥ 0, from (20), (49) is written
as:
?

Z

?

ti

kz2 (s)k2 ds.

J (e(ti )) − J (e(t0 )) ≤ −m

(50)

t0

Since t0 = 0 we obtain:
?

?

Z

J (e(ti )) ≤ J (e(0)) − m

ti

kz2 (s)k2 ds.

(51)

0

which implies that:
J ? (e(ti )) ≤ J ? (e(0)).

(52)

J ? (e(ti+1 )) ≤ J ? (e(ti )) ≤ J ? (e(0)), ∀ ti = i · h, i ≥ 0.

(53)

By combining (48), (52), we obtain:

November 15, 2017

DRAFT

30

Therefore, the value function J ? (e(ti )) has proven to be non-increasing for all the sampling
times. Let us define the function:
V (e(t)) = J ? (e(s)) ≤ J ? (e(0)), t ∈ R≥0 ,

(54)

where s = max{ti : ti ≤ t}. Since J ? (e(0)) is bounded, (54) implies that V (e(t)) is bounded.
Since the signals e(t), u(t) are bounded (e(t) ∈ E, u(t) ∈ U ), according to (16), it holds that
ė(t) is also bounded. From (51) we have that:
?

Z

?

V (e(t)) = J (e(s)) ≤ J (e(0)) − m

s

kz2 (s)k2 ds.

0

which due to the fact that s ≤ t, is equivalent to:
Z t
?
V (e(t)) ≤ J (e(0)) − m
kz2 (s)k2 ds, t ∈ R≥0 .

(55)

0

From (55), we get:
Z

t

1 ?
[J (e(0)) − V (e(t))] , t ∈ R≥0 .
(56)
m
0
Z t
?
kz2 (s)k2 ds is also bounded.
Since J (e(0)), V (e(t)) has been proven to be bounded, the term
kz2 (s)k2 ds ≤

0

Therefore, by employing Lemma 1, we have that kz2 (t)k → 0, as t → ∞. The latter implies
that:
lim ke(t)k = 0 ⇒ e(t) ∈ Ef , as t → ∞,

t→∞

and leads to the conclusion of the proof.
R EFERENCES
[1] S. A. Schneider and R. H. Cannon, “Object impedance control for cooperative manipulation: Theory and experimental
results,” IEEE Transactions on Robotics and Automation, vol. 8, no. 3, pp. 383–394, 1992.
[2] Y.-H. Liu, S. Arimoto, and T. Ogasawara, “Decentralized cooperation control: non-communication object handling,”
Proceedings of the IEEE Conference on Robotics and Automation (ICRA), vol. 3, pp. 2414–2419, 1996.
[3] Y.-H. Liu and S. Arimoto, “Decentralized adaptive and nonadaptive position/force controllers for redundant manipulators
in cooperations,” The International Journal of Robotics Research, vol. 17, no. 3, pp. 232–247, 1998.
[4] M. Zribi and S. Ahmad, “Adaptive control for multiple cooperative robot arms,” Proceedings of the IEEE International
Conference on Decision and Control (CDC), pp. 1392–1398, 1992.
[5] O. Khatib, K. Yokoi, K. Chang, D. Ruspini, R. Holmberg, and A. Casal, “Decentralized cooperation between multiple
manipulators,” IEEE International Workshop on Robot and Human Communication, pp. 183–188, 1996.
[6] F. Caccavale, P. Chiacchio, and S. Chiaverini, “Task-space regulation of cooperative manipulators,” Automatica, vol. 36,
no. 6, pp. 879–887, 2000.

November 15, 2017

DRAFT

31

[7] J. Gudiño-Lau, M. A. Arteaga, L. A. Munoz, and V. Parra-Vega, “On the control of cooperative robots without velocity
measurements,” IEEE Transactions on Control Systems Technology, vol. 12, no. 4, pp. 600–608, 2004.
[8] F. Caccavale, P. Chiacchio, A. Marino, and L. Villani, “Six-dof impedance control of dual-arm cooperative manipulators,”
IEEE/ASME Transactions On Mechatronics, vol. 13, no. 5, pp. 576–586, 2008.
[9] D. Heck, D. Kostić, A. Denasi, and H. Nijmeijer, “Internal and external force-based impedance control for cooperative
manipulation,” Proceedings of the IEEE European Control Conference (ECC), pp. 2299–2304, 2013.
[10] S. Erhart and S. Hirche, “Adaptive force/velocity control for multi-robot cooperative manipulation under uncertain kinematic
parameters,” Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 307–314,
2013.
[11] S. Erhart, D. Sieber, and S. Hirche, “An impedance-based control architecture for multi-robot cooperative dual-arm mobile
manipulation,” Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 315–
322, 2013.
[12] J. Szewczyk, F. Plumet, and P. Bidaud, “Planning and controlling cooperating robots through distributed impedance,”
Journal of Robotic Systems, vol. 19, no. 6, pp. 283–297, 2002.
[13] A. Tsiamis, C. K. Verginis, C. P. Bechlioulis, and K. J. Kyriakopoulos, “Cooperative manipulation exploiting only implicit
communication,” Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 864–
869, 2015.
[14] F. Ficuciello, A. Romano, L. Villani, and B. Siciliano, “Cartesian impedance control of redundant manipulators for humanrobot co-manipulation,” Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS),
pp. 2120–2125, 2014.
[15] A. Ponce-Hinestroza, J. Castro-Castro, H. Guerrero-Reyes, V. Parra-Vega, and E. Olguỳn-Dỳaz, “Cooperative redundant
omnidirectional mobile manipulators: Model-free decentralized integral sliding modes and passive velocity fields,”
Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), pp. 2375–2380, 2016.
[16] W. Gueaieb, F. Karray, and S. Al-Sharhan, “A robust hybrid intelligent position/force control scheme for cooperative
manipulators,” IEEE/ASME Transactions on Mechatronics, vol. 12, no. 2, pp. 109–125, 2007.
[17] M. Ciocarlie, F. M. Hicks, R. Holmberg, J. Hawke, M. Schlicht, J. Gee, S. Stanford, and R. Bahadur, “The velo gripper:
A versatile single-actuator design for enveloping, parallel and fingertip grasps,” The International Journal of Robotics
Research, 2014.
[18] D. E. Koditschek and E. Rimon, “Robot navigation functions on manifolds with boundary,” Advances in applied
mathematics, vol. 11, no. 4, pp. 412–442, 1990.
[19] B.Siciliano, L. Sciavicco, and L. Villani, “Robotics: Modelling, Planning and Control”. Advanced Textbooks in Control
and Signal Processing, Springer, 2009.
[20] K. S. de Oliveira and M. Morari, “Contractive Model Predictive Control for Constrained Nonlinear Systems,” IEEE
Transactions on Automatic Control, vol. 45, no. 6, pp. 1053–1071, 2000.
[21] R. Findeisen, L. Imsland, F. Allgower, and B. A. Foss, “State and Output Feedback Nonlinear Model Predictive Control:
An Overview,” European Journal of Control, vol. 9, no. 2-3, pp. 190–206, 2003.
[22] H. Chen and F. Allgöwer, “A Quasi-Infinite Horizon Nonlinear Model Predictive Control Scheme with Guaranteed
Stability,” Automatica, vol. 34, no. 10, pp. 1205–1217, 1998.
[23] R. Findeisen, L. Imsland, F. Allgöwer, and B. Foss, “Towards a Sampled-Data Theory for Nonlinear Model Predictive
Control,” New Trends in Nonlinear Dynamics and Control and their Applications, pp. 295–311, 2003.
[24] F. Fontes, “A General Framework to Design Stabilizing Nonlinear Model Predictive Controllers,” Systems and Control
Letters, vol. 42, no. 2, pp. 127–143, 2001.

November 15, 2017

DRAFT

32

[25] L. Grüne and J. Pannek, Nonlinear Model Predictive Control. Springer London, 2011.
[26] E. Camacho and C. Bordons, “Nonlinear Model Predictive Control: An Introductory Review,” pp. 1–16, 2007.
[27] B. Kouvaritakis and M. Cannon, Nonlinear Predictive Control: Theory and Practice. No. 61, Iet, 2001.
[28] J. Frasch, A. Gray, M. Zanon, H. Ferreau, S. Sager, F. Borrelli, and M. Diehl, “An Auto-Generated Nonlinear MPC
Algorithm for Real-Time Obstacle Cvoidance of Ground Vehicles,” European Control Conference (ECC), 2013.
[29] F. Fontes, L. Magni, and É. Gyurkovics, “Sampled-Data Model Predictive Control for Nonlinear Time-Varying Systems:
Stability and Robustness,” Assessment and Future Directions of Nonlinear Model Predictive Control, pp. 115–129, 2007.
[30] A. Nikou, C. K. Verginis, S. Heshmati-alamdari, and D. V. Dimarogonas, “A Nonlinear Model Predictive Control Scheme
for Cooperative Manipulation with Singularity and Collision Avoidance,” 25th IEEE Mediterranean Conference on Control
and Automation (MED), Valletta, Malta, 2017.
[31] A. Nikou, S. Heshmati-alamdari, C. K. Verginis, and D. V. Dimarogonas, “Decentralized Abstractions and Timed
Constrained Planning of a General Class of Coupled Multi-Agent Systems,” 56th IEEE Conference on Decision and
Control (CDC), ArXiv Link: https://arxiv.org/abs/1703.06070, 2017.
[32] A. Filotheou, A. Nikou, and D. V. Dimarogonas, “Decentralized Control of Uncertain Multi-Agent Systems with
Connectivity Maintenance and Collision Avoidance,” IEEE European Control Conference (ECC), Under Review, ArXiv
Link: https://arxiv.org/abs/1710.09204, 2017.
[33] A. Filotheou, A. Nikou, and D. Dimarogonas, “Robust Decentralized Navigation of Multiple Rigid Bodies with Collision
Avoidance and Connectivity Maintenance Using Model Predictive Controllers,” International Journal of Control (IJC),
Under Review, 2017.
[34] C. K. Verginis, A. Nikou, and D. Dimarogonas, “Communication-based Decentralized Cooperative Object Transportation
Using Nonlinear Model Predictive Control,” IEEE European Control Conference (ECC), Under Review, 2017.
[35] H. Khalil, Noninear Systems. Prentice-Hall, New Jersey, 1996.
[36] H. Michalska and R. Vinter, “Nonlinear Stabilization Using Discontinuous Moving-Horizon Control,” IMA Journal of
Mathematical Control and Information, vol. 11, no. 4, pp. 321–340, 1994.

November 15, 2017

DRAFT

