Abstract
The bilateral control of a teleoperator in Cartesian space with time-varying delay is studied in this paper. Compared with the traditional joint-space teleoperation mode, bilateral control in Cartesian space has advantages when dealing with the kinematically dissimilar (KDS) teleoperation systems. A Cartesian space-based PD-like bilateral controller with dissipation factors is designed. Considering the fact that attitude errors derived by rotation matrix cannot be directly used for PD control, a quaternion-based approach is adopted to calculate the attitude errors in Cartesian space. In order to overcome the instability brought about by communication delay, local damping components are employed at both ends of the teleoperator system. The variation of time delay may generate extra energy and influence the stability of the system, thus dissipation factors are introduced into the controller. The stability of the proposed bilateral controller is proved and the simulations show the effectiveness of the approach.
1. Introduction
In a bilateral teleoperation system, the human operator manipulates a haptic interface at local side (the master), then the motion commands are transferred through the communication channel to the remote side, where the telerobot (the slave) is controlled according to the received commands. Meanwhile, the information of motion and contact force at the remote side is gathered and sent to the local side through the communication channel, supplying reflected information to help the operator with accurate identification and decision making. The local and remote sides are often located at different places at long distances and the communication is therefore characterized by substantial time delay, data limitation and information loss. The time delay affects the overall stability of the bilateral teleoperation system, which requires much effort to remedy.
The scattering theory approach and wave variables method were first proposed by Anderson & Spong[1], and Niemeyer & Slotine[2], respectively. Both methods guarantee the stability of the bilateral teleoperators by preserving the passivity of the communication channel, regardless of the span of the time delay. Furthermore, Niemeyer[3] found the wave reflection phenomenon existing in the wave variables approach and pointed out that the reflection could be eliminated by designing appropriate matching impedance for the transmission line. A series of subsequent schemes arose based on the above two approaches. For more advanced surveys, the reader may refer to Hokayem & Spong[4], in which position feedback, impedance matching, robustness to time-varying delays, time-domain passivity approach and various other objectives were introduced.
The classic scattering transformation or wave variables approach would lead to position drift[5][6] and has a trade-off between stability and system performance[7][8]. To compensate the position drift and enhance the performance of the teleoperator, Namerikawa[9] proposed a PD-like controller and Nuno[10] proved the global stability of the controller under the assumption that the human and environment interaction are passive.
For convenience of stability analysis, most of the above schemes assumed that the master and the slave share the same kinematical structure, or in other words, they are kinematically similar (KS)[13], in which joint space-based control is available[11][12]. However, kinematically dissimilar (KDS) teleoperation systems are more universal in application. The kinematical dissimilarity may be caused by different structures or different DOFs. In those systems the master and slave cannot be synchronized through joint angles, thus the bilateral control can only perform in Cartesian space. Wang[14] addressed this problem and pointed out that the difficulty of synchronizing telemanipulators in Cartesian space lies in that the SO(3) used to denote the end-effector attitude is a Lie group other than Euclidean space R3. The additional two elements in SO(3) no longer belong to this space, so it is not convenient to calculate the control torques from rotation matrix errors. Wang proposed an attitude synchronization scheme for bilateral teleoperators. However, the approach was actually for constant time delay, which could be seen from the derivative of the Lyapunov function in [14].
In this paper bilateral teleoperator in Cartesian space is presented. The aim is to synchronize the end-effectors of master and slave under time-varying delay for more general cases. The teleoperator considered in this paper is not limited to KS, but might belong to any type. A PD-like +damping scheme is utilized, scattering transformation is not used in order to enhance the performance of the teleoperator.
2. Problem Formulation
2.1 System dynamics
The master and the slave manipulators are set as a couple of n-degree-of-freedom serial links with revolute joints. Considering the human operator and the environment interactions, the dynamics of the teleoperator system are expressed as[15]:
where qi,q̇i, q̈i∈ ℝ
n
are the joint positions, velocities and accelerations of each manipulator (i = m,s, denoting the master and slave, respectively); Mi(qi) ∈ ℝ
n
are the corresponding inertia matrices; Ci(qi,q̇i) ∈ ℝ
n
are the Coriolis and centrifugal effects; gi(qi) ∈ ℝ
n
are the gravitational forces; τm, τs ∈ ℝ
n
are the joint control torques applied on master and slave manipulators; τh, τe ∈ ℝ
n
are joint torques caused by the human operator and environment interaction. While mapping to the Cartesian space can be expressed as τh = JTm(qm)Fh, τe =JsT(qs)Fe, where Ji(qi) are the Jacobian matrices of the manipulators. Some properties of the model are given as follows[16]:
2.2 Bilateral tracking errors in Cartesian space
The end-effector velocities in Cartesian space are defined as Ẋi = [ẋi ωi], which are related to the joint velocities q̇i through the Jacobian matrices Ji(qi), i.e.,
In the case of 6-DOF manipulator, the positions and attitudes of the end-effector in Cartesian space are defined as xi ∈ ℝ3 and Ri ∈ SO(3) [18]. The end-effector attitude errors between the master and the slave at time t can be expressed as [14][19]
with the time derivative of attitude errors as follows:
Due to the advantage of global singularity-free property preserved by the quaternion, Wang[14] uses quaternion to represent the attitude error deviation. The same approach is adopted in this paper. The quaternion vector function is defined as
3. PD-like controller for time-varying delay teleoperation
In this paper the bilateral teleoperation system affected by time-varying delay both in forward channel and backward channel is investigated. The time delays are defined as Tl(t) and Tr(t), representing the forward time delay (from master to slave) and the backward delay (from slave to master), respectively. In order to obtain a general conclusion, the kinematical models of master and slave are not specified, i.e., the master and slave manipulators may be either KS (Jm= αJs) or KDS (Jm ≠ αJs). However, they should share the same DOFs in Cartesian space, for example 6-DOFs, otherwise the mapping between master and slave would fail. The objective is to ensure the tracking performance of slave end-effector to master end-effector in both position and attitude, and the tracking errors should converge regardless of the time delay. At the same time, the human operator at the master side should feel the force imposed on the slave. The following assumptions are made for further analysis[16][17].
A1 suggests that the power generated by human operating and environment interaction is lower bounded, which is a common property of the actual physical system. A2 is based on the fact that gravitational forces gi(qi) could be completely compensated by control torques τ
i
without influencing the dynamic performance of the control system. A3 could be satisfied in most real teleoperator systems. In addition, A4 limits the degree of time delay variation. Note that in the delay increasing case,
Inspired by the work of [10], [14] and [16], the PD-like bilateral controller is proposed in the form of (10),
where Kd =diag(kdxI kdωI) is a block diagonal matrix, I is a identity matrix, and kdxI, kdωI are both positive; Kp = diag (kpxI kpωI) is also a block diagonal matrix with kpx > 0 and kpω > 0; Dm_damp = diag(dmxI dmωI) > 0 and Ds_damp=diag(dsxI dsωI) > 0 are local damping matrices; ζr and ζl are dissipation factors, defined as ζl2 = 1 – Ṫl(t) and ζr2 = 1 – Ṫr(t). Note that A4 ensures the existence of ζl and ζr.
Before presenting the main proposition of this paper, a Lemma is introduced first, which will be utilized during the upcoming proof.
where λ is an arbitrary positive constant. The proof of Lemma 1 should refer to [16].
where λi(i = 1,2, m,s) are arbitrary positive constants. Then,
I. The bilateral teleoperation system (9) is stable. The position errors and velocities of the end-effectors in Cartesian space are bounded, specifically,
II. In the case of free motion, i.e., when the human operator does not control the master and the slave does not come into contact with any object, then the position and attitude errors of end-effectors in Cartesian space converge to zero as t → ∞.
III. If either the master or the slave is in steady states, i.e., the velocities and accelerations are zeros, then idea transparency would be guaranteed,
I). The following positive Lyapunov-Krasovskii function candidate is chosen,
The properties of quaternion together with A1 imply positive-definiteness of V5, V6, and V7. So (13) is positive, and P1 ensures the radially unboundedness of (11).
Substituting the control law (10) into the teleoperation dynamics (9), the closed-loop system is rewritten as
The time derivative of (13) can be calculated by
Using P2 and closed-loop dynamics (14), gives
Given ζl2 = 1 – Ṫl(t) and ζr2 = 1 – Ṫr(t), V̇3 and V̇4 are calculated as follows:
Decomposing V̇1 and V̇2 as Equation (16) and (17), (15) could be broken up into
Note that given Kd, Kp, Dm_damp and Ds_damp as defined in (10), the sum of (19) and (21) are negative definite. The property of (20) is revealed in the following analysis.
The first term of (20) is set to be Δ1. Integrating Δ1 from 0 to t, yields
Applying Lemma 1, the upper bound of
Using Equation (7) and the definition of quaternion differential[20], Δ2 and Δ3 could be rewritten as[14]
The upper bound of
Set λm1= λm2 = λ
m
, and using the fact that
Similarly, the upper bound of
So the following inequality holds
Combining (13), (19), (20), (21) and (28), yields
where
and
Choose dmx, dsx, dmω, dsω, kpx and kpω according to (32)
such that
x
Λ
m
,
x
Λ
s
,
ω
Λ
m
, and
ω
Λ
s
are all positive definite. Consequently, Lyapunov-Krasovskii function candidate (13) is bounded as 0 < V(t) ≤ V(0). This implies that
The delayed position errors xm(t) – xs(t – Tr) can be rewritten as
where
II). When the human does not control the master and the slave does not come into contact with the environment, Fh = Fe = 0 and the closed-loop dynamics could be rewritten as
The outline of the proof is as follows: the first step is to prove that q̈i and q̇i asymptotically converge to zero as t → ∞, then it is proved that the Kd term and the damping term also convergent to zero, Finally, from (34) the convergence of xs(t – Tr) – xm and q˜(RmTRs(t – Tr)) could be guaranteed.
Rewrite (34), yields
together with P1 and P3, it could be concluded that
Considering the mapping between q̇i and Ẋi as Ẋi = Ji(qi)q̇i, it is not difficult to infer that Ẋi → 0 as t → ∞, which means that DdampẊi → 0. Furthermore, because
III). In the case of the master in steady state, q̈m = q̇m = 0, and Ẋm = Ẋm = 0. Using the conclusion in (I), it can be concluded that Ẋs → 0 as t → ∞. In the case of the slave in steady state it is vice versa, that Ẋm → 0 as t → ∞. Then from the dynamics (14), the closed-loop can be described as
Note that Ẋi = 0 leads to xs(t – Tr) = xs(t), Rs(t – Tr) = Rs(t), xm(t – Tl) = xm(t) and Rm(t – Tl) = Rm(t). Consequently, it is established that
The proof is complete.
4. Simulations
In order to show the effectiveness of the proposed approach, simulations are carried out in this section. The simulations are carried out in the Matlab 7.10/Simulink environment. The variable-step 4th order Runge-Kutta numerical propagator is chosen as the solver. The teleoperator system adopted in the simulation contains a pair of 6-DOF PUMA-like manipulators, one is set as the master and the other the slave. The PUMA robots are modelled using Robotics Toolbox[23] for Matlab (release 9.5). Furthermore, to avoid the coupling of position and attitude during the control, the end-effectors of the manipulators are located at the wrist, i.e., the intersection point of the last three revolution axes. The dynamics of the manipulators are rather complex and hence not listed in this paper. Time delays are introduced between master and slave to simulate the real communication channel. The transport delay components are realized using the Simulink model. In order to simulate the time-varying property, the delay time could be designed as sinusoidal wave signals or triangular wave signals with constant slopes. In this simulation, the triangular wave signals are utilized. The peak values of the waves are all set to be 0.8 and the slopes are set to be ±0.3, which means that the bound of delays Tl and Tr are both 0.8. According to the derivatives of delays, the dissipation factors ζ i are selected to be 0.8.
The simulation is designed to proceed along with following steps: firstly the human operator moves the end-effector of the master towards a desired position, in this stage the slave follows the trajectory of the master without contact with the environment; in the second stage the human keeps on moving the master to the desired position, while at the slave side, the end-effector begins to contact with the environment and the motion of slave is hampered; in the last stage, the human operator feels the force reflected by the slave environment and loosens the master, then the teleoperator system switches into a free motion. The environment contacting with the slave is modelled as a spring-damp system[24] with high stiffness, where the stiffness is K = 500N/ m and damping is D = 20N.s/m.
The positive constants λ1, λ2, λ m and λs are all selected as 1. According to Theorem 1, the control gains are chosen to be dmx = dsx = 1.2, dmω = dsω = 0.5, kpx = 2, kpω = 0.9, kdx = 1 and kdω = 0.4.
The simulation results are shown in Figures 1-7.

