Passive Bilateral Control of Nonlinear Mechanical Teleoperators

Transcription

1 1 Passive Bilateral Control of Nonlinear Mechanical Teleoperators Dongjun Lee Coordinated Science Laboratory, University of Illinois at Urbana-Champaign, 117 Transportation Building, 14 S. Mathews Ave., Urbana, IL 6181 USA FAX: Perry Y. Li Department of Mechanical Engineering, University of Minnesota, 111 Church St. SE, Minneapolis MN USA FAX: 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 n-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 n-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. INTRODUCTION 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 [] 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], [1], [11], [1]. 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 4. Revised and resubmitted: July 4. Some of the results in this paper presented in ICRA and IEEE VR 3, Paper Type: Regular. Corresponding Author: Dongjun Lee Dongjun Lee is with Coordinated Science Laboratory, University of Illinois at Urbana-Champaign, 117 Transportation Building, 14 S. Mathews Ave., Urbana, IL 6181 USA. d-lee@control.csl.uiuc.edu. Perry Y. Li is with Department of Mechanical Engineering, University of Minnesota, 111 Church St. SE, Minneapolis MN

3 3 ρf 1 ρt 1 T F ρ Scaled Human Operator. q 1 ρ Scaled Master Robot. q 1 Controller. q Slave Robot. q Slave Environment Closed Loop Teleoperator Fig. 1. Two port closed-loop teleoperator with a power scaling ρ >, 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 [], an adaptive control scheme is proposed in which the virtual decomposition in [1] 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 [], a variable structure control is proposed. However, because coupling stability is not considered, some unstable behaviors are reported in [] 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 [3] that decomposes the nonlinear dynamics of the n-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 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, [4], [5] 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 [6], [7], [8] 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], [9] 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 n-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. PROBLEM FORMULATION A. Dynamics of Mechanical Teleoperators and Power Scaling Consider a n-dof teleoperator consisting of two n-dof nonlinear mechanical systems: ρ (M 1 (q 1 q 1 + C 1 (q 1, q 1 q 1 = T 1 + F 1, (1 M (q q + C (q, q q = T + F. ( where q i, T i, F i R n are the configurations, the control commands from actuators, and the human/environmental forces, M i (q i R n n are the symmetric and positive-definite inertia matrices and C i (q i, q i R n n are the Coriolis matrices s.t. Ṁ i (q i C i (q i, q i are skew-symmetric with i = 1 for master and i = for slave. The scalar ρ > 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-( is said to satisfy the energetic passivity condition if there exists a finite constant d R, s.t. (F 1 (t, F (t, and t, t s ρ ( q 1 (τ, q (τ, F 1 (τ, F (τ dτ = t [ρ F T 1 (τ q 1 (τ + F T (τ q (τ]dτ d. (3 scaled human power environment power where s ρ ( q 1, q, F 1, F := ρf T 1 q 1 + F T q is the scaled environmental supply rate.

5 5 Similarly, we say that the controller in figure 1 with ( q 1, q as input and (T 1, T as output satisfies the controller passivity condition [3], [9] if there exists a finite constant c R s.t. t, t s c ( q 1 (τ, q (τ, T 1 (τ, T (τ dτ = t [ρ T T 1 (τ q 1 (τ + T T (τ q (τ]dτ c, (4 scaled master control power where s c ( q 1, q, T 1, T := ρt T 1 q 1 + T T q is the scaled control supply rate. slave control power 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 [3], [9] For the mechanical teleoperator (1-(, controller passivity condition (4 implies energetic passivity condition (3. Proof: Let us define the scaled kinetic energy κ ρ (t := ρ qt 1 (tm 1 (q 1 q 1 (t + 1 qt (tm (q q (t, (5 then, using the dynamics (1-( with the skew-symmetric property of Ṁ i (q i C i (q i, q i (i = 1,, we have d dt κ ρ(t = ρt T 1 (t q 1 (t + T T (t q (t + ρf T 1 (t q 1 (t + F T (t q (t. (6 Thus, by integrating (6 with the controller passivity condition (4 and the fact that κ ρ (t, we have t [ρf T 1 (τ q 1 (τ + F T (τ q (τ]dτ κ ρ ( c =: d t. 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 n-dof teleoperator system (1-( 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; The motions of the master and the slave systems are perfectly coordinated s.t. q E (q 1 (t, q (t := q 1 (t q (t, F 1 (t, F (t, (7 where q E : R n R n defines the master-slave position coordination error. Once the coordination (7 is achieved, the coordinated teleoperator would have the following dynamics (similar to [3], by summing the master and the slave dynamics in (1-( with the condition that q 1 = q : M L (q q L + C L (q, q q L = T L + F L, (8

6 6 where M L (q := ρm 1 (q 1 + M (q, (9 C L (q, q := ρc 1 (q 1, q 1 + C (q, q, (1 F L := ρf 1 + F, T L := ρt 1 + T, (11 q L R n represents the configuration of the coordinated teleoperator s.t. q L = q 1 = q, F L R n is the total environmental / human forcing on the coordinated teleoperator, and T L R 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 H o := {(q 1, q R n q E (q 1, q = q 1 q = } with q L being a parameterization of H o [31], [3]. In this sense, M L (q and C L (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 n-dof control [T 1, T ] R n 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: η [M L (q q L + C L (q, q q L ] Ψ(q, q = F L = ρf 1 + F, (1 where η > is a user-specific scalar for adjusting the apparent inertia of the coordinated teleoperator that the human operator and the slave environment feel ( η ρ M L(q and ηm L (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 α : R n R n so that coordination requires q 1 α(q 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 ( in terms of q,α := α(q. III. THE PASSIVE DECOMPOSITION In this section, we propose a transformation S(q : R n R n of the tangent space, with which the n-dof nonlinear dynamics of the original teleoperator (1-( 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-( is decomposed into the sum of the kinetic energies of the locked and the shape systems; the locked system has the natural inertia M L (q = ρm 1 (q 1 + M (q 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 R n n s.t.: ( [ ] ( [ ] v L I φ(q φ(q q 1 =, with S 1 I φ(q (q = v E I I q I φ(q I =:S(q, (13 where φ(q := [ρm 1 (q 1 + M (q ] 1 M (q. (14

7 7 In (13, v L R n is the weighted mean velocity of the master / slave robots, and v E R n is the velocity of the coordination error as given by Note that v L = q 1 = q when v E =. v L = [ρm 1 (q + M (q] 1 (ρm 1 (q q 1 + M (q q, (15 v E = d dt q E(q 1 (t, q (t = q 1 (t q (t. (16 With the decomposition matrix S(q in (13, the inertia matrix of the original teleoperator system (1-( is transformed into another block diagonal inertia matrix (i.e. the first design criterion is satisfied: [ ] [ ] S T ρm 1 (q 1 (q S 1 M L (q (q =:, (17 M (q M E (q where M L (q = ρm 1 (q 1 + M (q is the natural inertia of the coordinated teleoperator in (8 and M E (q = ρφ T (qm 1 (q 1 φ(q + [ φ T (q I ] M (q [φ(q I]. (18 It is easy to see that these two matrices M L (q and M E (q are symmetric and positive definite. According to the transform (13, the compatible transform for T i, F i i = 1, are given as: ( ( ( ( ( T L = S T ρt 1 F L (q, and = S T ρf 1 ρf 1 + F (q = T E T F E F φ T (q(ρf 1 + F F. (19 ( ( v Then, using (13-(19 and the relation that L = ( E q + S(q 1, the n-dof nonlinear dynamics v E q of the teleoperator (1-( are transformed into two n-dof partially decoupled systems: M L (q v L + C L (q, qv }{{ L + C } LE (q, q q E = T L + F L, ( locked system dynamics coupling M E (q q E + C E (q, q q }{{ E + C } EL (q, qv L = T E + F E, (1 shape system dynamics coupling where C L (q, q = ρc 1 (q 1, q 1 + C (q, q as given by (1, and C E (q, q = ρφ T (qc 1 (q 1, q 1 φ(q + [ φ T (q I ] C (q, q [φ(q I], ( C LE (q, q = ρc 1 (q 1, q 1 φ(q + C (q, q [φ(q I] + [ρm 1 (q 1 + M (q ] φ(q, (3 C EL (q, q = ρφ T (qc 1 (q 1, q 1 + [ φ T (q I ] C (q, q. (4 The above expressions for C L (q, q, C E (q, q, C LE (q, q, C EL (q, q are derived from the following definition [ ] [ ] C L (q, q C LE (q, q := S T ρm 1 d ( (q S 1 (q [ ] + S T ρc 1 (q S 1 (q, (5 C EL (q, q C E (q, q M dt C where we omit arguments to avoid cluster. We call the n-dof system in ( the locked system, since it represents the dynamics of the teleoperator after being perfectly coordinated (locked. Also, the other n-dof system in (1 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 ( is exactly the desired natural inertia M L (q in (9, and the shape system velocity v E is the time-derivative of the coordination error q E (t (see Eq. (16. Therefore, the coordination error q E is explicitly represented by the shape system configuration. Also, F E in (1 and F L in ( (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 8 respectively. In contrast to the linearly dynamically similar case in [9], the locked and shape system dynamics in (-(1 are only partially decoupled because of the coupling terms C LE (q, q q E and C EL (q, qv L. Proposition The partially decomposed dynamics (-(1 have the following properties: 1 M L (q and M E (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. κ ρ ( q 1, q = 1 vt LM L (qv L + 1 vt EM E (qv E ; (6 κ L (q,v L κ E (q,v E Ṁ L (q C L (q, q and ṀE(q C E (q, q are skew-symmetric; 3 C LE (q, q + C T EL (q, q =. Proof: Item 1 is a direct consequence of (17. In order to prove items and 3, let us define M(q := diag[ρm 1 (q 1, M (q ] and C(q, q := diag[ρc 1 (q 1, q 1, C (q, q ] where q := [q T 1, qt ]T R n. Then, using (17 and (5 with the fact that Ṁ = C + C T (from Ṁ C = [Ṁ C]T, we have [ ] ṀL C L C LE = d ( S T MS 1 S T M d ( S 1 S T CS 1 C EL Ṁ E C E dt dt = d ( S T MS 1 S T M d ( S 1 S T [C C T ]S 1, } dt dt {{} skew symmetric where we omit the arguments to avoid cluster. Thus, Ṁ L (q C L (q, q and ṀE(q C E (q, q are skewsymmetric and C LE (q, q = C T EL (q, q. Following proposition, M L (q, M E (q and C L (q, q, C E (q, q in (-(1 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 T L and T E, the original n-dof nonlinear teleoperator (1-( 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, F 1, F = F T L q L + F T E q E, and s c ( q 1, q, T 1, T = T T L q L + T T E q E. (7 The significance of this property (7 is that the energetic passivity (3 (or controller passivity (4 resp. of the original teleoperator (1-( 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 (7 and item 3 of proposition. Corollary 1 The controller supply rate associated with the decoupling control T d L = C LE(q, q q E, and T d E = C EL (q, qv L is zero, since (T d L T v L + (T d E T q E = vlc T LE (q, q q E + q T EC EL (q, qv L =. Thus, the decoupling of the shape and locked systems in (-(1 does not violate energetic passivity of the closed-loop teleoperator.

9 9 Geometrically, the locked system velocity v L defines the projection of the velocity to the current level set: H c(t := {(p 1, p R n q E (p 1, p = q E (q 1 (t, q (t}, while the shape system velocity v E defines its orthogonal complement w.r.t. the inertia metric (Riemannian metric as shown by the property (17. Furthermore, if v E =, the shape system dynamics and the coupling term C LE (q, q q E vanish, while the locked system dynamics and the coupling term C EL (q, qv L become the constrained dynamics (Levi-Civita connection on H c(t and the second fundamental form, respectively [31], [3]. For more details on geometric property of the decomposition, please refer to [3]. IV. CONTROL LAW DESIGN To achieve the control objectives in section II-B, we design the locked and shape system controls T L, T E in (-(1 (hence, (T 1, T in (1-( via Eq. (19 to be: ( ( ( T L C LE (q, q q E T P L := + T E C EL (q, qv L T P E passive decoupling obstacle avoidance + ( T V L T V E motion guidance + ( T L T E inertia scaling +coordination, (8 where we assume that the position and velocity readings q 1, q, q 1, q are accurate, while the force sensings F 1, F and the identified inertias M 1 (q 1, M (q may possibly be incorrect. A. Coordination Control The control objective for the shape system (1 is q E (t := q 1 (t q (t. Thus, we design the shape system control T E in (8 (with TV E = TP E = temporarily to be: T E := K v q E K p q E F E (t, (9 P D control where K v, K p R n n are constant symmetric and positive-definite damping and spring gain matrices and F E (t is the estimation of the mismatched forcing F E (19 of the shape system (1. Proposition 3 Assume that the inertia matrices M 1 (q 1, M (q, the Coriolis matrices C 1 (q 1, q 1, C (q, q, and the human/environmentlal forces F 1, F in (1-( are bounded. Then, the shape system dynamics in (1 under the total control (8 with the coordination control (9 and T V E = TP E = has (q E(t, q E (t = (, as a global exponentially stable equilibrium if the estimate of the mismatched force F E (t in (9 is correct (i.e. F E (t = F E. If the feedforward cancellation of F E is not used or its estimation error F E := F E ˆF E in (9 is bounded, then (q E, q E is ultimately bounded [33]. Proof: The closed-loop dynamics shape system (1 under the control (9 is given by M E (q q E + C E (q, q q E + K v q E + K p q E = F E Define the Lyapunov function candidate to be: F E feedforward cancellation V (t := 1 qt EM E (q q E + 1 qt E (K p + ɛk v q E + ɛq T EM E (q q E = ( q E q E =: F E. (3 T P(t( q E q E, (31

10 1 where ɛ > is a sufficiently small scalar so that we can find a symmetric and positive-definite matrix P(t R n n and the cross-coupling term ɛq T E M E(q q E is to achieve the exponential convergence. Then, with the closed-loop dynamics (3 and the fact that Ṁ E C E = [ṀE C E ] T (from proposition, we have d dt V (t = qt E [K v ɛm E (q] q E ɛq T EK p q E + ɛ q T EC E (q, qq E + ( q E + ɛq E T FE, (3 where the induced norm of C E (q, q is bounded from the assumption and its definition (. Also, since K p, K p are positive definite, we can always find a small scalar ɛ > and a positive definite matrix Q(t R n n s.t.: ( d (q dt V (t T E q T q E E Q(t + ( q E + ɛq E T FE γ V (t + ( q E + ɛq E T FE, (33 q E for some exponential convergence rate γ > which may be estimated from γ inf x R n, t xt Q(tx x T P(tx >. Thus, from (33, the Lyapunov function (31 satisfies V (t γ V (t + λv 1 (t Fmax, (34 for some λ >, where F max F E (t t. Therefore, if F E (t =, t, (q E (t, q E (t, exponentially, and if F E (t in (9 is bounded, The Lyapunov function V (t (and hence (q E (t, q E (t is ultimately bounded with the bound given by: V := [ ] λ γ F max. (35 A sufficient condition for the bounded M i (q i and C i (q i, q i, i = 1,, is that the configuration space is compact with smooth inertia matrix (e.g. for revolute jointed robots and the operating speed ( q 1, q 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, ϕ are defined on the master and the slave configuration spaces, respectively: ϕ 1 : q 1 ϕ 1 (q 1 R +, ϕ : q ϕ (q R +, (36 so that ϕ 1 (q 1 and ϕ (q are large in the prohibited regions [6]. Let dϕ i (q i R n be their respective smooth and bounded gradient one-forms: [ ϕi dϕ i (q i := qi 1, ϕ i qi,,..., ϕ ] T i qi n R n, i = 1,, (37 where q i = [q 1 i, q i,..., qn i ]T, i = 1,. The potential field control in (8 is defined to be the negative gradient one-form of the potential functions applied on the master and slave robots given by ( [ ] ( T P L (q I I ρ dϕ T 1 T P E (q := 1 φ T (q φ T (q I dϕ T S T (q. (38 As desired, the locked system is affected by the scaled sum of the master and slave potential fields as shown by T P L (q = ρ dϕt 1 (q 1 dϕ T (q in (38. However, the potential fields control (38 also perturbs the shape system

11 11 (i.e. induces coordination error, since T P E (q, Notice that even if the same potential fields are imposed on the master and slave configurations, T P E (q would still be non-zero because the two robots will generally have different inertias. s.t. To combat the adverse effect of the potential fields on the coordination, the coordination control (9 is modified T E := K v q E K p q E F E T P E (q. (39 feedforward cancellation The feedforward cancellation of T P E (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: [ ] λ V = γ D max, (4 where D max F E (t + T P E (q(t, t. The cancellation of TP E (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 : R n R 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 (q L is a tangent vector to the coordination submanifold H o := {(q 1, q q E (q 1, q = q 1 q = } and q L = q 1 = q, 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 H o to the ambient configuration space (R n of the teleoperator (1-(. This is done via the projection map q L : (q 1, q q L with the property that q R n, q L (q, q = q R n. Examples of q L include q L (q := Aq 1 + (I Aq, where q = [q T 1, qt ]T and A R n n is a constant matrix. For a more detailed geometric prescription, please refer to [3]. Then, the control objective will be to make the locked system to follow the lifted velocity field V L q L : R n R n s.t. v L (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 q E (t = q 1 (t q (t, the velocity of the coordinated teleoperator converges to a scaled multiple of the desired velocity field V L on the coordination submanifold H o. Passive Velocity Field Control (PVFC [4], [5] 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 1 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 [4], [5], it is done by augmenting the velocity field by incorporating a fictitious state. Here, we explicitly define the normalized velocity field V L : R n R n s.t. where ξ(q = V L (q := ξ(q V L ( q L (q R n, E V 1 V L ( q L (q T M L (q V L ( q L (q with E V R + being a constant energy level. This ensures that locked system kinetic energy in Eq. (6 associated with the normalized velocity field V L (q is a constant s.t. κ L (q, V L (q = 1 V L(q T M L (qv L (q = E V, q R n. (4 Following [5], we now define PVFC for the locked system (. Let us define the following entities: p(q, q := M L (qv L, P(q := M L (qv L (q, w(q, q := M L (q V L (q + C L (q, qv L (q, (43 then, the PVFC for the guidance objective (41 is given by ( ( T V L G(q, qv L + σr(q, qv L T V E :=, (44 where σ R is a control gain and G(q, q and R(q, q are skew-symmetric matrices defined by G(q, q = 1 {w(q, qp T (q P(qw T (q, q} E, (45 } V {{} skew symmetric R(q, q = {P(qp T (q, q p(q, qp T (q}. (46 skew symmetric Proposition 4 Consider the locked system dynamics in ( under the total control (8 with the PVFC (44 and T P L + T L =. Then, 1 the control supply rate in (7 associated with the PVFC is zero, i.e. s P V F C ( q 1, q, T 1, T = ( T V L T vl + ( T V E T qe = ; if F L (t = t, v L (t βv L (q(t exponentially from almost every initial condition (except the set Proof: of unstable equilibria of v L = βv L (q, which is of measure, where the constant β is given by 1 β = sign(σ vt L M L(q q L 1 V L(q T M L (qv L (q. (47 The supply rate is because G(q, q and R(q, q are skew-symmetric. Convergence proof follows the standard PVFC proof in [5] with the velocity field following error defined to be e β := v L βv L (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 13 D. Inertia Scaling Control The closed-loop dynamics of the locked system ( under the total control (8 with the potential field control (38 and the PVFC (44 is given by M L (q v L + C L (q, qv L = T P L (q + [G(q, q + σ R(q, q] v L +T L + F L, (48 potential field P V F C where F L = ρf 1 + F is the combined human / environments forcing on the locked system, and T L is the inertia scaling control to be designed. Suppose that we achieve the master-slave coordination (7 so that the n-dof shape system (1 vanishes. Then, since v L q 1 q 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 q L := q 1 = q. Thus, comparing the dynamics (48 with the target dynamics (1, we design the inertia scaling control T L to be T L := 1 η η [F L + T P L (q ], (49 where η > is a user-specific inertia scaling factor. The inclusion of the potential field control T P L (q, q is necessary for the locked system flywheel initialization as will be shown in theorem. With the inertia scaling control (49, we have the closed-loop locked system dynamics s.t. η [M L (q v L + C L (q, qv L ] η [G(q, q + σr(q, q] v L T P L (q, q = F L. (5 =:Ψ(q, q where η > is the inertia scaling factor, and the function Ψ(q, q R 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 (1 will also be achieved from (5. A. Passive Control Implementation Structure V. PASSIVE CONTROL IMPLEMENTATION The total control law (8 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: ( [ ] ( ( ( T L G(q, q + σr(q, q C LE (q, q v L T P L = + (q 1 η η [F L + T P L T E C EL (q, q K v q E K p q E + T P E }{{ (q + (q] F E T P E } (q. =:Ω orig : PVFC and D-action Potential field and P-action =:T ff : feedforward (51 In order to see passivity property of the control (51, consider the following candidate storage function s.t. κ c,1 (t = 1 qt EK p q E + ρϕ 1 (q 1 + ϕ (q. (5 Then, differentiating (5 with the definition (38, we have ( T ( d dt κ c,1(t = q T v L T P L EK p q E + ρdϕ 1 (q 1 q 1 + dϕ (q q = (q q E K p q E + T P E (q. (53 Thus, the supply rates associated with the P-action (K p and the potential field control are taken from the storage function κ c,1 (t.

14 Let us multiply both sides of (51 by ( v T L q T E. Then, using the skew-symmetricity of G(q, q, R(q, q in (45-(46, C LE (q, q = C T EL (q, q (from proposition, and the equality (53, we can show that the controller supply rate (7 satisfies s c ( q 1, q, T 1, T = T T Lv L + T T E q E = d dt κ c,1(t q T EK v q E + ( v L T ( 1 η η 14 [F L + T P L (q] F E T P E (q. (54 q E } {{ } =:s ff (t Then, integrating (54 in time and using the fact that κ c,1 (t and K v is positive definite, we have, t s c ( q 1 (τ, q (τ, T 1 (τ, T (τdτ κ c,1 ( + t s ff (τdτ. (55 This inequality shows that, except for the feedforward terms T ff 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 T ff (t in (51 (i.e. t s ff (τdτ in (55 needs to be finite t. In order for this, we incorporate fictitious energy storage elements (with flywheel dynamics [9], [5], [9], [34] into the controller such that the energy required by the feedforward terms T ff (t is always taken from these flywheels. This way, the feedforward actions T ff (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 M f Lẍ f = L T f (locked system flywheel, (56 E M f Eẍ f = E T f (shape system flywheel, (57 where L M f, E M f are the inertias, L x f, L x f are the configurations, and L T f, E T f are the coupling torques (to be designed below. Then, we implement the total control (51 using the following negative semidefinite structure s.t. T L G(q, q + σr(q, q C LE (q, q Π ϕ (t v L T T L E T f = C EL (q, q d (t Σ E (t Π T q E ϕ(t f + K p q E + T P E, (58 E T f } Σ T E(t {{ } Eẋ f } {{ } Ω(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. T ff in in (51, respectively. Also, d (t will be designed to be negative semidefinite. Thus, since C T LE (q, q = C EL (q, q from proposition, 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: Lẋ f Π T ϕ(tv L = L T Lẋ f f = d ( 1 L M Lẋ f f dt (t, (59 Eẋ f Σ T E(t q E = E T Eẋ f f = d ( 1 E M Eẋ f f dt (t. (6 These two equalities (59-(6 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 15 To see that this NSD implementation structure (58 is sufficient to ensure controller passivity condition (4, let us multiply Eq. (58 by (vl T, qt E, L ẋ f, E ẋ f. Then, since Ω(t in (58 is NSD, we have ( T ( T T Lv L + T T E q E + L T Lẋ f f + L T Lẋ v L T P L f f (q q E K p q E + T P E (q = d dt κ c,1(t, (61 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 (7 satisfies s c ( q 1, q, T 1, T d dt κ c,1(t d dt ( 1 L M Lẋ f f (t d dt where κ c (t is defined to be the total controller storage function s.t. ( 1 E M Eẋ f f (t = d dt κ c(t, (6 κ c (t := 1 qt EK p q E + ρϕ 1 (q 1 + ϕ (q + 1 L M Lẋ f f (t + 1 E M Eẋ f f (t. (63 =κ c,1 in (5 Integrating (6 and using the fact that κ c (t, we have t s c ( q 1 (τ, q (τ, T 1 (τ, T (τdτ κ c (, t, (64 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: Π ϕ (t := 1 η η g( L ẋ f [F L + T P L (q ] = g( L ẋ f T L R n, (65 d (t := [1 g( E ẋ f Eẋ f ]K v R n n, (66 Σ E (t := g( E ẋ f K v q E p(t Eẋ (F E + T P E (q R n. (67 f energy recapture feedforward cancellation Here, the threshold function g(x is defined by 1 x x > f o g(x := 1 f o sign(x x f o 1 f o x =, where f o > 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 ẋ f, q E, q E C at time t p(t := otherwise. The design of the switching region C in (69 will be given later in theorem. Comparing the intended control (51 with the implemented control (58 and (65-(69, we make the following observations: (68 (69

16 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 f o. As the locked flywheel depletes energy below the threshold, the implemented inertia scaling control will be gradually deactivated (i.e. L ẋ f f o L ẋ f g( L ẋ f L ẋ f Π ϕ (t. 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. Roughly speaking, p(t = 1 when there is sufficient energy in the flywheel and the error (q E, 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 K v q E in the coordination control (39 will be fully recaptured by the shape system flywheel. When E ẋ f < f o, only a portion is recaptured, and the rest is dissipated through d (t which becomes negative definite. However, when E ẋ f f o, it is fully recaptured with d (t =. Theorem 1 Consider the teleoperator system (1-( 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 F 1, F and the estimates of the inertia parameters M 1 (q 1, M (q are inaccurate; Suppose that the estimations of F 1, F,M 1 (q 1, M (q are accurate. Then, if E ẋ f (t > f o and ( E ẋ f (t, q E (t, q E (t C for every t (i.e. E ẋ f g( E ẋ f = 1 and p(t = 1, (q E (t, q E (t exponentially (i.e. (7 is satisfied. Also, v L (t q 1 (t q (t; 3 Suppose again that the assumptions for item of this theorem are satisfied. Then, if the locked system flywheel Proof: speed is always above the threshold (i.e. L ẋ f (t > f o, t, the target dynamics (1 is achieved. 1 The case when the model inertia parameters are accurate have been demonstrated in section V-A. Denote to be the estimate of based on estimates of the human/environment force and of the model inertial parameters. Then, from (13 and (19, ( ( v L = q Ŝ(q q 1, and E q ( ( ρt 1 TL = T ŜT (q, (7 T E where Ŝ(q is the (inaccurate decomposition matrix S(q. The controls ( T L, T E in (7 are designed for the (incorrect locked and shape systems, and v L, q E are the estimated velocities of the locked and shape systems under the inaccuracy. Notice from (13 that q E is not affected by the model uncertainty, i.e. q E = q E and q E = q E. From (7, we have s c ( q 1, q, T 1, T = ρt T 1 q 1 + T T 1 q 1 = T T L v L + T T E q E, (71 i.e. the controller supply rate (7 is preserved with inaccurate decomposition matrix. Also, the potential field control (38 under the inaccuracy is given by ( ( TP L (q ρ dϕ T T P E (q := Ŝ T 1 (q (q 1 dϕ T (q. (7 Then, the passive control implementation (58 under the inaccuracy is given by T L v L T P L (q T E L T f = Ω(t q E Lẋ f Eẋ + K p q E + T P E (q, (73 f E T f

17 17 V(t slop = γδ v D max switching region C κ (f f o κ f (t Fig.. The switching region C given by the square root of the Lyapunov function V (t (31 and the shape system flywheel energy κ f ( E ẋ f = 1 E M Eẋ f f. where the implementation matrix Ω(t is still NSD regardless of model uncertainty and inaccurate force sensing. The implementation structure (73, the potential field control (7, and the supply rate preservation (71 under the inaccuracy are of the same forms as (58, (38, and (7, respectively. Thus, using the storage function given in (63, the rest of the proof follows the same argument of section V-A; Since g( E ẋ f (t = 1 Eẋ f (t and p(t = 1 t from the assumptions, the implemented control (58 generates the intended coordination control (51. Thus, from proposition 3, (q E (t, q E (t exponentially. Also, from (13, it is easy to show that v L (t q 1 (t q (t as q E (t = q 1 (t q (t ; 3 From the assumption, g( L ẋ f (t = 1 Lẋ f (t, t, thus, the implemented control (58 generates the intended locked system control in (51, therefore the desired locked system dynamics (5 is achieved. Also, since (q 1 (t, q 1 (t (q (t, q (t from item of this theorem, the shape system dynamics (1 vanishes and the target dynamics (1 is achieved from the desired locked system dynamics (5 with v L (t d dt q 1(t d dt q (t d dt v L(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 T ff 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 (1 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 18 Theorem (Flywheel initialization Consider the mechanical teleoperator (1-( under the implemented control (58 and (65-(69. 1 (Locked system flywheel Suppose that the operating speed v L ( is bounded and hence there exists a positive scalar E L > s.t. 1 vt L(tM L (q(tv L (t E L, t. (74 Then, energy will not be depleted in the locked system flywheel (i.e. L ẋ f (t > f initialized the flywheel speed L ẋ f (t so that where f is the threshold in (68. t, if we have 1 L M Lẋ f f ( > 1 L M f fo + 1 η E L, (75 (Shape system flywheel Let us design the switching region C in (69 to be (see figure C := {( E ẋ f, q E, q E 1 E [Eẋ M f f (t f ] o > D max V 1 (t}, (76 γδ v where V (t is the Lyapunov function (31, V is the ultimate bound (4 with D max F E (t+t P E (q t, γ > is the exponential convergence rate (34, and δ v > is defined by V (t = ( q T E q T E P(t ( q E q E δ v q E. (77 Suppose that we initialize the shape system flywheel (57 s.t. 1 E M Eẋ f f ( > 1 E M f f o + D max V 1 (, (78 γδ v i.e. ( E ẋ f (, q E (, q E ( C. Then, ( E ẋ f (t, q E (t, q E (t C t, i.e. the feedforward cancellation is turned on all the time (p(t = 1 t without depleting the flywheel energy ( E ẋ f (t f t from the definition of the region C. Proof: The proof is included in given in the Appendix. Theorem is applied to the case when force sensing and model parameters are accurate. In this ideal case, theorem ensures no flywheel energy depletion, thus, the implemented control (58 duplicates the intended control (51 t 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, safety will be improved. If ( E ẋ f, q E, q E / C in (67 so that p(t =, we have [ d 1 E M Eẋ ] f f = E ẋ f (t g( E ẋ f q T dt EK v q E (79 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 F E + T P E (q so that q E(t, we can eventually drive the system state ( E ẋ f, q E, q E to the switching region C in (76. Hereafter, from theorems 1-, the master-slave coordination is guaranteed without the state leaving the invariant region C, if the estimations of F 1, F,M 1 (q 1, M (q are accurate.