force tracking of dual-arm cooperative manipulators with unknown trajectory deviations

Robotics and Computer Integrated Manufacturing 57 (2019) 357–369

Contents lists available at ScienceDirect

Robotics and Computer Integrated Manufacturing journal homepage: www.elsevier.com/locate/rcim

Full length Article

Symmetrical adaptive variable admittance control for position/force tracking of dual-arm cooperative manipulators with unknown trajectory deviations ⁎,a,b

Duan Jinjuna,b, Gan Yahui a b

T

, Chen Minga,b, Dai Xianzhonga,b

School of Automation, Southeast University, Nanjing 210096, PR China Key Lab of Measurement and Control of Complex Systems of Engineering, Ministry of Education, Nanjing 210096, PR China

A R T I C LE I N FO

A B S T R A C T

Keywords: Dual-arm coordination Symmetrical adaptive variable admittance control Position/force tracking Load distribution Internal and external admittance

In this paper, a new symmetrical adaptive variable admittance control is proposed for position/force tracking of dual-arm cooperative manipulators. Thanks to this control approach, the dual-arm cooperative manipulator is able to track a dynamic desired force and compensate for the unknown trajectory deviations, resulted from in terms of external disturbances and calibration errors. An object-oriented motion planning approach for dual-arm cooperative manipulators is adopted firstly, the motion of the single arm in the system is computed via closedchain constrains. The trajectory deviations from external disturbance and calibration errors are considered, which will lead existing unknown external forces and internal forces to the closed-chain system. In particular, the modeling of closed-chain which including dual-arm and object is analyzed. According to the principle of ”nonsqueezing” pseudoinverse, the forces acting on the center of the object are decomposed into external and internal forces. Furthermore, the external and internal forces are transformed to the tip of the object by load distribution strategy. To track the desired position and forces, a symmetrical adaptive variable admittance control for dual-arm coordination is achieved for the first time by adjusting the admittance parameters on-line based on the tracking error to compensate the unknown trajectory deviations. In the end, the developed control scheme is experimentally tested on a dual-arm setup composed of two ESTUN industrial manipulators carrying a common object. The simulations and experimental results strongly prove that the proposed approach can achieve good position/force tracking performance.

1. Introduction Compared with single-arm robotic systems, dual-arm cooperative manipulators are considered to be predominate in terms of stronger operation ability, a wider range of workspace, more flexible system structure and organization mode [1]. Furthermore, dual-arm cooperative manipulators have both characteristics of industrial robots and service robots. Accordingly, they have better abilities to interact and collaborate with human workers. Therefore, dual-arm cooperative manipulators are often used to carry out a wide class of tasks, such as collaborative assembly, welding and carrying large or heavy payloads, and so on. The traditional pure position control is one of the main control methods in the dual-arm coordination strategy. The dual-arm cooperative manipulators maintain a certain constraint relationship during the process of coordination tasks. The basic idea of the pure

⁎

position control is to plan the trajectory of the object in the task space firstly, and obtain the trajectory of each arm according to the constraints between the object and the arm end-effector. However, the mechanical stresses(internal forces/moments) by dual-arm and the external forces/moments by external disturbance acting on the object are not considered. In order to achieve the task of dual-arm coordination, it is necessary to control simultaneously the trajectory of the object, the mechanical stresses and the external forces/moments on the object. So the position/force tracking for the dual-arm cooperative manipulators has been recognized as a critical issue. In the literature, a number of research on the position/force tracking of dual-arm cooperative manipulators has been done, which in general can be classified into four categorizes. 1) Master/slave control. The basic idea is to define one of them as the master arm, another as the slave arm. There is a certain constraint relationship between the master arm and the slave arm. The master arm is controlled by the position

Corresponding author at: School of Automation, Southeast University, Nanjing 210096, PR China. E-mail addresses: [email protected] (D. Jinjun), [email protected] (G. Yahui), [email protected] (C. Ming), [email protected] (D. Xianzhong).

https://doi.org/10.1016/j.rcim.2018.12.012 Received 2 March 2018; Received in revised form 17 December 2018; Accepted 17 December 2018 0736-5845/ © 2018 Elsevier Ltd. All rights reserved.

Robotics and Computer Integrated Manufacturing 57 (2019) 357–369

D. Jinjun et al.

impedance control for force tracking has been widely studied. However, most designs of commercial robots currently over emphasize the accuracy of position trajectory following without providing a force control mode [18]. Therefore, force-based impedance control is not suggested on these robots. Alternatively, admittance control is recognized as a practical approach to achieve compliant interaction of positioncontrolled manipulators. Therefore, in this paper, we propose a symmetrical adaptive variable admittance control scheme for coordination tasks of dual-arm cooperative systems with unknown and dynamic trajectory deviations. The remaining of this paper is organized as follows. The motion planning of dual-arm cooperative manipulators in the task-space and trajectory deviations are analyzed in Section 2. The model of closedchain and load distribution strategy are introduced in Section 3. The form of the symmetrical adaptive variable admittance control and the control block diagram are given in Section 4. A series of experiments are carried out in Section 5, followed by conclusions in Section 6.

