# An Overview of Energy-Optimal Impedance Control of Cooperative Robot
Manipulators

Amin Ghorbanpour, Hanz Richter The authors are with the Department of
Mechanical Engineering, Cleveland State University, Cleveland, OH 44122 USA
(e-mail<EMAIL_ADDRESS>h.richter@csuohio.edu).This article is
intended as a supplement to article titled “Energy-Optimal Impedance Control
of Cooperative Robot Manipulators” written by the same authors.

###### Abstract

An impedance-based control scheme is introduced for cooperative manipulators
grasping a rigid load. The position and orientation of the load are to be
maintained close to a desired trajectory, trading off tracking accuracy by low
energy consumption and maintaining stability. To this end, the augmented
dynamics of the robots, their actuators and the load is formed, and an
impedance control is adopted. A virtual control strategy is used to decouple
torque control from actuator control. An optimization problem is then
formulated using energy balance equations. The optimization finds the damping
and stiffness gains of the impedance relation such that the energy consumption
is minimized. Furthermore, $\mathcal{L}_{2}$ stability techniques are used to
allow for time-varying damping and stiffness in the desired impedance. A
numerical example is provided to demonstrate the results.

##  I Introduction

Saving energy has become a major driving force in engineering, owing to
increasing demand for cost-efficient and long-life systems, and as a result,
engineers sought to find solutions for optimal energy consumption. This is
particularly important in robotic automation. Robots with on-board, finite
energy storage are prevalent in electric vehicles, powered human assistive
devices, aerospace vehicles, etc. [1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11].

Therefore, in dynamical systems with limited on-board energy source, longer
operating times and smaller operating costs are possible by reducing the
amount of energy needed to complete the motion tasks. Energy regeneration is
defined here as the ability to transfer energy back to the system’s power
source during motion tasks. Energy-aware control in robotics, particularly
concerning energy regeneration, has gained a broad research interest recently
[1, 2, 8, 9, 6]. Ultracapacitors have been used in addition to or instead of
batteries as energy storage elements in regenerative motion systems. In
addition to being lightweight and durable, ultracapacitors can be charged and
discharged at high rates. This feature is essential for energy regeneration
because the storage element should be capable of admitting all the energy that
can be potentially recovered.

So far, most studies have considered energy-oriented control in single robots
where significant potential for energy recovery exists, e.g. industrial
robots, prosthetic legs, etc. [7, 1, 6]. These works sought to minimize energy
consumption by maximizing regenerative energy storage relative to
trajectories, controller parameters or design parameters. The latter problem
was quadratic, and admitted explicit solutions.

The focus of this study is to introduce a framework for energy-oriented
control in cooperative robots. Even though robots offer superior capabilities
than humans for carrying out motion tasks involving large loads, speeds and
tight precision requirements, some tasks are difficult or impossible for a
single robot. The limits in the structure of a robot prevent a sole robot to
operate with large, unbalanced or flexible loads. Cooperative robot
manipulators (CRM) offer significant advantages over a single robot, and they
show better performance in tasks such as grasping, gripping, lifting,
transferring, lowering, and approach an object. Regardless of the benefits
obtained by employing CRM, it comes at the cost of complexity for the
mathematical modeling, control, and coordination. Here, the term cooperative
means collaboration of multi robots in handling a payload.

This paper considers theoretical developments and extensions of a framework
previously established for a single robot in [2]. Here, a motion task is
defined as carrying a payload near a desired trajectory by grasping it using
the CRM, limiting grasping forces and reducing energy consumption. The motion
task thus demands a controller meeting three main objectives: 1) maintaining
acceptable tracking accuracy of desired trajectories for the position and
orientation of the load, 2) being in compliance with the closed kinematic
chain caused by grasping the load, and 3) minimizing the energy consumption
through energy regeneration. A novel controller is proposed based on an
impedance relation whose parameters are tuned such that maximum energy
regeneration is provided. It is assumed that all robots and their
corresponding actuators (DC motors) are equipped with regenerative drives
(4-quadrant drives), allowing energy to go back from the robots to the storage
element. Regenerative drives provide an opportunity to harvest the excess
mechanical energy by channeling it back to the source instead of being
dissipated [1].

###  I-A Review of related work

Most of the research literature on CRM does not place emphasis on energy
considerations. Optimization of the energy consumption for single robots and
general electromechanical systems is well documented [5, 2, 7, 1, 11, 4, 6, 3,
12, 13, 14, 15, 16]. In the field of robotics, researches in [2, 1, 4, 3]
minimized energy consumption considering capacitive storage elements. These
ideas were applied for powered human assistive devices [6, 11, 12], and
aerospace applications [7, 14, 5]. Energy-oriented control studies have also
been conducted with a focus on actuator design and dynamics [3, 15, 16].

The framework established in [2] combines the dynamics of a robot, joint
mechanisms, and actuators to present an augmented dynamical model of the
robotic system. Depending on the type of the actuators, e.g. DC motors,
hydraulic elements, etc., the energy transfer between the power source and the
actuators can be derived, which leads to find the energy change in the power
source. Hence, an energy minimization can be formed. These ideas were applied
to an industrial robot (PUMA manipulator), finding point-to-point energy-
optimal trajectories [1]. It is shown that 10-20% energy reduction is possible
by energy regeneration. Furthermore, application of the idea for prosthetic
leg showed promising results [6, 17, 12].

In the aforementioned works, robots are either isolated without any contact
with the environment, or have limited (in the sense of low and intermittent
interaction forces) interactions with it. Accordingly, interactions were not
modeled and regarded as external disturbances. This work provides a bridge
between energy-oriented control works and the subject of cooperative robots,
considering the stability of interactions.

Modeling and control of CRM have received wide attention for more than 3
decades[18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34,
35, 36]. Nevertheless, the energy-oriented studies of CRM have received little
attention. For instance, in [23] energy consumption is reduced by finding the
optimal operating schedule of a robotic manufacturing system, using the
consumption profiles of individual robots and their operations as pre-
determined information.

CRM tasks involve interaction between the robots and a payload, and the force-
position relationships between them are of fundamental concern. For trajectory
tracking, various control approaches have been proposed, with impedance
control being prevalent. Impedance control constitutes an effective, easy-to-
implement scheme for multiple interacting dynamical systems. Impedance control
includes regulation and stabilization of robot motion by establishing a
mathematical relationship between the interaction forces/moments and the robot
trajectories. The relation usually defined as a second order linear non-
homogeneous ordinary differential equation, often modeled after a forced
spring-mass-damper system. Impedance control has been used for CRM in many
works, for instance [26, 34, 25, 27, 32, 33, 29, 35, 36, 30, 31, 28].

One of the main differences among various approaches is the definition of
terms and the structure of the impedance relationship. This includes different
choices of the forces or moments appearing in the impedance relation [26, 25]
or the use of nonlinear [28] and variable [29] inertial, stiffness and damping
characteristics.

Furthermore, impedance control is useful when the payload handled by the CRM
may also be in contact with the environment. The algorithm of [30] controls
motion and internal forces of the payload, as well as the contact forces
between the payload and environment. In that work, the reference trajectory
for each robotic joint is calculated based on an impedance relationship such
that the desired internal and contact forces are achieved. In [31] the problem
of assembly operations using CRM was considered. Experiments showed the
effectiveness of impedance controls for typical tasks encountered in CRM
systems.

Impedance relationships have also been used in conjunction with adaptive
control, as an alternative to reduce the system’s sensitivity due to modeling
mismatch. In [32], an adaptive strategy is used to generate a desired motion
trajectory in compliance with desired forces at the end effectors, even when
the payload stiffness is unknown. Furthermore, adaptation is used in [33] to
obtain variable damping and stiffness, exhibiting different impedance
characteristics according to the forces exerted by the environment.

In this paper, passivity techniques are used to guarantee system stability in
the presence of interaction forces and moments between the payload and the
robots.

The remainder of the paper is organized as follows: In Section II,
mathematical preliminaries and a brief background on passivity and
$\mathcal{L}_{2}$ stability results are presented. In Section III, a model of
the CRM is presented. Section IV discusses the semi-active control
implementation and associated energy balance equations. The overall control
architecture and the impedance control approach are discussed in sections V
and VI, respectively. Section VII introduces the optimization and a numerical
simulation is presented in section VIII. Finally, conclusions and
recommendations for future work are given in section IX.

##  II Mathematical preliminaries

In this section, we briefly recall some basic properties and classical results
of input-output stability and passive systems [37, 38]. Denote
$\mathbb{R}_{+}$ as the set of non-negative real numbers, and let
$\mathbb{R}^{n}$ be the $n$-dimensional vector space over $\mathbb{R}$. Define
$\Pi$ as the set of all measurable, real-valued, $n$-dimensional functions of
time $f(t):\mathbb{R}_{+}\rightarrow\mathbb{R}^{n}$ and $t\in\mathbb{R}_{+}$
and define the sets:

$\displaystyle\mathcal{L}^{n}_{2}$
$\displaystyle\triangleq\\{x\in\Pi|~{}\|f\|^{2}_{2}\triangleq\int_{0}^{\infty}\|f(t)\|^{2}dt<\infty\\}$
(1a) $\displaystyle\mathcal{L}^{n}_{\infty}$
$\displaystyle\triangleq\\{x\in\Pi|~{}\|f\|^{2}_{\infty}\triangleq\sup_{t}\|f(t)\|^{2}<\infty\\}$
(1b)

with $\|.\|$ the standard Euclidean norm. Also, extended spaces are defined
as:

$\displaystyle\mathcal{L}^{n}_{2e}$
$\displaystyle\triangleq\\{x\in\Pi|~{}\|f\|^{2}_{2,T}\triangleq\int_{0}^{T}\|f(\xi)\|^{2}d\xi<\infty,\forall~{}T\in\mathbb{R}_{+}\\}$
(2a) $\displaystyle\mathcal{L}^{n}_{\infty e}$
$\displaystyle\triangleq\\{x\in\Pi|~{}\|f\|^{2}_{\infty,T}\triangleq\sup_{T}\|f(T)\|^{2}<\infty,\forall~{}T\in\mathbb{R}_{+}\\}$
(2b)

