S 5 S 3 S 4 S 1 S 2 P 6 P 5 P 3 TCP

Transcription

1 A BODY-ORIENTED METHOD FOR DYNAMIC MODELING AND ADAPTIVE CONTROL OF FULLY PARALLEL ROBOTS Alain Codourey, Marcel Honegger, Etienne Burdet Institute of Robotics, ETH-Zurich ETH-Zentrum, CLA, CH-89 Zurich, Switzerl Abstract: In this paper we propose a method based on the virtual work principle to nd a linear form of the dynamic equation of fully parallel robots. Compared to other methods, it has the advantage that it does not need to open the closed loop structure into a tree-structure robot. It considers rather each body separately using its Jacobian matrix to project the forces into the joint space of the robot. Thus, simplications can be made at the very beginning of the modeling, that is very usefull for real-time nonlinear adaptive control implementation. As an example, the method is applied to the Hexaglide, a parallel machine tool with degree-of-freedom. Based on this model, a simulation of a non-linear adaptive controller is performed, demonstrating the possibility to apply nonlinear adaptive control to complex parallel robots. Keywords: robot control, robot dynamics, adaptive control, parameter identication, parallel robot 1. INTRODUCTION The dynamic equation of robots is linear in its dynamic parameters èkhalil Kleinnger, 198; Khosla, 198è. This property has been exploited to identify the dynamic parameters of robots using Least-Square techniques èan et al., 198; Khosla Kanade, 198è or nonlinear adaptive control ècraig, 1988; Slotine Li, 198è. Many solutions have been proposed to nd a linear form of the dynamic equation of serial ègautier Khalil, 199; Khosla, 198è tree-structure robots èkhalil et al., 1989è. For manipulators with closed loop chains, the dynamic equation is generally dicult to establish. It is thus not easy to write it in a linear form. Solutions have been proposed èwalker, 199; Goldenberg et al., 199; Khalil Bennis, 199è in which the model is usually obtained by opening the loops in order to form a tree-structure robot. The dynamics equation is then derived using classical techniques developed for serial robots èkhalil et al., 1989è a loop closure constraint equation to eliminate the non-actuated joints from the tree-structure dynamic model. The method has been applied to planar parallel mechanisms, but becomes very cumbersome for spacial parallel robots. The problem has been recently addressed by èbhattacharya et al., 199è who proposed a solution based on an energy model of the parallel structure. It results in a parallelism in the dynamic equations which can be exploited eciently in a multiprocessor controller structure. It remains however very computationally intensive. In this paper we propose a new method for nding the dynamic equation in an explicit linear function of the the dynamic parameters. It is based on the

2 virtual work principle uses Jacobian matrices to ëproject" the forces of each body in the space of the active joints. It has the following advantages: The robot does not need to be opened in an equivalent tree-structure mechanism. Simplifying hypotheses can be used at the very beginning of the modeling already. It leads to simple equations that can be used for real time implementation of nonlinear adaptive control. In this paper, we rst present the proposed method to get a linear form of the dynamics equation. The method is applied to the Hexaglide, a parallel platform with dof then used in a nonlinear adaptive control scheme. Finally, simulations are performed showing the eciency of nonlinear adaptive control of complex parallel robots..1 Denitions. DYNAMIC EQUATION A robot is a system of N rigid bodies connected together in order to form a kinematic chain. Let q be the vector of active joint variables X the vector of operational space variables describing the pose of the end-eector in a reference frame O. A frame C i is attached to the centre of mass of body i. Let m i I i be the mass inertia tensor of body i, x i the position of its centre of mass in the reference frame O. _x i! i are its linear angular velocity respectively. The rotation matrix between body frame C i the inertial reference frame O is noted O i R.For each body, the linear angular velocity of the center of mass can be expressed as a function of the joint velocity vector _q by the mean of the basic Jacobian è?è dened by: X xi i = J i _q! i è1è This basic Jacobian can also be used to transform a virtual displacement of the joint space vector into a virtual displacement of body i. with Fi inertia mi íx i ; èè I i _! i +! i I i! i F applied Fi i ; èè T i where J i is the basic Jacobian as dened in equation è1è F i, T i the applied forces torques èe.g. motor torques, gravity, frictionè.. Extracting the actuator forces In order to write equation èè in a linear form of the dynamic parameters, the actuator forces must rstly be extracted. The wrench of applied forces torques F applied i can be splitted in an actuator wrench Fi a an external wrench Fi ext. Equation èè can thus be rewritten as follows: or also, J T i F a i = J = Ji T,Fi inertia Ji T,Fi inertia,fi ext ; èè,fi ext èè where is the actuator torque vector J is dened as follows ècodourey Burdet, 199è: J J T A1 1 J T A ::: J T Am m ; èè with m the number of actuators, J Ai the Jacobian matrix linking the velocity of frame A i attached to the motor the joint velocity vector _q, i i i ; è8è. Virtual work principle i for revolute joint 1 for prismatic joint ; i =1, i : Using the virtual work èor d'alembertè principle, the dynamics of a system of N rigid bodies can be described by ècodourey Burdet, 199è: J T i F inertia i,f applied i = èè Using this denition, the dynamic equation èè can nally be written as: = J,1 " J T i,f inertia i è,fi ext : è9è

