Institutionen för systemteknik

Transcription

1 Institutionen för systemteknik Department of Electrical Engineering Examensarbete Modeling and Identification of the Gantry-Tau Parallel Kinematic Machine Examensarbete utfört i Reglerteknik vid Tekniska högskolan i Linköping av Christian Lyzell LITH-ISY-EX--07/3919--SE Linköping 2007 Department of Electrical Engineering Linköpings universitet SE Linköping, Sweden Linköpings tekniska högskola Linköpings universitet Linköping

2

3 Modeling and Identification of the Gantry-Tau Parallel Kinematic Machine Examensarbete utfört i Reglerteknik vid Tekniska högskolan i Linköping av Christian Lyzell LITH-ISY-EX--07/3919--SE Handledare: Examinator: Erik Wernholt isy, Linköpings Universitet Geir Hovland ieee, University of Queensland Svante Gunnarsson isy, Linköpings Universitet Linköping, 9 February, 2007

4

5 Avdelning, Institution Division, Department Division of Automatic Control Department of Electrical Engineering Linköpings universitet SE Linköping, Sweden Datum Date Språk Language Svenska/Swedish Engelska/English Rapporttyp Report category Licentiatavhandling Examensarbete C-uppsats D-uppsats Övrig rapport ISBN ISRN LITH-ISY-EX--07/3919--SE Serietitel och serienummer Title of series, numbering ISSN URL för elektronisk version Titel Title Modellering och Identifiering av den Parallelkinematiska Roboten Gantry-Tau Modeling and Identification of the Gantry-Tau Parallel Kinematic Machine Författare Author Christian Lyzell Sammanfattning Abstract This report presents work done in the field of modeling and identification of parallel kinematic machines. The results have been verified on the new Gantry-Tau prototype situated at the University of Queensland. The inverse dynamic model for the 3-DOF Gantry-Tau has been validated and implemented to fit the new prototype. The present prototype enables 5-DOF of motion and a new model has been derived and the results are given. Finally, an attempt to identify the parameters in the inverse dynamics models is presented. It turns out that the identification was not able to give accurate estimates. Nyckelord Keywords Robotics,parallel,modeling,identification,Tau

6

7 Abstract This report presents work done in the field of modeling and identification of parallel kinematic machines. The results have been verified on the new Gantry-Tau prototype situated at the University of Queensland. The inverse dynamic model for the 3-DOF Gantry-Tau has been validated and implemented to fit the new prototype. The present prototype enables 5-DOF of motion and a new model has been derived and the results are given. Finally, an attempt to identify the parameters in the inverse dynamics models is presented. It turns out that the identification was not able to give accurate estimates. v

8

9 Acknowledgments There are a number of people without whom this project never would have come through. First and foremost I would like to thank Geir Hovland at the University of Queensland for giving me the opportunity to visit and all the support given during my stay. I hope our collaboration will continue. Svante Gunnarsson for initiating the contact and making this thesis possible. Torgny Brogårdh for financial support and giving a helping hand when needed. Erik Wernholt for his ideas and help during my struggles. Last but not least Carolin for your love and support. Without you I would not have the strength and courage to pull this project through. vii

10

11 Contents 1 Introduction Previous work Problem description Outline Robotics Introduction Mathematical modeling of robots The Gantry-Tau Introduction Actuators Platform Kinematics Introduction Forward kinematics Inverse kinematics DOF Gantry-Tau DOF Gantry-Tau Jacobians Introduction DOF Gantry-Tau The robot Jacobian The platform point Jacobians The inverse leg Jacobians DOF Gantry-Tau The robot Jacobian The platform points Jacobians The inverse leg Jacobians ix

12 x Contents 6 Inverse Dynamics General inverse dynamics Platform forces Link dynamics Validation DOF Gantry-Tau Simulations DOF Gantry-Tau Simulations Identification Introduction Experiments Validation Linear regression DOF Gantry-Tau Linear regression Experiments Results Conclusions Inverse dynamics Identification Future work Bibliography 45 A Joint angles 47

13 Notation Symbols, Operators and Functions q i The actuator position for actuator i = 1, 2,...,5. X The TCP coordinates (X, Y, Z) T. X k The platform points coordinates (X k, Y k, Z k ) T for k = A, B,..., F represented as the vector from TCP to the platform point. q ij The passive joint angles for i = 1, 2, and j = A, B,..., F. L i The link lengths for i = 1, 2, 3. r X The platform orientation around the X-axis. r Y The platform orientation around the Y -axis. The platform orientation around the Z-axis. r Z ( J r δxk δx J Γ F p H ) The robot Jacobian. The platform point Jacobian. The leg Jacobian. The actuator torques. The platform forces. The leg dynamics. Abbreviations and Acronyms PKM TCP LS Parallel Kinematic Machine Tool Point Center Least Square xi

14

15 Chapter 1 Introduction This chapter will give a brief introduction to the subject and summarize previous work. Later the contributions and the outline of this thesis will be given. The increasing demands of extreme accuracy, high stiffness and exceptional dynamical behavior in the production industry has led to the growth of a relatively new field in robotics, Parallel Kinematics Machines (PKM). The manipulator studied in this thesis has a unique arm structure designed by ABB, called the Taustructure, and is primarily intended for the assembly of aeroplane structures. This task is presently performed by large, inflexible and very expensive serial robots which fulfill the demands for high stiffness and a large workspace. The new manipulator is referred to as the Gantry-Tau and is designed to fulfill the same demands but to a much lower cost. The Gantry-Tau is developed at the University of Queensland, UQ, in Brisbane, Australia, where a prototype of the Gantry-Tau has been constructed. The research project is a cooperation between UQ and ABB Automation in Västerås, Sweden. 1.1 Previous work In [3] the solutions for the inverse kinematics and dynamics for the former 3-DOF Gantry-Tau are given. The thesis [2] analyzed the flexibilities of the prototype and an attempt for identification of the dynamics model was given. As a result an extensive reconstruction of the prototype was made to minimize the system hysteresis and backlash. The paper [10] presents the inverse kinematics for a Gantry-Tau machine with two additional telescopic links, i.e. the 5-DOF Gantry- Tau manipulator. 1.2 Problem description The first goal of this project is to implement the inverse kinematics and dynamics model for the 3-DOF Gantry-Tau derived in [3] to fit the new Gantry-Tau 1