Here, $\mathcal{L}^{n}_{2}$ and $\mathcal{L}^{n}_{2e}$ are inner product
spaces. The truncated inner product of two functions
$u(t),y(t)\in\mathcal{L}^{n}_{2e}$ is defined as:

$\langle y|u\rangle_{T}\triangleq\int_{0}^{T}u^{T}(\xi)y(\xi)d\xi$ (3)

where the two functions can be related by a dynamical operator, $H$, such that
$y=H(u)$. The operator $H$ is called causal if the value of output, $y$, at
time $t$ is a function of the value of input, $u$, up to time $t$ [38]. For a
causal operator, we have the following definitions:

######  Definition 1.

Let $H:\mathcal{L}^{n}_{2e}\rightarrow\mathcal{L}^{n}_{2e}$ be a causal
dynamic operator and $y=H(u)$ is the output. Then for some non-negative
constants $\beta$, $\epsilon$ and $\delta$, and
$\forall~{}u\in\mathcal{L}^{n}_{2e}$, $H(.)$ is called input-output strict
passive (IOSP) if:

$\langle
y|u\rangle_{T}\geq\epsilon\|u\|^{2}_{2,T}+\delta\|y\|^{2}_{2,T}-\beta$ (4)

$H$ is called passive if $\epsilon=\delta=0$, output strictly passive (OSP) if
$\epsilon=0$, and input strictly passive (ISP) if $\delta=0$ [38].

######  Definition 2.

The causal dynamical operator
$H:\mathcal{L}^{n}_{2e}\rightarrow\mathcal{L}^{n}_{2e}$ is said to be finite-
gain $\mathcal{L}_{2}$ stable if there are non-negative constants $\gamma_{0}$
and $\beta_{0}$ such that[39, 38]:

$\|y\|_{2,T}\leq\gamma_{0}\|u\|_{2,T}+\beta_{0}~{}~{}~{}~{}~{}\forall~{}T\geq
0$ (5)

So far, we have defined the concept of input-output stability for a single
causal system. The results can be extended for the feedback system as shown in
Fig. 1. Here we assume the feedback interconnection is well-defined. That is
for $r\triangleq(r_{1},r_{2})$ and $y\triangleq(y_{1},y_{2})$, the mapping
$y=\tilde{H}(r)$ is causal and
$\tilde{H}:\mathcal{L}^{n}_{2e}\rightarrow\mathcal{L}^{n}_{2e}$.

We elaborate the following result for the feedback interconnection:

######  Lemma 1.

Consider the well-defined feedback interconnection of two IOSP causal operator
dynamical systems $H_{1}$ and $H_{2}$ as shown in Fig. 1. The following
inequality is effective:

$c_{1}\|y_{1}\|_{2,T}^{2}+c_{2}\|y_{2}\|_{2,T}^{2}\leq
c_{3}\|r_{1}\|_{2,T}^{2}+c_{4}\|r_{2}\|_{2,T}^{2}-\hat{\beta}$ (6)

where:

$\displaystyle c_{1}$
$\displaystyle=\\{(\delta_{1}+\epsilon_{2})-\frac{\gamma_{1}}{2}-\epsilon_{2}\gamma_{4}\\}$
$\displaystyle c_{2}$
$\displaystyle=\\{(\delta_{2}+\epsilon_{1})-\frac{\gamma_{2}}{2}-\epsilon_{1}\gamma_{3}\\}$
$\displaystyle c_{3}$
$\displaystyle=\\{\frac{1}{2\gamma_{1}}+\frac{\epsilon_{1}}{\gamma_{3}}\\}$
$\displaystyle c_{4}$
$\displaystyle=\\{\frac{1}{2\gamma_{2}}+\frac{\epsilon_{2}}{\gamma_{4}}\\}$

for $\gamma_{j}>0,j\in\\{1,2,3,4\\}$ and constant $\hat{\beta}>0$.

Figure 1: Feedback interconnection with two external inputs.

###### Proof.

Since $H_{1}$ and $H_{2}$ are IOSP, we have:

$\langle
y_{i}|u_{i}\rangle_{T}\geq\epsilon_{i}\|u_{i}\|^{2}_{2,T}+\delta_{i}\|y_{i}\|^{2}_{2,T}-\beta_{i}\quad
i\in\\{1,2\\}$ (7)

From Fig. 1, we have $u_{1}=r_{1}-y_{2}$ and $u_{2}=r_{2}+y_{1}$, therefore:

$\langle y_{1}|u_{1}\rangle_{T}+\langle y_{2}|u_{2}\rangle_{T}=\langle
y_{1}|r_{1}\rangle_{T}+\langle y_{2}|r_{2}\rangle_{T}$ (8)

Using the interconnection relations, we can write:

$\displaystyle\|u_{1}\|^{2}_{2,T}$
$\displaystyle=\|r_{1}\|^{2}_{2,T}+\|y_{2}\|^{2}_{2,T}-2\langle
y_{2}|r_{1}\rangle_{T}$ $\displaystyle\geq\|y_{2}\|^{2}_{2,T}-2\langle
y_{2}|r_{1}\rangle_{T}$ (9a) $\displaystyle\|u_{2}\|^{2}_{2,T}$
$\displaystyle=\|r_{2}\|^{2}_{2,T}+\|y_{1}\|^{2}_{2,T}+2\langle
y_{1}|r_{2}\rangle_{T}$ $\displaystyle\geq\|y_{1}\|^{2}_{2,T}+2\langle
y_{1}|r_{2}\rangle_{T}$ (9b)

Using equations (7), (8), and (9), we have the following inequality:

$\begin{split}&\langle y_{1}|r_{1}\rangle_{T}+\langle
y_{2}|r_{2}\rangle_{T}+2\epsilon_{1}\langle
y_{2}|r_{1}\rangle_{T}-2\epsilon_{2}\langle y_{1}|r_{2}\rangle_{T}\geq\\\
&(\delta_{1}+\epsilon_{2})\|y_{1}\|^{2}_{2,T}+(\delta_{2}+\epsilon_{1})\|y_{2}\|^{2}_{2,T}-\beta_{1}-\beta_{2}\end{split}$
(10)

Using the Cauchy–Schwarz inequality, for any two arbitrary signals $r$ and
$y$, there is a $\gamma>0$, such that:

$\displaystyle\langle y|r\rangle_{T}$
$\displaystyle\leq\|y\|_{2,T}\|r\|_{2,T}+\frac{1}{2}(\frac{1}{\sqrt{\gamma}}\|r\|_{2,T}-\sqrt{\gamma}\|y\|_{2,T})^{2}$
$\displaystyle\leq\frac{1}{2\gamma}\|r\|^{2}_{2,T}+\frac{\gamma}{2}\|y\|^{2}_{2,T}$
(11a) $\displaystyle\langle y|r\rangle_{T}$
$\displaystyle\geq-\|y\|_{2,T}\|r\|_{2,T}-\frac{1}{2}(\frac{1}{\sqrt{\gamma}}\|r\|_{2,T}-\sqrt{\gamma}\|y\|_{2,T})^{2}$
$\displaystyle\geq-\frac{1}{2\gamma}\|r\|^{2}_{2,T}-\frac{\gamma}{2}\|y\|^{2}_{2,T}$
(11b)

If we assign $\gamma_{1},\gamma_{2},\gamma_{3}$ as constants for the pair of
signals $(r_{1},y_{1})$, $(r_{2},y_{2})$, and $(r_{1},y_{2})$, respectively,
in inequality (11a), and $\gamma_{4}$ for $(r_{2},y_{1})$ in inequality (11b),
then by substituting the obtained inequalities in inequality (10) and with
some simplification and manipulation, equation (6) can be obtained. ∎

Finally, we list two useful standard results:

######  Lemma 2.

The feedback in Fig. 1 is finite-gain $\mathcal{L}_{2}$ stable if
$\delta_{1}+\epsilon_{2}>0$, $\delta_{2}+\epsilon_{1}>0$ [40].

######  Theorem 1.

Let $y=H(s)u$, where $H(s)\in\mathbb{R}^{n\times n}$ is an exponentially
stable and strictly proper transfer function. If $u\in\mathcal{L}_{2}$, then
$y\in\mathcal{L}_{2}\cap\mathcal{L}_{\infty}$, $\dot{y}\in\mathcal{L}_{2}$ and
$y\rightarrow 0$ as $t\rightarrow\infty$. In addition, if $u\rightarrow 0$ as
$t\rightarrow\infty$, then $\dot{y}\rightarrow 0$ [38, 41].

Figure 2: Illustration of $N$ robots grasping an object. The coordinate frames
attached to the end-effector, robots’ base, load’s center of mass, and
arbitrary world frame are, respectively, $\Sigma_{ei}$, $\Sigma_{i}$,
$\Sigma_{O}$, $\Sigma_{W}$.

##  III Modeling

In this section, we introduce the dynamical model of $N$ cooperative robots
carrying a rigid load by grasping it as shown in Fig. 2. The objective in this
section is to model the robots, the load and to establish the coupling between
the robots. First we build the augmented model of each robot, its joint
mechanisms and the DC motor actuators. Then, based on the augmented model of
the individual robots, a comprehensive model of the cooperative robots is
introduced. The load is modeled assuming to be a rigid object. Finally, the
kinematic and dynamic coupling of the closed chain system is discussed.

To develop the dynamics equations, for the remainder of this paper, unless
otherwise told, the following notations are used: any parameter with subscript
$i$ indicates the parameter for ith robot, $i\in\\{1,...,N\\}$, and subscript
$ij$ indicates the parameter for jth joint of ith robot,
$j\in\\{1,2,...,n\\}$.

###  III-A Comprehensive dynamics of cooperative robots

We assume there are $N$ non-redundant robots carrying an object. The dynamics
of ith robot, without considering the drive systems, modeled as:

$D^{o}_{i}(q_{i})\ddot{q}_{i}+C^{o}_{i}(q_{i},\dot{q}_{i})\dot{q}_{i}+g_{i}(q_{i})=\tau_{i}+J_{i}^{T}(q_{i})F_{i}$
(12)