3 . LINEAR FORM OF THE DYNAMIC EQUATION Assuming that the kinematics of the system of N rigid bodies is known, its dynamic equation can be written in a linear form of the dynamic parameters èkhosla, 198; Sciavicco Siciliano, 199è: =èq; _q; íqè p; è1è where is the dynamic matrix p the dynamic parameter vector containing functions of the lengths, masses inertias..1 Inertial forces on a rigid body Inertial forces are usually described relatively to the center of mass of a body. If the body is described relatively to an arbitrary frame A attached to body i, a transformation has to be realized 1. We assume that the Jacobian matrix J A linking the joint velocity _q to the linear angular velocities of frame A is known, as well as its velocities _x A,! A accelerations. According to equation èè, inertial forces C F C acting at the centre of mass of the body are calculated using the following equation: C m FC = C íx C C C IC _! C + C! C C C è11è I C!C They can be rewritten in a linear form of the dynamic parameters of body i with respect to an arbitrary reference frame A attached to the body ècodourey Burdet, 199è: where with A FA = ~! = A íx A, A^íx A è! m mr I p ; è1è = A ^! AA^! A + A^_! A ; è1è è! = A ^! A A ~! A + A ~_! A ;! x! y! z! x! y! z! x! y! z è1è è1è where a hat è^aè over a vector indicates its skew symmetric matrix notation such that ^ab = 1 index i is omitted in the following to simplify the notation a b for any vector a = ëa x ;a y ;a z ë T, is dened as: ^r =,a z a y a z,a x : è1è,a y a x m is the mass of the body, mr its rst order moment vector I p ëi xx ;I xy ;I xz ;I yy ;I zy ;I zz ë T a vector composed of the second order moments èinertiaè of the body. Expression è1è is similar to the one obtained by ègoldenberg et al., 199è. The wrench A F A can nally be calculated in reference frame O as follows:. Linear form O FA = OA R O A R A FA è1è Assuming that the external forces are composed of gravitational forces only, equation è9è can be rewritten with respect to body points A i, i = 1:::; N, as: = J,1 " J T Ai O F A, J T Oi mi G è ;è18è where G is the vector of gravitational acceleration. By adding the acceleration of gravity to the acceleration of body i, this equation can be rearranged as follows: where with = J,1 i J T A1 1 JA T ::: JA T N N p 1 p. p N ; OAi R Ai a i i O A i R, Ai^a ; è19è i è!i A i a i Ai íx Ai, Ai G; p i =ëm i ;m i r T i ;IT pië T : Equation è19è is a linear representation of the dynamic equation. If a column of the dynamic matrix is composed of zeros only it can be cancelled together with the corresponding parameter, since it has no inuence on the dynamics. Furthermore, if columns of the dynamic matrix can be

