1
Passive Bilateral Control of Nonlinear Mechanical Teleoperators
Dongjun Lee
Coordinated Science Laboratory,
University of Illinois at Urbana-Champaign,
117 Transportation Building,
104 S. Mathews Ave., Urbana, IL 61801 USA
d-lee@control.csl.uiuc.edu
FAX: 217-244-1653
Perry Y. Li
Department of Mechanical Engineering,
University of Minnesota,
111 Church St. SE, Minneapolis MN 55455 USA
pli@me.umn.edu
FAX: 612-625-9395
Corresponding author: Dongjun Lee
Paper type: Regular
Abstract
In this paper, a passive bilateral control law is proposed for a teleoperator consisting of a pair of n-DOF
nonlinear robotic systems. The control law ensures energetic passivity of the closed-loop teleoperator with power
scaling, coordinates motions of the master and slave robots, and installs useful task-specific dynamics for inertia
scaling, motion guidance and obstacle avoidance. Consequently, the closed-loop teleoperator behaves like a common
passive mechanical tool. A key innovation is the energy preserving passive decomposition of the 2n-DOF nonlinear
teleoperator dynamics into two robot-like systems: a n-DOF shape system representing the master-slave position
coordination aspect (n-DOF holonomic constraint) and a n-DOF locked system representing the dynamics of the
coordinated teleoperator. The master-slave position coordination is then achieved by regulating the shape system,
while programmable apparent inertia of the coordinated teleoperator is achieved by scaling the effect of the
environmental and human forces on the locked system. Passive velocity field control (PVFC) and artificial potential
field control are used to implement guidance and obstacle avoidance for the coordinated teleoperator. A special
passive control implementation structure is used to ensure the energetic passivity of the closed-loop teleoperator even
in the presence of model parametric uncertainties and inaccurate force sensing. Experimental results are presented
to validate the properties of the proposed control framework.
Keywords: nonlinear mechanical teleoperator, passivity, power scaling, passive decomposition, coordination,
guidance, obstacle avoidance, inertia scaling
2
Passive Bilateral Control of Nonlinear Mechanical
Teleoperators
Dongjun Lee and Perry Y. Li
Abstract
In this paper, a passive bilateral control law is proposed for a teleoperator consisting of a pair of n-DOF
nonlinear robotic systems. The control law ensures energetic passivity of the closed-loop teleoperator with power
scaling, coordinates motions of the master and slave robots, and installs useful task-specific dynamics for inertia
scaling, motion guidance and obstacle avoidance. Consequently, the closed-loop teleoperator behaves like a common
passive mechanical tool. A key innovation is the energy preserving passive decomposition of the 2n-DOF nonlinear
teleoperator dynamics into two robot-like systems: a n-DOF shape system representing the master-slave position
coordination aspect (n-DOF holonomic constraint) and a n-DOF locked system representing the dynamics of the
coordinated teleoperator. The master-slave position coordination is then achieved by regulating the shape system,
while programmable apparent inertia of the coordinated teleoperator is achieved by scaling the effect of the
environmental and human forces on the locked system. Passive velocity field control (PVFC) and artificial potential
field control are used to implement guidance and obstacle avoidance for the coordinated teleoperator. A special
passive control implementation structure is used to ensure the energetic passivity of the closed-loop teleoperator even
in the presence of model parametric uncertainties and inaccurate force sensing. Experimental results are presented
to validate the properties of the proposed control framework.
I. I NTRODUCTION
A closed-loop teleoperator is, energetically, a two-port system where the master and the slave robots interact
mechanically with the human operators and the slave environments, respectively (Fig. 1). Thus, the foremost and
primary goal of the control design is to ensure interaction safety and/or coupling stability [1] when it is mechanically
coupled with a broad class of slave environments and human operators.
Energetic passivity (i.e. passivity with scaled mechanical power as the supply rate [2]) of the closed-loop
teleoperator has been widely utilized as a mean towards interaction safety and coupling stability [3], [4], [5],
[6], [7], [8], [9], [10], [11], [12]. This is due to the well-known fact that the interaction between a passive closedloop teleoperator and any passive environments or humans is necessarily stable [13] in the sense of norm-bounded
velocities and forces of the master and slave robots. In many practical cases, since the slave environments are usually
passive (e.g. pushing against a wall or grasping a ball) and humans’ mechanical impedances are indistinguishable
from that of passive systems [14], energetic passivity does result in stable interaction. Moreover, energetically
passive teleoperators would potentially be safer to interact with, since the maximum extractable energy from it
would be bounded, limiting the possible damages on the environments, humans, and the system itself.
Most control schemes in the literature are proposed for linear robotic teleoperators [3], [5], [6], [9], [15], [16],
[17], [18], [19]. However, multi-DOF robots are typically nonlinear due to their serial linkages. Applying linear
Submitted to the IEEE Transactions on Robotics and Automation. First submission October 24 2002. Revised and resubmitted: July 2004.
Some of the results in this paper presented in ICRA 2002 and IEEE VR 2003, Paper Type: Regular. Corresponding Author: Dongjun Lee
Dongjun Lee is with Coordinated Science Laboratory, University of Illinois at Urbana-Champaign, 117 Transportation Building, 104
S. Mathews Ave., Urbana, IL 61801 USA. E-mail: d-lee@control.csl.uiuc.edu. Perry Y. Li is with Department of Mechanical
Engineering, University of Minnesota, 111 Church St. SE, Minneapolis MN 55455.
3
ρ F1
ρ− Scaled
Human
Operator
.
q
ρ T1
ρ− Scaled
Master
Robot
1
T2
Slave
Robot
Controller
.
q
1
.
q
F2
2
Slave
.
q
Environment
2
Closed−Loop Teleoperator
Fig. 1. Two port closed-loop teleoperator with a power scaling ρ > 0, with which the mechanical strength of the master robot
and the human operator is magnified (when ρ > 1) or shrink (when ρ < 1) w.r.t. that of the slave robot and its environment.
approaches to nonlinear robotic teleoperators would result in degraded controller performance, since the nonlinear
effects are not compensated for (see [9] for an example).
Control schemes for nonlinear robotic teleoperators are rare compared to those for linear ones. In [7], the nonlinear
open-loop dynamics are completely cancelled out and replaced by the desired virtual passive tool dynamics.
Although energetic passivity is ensured when perfect cancellation is achieved, it is not guaranteed when model
parameters are uncertain or force sensing is not accurate. In [20], an adaptive control scheme is proposed in which
the virtual decomposition in [21] is used to address the nonlinear dynamics. Unfortunately, the approach is limited as
it depends on the over-simplified mass-spring-damper models for the slave environments and the human operators. In
[22], a variable structure control is proposed. However, because coupling stability is not considered, some unstable
behaviors are reported in [22] when the closed loop teleoperator is coupled with humans and environments.
In this paper, we propose a control scheme that explicitly addresses the nonlinear dynamics of the teleoperator and
enforces the requirement that energetic passivity be ensured robustly. The control scheme coordinates the motions
of the master-slave robots, and installs useful task-specific dynamics so that it behaves like a common passive
mechanical tool [9] with which both the human and environment interact. The closed loop teleoperator is robustly
energetically passive with bilateral power scaling which is useful to accommodate different sizes of the master and
slave robots (e.g. in micro-macro telemanipulation).
A key innovation is the passive decomposition [23] that decomposes the nonlinear dynamics of the 2n-DOF
mechanical teleoperator into the two decoupled n-DOF robot-like dynamics while enforcing passivity: the n-DOF
shape system that represents the master-slave position coordination aspect and the n-DOF locked system that
describes the overall behavior of the coordinated teleoperator. Moreover, the sums of the energies and the supply
rates of the shape and locked system are the total energy and the total supply rate of the original system, respectively.
This property enables us to easily design separate passive control laws for master-slave position coordination (shape
system) and for the useful dynamics installation on the coordinated teleoperator (locked system).
Our previous paper [9] considers a similar decomposition except that the master and slave systems need to
be linear dynamically similar (LDS). Although the control law in [9] can be applied to nonlinear systems by
approximating them as LDS systems, this results in degraded coordination performance especially when control
gains are limited. The restriction that the system has to be LDS or approximately LDS is completely removed
by the passive decomposition proposed in the present paper, which is applicable to any pair of nonlinear robots
with compatible degrees of freedom. The present paper also addresses the issue of rendering useful task specific
dynamics which is only alluded to in [9].
Once the nonlinear teleoperator has been decomposed into the shape and locked systems, master-slave position
coordination can be easily achieved by regulating the shape system via simple PD (proportional-derivative) regulation
and feedforward cancellation of the human / environmental disturbances acting on it. Coordinated behavior of the
4
teleoperator (inertia scaling, guidance and obstacle avoidance) is addressed in the locked system. The apparent inertia
scaling of the coordinated teleoperator is obtained by scaling the combined human and environmental forcings on the
locked system. By making this apparent inertia arbitrarily small, the teleoperator approaches an ideally transparent
system [15] (i.e. perfect position coordination with zero intervening inertia [16]). Also, by increasing the apparent
inertia, the inertial effects, such as in hammering tasks, can be emphasized.
To help human operators perform tasks more efficiently and comfortably, we endow the coordinated teleoperator
with dynamics for motion guidance and obstacle avoidance. The motion guidance objective is addressed using
Passive Velocity Field Control (PVFC, [24], [25]) which enables the locked system to follow the direction of a
desired velocity field passively. Due to its passivity property, PVFC allows human operators to safely intervene
with a task whenever necessary. For instance, the human operator can slow down or speed up the velocity field
following speed by extracting energy from or injecting energy into the teleoperator. Obstacle avoidance is achieved
by imposing artificial potential functions [26], [27], [28] on the master and slave configuration spaces to prohibit
the master and slave robots from penetrating into regions with dangerous obstacles or fragile objects (e.g. organs
for tele-surgery).
The proposed control law also uses a passive control implementation structure [9], [29] to limit the net energy
generation by the control action. This structure ensures that energetic passivity (with power scaling) is robust to
parametric uncertainty and inaccurate force sensing. Therefore, this implementation structure substantially enhances
the interaction safety and coupling stability.
The rest of this paper is organized as follows. Problem formulation and control objectives are given in section
II. In section III, the energy preserving passive decomposition of the nonlinear dynamics of 2n-DOF mechanical
teleoperators into the n-DOF shape and n-DOF locked systems is presented. Control laws for the shape and locked
systems are designed in section IV, and the passive implementation structure is presented in section V. Experimental
results are presented in section VI. Section VII contains some concluding remarks.
II. P ROBLEM F ORMULATION
A. Dynamics of Mechanical Teleoperators and Power Scaling
Consider a 2n-DOF teleoperator consisting of two n-DOF nonlinear mechanical systems:
ρ (M1 (q1 )q̈1 + C1 (q1 , q̇1 )q̇1 = T1 + F1 ) ,
(1)
M2 (q2 )q̈2 + C2 (q2 , q̇2 )q̇2 = T2 + F2 .
(2)
where qi , Ti , Fi ∈ <n are the configurations, the control commands from actuators, and the human/environmental
forces, Mi (qi ) ∈ <n×n are the symmetric and positive-definite inertia matrices and Ci (qi , q̇i ) ∈ <n×n are the
Coriolis matrices s.t. Ṁi (qi ) − 2Ci (qi , q̇i ) are skew-symmetric with i = 1 for master and i = 2 for slave. The
scalar ρ > 0 is a user-specified power scaling factor for comparing human and slave environment power inputs.
The human power is amplified when ρ > 1 and it is reduced when ρ < 1 (see figure 1).
The mechanical teleoperator (1)-(2) is said to satisfy the energetic passivity condition if there exists a finite
constant d ∈ <, s.t. ∀(F1 (t ≥ 0), F2 (t ≥ 0)), and ∀t ≥ 0,
Z t
Z t
(3)
sρ (q̇1 (τ ), q̇2 (τ ), F1 (τ ), F2 (τ )) dτ =
[ρ · FT1 (τ )q̇1 (τ ) + FT2 (τ )q̇2 (τ )]dτ ≥ −d2 .
{z
} |
{z
}
0
0 |
scaled
human power
environment
power
where sρ (q̇1 , q̇2 , F1 , F2 ) := ρFT1 q̇1 + FT2 q̇2 is the scaled environmental supply rate.
5
Similarly, we say that the controller in figure 1 with (q̇1 , q̇2 ) as input and (T1 , T2 ) as output satisfies the
controller passivity condition [23], [29] if there exists a finite constant c ∈ < s.t. ∀t ≥ 0,
Z t
Z t
sc (q̇1 (τ ), q̇2 (τ ), T1 (τ ), T2 (τ )) dτ =
[ρ · TT1 (τ )q̇1 (τ ) + TT2 (τ )q̇2 (τ )]dτ ≤ c2 ,
(4)
{z
} |
{z
}
0
0 |
scaled master
control power
slave control
power
where sc (q̇1 , q̇2 , T1 , T2 ) := ρTT1 q̇1 + TT2 q̇2 is the scaled control supply rate.
The teleoperator passivity condition (3) implies that the maximum extractable (scaled) energy from the closedloop teleoperator is always bounded, while the controller passivity condition (4) implies that the maximum amount
of (scaled) energy generated by the controller is bounded.
Proposition 1 [23], [29] For the mechanical teleoperator (1)-(2), controller passivity condition (4) implies energetic
passivity condition (3).
Proof: Let us define the scaled kinetic energy
κρ (t) :=
ρ T
1
q̇1 (t)M1 (q1 )q̇1 (t) + q̇T2 (t)M2 (q2 )q̇2 (t),
2
2
(5)
then, using the dynamics (1)-(2) with the skew-symmetric property of Ṁi (qi ) − 2Ci (qi , q̇i ) (i = 1, 2), we have
d
κρ (t) = ρTT1 (t)q̇1 (t) + TT2 (t)q̇2 (t) + ρFT1 (t)q̇1 (t) + FT2 (t)q̇2 (t).
dt
Thus, by integrating (6) with the controller passivity condition (4) and the fact that κρ (t) ≥ 0, we have
Z t
[ρFT1 (τ )q̇1 (τ ) + FT2 (τ )q̇2 (τ )]dτ ≥ −κρ (0) − c2 =: −d2 ∀t ≥ 0.
(6)
0
Proposition 1 allows us to conclude that a closed-loop teleoperator is energetically passive (3) by simply examining
the controller structure. In particular, by imposing a controller structure of section V that robustly enforces the
controller passivity (4), energetic passivity (3) will also be guaranteed robustly.
B. Control Objectives
In order to render the 2n-DOF teleoperator system (1)-(2) as a n-DOF common passive mechanical tool, a
controller will be designed to achieve the following objectives:
1) The closed-loop teleoperator robustly satisfies the energetic passivity condition with a power scaling ρ in (3)
regardless of model parametric uncertainty and inaccurate force measurements;
2) The motions of the master and the slave systems are perfectly coordinated s.t.
qE (q1 (t), q2 (t)) := q1 (t) − q2 (t) → 0,
∀F1 (t), F2 (t),
(7)
where qE : <2n → <n defines the master-slave position coordination error.
Once the coordination (7) is achieved, the coordinated teleoperator would have the following dynamics (similar
to [30], by summing the master and the slave dynamics in (1)-(2) with the condition that q1 = q2 ):
ML (q)q̈L + CL (q, q̇)q̇L = TL + FL ,
(8)
6
where
ML (q) := ρM1 (q1 ) + M2 (q2 ),
(9)
CL (q, q̇) := ρC1 (q1 , q̇1 ) + C2 (q2 , q̇2 ),
(10)
FL := ρF1 + F2 ,
(11)
TL := ρT1 + T2 ,
qL ∈ <n represents the configuration of the coordinated teleoperator s.t. qL = q1 = q2 , FL ∈ <n is the
total environmental / human forcing on the coordinated teleoperator, and TL ∈ <n is the control to be
designed to achieve the target dynamics below. It can be shown that Eq. (8) is the Levi-Civita connection
on the coordination submanifold Ho := {(q1 , q2 ) ∈ <2n |qE (q1 , q2 ) = q1 − q2 = 0} with qL being a
parameterization of Ho [31], [32]. In this sense, ML (q) and CL (q, q̇) can be thought of as the natural
inertia and Coriolis matrices of the coordinated (locked) teleoperator. Notice that n-DOF of the total 2n-DOF
control [T1 , T2 ] ∈ <2n have already been exploited for the coordination control to achieve (7) and (8);
3) We would like the dynamics of the coordinated teleoperator (8) to converge to the following n-DOF target
dynamics:
η [ML (q)q̈L + CL (q, q̇)q̇L ] − Ψ(q, q̇) = FL = ρF1 + F2 ,
(12)
where η > 0 is a user-specific scalar for adjusting the apparent inertia of the coordinated teleoperator that
the human operator and the slave environment feel ( ηρ ML (q) and ηML (q), respectively), and Ψ(q, q̇)
incorporates obstacle avoidance and guidance which will be designed later on. A condition for Ψ(q, q̇)
is that it is compatible with the energetic passivity requirement.
The coordination requirement (7) can be extended to include a kinematic scaling α : <n → <n so that coordination
requires q1 → α(q2 ) where α(·) is a one-to-one, onto mapping between the configuration manifolds of the master
and slave robots. This requirement can be transformed into the standard coordination requirement (7) by rewriting
the dynamics of (2) in terms of q2,α := α(q2 ).
III. T HE PASSIVE D ECOMPOSITION
In this section, we propose a transformation S(q) : <2n → <2n of the tangent space, with which the 2n-DOF
nonlinear dynamics of the original teleoperator (1)-(2) is decomposed into the two decoupled n-DOF robot-like
systems without violating passivity: the shape system representing the master-slave position coordination aspect
and the locked system describing the overall motion of the coordinated teleoperator. This generalizes the similar
decomposition for linear dynamically similar (LDS) teleoperators in [9] to general nonlinear teleoperators.
The decomposition is designed according to the following design criteria: 1) the scaled kinetic energy (5) of the
teleoperator (1)-(2) is decomposed into the sum of the kinetic energies of the locked and the shape systems; 2) the
locked system has the natural inertia ML (q) = ρM1 (q1 ) + M2 (q2 ) as in (8); and 3) the shape system velocity is
given by the velocity of the coordination error (7). The first criterion implies that the transformed inertia matrix is
block-diagonalized.
These three design criteria uniquely define a decomposition matrix S(q) ∈ <2n×2n s.t.:
!
"
#
Ã
! "
#Ã
q̇1
I
φ(q)
vL
I − φ(q) φ(q)
, with S−1 (q) =
,
(13)
=
q̇2
I φ(q) − I
vE
I
−I
{z
}
|
=:S(q)
where
φ(q) := [ρM1 (q1 ) + M2 (q2 )]−1 M2 (q2 ).
(14)
7
In (13), vL ∈ <n is the weighted mean velocity of the master / slave robots, and vE ∈ <n is the velocity of the
coordination error as given by
vL = [ρM1 (q) + M2 (q)]−1 (ρM1 (q)q̇1 + M2 (q)q̇2 ),
(15)
d
vE = qE (q1 (t), q2 (t)) = q̇1 (t) − q̇2 (t).
(16)
dt
Note that vL = q̇1 = q̇2 when vE = 0.
With the decomposition matrix S(q) in (13), the inertia matrix of the original teleoperator system (1)-(2) is
transformed into another block diagonal inertia matrix (i.e. the first design criterion is satisfied):
"
#
#
"
ML (q)
0
ρM1 (q1 )
0
−1
−T
S (q) =:
,
(17)
S (q)
0
ME (q)
0
M2 (q2 )
where ML (q) = ρM1 (q1 ) + M2 (q2 ) is the natural inertia of the coordinated teleoperator in (8) and
£
¤
ME (q) = ρφT (q)M1 (q1 )φ(q) + φT (q) − I M2 (q2 ) [φ(q) − I] .
It is easy to see that these two matrices ML (q) and ME (q) are symmetric and positive definite.
According to the transform (13), the compatible transform for Ti , Fi i = 1, 2 are given as:
Ã
!
à !
Ã
! Ã
!
à !
ρT
F
ρF
ρF
+
F
TL
1
1
1
2
L
= S−T (q)
, and
= S−T (q)
=
.
TE
T2
FE
F2
φT (q)(ρF1 + F2 ) − F2
Ã
Then, using (13)-(19) and the relation that
v̇L
v̇E
!
(18)
(19)
Ã
!
à !
−φ̇(q)vE
q̈
=
+ S(q) 1 , the 2n-DOF nonlinear dynamics
0
q̈2
of the teleoperator (1)-(2) are transformed into two n-DOF partially decoupled systems:
ML (q)v̇L + CL (q, q̇)vL + CLE (q, q̇)q̇E = TL + FL ,
|
{z
} |
{z
}
locked system dynamics
coupling
ME (q)q̈E + CE (q, q̇)q̇E + CEL (q, q̇)vL = TE + FE ,
|
{z
} |
{z
}
shape system dynamics
(20)
(21)
coupling
where CL (q, q̇) = ρC1 (q1 , q̇1 ) + C2 (q2 , q̇2 ) as given by (10), and
£
¤
CE (q, q̇) = ρφT (q)C1 (q1 , q̇1 )φ(q) + φT (q) − I C2 (q2 , q̇2 ) [φ(q) − I] ,
CLE (q, q̇) = ρC1 (q1 , q̇1 )φ(q) + C2 (q2 , q̇2 ) [φ(q) − I] + [ρM1 (q1 ) + M2 (q2 )] φ̇(q),
£
¤
CEL (q, q̇) = ρφT (q)C1 (q1 , q̇1 ) + φT (q) − I C2 (q2 , q̇2 ).
(22)
(23)
(24)
The above expressions for CL (q, q̇), CE (q, q̇), CLE (q, q̇), CEL (q, q̇) are derived from the following definition
"
#
"
#
"
#
CL (q, q̇) CLE (q, q̇)
ρM1 0
ρC1 0
d ¡ −1 ¢
−T
−T
:= S (q)
S (q) + S (q)
S−1 (q), (25)
CEL (q, q̇) CE (q, q̇)
0
M2 dt
0
C2
where we omit arguments to avoid cluster.
We call the n-DOF system in (20) the locked system, since it represents the dynamics of the teleoperator after
being perfectly coordinated (locked). Also, the other n-DOF system in (21) is referred to as the shape system which
reflects the coordination aspect.
Notice that, as the design criteria propose, the inertia of the locked system in (20) is exactly the desired natural
inertia ML (q) in (9), and the shape system velocity vE is the time-derivative of the coordination error qE (t) (see
Eq. (16)). Therefore, the coordination error qE is explicitly represented by the shape system configuration.
Also, FE in (21) and FL in (20) (both defined in (19)) represent the mismatched human / environmental forces
inducing coordination error and the combined effect of the human / environmental forces on the overall motion,
8
respectively. In contrast to the linearly dynamically similar case in [9], the locked and shape system dynamics in
(20)-(21) are only partially decoupled because of the coupling terms CLE (q, q̇)q̇E and CEL (q, q̇)vL .
Proposition 2 The partially decomposed dynamics (20)-(21) have the following properties:
1) ML (q) and ME (q) are symmetric and positive definite. Moreover, the scaled kinetic energy (5) of the original
teleoperator is decomposed into the sum of the kinetic energies of the shape and locked systems s.t.
1 T
1 T
κρ (q̇1 , q̇2 ) = vL
ML (q)vL + vE
ME (q)vE ;
2
|
{z
} |2
{z
}
κL (q,vL )
(26)
κE (q,vE )
2) ṀL (q) − 2CL (q, q̇) and ṀE (q) − 2CE (q, q̇) are skew-symmetric;
3) CLE (q, q̇) + CTEL (q, q̇) = 0.
Proof: Item 1 is a direct consequence of (17). In order to prove items 2 and 3, let us define M(q) := diag[ρM1 (q1 ),
M2 (q2 )] and C(q, q̇) := diag[ρC1 (q1 , q̇1 ), C2 (q2 , q̇2 )] where q := [qT1 , qT2 ]T ∈ <2n . Then, using (17) and (25)
with the fact that Ṁ = C + CT (from Ṁ − 2C = −[Ṁ − 2C]T ), we have
"
#
¢
ṀL − 2CL
−2CLE
d ¡ −T
d ¡ −1 ¢
=
S MS−1 − 2S−T M
S
− 2S−T CS−1
dt
dt
−2CEL
ṀE − 2CE
d ¡ −T ¢
d ¡ −1 ¢
=
S
MS−1 − S−T M
S
− S−T [C − CT ]S−1 ,
dt
dt
|
{z
}
skew−symmetric
where we omit the arguments to avoid cluster. Thus, ṀL (q) − 2CL (q, q̇) and ṀE (q) − 2CE (q, q̇) are skewsymmetric and CLE (q, q̇) = −CTEL (q, q̇).
Following proposition 2, ML (q), ME (q) and CL (q, q̇), CE (q, q̇) in (20)-(21) can be thought of as inertia and
Coriolis matrices of the shape and locked systems. Thus, with the cancellation of the coupling terms via the locked
and shape system controls TL and TE , the original 2n-DOF nonlinear teleoperator (1)-(2) can be decomposed into
the n-DOF locked and n-DOF shape systems whose dynamics are decoupled from each other and have structure
similar to the usual n-DOF robotic dynamics.
From (13) and (19), the environmental supply rate (3) and the controller supply rate (4) are given by merely the
sum of the respective supply rates of the locked and shape systems s.t.
sρ (q̇1 , q̇2 , F1 , F2 ) = FTL q̇L + FTE q̇E ,
and sc (q̇1 , q̇2 , T1 , T2 ) = TTL q̇L + TTE q̇E .
(27)
The significance of this property (27) is that the energetic passivity (3) (or controller passivity (4) resp.) of the
original teleoperator (1)-(2) can be assessed by considering energetic passivity (or controller passivity, resp.) of the
locked and shape systems with respect to their supply rates. The following corollary is a direct consequence of this
property (27) and item 3 of proposition 2.
Corollary 1 The controller supply rate associated with the decoupling control TdL = CLE (q, q̇)q̇E , and TdE =
CEL (q, q̇)vL is zero, since
T
(TdL )T vL + (TdE )T q̇E = vL
CLE (q, q̇)q̇E + q̇TE CEL (q, q̇)vL = 0.
Thus, the decoupling of the shape and locked systems in (20)-(21) does not violate energetic passivity of the
closed-loop teleoperator.
9
Geometrically, the locked system velocity vL defines the projection of the velocity to the current level set: Hc(t) :=
{(p1 , p2 ) ∈ <2n |qE (p1 , p2 ) = qE (q1 (t), q2 (t))}, while the shape system velocity vE defines its orthogonal
complement w.r.t. the inertia metric (Riemannian metric) as shown by the property (17). Furthermore, if vE = 0,
the shape system dynamics and the coupling term CLE (q, q̇)q̇E vanish, while the locked system dynamics and the
coupling term CEL (q, q̇)vL become the constrained dynamics (Levi-Civita connection) on Hc(t) and the second
fundamental form, respectively [31], [32]. For more details on geometric property of the decomposition, please
refer to [23].
IV. C ONTROL L AW D ESIGN
To achieve the control objectives in section II-B, we design the locked and shape system controls TL , TE in
(20)-(21) (hence, (T1 , T2 ) in (1)-(2) via Eq. (19)) to be:
à !
Ã
!
Ã
!
Ã
!
à !
TL
CLE (q, q̇)q̇E
TPL ?
TVL ?
T0L
:=
+
+
+
,
(28)
TE
CEL (q, q̇)vL
TPE ?
TVE ?
T0E
|
{z
}
| {z }
| {z }
| {z }
passive
decoupling
obstacle
avoidance
motion
guidance
inertia scaling
+coordination
where we assume that the position and velocity readings q1 , q2 , q̇1 , q̇2 are accurate, while the force sensings F1 , F2
and the identified inertias M1 (q1 ), M2 (q2 ) may possibly be incorrect.
A. Coordination Control
The control objective for the shape system (21) is qE (t) := q1 (t) − q2 (t) → 0. Thus, we design the shape
system control T0E in (28) (with TVE ? = TPE ? = 0 temporarily) to be:
b E (t),
T0E := −Kv q̇E − Kp qE −F
|
{z
}
(29)
P D−control
b E (t)
where Kv , Kp ∈ <n×n are constant symmetric and positive-definite damping and spring gain matrices and F
is the estimation of the mismatched forcing FE (19) of the shape system (21).
Proposition 3 Assume that the inertia matrices M1 (q1 ), M2 (q2 ), the Coriolis matrices C1 (q1 , q̇1 ), C2 (q2 , q̇2 ),
and the human/environmentlal forces F1 , F2 in (1)-(2) are bounded. Then, the shape system dynamics in (21)
under the total control (28) with the coordination control (29) and TVE ? = TPE ? = 0 has (qE (t), q̇E (t)) = (0, 0)
b E (t) in (29) is correct (i.e.
as a global exponentially stable equilibrium if the estimate of the mismatched force F
b E (t) = FE ). If the feedforward cancellation of FE is not used or its estimation error F̃E := FE − F̂E in (29)
F
is bounded, then (qE , q̇E ) is ultimately bounded [33].
Proof: The closed-loop dynamics shape system (21) under the control (29) is given by
ME (q)q̈E + CE (q, q̇)q̇E + Kv q̇E + Kp qE = FE
b
−F
| {zE}
eE.
=: F
(30)
f eedf orward
cancellation
Define the Lyapunov function candidate to be:
1
1
V (t) := q̇TE ME (q)q̇E + qTE (Kp + ²Kv ) qE + ²qTE ME (q)q̇E =
2
2
à !T
à !
qE
qE
P(t)
,
q̇E
q̇E
(31)
10
where ² > 0 is a sufficiently small scalar so that we can find a symmetric and positive-definite matrix P(t) ∈ <2n×2n
and the cross-coupling term ²qTE ME (q)q̇E is to achieve the exponential convergence. Then, with the closed-loop
dynamics (30) and the fact that ṀE − 2CE = −[ṀE − 2CE ]T (from proposition 2), we have
d
V (t) = −q̇TE [Kv − ²ME (q)] q̇E − ²qTE Kp qE + ²q̇TE CE (q, q̇)qE + (q̇E + ²qE )T F̃E ,
(32)
dt
where the induced norm of CE (q, q̇) is bounded from the assumption and its definition (22). Also, since Kp , Kp
are positive definite, we can always find a small scalar ² > 0 and a positive definite matrix Q(t) ∈ <2n×2n s.t.:
à !
³
´
qE
d
V (t) ≤ − qTE q̇TE Q(t)
+ (q̇E + ²qE )T F̃E ≤ −γ · V (t) + (q̇E + ²qE )T F̃E ,
(33)
dt
q̇E
T
for some exponential convergence rate γ > 0 which may be estimated from γ ≥ inf x∈<2n , t≥0 xxT Q(t)x
P(t)x > 0.
Thus, from (33), the Lyapunov function (31) satisfies
V̇ (t) ≤ −γ · V (t) + λV 2 (t)Femax ,
1
(34)
e E (t) = 0, ∀t ≥ 0, (qE (t), q̇E (t)) → 0,
for some λ > 0, where Femax ≥ kF̃E (t)k ∀t ≥ 0. Therefore, if F
exponentially, and if F̃E (t) in (29) is bounded, The Lyapunov function V (t) (and hence (qE (t), q̇E (t))) is ultimately
bounded with the bound given by:
·
¸2
λe
V̄ :=
Fmax .
(35)
γ
A sufficient condition for the bounded Mi (qi ) and Ci (qi , q̇i ), i = 1, 2, is that the configuration space is compact
with smooth inertia matrix (e.g. for revolute jointed robots) and the operating speed (q̇1 , q̇2 ) is bounded. The latter
condition can be ensured by making sure that the closed loop teleoperator is passive.
B. Obstacle Avoidance: Artificial Potential Field Control
To prohibit regions of the master or slave workspaces, artificial potential field method is used. Smooth positive and
bounded real potential functions ϕ1 , ϕ2 are defined on the master and the slave configuration spaces, respectively:
ϕ1 : q1 7→ ϕ1 (q1 ) ∈ <+ ,
ϕ2 : q2 7→ ϕ2 (q2 ) ∈ <+ ,
(36)
so that ϕ1 (q1 ) and ϕ2 (q2 ) are large in the prohibited regions [26]. Let dϕi (qi ) ∈ <n be their respective smooth
and bounded gradient one-forms:
·
¸
∂ϕi ∂ϕi
∂ϕi T
dϕi (qi ) :=
,
, , ..., n
∈ <n , i = 1, 2,
(37)
∂qi
∂qi1 ∂qi2
where qi = [qi1 , qi2 , ..., qin ]T , i = 1, 2.
The potential field control in (28) is defined to be the negative gradient one-form of the potential functions
applied on the master and slave robots given by
!
Ã
!
"
#Ã
−ρ · dϕT1 (q1 )
TPL ? (q)
I
I
.
(38)
:=
−dϕT2 (q2 )
TPE ? (q)
φT (q) φT (q) − I
{z
}
|
S−T (q)
As desired, the locked system is affected by the scaled sum of the master and slave potential fields as shown by
TPL ? (q) = −ρ·dϕT1 (q1 )−dϕT2 (q2 ) in (38). However, the potential fields control (38) also perturbs the shape system
11
(i.e. induces coordination error), since TPE ? (q) 6= 0, Notice that even if the same potential fields are imposed on
the master and slave configurations, TPE ? (q) would still be non-zero because the two robots will generally have
different inertias.
To combat the adverse effect of the potential fields on the coordination, the coordination control (29) is modified
s.t.
T0E := −Kv q̇E − Kp qE
b − TP ? (q)
−F
| E {z E }
.
(39)
f eedf orward cancellation
The feedforward cancellation of TPE ? (q) in (39) ensures that the potential field control does not affect the exponential
convergence result of the coordination error in proposition 3.
The passive implementation structure to be described in section V will turn off the feedforward cancellation term
in (39) when passivity will likely be violated. In this situation, the ultimate bound (35) for the Lyapunov function
in proposition 3 will be replaced with a new ultimate bound:
¸2
·
λ
V̄ =
Dmax ,
(40)
γ
where Dmax ≥ kFE (t) + TPE ? (q(t))k, ∀t ≥ 0. The cancellation of TPE? (q) in (38) can be omitted if its effect on
master-slave coordination is tolerable.
C. Guidance: Passive Velocity Field Control
We encode motion guidance objective for the coordinated teleoperator by defining a desired velocity field V̄L :
→ <n which assigns a desired velocity vector to each configuration of the coordinated teleoperator. Then
,guidance will be achieved by encouraging the coordinated teleoperator to move in the direction of the desired
velocity.
While it is natural to prescribe the desired velocity field for the coordinated teleoperator (i.e. V̄L (qL ) is a tangent
vector to the coordination submanifold Ho := {(q1 , q2 )|qE (q1 , q2 ) = q1 − q2 = 0} and qL = q1 = q2 ), the
control for it must also operate even when the teleoperator is not perfectly coordinated yet. To do this, the velocity
field V̄L is lifted from Ho to the ambient configuration space (<2n ) of the teleoperator (1)-(2). This is done via
the projection map q̃L : (q1 , q2 ) 7→ qL with the property that ∀q? ∈ <n , q̃L (q? , q? ) = q? ∈ <n . Examples of
q̃L include q̃L (q) := Aq1 + (I − A)q2 , where q = [qT1 , qT2 ]T and A ∈ <n×n is a constant matrix. For a more
detailed geometric prescription, please refer to [23].
Then, the control objective will be to make the locked system to follow the lifted velocity field V̄L ◦ q̃L : <2n →
<n s.t.
<n
vL (t) → α(V̄L ◦ q̃L )(q(t)),
(41)
for some scalar α which is determined by the kinetic energy of the locked system (higher speed when more energy
is present). This ensures that if qE (t) = q1 (t) − q2 (t) → 0, the velocity of the coordinated teleoperator converges
to a scaled multiple of the desired velocity field V̄L on the coordination submanifold Ho .
Passive Velocity Field Control (PVFC) [24], [25] is used to achieve the guidance objective (41) while enforcing
energetic passivity of the closed-loop teleoperator ((3) or (4)). PVFC does not generate any energy but only utilizes
the kinetic energy available in the open-loop locked system. Thus, velocity field following speed under PVFC is
determined by the kinetic energy in the open-loop locked system. Moreover, human operators and/or environments
can accelerate (or decelerate, resp.) the coordinated teleoperator by injecting energy into (or absorbing energy away
from, resp.) the teleoperator, while the direction of the motion is guided by PVFC.
12
Since PVFC does not increase nor decrease the kinetic energy of the locked system, any flow of the velocity
field (V̄L ◦ q̃L )(q(t)) needs to be of constant energy. In [24], [25], it is done by augmenting the velocity field by
incorporating a fictitious state. Here, we explicitly define the normalized velocity field VL : <2n 7→ <n s.t.
VL (q) := ξ(q)V̄L (q̃L (q)) ∈ <n ,
where
s
ξ(q) =
EV
1
T
2 V̄L (q̃L (q)) ML (q)V̄L (q̃L (q))
with EV ∈ <+ being a constant energy level. This ensures that locked system kinetic energy in Eq. (26) associated
with the normalized velocity field VL (q) is a constant s.t.
1
κL (q, VL (q)) = VL (q)T ML (q)VL (q) = EV ,
∀q ∈ <2n .
2
Following [25], we now define PVFC for the locked system (20). Let us define the following entities:
p(q, q̇) := ML (q)vL ,
(42)
P(q) := ML (q)VL (q),
w(q, q̇) := ML (q)V̇L (q) + CL (q, q̇)VL (q),
then, the PVFC for the guidance objective (41) is given by
Ã
!
Ã
!
TVL ?
G(q, q̇)vL + σR(q, q̇)vL
:=
,
TVE ?
0
(43)
(44)
where σ ∈ < is a control gain and G(q, q̇) and R(q, q̇) are skew-symmetric matrices defined by
G(q, q̇) =
1
{w(q, q̇)PT (q) − P(q)wT (q, q̇)},
2EV
|
{z
}
(45)
skew−symmetric
T
R(q, q̇) = {P(q)p (q, q̇) − p(q, q̇)PT (q)} .
|
{z
}
(46)
skew−symmetric
Proposition 4 Consider the locked system dynamics in (20) under the total control (28) with the PVFC (44) and
TPL ? + T0L = 0. Then,
1) the control supply rate in (27) associated with the PVFC is zero, i.e.
¡
¢T
¡
¢T
sP V F C (q̇1 , q̇2 , T1 , T2 ) = TVL ? vL + TVE ? q̇E = 0;
2) if FL (t) = 0 ∀t ≥ 0, vL (t) → βVL (q(t)) exponentially from almost every initial condition (except the set
of unstable equilibria of vL = −βVL (q), which is of measure 0), where the constant β is given by
s
1 T
2 vL ML (q)q̇L
β = sign(σ) 1
.
(47)
T
2 VL (q) ML (q)VL (q)
Proof: The supply rate is 0 because G(q, q̇) and R(q, q̇) are skew-symmetric. Convergence proof follows the
standard PVFC proof in [25] with the velocity field following error defined to be eβ := vL − βVL (q) where β is
the scalar defined in the proposition. Readers are referred there for details. (The proof is included in the Appendix
for convenience of the reviewers but will be omitted in the final version).
13
D. Inertia Scaling Control
The closed-loop dynamics of the locked system (20) under the total control (28) with the potential field control
(38) and the PVFC (44) is given by
TPL ? (q)
| {z }
ML (q)v̇L + CL (q, q̇)vL =
potential f ield
+ [G(q, q̇) + σ · R(q, q̇)] vL +T0L + FL ,
|
{z
}
(48)
PV FC
where FL = ρF1 + F2 is the combined human / environments forcing on the locked system, and T0L is the inertia
scaling control to be designed.
Suppose that we achieve the master-slave coordination (7) so that the n-DOF shape system (21) vanishes. Then,
since vL → q̇1 → q̇2 from the velocity decomposition (13), the left hand side of the given dynamics (48) becomes
the n-DOF dynamics of the coordinated teleoperator (8) with qL := q1 = q2 . Thus, comparing the dynamics (48)
with the target dynamics (12), we design the inertia scaling control T0L to be
¤
1−η £
T0L :=
· FL + TPL ? (q) ,
(49)
η
where η > 0 is a user-specific inertia scaling factor. The inclusion of the potential field control TPL ? (q, q̇) is
necessary for the locked system flywheel initialization as will be shown in theorem 2.
With the inertia scaling control (49), we have the closed-loop locked system dynamics s.t.
η [ML (q)v̇L + CL (q, q̇)vL ] −η [G(q, q̇) + σR(q, q̇)] vL − TPL ? (q, q̇) = FL .
|
{z
}
(50)
=:Ψ(q,q̇)
where η > 0 is the inertia scaling factor, and the function Ψ(q, q̇) ∈ <n is defined so that the potential field control
and the PVFC are embedded in it. Thus, if we achieve the master-slave coordination (7), the target dynamics (12)
will also be achieved from (50).
V. PASSIVE C ONTROL I MPLEMENTATION
A. Passive Control Implementation Structure
The total control law (28) consisting of the (modified) coordination control (39), the obstacle avoidance control
(38), the motion guidance control (44), and the inertia scaling control (49) can be written as:
à ! "
#Ã ! Ã
! Ã
¤!
1−η £
P?
TL
G(q, q̇) + σR(q, q̇) CLE (q, q̇)
vL
TPL ? (q)
η · FL + TL (q)
=
+
+
.
b E − TP ? (q)
TE
CEL (q, q̇)
−Kv
q̇E
−Kp qE + TPE ? (q)
−F
E
|
{z
}
|
{z
} |
{z
}
Ωorig : PVFC and D-action
=:Ω
Potential field and P-action
=:Tf f : feedforward
(51)
In order to see passivity property of the control (51), consider the following candidate storage function s.t.
1
κc,1 (t) = qTE Kp qE + ρϕ1 (q1 ) + ϕ2 (q2 ).
2
Then, differentiating (52) with the definition (38), we have
à !T Ã
!
vL
TPL ? (q)
d
T
κc,1 (t) = q̇E Kp qE + ρdϕ1 (q1 )q̇1 + dϕ2 (q2 )q̇2 = −
.
dt
q̇E
−Kp qE + TPE ? (q)
(52)
(53)
Thus, the supply rates associated with the P-action (Kp ) and the potential field control are taken from the storage
function κc,1 (t).
14
³
´
T q̇T . Then, using the skew-symmetricity of G(q, q̇), R(q, q̇) in
vL
E
(45)-(46), CLE (q, q̇) = −CTEL (q, q̇) (from proposition 2), and the equality (53), we can show that the controller
supply rate (27) satisfies
à !T Ã
¤!
1−η £
P ? (q)
·
F
+
T
v
d
L
L
L
η
sc (q̇1 , q̇2 , T1 , T2 ) = TTL vL + TTE q̇E = − κc,1 (t) − q̇TE Kv q̇E +
.
(54)
b E − TP ? (q)
dt
q̇E
−F
E
|
{z
}
Let us multiply both sides of (51) by
=:sf f (t)
Then, integrating (54) in time and using the fact that κc,1 (t) ≥ 0 and Kv is positive definite, we have,
Z t
Z t
sc (q̇1 (τ ), q̇2 (τ ), T1 (τ ), T2 (τ ))dτ ≤ κc,1 (0) +
sf f (τ )dτ.
(55)
0
0
This inequality shows that, except for the feedforward terms Tf f in (51) which is needed for good coordination
performance and apparent inertia scaling, the controller (51) satisfies the controller passivity condition (4). In fact,
this inequality (55) verifies the intuition that PD-control, potential field, and PVFC are intrinsically passive.
Therefore, to enforce the controller passivity condition (4), the energy associated with the feedforward terms
Rt
Tf f (t) in (51) (i.e. 0 sf f (τ )dτ in (55)) needs to be finite ∀t ≥ 0. In order for this, we incorporate fictitious energy
storage elements (with flywheel dynamics) [9], [25], [29], [34] into the controller such that the energy required by
the feedforward terms Tf f (t) is always taken from these flywheels. This way, the feedforward actions Tf f (t) in
(51) would be prevented from withdrawing too much energy unless the energy has been deposited in the flywheels.
Let the dynamics of the two 1-DOF fictitious flywheels (simulated in software) be:
L
E
Mf L ẍf = L Tf
E
(locked system f lywheel),
(56)
(shape system f lywheel),
(57)
E
Mf ẍf = Tf
where L Mf , E Mf are the inertias, L xf , L xf are the configurations, and L Tf , E Tf are the coupling torques (to be
designed below). Then, we implement the total control (51) using the following negative semidefinite structure s.t.





 
?
vL
G(q, q̇) + σR(q, q̇) CLE (q, q̇) Π ϕ (t)
0
TL
TP
(q)
L
  q̇ 