Positions of the end-effectors in Cartesian space

Velocities of the end-effectors in Cartesian space

Position tracking errors of slave to master

Velocity tracking errors of slave to master

Forces of human and environment in Cartesian space

Angular velocity errors between master and slave

Quaternions of master and slave end-effectors
From Figure 1 and Figure 3 it can be observed that the first stage lasts from 0s to 5s. At the time t=5s, the slave manipulator contacts with the environment when the end-effector arrives at the point [8 10 −10]. From 5s to 20s, the human operator continues imposing on the master to maintain the end-effector of master at the point [10 15 −12]. From 20s the master is loosened and in a state of free motion.
Figure 1 and Figure 3 show that there exist position errors between the master and the slave when Fh ≠ 0. The errors can be considered as steady state and are bounded, which conforms to part I of Theorem 1. Moreover, it could be observed in Figure 1 and Figure 3 that after the human operator loosens the master, the position errors asymptotically converge to zero. This can be explained by part II of Theorem 1. Figure 2 and Figure 4 are velocities and velocity errors of master and slave, respectively. It is illustrated that the velocity errors converge to zero when Fh ≠ 0, which ensures that the system could be controlled to a steady state, and indicates that the teleoperator system is input-output stable. The boundedness of end-effector velocities claimed in Theorem 1 can also be verified in Figure 2 and Figure 4. Figure 5 demonstrates the force the human is imposing on the master and the contacting force of the environment. Note that there is an oscillation when the slave comes into contact with the environment. The oscillation will be more conspicuous under higher environment stiffness. Figure 5 shows that when in steady state (5s~20s), Fh = –Fe, which means that the human operator could feel exactly the same force applied on the slave caused by contact. This fact is coincident with the description of Theorem 1, part III.
The angular velocity and quaternion of the manipulators are also demonstrated, as shown in Figure 6 and Figure 7. Note that the attitude and position of the end-effectors are already decoupled, so varying of attitude would not affect the location of end-effectors. This could explain why the stabilization of attitude is slower than the position. It is illustrated in Figure 6 and Figure 7 that the angular velocities converge to zero and attitude errors between master and slave can finally be eliminated.
To illustrate the contribution of dissipation factors, the controller designed in [14], which contains no dissipation factors, is chosen to compare with the controller proposed in this paper. Due to the fact that the dissipation factors are designed to deal with the varying of delay, the derivatives of delays in the comparative simulations should be as large as possible, so the value is set to be 0.99. In addition, the bounds of the delays are designed as 2s. The dissipation factors ζ i are selected to be 0.1. Other parameters and the operation schedule remain the same as the former simulation. Figure 8 shows the velocity errors between the master and the slave of the controller[14], and Figure 9 demonstrates that of the proposed approach. It can be obversed from Figure 8 that without dissipation factors, the bilateral controller is sensitive to the varying of time delay. Dramatic vibrations exist in velocity tracking errors, thus the stability cannot be guaranteed during quick operation. Figure 9 shows that velocity tracking performances are enhanced effectively owing to the introduction of dissipation factors.