16 2 Introduction prototype located at the University of Queensland. The solution presented in [3] was developed according to the former prototype, which had somewhat different geometry. The second goal is to implement the inverse kinematics solution for the 5-DOF Gantry-Tau presented in [10] to fit the present prototype and to derive a complete inverse dynamics model for this manipulator. The third and final goal is to estimate the parameters in the 3-DOF inverse dynamics model with the method of system identification. Also, if it is possible, make use of the newly found 5-DOF model to enable different platform orientations, by fixing the telescopic links to different lengths. 1.3 Outline The thesis starts with a brief introduction to robotics in Chapter 2. The geometry of the Gantry-Tau is explained in Chapter 3. In Chapter 4 the solutions to the kinematic problems are presented and Chapter 5 presents the Jacobian s needed in the ensuing chapters. Chapter 6 explains the solutions to the inverse dynamic problems and in Chapter 7 identification of the parameters in this model is considered. Finally in Chapter 8 we summarize the conclusions and results from the previous chapters.

17 Chapter 2 Robotics This chapter will give a short introduction to robotics in general. A brief description of the different types of manipulators is given and a comparison is made. Finally the process of mathematical modeling of robots is presented. 2.1 Introduction Today s factories are often designed to mass produce identical products and to change these manufacturing processes is difficult and costly. To lower these costs different types of reprogrammable machines has been invented, today called robots, which are able to perform highly repetitive tasks with high accuracy and speed. The most common types of robots today are the serial manipulators. These robots have their links connected in series, see Figure 2.1. Figure 2.1. The IRB 2400 serial manipulator with courtesy of ABB. The Gantry-Tau is a parallel kinematic machine (PKM), see Figure 2.2. book [5] defines this concept as The A parallel robot is composed of a mobile platform connected to a fixed base by a set of identical parallel kinematic chains, which are called legs. The end-effector is fixed to the mobile platform. 3

18 4 Robotics Figure 2.2. The Gantry-Tau PKM with courtesy of ABB. A comparison between the serial robot and the parallel manipulator is given in [5]. It states the differences 1. Workspace: The parallel robots have a relatively small workspace compared to the serial manipulator. 2. Payload - robot mass ratio: In the case of serial robots, the end-effector and the manipulated object are located at the end of the serial chain, which leads to that each actuator must have the necessary power to move not only the connected link itself but all the succeeding objects, which leads to poor payload. In parallel structures, the load is directly supported by all the actuators and is in general located close to the base. Therefore, the links between the platform and the base can be lightened considerably, which gives much higher payload, generally with a factor of at least Accuracy: The serial robots accumulate errors from one joint to the next, since friction, flexibility, etc. also act in a serial manner. Parallel manipulators do not present this drawback and their architecture provides remarkable rigidity even with light connecting links. 4. Dynamic behavior: Considering their high payload and their reduced coupling effect between joints, parallel robots have better dynamic performance. On one hand, the parallel structure is thus superior to the serial robot when high accuracy is needed and the demanded workspace is not too large. On the other hand, modeling of serial robots has a long history and extensive research

19 2.2 Mathematical modeling of robots 5 has been done for many years. This gives the serial architecture, for now, the upper hand in most fields. Since parallel robotics is a relatively young field and not many standardized methods for dynamical modeling have been developed, it is in general a very difficult problem to solve. There are some parallel robots in commercial use, like the IRB 340 Flexpicker from ABB, but not many companies support research in this area. 2.2 Mathematical modeling of robots To successfully produce a manufacturing robot, or a robot in general, one needs to know how it behaves for different kind of inputs, i.e. some kind of model is needed. The benefit of a mathematical model is that one can simulate the behavior without having the robot present or even realized. The first part of the mathematical modeling is to find out how different actuator positions affect the position of the Tool Center Point (TCP) of the robot. These equations are called the forward kinematics and are used, for example, when evaluating the reachable positions for the TCP, which is called the robot s workspace. Reversely, one also needs the actuator positions when the coordinates of the TCP are known, which is called the inverse kinematics. These equations are important, for example, for the end user who only wants to tell the robot where, at which position, it shall perform the tasks at hand. The kinematics for the Gantry-Tau is presented in Chapter 4. From these kinematic equations, both the forward and inverse, one often find the velocities and accelerations of the structure merely by differentiating these equations. These relationships are described by what is refered to as the Jacobian. There are different kinds of Jacobians, who describes the relationships between different points in the structure, and some of these Jacobians for the Gantry-Tau are presented in Chapter 5. The kinematics described above, which includes the Jacobians, describe the motion of the robot without any consideration of the forces and torques producing the motion. Therefore one derives a dynamical model of the robot, which describes the relationship between force and motion. These equations contain the different Jacobians as well as the masses and the moment of inertias for the different parts. In Chapter 6 the inverse dynamics model for the Gantry-Tau manipulator is presented. The final step in the modeling is to use system identification on the dynamics model to get, in some sense, the optimal parameters. Some of these parameters such as the moment of inertias of different links are often unknown and hard to measure. In this case, which is valid for the Gantry-Tau, one has the choice between using some kind of approximation like symmetry or to use identification to get a parameter value. An attempt to estimate the parameters in the inverse dynamics model for the 3-DOF Gantry-Tau is presented in Chapter 7.

20