where $q_{i}~{}\in~{}\mathbb{R}^{n}$ is the vector of joint coordinates,
$D^{o}_{i}(q_{i})$ is the inertia matrix, $C^{o}_{i}(q_{i})$ is the matrix
accounting for centripetal and Coriolis effects, $g_{i}(q_{i})$ is the gravity
vector, $J_{i}~{}\in~{}\mathbb{R}^{n\times n}$ is the Jacobian matrix which is
assumed to be nonsingular,
$F_{i}=[f^{T}_{i},m^{T}_{i}]^{T}~{}\in~{}\mathbb{R}^{n}$ is the vector of
forces $(f_{i})$ and moments $(m_{i})$ applied by the object at the end-
effector, and
$\tau_{i}=[\tau_{i1},\tau_{i2},...,\tau_{in}]^{T}~{}\in~{}\mathbb{R}^{n}$ is
the vector of forces/moments applied at the joints.

All $n$ joints of the robot are considered to be _semi-active_. A semi-active
joint mechanism (JM) only exchange mechanical power with the robot, and it is
connected to an electric energy storage element source, e.g. an
ultracapacitor. We consider that all semi-active joints of all robots are
connected to a common storage element, as illustrated in Fig 3, and they are
regenerative, allowing the power source to be charged whenever surplus
mechanical energy from the robots is available to flow back through DC motors.
Also, in each semi-active JM, the interface torque between the JM and the jth
robot link can be described by:

$\tau_{ij}=-J_{ij}\bar{n}^{2}_{ij}\ddot{q}_{ij}-b_{ij}\bar{n}^{2}_{ij}\dot{q}_{ij}+\bar{n}_{ij}\tau_{ind_{ij}}$
(13)

where $J_{ij}$, $\bar{n}_{ij}$, and $b_{ij}$ are the JM moment of inertia, the
gear ratio, and friction coefficient, respectively.
$\tau_{ind_{ij}}=\alpha_{ij}I_{{ij}}$ is the induced torque of the motor,
where $I_{{ij}}$ is the current in the motor and $\alpha_{ij}$ is the motor
constant.

By combining the dynamics of robot and JMs, the augmented model of robot-JMs
can be obtained. This can be done by finding $\tau_{ind_{ij}}$. In each JM, an
ideal regenerative four-quadrant power conversion element (motor driver) is
used to control the amount and direction of the applied voltage to the DC
motor. Fig. 4 shows the model of the JM, where the converter voltage ratio is
defined as $u_{ij}=V_{{ij}}/V_{s}$. With this, the applied voltage to a motor
can be written as:

$V_{{ij}}=R_{ij}I_{{ij}}+a_{ij}\dot{q}_{ij}$ (14)

where $R_{ij}$ is the resistance of the motor, and
$a_{ij}=\alpha_{ij}\bar{n}_{ij}$ [1]. By substituting $V_{{ij}}$ with $u_{ij}$
in equation (14) and obtaining the induced torque of motor as a function of
voltage ratio and angular velocity and finally, substituting the induced
torque in equation (13), the applied torque can be rewritten as:

$\tau_{ij}=-J_{ij}\bar{n}^{2}_{ij}\ddot{q}_{ij}-(b_{ij}\bar{n}^{2}_{ij}+\frac{a_{ij}^{2}}{R_{ij}})\dot{q}_{ij}+\frac{a_{ij}u_{ij}}{R_{ij}}V_{s}$
(15)

Replacing $\tau_{ij}$ from equation (15) into equation (12) and moving terms
containing $\dot{q}_{i}$ and $\ddot{q}_{i}$ from right- to left-hand side,
yields the following augmented robot-JM model:

$D_{i}(q_{i})\ddot{q}_{i}+C_{i}(q_{i},\dot{q}_{i})\dot{q}_{i}+g_{i}(q_{i})=\mathcal{T}_{i}+J_{i}^{T}(q_{i})f_{i}$
(16)

where in the coupled dynamics, $D^{o}_{i}$ and $D_{i}$ are the same in all
elements except in diagonal elements such that:
$D_{i}=D^{o}_{i}+\mbox{diag}(J_{ij}\bar{n}^{2}_{ij})$, also $C_{i}$ and
$C^{o}_{i}$ are only different in diagonal terms such that
$C_{i}=C^{o}_{i}+\mbox{diag}(b_{ij}\bar{n}^{2}_{ij}+a_{ij}^{2}/R_{ij})$, and
$\mathcal{T}_{i}=[U_{i1},...,U_{i{n}}]^{T}$, where:

$U_{ij}=\frac{a_{ij}u_{ij}}{R_{ij}}V_{s}$ (17)

Finally, by combining the augmented dynamics of all $N$ robots, the
comprehensive dynamics of CRM can be written as:

$D(q)\ddot{q}+C(q,\dot{q})\dot{q}+G(q)=\mathcal{T}+J^{T}(q)F$ (18)

where $X=[X^{T}_{1},...,X^{T}_{N}]^{T}$, $X\in\\{q,G,\mathcal{T},F\\}$ and
$Y=\mbox{diag}(Y_{1},...,Y_{N})$, $Y\in\\{D,C,J\\}$. Hereafter, for ease of
notation, we use $J^{T}(q)F\triangleq\mathcal{T}_{ext}$.

Figure 3: Illustration of semi-active joints connection to a single power
source. Figure 4: Semi-active joint setup and its connection to motor driver
and ultracapacitor.

###  III-B Dynamics of the load

The dynamics and kinematics of manipulators are coupled because the contact
forces in all robots are interacting through the load. The dynamic coupling
effects can be measured using the dynamics of the load. It is assumed that the
mass of load is in its center of mass at $\Sigma_{O}$ and the orientation is
the orientation of coordinate frame $\Sigma_{O}$ as shown in Fig. 2. In the
world frame ($\Sigma_{W}$), the translation and rotation dynamics for the load
are:

$\begin{bmatrix}m_{o}I_{3\times 3}&0_{3\times 3}\\\ 0_{3\times
3}&I_{o}\end{bmatrix}\begin{bmatrix}\ddot{P}_{o}\\\
\dot{\omega}_{o}\end{bmatrix}+\begin{bmatrix}-m_{o}\textbf{g}\\\
\Omega_{o}I_{o}\omega_{o}\end{bmatrix}=J_{o}^{T}f\triangleq F_{o}$ (19)

where
$[P_{o}^{T},\omega_{o}^{T}]^{T}=[x_{o},y_{o},z_{o},\omega_{x},\omega_{y},\omega_{z}]^{T}$
is the vector of positions and angular velocities of the load. $I_{3\times 3}$
and $0_{3\times 3}$ are the identity and the null matrices, respectively,
$m_{o}$ is the mass of the load, and $I_{o}$ is the moment of inertia about
the center of mass. $\Omega_{o}$ is a skew-symmetric matrix, representing the
cross product of $\omega_{o}\times I_{o}\omega_{o}$, $F_{o}$ is the vector of
all forces/moments applied at the center of mass of load, and
$J_{o}^{T}=[J_{o_{1}}^{T}...J_{o_{N}}^{T}]\in\mathbb{R}^{6\times 6N}$ is
called the grasp matrix and defined as:

$J_{o}^{T}=\begin{bmatrix}I_{3\times 3}&0_{3\times 3}&...&I_{3\times
3}&0_{3\times 3}\\\ -S(r_{1})&I_{3\times 3}&...&-S(r_{N})&I_{3\times
3}\end{bmatrix}$ (20)

where $r_{i}$ is the vector from $\Sigma_{ei}$ to $\Sigma_{o}$ (see Fig. 2)
and $S(r_{i})$ is a skew-symmetric matrix, representing $r_{i}\times F_{i}$.
The applied forces/moments by manipulators, $f$, can be decomposed to motion-
inducing and internal parts as:

$f=f_{M}+f_{I}$ (21)

where $f_{M}$ is the motion-inducing part that may balance the object’s
dynamics and $f_{I}$ is the internal part consists of compressive, tensile and
torsion forces/moments. Since $f_{I}$ does not contribute to the motion of the
object and has no net force, thus it spans the null space of $J_{o}^{T}$.
Based on this fact, decomposition as equations (22) can be used to find
$f_{M}$ and $f_{I}$ where $(J_{o}^{T})^{+}$ is the generalized inverse of
$J_{o}^{T}$ [25].

$\displaystyle f_{M}\triangleq$
$\displaystyle[f_{M_{1}}^{T},...,f_{M_{N}}^{T}]^{T}=(J_{o}^{T})^{+}J_{o}^{T}f$
(22a) $\displaystyle f_{I}\triangleq$
$\displaystyle[f_{I_{1}}^{T},...,f_{I_{N}}^{T}]^{T}=(I_{6N\times
6N}-(J_{o}^{T})^{+}J_{o}^{T})f$ (22b)

Using equations (22) requires measuring force/moment at the end-effectors.
This can be accomplished by installing a force sensor. To avoid direct
measurement of force/moment, methods based on load distribution are introduced
in [22, 20, 24]

###  III-C Coupling in cooperative robots

The dynamics (load dynamics) and kinematics coupling cause constraint motion
in each robot due to the closed chain configuration of the CRM. The kinematics
coupling is defined as a set of equations, relating the translational and the
rotational motions of the end-effectors that hold and move the rigid load
securely [18]. In the world frame, the translational and rotational
constraints are defined as:

$\displaystyle P_{i}(q_{i})+r_{i}(q_{i})$
$\displaystyle=P_{\acute{i}}(q_{\acute{i}})+r_{\acute{i}}(q_{\acute{i}})$
(23a) $\displaystyle\Gamma_{i}(q_{i})-\Gamma_{k}(q_{\acute{i}})$
$\displaystyle=\delta R_{i\acute{i}}\quad i,\acute{i}\in\\{1,...,N\\}$ (23b)

where the first constraint expresses the concatenated vectors, through each
robot, that connect $\Sigma_{w}$ to $\Sigma_{o}$, $\Gamma_{i}(q_{i})$ is the
vector that expresses the orientation of the ith end-effector, and $\delta
R_{i\acute{i}}$ is a constant vector showing the difference between the
orientation of the end-effectors at all time of the maneuver.