4 Bz S Bx S S By S S 1 S P P Px Tx P P P P 1 Py TCP Ty Fig. 1. Drawing of the Hexaglide parallel machine tool expressed as linear combinations of other columns, the corresponding parameters can be grouped only one column kept in the dynamic matrix ègautier, 199; Goldenberg et al., 199; Khalil Bennis, 199è. In nonlinear adaptive control, it is necessary to compute the inverse dynamic model in real-time. For fully parallel robots, an ecient solution is to neglect the inertias of the legs. It has been shown in ècodourey, 1991è that for most fully parallel robots this assumption leads to a simple model without too much loss. In this case, nding a linear form of the dynamics is highly simplied by using the method proposed in this paragraph. Indeed, the complicated kinematics of the legs does not need to be computed, the robot is reduced to few bodies. As an illustration, the method described in this paragraph is applied to the Hexaglide, a dof parallel machine.. MODELING OF THE HEXAGLIDE MACHINE The Hexaglide is a concept developed at the Institute of Machine Tools of the Swiss Federal Institute of Technology Zurich èwieg et al., 199è. It has a fully parallel structure with dof is intended to be used as a high speed milling machine. A drawing of the machine is shown in gure 1. It is similar to a Stewart platform but instead of variable length bars, it has constant length bars linking the platform to linear motors distributed on linear rails. In this section, we rst derive the kinematics of the Hexaglide then use it for calculating a linear form of the dynamic equation. Fig.. Denition of the frames geometric parameters of the Hexaglide.1 Kinematics of the Hexaglide For modeling the Hexaglide structure, a base reference frame B is dened as shown in gure. A frame P is attached to the center of gravity of the platform. A third frame T is attached to the tool-center-point ètcpè of the robot. The points linking the legs to the platform are noted P i, i =1; :::;, each leg is attached to the linear motor at the points S i, i = 1; :::;. The pose of the TCP is represented by the vector x T ëx; y; z; ; ; ë T ; èè where x; y; z are the cartesian positions of the TCP ; ; the xed angles ZYX representation of its rotation ècraig, 1989è. The rotation matrix between the frames T B is given by: B T R = R xèè R y èè R z èè è1è The inertia of the legs will be neglected in order to simplify the computation of the dynamic model. With this assumption, the robot can be considered as composed of bodies, i.e. motors the platform. In order to compute the dynamic equation, the velocity acceleration of each body, as well as the Jacobian relating its cartesian velocity to the joint velocity of the robot, have to be known. In the following, the motors will be represented by the indices 1 to the platform by index. Let q i denote the joint coordinates, the joint vector be: q ëq 1 ;q ;q ;q ;q ;q ë T : èè To simplify the notation, the Jacobian matrices of the motors J i can be decomposed in two parts: one part aecting linear velocities èj vi è another aecting angular velocities only èj!i è: We have J i Jvi J!i èè

5 íx 1 = J v1 = 1 ;. J v = 1 ; èè íq 1 ; ; íx = íq : èè The linear acceleration of body is given by íx T which is calculated by numerical dierentiation of _x T. The angular velocity acceleration are further calculated as follows: B!T = E + r _ ; èè B _! T = d dt èb! T è : èè. Linear form of the dynamic equation Because no rotation occurs at the motor level, the corresponding Jacobian matrices, angular velocities accelerations are all equal to zero: J!i =; èè! i =; _! i =; ; :::; : èè The Jacobian matrix J T of the TCP relates the time derivative of the operational coordinates the joints velocity vector: _x T J T _q; è8è with x T dened in èè q in èè. The Jacobian J T has been derived in èhonegger, 199è using the partial dierentiation of the inverse geometric model of the machine. J T is representation dependant. In order to nd the instantaneous linear angular velocities of the TCP, J T has to be multiplied by the representation dependant matrix E + èx T è ècraig, 1989è: with _x T = E + J T _q = J T _q; _x T ëv T T ;!T Të T ; E + 1 E + r è9è E r + = 1 sin cos, cos sin : èè sin cos cos It has to be noted that matrix E + is singular for = =. This is however not a problem in our case, since this happens outside of the workspace of the robot. From è9è, it follows for the Jacobian matrix of the platform èbody è: J = J T = E + J T : è1è Using equation è19è, noting that the use of equation èè leads to the identity matrix for J, the dynamic model of the Hexaglide can be written in a linear form of the dynamic parameters as follows: =p èè where the matrix correspondingly the parameter vector p can be splitted in three parts: p 1:: b ; p1:: p b p T : èè èè In this representation, the index b corresponds to the friction, index 1:: to the dynamics of the motors to the dynamics of the platform. We evaluate these terms in the next three sections...1. Determination of 1:: p 1:: The Jacobians J i ;i = 1; :::;, have been calculated above è,, è. The i matrices are dened as follows: i Bai i, B^a ; i =1; :::; ; èè i è!i where a is the linear acceleration of the body, ^a the cross product matrix form of a as dened in equation è1è, è as dened in equations è1è è1è. Using equations èè èè, we can compute i : i = íq i g 1 íq i,íq i è8è We can now determine the elements J T i i of the matrix 1:: :