21 Chapter 3 The Gantry-Tau This chapter will give a short presentation of the Gantry-Tau PKM. A brief description of the actuator and platform geometry will be given and the notation of the variables used throughout this report will be specified. 3.1 Introduction The Gantry-Tau is a PKM with a 1/2/3 link configuration, see Figure 2.2, which is constructed to overcome the limitations of the workspace and still have the advantages of the parallel structure. Its application is first and foremost to process long aeroplane components where today s machines are very large and expensive. Figure 3.1 shows a Matlab plot of the manipulator with some of the notation described in this chapter. The arms are connected with 2-DOF joints to the actuators and 3-DOF to the platform. This implies that there are no torques acting on the arms, only forces in the same direction as the arms. 3.2 Actuators Table 3.1 shows the notations for the actuator points that will be used throughout this thesis. The base actuators are rodless actuators delivered by Tollo Linear AB and are powered by an AC-motor where a rotational movement of the motor axis is transformed into a linear movement of the saddle via a drive train. The drive train consists of a belt gear, which connects the motor to a lead screw, and a nut, which connects the screw to the saddle [2]. Figure 3.2 shows the geometry of the actuators. The parameter Z offs is the length from the base of the actuator tracks to the center of the link joints and we will here ignore the small differences of size in Z offs for the different actuators. The Y and Z-coordinates given, are all expressed in terms of the the coordinates of the base of respective track and the Y offs and Z offs constants. Therefore, these 7

22 8 The Gantry-Tau 1.6 B A q q4 L2 1 Z 0.8 F q1 0.6 L1 D C B A 0.4 q5 L3 F E TCP = (X,Y,Z) 0.2 L D E C q3 0 Y X Figure 3.1. A plot of the Gantry-Tau using Matlab. The single arrows defines the positive directions for the base actuators while the double arrows shows the location of the telescopic links (positive direction is an extension of the arm). Point X Y Z A q 2 Y 2a Z 2a B q 2 Y 2b Z 2b C q 3 Y 3c Z 3c D q 3 Y 3d Z 3d E q 3 Y 3e Z 3e F q 1 Y 1f Z 1f Table 3.1. The notations for the actuator points. Point X Y Z A X a Y a Z a B X b Y b Z b C X c Y c Z c D X d Y d Z d E X e Y e Z e F X f Y f Z f TCP X Y Z Table 3.2. The notations for the platform points.

23 3.3 Platform 9 Yoffs A Yoffs Yoffs F Zoffs B Zoffs Y q1 q2 Actuator 1 X Z Actuator 2 X Y D Yoffs Z Yoffs E C Z Zoffs q3 Actuator 3 X Y Figure 3.2. The actuator geometry. coordinates are constant and known. The only variables, in the 3-DOF case, connected to the actuators are therefore the X-coordinates, denoted by q i where i = 1, 2, 3, and the passive joint variables presented in Section 4.3. The links connected to the points B and D are telescopic arms from Tollo Linear AB. In the 3-DOF case we have these arms locked to the lengths L 2 and L 3, respectively. In the 5-DOF case these lengths are variable and will be denoted by q 4 and q 5, respectively. 3.3 Platform The platform is shaped as a cylinder with six pins where the arms are connected. Figure 3.1 shows the geometry of the platform. Table 3.2 shows the notations for the platform points that will be used throughout this thesis. All the lengths and radii of the platform are known. Therefore all the platform points A-F can be expressed in terms of these elementary lengths in the TCP-coordinate frame. To be able to calculate the inverse kinematics, we will also introduce the platform point M which is the midpoint between the points C and E, see Section 4.3. To get the correct weight of the platform and be able to do some future testing of the machine, the application intended rotational spindle has been integrated into the platform.

24

25 Chapter 4 Kinematics This chapter will describe how the inverse kinematics for the Gantry-Tau was derived as described in the papers [3] and [10]. The ease and the difficulties of finding an analytical solution for the Gantry-Tau will be shown, at least for the 3-DOF machine. A completely analytical solution for the 5-DOF parallel machine is not possible and only a numerical solution for the platform rotation angle r Y can be found. This will complicate things at a later stage. 4.1 Introduction Kinematics is the problem of describing the motion of the manipulator without any consideration of the forces and torques, i.e. calculating the positions, velocities and accelerations of the various elements of the robot. There are two separate kinematic problems: Forward kinematics describes the position, velocity and acceleration of the platform parameters as a function of the actuator variables. This problem is in general very hard to solve analytically for a general parallel kinematic machine (PKM), due to the complex geometric arm structures. Inverse kinematics is, as the name implies, the reverse problem: To describe the positions, velocities and accelerations of the actuator variables as a function of the platform parameters. Also all the values of the passive and redundant variables are derived. This problem is usually a lot easier than the forward kinematics problem and an analytical solution can often be found even for parallel machines with many degrees of freedom. A good introduction to the kinematics problem, and to robotics in general, is given in [12]. This book focuses mostly on serial robots and to get a feel for the parallel machines an introduction is given in [5]. The notations are presented in Chapter 3. 11

26 12 Kinematics 4.2 Forward kinematics As told in the introduction above, forward kinematics for PKMs is in general hard to derive. This is also the case for the triangular link Gantry-Tau where an analytical solution is yet to be found. A solution is needed for two reasons: Firstly, in the absence of an advanced laser measurement system to measure the TCP coordinates accurately, we need to calculate them from the measurements of the actuators given by the built-in decoders. Secondly, it would be good to be able to verify the equations of the inverse kinematics. The problem is stated as a nonlinear least squares (NLS) with the loss function V = z T z whose components are given by z 1 = [ (q 2 (X + X a )) 2 + (Y 2a (Y + Y a )) 2 + (Z 2a (Z + Z a )) 2] L 2 2 z 2 = [ (q 2 (X + X b )) 2 + (Y 2b (Y + Y b )) 2 + (Z 2b (Z + Z b )) 2] q4 2 z 3 = [ (q 3 (X + X c )) 2 + (Y 3c (Y + Y c )) 2 + (Y 3c (Z + Z c )) 2] L 2 3 z 4 = [ (q 3 (X + X d )) 2 + (Y 3d (Y + Y d )) 2 + (Z 3d (Z + Z d )) 2] q5 2 z 5 = [ (q 3 (X + X e )) 2 + (Y 3e (Y + Y e )) 2 + (Z 3e (Z + Z e )) 2] L 2 3 z 6 = [ (q 1 (X + X f )) 2 + (Y 1f (Y + Y f )) 2 + (Z 1f (Z + Z f )) 2] L 2 1 (4.1a) (4.1b) (4.1c) (4.1d) (4.1e) (4.1f) where the unknowns are the TCP coordinates (X, Y, Z) T and orientation (r X, r Y, r Z ) T which is hidden in the expressions for the platform points (X i, Y i, Z i ) T. The expressions inside the square brackets are the distances between the actuator and platform points A, B,..., F, respectively, and shall equal the length of the adherent arm which is subtracted. The platform points are rotated with r X, r Y and r Z bounded by π 6 r X π 6, π 2 r Y 0, π 6 r Z π 6 (4.2) The TCP coordinates are bounded by the workspace calculated in [3]: 1 X 2, 1 Y 1, 0 Z 1.5 (4.3) The NLS problem is solved in Matlab using the function lsqnonlin and provides numerical values for the TCP coordinates and the platform rotations given the values for the actuator positions. Since the solution is based on numerical optimization it is very slow, even with good starting values, and thus not suitable for real-time calculations. The solution presented is for the general 5-DOF model and in the 3-DOF case it is given by substituting q 4 and q 5 with L 2 and L 3, respectively. 4.3 Inverse kinematics The inverse kinematics for the triangular Gantry-Tau was presented in the papers [3] and [10]. These papers developed the inverse kinematics according to the former prototype and in this report, the results are applied and verified for

