Position/force control for constrained robotic systems ...

Position/force control for constrained robotic systems: A Lyapunov approach Haifa Mehdi

Olfa Boubaker

[email protected]

[email protected]

National Institute of Applied Sciences and Technology,

National Institute of Applied Sciences and Technology,

University of Carthage, Tunisia

University of Carthage, Tunisia

Abstract—In this paper, new sufficient conditions of asymptotic stability are proposed for position/force control of constrained robotic systems. The new conditions are obtained according to the following steps: 1) Desired dynamics for the contact force are imposed following the impedance control concept, 2) The errors dynamics in the joint and task spaces of the constrained robot system are derived. 3) A relationship between the dynamics of the robot and its energy will be developed. 4) New sufficient conditions of asymptotic stability using a suitable Lyapunov approach are finally conducted. To prove the efficiency of the proposed approach, an example of two degrees of freedom of a robot manipulator is used. The simulation results show the evolution of robot end effector at the constrained motion phase.

[16]. 2) Simultaneously position/force control including hybrid position/force control [17, 18]. 3) Parallel position/force control [19, 20]. When the robot is constrained to the environment it is possible that instable behavior occur. So, to find stability conditions for robotic systems in contact with the environment, many researchers used linearized models [21, 22, 23]. Further analyses are done on the basis of nonlinear models and generally use Lyapunov approaches. However they generally require very hard developments [24] or need decoupling between the position and force control [25]. In this work, we present an improved proof of asymptotic stability of constrained robotic systems based on the Lyapunov method using a relationship between the dynamics of the robot and its energy. The proposed approach is enough straightforward Lyapunov approach without force and position control separation. The paper is organized as follows. In the section 2, the problem of position/ force control of constrained robotic systems will stated under some assumptions. The concept of impedance control will be introduced in section 3. In section 4, a relationship between the dynamics of the constrained robot system and its energy will be developed. New sufficient stability conditions will be proposed in section 5. To prove the efficiency of the proposed approach, simulation results were finally carried out for a robot manipulator of two degrees of freedom.

I.

INTRODUCTION

Position/force control of constrained robotic systems is a very important issue for many practical manipulation tasks like grinding, cutting, drilling, insertion, joining, contour following, debarring, scribing, drawing, sweeping and all assembly related tasks. Various control strategies are proposed during recent years to solve such problems. These works were first introduced by Ferrell and Sheridan [1] and leads to an extensive bibliography. The handbook of Sciciliano and Khatib [2] , the books of Fu, Gonzalez and Lee [3], Siciliano and Villani [4], Canudas de Wit, Siciliano and Bastin [5], and Khalil and Dombre [6], the surveys of Whitney [7], Patarinski and Botev [8], Vukobratovi and Nakamura [9] Volpe and Khosla [10], Zeng and Hemami [11], De Schutter Bruyninckx, Zhu and Spong [12] Chiaverini, Siciliano and Villani [13] and Yoshikawa [14] reveal the wealth, development and maturity of this field. According to the control goal, force/position control algorithms can be categorized into three classes: 1) Force/position control based in desired dynamic relationship between the end-effector position and the contact force including stiffness control that involves a relation between position and applied force [15] and impedance control involving the relation between velocity and applied force

II.

PROBLEM FORMULATION

Consider a constrained robotic system described by the following dynamical model [13]: (1) M (θ )θ&& + H (θ , θ& ) + G (θ ) = U − J T (θ ) F and the following kinematic model: (2) X& = J (θ )θ&

Proceedings of the 2010 International Symposium on Robotics and Intelligent Sensors (IRIS2010) Nagoya University, Nagoya, Japan, March 8-11, 2010. ISBN: 987-4-9905048-0-9, ISSN: 1884-1023

299

where θ ,θ&,θ&& ∈ R n are joint position, velocity and acceleration vectors respectively. M (θ ) ∈ R n is the inertia matrix, H (θ ,θ&) ∈ R n is the vector of centrifugal and Coriolis

forces. G(θ ) ∈ R n is the vector of gravity terms. U ∈ R n is the generalized joint force vector. F ∈ R n is the vector of contact generalized forces exerted by the manipulator on the environment. J (θ ) ∈ R n×n is the Jacobian matrix. X& is cartesian velocity. Position /force problem: Design a control law U ∈ R n satisfying asymptotic stability for the constrained robotic system described by the dynamical model (1) and the kinematic model (2) under a force law F ∈ R n .

Figure 1. Impedance control design.

IV. The last problem will be solved under the following assumptions: Assumption 1: The entire vectors of force, position, and velocity are measured. Assumption 2: All feedback gains, used to solve the control problem are diagonal matrix with equal elements. Assumption 3: The constrained environment of the robotic system is static. III.