control mode, and the slave arm follows the motion trend of the master arm detected by the force/torque sensor mounted at the wrist of the slave arm. The master/slave force control approach for the coordination of 2 arms carrying a common object cooperatively was proposed in [2], and the necessity of force control for cooperative multiple robots is pointed out. However, the coordinated control based on master/slave control strategy requires fast response of the force following control of the slave arm. Otherwise the system is apparently unstable. 2) Hybrid position/force control. The basic idea is that 2 arms work equally and coordinated by the centralized control. The position control is used in the free space, and the force control is used in the constrained space, such as [3,4]. However, this algorithm needs to be switched between force and position control, without guaranteeing a fully compliant behavior [5]. Furthermore, a difficulty in implementing the hybrid control law in rigid environment is shown in [6]. 3) Impedance/Admittance control. The two main implementations both aim at shaping the dynamical relation between actuator velocity(or position error) and applied external forces [5]. The difference is that the admittance schema uses an inner position loop and an outer force loop, while the impedance schema does the opposite, as shown in [5]. Impedance control achieves the adjustment of the force based on the position error, and admittance control achieves the adjustment of the position based on the force error [5]. Impedance/Admittance control is a stable and effective method widely used in many fields including coordination. The coordination strategy of the object based on impedance control was studied in [7,8]. The coordination is often achieved by considering a virtual stiffness between the 2 arms [9,10]. The force acting on the object was decomposed into the external force that contributes to the object’s motion and the internal force by the end-effector of both arms. Impedance control schemes for cooperative manipulators were applied separately to the control the object/environment interaction forces [11] or internal forces [12]. Following the guidelines in above references, the external impedance and the internal impedance were combined in a unique control framework [13], aiming at controlling both the contact forces due to object/environment interaction which is controlled by external impedance and the internal forces due to the manipulators/ object interaction which is controlled by internal impedance. However, the impedance/admittance parameter used in the above studies is usually constant. Furthermore, the presented control strategies are not concerned with the unknown trajectory deviations which are resulted from external disturbances and calibration errors. 4) Synchronization control. The basic idea is to track the desired trajectory which is generated by the desired force based on the dynamic model of the manipulators. The control problem is formulated in terms of suitably defined errors accounting for the motion synchronization between the manipulators involved in the cooperative task. The concept of motion synchronization was used in [14,15]. And an adaptive control strategy was adopted to track the desired trajectory, ensuring the synchronization position errors converged to zero. In addition, intelligent control strategies were also used in the coordination control of nonlinear cooperative manipulator systems [16,17]. Although the synchronization error at the end effector of the manipulator was considered, the calibration errors of the manipulator itself and the arm base coordinates were not taken into account. In a real dual-arm cooperative system, the time-varying external disturbances and calibration errors will lead to existing unknown and dynamic trajectory deviations, which further introduces great difficulties in position/force coordination control. The optimal control like adaptive control is robust to uncertain systems and dynamically changing systems, the system can be achieved according to adjust the gains by the feedback information [18]. To the best of our knowledge, although adaptive control is used to improve force tracking in [19–22], no research has been reported to combine adaptive control and variable impedance/admittance control [23] to solve the trajectory deviation problem during the dual-arm coordination with the actual industrial robotic systems. In the majority of the literature, force-based

2. Task-space motion planning of cooperative manipulators and analysis of trajectory deviations In this section, the object-oriented motion planning of dual-arm cooperative manipulators used in this paper is discussed. Then the trajectory deviations from external disturbances and calibration errors are analyzed. 2.1. Dual-arm motion planning For the dual-arm coordination tasks, an object-oriented general formulation is proposed, where the user is asked to specify the motion of the system only at the object level, while the motion of the single arms in the system is computed via kinematic transformations between the relevant coordinate frames. The diagram of the coordinate transformations between cooperative manipulators and object are shown in Fig. 1. The relevant coordinate frames are (Fig. 1),

• The world frame of the system, T ; • The reference frame of the object, T ; • The N base frames of the robots, T (i = 1, …, N ); • The N tool frames of the robots, T (i = 1, …, N ), each attached to the w

o

bi

i

end-effector tool of the corresponding robot.

The motion of the object mass with respect to To can be described as o Ro (t ) pm (t ) ⎤ o Tm (t ) = ⎡ m ⎥ ⎢ 0 1 ⎦ ⎣

(1)

o Rm (t )

will denote the (3 × 3) rotation matrix expressing the where orientation of the reference frame Tm with respect to frame To, while o pm (t ) will denote the (3 × 1) position vector expressing the position of the reference frame Tm with respect to frame To . Given the closed-chain constraint conditions between both arms and the object as in [25], we have, w bi i w ⎧ Tbi·Ti (qi)·T m = Tm w o w ⎨ To ·T m (t ) = Tm ⎩

(2)

w is homogeneous transform representing the robot base frame where Tbi Tbi with respect to the world frame Tw . Tibi (qi) is homogeneous transform representing the end-effector frame of robot Ti with respect to its base frame Tbi . T im is homogeneous transform representing the mass w frame of object Tm with respect to the end-effector frame of robot Ti . Tm is homogeneous transform representing the mass frame of object Tm with respect to the world frame Tw . Tow is homogeneous transform representing the object frame To with respect to the world frame Tw . From Eq. (2), the kinematics of the i − th manipulator can be obtained as

358

Robotics and Computer Integrated Manufacturing 57 (2019) 357–369

D. Jinjun et al.

Fig. 1. The diagram of Dual-arm coordinate object. w −1 w Tibi (qi) = (Tbi ) ·To ·T om (t)·(Tim)−1

=

w o m T bi w · To · T m (t )· T i

T bi w,

Tow

The pose error equation between two adjacent joints can be written as Eq. (6). Where Δai, Δαi, Δdi, Δθi and Δβi are the kinematics parameters of i robot joint minor deviations. Substituting Eq. (6) into the forward kinematics of the manipulator, yields Eq. (7). Parameter deviations from mechanical calibration are bound to trajectory deviations, which can be computed as,