For $N$ robots, there are $N(N-1)/2$ translational constraint vectors in the
format of equation (23a) which each constraint contains three independent
scalar equations, i.e. constraints in $(X,Y,Z)_{W}$ directions. There are
$N(N-1)/2$ rotational constraint vectors in the format of equation (23b).
Depending on the representation of the orientation, e.g. Euler or quaternion,
various number of scalar constraints can be obtained.

##  IV Control implementation and energy balances

To devise a control scheme for CRM, there are two main consecutive steps. The
first step is to design a controller to achieve the motion control objective
of CRM; moving an object along a desired trajectory while grasping it securely
without any damage to either object or robots. In the second step, a
relationship is established between the control signal in the first step and
the voltage ratios $u_{ij}$, which are available control inputs. This arises
from the incorporation of storage element voltage feedback $V_{s}$ in the
augmented model. The voltage ratios $u_{ij}$ are adjusted in this work using a
method called semi-active virtual control (SVC) [2]. Using SVC enables us to
describe energy exchange with the storage element and write energy balance
equations for the closed system including robots and storage elements.

###  IV-A Semi-active virtual control strategy

Assume that a motion controller, called virtual control law ($\tau^{v}_{ij}$),
for $U_{ij}$ in equation (18) has been designed. For $\tau^{v}_{ij}$ to meet
the control objectives for the augmented model, a solution for $u_{ij}$ is
sought that enforces:

$\tau^{v}_{ij}=\frac{a_{ij}u_{ij}}{R_{ij}}V_{s}$ (24)

To design the virtual control, any feedback law compatible with the desired
motion control objectives can be selected. If virtual matching, i.e.
$\tau^{v}_{ij}=U_{ij}$, holds at all times, properties that apply to the
virtual design such as stability, tracking performance, robustness, etc. will
be propagated to the actual system. Virtual matching is possible as long as
the storage element has nonzero voltage and it will hold exactly whenever the
power converters are not saturated (e.g., $-1\leq u_{ij}\leq 1$) and there is
accurate knowledge of parameters $a{ij}$ and $R_{ij}$. The modulation law for
exact virtual matching is obtained by solving for $u_{ij}$ from equation (24).

The SVC technique uses $V_{s}$ as feedback information in the virtual matching
of equation (24). This approach permits the formulation of control laws and
energy balance equations _without the need for a model of the storage
element_. Devices such as ultracapacitors and batteries have complex and
uncertain models.

###  IV-B Internal and external energy balance

The internal energy balance describes the power exchange between the semi-
active joints and power storage element. Assuming there is no power lost
during power exchange between the power source and DC motors, the input/output
energy balance for the motor drives can be written as:

$V_{s}I_{s}=\sum_{i=1}^{N}\sum_{j=1}^{n}V_{{ij}}I_{{ij}}$ (25)

where $V_{s}$ and $I_{s}$ are the voltage and current of the power storage,
respectively. Dividing both sides by $V_{s}$, substituting current from
equation (14) into equation (25), and taking the integral in the time interval
$[0,T]$, the internal energy balance equation can be obtained as follows:

$\Delta\,E_{s}=\int_{0}^{T}(\dot{q}^{T}{\mathcal{T}}^{v}-({\mathcal{T}}^{v})^{T}R_{a}{\mathcal{T}}^{v})dt$
(26)

where $\Delta E_{s}$ is the electric energy change in the power storage,
$\mathcal{T}^{v}=[(\tau^{v}_{1})^{T},...,(\tau^{v}_{N})^{T}]^{T}$,
$\tau^{v}_{i}=[\tau^{v}_{i1},...,\tau^{v}_{in}]^{T}$, and
$R_{a}=\mbox{diag}(R_{ij}/a^{2}_{ij})\in\mathbb{R}^{nN\times nN}$. The
electric energy, $E_{s}$ is assumed to be a positive-definite function of the
power storage charge $y$, i.e. $E_{s}=E_{s}(y)$. We assume during a maneuver,
there is enough charge in the storage element for maneuver execution. An
example of a power source is the ultracapacitor. All results obtained in this
section can be used interchangeably for virtual control design using an
ultracapacitor as power source, i.e. $V_{s}\triangleq V_{cap}$.

An external energy balance for CRM can be derived as:

$W_{{ext}}=\Delta\,E_{c}\,+\,\Delta\,E^{Tot}_{m}\,+\,\Sigma^{Tot}_{m}\,+\,\Sigma_{e}$
(27)

where $W_{{ext}}$ is the work done by external forces/moments, $\Delta
E^{Tot}_{m}$ is the change in the mechanical energy of CRM, $\Sigma^{Tot}_{m}$
is the mechanical losses, and $\Sigma_{e}$ is the resistance heating (Joule)
losses [1]. The derivation and definition of terms in equation (27) is
outlined in Appendix A.

Figure 5: Control scheme to control CRM in joint space, using impedance
relation and SVC.

##  V Control scheme

In this study, the motion task is defined as following a desired trajectory
closely but allowing deviations from the desired trajectory to limit the
interaction forces/moments that could damage the robots or load. This can be
captured by requiring all internal signals in a feedback control loop to be
bounded, which is conveniently achieved with the passivity and
$\mathcal{L}_{2}$ stability tools. Therefore, a control scheme based on
impedance relation is developed to accomplish the motion task. Impedance
control enforces a compliant behavior to avoid large values of contact
force/moment [25, 34]. This technique is particularly useful in CRM since
there is interaction between robots and the load.

The proposed control scheme is according to Fig (5). We assume the load’s
center of mass is following a pre-designed desired trajectory in world space.
That is, the translational and rotational information of center of mass are
$P^{d}_{o}$ and $R^{d}_{o}$, respectively.

For the ith robot, the end-effector’s desired position ($P^{d}_{i}$),
orientation ($R^{d}_{ei}$), linear velocity ($\dot{P}^{d}_{i}$), angular
velocity ($\omega^{d}_{ei}$), proportional linear acceleration
($\ddot{P}^{d}_{i}$), and angular acceleration ($\dot{\omega}^{d}_{ei}$) can
be calculated using:

$\begin{split}P^{d}_{i}&=P^{d}_{o}-R^{d}_{o}r_{i}\\\ R^{d}_{ei}&=R^{d}_{o}\\\
\dot{P}^{d}_{i}&=\dot{P}^{d}_{o}+S(R^{d}_{o}r_{i})\omega^{d}_{o}\\\
\omega^{d}_{ei}&=\omega^{d}_{o}\\\
\ddot{P}^{d}_{i}&=\ddot{P}^{d}_{o}+S(\omega^{d}_{o})S(R^{d}_{o}r_{i})\omega^{d}_{o}+S(R^{d}_{o}r_{i})\dot{\omega}^{d}_{o}\\\
\dot{\omega}^{d}_{ei}&=\dot{\omega}^{d}_{o}\\\ \end{split}$ (28)

where $S(.)$ is the matrix performing cross-product and $R^{d}_{o}$ is a
matrix transformation that transfers $r_{i}$ from object’s frame
($\Sigma_{o}$) to the frame attached to the ith end-effector ($\Sigma_{ei}$)
[34]. Using translational and rotational information of the end-effector, from
Equation. (28), kinematics of the robot, and any inverse kinematics technique,
the desired position ($q^{d}_{i}$), velocity ($\dot{q}^{d}_{i}$) and
acceleration ($\ddot{q}^{d}_{i}$) of joints, can be obtained [42, 34]. The
desired states in joint space along with the actual states and the interaction
force between robots and the load are used in the impedance relation to
calculate the virtual control, ${\mathcal{T}}^{v}$. The SVC uses the power
source voltage information to regulate the virtual control and creates the
actual control signal.

##  VI Impedance control

The characteristics of an impedance control problem are given as a
relationship between a desired trajectory and a desired dynamics. In this
sense, the system is forced to establish a mathematical relationship between
the interaction forces and the position error. To design the virtual control,
we enforce the following relationship introduced in [41]:

$\lim_{t\to\infty}M\ddot{\tilde{q}}+B\dot{\tilde{q}}+K\tilde{q}=\mathcal{T}_{ext}$
(29)

where $\tilde{q}=q^{d}-q$ and the inertia ($M$), damping ($K$), and stiffness
($K$) are diagonal positive-definite matrices called gain matrices here, and
all parameters are defined for each robot accordingly.

To achieve the objective in equation 29, we define an auxiliary error as:

$\zeta=\tilde{q}-[p^{2}M+pB+K]^{-1}\mathcal{T}_{ext}$ (30)

where $p=d/dt$. It is easy to verify that if $\zeta$ converges to zero, the
desired impedance in equation (29) is achieved. Therefore, we define the
following variables:

$\displaystyle S=-(\dot{\zeta}+\Lambda\zeta)$ (31a)
$\displaystyle\dot{q}_{r}=\dot{q}-S$ (31b)

where $\Lambda$ is a diagonal positive definite matrix. The virtual torque can
be calculated using:

${\mathcal{T}}^{v}=D(q)\ddot{q}_{r}+C(q,\dot{q})\dot{q}_{r}+G(q)-K_{D}S-\mathcal{T}_{ext}$
(32)

where $K_{D}$ is a positive definite matrix. From a practical point of view,
$\dot{q}_{r}$ and $\ddot{q}_{r}$ in equation. (32) can be implemented by:

$\displaystyle\dot{q}_{r}$
$\displaystyle=\dot{q}^{d}+\Lambda{\tilde{q}}-(pI+\Lambda)[p^{2}M+pB+K]^{-1}\mathcal{T}_{ext}$
(33a) $\displaystyle\ddot{q}_{r}$
$\displaystyle=\ddot{q}^{d}+\Lambda\dot{\tilde{q}}-p(pI+\Lambda)[p^{2}M+pB+K]^{-1}\mathcal{T}_{ext}$
(33b)

which shows that the implementation of the controller does not require the
measurement of the acceleration.

######  Theorem 2.

Consider the auxiliary error in equation (30) and $S$ in equation (31a). Then
the followings hold:

  1. (a)

$S\in\mathcal{L}_{2}$

  2. (b)