IMPEDANCE CONTROL DESIGN

Impedance control design, that will be used to solve the position/force control problem, is based on the following concept: the controller ensures desired impedance dynamics while regulates position in all directions [16]. The desired impedance is defined by: F −F Zd = d (3) Xd − X where X d and Fd are desired cartesian position and desired contact force. It is generally required that the desired impedance verifies: Z d = K d + Bd s + M d s 2 (4)

RELATIONSHIP BETWEEN THE DYNAMICS OF THE CONSTRAINED ROBOTIC SYSTEM AND ITS ENERGY

Let Φ and Y (Φ ) the errors in the joint and task space of the constrained robotic system defined respectively by: Φ = θ − θd (7)

Y (Φ) = X (θ ) − X d

(8)

Consider the constrained robot system described by the dynamic model (1) for the force design (5) and the control law (6). Using the relations (7) and (8) we can write: && + H (Φ, Φ &) M (Φ)Φ + J T (Φ) K1Y (Φ) + J T (Φ) K2Y& (Φ) + J T (Φ) K3Y&&(Φ) = 0

(9)

where: K1 = K p + ( I + K f )K d K 2 = K v + ( I + K f )B d

(10)

K 3 = ( I + K f )M d

Recall that the Lagrange equation of a constrained robotic system is described by [25]: d  ∂T  ∂T ∂P ∂D + + =0  & − & dt  ∂Φ  ∂Φ ∂Φ ∂Φ

(11)

K d , Bd , M d ∈ R n × n are desired stiffness, damping and

