Figure : Lagrangian coordinates for the sphere: x, y, u, v,. reference frame xed to the oor; if denotes the inner mass orientation, we get _x = cos u

Transcription

1 Nonholonomic Kinematics and Dynamics of the Sphericle Carlo Camicia Fabio Conticelli Antonio Bicchi Centro \E. Piaggio" University of Pisa Abstract In this paper we consider a complete dynamic model for the \Sphericle", a spherical vehicle that has been designed and realized in our laboratory. The sphericle is able to roll on the oor of the laboratory and reach arbitrary positions and orientations, through the use of only two motors placed within the rolling sphere. In this paper, we report on the derivation of the kinematic model of the Sphericle, which incorporates two types of nonholonomic constraints, and its dynamic model. 1 Introduction In recent years, the study of systems with nonholonomic constraints has attracted a lot of attention for several reasons (see e.g. [1]). Such constraints arise naturally in many mechanical devices: typical cases are car{like vehicles [, ], underwater vehicles, underactuated satellites, or dexterous robotic hands [,,,,8,9,1,11,1]. Sometimes nonholonomic constraints are introduced on purpose to obtain a better behaviour of the system. An interesting aspect of such systems is due to the fact that they need a smaller number of actuators than the number of independent congurations at equilibrium; this imply a lower complexity (and cost) of the mechanical system. On the other hand, nonholonomic systems, introduce many diculties in the analysis and control of the system. The dynamic modeling is more complicated than for unconstrained or holonomically constrained systems. Control design, on the other hand, cannot aord the powerful results of linear systems theory (the linear approximation of nonholonomic system causes the loss of controllability), and it is well known by now that nonholonomic systems can not be stabilized via continuous feedback control laws. In this paper we describe an experimental apparatus developed in our laboratory for research and advanced teaching purposes, previously introduced in [1]. The kinematics of the vehicle are nonholonomic, and result from the combination of the kinematics of two classical nonholonomic systems, namely, a unicycle and a plate-ball system. The \sphericle" is a ball that rolls freely on the oor, and can reach any arbitrary position therein and orientation (as shown in [1]). To make the ball move, a mobile mass is placed Figure 1: A transparentversion of the Sphericle shows the inner moving mass with wheels and suspensions. within the cavity of the ball. To implement motion, we built a mechanism with an inner mass moving by means of two weels (dierential drive), which roll on the internal surface of the sphere. The inner vehicle is inserted through an opening in the ball, which is sealed afterwards. In section, the kinematics of the Sphericle are derived using a dierent approach than was used in [1]. A dynamic model of the vehicle is then considered, which uses Euler{Lagrange equations in quasi{ coordinates (section ). Kinematic Modeling The kinematic model of the Sphericle (see g.1) will be derived in the assumption that the sphere rolls without slipping on the oor. Moreover, rotations of the sphere around the vertical axis are not allowed. The ball thickness is neglected, and the projection of the center of the inner vehicle is always considered to coincide with the contact point between the sphere and the oor. To describe the sphere and inner mass position, choose the coordinates x and y of the contact point between the sphere and the oor, in an orthogonal

2 Figure : Lagrangian coordinates for the sphere: x, y, u, v,. reference frame xed to the oor; if denotes the inner mass orientation, we get _x = cos u 1, _y = sin u 1, _ = u, i.e., the classical unicycle kinematics equations, u 1 is the forward velocity of the inner mass, i.e. the average of the linear velocity of the wheels, and u is the angular velocity of the inner mass, i.e. the dierence of the linear velocity of the wheels. Three variables describing the sphere orientation are the azimuth u and elevation v of the contact point between the sphere and the oor in a spherical coordinate reference frame xed to the ball, and the holonomy angle between the x{axes of the oor and ball Gauss frames at the contact point (see g.), so that the rotation matrix R s between the sphere frame and the inertial frame (whose columns are the sphere frame axes projected on the inertial frame) is R s (u; v; ) =R x () R z ( ) R x (v) R y (,u) (1) R x, R y and R z are the elementary rotations (the constant matrix R x () is due to the fact that the Gauss frames have opposite z{axes). The Jacobian matrix that relates the Lagrangian velocities _u, _v and _ with the sphere angular velocity! s, i.e.! s = J s d dt [u; v; ]T () is J s (u; v; ) =[,R s j j R x ()R z ( )R x (v)i j R x ()R z ( )k] = " cos v sin cos # = cos v cos, sin sin v,1 i = [1; ; ] T, j = [; 1; ] T and k = [; ; 1] T. The description 1 is not globally valid, as v = = is singularity point for the chosen coordinates (in fact det(j s ) = cos v). A suitable change of coordinates should be applied when in a neighborhood of singularities. The system variables are q =[x; y; u; v; ; ] T : We dene v Px := _x,! sy and v Py := _y +! sx the velocities of the point of the sphere in contact with the plane. Since the nonholonomic constraints for the system are 8 >< v Px = v Py =! sz = >: _y _x = sin cos (respectively the sphere rolls without slip, the sphere don't spin and the direction is constrained by the inner mass wheels), the following equation describes the sphericle kinematics g 1 (q) = _q = g 1 (q)u 1 + g (q)u ; () cos sin cos(+ ) cos v, sin(+ ) tan v cos(+ ) ; g (q) = 1 : The system has two inputs (u 1 and u, as already dened) and six states. Dynamic Modeling In this section the dynamic equations for the Sphericle are derived applying the Lagrangian approach in quasi coordinates. Recall that the Lagrange equation in nonholonomically constrained, generalized coordinates is written + AT = Fu () q is a set of independent variable that describes the system conguration, the Lagrangian L(q; _q)isthe dierence of the kinetic energy and potential energy i.e. L(q; _q) =T (q; _q), V (q), F (q) isan m full rank matrix, the vector of the generalized external forces u is the control, and A is an k n matrix that represents the k nonholonomic constraints in the so called Pfaan form (i.e. the velocities appear linearly in the matrix equations): A(q)_q =: ()

3 The resulting equations can be written in matrix form M(q)q + N(q; _q)+a T (q) = F (q)u () A(q)_q = () M(q) is the nn symmetric inertia matrix, and the term A T (q) represents the generalized constraint forces. If we consider the n (n, k) full rank matrix S(q) such that A(q)S(q) =, we can eliminate the unknown Lagrange multipliers pre-multiplying the eq. () by S T (q), and then we can eliminate the equation () using a set of n, k independent velocities, such that _q = S(q): (8) Finally we obtain a system in the form M(q)_ + N(q; ) = F (q)u (9) M(q) isa(n, k) (n, k) symmetric matrix, N(q; ) and F (q) are respectively a (n, k) vector and a(n, k) m matrix. The problem with applying such classical derivation to the sphericle dynamics is that the expression of kinetic and potential energy in terms of generalized coordinates for the system is quite cumbersome. In systems with nonholonomic constraints, in order to simplify the energetic description and to obtain directly the equation in the form (9), it is expedient to dene the kinetic energy by means of a set of nonintegrable velocities instead of the time derivatives of the Lagrangian variables _q. We can thus include the constraints in the Lagrange equation () before the calculus of derivatives. In fact, if we choose the set f; g, with dened in (8) and such that the nonholonomic constraints become simply =, using (), we obtain = S+ (q) A(q) _q: The Lagrange equation in quasi{coordinates evaluates + ST,S = Fu (1) =,(q; ; ) is the skew symmetric matrix dened by, col i col + col i (11) Notice that, ver the derivatives with respect to the constrained need not be computed, we can use a simplied form for the kinetic energy T (q; ) := T (q; ; ) (i.e. we include the nonholonomic constraints). Figure : Lagrangian coordinates for the inner mass:,, and. In order to compute equation (1), rst dene the conguration variables q to include angles,, and describing respectively the roll, pitch and yaw angles of the inner mass with respect to a frame xed to the oor (see g.), so that the rotation matrix R u between the inner mass frame and the inertial frame (the R u columns are the inner mass frame axes projected on the inertial frame) is given by = " R u (; ; ) :=R x () R y () R z () = c c,c s s s s c + c s,s s s + c c,s c,c s c + s s c s s + s c c c s = sin(), c = cos() and so on. The Jacobian matrix that relates the Lagrangian variables with the inner mass angular velocities! u, i.e. is J u (; ; ) =! u = J u d dt [; ; ]T (1) " 1 sin cos, sin cos sin cos cos The other Lagrangian variables remains the same of the quasi-static model, so the conguration variables are described by the 8{dimensional vector q =[x y u v ] T : The singularity points for the chosen coordinates are v = = and = =. The system has degrees of freedom: the angular velocity of the sphere with respect to the inertial frame! sx and! sy (the rotation around the vertical axis! sz is not allowed), and the angular velocity of the inner mass with respect to the # : #

4 sphere, projected on the frame xed to the inner mass:! uy and! uz (! ux = because of the nonholonomic constraint due to the wheels). So the {dimensional independent velocity vector is := [! sx! sy! uy! uz ] T (1) while the constrained velocity could be chosen as := [v Px v Py! sz! ux ] T = (1), as already dened, v Px = _x,! sy and v Py = _y +! sx are the velocity of the point of the sphere in contact with the plane. Notice that each item of describes a nonholonomic constraint; therefore, by denition, is not integrable. The 8 constraints matrix, dened in () is A(q) = I,j T i T J s 1,i T R T u J s i T R T u J u 1 k T J s 1 while, using denition 1, we obtain the 8 matrix (dened in (8)) that relates the independent velocity with the (non-independent) Lagrangian variables derivatives, i.e. _q = S(q): S(q) := 1,1 J s,1 [ i j ] J u,1 [ i j ] J u,1r u [ j k ] Obviously it holds A(q)S(q) =.The potential energy of the sphere is clearly constant, so V (q) depends only on the inner mass position: V (q) =,M u g(k r u )=,M u g cos q cos q r u = r s, R u k is the inner mass position, r s = [x; y; ] T is the sphere position, M u is the inner mass mass, g is the gravitational acceleration. The kinetic energy is T (q; _q) = M s + M u dr s dt + 1!T s I s! s dr u dt + 1!T u I u! u (1)! s is the sphere angular velocity, and! u can be written using eq. (1), M s is the sphere mass, I s and I u are respectively the inertia matrix of the sphere and the inner mass; with no loss of generality we suppose I u is a diagonal matrix, i.e. I u = diag(i ux ;I uy ;I uz ). Because of the sphere symmetry, I s is proportional to the identity matrix, so in the further we refer I s as a scalar; notice that for a small tickness of the ball surface it results I s ' M s. A simple choice for the pseudo{inverse S + (q), such that = S + (q)_q is i T S + (q) = j T J s j T, Ru T j T J s Ru T J u k T The system input in eq. (1) is represented by Fu; as only the inner mass is actuated, using denition 1, we choose F := 1 ; u := uf u f is the forward force applied by the inner mass wheels (i.e. the average of the torque of the wheels divided by the wheel radius) and u is the rotational torque applied by the actuators (proportional to the dierence of the wheels torque)..1 Analytic Results Summarizing, the dynamics of the sphericle can be written in the form M(q) = u k T M(q)_ +n (q; )+n (q) = Fu M 1;1 M 1; M 1; I uz s M ;1 M ; M ;,I uz s c M ;1 M ; M u I uz s,i uz s c I uz M 1;1 = M s + I sx + M u (1, c c + c )+Iuzs M 1; = M ;1 =(M u, I uz)s s c M 1; = M ;1 = M u (c s 8, s 8 c + s c 8 s ) M ; = M s + I sx + M u (, s c, c c )+I uzs c M ; = M ; = M u (,c c 8 + c c 8, s 8 s s ) The vector n (q; ) =[n 1 ; n ; n ; n ] T is dened by n i(q; ) := T N i (q) N i(j;k) = for j>= k N 1(1;) = Mu s, (M u, I uz)s c c N 1(1;) =Mu (c s c 8, s s 8 ), (M u, I uz)c c 8 s N 1(;) =(Mu, I uz)s c c 8 + I uz(s 8 c s + c 8 s ) N 1(;) =,N (1;) = Iuzc c N 1(;) = Mu (c c 8, s 8 s s ), (M u, I uz)c c 8 N (1;) = Mu s c, (M u, I uz)s c c N (1;) =,Mu (c s s 8 + s c 8 )+ +(M u, I uz)(s c c 8, c s s 8, s c 8 ) N (;) =Mu (,s s 8 + c s c 8 )+ +(M u, I uz)(c s 8 c s, c c s c 8 + c s c 8 ) N (;) = Mu s 8 c, (M u, I uz)(c s 8 + s c 8 s ) N (1;) =(Mu, I uz)(s c 8 + c s s 8, s c 8 c ) N (1;) =,N (1;) =,Iuzc 8c N (;) =,N (;) =,Iuz(s c 8 s + c s 8 ) N 1(1;) = N (;) = N (1;) = N (;) = N (;) = = N (1;) = N (1;) = N (;) = N (;) =

5 The the gravitational force vector n (q) is given by n (q) =gm u s c s c s c 8, s s 8. Linearized Longitudinal Dynamics In this section, we focus on the the longitudinal dynamics of the sphericle, corresponding to rolling in the forward direction of the inner mass, without steering, and neglecting the (nonoholonomic) coupling between lateral oscillations and longitudinal motion. To obtain such approximate decoupling between longitudinal and lateral dynamics, it is expedient to consider a linearized model of the sphericle. Although such linearization will undoubtedly destroy controllability, it provides a good approximated decoupled model for the longitudinal dynamics (which are holonomic). The linearized equations of the sphericle system are Dening _x = Ax + Bu (1) q x := (1) (dim(x) = 1) and if choosing as equilibrium point the state x =,we get A = were H := it results, ,H,H Mug I s+m s, and B =,g 1, 1 M u 1 Iuz ; rank ([BjABj :::ja 11 B]) = : As the system has an intrinsic tendency to oscillate, in order to ensure the validity of the quasi-static kinematic model used for steering (see sect. ), a feedback control law that reduces any undesired oscillations is needed. Although to obtain the controller in general is Figure : Projection of the states on a vertical plane containing the inner mass direction vector. an open problem, we consider here a {dimensional reduced model which is obtained by projecting the states on the plane perpendicular to the axle of the wheels (see g.). This results in an independent controllable subsystem of the linearized model. Naturally, feedbacklaws designed for such subsystem will not control rolling oscillations. However, maneuvers such as accelerating in the longitudinal direction, and slow steering, typically will generate small such oscillations, which will in turn aect the longitudinal dynamics (through nonholonomic coupling) negligibly. Let us consider the two dimensional reduced model of longitudinal dynamics in g.. In this case, it is convenient tochange the denition of the state describing the inner mass position: while in the three dimensional model the inner mass position is projected on the inertial frame, here we use the inner mass position with respect to the sphere position (see in g.). Dening := [; ; _ ; _] T ; and using denition (1), we obtain f 1 ; ; ; g := fx ; x, x ; x 1 ;,x 1 g. With respect to the model described above, we introduce few modications for the particular implementation of the sphericle. In particular, because the inner moving mass is actuated in our prototype by stepper motors, instead of considering torques, we will take as inputs the derivatives of the pulse rates commanded at the motors (i.e., up to neglecting quantization errors, angular accelerations of the wheels). Also, we consider explicitly the distance h along the radial direction of the center of mass of the inner vehicle from the sphere surface, and include a frictional dissipation torque,. _ The corresponding linearized model of the longitudinal dynamics is _ = A + Bu (18)

6 experimental system. Work partially funded through grants ASI ARS-9-1, ENEA/MURST SIRO. References Figure : The two dimensional model. Notice that x sphere =. A = 1 1 M H g u(,h) H g H ; B =,M uh(,h)+i uy H 1, Mu(,h) with H := M s + M u h + I s + I uy. The subsystem (18) is completely controllable. Measurements available for this system are _, (by odometry) and the pitch angle, using an inclinometer. With these outputs, system (18) is also observable. Conclusion We have obtained a complete dynamic model for the Sphericle, an interesting nonholonomic system for locomotion. Dynamics have been computed in closed form through use of the Euler{Lagrange equations in quasi{coordinates, a method which ts well nonholonomic dynamics. A simplied linear model of the longitudinal dynamics of the vehicle have been reported. These models are fundamental tools for implementing control laws for the vehicle. At the moment of writing, no general control law for the sphericle is available, while results on the stabilization of the longitudinal dynamics were obtained by the authors that applied satisfactorily to the experimental device, but are not reported here because of space limitations. Acknowledgment Authors wish to thank Mr. Andrea Gorelli, University of Siena, for his valuable help in setting up the [1] Murray, R.M., Li, Z., and Sastry, S.S.: \A Mathematical Introduction to Robotic manipulation", CRC Press, 199. [] Koshiyama, A., and Yamafuji, K.: \Design and Control of an All{Direction Steering Type Mobile Robot", Int. J. of Robotics Research, vol. 1, no., pp. 11{19, 199. [] De Luca, A., Mattone, R., and Oriolo, G.: \Dynamic Mobility of Redundant Robots Using End{ Eector Commands", Proc. IEEE Int. Conf. on Robotics and Automation, 199. [] Montana, D. J.: \The Kinematics of Contact and Grasp", Int. J. of Robotics Research, (), pp. 1:, 1988 [] Jurdjevic, V.: The geometry of the Plate{Ball Problem", Arch. Rational Mech. Anal. 1, pp. { 8, 199. [] Bicchi, A., and Sorrentino, R.: \Dextrous manipulation Through Rolling", Proc. IEEE Int. Conf. on Robotics and Automation, pp. {, 199. [] Bicchi, A., Prattichizzo, D., and Sastry, S. S. : \Planning Motions of Rolling Surfaces", IEEE Conf. on Decision and Control, 199. [8] Li, Z., and Canny, J.: \Motion of Two Rigid Bodies with Rolling Constraint", IEEE Trans. on Robotics and Automation, vol., no.1, pp. {, 199. [9] Ostrowski, J., Lewis, A., Murray, R., and Burdick, J.:\Nonholonomic Mechanics and Locomotion: the Snakeboard Example", Proc. IEEE Int. Conf. on Robotics and Automation, pp. 91{9, 199. [1] De Luca, A., and Di Benedetto, M.: \Control of Nonholonomic Systems via Dynamic Compensation", Kibernetica, vol. 9, no., pp. 9{8, 199. [11] Rouchon, P., Fliess, M., Levine, J., and Martin, P.: \Flatness, motion planning, and trailer systems", Proc. Conf. on Decision and Control, pp. {, 199. [1] Sordalen, O. J., and Nakamura, Y.: \Design of a Nonholonomic Manipulator", Proc. IEEE Int. Conf. on Robotics and Automation, pp. 8{1, 199. [1] A. Bicchi, A. Balluchi, D. Prattichizzo, A. Gorelli: \Introducing the Sphericle: an experimental testbed for research and teaching in nonholonomy," IEEE Int. Conf. on Robotics and Automation, 199.