Velocity tracking errors without dissipation factors

Velocity tracking errors with dissipation factor ζ = 0.1
5. Conclusions
In this paper the bilateral control of a time-varying delay teleoperator in Cartesian space is addressed. A PD-like + damping controller is proposed. The advantage of bilateral control in Cartesian space over joint space is that it can deal with all kinds of telemanipulators, including both KS and KDS types. However, difficulties also exist in applying the bilateral teleoperator in Cartesian space, because the attitude rotation matrix belongs to the unclosed SO(3) space and it is not convenient to calculate the control torques from rotation matrix errors. Thus, a quaternion-based approach is adopted in this paper to synchronize the attitude of master and slave end-effectors. The basic idea of the present approach is PD control. In order to keep the stability of the system under time delay, a local damping term is injected into each side of the teleoperator. Dissipation factors are introduced into the teleoperator in order to dissipate the extra energy generated by variation of time delay. It is proved that when the derivative of time delay satisfies the given condition, the dissipation factors could guarantee the stability of the system effectively. Simulations show the effectiveness of the proposed method. Further research will be conducted into verifying the controller by experiments with real telemanipulators. In addition, how to extend the proposed assumptions to more general conditions is yet to be solved.
Footnotes
6. Acknowledgments
This work is supported by Postdoctoral Science Foundation of China (Grant no. 2011M501342).