T  
−K q + TP ? (q))
CEL (q, q̇)
∆ d (t)
0
Σ E (t)  
E 
 E 

p E
E
+
(58)
L  = 

,

T


L
 Tf  


Π
−Π ϕ (t)
0
0
0
  ẋf 
0
E
E ẋ
ΣTE (t)
0
−Σ
0
0
0
Tf
f
{z
}
|
{z
}
|
Ω (t): negative semidefinite
Potential field and P-action
where Π ϕ (t) and Σ E (t) will be designed to generate the inertia scaling control and the feedforward cancellation (i.e.
Tf f in in (51)), respectively. Also, ∆ d (t) will be designed to be negative semidefinite. Thus, since CTLE (q, q̇) =
−CEL (q, q̇) from proposition 2, and G(q, q̇) and R(q, q̇) are skew-symmetric from (45)-(46), the implementation
matrix Ω (t) in (58) is negative semidefinite (NSD).
With this NSD implementation structure in (58), the following equalities are satisfied:
µ
¶
d 1L
T
L
L
L
L 2
ẋf Π ϕ (t)vL = − Tf ẋf = −
Mf ẋf (t) ,
(59)
dt 2
µ
¶
d 1E
T
E
E
E
E 2
ẋf Σ E (t)q̇E = − Tf ẋf = −
Mf ẋf (t) .
(60)
dt 2
These two equalities (59)-(60) show that, under the NSD implementation structure (58), the controller supply rates
of the (implemented) inertia scaling control (L ẋf Π ϕ (t)) and feedforward cancellation (E ẋf Σ E (t)) will be taken
from the locked and shape system flywheels (56)-(57), respectively.
15
To see that this NSD implementation structure (58) is sufficient to ensure controller passivity condition (4), let
T , q̇T , L ẋ , E ẋ ). Then, since Ω (t) in (58) is NSD, we have
us multiply Eq. (58) by (vL
f
f
E
à !T Ã
!
P ? (q)
v
T
d
L
L
= − κc,1 (t),
(61)
TTL vL + TTE q̇E + L Tf L ẋf + L Tf L ẋf ≤
P
?
dt
q̇E
−Kp qE + TE (q)
where the last equality is from (53). Thus, using (61) and the dynamics of the flywheels (56)-(57), we can show
that the controller supply rate (27) satisfies
µ
¶
µ
¶
d
d 1L
d 1E
d
L 2
E 2
sc (q̇1 , q̇2 , T1 , T2 ) ≤ − κc,1 (t) −
(62)
Mf ẋf (t) −
Mf ẋf (t) = − κc (t),
dt
dt 2
dt 2
dt
where κc (t) is defined to be the total controller storage function s.t.
1
1
1
κc (t) := qTE Kp qE + ρϕ1 (q1 ) + ϕ2 (q2 ) + L Mf L ẋ2f (t) + E Mf E ẋ2f (t).
2
|2
{z
} 2
(63)
=κc,1 in (52)
Integrating (62) and using the fact that κc (t) ≥ 0, we have
Z t
sc (q̇1 (τ ), q̇2 (τ ), T1 (τ ), T2 (τ ))dτ ≤ κc (0),
∀t ≥ 0,
(64)
0
i.e. the controller implemented in (58) satisfies the controller passivity condition (4). Thus, by proposition 1, the
teleoperator satisfies the required scaled energetic passivity property (3).
B. Design of Implementation Parameters
The remaining task now is to design the entries Π ϕ (t), ∆ d (t), and Σ E (t) in (58) so that the intended control
law in (51) can be duplicated. This is done as follows:
£
¤
1−η
Π ϕ (t) :=
· g(L ẋf ) · FL + TPL ? (q) = g(L ẋf ) · T0L ∈ <n ,
(65)
η
∆d (t) := −[1 − g(E ẋf ) · E ẋf ]Kv ∈ <n×n ,
(66)
p(t)
Σ E (t) := −g(E ẋf )Kv q̇E − E (FE + TPE ? (q)) ∈ <n .
(67)
ẋf
|
{z
}
{z
}
energy recapture |
f eedf orward cancellation
Here, the threshold function g(x) is defined by
g(x) :=

1



x
1
fo



1
fo
|x| > fo
sign(x) 0 6= |x| ≤ fo
(68)
|x| = 0,
where fo > 0 is a small threshold value on the flywheel speeds (|L ẋf |, |E ẋf |) so that the terms Π ϕ (t) and Σ E (t)
will not be too large (e.g. numerical resolution in the digital implementation), and p(t) is a switching function
designed to be

1 if (E ẋ , q , q̇ ) ∈ C at time t
E
E
f
p(t) :=
(69)
0 otherwise.
The design of the switching region C in (69) will be given later in theorem 2.
Comparing the intended control (51) with the implemented control (58) and (65)-(69), we make the following
observations:
16
•
•
•
The intended inertia scaling control in (51) will be duplicated via the term Π ϕ (t) in (65) when g(L ẋf )L ẋf = 1
(i.e. when L ẋf ≥ fo ). As the locked flywheel depletes energy below the threshold, the implemented inertia
scaling control will be gradually deactivated (i.e. L ẋf ¿ fo → L ẋf · g(L ẋf ) ≈ 0 → L ẋf · Π ϕ (t) ≈ 0).
Through the terms ∆ d (t) and Σ E (t) in (58), the intended coordination control (39) will be duplicated when
p(t) = 1 regardless of the value of shape system flywheel threshold function g(E ẋf ). Design of the switching
region C in (69) for p(t) will be given later in theorem 2. Roughly speaking, p(t) = 1 when there is sufficient
energy in the flywheel and the error (qE , q̇E ) are not too large.
The threshold function g(E ẋf ) in (66) and (67) also determines whether the dissipated energy associated
with the damping term −Kv q̇E in the coordination control (39) will be fully recaptured by the shape system
flywheel. When |E ẋf | < fo , only a portion is recaptured, and the rest is dissipated through ∆ d (t) which
becomes negative definite. However, when |E ẋf | ≥ fo , it is fully recaptured with ∆ d (t) = 0.
Theorem 1 Consider the teleoperator system (1)-(2) under the passive control implementation (58) and (65)-(69).
1) The closed-loop teleoperator is energetically passive (i.e. satisfies (3)), even if the sensing of the human /
environment forces F1 , F2 and the estimates of the inertia parameters M1 (q1 ), M2 (q2 ) are inaccurate;
2) Suppose that the estimations of F1 , F2 ,M1 (q1 ), M2 (q2 ) are accurate. Then, if |E ẋf (t)| > fo and (E ẋf (t),
qE (t), q̇E (t)) ∈ C for every t ≥ 0 (i.e. E ẋf g(E ẋf ) = 1 and p(t) = 1), (qE (t), q̇E (t)) → 0 exponentially
(i.e. (7) is satisfied). Also, vL (t) → q̇1 (t) → q̇2 (t);
3) Suppose again that the assumptions for item 2 of this theorem are satisfied. Then, if the locked system flywheel
speed is always above the threshold (i.e. |L ẋf (t)| > fo , ∀t ≥ 0), the target dynamics (12) is achieved.
Proof:
1) The case when the model inertia parameters are accurate have been demonstrated in section V-A. Denote b
? to
be the estimate of ? based on estimates of the human/environment force and of the model inertial parameters.
Then, from (13) and (19),
Ã
!
à !
à !
à !
b
bL
ρT
v
q̇
1
1
b T (q) TL ,
b
(70)
, and
=S
= S(q)
bE
T
q̇2
T2
q̇E
b
b L, T
b E ) in (70) are designed for
where S(q)
is the (inaccurate) decomposition matrix S(q). The controls (T
bL , q̇E are the estimated velocities of the locked and shape
the (incorrect) locked and shape systems, and v
bE = qE
systems under the inaccuracy. Notice from (13) that qE is not affected by the model uncertainty, i.e. q
ḃE = q̇E .
and q
From (70), we have
bTv
bT
sc (q̇1 , q̇2 , T1 , T2 ) = ρTT1 q̇1 + TT1 q̇1 = T
L bL + TE q̇E ,
(71)
i.e. the controller supply rate (27) is preserved with inaccurate decomposition matrix. Also, the potential field
control (38) under the inaccuracy is given by
Ã
!
Ã
!
T (q )
b P ? (q)
T
−ρ
·
dϕ
1
1
L
b −T (q)
:= S
.
(72)
T (q )
b P ? (q)
T
−dϕ
2
2
E
Then, the passive control implementation (58) under the inaccuracy is given by

 



b P ? (q)
bL
bL
T
v
T
L

 



b P ? (q)
bE 
 q̇E  −Kp qE + T
T
E
b (t)
,
+
=Ω


 L ẋ  
LT 
0

 f 
 f
E ẋ
ET
0
f
f
(73)
17
V(t)
slop =
γδv
2Dmax
switching
region C
κf (f )
o
Fig. 2. The switching region C given by the square root of the Lyapunov function
E
energy κf ( ẋf ) =
κf (t)
p
V (t) (31) and the shape system flywheel
1E
E 2
2 Mf ẋf .
b (t) is still NSD regardless of model uncertainty and inaccurate force
where the implementation matrix Ω
sensing.
The implementation structure (73), the potential field control (72), and the supply rate preservation (71) under
the inaccuracy are of the same forms as (58), (38), and (27), respectively. Thus, using the storage function
given in (63), the rest of the proof follows the same argument of section V-A;
2) Since g(E ẋf (t)) = E ẋ1f (t) and p(t) = 1 ∀t ≥ 0 from the assumptions, the implemented control (58) generates
the intended coordination control (51). Thus, from proposition 3, (qE (t), q̇E (t)) → 0 exponentially. Also,
from (13), it is easy to show that vL (t) → q̇1 (t) → q̇2 (t) as q̇E (t) = q̇1 (t) − q̇2 (t) → 0;
3) From the assumption, g(L ẋf (t)) = L ẋ1f (t) , ∀t ≥ 0, thus, the implemented control (58) generates the intended
locked system control in (51), therefore the desired locked system dynamics (50) is achieved. Also, since
(q1 (t), q̇1 (t)) → (q2 (t), q̇2 (t)) from item 2 of this theorem, the shape system dynamics (21) vanishes and
d
the target dynamics (12) is achieved from the desired locked system dynamics (50) with vL (t) → dt
q1 (t) →
d
d
dt q2 (t) → dt vL (t) from (15).
This passive control implementation has some similarity to the PC/PO approach proposed in [35] in that both keep
account of the net flow of energy. In our case, this is done using the stored energy in the flywheels. Only possibly
non-passive portion of the control law (feedforward action Tf f in (51)) is related to the state of the flywheels,
while the remaining portion of the control is designed to be intrinsically passive. Thus, our approach will likely
result in better performance, since only the feedforward action (e.g. inertia scaling and feedforward cancellation)
will be cut off when too much energy is withdrawn. Another advantage of our approach is that performance can be
ensured by appropriately initializing the flywheel energy when model parameters and force sensing are accurate,
as will be shown next.
C. Initialization of the Flywheels
When model parameters and force measurements are accurate and the flywheels have sufficient energy, theorem
1 ensures the exponential convergence of the coordination error, and that the target dynamics (12) is achieved.
The following theorem provides an upper bound for the minimum initial energy with which the flywheels will not
deplete energy so that the results of theorem 1 are guaranteed.
18
Theorem 2 (Flywheel initialization) Consider the mechanical teleoperator (1)-(2) under the implemented control
(58) and (65)-(69).
1) (Locked system flywheel) Suppose that the operating speed vL (·) is bounded and hence there exists a positive
scalar EL > 0 s.t.
1 T
v (t)ML (q(t))vL (t) ≤ EL , ∀t ≥ 0.
(74)
2 L
Then, energy will not be depleted in the locked system flywheel (i.e. |L ẋf (t)| > f0 ∀t ≥ 0), if we have
initialized the flywheel speed L ẋf (t) so that
1L
1
Mf L ẋ2f (0) > L Mf fo2 + |1 − η|EL ,
(75)
2
2
where f0 is the threshold in (68).
2) (Shape system flywheel) Let us design the switching region C in (69) to be (see figure 2)
£
¤ 2Dmax 1
1
C := {(E ẋf , qE , q̇E ) | E Mf E ẋ2f (t) − fo 2 >
V 2 (t)},
(76)
2
γδv
where V (t) is the Lyapunov function (31), V̄ is the ultimate bound (40) with Dmax ≥ kFE (t)+TPE ? (q)k ∀t ≥
0, γ > 0 is the exponential convergence rate (34), and δv > 0 is defined by
à !
³
´
qE
V (t) = qTE q̇TE P(t)
≥ δv2 kq̇E k2 .
(77)
q̇E
Suppose that we initialize the shape system flywheel (57) s.t.
1E
1
2Dmax 1
Mf E ẋ2f (0) > E Mf fo 2 +
V 2 (0),
2
2
γδv
(78)
i.e. (E ẋf (0), qE (0), q̇E (0)) ∈ C . Then, (E ẋf (t), qE (t), q̇E (t)) ∈ C ∀t ≥ 0, i.e. the feedforward cancellation
is turned on all the time (p(t) = 1 ∀t ≥ 0) without depleting the flywheel energy (|E ẋf (t)| ≥ f0 ∀t ≥ 0 from
the definition of the region C ).
Proof: The proof is included in given in the Appendix.
•
•
•
Theorem 2 is applied to the case when force sensing and model parameters are accurate. In this ideal case,
theorem 2 ensures no flywheel energy depletion, thus, the implemented control (58) duplicates the intended
control (51) ∀t ≥ 0 and the perfect coordination and the target dynamics are guaranteed. When force sensing
and model parameters are not accurate, the system will still remain energetically passive as guaranteed by
theorem 1 at the expense of possible performance degradation, since the feedforward terms might be turned
off.
The maximum amount of energy that can cause potential damages on humans and environments is determined
by the initial stored energy in the system (including the flywheels). Thus, by minimizing the initial stored
energy (e.g. using theorem 2), safety will be improved.
If (E ẋf , qE , q̇E ) ∈
/ C in (67) so that p(t) = 0, we have
·
¸
d 1E
E 2
Mf ẋf = E ẋf (t) · g(E ẋf )q̇TE Kv q̇E ≥ 0
(79)
dt 2
i.e. the shape system flywheel energy is non-decreasing. Thus, even if we fail to initialize the shape system
flywheel according to the condition (78), by exciting the shape system with time varying disturbance FE +
TPE ? (q) so that q̇E (t) 6= 0, we can eventually drive the system state (E ẋf , qE , q̇E ) to the switching region
C in (76). Hereafter, from theorems 1-2, the master-slave coordination is guaranteed without the state leaving
the invariant region C , if the estimations of F1 , F2 ,M1 (q1 ), M2 (q2 ) are accurate.
19
Aluminum
Wall
Edge of
Potential
Field
Desired
Path
Signal
Control
Master
Robot
Slave
Robot
Control
Law
Signal
Control
Velocity
Field
Velocity
Field
Human
Operator
Master Environment
Desired
Path
Slave Environment
An illustration of the experimental setup. Both the master and slave robots are 2-DOF planar robots and equipped
with force sensors. An aluminum wall is implemented in the slave environment for hard contact experiment. A potential field
(kidney shaped) is imposed in the slave environment, which also affects the master through the coordination (see (38)). A
velocity field is also defined for the locked system so that the master and slave follow the desired paths when coordinated.
Fig. 3.
VI. EXPERIMENTS
Experiments are performed for a pair of 2-links planar robots. To measure human force, a commercial force
sensor (JR3 Inc. CA) with a 120Hz low pass filtering is used, while slave environmental force is measured by a
custom built 2-DOF force sensor based on semiconductor-type strain gages with a 50Hz low pass filtering. Encoders
are used to read positions of the master and slave robots. MatLab xPC Target system (MathWork, MA) is used to
implement the controller with a 500Hz sampling rate. The power scaling ρ = 15 is used to allow human operators
to perceive shrink slave environment. The inertia scaling η = 0.6 is implemented to scale down the apparent inertia
of the coordinated teleoperator.
The best-identified (error bound ≤ 20%) inertia matrices of the master and the slave are [kgm2 ]:
"
#
0.0889 + 0.0144c2 0.0113 + 0.00719c2
M1 (q1 ) =
,
0.0113 + 0.00719c2
0.0113
"
#
3.74 + 0.419c2 0.286 + 0.210c2
M2 (q2 ) =
0.286 + 0.210c2
0.286
where c2 is the cosine of the distal link angle w.r.t. the proximal link. The lengths of the proximal and distal
links of the master and slave are (14cm, 14cm) and (38cm, 36cm), respectively. In nearly static manipulation
(e.g. interacting with a wall), the power scaling ρ becomes the joint torque scaling, since, from (12), we have
FL = ρF1 + F2 = 0 with q̇L (t) = q̈L (t) = 0. Consdering that the master and slave robots have different lengths,
the static Cartesian force scaling at the end effectors is given by ρ LLms ≈ 5.6 where Lm = 14cm and Ls = 37cm
are the average lengths of the master and slave robots, respectively. An illustration of the experimental setup is
given in figure 3.
20
Trajectories of Coordinated Teleoperator
0.8
Hard Contact with
Aluminum Wall
(T=47−53sec)
Final
point
0.6
B
T=30sec
0.4
x−Axis [m]
Potential
Field
T=32sec
0.2
0
T=17sec
−0.2
Potential
Field
Starting
APoint
−0.4
Velocity
Field Tracking
T=28sec
T=27sec
−0.6
−0.8
0.8
0.6
0.4
0.2
0
−0.2
−0.4
−0.6
−0.8
y−Axis [m]
Trajectory of the coordinated teleoperator represented in the slave environment. A velocity field (arrows), a potential
field (kidney-shaped), and a real aluminum wall in the slave environment (straight line) are implemented.
Fig. 4.
A. Tool Dynamics Rendering
As in figure 3, we implement a potential field on the slave configuration space so that the motion of the coordinated
teleoperator is confined inside of the kidney shaped region (see (38)). Note that the potential field also affects the
master through the master-slave coordination. We also impose a velocity field (collection of arrows) for the locked
system so that it guides the master and the slave to follow the common desired path when coordinated (see figure
3). This desired path is given by the following joint motions: the proximal link is moving counterclockwise while
the distal link is fixed at −90◦ (clockwise) w.r.t. the proximal link. These potential and velocity fields are similar
to the virtual tool dynamics of [7] in the sense that they help human operators perform a task more efficiently and
comfortably. An aluminum wall is also installed in the slave environment as shown in figure 3 for hard contact
experiment.
Experimental results are shown in figures 4 and 5. In region A (17 − 26sec), as shown in figure 4, the operator
can move the coordinated teleoperator along the edge of the potential field (stiffness of 2.5kN/m) utilizing it as
a virtual rail. When the teleoperator enters the region of the velocity field (around 27sec), the operator releases it
into an arbitrary direction to make it digress from the desired velocity field. However, as shown in figure 4, the
PVFC guides the teleoperator successfully so that, from 28sec, the coordinated teleoperator follows the desired
path (solid line) without human intervention (negligible human force during 28 − 30sec in figure 5). In region B
(32 − 40 sec), again the operator moves the teleoperator along the arc edge of the potential field (see figure 4).
At 47 − 53sec in region B, the operator makes a static hard contact against the aluminum wall. Effective
contact stiffness is 33kN/m due to the slave force sensor compliance. As shown in figure 5, excellent master-slave
21
Joint angle[degree]
Trajectory of Coordinated Teleoperator : η = 0.6
150
100
Velocity Field
Tracking
50
0
−50
−100
−150
Link1
Link2
−200
−250
15
20
25
30
35
40
45
50
55
60
Coordination Error
Error[degree]
1
Link1
Link2
0.5
0
−0.5
−1
15
20
25
30
35
40
45
50
55
60
Time[sec]
Master and Slave Forcings (x−Axis) : η = 0.6
100
Interaction w/
Potential Field
Force[N]
50
Hard Contact
w/ Alum. Wall
0
−50
Master (5.6 Fx)
Slave (−Fx)
−100
15
20
25
30
35
40
45
50
55
60
Master and Slave Forcings (y−Axis) : η = 0.6
150
Interaction w/
Potential Field
Force[N]
100
Hard Contact
w/ Alum. Wall
50
0
−50
−100
15
Master (6.8 Fy)
Slave (−Fy)
Pushing into
Velocity Field
20
25
30
35
40
45
50
55
60
Time[sec]
Locked and Shape Flywheel Energies
40
Locked Flywheel
Shape Flywheel
Energy[Nm]
35
30
25
20
15
10
15
20
25
30
35
40
45
50
55
60
Time[sec]
Fig. 5. Plots of position, coordination error, forcing, and flywheel energy corresponding to figure 4: PVFC (28 − 30sec),
contact with potential field (17 − 26sec, 32 − 40sec), and hard contact with the aluminum wall (47 − 53sec).
22
coordination is achieved (error less than ±0.2◦ for both links) due to the feedforward cancellation of FE in (39),
where TPE ? = 0 since the aluminum wall is located outside the potential field. Cartesian force scaling around 6.2
is obtained from the power scaling ρ = 15 (see figure 5).
As shown in figure 5, the locked system flywheel energy continuously decreases due to the friction which is
opposing to the inertia scaling down control (η < 1). In contrast, it was reported in [9] that the locked system
flywheel continuously gains energy when the inertia is scaled up (η > 1). Therefore, the locked system flywheel
energy may need to be reset occasionally if the friction is not compensated for.
Another experiment is performed to show the energetic passivity property of PVFC. As shown in figure 6, at
17sec and 30sec, the operator releases the teleoperator from almost the same positions with two different pushing
forces (30N at 17sec and 7N at 30sec) so that the teleoperator has two different levels of kinetic energy. The
teleoperator successfully follows the desired velocity field with different speeds and finishes the path following in
about 1sec and 3sec with larger and smaller forces, respectively. This shows that the PVFC guidance is passive in
the sense that the operator can speed up (or slow-down, resp.) the velocity field following speed of the teleoperator
by injecting (or extracting, resp.) energy. Along the return path in figure 6, the PVFC is switched off. Also, for
simplicity of data interpretation, the potential field control is turned off.
B. Robust Passivity
To demonstrate robust passivity property (item 1 of theorem 1) of the passive implementation structure (58), two
levels of time delay (35ms and 350ms) are imposed on the master and slave force sensing during the static hard
contact with the aluminum wall. To simplify data interpretation, the potential field and the PVFC are turned off.
Experimental results with 35ms and 350ms time-delays are shown in figures 7-8 and 9, respectively.
In figure 7, the contact becomes unstable due to the 35ms time-delay in force measurement and the closed-loop
teleoperator discharges energy (in the top of figure 7) to the ambient environment. This unstable energy discharge
comes from the locked system flywheel energy through the inertia scaling control as shown by the locked system
flywheel energy decrease.
As the flywheel depletes energy (around 6.5sec), the implemented inertia scaling control through the term Π ϕ (t)
in (58) is nearly turned off (i.e. L ẋf ¿ fo → L ẋf · g(L ẋf ) ≈ 0 → L ẋf · Π ϕ (t) ≈ 0) and the contact becomes stable
with a Cartesian force scaling of 6.4. Excellent coordination (error less than ±0.5◦ for both links) is preserved
during this experiment, since the shape system flywheel maintains sufficient energy level to generate the feedforward
cancellation.
In figure 8, the operator reduces and increases the contact force while maintaining the instability, respectively.
Comparing the mild instability (above figure) with the aggressive instability (below figure), we can see that as the
instability becomes mild (or aggressive, resp.), the flywheel depletes energy slowly (or faster, resp.), thus, contact
stability is regained also slowly after 10sec of instability (or quickly after 1 sec of instability, resp.). During the
aggressive instability, some force sensor saturations occurred (not shown), however, passivity is still ensured by the
passive implementation structure (58).
With the 350ms time-delay (figure 9), the inertia scaling control induces instability first (4 − 13sec) and becomes
deactivated (around 13sec) as the locked system flywheel depletes energy. However, after 13sec, the contact still
remains unstable, since the feedforward cancellation starts inducing instability due to the large time-delay. When
the shape system flywheel also depletes energy below the threshold (around 30sec), the feedforward cancellation is
turned off (p(t) = 0 from (67)) and the contact becomes finally stable with drastically degraded coordination (≥ 2◦
for both links) and Cartesian force scaling of 6.1. Several switchings of the feedforward cancellation occur around
23
Joint angle[degree]
Position of Coordinated Teleoperator under PVFC
100
Slower Tracking with
Smaller Pushing Force
50
Faster Tracking with
Larger Pushing Force
0
−50
−100
−150
Link1
Link2
−200
15
20
25
30
35
40
Total Pushing Force
30
Force[N]
25
Larger
Pushing Force
20
15
10
Stopping
Force
5
0
15
Smaller
Pushing Force
20
25
30
35
40
Time[sec]
Trajectories of Coordinated Teleoperator: PVFC
0.8
0.6
End of PVFC:
T= 18sec (Larger Force)
33.5 sec (Smaller Force)
T=21sec
Potential
Field
0.4
End
Point
x−Axis
0.2
0
Desired
Path
−0.2
Releasing
Point
−0.4
−0.6
−0.8
0.8
Large Push
Forcing (T=17sec)
Potential
Field
0.6
0.4
Return
Path
T=27sec
Small Push
Forcing (T=30sec)
0.2
0
−0.2
−0.4
−0.6
−0.8
y−Axis
Fig. 6. Plots for PVFC experiment. Two different pushing forces (30N at 17sec and 7N at 30sec) when releasing the teleoperator
result in two different speed with which the teleoperator is following the desired path (17 − 18sec and 30 − 33.5sec). For
simplicity of data interpretation, the potential field control is turned off.
24
1
Energy Inflow to the System: Time−Delay 35ms, η=0.6
Energy[Nm]
0
−1
−2
−3
−4
−5
−6
0
2
6
8
10
12
14
Locked Flywheel Energy : Time−Delay 35ms, η = 0.6
10
Energy[Nm]
4
Flyw. Energy
Inertia Scaling
8
Inertia Scaling
Fully Turn−On
6
4
2
Inertia Scaling Nearly Turn−Off
0
0
2
4
6
8
10
12
Shape Flywheel Energy : Time−Delay 35ms, η = 0.6
14
Energy[Nm]
13.5
13
12.5
12
Feedforward Cancellation Activated p(t)=1
11.5
11
Flyw. Energy
Switching p(t)
10.5
10
0
2
4
6
8
10
12
Time[sec]
Coordination Error : Time−Delay 35ms, η = 0.6
1
Error[degree]
Link1
Link2
0.5
0
−0.5
−1
0
2
4
6
8
10
12
Total Master and Slave Forcings : Time−Delay 35ms, η = 0.6
150
Master
Slave
Force[N]
Instability induced
from Inertia Scaling
100
Stable Contact
with Inertia Scaling Off
50
0
0
2
4
6
8
10
12
Time[sec]
Fig. 7. Plots for the robust passivity experiment with 35ms time-delay on force sensing. The locked system flywheel discharges
energy to the master-slave environments (negative energy inflow) via the inertia scaling until it depletes energy so that the
inertia scaling is nearly turned off and the contact becomes stable (around 6.5sec).
25
Total Forcings of Mild Instability : Time−Delay 35ms, η = 0.6
200
Force[N]
Master
Slave
150
100
Mild Forcing during the Instability
Stable
Contact
50
0
0
2
4
6
8
10
12
Locked Flywheel Energy and Inertia Scaling Switchings
15
Energy[Nm]
Flywh. Energy
Inertia Scaling
10
5
Inertia Scaling Fully Activated
Inertia Scaling
Nearly Turn−Off
0
0
2
4
6
8
10
12
Time[sec]
Total Forcings of Aggressive Instability : Delay 35ms, η = 0.6
200
150
Force[N]
Master
Slave
Aggressive Forcing
during the Instability
Stable Contact
100
50
0
0
2
4
6
8
10
12
Locked Flywheel Energy and Inertia Scaling Switching
15
Energy[Nm]
Flywh. Energy
Inertia Scaling
10
Inertia Scaling
Fully Activated
5
Inertia Scaling Nearly Deactivated
0
0
2
4
6
8
10
12
Time[sec]
Plots of “mild” (above) and “aggressive” (below) instability with 35ms time-delay. In mild instability (above), due to
the smaller forcings, the instability becomes less harmful as represented by the slow energy depletion of the locked system
flywheel and time duration of the instability (around 10sec) is relatively longer, while in aggressive instability (below), time
duration of instability is shorter (1sec) and the locked system flywheel depletes energy faster due to the larger forcing. Thus,
the inertia scaling is deactivated faster so that the contact becomes stable quickly.
Fig. 8.
26
Energy Inflow to the System: Time−Delay 350ms, η=0.6
2
Energy[Nm]
0
−2
−4
−6
−8
−10
−12
−14
0
Energy[Nm]
12
5
10
15
20
25
30
35
Locked Flywheel Energy : Time−Delay 350ms, η = 0.6
Flywh. Energy
Inertia Scaling
10
8
Inertia Scaling
Fully On
6
4
2
0
Inertia Scaling Nearly Off
0
5
10
15
20
25
30
35
Shape Flywheel Energy : Time−Delay 350ms, η = 0.6
14
Energy[Nm]
12
10
Feedforward
Cancellation
Deactivated p(t)=0
8
Feedforward Cancellation Activated p(t)=1
6
4
Flywh. Energy
Switching 5×p(t)
2
0
0
5
10
15
20
25
30
35
Time[sec]
Coordination Error : Time−Delay 350ms, η = 0.6
2.5
Link1
Link2
Error[degree]
2
Stable
Contact
1.5
1
0.5
0
−0.5
−1
0
5
10
15
20
25
30
35
Total Master and Slave Forcings : Time−Delay 350ms, η = 0.6
150
Force[N]
Instability from
Inertia Scaling
Slave
Instability from
Feedforward
Cancellation
100
50
Master
0
0
5
10
15
20
25
30
35
Time[sec]
Fig. 9. Plots for the robust passivity experiment with 350ms time-delay on force sensing. Instability is induced both from the
inertia scaling and the feedforward cancellation due to the large time delay. The Locked and shape system flywheels discharge
energy sequentially through the inertia scaling and the feedforward cancellation. As both flywheels deplete energy after 30sec,
the contact becomes stable eventually.
27
30sec due to the energy re-charging of the shape system flywheel with deactivation of the feedforward cancellation
(see Eq. (79)).
This experiment clearly shows the robust passivity property of the passive implementation structure (58), i.e.
passivity is guaranteed robustly even in the presence of the drastically corrupted force sensing, while controller
performance might be compromised. Without using the passive implementation (58), the induced instability will not
disappear and the amount of energy generated by the closed-loop teleoperator will be unbounded. Thus, the passive
implementation structure enhances interaction safety and coupled stability (asymptotic) substantially. In fact, due
to the passive implementation, this robust passivity property is guaranteed in the presence of the time-delay of an
arbitrary magnitude or even saturations in the force sensing.
VII. C ONCLUSIONS
In this paper, we propose a novel passive bilateral control framework for nonlinear mechanical teleoperators,
which has been a long standing problem due to the highly nonlinear system dynamics and the difficulty in ensuring
non-fragile energetic passivity of the closed-loop teleoperator.
The key innovation of the proposed control framework is the passive decomposition which enables us to
decompose the nonlinear dynamics of a 2n-DOF mechanical teleoperator into two decoupled n-DOF robot-like
dynamics according to two aspects of teleoperation while preserving energetic passivity: master-slave coordination
(shape system) and the overall behavior of the coordinated teleoperator (locked system). Thus, we can achieve
the master-slave coordination and some desired behavior of the coordinated teleoperator by controlling the n-DOF
shape system and the n-DOF locked system, respectively and separately, while enforcing energetic passivity.
Using the feedforward cancellation of the mismatched forcings on the shape system, we achieve the master-slave
position coordination in the presence of arbitrary human / environmental forcing. Also, by scaling the combined
human / environment forcing on the locked system, we endow the coordinated teleoperator with a user-specific
programmable apparent inertia. To assist human operators perform a task more efficiently and comfortably, motion
guidance and obstacle avoidance for the coordinated teleoperator are also achieved by utilizing Passive Velocity
Field Control (PVFC) and artificial potential function, respectively.
By implementing the designed controls in the special passive implementation structure, energetic passivity of
the closed-loop teleoperator is enforced robustly even in the presence of model uncertainty and inaccurate force
measurement, thus, interaction safety and coupled stability are substantially enhanced. A bilateral power scaling
is also achieved while ensuring energetic passivity of the closed-loop teleoperator. Experiments are performed to
validate and demonstrate the properties of the proposed control scheme.
APPENDIX 1: Proof of Proposition 4 (will be omitted in the final version)
In order to prove the proposition 4, we need the following lemma.
Lemma 1 If the desired velocity filed VL (q) satisfies the constant energy condition (42), then the inverse dynamic
compensation term w(q, q̇) in (43) annihilates VL (q) such that
wT (q, q̇)VL (q) = 0
Proof: Notice that, from the condition (42),
1 d
1
d
T
κVL (qL , VL (q)) =
{VL (q)T ML (q)VL (q)} = VL
(q){ ṀL (q)VL (q) + ML (q)V̇L (q)}
dt
2 dt
2
T
T
= VL (q){ML (q)V̇L (q) + CL (q, q̇)VL (q)} = VL (q)w(q, q̇) = 0
(80)
28
With the assumption of proposition 1, the closed-loop locked system dynamics is given as
ML (q)v̇L + CL (q, q̇)vL = G(q, q̇)vL + σR(q, q̇)vL + FL .
(81)
Let us add the term −βw(q, q̇) to both sides of the locked system dynamics (81), then we have
ML (q)ėβ + CL (q, q̇)eβ = −βw(q, q̇) + G(q, q̇)vL + σR(q, q̇)vL + FL ,
(82)
where eβ := vL − βVL (q) is β -velocity error eβ .
Using Lemma 1, EV in (42), (44), and (45), we have
G(q, q̇)eβ = G(q, q̇)vL −
β
w(q, q̇)PT (q)VL (q) = G(q, q̇)vL − βw(q, q̇).
2EV
(83)
Combining (82) and (83), we have
ML (q)ėβ + {CL (q, q̇) − G(q, q̇)}eβ = σR(q, q̇)vL + FL .
(84)
Let’s define a Lyapunov function
1
Wβ (q, eβ ) = eTβ ML (q)eβ ,
2
then, by differentiating it using (84) and FL = 0, we obtain
d
T
Wβ (q(t), eβ (t)) = σeTβ R(q, q̇)vL = −σβVL (q)R(q, q̇)vL = −σβ{4β 2 EV2 − (VL
(q)ML (q)vL )2 },
dt
where we use the skew-symmetricity of G(q, q̇) and R(q, q̇) in (45)-(46), and the definition of β (47) s.t.
β2 =
(85)
(86)
1 T
v ML (q)vL .
2EV L
With β 2 above and EV in (42), we can rewrite the Lyapunov function (85) such that
Wβ (t)
T
= 2βEV − VL
(q)ML (q)vL .
β
(87)
d
Wβ (q(t), eβ (t)) = −4σβEV µ(t)Wβ (q(t), eβ (t)),
dt
(88)
·
T (q)M (q)v ¸
VL
1
L
L
1+
.
µ(t) :=
2
2βEV
(89)
Thus, by using (86) and (87), we have
where
T (q)M (q)v | ≤ 2|β|E , 0 ≤ µ(t) ≤ 1.
Note that, from Schwartz’s inequality |VL
L
L
V
Following proposition 2, we have
·
¸
d 1 T
v ML (q)vL = FTL vL
dt 2 L
(from (81) and the skew-symmetricity of G(q, q̇) and R(q, q̇)), thus, β(t) will be constant if FL = 0. Therefore,
if FL = 0, using (88) and differentiating (89), we have
d
Wβ (t) = −4σβEV · µ(t) · Wβ (t),
dt
d
σβ
µ(t) = + 2 · µ(t) · Wβ (t).
dt
β
(90)
(91)
29
Eq. (91) is obtained by utilizing (87) and recognizing that β is a constant due to FL = 0, before differentiating (89),
and finally by using (90). Thus, if σβ > 0, (91) shows that µ(t) is nondecreasing, since µ(t) ≥ 0 and Wβ (t) ≥ 0.
Thus, (90) becomes
d
Wβ (t) ≤ −4σβEV · µ(0) · Wβ (t),
dt
showing that Wβ (t) → 0 exponentially from any initial condition (q(0), eβ (0)) with µ(0) 6= 0. Hence, eβ → 0
exponentially at a rate ≥ 2σβµ(0)EV .
The set of initial condition of µ(0) = 0 is exactly given by the set of unstable equilibria of vL = −βVL (q),
which is measure 0. Thus, for the case of σβ > 0, vL → βVL (q) globally and exponentially from almost every
initial condition.
APPENDIX 2: Proof of Theorem 2
[This proof is nearly the same as the proof in [9] and therefore will be omitted in the final version. It is retained
for the convenience of the reviewers.]
1. (Locked system flywheel initialization) From (58) with ζ := 1−η
η > −1, we have
·
¸
£
¤
d 1L
T
Mf L ẋ2f = −ζ L ẋf g(L ẋf ) · vL
FL + TPL ? (q) .
(92)
dt 2
Also, from the closed-loop locked system dynamics (20) under the implemented control (58) with skew-symmetric
(45)-(46), we have
·
¸
£
¤
d 1 T
T
vL ML (q)vL = {1 + ζ L ẋf g(L ẋf )} · vL
FL + TPL ? (q) .
(93)
dt 2
Combining (92)-(93), we have
¸
·
·
¸
ζ · L ẋf · g(L ẋf ) d 1 T
d 1L
Mf L ẋ2f = −
v
M
(q)v
(94)
L
L ,
dt 2
1 + ζ · L ẋf · g(L ẋf ) dt 2 L
where ζ > −1 and 0 ≤ L ẋf g(L ẋf ) ≤ 1, thus, the denominator of the right term is non-zero.
¯L
¯
¯ ẋf (τ )¯ > fo ,
Suppose that L ẋf (0) is initialized following (75). Then, there exists
t
≥
0
so
that
∀
τ
∈
[0,
t)
,
¯ L
¯
L
L ẋ (τ ) · g(L ẋ (τ )) = 1. Thus, by integrating (94) for [0, t) with ¯ ζ· ẋf ·g( ẋf ) ¯ ≤ |ζ| and ζ > −1, we have,
¯ 1+ζ·L ẋf ·g(L ẋf ) ¯
f
f
1+ζ
∀t ≥ 0,
¯
¯
¯
1L
|ζ| ¯¯ 1 T
1 T
L 2
L 2
Mf ( ẋf (t) − ẋf (0)) ≥ −
vL (t)ML (q)vL (t) − vL (0)ML (q)vL (0)¯¯
¯
2
1+ζ 2
2
|ζ|
≥−
EL = −|1 − η|EL .
(95)
1+ζ
Thus, if the locked system flywheel (56) is initialized following (75), from (95), |L ẋf (t)| > fo , ∀t ≥ 0, since t in
(95) can be arbitrary.
2. (Shape system flywheel initialization) It suffice to prove that the switching region C (76) is invariant. Suppose
that (E ẋf (t1 ), qE (t1 ), q̇E (t1 )) ∈ C so that p(t1 ) = 1 and E ẋf (t1 )g(E ẋf (t1 )) = 1. Then, from (58),
·
¸
£
¤T
d 1E
E 2
Mf ẋf (t1 ) = E Tf E ẋf = q̇TE Kv q̇E + FE + TPE ? (q) q̇E
dt 2
Dmax 1
≥ −kFE + TPE ? (q)k kq̇E k ≥ −
(96)
V 2 (t1 ),
δv
where δv is defined in (77). Because C (76) is an open set, (96) can be integrated to at least up to some t > t1
1
1
1
d
without the state leaving C . By integrating (96) using dt
V 2 (t) ≤ − γ2 V 2 (t) (from V − 2 (t) ≥ 0 and (34)), we have,
for some t ≥ t1 ,
i
1
1E
1
2Dmax h 1
Mf E ẋ2f (t) − E Mf E ẋ2f (t1 ) ≥
V 2 (t) − V 2 (t1 ) .
(97)
2
2
γδv
30
Also, since (E ẋf (t1 ), qE (t1 ), q̇E (t1 )) ∈ C , we have
1E
1
2Dmax 1
Mf E ẋ2f (t1 ) − E Mf fo2 >
V 2 (t1 ),
2
2
γδv
(98)
from the definition of C (76). By summing (97) and (98), we have
£
¤ 2Dmax 1
1E
Mf E ẋ2f (t) − fo2 ≥
V 2 (t)
2
γδv
i.e. (E ẋf (t), qE (t), q̇E (t)) ∈ C at time t. Since t can be arbitrary, C is invariant, thus if (E ẋf (0), qE (0), q̇E (0)) ∈ C ,
then (E ẋf (t), qE (t), q̇E (t)) ∈ C , ∀t ≥ 0.
R EFERENCES
[1] J. E. Colgate. Coupled stability of multiport systems - theory and experiments. Transactions of the ASME, Journal of Dynamic Systems,
Measurement and Control, 116(3):419–428, 1994.
[2] J. C. Willems. Dissipative dynamical systems part1: general theory. Arch. Rational Mech. Anal., 45(22):321–351, 1972.
[3] R. J. Anderson and M. W. Spong. Bilateral control of tele-operators with time delay. IEEE Transactions on Automatic Control,
34(5):494–501, 1989.
[4] G. Niemeyer and J. J. E. Slotine. Stable adaptive teleoperation. IEEE Journal of Oceanic Engineering, 16(1):152–162, 1991.
[5] J. E. Colgate. Robust impedance shaping telemanipulation. IEEE transactions on Robotics and Automation, 9(4):374–384, 1993.
[6] D. A. Lawrence. Stability and transparency in bilateral teleoperation. IEEE Transactions on Robotics and Automation, 9(5):624–637,
1993.
[7] K. Kosuge, T. Itoh, and T. Fukuda. Human-machine cooperative telemanipulation with motion and force scaling using task-oriented
virtual tool dynamics. IEEE Transactions on Robotics and Automation, 16(5):505–516, 2000.
[8] S. Stramigioli, A. van der Schaft, B. Maschke, and C. Melchiorri. Geometric scattering in robotic telemanipulation. IEEE Transactions
on Robotics and Automation, 18(4):588–596, 2002.
[9] D. J. Lee and P. Y. Li. Passive bilateral feedforward control of linear dynamically similar teleoperated manipulators. IEEE Transactions
on Robotics and Automation, 19(3):443–456, 2003.
[10] D. J. Lee and P. Y. Li. Passive coordination control to nonlinear bilateral teleoperated manipulators. In Proceedings of IEEE International
Conf. on Robotics and Automation, volume 3, pages 3278–3283, 2002.
[11] J. Ryu, D. Kwon, and B. Hannaford. Stable teleoperation with time domain passivity control. IEEE Transactions on Robotics and
Automation, 20(2):365–373, 2004.
[12] K. Krishnaswamy and P. Y. Li. Passive teleoperation of a multi degree of freedom hydraulic backhoe using dynamic passive valve.
In ASME International Mechanical Engineering Congress: The Fluid Power and Systems Technology Division (Publication) FPST,
volume 10, pages 149–156, 2003.
[13] M. Vidyasagar. Analysis of nonlinear dynamic systems. Prentice Hall, second edition, 1993.
[14] N. Hogan. Controlling impedance at the man/machine interface. In Proceedings of the 1989 IEEE International Conf. on Robotics and
Automation, pages 1626–1631, 1989.
[15] B. Hannaford. A design framework for teleoperators with kinesthetic feedback. IEEE transactions on Robotics and Automation,
5(4):426–434, 1989.
[16] Y. Yokokohji and T. Yoshikawa. Bilateral control of master-slave manipulators for ideal kinesthetic coupling - formulation and
experiment. IEEE Transactions on Robotics and Automation, 10(5):605–620, 1994.
[17] H. Kazerooni, T. I. Tsay, and K. Hollerbach. A controller design framework for telerobotic systems. IEEE Transactions on Control
Systems Technology, 1(1):50–62, 1993.
[18] H. Kazerooni and C. L. Moore. An approach to telerobotic manipulations. ASME Journal of Dynamic Systems, Measurements, and
Control, 119(3):431–438, 1997.
[19] K. Hashtrudi-Zaad and S. Salcudean. Analysis of control architectures for teleoperation systems with impedance/admittance master
and slave manipulators. International Journal of Robotics Research, 20(6):419–445, 2001.
[20] W. H. Zhu and S. E. Salcudean. Stability guaranteed teleoperation: an adaptive motion/force control approach. IEEE Transactions on
Automatic Control, 45(11):1951–1969, 2000.
[21] W. H. Zhu, Y. G. Xi, Z. J. Zhang, Z. Bien, and J. De Schutter. Virtual decomposition based control for generalized high dimensional
robotic systems with complicated structure. IEEE Transactions on Robotics and Automation, 13(3):411–436, 1997.
[22] A. V. Ephanov and Y. Hurmuzlu. Implementation of sensory feedback and trajectory tracking in active telemanipulation systems. ASME
Journal of Dynamic Systems, Measurements, and Control, 119:447–454, 1997.
31
[23] Dongjun Lee. Passive Decomposition and Control of Interactive Mechanical Systems under Coordination Requirements. Doctoral
Dissertation, University of Minnesota, 2004. available at http://decision.csl.uiuc.edu/∼d-lee/Dissertation.pdf.
[24] P. Y. Li and R. Horowitz. Passive velocity field control: Part 1. geometry and robustness. IEEE Transactions on Automatic Control,
46(9):1346–1359, 2001.
[25] P. Y. Li and R. Horowitz. Passive velocity field control of mechanical manipulators. IEEE Transactions on Robotics and Automation,
15(4):751–763, 1999.
[26] O. Khatib. Real-time obstacle avoidance for manipulators and mobile robots. International Journal of Robotics Research, 5(1):90–98,
1986.
[27] D. E. Koditschek. The control of natural motion in mechanical systems. ASME Journal of Dynamic Systems, Measurements, and
Control, 113:547–551, 1991.
[28] E. Rimon and D. E. Koditschek. Exact robot navigation using artificial potential functions. IEEE Transactions on Robotics and
Automation, 8(5):501–518, 1992.
[29] D. J. Lee and P. Y. Li. Toward robust passivity: a passive control implementation structure for mechanical teleoperators. In Proceedings
of IEEE VR Symposium on Haptic Interfaces for Virtual Reality and Teleoperator Systems, 2003.
[30] O. Khatib, K. Yokoi, K. Chang, D. Ruspini, R. Holmberg, A. Casal, and A. Baader. Force strategies for cooperative tasks in multiple
mobile manipulation systems. In International Symposium of Robotics Research, 1995.
[31] M. P. Do Carmo. Riemannian Geometry. Birkhauser, 1992.
[32] G. Liu and Z. Li. A unified geometric approach to modelling and control of constrained mechanical systems. IEEE Transactions on
Robotics and Automation, 18(4):574–587, 2002.
[33] H. K. Khalil. Nonlinear systems. Prentice-Hall, Upper Saddle River, NJ, second edition, 1995.
[34] P. Y. Li and R. Horowitz. Control of smart exercise machine: Part 1. problem formulation and non-adaptive control. IEEE/ASME
Transactions on Mechatronics, 2(4):237–247, 1997.
[35] B. Hannaford and J. Ryu. Time domain passivity control of haptic interfaces. IEEE Transactions on Robotics and Automation,
18(1):1–10, 2002.