27 4.3 Inverse kinematics 13 the present machine. The equations are still valid, only the basic geometry was changed, and will be presented here to give a deeper understanding for the Gantry- Tau and kinematics of PKMs DOF Gantry-Tau According to [3] we need to start by finding the redundant platform rotation r Y. The rotation angles r X and r Z are equal to zero in the 3-DOF case, which is due to the triangular link and the precise lengths of the arms [3]. To solve the inverse kinematic problem, the coordinates for the TCP are specified by the user and the objective is to express the actuator positions, the redundant rotation r Y and all the passive joint variables in those coordinates. (q 2, Z 2a,b ) L 2 X TCP Z TCP (X c, Z c ) TCP = (X, Z) L 4 (X m, Z m ) positive r Y direction r Y L 3 L 3m (X e, Z e ) Z L 3 X (q 3, Z 3c,e ) Figure 4.1. The 3-DOF Gantry-Tau in the (X, Z)-plane. Figure 4.1 shows the Gantry-Tau from the front perspective, compare with Figures 2.2 and 3.1. We see that cosr Y = Z m L 4 (4.4) where L 4 is given by the Z-coordinate distance between the TCP and the platform points C, E and M, which is constant and is easy to calculate from the platform geometry, see Section 3.3. To derive an expression for Z m in only the given variables,

28 14 Kinematics we move to the global frame and note that cosr Y = Z + Z m Z 3m (X + Xm q 3 ) 2 + (Z + Z m Z 3m ) 2 (4.5) Since L 3m = (X + X m q 3 ) 2 + (Y + Y m Y 3m ) 2 + (Z + Z m Z 3m ) 2 (4.6) equation (4.5) can be written as cosr Y = Z + Z m Z 3m L 2 3m (Y + Y m Y 3m ) 2 (4.7) Combining equation (4.4) and (4.7) yields Z m = L 4 (Z 3m Z) L 2 3m (Y + Y m Y 3m ) 2 + L 4 (4.8) The above expression is free from unknown parameters (the TCP-coordinates (X, Y, Z) T are specified by the user, Y m is known since the rotation r Y is around the Y -axis and L 3m is constant since it is specified in the (X, Z) plane as the length from actuator three to the midpoint M) which makes it possible to calculate the rotation r Y according to (4.4) ( ) Zm r Y = arccos (4.9) where the numerator on the right-hand side of the equation is given by (4.8) and thus expressed only in known coordinates. Hence an analytical expression for r Y has been derived in the 3-DOF case. Now we are ready to calculate the expressions for the actuator positions. First, the platform is rotated with r Y around the global Y -axis. This is done by multiplying the platform points with the rotational matrix R Y = cosr Y 0 sin r Y (4.10) sinr Y 0 cosr Y Now the expressions are quite easy to find q 1 = X + X f + L 2 1 (Y + Y f Y 1f ) 2 (Z + Z f Z 1f ) 2 (4.11) q 2 = X + X a + L 2 2 (Y + Y a Y 2a ) 2 (Z + Z a Z 2a ) 2 (4.12) q 3 = X + X c + L 2 3 (Y + Y c Y 3c ) 2 (Z + Z c Z 3c ) 2 (4.13) which are analytical as well. L 4

29 4.3 Inverse kinematics 15 X q1 q2b q1f X q2 L2 L1 [X + Xf, Y + Yf, Z + Zf] Y q2f q2a q1a, q1b L2 [X + Xb, Y + Yb, Z + Zb] [X + Xd, Y + Yd, Z + Zd] L3 [X + Xe, Y + Ye, Z + Ze] [X + Xa, Y + Ya, Z + Za] X q3 q2d L3 [X + Xc, Y + Yc, Z + Zc] L3 q2c, q2e q1c, q1d, q1e Figure 4.2. The passive actuator joints. What remains now is to calculate the passive joint variables for each arm. Each arm joint at the actuators has two passive angles each, see Figure 4.2. This figure shows the actuators, represented by rectangular blocks, and the passive joint angles and their orientation, represented by cylinders. The robot arms are the lines connected to the platform points, represented by dots. For the interested reader, the equations for the joint variables are presented (without derivation) in Appendix A. Remark 1 Note that the passive joint variables and the physical joint angles are not the same. The passive joint variables are defined by first rotating the system with q 1i around the global Y and then rotating with q 2i around the new local X. If we look at the physical joints, one always work around the global Z and the other one work around the local Y axis, see Figure 4.3. We have thus reached an analytical solution to the inverse kinematics problem for the 3-DOF machine. This enables the use of symbolic software packages, such as Mathematica or Maple, to get explicit expressions for the equations above. This also makes it possible to calculate their time derivatives, by the use of the chain rule, as analytical expressions in terms of the TCP-coordinates and its time derivatives.