6 J T 1 1 =. J T = íq íq ; : è9è As expected, for the bodies 1 to, there is no contribution of inertial terms. These terms can be cancelled, reducing correspondingly the parameter vector. This gives: 1:: íq 1 íq íq íq íq íq p 1:: ëm 1 ;m ; :::; m ë T :... Friction terms ; èè è1è For the Hexaglide, we will assume that the friction is located at the motorized joints is composed of viscous Coulomb friction: b i è_q i èb i signè _q i è+c i _q i ; ; :::; : èè Using the Jacobian J i of the motor dened in equations è,, è, we nd b signè_q 1 è _q 1 ::: signè _q è _q p b ëb 1 ;c 1 ; :::; b ;c ë T :... Determination of p èè èè For body èplatformè, the parameters èíx T,! T, _! T è needed to nd have been calculated in equations è8è, èè èè. We have: BaT B = T R, B^a B T T R B T Rè ; èè! with B a T = B íx T, G, G =ë;;,gë T, è! calculated using the components of T! T T _! T in frame T. The Jacobian matrix J comes from equation è1è. We obtain nally J T p ëm ;m r ;x ;m r ;y ;m r ;z ;... Complete form We nally nd =p I ;xx ;I ;xy ; :::; I ;zz ë T : 1:: b p 1:: p b p èè èè è8è with 1:: p 1:: dened by equations è, 1è, b p b in è, è, p in è, è. This relatively compact dynamic equation can be used for nonlinear control of the Hexaglide. This will be shown in the next section.. NONLINEAR ADAPTIVE CONTROL Algorithms from nonlinear adaptive control theory based on the minimization of the tracking error function ècraig, 1988; Slotine Li, 198; Sadegh Horowitz, 199è provide the nonlinear control the dynamical calibration simultaneously. In the following, we will use the Adaptive FeedForward Controller èaffcè of èsadegh Horowitz, 199è. The control law of the AFFC is ègure è: =èq d ;_q d ;íq d èp+ FB ; è9è where FB is the joint linear feedback portion of the controller: FB K p e+k d _e èè where e q d, q is the error vector between the desired actual joint positions K p K d are diagonal matrices with positive terms. In the AFFC, the adaptation is performed by gradient descent of the tracking error function E = è, pè ; which gives the learning rule ep new ep old +p; è1è p, T FB ;èè

7 q d, q d, q d inverse dynamics I,yy q d + e - q PD τ FB + τ FF + τ robot q, q, q Fig.. Functional scheme of the adaptative feedforward controller. inertia [kgm] 1 1 I,xx I,zz c1..c m b1..b time [s] Fig.. Learning the inertia tensor by performing pure rotations m1.. mrz Fig.. Learning the friction masses using a translational movement with,a positive denite matrix composed of the learning factors. m 1:: b 1:: èminèmaxè c 1:: èminèmaxè m ëkgë ënë ënë ëkgë actual learned. 11.è1.9 1.è m r z I ;xx I ;yy I ;zz ëkgmë ëkgmë ëkgmë ëkgmë actual learned Table 1. Comparison of learned with actual parameters p ëm 1:: ;b 1 ;c 1 ; :::; b ;c ;m ;m r z ; I ;xx ;I ;yy ;I ;zz ë T : èè. SIMULATION RESULTS This section presents some simulations performed in order to test the nonlinear adaptive controller approach designed for the Hexaglide. To simplify the learning of the parameters, the six masses of the sliders which should all be equal are collected together in a single parameter m 1::. Because they are more subject to variation, the friction terms are however considered individually. The frame attached to the TCP is supposed to be oriented such that the inertia moments I ;xy, I ;xz I ;yz are equal to zero. We further suppose that r =ë;;r z ë T, i.e. that only the position of the center of mass in the z direction of frame T has to be learned. Thus, the matrix the parameter vector p can be rewritten as íq 1 íq íq b J T íq íq íq ; èè with b J T dened in equation èè èè respectively, Figures show how the parameters have been learned. Table 1 compares the learned parameters with their real value. In this computer-simulation the dynamic parameters are learned in a relatively short time with a good precision. It has however to be mentioned that a simplied model of the dynamics of the machine has been used, including however viscous Coulomb friction terms. Furthermore, no other perturbations such as noise in the measurement data have been used. Because such eects are inherent to a real application, the learning process won't probably be so fast. This simulation presents therefore only an optimal case, but demonstrates the possibility to apply nonlinear adaptive control to complex parallel robots.. CONCLUSION Finding a linear form of the dynamic equation of parallel robots is a dicult task, especially when a real-time implementation is needed. In this paper, we presented a new method based on the virtual work principle. It considers each body separately in a body-oriented approach. Jacobian