$\zeta\in\mathcal{L}_{2}\cap\mathcal{L}_{\infty}$,
$\dot{\zeta}\in\mathcal{L}_{2}$

  3. (c)

$\zeta\rightarrow 0$ as $t\rightarrow\infty$.

  4. (d)

$q,\dot{q}\in\mathcal{L}_{2e}$ and bounded.

and therefore, the closed-loop system is $\mathcal{L}_{2}$ finite gain stable.

###### Proof.

(a) Assuming ${\mathcal{T}}^{v}={\mathcal{T}}$, by substituting equation (32)
in equation (18), the closed-loop system is obtained as:

$D(q)\dot{S}+[C(q,\dot{q})+K_{D}]S=0$ (34)

We choose the following Lyapunov function candidate for the closed-loop
system:

$V(t)=\frac{1}{2}S^{T}D(q)S$ (35)

Differentiating Lyapunov function with respect to time and with some
manipulation and simplification, it yields:

$\dot{V}(t)=-S^{T}K_{D}S$ (36)

Equations (35) and(36) imply that $S$ is asymptotically stable and therefore
$S\in\mathcal{L}_{2}$.

(b,c) Using the definition of $S$ in equation (31a), $\zeta$ and its
derivative can be written as:

$\displaystyle\zeta$ $\displaystyle=-(pI+\Lambda)^{-1}S$ (37a)
$\displaystyle\dot{\zeta}$ $\displaystyle=-p(pI+\Lambda)^{-1}S$ (37b)

where $(pI+\Lambda)^{-1}$ describes a strictly proper, exponentially stable
transfer function [39]. Using Theorem 1 and the fact that
$S\in\mathcal{L}_{2}$, we conclude
$\zeta\in\mathcal{L}_{2}\cap\mathcal{L}_{\infty}$,
$\dot{\zeta}\in\mathcal{L}_{2}$, and hence $\zeta\rightarrow 0$ as
$t\rightarrow\infty$. The fact that auxiliary error, $\zeta$, converges to
zero, shows the objective of achieving the desired impedance relation in
equation (29).

(d) By taking the derivative of both sides of equation (30) and rearranging
the terms, we have:

$\dot{q}=\dot{q}^{d}-\dot{\zeta}-p[p^{2}M+pB+K]^{-1}\mathcal{T}_{ext}$ (38)

Using equation (37b), equation (38) can be represented in a feedback
interconnection form as depicted in Fig 6(a). Here we define
$\mbox{ENV}(\dot{q})$ as a passive operator that maps $\dot{q}$ to
$\mathcal{T}_{ext}$. The environment (load) is passive because the load is
assumed to be a pure mass that is held by grasping. Since it is passive, we
can write:

$\langle\mathcal{T}_{ext}|\dot{q}\rangle_{T}\geq-\tilde{\beta}$ (39)

Figure 6: (a) Feedback interconnection with one external input. (b)
Transformation of interconnection to two external inputs.

It can be easily shown that the operator $\bar{H}_{2}\triangleq
p[p^{2}M+pB+K]^{-1}$ is only passive. Therefore, this feedback loop does not
reveal any information about boundedness of signals and it is inconclusive. By
performing the loop transformation technique [38, 37], the equivalent
interconnection can be presented as Fig 6(b). In this figure, the virtual
environment operator ($H_{1}$) is composed of passive environment as
represented by equation (39) and a feed forward term. So it is an ISP system
with the following property:

$\langle
y_{1}|\dot{q}\rangle_{T}\geq-\tilde{\beta}+\underline{\lambda}_{B}\|\dot{q}\|^{2}_{2,T}$
(40)

where $\underline{\lambda}_{B}>0$ is the smallest eigenvalue of the gain
matrix $B$. The operator $H_{2}\triangleq pB[p^{2}M+K]^{-1}$, which maps
$u_{2}$ to $y_{2}$, is only passive. By comparing Fig 6(b) and Fig 1, and
using equation (4), we can conclude that
$\epsilon_{1}=\underline{\lambda}_{B}$,
$\epsilon_{2}=\delta_{1}=\delta_{2}=0$. Based on Lemma 1, we can write:

$\bar{c}_{2}\|y_{2}\|_{2,T}^{2}\leq\bar{c}_{3}\|\dot{q}^{d}-\dot{\zeta}\|_{2,T}^{2}+\bar{c}_{4}\|\dot{\zeta}-\dot{q}^{d}\|_{2,T}^{2}-\bar{\bar{\beta}}$
(41)

where $\bar{c}_{i}$ can be always chosen as positive numbers. It was shown
that $\zeta\in\mathcal{L}_{2}$ and also using the fact that desired trajectory
is bounded, we can conclude $\dot{q}^{d},\dot{\zeta}\in\mathcal{L}^{n}_{2e}$.
Therefore, the right-hand side of equation (41) is in $\mathcal{L}^{n}_{2e}$
and so is the left-hand side, i.e. $y_{2}\in\mathcal{L}^{n}_{2e}$ and bounded.
Consequently, all signals inside the loop are bounded, i.e.
$\dot{q},\mathcal{T}_{ext},y_{1},u_{2}\in\mathcal{L}^{n}_{2e}$.

In particular, $\exists~{}\bar{\mathcal{T}}$ such that
$||\mathcal{T}_{ext}||\leq\bar{\mathcal{T}}$. The impedance relation as a
function of the auxiliary error in equation (30), can be rewritten as:

$M\ddot{Z}+B\dot{Z}+KZ=\mathcal{T}_{ext}$ (42)

where $Z=\tilde{q}-\zeta$. It can be shown that all solutions of equation (42)
comply with the inequality:

$\mbox{max}(\|\dot{Z}\|_{2,T},\|Z\|_{2,T})\leq C^{-\kappa
t}\sqrt{\|\dot{Z}_{0}\|^{2}+\|Z_{0}\|^{2}}+\frac{C\bar{\mathcal{T}}}{\kappa}$
(43)

where $C$ is a constant, $\kappa>0$, and $\dot{Z}_{0},Z_{0}$ are the initial
conditions [43]. Because the right-hand side of equation (43) is monotonically
decreasing and tends to a finite value and $\zeta\in\mathcal{L}_{2}$, we have
$q\in\mathcal{L}^{n}_{2e}$.

∎

##  VII Optimization problem

We aim to find the impedance gains, $B$ and $K$, that maximize the energy
regeneration given the pre-designed references used in the impedance relation
of 29. Maximizing $\Delta E_{s}$ in equation (26) means less amount of energy
is drawn from the storage element. A value of $\Delta E_{s}>0$ indicates
energy regeneration and $\Delta E_{s}<0$ indicates energy consumption.
Considering constant impedance parameters first, we define the damping and
stiffness gains as:

$\begin{cases}B=B_{c}+\bar{B}\\\ K=K_{c}+\bar{K}\\\ \end{cases}$ (44)

where $B_{c}$ and $K_{c}$ are fixed and $\bar{B}$ and $\bar{K}$ are limited
within an upper- and lower-bounds. The optimization is aimed to find $\bar{B}$
and $\bar{K}$ such that $\Delta E_{s}$ is maximized. Considering equation (26)
and the constraints in equation (23), a static optimization problem is
formulated as:

$\displaystyle\max_{\acute{B},\acute{K}}~{}\Delta
E_{s}=\int_{0}^{T}(\dot{q}^{T}{\mathcal{T}}^{v}-({\mathcal{T}}^{v})^{T}R_{a}{\mathcal{T}}^{v})dt$
(45a) $\displaystyle\mbox{subject
to:}\begin{cases}\mbox{I}&:P_{i}(q_{i})+r_{i}(q_{i})=P_{\acute{i}}(q_{\acute{i}})+r_{\acute{i}}(q_{\acute{i}})\\\
\mbox{II}&:\Gamma_{i}(q_{i})-\Gamma_{k}(q_{\acute{i}})=\delta
R_{i\acute{i}}\quad i,\acute{i}\in\\{1,...,N\\}{}\\\
\mbox{III}&:-V_{s}\bar{a}_{R}\leq\mathcal{T}^{v}\leq V_{s}\bar{a}_{R}\\\
\mbox{IV}&:LB_{B}\leq\acute{B}\leq UB_{B}\\\ \mbox{V}&:LB_{K}\leq\acute{K}\leq
UB_{K}\\\ \end{cases}$ (45b)

where the conditions I and II are the translational and rotational
constraints, condition III is derived from equation (24) and the fact that
$-1\leq u_{ij}\leq 1$.
$\bar{a}_{R}=\mbox{Vec}(a_{ij}/R_{ij})\in\mathbb{R}^{nN}$ and $\mbox{Vec}(.)$
represents the vectorization operator which creates a column vector by
stacking $a_{ij}/R_{ij}$ for all joints. The lower- and upper-bound for the
damping and stiffness gains are indicated in conditions IV and V,
respectively. These bounds determine the search area for the gains. The
problem can be solved with a variety of suitable methods, such as the genetic
algorithm used in the simulation example.

##  VIII Simulation

To simulate CRM system using the proposed impedance control, two identical RRR
planar robots are considered. The robots are grasping a rigid rod as shown in
Fig. 7. It is assumed the actuators in the robots are DC motors and all joints
are powered using an ultracapacitor as storage element. The robots’ and the
rod’s parameters are given in Table I.

TABLE I: The parameters for two identical robots and the load (rod). Parameter | Value | Unit
---|---|---
Robot: |  |
1st, 2nd, and 3rd arm lengths | [0.425,0.39,0.13] | m
1st, 2nd, and 3rd arm masses | [8.05,2.84,1.37] | kg
DC motor resistance | 0.4 | $\Omega$
DC motor torque constant | 0.07 | $\Omega$
DC motor gear ratio | 50 | -
Rod: |  |
Mass | 5 | kg
Length ($L_{0}$) | 0.5 | m
Figure 7: Setup for two identical cooperative robots carrying a load. The
world frame is attached to the base of the left robot. The distance between
robots is $d_{0}=0.8~{}m$ and robots are placed on the same level. $(.)_{ij}$
represents any parameter for the jth joint of ith robot.