30 16 Kinematics β α Figure 4.3. The physical joint angles DOF Gantry-Tau The solution for the 5-DOF machine presented in [10] follows a similar approach as above. The calculation of r Y becomes a bit more complicated as we shall see. We start as before by determining the redundant angle r Y. In the 5-DOF case, in addition to the TCP coordinates, the rotations r X and r Z are also specified. The rotational matrices are given by R X = cosr X sinr X 0 sin r X cosr X, R Z = cosr Z sinr Z 0 sin r Z cosr Z (4.14) and R y is given by (4.10). The link lengths L 3c and L 3e, both equal to L 3, can be described as L 3c = (q 3 (X + X c )) 2 + (Y 3c (Y + Y c )) 2 + (Z 3c (Z + Z c )) 2 (4.15) L 3e = (q 3 (X + X e )) 2 + (Y 3e (Y + Y e )) 2 + (Z 3e (Z + Z e )) 2 (4.16) When the platform points C and E are rotated by the sequence R X R Z R Y (the order of multiplications is significant) and we end up with two equations with two unknowns (q 3, r Y ). Introduce the variables S = X + X c (4.17) T = (Y 3c (Y + Y c )) 2 + (Z 3c (Z + Z c )) 2 (4.18) U = X + X e (4.19) V = (Y 3e (Y + Y e )) 2 + (Z 3e (Z + Z e )) 2 (4.20) Then (4.15) and (4.16) can be written as L 3c = (q 3 S) 2 + T (4.21) L 3e = (q 3 U) 2 + V (4.22) Keeping in mind that L 3c = L 3e = L 3 an expression for q 3 can be found q 3 = U2 S 2 + V T 2(U S) (4.23)

31 4.3 Inverse kinematics 17 Substitution into (4.21) yields ( U 2 S 2 + V T 0 = 2(U S) ) 2 S + T L 2 3 (4.24) This equation contains terms of sin r Y and cosr Y. Hence the Weierstrass substitution W = tan (r Y /2) is in order, which can be written as sin r Y = 2W 1 + W 2, cosr Y = 1 W W 2 (4.25) With these expressions for sin r Y and cosr Y, (4.24) turns out to be, in the general case, a sixth order polynomial equation in W. Since there is no general solution for such an high order equation, an analytical solution for W does not exist. Thus an analytical solution for r Y = 2 atan(w) is not possible. With the assumption that the arms C and E are connected at the same point on the actuators, the equation reduces to a fourth order polynomial equation. There is a bundle of algorithms to solve these equations but, to the authors knowledge, none of them can solve a fourth order with general symbolic coefficients. The algorithms found use some sort of test to find out if a quantity is complex or in other cases positive. Since our coefficients are symbolic these tests are not possible. Thus an analytical solution for W is not possible and hence not possible for r Y. Therefore only a numerical solution for the sixth order polynomial equation can be used, which will turn out to be a small inconvenience when calculating the Jacobians later on. Now we are ready to determine the actuator positions. Equations (4.11) to (4.13) are still valid and the equations for the telescopic arms are calculated as the length from the platform points to the adherent actuator point. q 4 = (q 2 (X + X b )) 2 + (Y 2b (Y + Y b )) 2 + (Z 2b (Z + Z b )) 2 (4.26) q 5 = (q 3 (X + X d )) 2 + (Y 3d (Y + Y d )) 2 + (Z 3d (Z + Z d )) 2 (4.27) The equations for the passive joint variables are valid as well if the lengths L 2 and L 3 are substituted with q 4 and q 5, respectively, in equations (A.1) where appropriate. Since we were not able to deduce an analytical solution for the 5-DOF machine another way to calculate the velocities and accelerations are needed. The way that was chosen, to minimize the use of numerical differentiation, is to consider the variable r Y as given, which can be calculated with the help of the numerical forward kinematics, and in the Jacobians presented later consider r Y as a user defined variable.

32

33 Chapter 5 Jacobians This chapter will explain and derive the necessary Jacobian matrices which are used to calculate different velocities and accelerations for the robot. These Jacobians are used in the inverse dynamic model of the manipulator as described in the next chapter. 5.1 Introduction We introduce the notation q 1 X q 2 q = q 3 q 4, X = Y Z r X, X k = X k Y k (5.1) Z k q 5 r Z where q denotes the actuator positions, X the TCP position and X k the platform points, where k = A, B,..., F. The most common Jacobian, which is also used in modeling of serial manipulators, is the robot Jacobian J r. This Jacobian describes the relation between the actuator velocity q and the TCP velocity Ẋ as Ẋ = J r q, [J r ] ij = X i q j (5.2) The TCP X to platform points X k Jacobian is defined by ( ) ( ) δxk δxk Ẋ k = Ẋ, = (X i + X ki ) (5.3) δx δx X j where k = A, B,..., F are refered to as the platform points Jacobians. The final set of Jacobians needed are the link variables to platform point Jacobians J k, refered to as the leg Jacobians, which describe the relationship between the link variables (the actuator position, the two passive joint angles and telescopic 19 ij

34 20 Jacobians link position if present) velocities and the velocities of the platform points. For example, the arm 3d has the link variables Q 3d = (q 3, q 1d, q 2d, q 5 ) T in the 5-DOF case and Ẋ d = J 3d Q 3d, DOF Gantry-Tau J 3d = δx d δq 3d (5.4) In the 3-DOF case all equations for the inverse kinematics are analytically expressed in the user specified TCP coordinates, and hence it is quite easy to calculate the Jacobians with symbolic software The robot Jacobian By differentiating the equations (4.11) (4.13) we end up with the inverse robot Jacobian J 1 r which is a square matrix. Hence, the robot Jacobian is derived simply by inverting the result The platform point Jacobians Since the coordinates of the platform points are easily expressed in terms of the TCP variables, see definition (5.3), the platform point Jacobians are readily calculated by differenting these equtions The inverse leg Jacobians The final set of Jacobians needed in the dynamics model (6.1) are the inverse leg Jacobians J 1. When calculating the leg Jacobian one shall disconnect the link from the platform which has been done in Figure 5.1. Here the link variables are L m c (Q 1, Y 0, Z 0 ) T Q = (Q 1, Q 2, Q 3 ) T Figure 5.1. The link variables for the leg Jacobian. defined, where Y 0 and Z 0 are the constant actuator coordinates Y and Z, m is the mass of the link, c is the end point of the link, L is the length of the link and Q are the passive joint variables whose elements consist of the actuator position q i, the passive joints angles q 1i and q 2i, defined in Section 4.3.1, respectively.