8 matrices are used to project the forces torques of each body onto the space of actuated joints. An advantage of the method is that simplifying hypothesis can be used at the very beginning already. As an example, the method was applied to the Hexaglide, a dof fully parallel platform. It led to compact equations which can be used in a nonlinear adaptive control scheme. Simulation results are presented demonstrating the possibility to apply nonlinear adaptive control to complex parallel robots. 8. REFERENCES An, C.H., C.G. Atkeson J.M. Hollerbach è198è. Estimation of inertial parameters of rigid body links of manipulators. In: th Conference on Decision Control. Bhattacharya, S., H. Hatwal A. Ghosh è199è. An on-line parameter estimation scheme for generalized stewart platform type parallel manipulators. Mech. Mach. Theory è1è, 9í89. Codourey, A. è1991è. Contribution a la comme des robots rapides et precis, application au robot Delta a entrainement direct. PhD thesis. Ecole Polytechnique Federale de Lausanne. Codourey, A. E. Burdet è199è. A bodyoriented method for nding a linear form of the dynamic equation of fully parallel robots. In: IEEE International Conference on Robotics Automation. Craig, J.J. è1988è. Adaptive Control of Mechanical Manipulators. Addison-Wesley. Craig, J.J. è1989è. Introduction to Robotics, Mechanics Control. Addison-Wesley. Gautier, M. è199è. Numerical calculation of the base inertial parameters. In: IEEE International Conference on Robotics Automation. pp. 1í1. Gautier, M. W. Khalil è199è. Direct calculation of minimum set of inertial parameters of serial robots. IEEE Transactions on Robotics Automation èè, 8í. Goldenberg, A.A., X. He S.P. Ananthanarayanan è199è. Identication of inertial parameters of a manipulator with closed kinematic chains. IEEE Transactions on Systems, Man, Cybernetics èè, 99í8. Honegger è199è. Steuerung und regelung einer stewart-plattform als werzeugmaschine. Technical report. Institut fuer Robotik, ETH- Zuerich. Khalil, W. F. Bennis è199è. Symbolic calculation of the base inertial parameters of closed-loop robots. The International Journal of Robotics Research 1èè, 11í118. Khalil, W. J.F. Kleinnger è198è. Minimum operations minimum parameters of the dynamic models of tree structure robots. IEEE Transactions on Robotics Automation èè, 1í. Khalil, W., F. Bennis M. Gautier è1989è. Calculation of the minimum inertial parameters of tree structure robots. In: rd ICAR Conference. pp. 189í1. Khosla, P.K. è198è. Real-time Control Identication of Direct-drive Manipulators. PhD thesis. Carnegie-Mellon University. Khosla, P.K. T. Kanade è198è. Parameter identication of robot dynamics. In: th Conference on Decision Control. Sadegh, N. R. Horowitz è199è. Stability robustness analysis of a class of adaptive controller for robotic manipulators. International Journal of Robotics Research 9èè, í9. Sciavicco, L. B. Siciliano è199è. Modeling Control of Robot Manipulators. The McGraw- Hill Companies, Inc. Slotine, J.-J.E. W. Li è198è. Adaptive manipulator control, a case study. In: IEEE International Conference on Robotics Automation. Walker, M.W. è199è. Adaptive control of manipulators containing closed kinematic loops. IEEE Transactions on Robotics Automation è1è, 1í19. Wieg, A., M. Hebsacker M. Honegger è199è. Parallele kinematik und linearmotoren: Hexaglide - ein neues, hochdynamisches werkzeugmaschinenkonzept. Technische Rundschau Transfer.