& ) is the kinetic energy of the constrained robotic T (Φ, Φ

inertia matrices and s is the Laplace operator. Substituting (4) in (3) gives: (5) Fd − F = Kd ( X d − X ) + Bd ( X& d − X& ) + Md ( X&&d − X&& )

system (1) defined by:

X , X& , X&& ∈ R n are cartesian end effector position, velocity

and acceleration respectively. The block diagram of the entire control system is shown by the Figure 1 [26]. X = f g (θ ) is the direct kinematic model of the constrained robotic system. K p , K v , K f ∈ R n×n are position, velocity

and force gain matrices respectively. Based on Figure1, the control law is then given by: U = J T Kp (Xd − X) + Kv (X& d − X& ) + Kf (Fd − F) + Fd + G (6)

[

]

&)= 1Φ & T M (Φ )Φ & T (Φ , Φ 2

(12)

& ) are potential energy and dissipation function P (Φ ), D (Φ , Φ respectively. We can show that [see appendix A]: ∂P = J T (Φ ) K1Y (Φ) (13) ∂Φ ∂D = J T (Φ ) K 2 Y& (Φ ) + J T (Φ ) K 3Y&&(Φ ) & ∂Φ

&)= H (Φ, Φ

 dΦ i ∂M dt ∂Φ i

∑ 

& Φ  2 

Proceedings of the 2010 International Symposium on Robotics and Intelligent Sensors (IRIS2010) Nagoya University, Nagoya, Japan, March 8-11, 2010. ISBN: 987-4-9905048-0-9, ISSN: 1884-1023

(14) (15)

300

V.

(m L + m1k1 ) cos θ1  G (θ ) = g  1 1  m2 k 2 cos θ 2  

NEW SUFFICIENT STABILITY CONDITIONS

In order to prove the stability of the robotic system (1) constrained to its environment, it is necessary to prove global stability of the error dynamics (9) via the control law (6). Impose then to the system (9) to have a Lyapunov Hamiltonian function defined by [27]: & ) = T ( Φ, Φ & ) + P(Φ) − P(0) V ( Φ, Φ (16) &) The error dynamics (9) are asymptotically stable if V (Φ , Φ satisfies the following conditions [25]: & =0 V (0,0) = 0 if Φ = 0, Φ (17) & ) > 0 if Φ ≠ 0, Φ & ≠0 V (Φ , Φ (18)

& ) < 0 if Φ ≠ 0, Φ & ≠0 V& (Φ, Φ

(19)

Theorem: For desired matrices K d , B d , M d ∈ R n×n and if there exist diagonal

n×n such K p, Kv,K f ∈ R

matrices

that

following conditions :  K p + ( I + K f )K d > 0   K v + ( I + K f )B d > 0   Md = 0

the

 − L1 sin θ 1 J (θ ) =   L1 cos θ 1

− L 2 sin θ 2  L 2 cos θ 2 

m1 , m 2 , I 1 , I 2 , k 1 , k 2 I 1 , I 2 are masses, moments of inertia

and

positions of gravity centers of

links 1 and 2

respectively. In figure 1, we design by:  L1 cos θ1 + L 2 cos θ 2   f g (θ ) =   L1 sin θ1 + L 2 sin θ 2 

The robot parameters are fixed as follows: m1 = 10 Kg ,

m2 = 10 Kg , L1 = 0.5m , L2 = 0.5m , k1 = 0.25 m k 2 = 0.25m , I1 = 2Kg.m2 , I 2 = 2Kg.m2 , g = 9.8N .Kg −1 The initial and desired conditions are chosen as follows: X 0 = [0.7 0.65]T , X d = [0.7071 0.7071]T , Fd = [10 0]T .

(20)

or

K p > 0  K v > 0 K = − I  f

(21)

are satisfied, then the system described by (1) and (2) is asymptotically stable under the force model described by: (22) F = Fd - Kd ( X d − X ) − Bd ( X& d − X& ) − Md ( X&&d − X&& )

and the control law:

[

(

)

]

U = J T K p (X d − X ) + Kv (X& d − X& ) + Fd K f + I − K f F + G

Figure 2. Two degrees of freedom robot

(23)

Proof. See appendix B

VI.

SIMULATION RESULTS

To show the efficiency of the proposed approach, we consider a constrained robot of two degrees of freedom (see Figure 2). The robot manipulator moves on the vertical plane. x and y represent end-effector coordinates in the work space. The dynamic model of this robot is described by the model (1) where:  I + m1k12 + m2 L12 m2 L1k 2 cos(θ1 − θ 2 ) M (θ ) =  1  I 2 + m2 k 22 m2 L1k 2 cos(θ1 − θ 2 )  0 m2 L1k 2 sin(θ1 − θ 2 ) θ&12   H θ ,θ& =    &2  0 − m2 L1k 2 sin(θ1 − θ 2 )  θ 2 

( )

For the two cases of asymptotic stability conditions (20) and (21) given by the proposed theorem we adopt the following numerical values: Case 1:. K d = diag[10 10] , B d = diag [5 5] , M d = diag [0 0]

K p = diag[50 50] , K v = diag [30

Case 2: K d = diag[10 10] , B d = diag [5

K p = diag[50 50] , K v = diag [30

K f = diag [− 1

− 1]

30] , K f = diag[2

2]

5] , M d = diag [5

5]

30] ,

Figures 3 to 5 show simulation results in case 1 whereas Figure 6 to 8 show simulation results in case 2. Figure 9 shows robot evolution in the cartesian space. We can note that desired positions and contact forces are followed with surpassing in the second case whereas control laws are realizable and smooth.

Proceedings of the 2010 International Symposium on Robotics and Intelligent Sensors (IRIS2010) Nagoya University, Nagoya, Japan, March 8-11, 2010. ISBN: 987-4-9905048-0-9, ISSN: 1884-1023

301

Figure 6. End effector position: case 2 Figure 3. End effector position: case 1

Figure 4. Contact force response: case 1

Figure 7. Contact force response: case 2

Figure 5. Control laws: case 1

Figure 8. Control laws: case 2

Proceedings of the 2010 International Symposium on Robotics and Intelligent Sensors (IRIS2010) Nagoya University, Nagoya, Japan, March 8-11, 2010. ISBN: 987-4-9905048-0-9, ISSN: 1884-1023

302

Figure 9. Evolution of the robot in the Cartesian space

VII.

CONCLUSION

In this paper, we have proposed new sufficient conditions of stability for position/force control of constrained robotic systems. By developing the relationship between the dynamics of the robot and its energy and using Lyapunov theory it was shown that under the proposed control scheme the motion of the robot is stable. The proposed approach was finally applied on a two degrees of freedom robot manipulator. The results confirmed the effectiveness and good performance of the proposed approach. REFERENCES [1]

W. R. Ferrell and T.B. Sheridan, “Supervisory control of remote manipulators”, IEEE Spectrum, Vol. 4, pp. 81– 88, 1967. [2] B. Siciliano and O. Khatib, Springer handbook of robotics. Springer Berlin Heidelberg, 2008. [3] K.S. Fu, R.C. Gonzalez and C.S. Lee, Robotics: control, sensing, vision and intelligence, New York: Mc Graw-Hill, 1987. [4] B. Siciliano and L. Villani, Robot force control, Kluwer Academic Publishers, Boston, 1999. [5] C. Canudas de Wit, B. Siciliano and G.Bastin. Theory of robot control, Great Britain: Spring-Verlag London, 1996. [6] W. Khalil and E. Dombre, Modélisation, identification et commande des robots, France: Hermès, 2002. [7] E. Whitney, “Historical perspective and state of art in robot force control,” IEEE International Conference on Robotics and Automation, Vol. 6, no. 1, pp.262 – 268, 1985. [8] S.P Patarinski and R. Botev, “Robot force control: A review”, Mechatronics, Vol. 3, no. 4, pp. 377 – 398, 1993. [9] M.Vukobratovi and Y. Nakamura, “Force and contact control in robotic systems”, IEEE conference on robotics and automation, 1993. [10] R.Volpe and P. Khosla, “Equivalence of second-order impedance control and proportional gain explicit force control”, International Journal of Robotics Research, Vol. 14, no. 6, pp. 574 – 589, 1995. [11] G. Zeng and A. Hemami, “An overview of robot force control”, Robotica, Vol. 15, no. 5, pp.473 – 482, 1997. [12] J. De Schutter, H.Bruyninckx, W. H. Zhu and M. W.Spong, “Force control: A bird’s eye view”, In Siciliano & Valavanis (Eds.), Control problems in robotics and automation. London, UK:Springer, 1998.

[13] S. Chiaverini, B. Siciliano and L. Villani, “A survey of robot interaction control schemes with experimental comparison”, IEEE/ASME Transactions on Mechatronics, Vol. 4, no. 3, pp. 273 – 285, 1999. [14] T. Yoshikawa, “Force control of robot manipulators”, IEEE International Conference on Robotics and Automation, Vol. 1, pp. 220 – 226, 2000. [15] J. K. Salisbury,“Active stiffness control of a manipulator in Cartesian coordinates”, IEEE International Conference on Robotics and Automation, Vol. 1, pp. 95 – 100, 1980. [16] N. Hogan, “Impedance control: An approach to manipulators: Part 1, 2, 3”, ASME Journal of Dynamic Systems”, Measurement and Control, Vol. 107, no. 1, pp. 1 – 24, 1985. [17] J. J. Craig and M. Raibert, “A systematic method of hybrid position/force control of a manipulator”, IEEE Computer Software and Applications Conference, Vol. 1, pp. 446 – 451, 1979. [18] M. Raibert and J. J. Craig. Hybrid position/force control of manipulators. ASME J. of Dyn. Systems, Measurement and Control, 120(2):126–133 1981. [19] S. Chiaverini and L. Sciavicco, “Force/position control of manipulators in task space with dominance in force”, 2nd IFAC Symposium Robot control, Karlsruhe, pp. 137 – 143, 1988. [20] B. Siciliano, L. Villani, "Parallel force and position control of flexible manipulators", IEE Proceedings – Control Theory and Application, 147, 605–612, 2000. [21] S. B. Karunakar and A. A. Goldenberg, “Contact stability in modelbased force control systems of robot manipulators”, IEEE International Symposium on Intelligent Control, pp. 412 – 417,1989. [22] D.A. Lawrence, “Impedance control stability properties in common implementation”, IEEE International Conference on Robotics and Automation, Vol. 4, pp.1185 – 1190, 1988. [23] Ch. An and J. Hollerbach, “Kinematic stability issues in force control of manipulators”, IEEE International Conference on Robotics and Automation, Vol. 4, pp.897 – 903, 1987. [24] S. Chiaverini, B.Siciliano and L. Villani, “Force/position regulation of compliant robot manipulators”., IEEE Transactions on Automatic Control, Vol. 39, no. 3, pp. 647 – 652, 1994. [25] T. Yabuta, A.J. Chona and G. Beni, “On the asymptotic stability of the hybrid position/force control scheme for robot manipulators”, IEEE International Conference on Robotics and Automation, Vol. 1, pp. 338 – 343, 1988. [26] N. Hogan, “Impedance control of industrial robots”, Robotics and Computer Integrated Manufacturing, Vol. 1, no. 1, pp. 97 – 113,1984. [27] J.J.E. Slotine and W. Li, Applied Nonlinear Control, Prentice Hall, 1991.

Appendix A: Relationship between the dynamics of the robot and its energy Knowing that the dynamic model, the Lagrange equation and the kinetic energy of the constrained robotic system are described by the expressions (9), (11) and (12) respectively and tacking on account that M (Φ) is a symmetric matrix we can write that: ∂T & = M (Φ)Φ (A.1) & ∂Φ n d  ∂T  && + ∑  dΦ i ∂M (Φ ) Φ &  &  = M (Φ )Φ (A.2)  dt  ∂Φ  ∂Φ i  i =1  dt

Proceedings of the 2010 International Symposium on Robotics and Intelligent Sensors (IRIS2010) Nagoya University, Nagoya, Japan, March 8-11, 2010. ISBN: 987-4-9905048-0-9, ISSN: 1884-1023

303

and

where ∂T 1 = ∂Φ 2

T

n

& T  ∂M (Φ ) Φ & ei Φ  ∂Φ  i   i =1

∑

(A.3)

where n is the number of freedom of the robot and ei ∈ R n is a unit vector. Using the relation (A.2) and (A.3) we have: d  ∂T  ∂T 1 n  dΦ i ∂M (Φ )  & (A.4) Φ = ∑   & −  dt  ∂Φ ∂ Φ dt 2 ∂ Φ  i =1  i  Using (A.4) and comparing (9) to (11) we have (15) and we can write: && + H (Φ, Φ & ) = M (Φ)Φ && + M (Φ)Φ

1 2

n

 dΦi ∂M (Φ)  & Φ dt ∂Φi 

∑ i =1

(A.5)

dD is equivalent to J T (Φ) K 2Y& (Φ) + J T (Φ) K 3Y&&(Φ) & dΦ dP in the dynamic equation so, the term of Lagrange’s dΦ

 ∂J  wij =  i  K1Y (Φ )  ∂Φ i  At the equilibrium point we have:

[W ]Φ =0 = J T (0)K1J (0)

Thus equation (A.10) shows that W is defined positive in the equilibrium point if K 1 is positive definite. Hence the second Lyapunov condition (18) is satisfied if K 1 is positive definite. Using the expression (16) we have:

dV dT dP = + dt dt dt From equation (12) we can write: & dT & T && & T dM Φ = Φ MΦ + Φ dt dt 2 & T MΦ && + H (Φ, Φ &) =Φ

Since

equation is the same as the term J T (Φ) K1Y (Φ) of the dynamic equation.

the expression (12) we have T (0,0) = 0 then V (0,0) = 0 .Condition (17) is then verified. To verify condition (18), it is sufficient to verify the positive definiteness of the following potential function since the & ) is positive definite. Let: kinetic energy T (Φ , Φ From

(A.6)

V p (Φ ) is positive definite if it is a convex function [25].

From (A.6) we have V p (0 ) = 0 From (A.6) and (13) we can write:  ∂V p (Φ )   ∂P (Φ )  = = J T (Φ ) K1Y (Φ )    ∂ Φ ∂ Φ  Φ =0   Φ =0 

 ∂V p (Φ )  = J T (0) K1Y (0) = 0 (A.7)    ∂Φ  Φ = 0 In addition to equation (A.7), the matrix W (shown below) has to be positive definite so that V p (Φ ) is convex. ∂

(

)

J T (Φ ) K1Y (Φ ) = wij + J T (Φ ) K1 J (Φ ) ∂Φ T Applying equation (13) in (A.8) we have: ∂ W = J T (Φ ) K1Y (Φ ) = wij + J T (Φ) K1 J (Φ ) ∂Φ T

(

)

(A.12)

and dP & T ∂P =Φ dt ∂Φ dP & T JTK Y =Φ 1 dt

(A.13)

(A.8)

(A.9)

(A.14)

Substituting (A.12) and (A.14) in (A.11) we have: dV & T MΦ && + Φ &TH +Φ & T JT K Y =Φ 1 dt

(A.15)

From (9) we can write: && + H + J T K Y = − ( J T K Y& + J T K Y&&) MΦ 1 2 3

(A.16)

Substitution of equation (A.16) in (A.15) gives: dV & T ( J T K Y& + J T K Y&&) = −Φ 2 3 dt By using the relation (2) we obtain:

(A.17)

& ) = J (Φ )Φ & Y& (Φ, Φ

(A.18)

Applying (A.18) in (A.17) gives: dV = −Y&T (K2Y& + K3Y&&) dt

then

W =

(A.11)

Substituting (13) in (A.13) we have:

Appendix B: Proof theorem

V p (Φ ) = P (Φ ) − P (0)

(A.10)

(A.19)

From equation (A.19), it is possible to show that dV is dt

negative if K 2 is positive definite and K 3 is null. From (10),

K 3 is null if: I + K f = 0 or M d = 0 . To guarantee stability of the system described by the models (1) and (2) under the force design (5) and the control law (6) it is necessary that K1 is positive definite (condition (18)), K 2 is positive definite and I + K f = 0 or M d = 0 (condition (19)) so asymptotic stability is guaranteed if conditions (20) and (21) are satisfied.

Proceedings of the 2010 International Symposium on Robotics and Intelligent Sensors (IRIS2010) Nagoya University, Nagoya, Japan, March 8-11, 2010. ISBN: 987-4-9905048-0-9, ISSN: 1884-1023

304