CONTROL OF MECHANICAL SYSTEMS SUBJECT TO UNILATERAL

CONTROL OF MECHANICAL SYSTEMS SUBJECT TO UNILATERAL CONSTRAINTS PRABHAKAR PAGILLA AND MASAYOSHI TOMIZUKA DEPARTMENT OF MECHANICAL ENGINEERING UNIVERSITY OF CALIFORNIA AT BERKELEY BERKELEY, CA 94720

Abstract In this work we consider the problem of control of mechanical systems subject to unilateral constraints. Impulsive forces arise whenever the constraints become active and these forces give rise to nonsmooth dynamics. The dynamics of the system is de ned by a set of dierential equations with discontinuous righthand side using Hamilton's equations of motion. A nonlinear transformation is applied and the dynamics of the system is written in two sets of dierential equations in the transformed coordinates. The rst set of equations has the constraint force terms and the second set has only the free motion variables. Three dierent phases (inactive, transition and active) for the system are formulated depending on the activation/deactivation of the constraints. A discontinuous controller is designed for the three phases. Stability analysis is conducted for all the phases using tools like Filippov's dierential inclusions, nonsmooth Lyapunov analysis and generalized gradients. We give an illustrative example for the theory developed. 1. Introduction In many mechanical systems interacting with an environment, there are moments of time when they experience a change of state abruptly. One common application in industry is a robot following an external surface. In many of these type of applications, rst the robot moves in free space, then makes contact with a surface and follows constrained motion for a speci ed time and leaves the surface. A mathematical model for such systems is a set of dierential equations subject to unilateral constraints. When the robot is in free space then the surface is represented as strict inequality constraints and the constraints are inactive, and when the robot is in contact with the surface the constraints are active and expressed as equality constraints. Whenever the inactive constraints become active, impulsive forces are generated which give rise to nonsmooth dynamics. The impulsive forces on the system have to vanish before contact force on the surface can be controlled. This phase where the inactive constraints become active and there are impulses in the system is called a transition phase. Eective control algorithms have to be designed for each phase of the system. Considerable research has been done in control of robots in constrained motion; see [8, 11] and references therein. Much of this research has been based on the assumption that the robot is already in contact with the external environment. In many of the industrial applications the mechanical system is in free motion before constrained motion starts. The transition from free motion to constrained motion leads to impulsive forces on the system. Stability and control of task transition for robots has been considered in [13] for compliant environment, wherein the transition are assumed to take place smoothly. Impact minimization by using redundant degrees of freedom in the robots has been considered in [14]. Nonsmooth Lyapunov analysis using Filippov's dierential inclusions and generalized gradients is shown in [5] and [6]. Also, a calculus for computing Filippov's dierential inclusion is described in [6]. Research Supported by FANUC Ltd. 1

2

PAGILLA AND TOMIZUKA

The formulation given in this paper covers a broad class of control problems for mechanical systems interacting with environment including impact. We formulate the nonsmooth equations of motion using the Hamiltonian framework. The equations of motion are expressed as rst order dierential equations in generalized coordinates and generalized momentum variables. We transform the generalized momentum variables using a nonlinear transformation. In the tranformed coordinates only the rst set depends on the contact force terms. Three phases of motion are formulated depending on the activation/deactivation of the constraints, these are the inactive, transition and the active phases. A dierent set of dierential equations describes the dynamics of the system in each phase. Discontinuous control laws are proposed for each phase. Stability analysis is conducted for the proposed control laws for dierent phases using tools like Filippov's dierential inclusions, generalized gradients and nonsmooth Lyapunov analysis. We use nonsmooth Lyapunov analysis and dierential inclusions to analyze the discontinous dierential equations obtained for mechanical systems subject to unilateral constraints in the same lines as [5] and [6]. The rest of the paper is organized as follows: In section 2, we give the basic equations of the system, explain an impact model and derive dierential equation for dierent phases of the system. In section 3 some mathematical preliminaries are given and control laws are proposed. Stability of the proposed controllers is also shown. We discuss the practical limitations associated with the problem and the future research that has to be conducted in section 4. Conclusions are given in section 5. 2. Basic Equations By mechanical systems, we mean systems with kinetic and potential energy functions of the form,

K(q; q_) = 21 q_> M (q)q_ and P (q), where q 2 : and the Hamilton's equations of motion for the system are, using these we obtain,

q_ = @@pH p_ = ? @@qH + ; q_ = M ?1 (q)p ?1