35 5.3 5-DOF Gantry-Tau 21 Rotate the link into place by multiplication of the matrix R x R y where R x = cos(q 3 ) sin (Q 3 ), R y = cos(q 2) 0 sin(q 2 ) (5.5) 0 sin(q 3 ) cos(q 3 ) sin(q 2 ) 0 cos(q 2 ) which yields c = Q 1 Y 0 + R x R y 0 Q 1 + sin (Q 2 )L 0 = Y 0 sin (Q 3 )cos(q 2 )L (5.6) L Z 0 + cos(q 3 )cos(q 2 )L Z 0 By differentiating (5.6) with respect to Q we obtain J = 1 cos(q 3)L 0 0 sin (Q 3 )sin (Q 2 )L cos(q 3 ) cos(q 2 )L (5.7) 0 cos(q 3 )sin (Q 2 )L sin (Q 3 )cos(q 2 )L Hence, we end up with an invertable square matrix, so to derive the inverse leg Jacobian J 1 i we only need to invert J in (5.7) DOF Gantry-Tau Since we could not find an analytical expression for the redundant angle r Y, this will give us some problem in the 5-DOF case. To solve this problem it will be assumed, in some cases, that the rotation r Y is specified by the user, i.e. as an independent variable. All the Jacobians are easily verified by their definitions in Section The robot Jacobian To find the robot Jacobian J r we will use the method presented in [1] and applied in [4]. Let F a = (F a, F b,...,f f ) T represent the arm forces and F = (F x, F y, F z ) T be the external forces acting on the TCP. Denote by M = (M x, M y, M z ) T the external torques acting on the TCP. Then we can write F = 6 F i u i, M = i=1 6 F i (r i u i ) (5.8) where u i is a unit vector in the direction of link i and r i is a vector pointing from the TCP to the platform point i. Equation (5.8) can be written as ( F M) = HF a (5.9) i=1 where H = ( ) u1 u 2... u 6 r 1 u 1 r 2 u 2... r 6 u 6 (5.10)

36 22 Jacobians The paper [1] showed the duality between the statics and the link Jacobian, i.e. H 1 = J r T (5.11) In the Gantry-Tau case, the actuator at the base only take up forces in the X- direction, see Figure 5.2, and the telescopic links take up the entire force. Thus, A F au a F fu f F bu b F B Y q 1 q 2 Actuator 1 τ 1 = F fu f (1, 0, 0) T X Z Actuator 2 τ 2 = (F au a + F bu b) (1, 0, 0) T F du d X Z Y D F eu e E F cu c C Z q 3 Actuator 3 τ 3 = (F cu c + F du d + F eu e) (1, 0, 0) T X Y Figure 5.2. The forces acting on the base actuators. the robot Jacobian for the 5-DOF Gantry-Tau can be calculated as J r = (H 2 H 1 ) T (5.12) where H is given in (5.10) and (u f ) x (u a ) x (u b ) x H 2 = 0 0 (u c ) x (u d ) x (u e ) x (5.13) The platform points Jacobians The definition of the platform Jacobians ( δx k ) δx would easily be used if we had an analytical expression for the redundant angle r Y. Since this is not the case for the 5-DOF machine, we need to come up with a way to handle this difficulty. One way is to see r Y as user defined, i.e. an independent variable, and add an extra

37 5.3 5-DOF Gantry-Tau 23 column to the Jacobian. Thus, for k = A, B,..., F, we have ( ) δxk = δx (X+X k ) X (Y +Y k ) X (Z+Z k ) X (X+X k ) Y (Y +Y k ) Y (Z+Z k ) Y (X+X k ) Z (Y +Y k ) Z (Z+Z k ) Z The inverse leg Jacobians (X+X k ) r X (X+X k ) r Y (X+X k ) r Z (Y +Y k ) (Y +Y k ) (Y +Y k ) r X r Y r Z (Z+Z k ) (Z+Z k ) (Z+Z k ) r X r Y r Z (5.14) In the dynamics model (6.1) we need the inverse of the leg Jacobian which turns out to be quite difficult. First we try to calculate it by manipulating the expression (5.6) and later describe a solution using the singularity-robust inverse which in our case turns out to coincide with the pseudo inverse since the trajectories are chosen to avoid any collisions (singularities). Introduce the same notation as in Section with the difference that L may vary. Then (5.6) is still valid c x = Q 1 + sin (Q 2 )L (5.15) c y = Y 0 sin (Q 3 )cos(q 2 )L (5.16) c z = Z 0 + cos(q 3 ) cos(q 2 )L (5.17) To derive the inverse leg Jacobian we need to express the four link variables velocities in terms of three Cartesian velocities. For this a fourth equation is added L 2 = (c x Q 1 ) 2 + (c y Y 0 ) 2 + (c z Z 0 ) 2 (5.18) Differentiation of (5.15) (5.18) with respect to time, keeping in mind that Y 0 and Z 0 are constants, yields the following equations ċ x = Q 1 + L cos(q 2 ) Q 2 + sin(q 2 ) L (5.19) ċ y = L sin(q 2 )sin (Q 3 ) Q 2 L cos(q 2 )cos(q 3 ) Q 3 cos(q 2 )sin(q 3 ) L (5.20) ċ z = L sin(q 2 )cos(q 3 ) Q 2 L cos(q 2 )sin (Q 3 ) Q 3 + cos(q 2 )cos(q 3 ) L (5.21) L = (2(c x Q 1 )(ċ x Q ) 1 ) + 2(c y Y 0 )ċ y + 2(c z Z 0 )ċ z /(2L) (5.22) Substituting (5.22) into the remaining equations yields a system on the form Q 1 A ċx ċ y + B Q 2 = 0 (5.23) ċ z Q 3 which gives the obvious solution J 1 = ( ) A 1 B C (5.24)