(3) o Tm (t )

Tim

and are constant matrix. is computed or spewhere cified according to the task requirement. Then the joint’s motion of the i − th manipulator could be computed by solving the inverse kinematics as, w o m qi = fik (Tibi (qi)) = fik (T bi w · To · T m (t )· T i )

n

(4)

δ T n0 =

where fik( · ) is inverse kinematics solution.

i=1

T ii − 1 = Rot(Zi − 1, θi )·Trans (0, 0, di )·Trans (ai , 0, 0)·Rot (Xi , αi )·Rot (Yi , βi ) ⎡ cθi cβi − sθi sαi sβi −sθi cαi cθi sβi + sθi sαi cβi ai cθi ⎤ ⎢ sθ cβ + cθi sαi sβi cθi cαi sθi sβi − cθi sαi cβi ai sθi ⎥ =⎢ i i ⎥ −cαi sβi sαi cαi cβi di ⎥ ⎢ ⎢ 0 0 0 1 ⎥ ⎦ ⎣ (5)

d T ii − 1 =

∂ai

Δai +

∂T ii − 1 ∂αi

Δαi +

∂T ii − 1 ∂di

Δdi +

∂T ii − 1 ∂θi

Δθi +

∂T ii − 1 ∂βi

Δβi (6)

T n0 + d T n0 = (T10 + d T10)(T12 + d T12)…(Tnn − 1 + d Tnn − 1) n

=

∏ (Tii −1 + d Tii −1) i=1

p˜ bi bj =

1 bi bj bi ˜ bi ˜ bi bj ˜ bi bj [(p1bi − R bj· p1 ) + (p2 − R bj· p2 ) + (p3 − R bj· p3 ) 4 bj ˜ bi + (pbi − R bj· p )] 4

4

i=1

(9)

Although mechanical calibration errors can be compensated, they can’t be totally eliminated. Therefore, the trajectory deviations always exist in each robotic system. (2) The calibration error between base coordinates of both arms Without lose of generality, suppose one of the robot base frames is coincide with the world frame, so calibrations between each base frame and the world frame are simplified as one period of calibration between bi both arm bases. Denote pbi bj and R bj as the position vector and the rotation matrix describing Tbj with respect to Tbi, respectively. By running a series of ”handclasp” manipulators presented in [24], the calibration results may be computed as Eq. (8). Where p˜ bi bj denotes the position vector expressing the position of the base frame Tbj with respect to the ˜ bi base frame Tbi . R bj denotes the rotation matrix expressing the orientation of the base frame Tbj with respect to the base frame Tbi, it can be obtained by [24]. However, as discussed in [24], the calibration results may be degenerated because of noise from three aspects: 1) inaccuracy of mechanism parameters, 2) noise in robot joint position sensors, and 3) inaccuracy in handclasp operation. Normally, it is difficult to avoid these three aspects in the real system, resulting in base coordinate calibration errors, and thereby trajectory deviations. In other words, trajectory deviations always exist in real cases. Denote Tc and Te as the trajectory deviations caused by calibration errors and external disturbances, respectively. From Eq. (8) and Eq. (9),

2.2. Analysis of trajectory deviations

∂T ii − 1

n

∏ (Tii −1 + d Tii −1) − ∏ Tii −1

(7)

(8)

o (t ) is known, the trajectory Ideally, if the trajectory of the object T m of the end-effector Tibi (qi ) can be obtained according to Eq. (3), and then the joint trajectory of each robot can be solved by Eq. (4). However, the external disturbances and calibration errors will lead to trajectory dynamic deviations. Calibration errors include the mechanical calibration error {δTibi } of each arm and the calibration error w {δTbi } between base coordinates of both arms, which can be analyzed as follows. (1) The mechanical calibration error of each single arm According to the Modified Denavit Hartenberg notation, the transform relationship between two adjacent joints can be expressed as Eq. (5). Where ai, αi, di, θi and βi are the i − th joint of robot kinematics parameters, cθi denotes cos θi, sθi denotes sin θi.

⎞ ΔTe = f (h external) . Where f( · ) is the we have ΔTc = f (δ T n0) + f ⎜⎛δ p˜ bi bj⎟ and ⎠ ⎝ trajectory deviations function caused by external disturbances or calibration errors. The total trajectory deviations of each manipulator are ⎞ finally obtained as ΔT = f (δ T n0) + f ⎜⎛δ p˜ bi bj⎟ + f (h external) . ⎝ ⎠ In conclusion, owing to the various calibration errors and the external disturbances, there always exist unknown trajectory deviations in dual-arm cooperative systems. These trajectory deviations are nonlinear with uncertainty, leading to damage to the system and overload of the actuators. 359

Robotics and Computer Integrated Manufacturing 57 (2019) 357–369

D. Jinjun et al.

And

the

loads

being

h = [f1T , n1T , f2T , n2T ]T ,with

imparted

by the arms ⎡ 0 − riz riy ⎤ 0 − rix ⎥ S (ri) = ⎢ riz ⎢− r r 0 ⎥ iy ix ⎦ ⎣

are and

ri = [rix riy riz ]T . If the object motion is known, the forces and moments need to be applied to the center mass of the object by the i − th manipulator can be obtained by Eq. (10). But in practical situation, it is the inverse problem of the Eq. (11), the issue can be summarized as the load distribution problem. 3.2. Load distribution Load distribution of a dual-arm cooperative system is to solve the wrench hi for the i − th manipulator by Eq. (12), while hm is given. But W is row full rank matrix in general. As a result, h has an infinite number of solutions. The general solution to Eq. (12) is given by [7],