The motion task objective is defined as moving the load’s center of mass (CM)
from initial position, $(x,y)_{0}$, to final position, $(x,y)_{f}$, along a
pre-designed desired trajectory. The final condition is relaxed to allow for a
search of the optimal impedance gains. Accordingly, a set has been defined as:

$IS=\\{p_{o}(t)|~{}||p_{o}(t)-p^{d}_{o}(t_{f})||\leq\epsilon_{f}\\}$ (46)

where $p_{o}(t)$ is the position of the load’s CM, $p^{d}_{o}(t_{f})$ is the
final desired position, $||.||$ is the Euclidean norm, and $\epsilon_{f}$ is a
scalar boundary. It is assumed that the motion tasks starts from the initial
position, and it is accomplished whenever the final position enters the $IS$
set. After finishing the maneuver, the load will be detached from the robots.

The desired load trajectory is based on quintic polynomial and consists of
moving the load 0.4 m in the x direction, -0.4 m in the y direction, and a
rotation of zero degrees in 1 s. The desired trajectories in joint space is
obtained using equations 28.

The fixed inertia, damping, and stiffness impedance gains are selected as
$M=\mbox{diag}(18)$, $B_{c}=\mbox{diag}(197.5)$, and $K_{c}=\mbox{diag}(825)$.
Note that the same parameters are selected for both robots. The optimization
in equation 45 is solved using genetic algorithm (GA). The optimization
searches for 12 variables (one damping and one stiffness gain for each joint).
The upper- and lower-bounds for the damping and stiffness gains in each joint
are $-22\leq\bar{B}_{ij}\leq 22$ and $-75\leq\bar{K}_{ij}\leq 75$,
respectively. The values and types of GA operators of this study are given in
Table II.

TABLE II: Parameters of genetic algorithm. Parameter | Value
---|---
Initial population size | 50
Max. No. of generation | 30
Crossover probability | 0.75
Mutation rate | 0.02

The optimization gives the following impedance gains as optimal values:

$\displaystyle B_{1}=\begin{bmatrix}176.1&0&0\\\ 0&178.0&0\\\
0&0&181.0\end{bmatrix},B_{2}=\begin{bmatrix}175.6&0&0\\\ 0&176.8&0\\\
0&0&176.3\end{bmatrix}$ $\displaystyle K_{1}=\begin{bmatrix}752.9&0&0\\\
0&754.2&0\\\ 0&0&763.6\end{bmatrix},K_{2}=\begin{bmatrix}756.4&0&0\\\
0&761.2&0\\\ 0&0&755.1\end{bmatrix}$

where subscripts 1 and 2 denote the matrix for the first and second robot (in
Fig. 7, the robot on the left is named robot 1 and the right one is called
robot 2). Figures 8 and 9 show the time histories of the reference trajectory
and the actual angles for both robots. The reference trajectories are closely
tracked due to the relatively high value chosen for $K_{c}$. The 2D movement
is depicted in Fig. 10 and the virtual controls are shown in Fig.11.

The power consumption in each joint is shown in Fig. 12. Positive power
indicates power consumption by the joint and negative power shows energy
regeneration. The energy consumption and regeneration is more pronounced in
the first joint of each robot.

Figure 13 shows the Sankey diagrams for the external energy balance of
equation 27. Most of the energy needed to accomplish the motion task was
recovered from the potential energy difference between the initial and final
positions. This is a direct consequence of maximizing energy regeneration. To
study the effect of energy regeneration, we conclude the results by defining
the effectiveness of energy regeneration as [1]:

$\epsilon=1-\frac{\Delta E_{R}}{\Delta E_{NR}}$ (47)

where $\Delta E_{R}$ and $\Delta E_{NR}$ are the system energy consumption
with and without energy regeneration, respectively. $\Delta E_{NR}$ is
computed by integrating the power flows in all joints, assuming any negative
power is dissipated (i.e. $P_{ij}~{}(P_{ij}~{}\leq~{}0)=0$). We have
$0\leq\epsilon\leq 1$ where $\epsilon=0$ means energy regeneration has zero
effect in reducing the energy consumption and $\epsilon=1$ indicates that
energy regeneration completely reduces energy consumption. For the condition
of simulation, $\Delta E_{R}=9.69~{}J$ and $\Delta E_{NR}=25.84~{}J$ results
in $\epsilon=0.62$. This shows approximately 60% reduction in energy
consumption due to energy regeneration.

Figure 8: The actual and desired joint angles for Robot #1. Figure 9: The
actual and desired joint angles for Robot #2. Figure 10: Movement in 2D.
Frames are captured in each 0.05 s. Figure 11: Virtual torques applied to the
robots. Figure 12: Power in each joint. Figure 13: Sankey diagram showing the
energy balance for the CRM when following the desired trajectory.

###  VIII-A Variable impedance parameters

The optimization in equation 45 searched for constant values of impedance gain
during the motion task. It is possible to look for variable gains, i.e. $B(t)$
and $K(t)$. Variable impedance shows better versatility when there is
interaction with environment, and it has been shown to have better performance
[44, 29, 45, 46, 47]. Optimal variable gains may result in further reductions
in energy consumption.

If the damping and stiffness gains are time-varying matrices constrained to be
positive definite, the relation between $y_{2}$ and $u_{2}$ in Fig. 6(b) can
be written in the format of a linear time-varying system as:

$\displaystyle\dot{\mathcal{X}}(t)$
$\displaystyle=\mathfrak{A}(t)\mathcal{X}(t)+\mathfrak{B}u_{2}(t)$ (48a)
$\displaystyle y_{2}(t)$ $\displaystyle=\mathfrak{C}(t)\mathcal{X}(t)$ (48b)

where

$\mathfrak{A}(t)=\begin{bmatrix}\textbf{0}&\textbf{1}\\\
-M^{-1}K(t)&\textbf{0}\end{bmatrix},\mathfrak{B}=\begin{bmatrix}\textbf{0}\\\
M^{-1}\end{bmatrix},\mathfrak{C}(t)=\begin{bmatrix}\textbf{0}&B(t)\\\
\end{bmatrix}$

In the above matrices, $\textbf{0}\in\mathbb{R}^{nN\times nN}$ is a null
matrix and $\textbf{1}=\mbox{diag}(1)\in\mathbb{R}^{nN\times nN}$ is a
diagonal matrix.

Variable gains may result in the loss of passivity in the desired impedance
relation [44]. However, we establish a result providing conditions to preserve
passivity in the time-varying case:

######  Theorem 3.

The states of the system are bounded and passivity is guaranteed if the
following condition is satisfied:

$\overline{\lambda}<2\underline{\lambda}^{2}_{B}$ (49)

where $\underline{\lambda}_{B}$ is the smallest eigenvalue of $B(t)$,
$\overline{\lambda}$ is the largest eigenvalue of
$\dot{P}(t)+P(t)\mathfrak{A}(t)+\mathfrak{A}^{T}(t)P(t)$, and $P(t)=P^{T}(t)$
is a positive-definite matrix defined as:

$P(t)=\begin{bmatrix}K(t)&\textbf{0}\\\ \textbf{0}&MB(t)\end{bmatrix}$ (50)

###### Proof.

We assume $B(t)$ and $K(t)$ are bounded, diagonal, positive definite matrices,
therefore $P(t)$ is a continuous and bounded positive-definite matrix.
Henceforth, to be concise, we will neglect writing the temporal argument. To
study the passivity of the time varying linear system in equation (48), the
following storage function and its temporal derivative can be considered [48]:

$\displaystyle\mathcal{V}$
$\displaystyle=\frac{1}{2}\mathcal{X}^{T}P\mathcal{X}$ (51a)
$\displaystyle\dot{\mathcal{V}}$
$\displaystyle=\frac{1}{2}\mathcal{X}^{T}\\{\dot{P}+P\mathfrak{A}+\mathfrak{A}^{T}P\\}\mathcal{X}+\mathcal{X}^{T}P\mathfrak{B}u_{2}$
(51b)

Integrating $\dot{\mathcal{V}}$ in the time interval $[0,T]$ gives:

$\int_{0}^{T}\dot{\mathcal{V}}(\xi)d\xi=\mathcal{V}(t)-\mathcal{V}(0)\geq-\mathcal{V}(0)$
(52)

Substituting equation (51b) in the above inequality results in the following
inequality:

$\int_{0}^{T}[\frac{1}{2}\mathcal{X}^{T}\\{\dot{P}+P\mathfrak{A}+\mathfrak{A}^{T}P\\}\mathcal{X}+\mathcal{X}^{T}P\mathfrak{B}u_{2}]d\xi\geq-\mathcal{V}(0)$
(53)

Because $P\mathfrak{B}=\mathfrak{C}^{T}$, assuming
$\bar{P}=\dot{P}+P\mathfrak{A}+\mathfrak{A}^{T}P$, we have:

$\int_{0}^{T}\frac{1}{2}\mathcal{X}^{T}\bar{P}\mathcal{X}d\xi+\int_{0}^{T}y^{T}_{2}(\xi)u_{2}(\xi)d\xi\geq-\mathcal{V}(0)$
(54)

where:

$\bar{P}=\begin{bmatrix}\dot{K}&K-K^{T}B^{T}\\\
K^{T}-BK&M\dot{B}\end{bmatrix}$ (55)

To study the passivity of input/output in the above inequality, we study three
cases of $\bar{P}$ being negative-definite (ND), positive-definite (PD), and
indefinite (ID).

1) In case of ND, the integral including $\bar{P}$ is negative, so inequality
(54) can be written as:

$\langle y_{2}|u_{2}\rangle_{T}\geq-\mathcal{V}(0)$ (56)

which proves the passivity of the system and therefore, Theorem 2 and its
results are valid.

2) In the case of PD, because $\bar{P}>0$, passivity can not be concluded
directly from equation (54). In this case, we consider the following
inequalities:

$\frac{\underline{\lambda}}{2}\int_{0}^{T}\mathcal{X}^{T}\mathcal{X}d\xi\leq\int_{0}^{T}\frac{1}{2}\mathcal{X}^{T}\bar{P}\mathcal{X}d\xi\leq\frac{\overline{\lambda}}{2}\int_{0}^{T}\mathcal{X}^{T}\mathcal{X}d\xi$
(57)