38 24 Jacobians where C is derived from (5.22) with Q 1 substituted with the solution from (5.23). Thus C = (0, C 2, C 3 ) where C 2 = (Q 1 c x )cos(q 2 )sin (Q 3 ) + (Y 0 c y )sin (Q 2 ) Q 1 + sin (Q 2 )L c x (5.25) C 3 = (Q 1 c x )cos(q 2 )cos(q 3 ) + (c z Z 0 )sin (Q 2 ) Q 1 + sin (Q 2 )L c x (5.26) The ideal solution to the problem of finding the inverse leg Jacobian would be a left inverse but the solution given by (5.24) turns out to be a right inverse, i.e. J J 1 = I 3. This is due to the fact that (5.15) is an under determined system, that is we have a redundant manipulator. Multiplication from the left yields 1 J 1 J = (5.27) 0 from which we can draw the conclusion that the reconstruction of Q 3 should be good. The quality of the others are hard to predict due to the lengthy expressions. A simulation of the solution (5.24) is given in Figure 5.3. This solution is as Link variable Q Velocity Link variable Q Velocity Link variable Q 3 Velocity Link variable L Velocity Figure 5.3. The thick solid line represents the true output, the thin grey line the output generated from the analytic solution and the thin black line is calculated with the pseudo inverse. The third velocity is reconstructed perfectly.