p_ = ? 21 (I p>) @M@q p + + g + G>c

(2.3) (2.4)

where g = @@qP , represents the Kronecker product, and In represents n n identity matrix. Notice that @M@q?1 is a n2 n matrix, and (In p>) is a n n2 matrix. Notice that the Hamilton's equations of motion represents 2n rst order dierential equations in q and p as compared to n second order dierential equations in the Lagrangian case. Also, in the presence of the unilateral constraints the system state, q, lies in the region, E := fq 2 c ; G>u ]. The matrix Gu (q) is (n ? m) n and its columns are orthogonal and obtained by completing the basis. Notice that with this construction, G(q) is an orthogonal transformation matrix. Now, using the transformed momenta the equations of motion (2.4) can be written as q_ = M ?1(q)G?1 (q) (2.9) ? 1 ?1 > _ ?1 (2.10) _ = ? 21 G(q)(In > G?> ) @M @q G + GG + G + Gg + GGc

4


The equations (2.10) can also be written in short as _ + N (q; ) = G(q) + G(q)G>c (q) (2.11) Noting that [G(q)G>c (q)]> = [Gc G>c ; Om(n?m) ], where Om(n?m) is a zero matrix of size m (n ? m), the equations of motion (2.11) can be split up into two sets of dierential equations as follows _c + Nc(q; ) = c + Jc> (q)c (2.12) _u + Nu (q; ) = u (2.13) where > = [c> ; u> ], Nc(q; ) 2 c (q). c and u are the rst m and last (n ? m) components of the vector G(q) respectively. The rst set of dierential equations (2.12) represent motion of the mechanical system normal to the surface S , and the second set of dierential equations represent motion in the tangential directions of the surface S . Notice that, the motion tangential to the surface does not involve any constraint force terms, and the motion in the constrained directions involves the constraint force terms when the constraints are activated.

2.2. Impact Model. There is a discontinuous velocity change whenever an inactive constraint

becomes active [1]. The discontinuous velocity change is caused by an impulsive force on the system. The impulsive forces depends on the impact model. We choose a simple rigid body collision to model impact. Consider a collision between two rigid bodies, the relationship between velocities before impact fv1 ; v2 g and after impact fv10 ; v20 g is given by 0 ? v20 ) (2.14) " = ? ((vv1 ? 1 v2 ) where " is a non-negative constant called the coecient of restitution. This equation holds if the volume of contact is small. The value of " depends on the type of collision, " = 1 for perfectly elastic impact and " = 0 for perfectly plastic impact. A smaller value of " means a more loss of mechanical energy due to the collision. If we consider the second particle is stationary before and after impact, we can write (2.14) as 4v1 = ?(1 + ")v1 (2.15) We will use this collision model for the mechanical systems we consider, to compute the velocity changes during impacts, i.e. when the constraints are activated. Generally, impacts are treated as very large forces acting over a short duration of time. If we assume that the impact occurs over an in nitesimally small period of time, then (i) all velocities remain nite and (ii) there is no change in the position of the system. If 4t is the duration of collision and F (!) is impact force during collision, then the force impulse FI due to the impact at time t is given by: Z t +4t FI = 4lim F (!)d!: (2.16) t!0 t Since the integration interval is of zero measure, F (!) must take in nitely large values for FI to be non-zero. So, F (!) must be considered as a Dirac measure at time t with magnitude FI . Expressing F = FI t makes the notion of large forces acting for a short time dissappear, as it allows one to separate the magnitude of impact force FI and its distribution on time axis t . See reference [12] for further details. Now, integrating (2.12) over the interval t to t + 4t, we obtain, (c+ ? c? ) = Jc> (q)I (2.17) where I is the magnitude of the impulsive force and the direction of which is opposite to the direction of _c? . Similar to equation (2.15) we can write 4c = ?(1 + ")c? (2.18)

CONTROL OF MECH. SYSTEMS S.T. UNILATERAL CONSTRAINTS

Combining (2.17) and (2.18),

5