where $\underline{\lambda}>0$ and $\overline{\lambda}$ are the smallest and
biggest eigenvalues of $\bar{P}$, respectively. Also, in the view of output in
equation (48b), we can write:

$\underline{\lambda}_{B}\int_{0}^{T}\mathcal{X}^{T}\mathcal{X}d\xi\leq\int_{0}^{T}\mathcal{X}^{T}\mathfrak{C}^{T}\mathfrak{C}\mathcal{X}d\xi\leq\overline{\lambda}_{B}\int_{0}^{T}\mathcal{X}^{T}\mathcal{X}d\xi$
(58)

where $\underline{\lambda}_{B}>0$ and $\overline{\lambda}_{B}$ are the
smallest and biggest eigenvalues of $\mathfrak{C}$, respectively. From
equations (57) and (58), the following inequality can be resulted:

$\frac{\overline{\lambda}}{2}\int_{0}^{T}\mathcal{X}^{T}\mathcal{X}d\xi\leq\frac{\overline{\lambda}}{2\underline{\lambda}_{B}}\int_{0}^{T}y^{T}_{2}y_{2}d\xi$
(59)

Using the above inequality together with inequality (54), we have:

$\langle
y_{2}|u_{2}\rangle_{T}\geq-\mathcal{V}(0)-\frac{\overline{\lambda}}{2\underline{\lambda}_{B}}\|y_{2}\|^{2}_{2,T}$
(60)

From Lemma 1 and in the view of closed-loop interconnection in Fig 6, we know
the lack of passivity in output of one system can be compensated by passivity
in input of the other system. In other word, if $\delta_{2}+\epsilon_{1}>0$,
then all results in Theorem 2 are valid. Therefore, from equations (40) and
(60), the passivity is guaranteed if:

$\underline{\lambda}_{B}-\frac{\overline{\lambda}}{2\underline{\lambda}_{B}}>0\Rightarrow\overline{\lambda}<2\underline{\lambda}^{2}_{B}$
(61)

3) In the case of ID, using the fact that an indefinite matrix has at least
one positive eigenvalue and at least one negative eigenvalue, we can assume
that $\overline{\lambda}>0$ and all results for PD case are valid for ID case.

In conclusion, the gain matrices can vary as long as inequality (61) is
satisfied. If the change, i.e. sign of $\dot{K}$ and $\dot{B}$, leads to a ND
value for $\bar{P}$, inequality (61) is automatically satisfied, and the
passivity is preserved. If the change leads to PD value for $\bar{P}$ and
inequality (61) is effective, system stays passive. If none of above happens
and change leads to an ID $\bar{P}$, having inequality (61) would be enough
for the passivity. Note that the variation of damping matrix does not affect
the passivity of $H_{1}$ in the interconnection of Fig. 6(b).

∎

##  IX Concluding remarks and Future works

In this research, a framework is established for controlling cooperative
robots and executing the motion task of moving a load along a desired
trajectory. A comprehensive model of an augmented dynamics of the robot, JMs
and the motors, provides the opportunity to introduce a new control scheme for
semi-active joints called virtual control strategy (SVC). Based on SVC, any
suitable control approach can be utilized to control the motion task in CRM.
Here we used the concept of impedance control to devise the control scheme.
The controller was studied and developed for two cases; constant damping and
stiffness gains in the impedance relation; and variable gains. For both cases,
the input/output passivity tool was used to analysis the stability.

Moreover, an optimization was introduced to obtain an energy-oriented
impedance control. The optimization finds the best impedance gains such that
the energy extraction from the power source is minimized. Using a simulation
example, it was shown that energy regeneration can occur during the motion
task, and it has huge effect in terms of energy saving.

The lab experiment of this research is left as the future work. Also, another
possible future work is to find an energy-optimal path for moving the load
from point to point, which requires forming an optimization to find the
optimal trajectory.

##  Appendix A  EXTERNAL ENERGY BALANCE IN CRM

We start by obtaining Joule losses in terms of the desired control. The Joule
losses due to resistance in motor of ijth semi-active joint is:

$L_{R_{ij}}=R_{ij}I_{{ij}}^{2}$ (62)

Substituting current and $u_{ij}$ from equations (14) and (24), respectively,
and after some manipulation and simplification, we get:

$L_{R_{ij}}=\frac{R_{ij}}{a_{ij}^{2}}(\tau^{d}_{ij})^{2}+\frac{a_{ij}^{2}\dot{q}_{ij}^{2}}{R_{ij}}-2\tau^{d}_{ij}\dot{q}_{ij}$
(63)

Equation (15) expresses the relation between $\tau_{ij}$ and $\tau^{d}_{ij}$.
Multiplying both sides of this equation by $\dot{q}_{ij}$ yields:

$\tau_{ij}\dot{q}_{ij}=-m_{ij}\bar{n}^{2}_{ij}\ddot{q}_{ij}\dot{q}_{ij}-(b_{ij}\bar{n}^{2}_{ij}+\frac{a_{ij}^{2}}{R_{ij}})\dot{q}^{2}_{ij}+\tau^{d}_{ij}\dot{q}_{ij}$
(64)

The kinetic energy of the actuator is expressed as
$K_{ij}=\frac{1}{2}I_{ij}\bar{n}^{2}_{ij}\dot{q}^{2}_{ij}$. So equation (64)
can be simplified as:

$\tau_{ij}\dot{q}_{ij}=-\frac{dK_{ij}}{dt}-(b_{ij}\bar{n}^{2}_{ij}+\frac{a_{ij}^{2}}{R_{ij}})\dot{q}^{2}_{ij}+\tau^{d}_{ij}\dot{q}_{ij}$
(65)

Replacing $a_{ij}^{2}\dot{q}_{ij}/R_{ij}$ from equation (64) and rearranging
the result and taking the integral from $t_{1}$ to $t_{2}$ of both sides of
obtained equation gives:

$\begin{split}&\int_{t_{1}}^{t_{2}}(\tau^{d}_{ij}\dot{q}_{ij}-\frac{R_{ij}}{a_{ij}^{2}}(\tau^{d}_{ij})^{2})dt=\\\
&\int_{t_{1}}^{t_{2}}(-\frac{dK_{ij}}{dt}-\tau_{ij}\dot{q}_{ij}-L_{R_{ij}}-b_{ij}\bar{n}^{2}_{ij}\dot{q}^{2}_{ij})dt\end{split}$
(66)

On the other hand, using $\Delta E_{c}$ from equation (26), we can write:

$\Delta E_{c}=\sum_{i=1}^{N}\sum_{j=1}^{n}\Delta E_{c_{ij}}$ (67)

where:

$\Delta
E_{c_{ij}}=\int_{t_{1}}^{t_{2}}(\tau^{d}_{ij}\dot{q}_{ij}-\frac{R_{ij}}{a_{ij}^{2}}(\tau^{d}_{ij})^{2})dt$
(68)

Using equations (66) and (68), it can be concluded:

$\Delta
E_{c_{ij}}=\int_{t_{1}}^{t_{2}}(-\frac{dK_{ij}}{dt}-\tau_{ij}\dot{q}_{ij}-L_{R_{ij}}-b_{ij}\bar{n}^{2}\dot{q}^{2}_{ij})dt$
(69)

To derive equation (27), we start by writing the overall energy balance for
CRM:

$\int_{t_{1}}^{t_{2}}\dot{q}^{T}\mathcal{T}dt+\int_{t_{1}}^{t_{2}}\dot{q}^{T}\mathcal{T}_{ext}dt=\Delta
E^{\circ}_{m}+\Sigma^{\circ}_{m}$ (70)

where the first term on the left-hand side is the work done by the semi-active
joints, the second term is the work done by the external forces and moments
($W_{ext}$), $\Delta E^{\circ}_{m}$ is the total change in mechanical energy,
and $\Sigma^{\circ}_{m}$ is the dissipated mechanical energy in the system.
Also $\mathcal{T}$ is defined as the vector of all applied forces/moments and
$\mathcal{T}_{ext}=J^{T}(q)f$ is the vector of external forces/moments, i.e.
the forces/moments applied by the load.

If we write equation (66) for all joints in CRM and substitute in equation
(70), results:

$W_{{ext}}=\Delta\,E_{c}\,+\,\Delta\,E^{Tot}_{m}\,+\,\Sigma^{Tot}_{m}\,+\,\Sigma_{e}$
(71)

where

$\begin{split}\Delta\,E^{Tot}_{m}&=\Delta
E^{\circ}_{m}+\sum_{i=1}^{N}\sum_{j=1}^{n}\Delta K_{ij}\\\
\Sigma^{Tot}_{m}&=\Sigma^{\circ}_{m}+\sum_{i=1}^{N}\sum_{j=1}^{n}\int_{t_{1}}^{t_{2}}b_{ij}\bar{n}^{2}_{ij}\dot{q}^{2}_{ij}dt\\\
\Sigma_{e}&=\sum_{i=1}^{N}\sum_{j=1}^{n}\int_{t_{1}}^{t_{2}}L_{R_{ij}}dt\end{split}$
(72)

## Acknowledgment