Fig. 2. The diagram of the closed-chain system.

3. Model of closed-chain dual-arm system and load distribution

h = W †hm + (I − W †W) ɛ

3.1. Closed-chain modeling Fig. 2 illustrates the diagram of a dual-arm cooperative system holding a rigid object, and thereby forms a closed chain. The following analysis refers to [7],[13] and incorporates some of their ideas. Supposing the reference frame To of the object is affixed to its mass center. The virtual stick is defined as the vector ri (i = 1, 2) which determines the position of To with respect to Ti (i = 1, 2) . It is assumed that ri behaves as a rigid stick fixed to the i − th end-effector, and it meets pm = pi + ri . The motion of the object can be expressed by using the Newton and Euler equations as,

⎧ fm = Mc¨ − Mg ⎨ ⎩ nm = Iω˙ + ω × Iω

(14)

W †W) ɛ

= 0, where the term From Eq. (14), we obtain that W (I − (I − W †W) ɛ does not contribute to the motion of the object. So it can be regarded as the internal forces imparted by the end-effectors. The term W †hm determinates the motion of the object, which can be regarded as the external forces due to the object/environment interaction and/or the external disturbances. According to the analysis in [7], the term W †hm will include the internal forces, if A is chosen reasonably such as,

(10)

⎡ 0 k I3 0 0 ⎤ ⎢ k I3 0 0 0 ⎥ A=⎢ 0 0 0 k I3 ⎥ ⎥ ⎢ ⎣ 0 0 k I3 0 ⎦

2

(15)

where k is any nonzero scalar. Substituting Eq. (15) into W † = AWT (WAWT )−1, we obtain

(11)

where fi and ni are the force and moments applied to the object by the i − th manipulator. According the concept of ”virtual sticks” [4], Eq. (11) can be converted to

hm = Wh

AWT (WAWT )−1,

Wh = WW †hm + W (I − W †W) ɛ = WAWT (WAWT )−1hm + W (I − W †W) ɛ = hm

where M and I are the mass and inertia matrices of the object, c and ω are the position and the angular velocity vector of the object, g is the acceleration of gravity, respectively. As shown in Fig. 3, the wrench acting on the object by the manipulators is,

⎧ fm = ∑i = 1 fi ⎨ nm = ∑2 ni + ∑2 ri × fi i=1 i=1 ⎩

(13)

= A is a positive definite matrix, ε is an where arbitrary vector determined by the possible solution of Eq. (12). Because W is a right pseudoinverse, so it satisfies with WW † = I . Therefore, the following form is obtained W†

W†

(12)

⎡I 1 ⎢−S (r1) = ⎢ 2 I ⎢ S (r ) ⎣ 2

0⎤ I⎥ 0⎥ I⎥ ⎦

(16)

Substituting Eq. (16) into = I. A is appropriate if Furthermore, we know that in Eq. (13) does not result in internal loading of the object. Eq. (13) is then converted to (17)

W †W,

I3 03 I3 03 ⎤ where W = ⎡ stands for the grip matrix. hm denotes ⎢ S (r1) I3 S (r2) I3 ⎥ ⎣ ⎦ f the dual-arm acting on the center mass of the object, with hm = ⎡ m ⎤. ⎢ nm ⎥ ⎣ ⎦

h = W †hm + Vhi

WW †

(17)

where hi represents the internal forces exerted on the center mass of the object by the i − th manipulator, it satisfies WV = 0 . V can be chosen as the following form [26]

0 ⎤ ⎡I ⎢− S (r1) I ⎥ V=⎢ −I 0 ⎥ ⎢ S (r ) − I⎥ ⎣ 2 ⎦

(18)

In general, both of the internal forces and the external forces acting on the center mass of the object are known. By using Eq. (17), the wrench which the end-effector exerts on the object can be obtained, it is the solution of the load distribution.

Fig. 3. Dual-arm cooperating system. 360

Robotics and Computer Integrated Manufacturing 57 (2019) 357–369

D. Jinjun et al.

4. The coordination strategy based on symmetrical adaptive variable admittance control

the diagram of the symmetrical coordination strategy with internal and external admittance controllers. The internal admittance controller is used to track the wrench between end-effector and the object, while the external admittance controller to track the wrench of the object/environment and/or external disturbances. The trajectory of the end-effector and the object are modified by the results of the internal admittance controller and the external admittance controller, respectively. Fig. 5 shows us the scheme of the whole system combing symmetrical coordination and adaptive variable admittance control strategies. In Fig. 5, the system input is the desired trajectory of the object Tmd (t ) . When in the ideal condition (without external force disturbances and trajectory deviations, etc.), we have Tmd (t ) = Tmr (t ) . According to the closed-chain constraints, the trajectory (Tmr1 (t ) and Tmr2 (t ) ) of both manipulators can be computed. Due to the use of the commercial position-controlled industrial manipulators, we assume that there are no trajectory errors, so they may satisfy Tmr1 (t ) = Tr1 (t ) and Tmr 2 (t ) = Tr 2 (t ) . When the system is subjected to the calibration errors and external disturbances, these factors will result in trajectory uncertainties and consequently generate the unpredictable wrench. The wrench can be measured by the force/monment sensor, i.e., hr1 and hr2, respectively. According to Eq. (21), the measured wrench can be decomposed into external wrench (hEr1 and hEr2 ) and internal wrench (hIr1 and hIr2 ). When there exists deviation between the measured external wrench (hEr1 and hEr2 ) and the desired external wrench (hEd ), the deviation trajectory (σm(t)) can be obtained by the adaptive variable external admittance controller, then the modified trajectory (Tmr (t ) ) of the object can be achieved. When there exists deviation between the measured internal wrench (hIr1 and hIr2 ) and the desired internal wrench (hId1 and hId2 ), the deviation trajectory (σ1(t) and σ2(t)) can be obtained by the adaptive variable internal admittance controller, and then the modified trajectory (Tr1 (t ) and Tr2 (t ) ) of the dual-arm can be achieved. The adaptive variable external admittance law shown in Fig. 5 is proposed as Eq. (22). For the internal situation, principles are the same. The adaptive variable internal admittance law shown in Fig. 5 is expressed as Eq. (23). Both of them have the same implementation structure, as shown in Fig. 6.

4.1. Decomposition of internal forces and external forces The wrench required to exert on the object by the manipulator can be computed by Eq. (17). To control the external and internal wrench, Eq. (17) needs to be decomposed as

h = hE + hI

(19)

where hE and hI represent for the external and internal wrench exerted on the object by the manipulator, respectively. Their desired values can be calculated as † ⎧ hE = W hm ⎨ h = Vh i ⎩ I

(20)

In an actual dual-arm cooperative system, the real wrench hr can be measured by force/torque sensors mounted at the wrist of manipulators. Subsequently, we have † ⎧ hEr = W Whr ⎨ h = ( I − W †W) hr Ir ⎩

(21)

where hEr and hIr represent the measured external wrench and internal wrench exerted on the object by the manipulator, respectively. 4.2. Symmetrical adaptive variable admittance control for cooperative manipulators According to the desired motion of the object and the squeeze forces, the desired external wrench hm and internal wrench hi which the end-effector exerts on the center mass of the object can be obtained by Eq. (10). Then the desired external wrench hE and internal wrench hI which the end-effector exerts on the object can be obtained by Eq. (20). On the one hand, the end-effector’s trajectory can be computed by Eq. (3). On the other hand, the joints of a single arm can be obtained by Eq. (4). In summary, both the wrench and the trajectory of the end-effector are constrained. In an actual dual-arm cooperative manipulator system, external disturbances and calibration errors will lead to unknown trajectory deviations as analyzed in Section 3. The unknown trajectory deviations cause dynamic or unknown wrench that affect both external and internal wrench. To track the desired external and internal wrench of single manipulator, a symmetrical adaptive variable admittance control strategy is proposed in this paper as shown in Fig. 4. Fig. 4 illustrates

¨mr (t ) − T ¨md (t )] + [B + ΔB (t )][T˙ mr (t ) − T˙ md (t )] ⎧ M [T ⎪ + K [Tmr (t ) − Tmd (t )] = hEr1 (t ) + hEr 2 (t ) − hEd (t ) ⎪ B ⎨ ΔB (t ) = T˙ mr (t ) − T˙ md (t ) Φ(t ) ⎪ ⎪ Φ(t ) = Φ(t − λ ) + σ hEd (t − λ) − hEr1(t − λ) − hEr 2 (t − λ) B ⎩

Fig. 4. The diagram of Dual-arm coordination strategy. 361

(22)

Robotics and Computer Integrated Manufacturing 57 (2019) 357–369

D. Jinjun et al.

Fig. 5. The diagram of symmetrical adaptive variable internal/external admittance control.

Fig. 6. The adaptive variable admittance control schematic.

Fig. 7. The definition of the coordination system in the linear motion.

¨ri (t ) − T ¨mri (t )] + [BI + ΔBI (t )][T˙ ri (t ) − T˙ mri (t )] ⎧ MI [T ⎪ + KI [Tri (t ) − Tmri (t )] = hIri (t ) − hIdi (t ) ⎪ BI ⎨ ΔBI (t ) = T˙ ri (t ) − T˙ mri (t ) Φ(t ) ⎪ ⎪ Φ(t ) = Φ(t − λ ) + σ hIdi (t ) − hIri (t ) BI ⎩

¨r (t ) = T ¨d (t ) + 1 [hr (t ) − h d (t ) − B (t )(T˙ r (t − 1) − T˙ d (t − 1))] ⎧T M ⎪ ˙ r (t ) = T˙ r (t − 1) + T ¨r (t )*T T ⎨ ⎪ Tr (t ) = Tr (t − 1) + T˙ r (t )*T ⎩

(24)

Where M is the desired inertia matrix, B is the damping matrix, K is the stiffness matrix, λ is the sampling period of the controller and σ is the update rate. When the environment stiffness/location is unknown or

(23)

362

Robotics and Computer Integrated Manufacturing 57 (2019) 357–369

D. Jinjun et al.

Fig. 8. Snapshots of tracking along a circle in Cartesian space.

Fig. 9. The external disturbance forces exerted on the object in the x and y axis.

Fig. 11. The simulation result of the external disturbance forces operate in the x and y axis.

unknown stiffness, it is same to the above situation, so K should be set to zero. Where MI denotes the desired inertia matrix of the internal wrench, BI denotes the damping matrix of the internal wrench, KI is the stiffness matrix of the internal wrench, and i denotes i − th robot. It is the same as the above conclusion, KI is also set to zero. In our previous work [18], the adaptive variable admittance control has been proven stable and convergent and been used to track the dynamic force with unknown trajectory deviations. Considering that the internal admittance and the external admittance control have the same adaptive variable admittance form, so Eq. (22) and Eq. (23) can be further converted to the discrete format as Eq. (24). Where T is the system communication cycle between the robot controller and the servo driver.

Fig. 10. The simulation result of the internal forces.

dynamically changes, the desired force tracking effect can be achieved by setting K = 0 as shown in [18]. For the closed-chain constraints formed by dual-arm, due to the dynamic trajectory deviations and the 363

Robotics and Computer Integrated Manufacturing 57 (2019) 357–369

D. Jinjun et al.

Fig. 12. The definition of the coordination system in the variable pose motion.

Fig. 13. The simulation process of the variable pose motion.

Fig. 14. The simulation result of the external disturbance forces operate in the x and y axis.

Fig. 15. The simulation result of the internal forces.

presented in this section.

5. Simulations and experiments To verify the proposed position/force tracking approach and analysis, a series of simulations and experiments are conducted and 364

Robotics and Computer Integrated Manufacturing 57 (2019) 357–369

D. Jinjun et al.

Fig. 16. Hardware architecture of test-bed.

Fig. 17. The external disturbance and the internal force exert on the object.

where r = 0.25 m, θ = 2π , i denotes the i − th communication cycle. Snapshots of the simulation process of tracking along a circle are shown in Fig. 8. The object is a rigid cylinder which with the length of 0.2 m, the radius of 0.05 m, and the density of 1000kg/m3. The endeffector of the center coincides with the centre axis of the cylinder. Assume that the desired internal forces is [0 0 20 0 0 0]. The external disturbance forces exerted on the object in the x and y axes are the same, as shown in Fig. 9. The effect of internal forces tracking is shown in Fig. 10. The desired trajectory and the actual trajectory are compared in Fig. 11. From Fig. 8 and Fig. 11, we can conclude that when the external disturbances exert on the object, the object shows the compliance. Fig. 10 shows that the desired forces can be tracked, and the tracking error converges to zero. But the traditional constant admittance control can’t track the desired force. (2) Tracking the variable pose motion of the object in cartesian space Same as the above mentioned, to maintain the layout of the robot in the actual physical system and the coordinate system in the simulation, the coordinate system defined is shown in Fig. 12. Set the base frame of 1-th robot coinciding with the world frame. The position vector of the base frame of 2-th robot with respect to the world frame and the reference frame of the object with respect to the world frame are [0 -1.5 0] and [0.7 -0.75 0.75], respectively. The object is composed of two rigid pipes, Ra and Rb, respectively.

5.1. Simulation studies The first simulation is to verify the feasibility of the symmetrical adaptive variable admittance control for dual-arm coordination with our self-developed simulation environment based on Matlab Simulink. Two groups of simulations, manipulating the object to track pure translation and rotation motion, respectively, are organized. External disturbances are taken into account in both two groups of simulation, assuming that there is a certain squeeze on the object. (1) Tracking a circle in cartesian space The coordinate system is defined as shown in Fig. 7. Set the base frame of 1-th robot coinciding to the world frame. The position vector of the base frame of 2-th robot with respect to the world frame and the reference frame of the object with respect to the world frame are [0 -1.5 0] and [0.8 -0.75 0.85], respectively. Assume that the total time of system operation is t = 50 s, the system communication cycle between the robot controller and the servo driver is T= 5 ms. The trajectory of the object can be expressed as

⎧ x = r·cos(θ ·i·t / T ) ⎪ y = r·sin(θ ·i·t / T ) ⎪ z=0 ⎨R = 0 ⎪P = 0 ⎪ ⎩Y = 0

(25) 365

Robotics and Computer Integrated Manufacturing 57 (2019) 357–369

D. Jinjun et al.

f (θ) = cos−1

ra2 − rb2 sin2 θ r

ra 2 + 2 rb sin2 θ a

(27)

Assume that the desired internal forces is [0 0 20 0 0 0], which means that setting the desired internal forces in the z axes direction. The external disturbance forces which are operating on the object in the x and y axis are the same, as shown in Fig. 9. The simulation process of variable pose motion is shown in Fig. 13. The results of the desired trajectory and the actual trajectory are compared as shown in Fig. 14. The effect of internal forces tracking is shown in Fig. 15. Combined with the results of Fig. 13 and Fig. 14, we can conclude that when the external disturbances operate on the object, the object shows the compliance. Fig. 15 shows that the desired forces can be tracked, and the tracking error is converge to zero. But the traditional constant admittance control can’t track the desired force. 5.2. Experimental studies To demonstrate the performance of the proposed algorithm, experiments are conducted using the test-bed as shown in Fig. 16. The test platform consists of self-developed open controller, servo drivers, two ESTUN ER16 industrial manipulators, two Optoforce HEX70-XE-1000N force sensors, an ATI collision sensor, and two pairs of gripper. The self-developed open controller is used for task coordination, position/force control, motion planning, forward/inverse kinematics, 12-axis cycle synchronization interpolation and man-machine interface. The force sensor is mounted at the wrist of each manipulator. Two pneumatic grippers are used to grasp the object. An ATI collision sensor is installed between the end-effector and the gripper of the 1-th robot for protection of the whole system. The servo drivers communicate with the controller through EtherCAT. The force sensor and the controller communicate through UDP. Both communication cycles are set as 5ms. Two force sensors are initialized by gravity compensation. To confirm simulation studies, two experiments were conducted to test the feasibility of the proposed algorithm. (1) Tracking a circle in cartesian space Two industrial manipulators cooperate on a rigid pipe of 1m length. The trajectory of the object is shown in Eq. (25), and the desired internal forces is [0 0 0 0 0 0]. By the above equations (Eq. (11) and Eq. (20)), the desired external forces and the internal forces can be obtained. The actual forces can be measured by the wrist sensor, and the actual external forces and the internal forces can be computed by Eq. (21). During the movement, an external disturbance is exerting on the object. The physical experiment process of tracking a circle is shown in Fig. 19. The external disturbance exerts on the object and the internal force between the dual-arm and the object are shown in Fig. 17. The

Fig. 18. The real trajectory and the desired trajectory of the object in x and y axis.

ra = 0.051m , rb = 0.0445m , The radius is the length is la = 0.12m , lb = 0.08m , respectively. The thickness of the pipe is h = 0.003m , and the density is 1000 kg/m3. The center of the 1-th robot’s end-effector and the center of pipe Ra coincide the center of the 2nd robot’s end-effector and the centerline of pipe Rb is coincide. The trajectory of the object can be expressed as

⎧x = 0 ⎪y = 0 ⎪z = 0 ⎨ R = f (2π·i·t / T ) ⎪P = 0 ⎪ ⎩Y = 2π·i·t / T

(26)

where i denotes the i − th communication cycle, f( · ) is the function of the variable pose. The equation is chosen as

Fig. 19. The physical experiment process of the linear motion. 366

Robotics and Computer Integrated Manufacturing 57 (2019) 357–369

D. Jinjun et al.

Fig. 20. The physical experiment process of the variable pose motion.

Fig. 21. The internal force exert on the object.

damage the object and the manipulators. In the simulations and experiments, the selection method of the design parameters of the control system (M, B, λ and σ) can be referenced in [18]. The following parameters can obtain the good performance in the above simulations and experiments, the inertia coefficient M = diag {1, 1, 1, 0, 0, 0}, MI = diag {1, 1, 1, 0, 0, 0}, the initial BI = B = diag {60, 60, 60, 0, 0, 0}, damping coefficient diag {100, 100, 100, 0, 0, 0}, λ = 0.005s and σ = 0.01.

real trajectory and the desired trajectory of the object are shown in Fig. 18. From Figs. 17 and 18, the external disturbance exerting on the object and the internal force between the dual-arm and the object can be obtained. Under the external disturbance with unknown trajectory deviations, the object shows the compliant behavior. With the internal force operating on the object being controllable, the experiment shows that the deviations does not damage the object or the manipulators. (2) Tracking the variable pose motion of the object in cartesian space Two industrial manipulators cooperate on two rigid pipes as shown above (the pipe’s parameters are same as the simulation). The trajectory of the object is shown in Eq. (26), and the desired internal forces is [0 0 0 0 0 0]. The physical experiment process of tracking variable pose motion is shown in Fig. 20. The result of the internal force exerting on the object is shown in Fig. 21. From Fig. 21, the internal force between the dual-arm and the object can be obtained. The real and desired end-effector trajectory of robot1 and robot2, and the Euclidean trajectory errors are shown in Figs. 22 and 23, respectively. Under the external disturbance with unknown trajectory deviations, the internal force operating on the object being controllable. The experiment shows that the deviations does not

6. Conclusion In the real dual-arm cooperative system, the time-varying external disturbances and calibration errors will lead to trajectory deviations. But those factors have never been analyzed during the dual-arm coordinating a common object, and the traditional constant admittance control is not robust in solving these actual problems. So a new symmetrical adaptive variable admittance control is proposed for position/force tracking with unknown trajectory deviations. The trajectory deviations caused by time-varying external disturbances and calibration errors were analyzed, which further result in unknown external and internal forces to the closed-chain system showing unknown and dynamic characteristics. 367

Robotics and Computer Integrated Manufacturing 57 (2019) 357–369

D. Jinjun et al.

Fig. 22. The real and the desired end-effector trajectory of robot1, robot2 and saddle curve.

variable admittance control for dual-arm coordination was achieved by adjusting admittance parameters on line based on the force tracking error to compensate the unknown trajectory deviations. To verify the feasibility of the proposed algorithm, both simulation and experimental studies were conducted and tested. Test results showed that during the external disturbances with unknown trajectory deviations, the forces operating on the object were controllable based on the proposed scheme, and the deviations did not damage the object and the manipulators. It can be used to the actual dual-arm coordination system, and provide a more effective solution to the cooperative operation. Acknowledgement This work was supported by the National Natural Science Foundation of China under Grant No.61873308,61503076,61175113. Natural Science Foundation of Jiangsu Province under Grant No. BK20150624. Open Project Program of the Key Laboratory of Measurement and Control of Complex Systems of Engineering, Ministry of Education, Southeast University under Grant No. MCCSE2014B02. Supplementary material Supplementary material associated with this article can be found, in the online version, at 10.1016/j.rcim.2018.12.012 Fig. 23. The Euclidean trajectory errors of robot1, robot2 end-effector and saddle curve.

References [1] C. Smith, Y. Karayiannidis, L. Nalpantidis, et al., Dual arm manipulationła survey [j], Robot. Autonom. Syst. 60 (10) (2012) 1340–1353. [2] E. Nakano, Cooperational Control of the Anthropomorphous Manipulator ”MELARM”[C], Proc. of, International Symposium on Industrial Robots, (1974), pp. 251–260. [3] M. Uchiyama, N. Iwasawa, K. Hakomori, Hybrid Position/force Control for Coordination of a Two-arm Robot[c], IEEE International Conference on Robotics and Automation, Proceedings. IEEE, 1987, pp. 1242–1247. [4] M. Uchiyama, P. Dauchez, A Symmetric Hybrid Position/force Control Scheme for

To track the desired external forces and internal forces, the endeffector of the manipulator exerts on the object based on the principle of ”nonsqueezing” pseudoinverse, the forces acting on the center of the object were decomposed into external and internal forces. Furthermore, the external and internal forces were transformed to the tip of the object by load distribution strategy. To track the desired position and forces (the external forces and internal forces), a symmetrical adaptive 368

Robotics and Computer Integrated Manufacturing 57 (2019) 357–369

D. Jinjun et al.

[5]

[6] [7]

[8]

[9]

[10]

[11]

[12]

[13]

[14]

[15]

the Coordination of Two Robots[c], IEEE International Conference on Robotics and Automation,1988, Proceedings. IEEE, 1988, pp. 350–356. Vol.1 A. Calanca, R. Muradore, P. Fiorini, A review of algorithms for compliant control of stiff and fixed-compliance robots[j], IEEE/ASME Trans. Mechatron. 21 (2) (2016) 613–624. K.M. Lynch, F.C. Park, Modern robotics: mechanics, planning, and control, Cambridge Univeristy Press, 2017. 544 I.D. Walker, R.A. Freeman, S.I. Marcus, Analysis of motion and internal loading of objects grasped by multiple cooperating manipulators [j], Int. J. Rob. Res. 10 (4) (1991) 396–409. R.G. Bonitz, T.C. Hsia, Force Decomposition in Cooperating Manipulators Using the Theory of Metric Spaces and Generalized Inverses[c], IEEE International Conference on Robotics and Automation, 1994, Proceedings. IEEE, 1994, pp. 1521–1527. Vol.2. M. Grebenstein, A. Albu-Schaffer, T. Bahls, et al., The DLR Hand Arm System[c], IEEE International Conference on Robotics and Automation, IEEE, 2011, pp. 3175–3182. T. Wimboeck, C. Ott, G. Hirzinger, Passivity-based Object-level Impedance Control for a Multifingered Hand[c], IEEE/RSJ International Conference on Intelligent Robots and Systems, IEEE, 2006, pp. 4621–4627. S.A. Schneider, R.H. Cannon, Object impedance control for cooperative manipulation: theory and experimental results[j], IEEE Transactions on Robotics and Automation 8 (3) (1992) 383–394. R.G. Bonitz, T.C. Hsia, Internal Force-based Impedance Control for Cooperating Manipulators[c], IEEE International Conference on Robotics and Automation, 1993, Proceedings. IEEE, 1996, pp. 944–949. Vol.3 F. Caccavale, P. Chiacchio, A. Marino, et al., Six-DOF impedance control of dualarm cooperative manipulators[j], IEEE/ASME Trans. Mechatron. 13 (5) (2008) 576–586. D. Sun, J.K. Mills, J.K. Mills, Adaptive synchronized control for coordination of multirobot assembly tasks, IEEE Trans. Robot. Autom. 18 (4) (2002) 498–510. [J]. IEEE Transactions on Robotics & Automation, 18(4):498-510 A. Rodriguez-Angeles, H. Nijmeijer, Mutual synchronization of robots via estimated

[16]

[17]

[18]

[19]

[20]

[21]

[22] [23]

[24] [25] [26]

369

state feedback: a cooperative approach [j], Control Syst. Technol. IEEE Trans. 12 (4) (2004) 542–554. K.Y. Lian, C.S. Chiu, P. Liu, Semi-decentralized adaptive fuzzy control for cooperative multirobot systems with h-inf motion/internal force tracking performance[j], IEEE Trans. Syst. Man Cybernet.Part B Cybernet. A Pub. IEEE Syst. Man Cybernet.Soc. 32 (3) (2002) 269. W. Gueaieb, F. Karray, S. Al-Sharhan, A robust hybrid intelligent position/force control scheme for cooperative manipulators [j], IEEE/ASME Trans. Mechatron. 12 (2) (2007) 109–125. J. Duan, Y. Gan, M. Chen, et al., Adaptive variable impedance control for dynamic contact force tracking in uncertain environment[j], Robot. Autonom. Syst. 102 (2018) 54–65. H. Seraji, Adaptive Admittance Control: An Approach to Explicit Force Control in Compliant Motion[c], IEEE International Conference on Robotics and Automation, 1994, Proceedings. IEEE, 2002, pp. 2705–2712. Vol.4 B. Yao, Z. Zhou, L. Wang, et al., Sensorless and adaptive admittance control of industrial robot in physical human-robot interaction[j], Robot. Comput. Integr. Manuf. 51 (2018) 158–168. N. Nikdel, M.A. Badamchizadeh, V. Azimirad, et al., Adaptive backstepping control for an n-degree of freedom robotic manipulator based on combined state augmentation[j], Robot. Comput. Integr. Manuf. 44 (2017) 129–143. A. Calanca, P. Fiorini, Understanding environment-adaptive force control of series elastic actuators[j], IEEE/ASME Trans. Mechatron. PP (99) (2018). 1-1 P.D. Labrecque, C. Gosselin, Variable admittance for pHRI: from intuitive unilateral interaction to optimal bilateral force amplification[j], Robot. Comput. Integr. Manuf. 52 (2018) 1–8. Y. Gan, X. Dai, Base frame calibration for coordinated industrial robots [j], Robot. Autonom. Syst. 59 (7C8) (2011) 563–570. F. Basile, F. Caccavale, P. Chiacchio, et al., Task-oriented motion planning for multiarm robotic systems[j], Robot. Comput. Integr. Manuf. 28 (5) (2012) 569–582. S. Erhart, S. Hirche, Internal force analysis and load distribution for cooperative multi-robot manipulation[j], IEEE Trans. Rob. 31 (5) (2017) 1238–1243.