Proceedings of ICCC 2003 IEEE International Conference on Computational Cybernetics Siófok, Hungary, August 29 – 31, 2003
Fractional-Order Position/Force Control of Two Cooperating Manipulators N. M. Fonseca Ferreira
J. A. Tenreiro Machado
Institute of Engineering of Coimbra Coimbra Polytechnic Dept. of Electrical Engineering, Rua Pedro Nunes - Quinta da Nora 3030-199 COIMBRA Portugal
Institute of Engineering of Porto Porto Polytechnic Dept. of Electrical Engineering, Rua Dr. António Bernardino de Almeida 431 4200-072 PORTO Portugal
[email protected]
[email protected]
Abstract – In this paper it is studied the implementation of fractional-order algorithms in the position/force control of two cooperating robotic manipulators. The system performance is analyzed in terms of time and frequency response for different operating conditions.
I. INTRODUCTION Two robots carrying a common object are a logical alternative for the case in which a single robot is not able to handle the load [1 - 2]. Nevertheless, with two cooperative robots the resulting interaction forces have to be accommodated rather than rejected. Consequently, in addition to position feedback, force control is also required to accomplish the task [3 - 4]. There are two basic methods for force control, namely the hybrid position/force and the impedance schemes. The first algorithm was proposed by Raibert [4] and separates the task into two orthogonal subspaces corresponding to the force and the position subspaces. Once established the subspace decomposition two independent controllers are designed. The second algorithm was first proposed by Hogan [1]. In this method, by a proper choice of the arm impedance the interaction forces can be controlled to obtain an adequate response. This paper studies the position/force control of two cooperative manipulators, carrying a common load, using fractional-order (FO) controllers [6 - 8]. In this line of thought the paper is organized as follows. Section two develops a FO algorithm for the position/force control of two robotic arms. Section three presents several experiments for the system performance evaluation. Finally, section four outlines the main conclusions.
II. POSITION FORCE CONTROL OF TWO ARMS When two robots grasp an object (Fig. 1), and move it from one location to another, a coordinated motion is required. In order to get good performances it is necessary to specify no only the desired motion of each robot but also the corresponding handling force. In the system under study the contact of the robot gripper with the load is modeled through a linear system with a mass M, a damping B and a stiffness K.
On the other hand, the dynamics of a robot with n links interacting with the environment is modeled as: && + C(q, q& ) + G(q) − J T (q)F τ = H(q)q
(1)
where τ is the n × 1 vector of actuator torques, q is the n × 1 vector of joint coordinates, H(q) is the n × n inertia matrix, C(q, q& ) is the n × 1 vector of centrifugal/Coriolis terms and G(q) is the n × 1 vector of gravitational effects. The n × m matrix JT(q) is the transpose of the Jacobian matrix of the robot and F is the m × 1 vector of the force that the (m-dimensional) environment exerts in the robot gripper. y Robot 1
(x2,y2) l0
Robot 2
θ0
(x1,y1) x
lb
Fig. 1. Two 2R cooperating robots for the manipulation of an object with length l0 and orientation θ0.
We consider 2R manipulators with dynamics given by: (m1 + m2 )r12 + m2 r2 2 + m r 2 + 2 2 2m2r1r2C2 + J1m + J1g m2r1r2C2 H(q ) = m2r2 2 + m2r2 2 + m2r1r2C2 J 2 m + J 2 g
(2a)
− m 2 r1 r2 S 2 q& 2 2 − 2m 2 r1 r2 S 2 q&1 q& 2 C(q, q& ) = m 2 r1 r2 S 2 q&1 2
(2b)
g (m1 r1C1 + m 2 r1C1 + m 2 r2 C12 ) G (q ) = gm 2 r2 C12 − r S − r S J T (q ) = 1 1 2 12 − r2 S12
r1C11 + r2C12 r2C12
where Cij = cos(qi + qj) and Sij = sin(qi + qj).
(2c) (2d)
Position/Force Trajectory Planning
The numerical values adopted for the 2R robots and the object are m1 = 0.5 kg, m2 = 6.25 kg, r1 = 1.0 m, r2 = 0.8 m, J1m = J2m = 1.0 kgm2, J1m = J2m = 4.0 kgm2 lb = l0 = 1.0 m and θ0 = 0 deg, B1 = B2 = 1 Ns/m and K1 = K2 = 104 N/m. The controller architecture (Fig. 3) is inspired on the impedance and compliance schemes. Therefore, we establish a cascade of force and position algorithms as internal an external feedback loops, respectively, where xd and Fd are the payload desired position coordinates and contact forces.
Fd +
Cp
xd
CF
Position Controller
─
+
+
τ
Force Controller
Kinematics
a 0 z n + ... + a n b0 z n + ... + bn
(3a)
C F (s ) = K F0 s αF ⇒ C F ( z ) ≈ K F
c 0 z n + ... + c n d 0 z n + ... + d n
(3b)
where KP and KF are the position/force loop gains and ai, bi, ci, di, ∈ ℜ. The FO controllers were tuned by trial and error, establishing a compromise between fast transients and large overshoots leading to the parameters KPi = 104, KFi = 104, αPi = ½, αFi = − ½, (i = 1, 2). III. CONTROLLER PERFORMANCES
Robots
─ F
x
C P (s ) = K P0 s αP ⇒ C P (z ) ≈ K P
This section analyses the system performance both in the time and the frequency domains.
Forces Object
Position Kinematics
A) TIME RESPONSE
Velocity
Fig. 2. The position/force controller.
We adopt FO algorithms, both at the position and force control loops, namely of the type C(s)=K0sα with gain K0 and fractional order −1 < α < 1. The corresponding discrete − time approximations C(z) are implemented through the 4th−order Pade equations:
We consider different working situations in order to study the effect of changing the payload mass and the contact surface between the gripper and the object. Therefore, the parameters M, Bi and Ki (i = 1, 2) are varied shown in Table I. In all experiments the controller sampling frequency is fc = 10 kHz for the operating point A ≡ {x,y}≡{1,1} of the object and a contact force of each gripper of {Fxi,Fyi}≡{0.5,5} Nm.
0.15
800
dxd dx
dxd dFx
600 400
0.1
Fx(N)
X(m)
200
0.05
0 -200 -400
0
-600
-0.05
0
0.5
1
-800
1.5
0
0.5
1
1.5
Time (s)
Time (s)
200
0.015 dxd dy
dxd dFy
150
0.01 100 50 Fy(N)
Y(m)
0.005
0
0 -50
-0.005 -100
-0.01
-0.015
-150
0
0.5
1 Time (s)
1.5
-200
0
0.5
1
1.5
Time (s)
Fig. 3. Time response δx(t), δy(t), δFx(t), δFy(t) for a reference position perturbation δxd = 0.1 m and a payload with the parameters of case S2.
0.015
200 dyd dx
dyd dFx
150
0.01 100 0.005
Fx(N)
X(m)
50 0
0 -50
-0.005 -100 -0.01
-0.015
-150
0
0.5
1
-200
1.5
0
0.5
Time (s)
1
1.5
Time (s)
0.15
800
dyd dy
dyd dFy
600
0.1
400
Fy(N)
Y(m)
200
0.05
0 -200
0
-400 -600
-0.05
0
0.5
1
-800
1.5
0
0.5
Time (s)
1
1.5
Time (s)
Fig. 4. Time response δx(t), δy(t), δFx(t), δFy(t) for a reference position perturbation δyd = 0.1 m and a payload the parameters of case S2. -7
5
-3
x 10
5 dFyd dx
3
3
2
2
1
1
0
0
-1
-1
-2
-2
-3
-3
-4
-4
-5
0
0.5
1
dFyd dFx
4
Fx(N)
X(m)
4
x 10
-5
1.5
0
0.5
Time (s)
1
1.5
Time (s)
-5
1
x 10
0.15 dFyd dy
0.8
dFyd dFy
0.6 0.1 0.4
Fy(N)
Y(m)
0.2 0
0.05
-0.2 -0.4 0 -0.6 -0.8 -1
0
0.5
1 Time (s)
1.5
-0.05
0
0.5
1
1.5
Time (s)
Fig. 5. Time response δx(t), δy(t), δFx(t), δFy(t) for a reference force perturbation δFyd = 0.1 N and a payload with the parameters of case S2
-5
x 10
1
0.15 dFxd dx
0.8
dFxd dFx
0.6 0.1 0.4
Fx(N)
X(m)
0.2 0
0.05
-0.2 -0.4 0 -0.6 -0.8 -1
0
0.5
1
1.5
-0.05
0
0.5
Time (s) -6
1
5 dFxd dy
2
0.2
1 Fy(N)
Y(m)
3
0.4
0
-1
-0.4
-2
-0.6
-3
-0.8
-4 1
dFxd dFy
0
-0.2
0.5
x 10
4
0.6
0
1.5
-3
x 10
0.8
-1
1 Time (s)
1.5
Time (s)
-5
0
0.5
1
1.5
Time (s)
Fig. 6. Time response δx(t), δy(t), δFx(t), δFy(t) for a reference force perturbation δFxd = 0.1 N and a payload with the parameters of case S2.
Bearing this fact in mind, we introduce, separately, stepwise perturbations in the position and force references of the robot 1, namely δxd = 0.1 m, δyd = 0.1 m, δFxd = 0.1 N and δFyd = 0.1 N, at t = 1 sec with a duration δt = 1 sec. Figures 3-6 shows the resulting perturbations at the different outputs for the case S2 and Figure 7 shows the time responses for the different load parameters of Table 1. It is clear that positioning errors increase with the contact friction Bi. Furthermore, the larger the contact stiffness Ki the smaller the position overshoot. On the other hand, the force response presents a large peak for high values of Ki/Bi, revealing differential-like behaviors.
|Fx(ω)/Fxd(ω)| and |Fy(ω)/Fyd(ω)|. Furthermore, the charts reveal that we have a limited variation for a dramatic change on the load parameters. Therefore, we conclude that the FO algorithms reveal au adequate stability and a good robustness.
TABLE I – The 2R robot parameters.
V. REFERENCES
Simulation S1 S2 S3 S4 S5 S6 S7 S8
M (kg) 1 1 1 1 2 2 2 2
Bi (Ns/m) 10 10 102 102 10 10 102 102
Ki (N/m) 103 104 103 104 103 104 103 104
B) FREQUENCY RESPONSE Based on the time response to small perturbations at the position and force references we can establish the frequency response, corresponding to linearized transfer functions around the operating point A. Figure 8 show the frequency response of the robot 1 position and force, namely |X(ω)/Xd(ω)|, |Y(ω)/Yd(ω)|,
IV. CONCLUSIONS This paper studied the position/force control of two robots working in cooperation using a fractional-order control algorithm. The dynamic performance of two arms holding an object was analyzed both in the time and the frequency domains. The results revealed that the fractional-order algorithm has a good performance and a high robustness.
[1] N. Hogan, “Impedance control: An Approach to Manipulation, Parts I-Theory, II-Implementation, III-Applications”, ASME J. of Dynamic Systems, Measurement and Control, vol. 107, No. 1, pp. 124, 1985. [2] E. Whitney, “Historical Perspective and State of the Art in Robot Force Control”, IEEE Conf. on Robotics and Automation, St Louis, 1985. [3] B. Siciliano and L. Villani, “Robot Force Control”, Kluwer Academic Publishers, 1999. [4] M. H. Raibert and J. J. Craig, , “Hybrid Position/Force Control of Manipulatores”, ASME J. of Dynamic Systems, Measurement, and Control” vol. 2, No. 2, pp.126-133, vol. 1, 1981. [5] N. M. Fonseca Ferreira and J. A. Tenreiro
Machado, “Manipulation Analysis of Two Cooperating Arms”, 10th IEEE Int. Conf. on Advanced Robotics, Budapest, Hungary, 2001. [6] A. Oustaloup, La Commande CRONE: Commande Robuste d’Ordre Non Entier, Editions Hermes, 1991. [7] J. Tenreiro Machado, “Analysis and Design of
Fractional-Order Digital Control Systems”, J. of Systems Analysis, Modelling and Simulation, vol. 27, pp. 107−122, 1997. [8] I. Podlubny, “Fractional-Order Systems and PIλDµ-Controllers”, IEEE Trans. on Automatic Control, vol. 44, No. 1, pp. 208−213, 1999.
dxd dx (S1) dx (S2) dx (S5) dx (S6)
dxd dx (S3) dx (S4) dx (S7) dx (S8) 0.1
X(m)
X(m)
0.1
0.05
0
0.05
0
0.02
0.04
0.06
0.08
0.1 0.12 Time (s)
0.14
0.16
0.18
0
0.2
0
0.02
0.04
0.06
0.08
0.1 0.12 Time (s)
0.14
0.16
dyd dy (S1) dy (S2) dy (S5) dy (S6)
Y(m)
Y(m)
0.1
0.05
0
0.05
0
0.02
0.04
0.06
0.08
0.1 0.12 Time (s)
0.14
0.16
0.18
0
0.2
0.02
0.016
(S1) (S2) (S5) (S6)
0.04
0.06
0.08
0.1 0.12 Time (s)
0.14
0.16
0.18
dFx dFx dFx dFx dFx
0.04
0.2
(S3) (S4) (S7) (S8)
0.035 0.03 Fx(N)
0.012 0.01
0.025
0.008
0.02
0.006
0.015
0.004
0.01
0.002
0.005 0
0.05
0.1
0.15
0.2
0.25 0.3 Time (s)
0.35
0.4
0.45
0
0.5
0.02
0
0.05
0.1
0.15
0.2
0.25 0.3 Time (s)
0.35
0.4
0.45
0.5
0.05 dFy dFy dFy dFy dFy
0.018 0.016
(S1) (S2) (S5) (S6)
0.045 0.04
0.014
0.035 0.03 Fy(N)
0.012 0.01
0.025
0.008
0.02
0.006
0.015
0.004
0.01
0.002
0.005
0
0.02
0.045
0.014
0
0
0.05 dFx dFx dFx dFx dFx
0.018
Fx(N)
0.2
dyd dy (S3) dy (S4) dy (S7) dy (S8)
0.1
Fy(N)
0.18
0
0.05
0.1
0.15
0.2
0.25 0.3 Time (s)
0.35
0.4
0.45
0.5
0
dFy dFy dFy dFy dFy 0
0.05
0.1
0.15
0.2
0.25 0.3 Time (s)
0.35
0.4
(S3) (S4) (S7) (S8)
0.45
0.5
Fig. 7. Time response δx(t), δy(t), δFx(t) and δFy(t) for reference position perturbations δxd = 0.1 m, δyd = 0.1 m, δFxd = 0.1 N, δFyd = 0.1 N, and a payload with the different parameters of cases S1 to S8.
5 dx dx dx dx
-5
-5
-10
-10
-15 -20
-20 -25
-30
-30
0
10
1
2
10
10
3
-35 -1 10
4
10
10
w(rad/s)
dx dx dx dx
-5
-10
-10
-15 -20
-30
-30
2
10
3
10
dFx dFx dFx dFx
-10
1
2
10
3
10
4
10
10
dFx dFx dFx dFx
-10
(S3) (S4) (S7) (S8)
-20 |Fx(w)/Fxd(w)|
|Fx(w)/Fxd(w)|
0
10
0
(S1) (S2) (S5) (S6)
-20
-30
-40
-30
-40
-50
-50
0
10
1
2
10
10
3
-60 -1 10
4
10
10
w(rad/s)
0
10
1
2
10
10
3
4
10
10
w(rad/s)
0 dFy dFy dFy dFy
-20
0
(S1) (S2) (S5) (S6)
dFy dFy dFy dFy
-20
-40
(S3) (S4) (S7) (S8)
-40
-60
|Fy(w)/Fyd(w)|
|Fy(w)/Fyd(w)|
(S3) (S4) (S7) (S8)
w(rad/s)
0
-80
-100
-60
-80
-100
-120
-140 -1 10
dy dy dy dy
-35 -1 10
4
10
w(rad/s)
-60 -1 10
4
10
-20 -25
1
3
10
-15
-25
10
2
10
0
-5
0
1
10
5
(S3) (S4) (S7) (S8)
|x(w)/xd(w)|
|x(w)/xd(w)|
0
10
0
10
w(rad/s)
5
-35 -1 10
(S3) (S4) (S7) (S8)
-15
-25
-35 -1 10
dx dx dx dx
0
|x(w)/xd(w)|
|x(w)/xd(w)|
0
5
(S1) (S2) (S5) (S6)
-120
0
10
1
2
10
10 w(rad/s)
3
10
4
10
-140 -1 10
0
10
1
2
10
10
3
10
4
10
w(rad/s)
Fig. 8. Frequency response |X(ω)/Xd(ω)|, |Y(ω)/Yd(ω)|, |Fx(ω)/Fxd(ω)| and |Fy(ω)/Fyd(ω)| for reference perturbations δxd = 0.1 m, δyd = 0.1 m, δFxd = 0.1 N, δFyd = 0.1 N and a payload with the different parameters of cases S1 to S8.