39 5.3 5-DOF Gantry-Tau 25 seen quite poor. Especially for Q 1 and L. This is due to the fact that no means to minimize the norm of the error ( ) δc J δq (5.28) δq where W is a weight (given by the scale factor k below), was taken. In [11] an approach to calculate the inverse Jacobian for a redundant manipulator which minimize the error (5.28) is presented. It is a generalization of the pseudo inverse called the singularity-robust inverse (SR-inverse). By using a singular value decomposition J = UΣV one can calculate the SR-inverse W J VΣ U T (5.29) where Σ = ( ) Σl (5.30) and Σ l diag ( σ i /(σi 2 + k)) for all the singular values i = 1, 2,..., l. The scale factor k can be chosen in different ways and determines the size of the neighborhoods containing the singularities where one should be cautious. One suggestion is to use a variable scale factor { k 0 (1 w/w 0 ) for w < w 0 k = (5.31) 0 for w w 0 where w is a measure of the manipulability w det {J J T } (5.32) and w 0 a threshold. This enables large k near singularities which counteracts large values of δq near singularities. For theoretical motivation of the results, see [11]. In our case, where the trajectories do not contain any singularities, the SR and the pseudo inverse (k = 0) coincide and is therefore the method chosen. A simulation is given in Figure 5.3 in which we see a big improvement in the reconstruction of Q 1 and L but it is still not satisfactory. This is due to the fact that the pseudo inverse also yields a right inverse. The pseudo inverse is implemented in Matlab through the command pinv. In the future one should try to improve this inverse to enhance the results of the dynamics model.

40

41 Chapter 6 Inverse Dynamics This chapter will present the inverse dynamics model of the Gantry-Tau prototype. The chapter is based on the paper [6] where a general solution to the inverse dynamics problem was first derived. The dynamics of the 3-DOF Gantry-Tau was presented in [3] and an early attempt of identification was made in [2]. The work in [2] led to a reconstruction of the Gantry-Tau prototype and we will here present the results. In addition, for the first time, we will present a dynamic model for the 5-DOF Gantry-Tau machine. 6.1 General inverse dynamics In this section we will present the equations for the general inverse dynamics model presented in [6]. These equations were used in [3] for dynamic modeling of the 3-DOF Gantry-Tau. All the Jacobians used in this section are presented in Chapter 5. The general inverse dynamics model of a parallel machine is given by Γ = J r T [ F p + m k=1 ( ) ] T δxk J T k H k δx (6.1) where Γ is the actuator forces, J r is the direct robot Jacobian, F p is the platform forces (see Section 6.1.1), X k is the Cartesian coordinates of platform point k (the points A to F), X is the Cartesian coordinates of the TCP, J k is the link variables to platform point Jacobian of link i and H k (see Section 6.1.2) is the inverse dynamic model of link i. Both J k and H k are calculated with the platform disconnected from the links. Below we will describe the different equation elements, except for the Jacobians who are given in Chapter 5. 27

42 28 Inverse Dynamics Platform forces The platform forces are calculated, as described in [6], through the following Newton-Euler equation ( ) ( ) ωp (ω F p = γ p v p + p MS p ) mp I 3 g (6.2) ω p (I p ω p ) MŜ p where ω p = (ṙ X, ṙ Y, ṙ Z ) T is the angular velocity of the platform, m p is the platform mass, I p is the moment of inertia matrix of the platform, I 3 is the 3 3 identity matrix, g = (0, 0, g) T is the gravity vector and MS p = m p c is the vector of first moments, where c = (c x, c y, c z ) T is the platform s center of mass. The skew symmetric matrix MŜ p associated with MS p is given by MŜ p = 0 m pc z m p c y m p c z 0 m p c x (6.3) m p c y m p c x 0 The spatial inertia matrix γ p can now be calculated as ( ) m γ p = p I 3 MŜT p MŜ p I p and the platform spatial acceleration can be expressed as (Ẍ ) v p = ω p (6.4) (6.5) For further information about the derivation of these equations, see [6] Link dynamics For the derivation of the link inverse dynamics, we will follow the presentation given in [12] but with a slightly different notation. The standard form H = M(Q) Q + C(Q, Q) Q + g(q) (6.6) is given, where Q are the actuator variables, M(Q) is the position dependent inertia matrix of the link, C(Q, Q) is the Coriolis matrix and g(q) is the gravity vector. Equation (6.6) is based on analytical mechanics, the Euler-Lagrange equation, where the first two terms on the right hand side are derived from the expression of the kinetic energy and the last term, not to be confused with g in (6.2), from the potential energy of the system shown in Figure 6.1. Position dependent inertia matrix When calculating the link dynamics one shall disconnect the link from the platform which has been done in Figure 6.1. In this figure the arm variables are defined, where Y 0 and Z 0 are the constant actuator coordinates Y and Z, m is the mass of

43 6.1 General inverse dynamics 29 the link, c is the center of mass of the link, L is the length from the base to the center of mass (which is variable for the telescopic links) and Q are the passive joint variables whose elements consist of the actuator position q k, the passive joints angles q 1k and q 2k, defined in Section 4.3.1, respectively. In [12] it is shown that m L c (Q 1, Y 0, Z 0 ) T Q = (Q 1, Q 2, Q 3 ) T Figure 6.1. The link variables for the leg dynamics. M = mj v T J v + J ω T RIRJ ω (6.7) where J v is the linear velocity Jacobian, J ω is the rotational velocity Jacobian, R is the rotational transformation from the link attached frame to the global frame and I is the moment of inertia matrix of the link. Hence, we need to find the Jacobians J v and J ω. We start by finding J v. First, rotate the link into place by multiplication of the matrix R x R y where R x = cos(q 3 ) sin (Q 3 ), R y = cos(q 2) 0 sin(q 2 ) (6.8) 0 sin(q 3 ) cos(q 3 ) sin(q 2 ) 0 cos(q 2 ) which yields c = Q 1 Y 0 + R x R y 0 Q 1 + sin (Q 2 )L 0 = Y 0 sin (Q 3 )cos(q 2 )L (6.9) L Z 0 + cos(q 3 )cos(q 2 )L Z 0 By differentiating (6.9) with respect to Q and L, J v is obtained as J v = 1 cos(q 3)L 0 sin (Q 2 ) 0 sin (Q 3 )sin (Q 2 )L cos(q 3 )cos(q 2 )L sin(q 3 )cos(q 2 ) (6.10) 0 cos(q 3 )sin (Q 2 )L sin(q 3 )cos(q 2 )L cos(q 3 ) cos(q 2 ) In the 3-DOF case we can remove the last column of the Jacobian above, since the telescopic arms are fixed and thus L = 0. To find the Jacobian J ω we note the following. A pure translation and a pure extension of the link, Q 2 = Q 3 = 0, does not yield any rotations and therefore the first and fourth columns in J ω must equal zero. By the definitions of the

44 30 Inverse Dynamics passive joint variables, Q 2 = q 1i only gives rotations around the global Y and therefore the second column in J ω must equal (0, 1, 0) T and Q 3 = q 2i yield rotation around X = cos(q 2 )X sin (Q 2 )Z, hence the third column must equal ((cos(q 2 ), 0, sin(q 2 )) T. Thus J ω = 0 0 cos(q 2) (6.11) 0 0 sin(q 2 ) 0 In the 3-DOF case we can remove the last column of J ω. By substitution of (6.10) and (6.11) with R = (R x R y ) 1 into equation (6.7) we get an expression for M whose elements are denoted by m ij. Coriolis matrix According to [12], the elements of the Coriolis matrix can be calculated from the elements of the matrix M presented above c ijk = 1 { mkj + m ki + m } ij (6.12) 2 Q i Q j Q k These matrix components c ijk are known as Christoffel symbols. Note that c ijk = c jik when k is fixed, which reduces the computational effort about one half. Gravity vector The potential energy in the system can be calculated as P = mg(z 0 + cos(q 3 )cos(q 2 )L) (6.13) where g is the gravitational constant. According to [12] we get the gravitational vector by differentiating (6.13) with respect to Q and L 0 g = mg cos(q 3 )sin(q 2 )L mg sin (Q 3 )cos(q 2 )L (6.14) mg cos(q 3 )cos(q 2 ) not to be confused with g in (6.2). In the 3-DOF case we can remove the last row in (6.14) since the telescopic arms are fixed and thus no change in potential energy is invoked by L. 6.2 Validation As a quality measure of the model we used the model fit, defined in [7] as N t=1 (y(t) ŷ(t))2 fit = N (6.15) (y(t) ȳ)2 t=1

45 6.3 3-DOF Gantry-Tau 31 where y(t) is the measured output, ŷ(t) is the predicted output and ȳ is the mean value of y(t). The model fit tells how well the model predicts the output of the system DOF Gantry-Tau The dynamics for the 3-DOF Gantry-Tau were first derived in [3]. A brief description of the model is given below and for further information see the original paper. An analysis of the matrix dimensions in (6.1) for the 3-DOF case yields T }{{} Γ = J }{{} r (3 1) (3 3) {}}{ m ( ) T F δxk p + J T k H k }{{} δx }{{}}{{} (3 1) i=1 (3 3) (3 3) (3 1) (6.16) where the Jacobians in (6.16) have been explained in Chapter 5 and the link dynamics H k in the section above. The platform forces F p need some adaptation though. Since we were able to express r Y in terms of X we do not need to consider the platform torques [3], i.e. the terms dependent of I p and MŜ p can be removed from (6.2) and we end up with F p = m p (Ẍ g) + ω p (ω p MS p ) (6.17) where g = (0, 0, g) T is the gravity constant and ω p = (0, r Y, 0) T Simulations The paper [3] presented simulations of the former prototype. Here we will present a simulation of the present prototype which has a much more rigid structure, which should lead to a better model fit. A few corrections to the implementation of the model have been made and will probably improve the results further. All the simulation results presented in this report are based on experimental data. Figure 6.2 shows the result when the signals q 1 (t) = q 2 (t) = q 3 (t) = 4 (sin (2kπw 1 (t + ϕ 1 )/4) + sin (2kπw 1 (t + ϕ 1 )/4)) k=1 4 (sin (2kπw 2 (t + ϕ 2 )/4) + sin(2kπw 2 (t + ϕ 2 )/4)) k=1 4 (sin(2kπw 3 (t + ϕ 3 )/4) + sin (2kπw 3 (t + ϕ 3 )/4)) k=1 (6.18a) (6.18b) (6.18c) where w 1 = 1.7, ϕ 1 = 0.2, w 2 = 2.3, ϕ 2 = 0.15, w 3 = 2.5 and ϕ 3 = 0.20, have been normalized and applied to the base actuators. The measured signals at the actuators are the positions and torques. Since the model only considers