The authors would like to thank the National Science Foundation for funding
this work (NSF grant $\\#$1536035).

## References

  * [1] P. Khalaf and H. Richter, “Trajectory optimization of robots with regenerative drive systems: Numerical and experimental results,” _IEEE Transactions on Robotics_ , vol. 36, no. 2, pp. 501–516, 2019.
  * [2] H. Richter, “A framework for control of robots with energy regeneration,” _Journal of Dynamic Systems, Measurement, and Control_ , vol. 137, no. 9, 2015\.
  * [3] A. Ghorbanpour and H. Richter, “Control with optimal energy regeneration in robot manipulators driven by brushless dc motors,” in _ASME 2018 Dynamic Systems and Control Conference_. American Society of Mechanical Engineers Digital Collection, 2018\.
  * [4] P. Khalaf and H. Richter, “Parametric optimization of stored energy in robots with regenerative drive systems,” in _Advanced Intelligent Mechatronics (AIM), 2016 IEEE International Conference on_. IEEE, 2016, pp. 1424–1429.
  * [5] T. Shimizu and C. Underwood, “Super-capacitor energy storage for micro-satellites: Feasibility and potential mission applications,” _Acta Astronautica_ , vol. 85, pp. 138–154, 2013.
  * [6] P. Khalaf, H. Warner, E. Hardin, H. Richter, and D. Simon, “Development and experimental validation of an energy regenerative prosthetic knee controller and prototype,” in _ASME 2018 Dynamic Systems and Control Conference_. American Society of Mechanical Engineers Digital Collection, 2018.
  * [7] H. Richter, “Control for optimal energy regeneration from autorotation in uavs,” in _2020 American Control Conference (ACC)_. IEEE, 2020, pp. 5108–5113.
  * [8] X. He, H. Liu, S. He, B. Hu, and G. Xiao, “Research on the energy efficiency of energy regeneration systems for a battery-powered hydrostatic vehicle,” _Energy_ , vol. 178, pp. 400–418, 2019.
  * [9] P. Boscariol and D. Richiedei, “Trajectory design for energy savings in redundant robotic cells,” _Robotics_ , vol. 8, no. 1, p. 15, 2019.
  * [10] P. Khalaf and H. Richter, “On global, closed-form solutions to parametric optimization problems for robots with energy regeneration,” _Journal of Dynamic Systems, Measurement, and Control_ , vol. 140, no. 3, 2018.
  * [11] H. Richter and D. Selvaraj, “Impedance control with energy regeneration in advanced exercise machines,” in _American Control Conference (ACC), 2015_. IEEE, 2015, pp. 5890–5895.
  * [12] H. Warner, D. Simon, and H. Richter, “Design optimization and control of a crank-slider actuator for a lower-limb prosthesis with energy regeneration,” in _Advanced Intelligent Mechatronics (AIM), 2016 IEEE International Conference on_. IEEE, 2016, pp. 1430–1435.
  * [13] G. Carabin, E. Wehrle, and R. Vidoni, “A review on energy-saving optimization methods for robotic and automatic systems,” _Robotics_ , vol. 6, no. 4, p. 39, 2017.
  * [14] H. Richter, J. W. Connolly, and D. L. Simon, “Optimal control and energy management for hybrid gas-electric propulsion,” _Journal of Engineering for Gas Turbines and Power_ , vol. 142, no. 9, 2020.
  * [15] J. Joy and S. Ushakumari, “Regenerative braking mode operation of a three-phase h-bridge inverter fed pmbldc motor generator drive in an electric bike,” _Electric Power Components and Systems_ , pp. 1–19, 2018.
  * [16] A. Ivoilov, V. Trubin, V. Zhmud, and L. Dimitrov, “The power consumption decreasing of the two-wheeled balancing robot,” in _2018 International Multi-Conference on Industrial Engineering and Modern Technologies (FarEastCon)_. IEEE, 2018, pp. 1–8.
  * [17] G. Khademi, H. Mohammadi, H. Richter, and D. Simon, “Optimal mixed tracking/impedance control with application to transfemoral prostheses with energy regeneration,” _IEEE Transactions on Biomedical Engineering_ , 2017\.
  * [18] A. Koivo and M. Unseren, “Modeling closed chain motion of two manipulators holding a rigid object,” _Mechanism and machine theory_ , vol. 25, no. 4, pp. 427–438, 1990.
  * [19] P. Chiacchio, S. Chiaverini, and B. Siciliano, “Cooperative control schemes for multiple robot manipulator systems.” in _ICRA_ , 1992, pp. 2218–2223.
  * [20] I. D. Walker, R. A. Freeman, and S. I. Marcus, “Analysis of motion and internal loading of objects grasped by multiple cooperating manipulators,” _The International journal of robotics research_ , vol. 10, no. 4, pp. 396–409, 1991.
  * [21] J.-H. Jean and L.-C. Fu, “An adaptive control scheme for coordinated multimanipulator systems,” _IEEE Transactions on Robotics and Automation_ , vol. 9, no. 2, pp. 226–231, 1993.
  * [22] S. Erhart and S. Hirche, “Internal force analysis and load distribution for cooperative multi-robot manipulation,” _IEEE Transactions on Robotics_ , vol. 31, no. 5, pp. 1238–1243, 2015.
  * [23] A. Vergnano, C. Thorstensson, B. Lennartson, P. Falkman, M. Pellicciari, F. Leali, and S. Biller, “Modeling and optimization of energy consumption in cooperative multi-robot systems,” _IEEE Transactions on Automation Science and Engineering_ , vol. 9, no. 2, pp. 423–428, 2012.
  * [24] H. Kawasaki, S. Ueki, and S. Ito, “Decentralized adaptive coordinated control of multiple robot arms without using a force sensor,” _Automatica_ , vol. 42, no. 3, pp. 481–488, 2006.
  * [25] R. Bonitz and T. C. Hsia, “Internal force-based impedance control for cooperating manipulators,” _IEEE Transactions on Robotics and Automation_ , vol. 12, no. 1, pp. 78–89, 1996.
  * [26] 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.
  * [27] 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.
  * [28] M. Shimizu, “Nonlinear impedance control to maintain robot position within specified ranges,” in _2012 Proceedings of SICE Annual Conference (SICE)_. IEEE, 2012, pp. 1287–1292.
  * [29] J. He, M. Luo, and Q. Zhang, “Dual impedance control with variable object stiffness for the dual-arm cooperative manipulators,” in _2016 Asia-Pacific Conference on Intelligent Robot Systems (ACIRS)_. IEEE, 2016, pp. 102–108.
  * [30] D. Heck, D. Kostić, A. Denasi, and H. Nijmeijer, “Internal and external force-based impedance control for cooperative manipulation,” in _2013 European Control Conference (ECC)_. IEEE, 2013, pp. 2299–2304.
  * [31] S. Schneider and R. H. Cannon, “Object impedance control for cooperative manipulation: Theory and experimental results,” in _1989 IEEE International Conference on Robotics and Automation_. IEEE Computer Society, 1989, pp. 1076–1077.
  * [32] M. Li, K. Li, P. Wang, Y. Liu, F. Zha, and W. Guo, “Indirect adaptive impedance control for dual-arm cooperative manipulation,” in _2017 2nd International Conference on Advanced Robotics and Mechatronics (ICARM)_. IEEE, 2017, pp. 650–655.
  * [33] Y. Ren, Y. Zhou, Y. Liu, M. Jin, and H. Liu, “Adaptive object impedance control of dual-arm cooperative humanoid manipulators,” in _Proceeding of the 11th World Congress on Intelligent Control and Automation_. IEEE, 2014, pp. 3333–3339.
  * [34] F. Caccavale and L. Villani, “An impedance control strategy for cooperative manipulation,” in _2001 IEEE/ASME International Conference on Advanced Intelligent Mechatronics. Proceedings (Cat. No. 01TH8556)_ , vol. 1. IEEE, 2001, pp. 343–348.
  * [35] S. Erhart, D. Sieber, and S. Hirche, “An impedance-based control architecture for multi-robot cooperative dual-arm mobile manipulation,” in _2013 IEEE/RSJ International Conference on Intelligent Robots and Systems_. IEEE, 2013, pp. 315–322.
  * [36] A. Stolfi, P. Gasbarri, and M. Sabatini, “A combined impedance-pd approach for controlling a dual-arm space manipulator in the capture of a non-cooperative target,” _Acta Astronautica_ , vol. 139, pp. 243–253, 2017.
  * [37] A. J. Van der Schaft and A. Van Der Schaft, _L2-gain and passivity techniques in nonlinear control_. Springer, 2000, vol. 2.
  * [38] M. Vidyasagar, _Nonlinear systems analysis_. SIAM, 2002.
  * [39] R. Ortega, J. A. L. Perez, P. J. Nicklasson, and H. J. Sira-Ramirez, _Passivity-based control of Euler-Lagrange systems: mechanical, electrical and electromechanical applications_. Springer Science & Business Media, 2013.
  * [40] B. Brogliato, R. Lozano, B. Maschke, and O. Egeland, _Dissipative Systems Analysis and Control: Theory and Applications_ , ser. Communications and Control Engineering. Springer International Publishing, 2019. [Online]. Available: https://books.google.com/books?id=-ZqgDwAAQBAJ
  * [41] R. Kelly, R. Carelli, M. Amestegui, and R. Ortega, “On adaptive impedance control of robot manipulators,” in _1989 IEEE International Conference on Robotics and Automation_. IEEE Computer Society, 1989, pp. 572–573.
  * [42] M. W. Spong, S. Hutchinson, M. Vidyasagar _et al._ , _Robot modeling and control_ , 2006.
  * [43] M. Vukobratović, “How to control robots interacting with dynamic environment,” _Journal of Intelligent and Robotic Systems_ , vol. 19, no. 2, pp. 119–152, 1997.
  * [44] F. Ferraguti, C. Secchi, and C. Fantuzzi, “A tank-based approach to impedance control with variable stiffness,” in _2013 IEEE international conference on robotics and automation_. IEEE, 2013, pp. 4948–4953.
  * [45] M. Zheng, T. Yuan, and T. Huang, “Time-varying impedance control of port hamiltonian system with a new energy-storing tank,” _Complexity_ , vol. 2018, 2018.
  * [46] Y. Kishi, Z. W. Luo, F. Asano, and S. Hosoe, “Passive impedance control with time-varying impedance center,” in _Proceedings 2003 IEEE International Symposium on Computational Intelligence in Robotics and Automation. Computational Intelligence in Robotics and Automation for the New Millennium (Cat. No. 03EX694)_ , vol. 3. IEEE, 2003, pp. 1207–1212.
  * [47] K. Kronander and A. Billard, “Stability considerations for variable impedance control,” _IEEE Transactions on Robotics_ , vol. 32, no. 5, pp. 1298–1305, 2016.
  * [48] J. R. Forbes and C. J. Damaren, “Passive linear time-varying systems: State-space realizations, stability in feedback, and controller synthesis,” in _Proceedings of the 2010 American Control Conference_. IEEE, 2010, pp. 1097–1104.