I = ?(1 + ")Jc?> (q)c?

(2.19) Equations (2.18) gives the relationship between the velocity after impact and the velocity before impact, and equation (2.19) is the expression for the magnitude of the impact force acting on the system.

2.3. Mathematical Model for Control. In this section we develop a mathematical model for control considering all the possible scenarios the system (2.2) goes through when subjected to unilateral constraints of the form (2.1). We design the system to go through three phases of motion: (i) inactive phase (ii) transition phase (iii) active phase. The system can be in any one or more of these phases for any given control task. In the inactive phase the system is in free space and the constraints are strict inequality constraints. When the constraints are suddenly introduced then the system is in a transition motion, where in the velocities normal to the surface S are non-zero and impact forces act on the system. So, in this transition phase the goal is to drive the velocities normal to the surface S to zero. In the active phase, motion control is applied for the coordinates, u , which are tangential to the surface S , and contact force control is applied for the normal directions. We also design the impact forces on the system during the transition phase as impulsive disturbances and express the rst set of dierential equations during that phase to be acted upon by these impulses. The equations of motion (2.12) and (2.13) can be written for the dierent phases as Inactive phase:

Transition phase :

Active phase :

q_ = M ?1(q)G?1 (q) _c + Nc (q; ) = c _u + Nu (q; ) = u

(2.20) (2.21) (2.22)

q_ = M ?1(q)G?1 (q) _c + Nc(q; ) + Dc ()((q)) = c _u + Nu (q; ) = u

(2.23) (2.24) (2.25)

q_ = M ?1(q)G?1 (q) (2.26) > Nc (q; u ) = c + Jc c (2.27) _u + Nu (q; u ) = u (2.28) where > (()) = [(1 ()); (2 (); ; (m ())], each term (i ()) means an impulse de ned by the condition i () = 0, Dc() is the matrix of magnitudes of the impulsive forces, and c is the

contact force on the surface. Remarks: Notice that the system has to follow a particular sequence of phases for a given task. For example, the system cannot go from inactive phase to active phase, when the constraints are activated suddenly, i.e. when the velocities normal to the surface S are non-zero. 3. Controller Design

The dynamics of a mechanical system subject to unilateral constraints is nonsmooth and is given by discontinuous dierential equations (2.21) - (2.28). Filippov [2] developed a solution concept for dierential equations with discontinuous right hand side. In [6] a calculus for computing Filippov's dierential inclusion is presented and in [5] Filippov's dierential inclusions and generalized gradients are used to show stability of nonsmooth systems using nonsmooth Lyapunov functions. We use a

6


similar approaches as given in [6] and [5] to prove the stability of the discontinuous dierential equations (2.21) - (2.28) under discontinuous control laws. We give some of the mathematical preliminaries from [2] and [3] that are used in the stability analysis.

3.1. Mathematical Preliminaries. Consider the dierential equations x_ = f (x)

(3.1)

where f : c ; e>u ], > = [c> ; u> ] and = diag(c ; u). We assume that q1d and q2d are twice dierentiable. The matrices c 2 e_ vc The above equation is true 8c 2 @Vc . Substituting equation (3.14) into the above equation, V_c (evc ) = ?kic c> c + c> [ c + Qcc e_ c + Qc1 u e_u + Q_ cc ec + Q_ c1u eu ? _cd]

This equation is true 8c 2 @Vc (evc ), for some c 2 @Vc (evc ), for some c 2 K [Nec]. Choosing c = arg minfk k j 2 @Vc g and from the convexity of the set @Vc , we obtain V_c (evc ) ?kic c> c + c> [ c + Qcc e_ c + Qc1 u e_u + Q_ cc ec + Q_ c1u eu ? _cd] (A.1) Now, using the value of kic as given in theorem (3.1), we obtain V_c ?ic kc k2 + (kc k ? kc k2)kkic + Qcc e_ c + Qc1u e_u + Q_ cc ec + Q_ c1u eu ? _cdk (A.2) The generalized gradient of Vc (evc )Tat 0 is @Vc (0) = [?1; 1]m which is the unit m-cube. Since Vc (evc ) is convex, the set @Vc (evc ) (?1; 1)m is empty 8evc 6= 0. Recall that the value of c is c = arg minfkck : c 2 @Vc (evc )g, which implies kck 1 8evc 6= 0. Therefore we obtain, V_ ?ic kc k2 (A.3)

So we have Vc (evc (t)) absolutely continuous and V_c is negative, therefore from the Lyapunov theorem, evc ! 0. Proceeding along the same lines for the unconstrained directions it can be shown that evu ! 0. Now, from the de nition of errors, we can write vc = (c ? cd ) + Qc (q )c ec + Qc1 (q )u eu ev := eevu (u ? ud ) + Qu (q)u eu + Qc2(q)c ec After reorganizing and recalling the decomposition of G(q)M (q) we obtain ev = (?d )+G(q)M (q)e. Premultiplying ev by M ?1 (q)G?1 (q) we obtain, e_ + e ! 0. Thus, e_ ! 0 and e ! 0.

Appendix B. Proof of Theorem 3.3. Substituting the value of kf given in theorem (3.3), in the derivative of the Lyapunov function (3.20), we get V_f ?f kf k2 + (kf k ? kf k2 )kac k

(B.1) Using, similar arguments as given in appendix I, we can see that, evf ! 0, and from the error equation (3.18), ef is bounded, and hence ef ! 0.


11

Appendix C. Example : Here, we give an illustrative example. The mechanical system under consideration is a two link UCB-NSK planar robot arm shown in Fig. 1. The mass matrix for the robot is

Motor 2 NSK RS 608 Max. Torque 39.2 N-m Max. Speed 1.1 rps Encoder Res. 153,600 cpr

Link 1 Mass 10.6 kg Length 380 mm

Link 2

Motor 2

Link 2 Mass 4.85 kg Length 240mm

Motor 1

Link 1

UCB-NSK

Motor 2 NSK RS 1410 Max. Torque 245 N-m Max. Speed 1.1 rps Encoder Res. 153,600 cpr

Figure 1. UCB-NSK TWO LINK ROBOT

M (q) = aa1 ++2aa3cc2 a2 +a a3 c2 2 32 2 where ci = cos(qi ); cij = cos(qi + qj ). Suppose that the constraint is given by (q) := l1 s1 + l2 s12 ? d 0 Φ( q)

Y

q2

q1 X d

Figure 2. UCB-NSK ROBOT AND THE CONSTRAINT SURFACE

12


The constrained Lagrangian for the system is L = 21 q_> M (q)q_ + c and the generalized momenta are p1 = (a1 + 2a3 c2 )q_1 + (a2 + a3 c2 )q_2 (a2 + a3 c2 )q_1 + a2 q_2 p2 We compute the inverse of the mass matrix before giving the expression for the Hamiltonian, which is M ?1 (q) = 1 ?a a?2 a c ?a a2+?2aa3cc2 2 32 1 32 m 2 2 2 where m = a1 a2 ? a2 ? a3 c2 is the determinant of the mass matrix. The Hamiltonian of the system is H = 21 fa2p21 + (a1 + 2a3c2 )p22 ? 2(a2 + a3 c2 )p1 p2 :g m The Hamilton's equations of motion are a2 p1 ? (a2 + 2a3 c2 )p2 q_1 = 1 q_2 m ?(a2 + a3 c2 )p1 + (a1 + 2a3 c2 )p2 0 p_1 = 1 + ?l1s1 ? l2 s12 + 1 2 c ? 22m (p1 p2 y1 + p2 y2 2 p_2 ?l2s12 where 2 2 y1 = (a1 a2 ? a2 + 2a2 a23 c2 + a3 c2 )a3 s2 ) m 2 2 2 y2 = (a2 ? a1 a2 ? a3c22 ? a1 a3 c2 )2a3 s2 m Now, the matrix G(q) can be constructed as G ( q ) ? l c ? l c ? l c c 1 1 2 12 2 12 G(q) := G (q) = ?l2 c12 l1 c1 + l2 c12 u and its inverse is given by 1 l c + l c l c 1 1 2 12 2 12 ? 1 G (q) = l2 c12 ?l1c1 ? l2 c12 g where g = ?(l1 c1 + l2 c12 )2 ? l22 c212 is the determinant of the matrix G(q). Now consider the transformation of p by G(q), = G(q)p; the equations of motion in the transformed coordinates are q_ = M ?1(q)G?1 (q) ?1 _ ?1 (q) + G(q) + G(q)G>c (q)c : _ = ? 12 G(q)(In > G?> (q)) @M@q G?1 (q) + GG We compute the dierent matrices involved and express in terms of c and u . ?1 0 @M z y 2 1 > ?> (In G ) @q = 0 z y + z y 1 1 2 2 where z1 = (l1 c1 + l2 c12 )c + l2 c12 u , and z2 = l2 c12 c ? (l1 c1 + l2 c12 )u and ?1 @M 1 l c w ? ( l c + l c ) w 2 12 1 1 1 2 12 1 > ?> ? 1 G(In G ) @q G = l c w ?(l c + l c )w 1 1 2 12 2 g 2 12 2


13

where w1 = ?z2y1 (l1 c1 + l2c12 ) ? l2 c12 (z1 y1 + z2y2 ), and w2 = ?l2 c12 z2 y1 +(l1 c1 + l2 c12 )(z1 y1 + z2 y2 ). Now, for simplicity de ne _G(q; )G?1 (q) = g11 (q; ) g12 (q; ) g21 (q; ) g22 (q; ) Notice that G_ depends on both q and . So the equations of motions can be written as q_ = M ?1 (q)G?1 (q) =: bb11 ((qq)) bb12 ((qq)) c 21 22 u _c = ?Nc(q; ) + c + Gc G>c c _u = ?Nu (q; ) + u 1 where Nc(q; ) = g (l2 c12 w1 c ? (l1 c1 + l2 c12 )w1 u ) + g11 c + g12 u and Nu (q; ) = 1g (l2 c12 w2 c ? (l1 c1 + l2 c12 )w2 u )+ g21 c + g22 u . The equations of motion are four rst order dierential equations in q and , q_1 = b11 (q)c + b12 (q)u (C.1) q_2 = b21 (q)c + b22 (q)u (C.2) _c = ?Nc(q; ) + c + ((l1 c1 + l2 c12 )2 + (l2 c12 )2 )c (C.3) _u = ?Nu (q; ) + u (C.4) Now from (C.1) to (C.4), we can obtain the equations for dierent phases and design the control laws for each phase as given in section 3.

References

1. R.M. Rosenberg, Analytical Dynamics of Discrete Systems, Plenum Press, New York, 1977. 2. A.F. Filippov, Dierential Equations with Discontinuous Right Hand Side, Amer. Math. Soc. Translations, vol.42, ser.2, pp.199-231, 1964. 3. F.H. Clarke, Optimization and Nonsmooth Analysis, Classics in Applied Mathematics, SIAM, 1983. 4. W. Goldsmith, Impact: The Theory and Physical Behaviour of Colliding Solids, Edward Arnold Publishers, 1960. 5. D. Shevitz and B. Paden, Lyapunov Stability Theory of Nonsmooth Systems, in IEEE Transactions on Automatic Control, 39 (1994), no. 9, 1910{1914. 6. B.E. Paden and S.S. Sastry, A Calculus for Computing Filippov's Dierential Inclusion with Application to the Variable Structure Control of Robot Manipulators, in IEEE Transactions on Circuits and Systems, 34 (1987), no. 1, 73{81. 7. S. Gutman, Uncertain Dynamical Systems { a Lyapunov Min-Max Approach, IEEE Transaction on Automatic Control 24 (1979), no. 3, 437{443. 8. N.H. McClamroch and D. Wang, Feedback Stabilization and Tracking of Constrained Robots, in IEEE Transactions on Automatic Control 33 (1988), no. 5, 419{426. 9. V.I. Arnold, Mathematical Methods of Classical Mechanics, Springer-Verlag Inc., New York, 1978. 10. D. Wang and N.H. McClamroch, Position and Force Control for Constrained Manipulator Motion: Lyapunov's Direct Method, IEEE Transactions on Robotics and Automation, 9 (1993), no. 3, 308{312. 11. R. Carelly and R. Kelly, Adaptive Control of Constrained Robots Modeled by Singular Systems, in Proceedings of Conference on Decision and Control, (1989), 2635{2640. 12. B. Brogliato and P. Orhant, On the Transition Phase in Robotics: Impact Models, Dynamics and Control, in Proceedings of Conference on Decision and Control, (1994), 346{351. 13. J.K. Mills and D.M. Lokhorst, Stability and Control of Robotic Manipulators During Contact/Noncontact Task Transition, IEEE Transactions on Robotics and Automation, 9 (1993), no. 3, 335{346. 14. I.D. Walker, Impact Con guarations and Measures for Kinematically Redundant and Multiple Armed Robot Systems, in IEEE Transactions on Robotics and Automation, 10 (1994), no. 5, 670{683. 15. C.A. Desoer and M. Vidyasagar, Feedback Systems: Input-Output Properties, Academic press, New York, 1975. 16. M.M. Marques, Dierential Inclusions in Non-smooth Problems, Birkhauser, Boston, 1993. (Prabhakar R. Pagilla and Masayoshi Tomizuka) E-mail address : [email protected], [email protected]