modeling and identification of the gantry-tau parallel...
TRANSCRIPT
Institutionen för systemteknikDepartment of Electrical Engineering
Examensarbete
Modeling and Identification of the Gantry-Tau
Parallel Kinematic Machine
Examensarbete utfört i Reglerteknikvid Tekniska högskolan i Linköping
av
Christian Lyzell
LITH-ISY-EX--07/3919--SE
Linköping 2007
Department of Electrical Engineering Linköpings tekniska högskolaLinköpings universitet Linköpings universitetSE-581 83 Linköping, Sweden 581 83 Linköping
Modeling and Identification of the Gantry-Tau
Parallel Kinematic Machine
Examensarbete utfört i Reglerteknik
vid Tekniska högskolan i Linköpingav
Christian Lyzell
LITH-ISY-EX--07/3919--SE
Handledare: Erik Wernholt
isy, Linköpings Universitet
Geir Hovland
ieee, University of Queensland
Examinator: Svante Gunnarsson
isy, Linköpings Universitet
Linköping, 9 February, 2007
Avdelning, Institution
Division, Department
Division of Automatic ControlDepartment of Electrical EngineeringLinköpings universitetSE-581 83 Linköping, Sweden
Datum
Date
2007-02-09
Språk
Language
� Svenska/Swedish
� Engelska/English
�
⊠
Rapporttyp
Report category
� Licentiatavhandling
� Examensarbete
� C-uppsats
� D-uppsats
� Övrig rapport
�
⊠
URL för elektronisk version
http://www.control.isy.liu.se
http://www.ep.liu.se/2007/3919
ISBN
—
ISRN
LITH-ISY-EX--07/3919--SE
Serietitel och serienummer
Title of series, numberingISSN
—
Titel
TitleModellering och Identifiering av den Parallelkinematiska Roboten Gantry-Tau
Modeling and Identification of the Gantry-Tau Parallel Kinematic Machine
Författare
AuthorChristian Lyzell
Sammanfattning
Abstract
This report presents work done in the field of modeling and identification of par-allel kinematic machines. The results have been verified on the new Gantry-Tauprototype situated at the University of Queensland.
The inverse dynamic model for the 3-DOF Gantry-Tau has been validated andimplemented to fit the new prototype. The present prototype enables 5-DOF ofmotion and a new model has been derived and the results are given.
Finally, an attempt to identify the parameters in the inverse dynamics modelsis presented. It turns out that the identification was not able to give accurateestimates.
Nyckelord
Keywords Robotics,parallel,modeling,identification,Tau
Abstract
This report presents work done in the field of modeling and identification of par-allel kinematic machines. The results have been verified on the new Gantry-Tauprototype situated at the University of Queensland.
The inverse dynamic model for the 3-DOF Gantry-Tau has been validated andimplemented to fit the new prototype. The present prototype enables 5-DOF ofmotion and a new model has been derived and the results are given.
Finally, an attempt to identify the parameters in the inverse dynamics modelsis presented. It turns out that the identification was not able to give accurateestimates.
v
Acknowledgments
There are a number of people without whom this project never would have comethrough. First and foremost I would like to thank Geir Hovland at the University ofQueensland for giving me the opportunity to visit and all the support given duringmy stay. I hope our collaboration will continue. Svante Gunnarsson for initiatingthe contact and making this thesis possible. Torgny Brogårdh for financial supportand giving a helping hand when needed. Erik Wernholt for his ideas and helpduring my struggles.
Last but not least Carolin for your love and support. Without you I would nothave the strength and courage to pull this project through.
vii
Contents
1 Introduction 1
1.1 Previous work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1
1.2 Problem description . . . . . . . . . . . . . . . . . . . . . . . . . . 1
1.3 Outline . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2
2 Robotics 3
2.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
2.2 Mathematical modeling of robots . . . . . . . . . . . . . . . . . . . 5
3 The Gantry-Tau 7
3.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7
3.2 Actuators . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7
3.3 Platform . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
4 Kinematics 11
4.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
4.2 Forward kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
4.3 Inverse kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
4.3.1 3-DOF Gantry-Tau . . . . . . . . . . . . . . . . . . . . . . . 13
4.3.2 5-DOF Gantry-Tau . . . . . . . . . . . . . . . . . . . . . . . 16
5 Jacobians 19
5.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19
5.2 3-DOF Gantry-Tau . . . . . . . . . . . . . . . . . . . . . . . . . . . 20
5.2.1 The robot Jacobian . . . . . . . . . . . . . . . . . . . . . . 20
5.2.2 The platform point Jacobians . . . . . . . . . . . . . . . . . 20
5.2.3 The inverse leg Jacobians . . . . . . . . . . . . . . . . . . . 20
5.3 5-DOF Gantry-Tau . . . . . . . . . . . . . . . . . . . . . . . . . . . 21
5.3.1 The robot Jacobian . . . . . . . . . . . . . . . . . . . . . . 21
5.3.2 The platform points Jacobians . . . . . . . . . . . . . . . . 22
5.3.3 The inverse leg Jacobians . . . . . . . . . . . . . . . . . . . 23
ix
x Contents
6 Inverse Dynamics 27
6.1 General inverse dynamics . . . . . . . . . . . . . . . . . . . . . . . 276.1.1 Platform forces . . . . . . . . . . . . . . . . . . . . . . . . . 286.1.2 Link dynamics . . . . . . . . . . . . . . . . . . . . . . . . . 28
6.2 Validation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 306.3 3-DOF Gantry-Tau . . . . . . . . . . . . . . . . . . . . . . . . . . . 31
6.3.1 Simulations . . . . . . . . . . . . . . . . . . . . . . . . . . . 316.4 5-DOF Gantry-Tau . . . . . . . . . . . . . . . . . . . . . . . . . . . 33
6.4.1 Simulations . . . . . . . . . . . . . . . . . . . . . . . . . . . 33
7 Identification 37
7.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 377.1.1 Experiments . . . . . . . . . . . . . . . . . . . . . . . . . . 377.1.2 Validation . . . . . . . . . . . . . . . . . . . . . . . . . . . . 387.1.3 Linear regression . . . . . . . . . . . . . . . . . . . . . . . . 38
7.2 3-DOF Gantry-Tau . . . . . . . . . . . . . . . . . . . . . . . . . . . 387.2.1 Linear regression . . . . . . . . . . . . . . . . . . . . . . . . 397.2.2 Experiments . . . . . . . . . . . . . . . . . . . . . . . . . . 407.2.3 Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40
8 Conclusions 43
8.1 Inverse dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . 438.2 Identification . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 438.3 Future work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44
Bibliography 45
A Joint angles 47
Notation
Symbols, Operators and Functions
qi The actuator position for actuator i = 1, 2, . . . , 5.X The TCP coordinates (X, Y, Z)T .Xk The platform points coordinates (Xk, Yk, Zk)T for k = A, B, . . . , F
represented as the vector from TCP to the platform point.qij The passive joint angles for i = 1, 2, and j = A, B, . . . , F .Li The link lengths for i = 1, 2, 3.rX The platform orientation around the X-axis.rY The platform orientation around the Y -axis.rZ The platform orientation around the Z-axis.
Jr The robot Jacobian.(
δXk
δX
)The platform point Jacobian.
J′ The leg Jacobian.
Γ The actuator torques.Fp The platform forces.H The leg dynamics.
Abbreviations and Acronyms
PKM Parallel Kinematic MachineTCP Tool Point CenterLS Least Square
xi
Chapter 1
Introduction
This chapter will give a brief introduction to the subject and summarize previouswork. Later the contributions and the outline of this thesis will be given.
The increasing demands of extreme accuracy, high stiffness and exceptional dy-namical behavior in the production industry has led to the growth of a relativelynew field in robotics, Parallel Kinematics Machines (PKM). The manipulator stud-ied in this thesis has a unique arm structure designed by ABB, called the Tau-structure, and is primarily intended for the assembly of aeroplane structures. Thistask is presently performed by large, inflexible and very expensive serial robotswhich fulfill the demands for high stiffness and a large workspace. The new ma-nipulator is referred to as the Gantry-Tau and is designed to fulfill the same de-mands but to a much lower cost. The Gantry-Tau is developed at the Universityof Queensland, UQ, in Brisbane, Australia, where a prototype of the Gantry-Tauhas been constructed. The research project is a cooperation between UQ and ABBAutomation in Västerås, Sweden.
1.1 Previous work
In [3] the solutions for the inverse kinematics and dynamics for the former 3-DOFGantry-Tau are given. The thesis [2] analyzed the flexibilities of the prototypeand an attempt for identification of the dynamics model was given. As a resultan extensive reconstruction of the prototype was made to minimize the systemhysteresis and backlash. The paper [10] presents the inverse kinematics for aGantry-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 dynam-ics model for the 3-DOF Gantry-Tau derived in [3] to fit the new Gantry-Tau
1
2 Introduction
prototype located at the University of Queensland. The solution presented in [3]was developed according to the former prototype, which had somewhat differentgeometry.
The second goal is to implement the inverse kinematics solution for the 5-DOFGantry-Tau presented in [10] to fit the present prototype and to derive a completeinverse dynamics model for this manipulator.
The third and final goal is to estimate the parameters in the 3-DOF inverse dy-namics model with the method of system identification. Also, if it is possible, makeuse of the newly found 5-DOF model to enable different platform orientations, byfixing the telescopic links to different lengths.
1.3 Outline
The thesis starts with a brief introduction to robotics in Chapter 2. The geometryof the Gantry-Tau is explained in Chapter 3. In Chapter 4 the solutions to thekinematic problems are presented and Chapter 5 presents the Jacobian’s neededin the ensuing chapters. Chapter 6 explains the solutions to the inverse dynamicproblems and in Chapter 7 identification of the parameters in this model is con-sidered. Finally in Chapter 8 we summarize the conclusions and results from theprevious chapters.
Chapter 2
Robotics
This chapter will give a short introduction to robotics in general. A brief descrip-tion 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 tochange these manufacturing processes is difficult and costly. To lower these costsdifferent 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. Theserobots 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. Thebook [5] defines this concept as
A parallel robot is composed of a mobile platform connected to a fixedbase by a set of identical parallel kinematic chains, which are calledlegs. The end-effector is fixed to the mobile platform.
3
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 comparedto the serial manipulator.
2. Payload - robot mass ratio: In the case of serial robots, the end-effectorand the manipulated object are located at the end of the serial chain, whichleads to that each actuator must have the necessary power to move not onlythe connected link itself but all the succeeding objects, which leads to poorpayload.
In parallel structures, the load is directly supported by all the actuatorsand is in general located close to the base. Therefore, the links betweenthe platform and the base can be lightened considerably, which gives muchhigher payload, generally with a factor of at least 10.
3. 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 architectureprovides remarkable rigidity even with light connecting links.
4. Dynamic behavior: Considering their high payload and their reduced cou-pling effect between joints, parallel robots have better dynamic performance.
On one hand, the parallel structure is thus superior to the serial robot whenhigh accuracy is needed and the demanded workspace is not too large. On theother hand, modeling of serial robots has a long history and extensive research
2.2 Mathematical modeling of robots 5
has been done for many years. This gives the serial architecture, for now, theupper hand in most fields. Since parallel robotics is a relatively young field andnot many standardized methods for dynamical modeling have been developed, itis in general a very difficult problem to solve. There are some parallel robots incommercial use, like the IRB 340 Flexpicker from ABB, but not many companiessupport research in this area.
2.2 Mathematical modeling of robots
To successfully produce a manufacturing robot, or a robot in general, one needs toknow 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 withouthaving the robot present or even realized.
The first part of the mathematical modeling is to find out how different actua-tor positions affect the position of the Tool Center Point (TCP) of the robot. Theseequations are called the forward kinematics and are used, for example, when eval-uating 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 TCPare 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 whichposition, it shall perform the tasks at hand. The kinematics for the Gantry-Tauis presented in Chapter 4.
From these kinematic equations, both the forward and inverse, one often findthe velocities and accelerations of the structure merely by differentiating theseequations. These relationships are described by what is refered to as the Jacobian.There are different kinds of Jacobians, who describes the relationships betweendifferent points in the structure, and some of these Jacobians for the Gantry-Tauare presented in Chapter 5.
The kinematics described above, which includes the Jacobians, describe themotion of the robot without any consideration of the forces and torques producingthe motion. Therefore one derives a dynamical model of the robot, which describesthe relationship between force and motion. These equations contain the differentJacobians 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 pre-sented.
The final step in the modeling is to use system identification on the dynamicsmodel to get, in some sense, the optimal parameters. Some of these parameterssuch as the moment of inertias of different links are often unknown and hardto measure. In this case, which is valid for the Gantry-Tau, one has the choicebetween using some kind of approximation like symmetry or to use identificationto get a parameter value. An attempt to estimate the parameters in the inversedynamics model for the 3-DOF Gantry-Tau is presented in Chapter 7.
Chapter 3
The Gantry-Tau
This chapter will give a short presentation of the Gantry-Tau PKM. A brief de-scription of the actuator and platform geometry will be given and the notation ofthe 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, whichis constructed to overcome the limitations of the workspace and still have theadvantages of the parallel structure. Its application is first and foremost to processlong 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 notationdescribed in this chapter.
The arms are connected with 2-DOF joints to the actuators and 3-DOF to theplatform. This implies that there are no torques acting on the arms, only forcesin the same direction as the arms.
3.2 Actuators
Table 3.1 shows the notations for the actuator points that will be used throughoutthis thesis.
The base actuators are rodless actuators delivered by Tollo Linear AB andare powered by an AC-motor where a rotational movement of the motor axis istransformed into a linear movement of the saddle via a drive train. The drivetrain 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 theactuators. The parameter Zoffs is the length from the base of the actuator tracksto the center of the link joints and we will here ignore the small differences of sizein Zoffs for the different actuators.
The Y and Z-coordinates given, are all expressed in terms of the the coordinatesof the base of respective track and the Yoffs and Zoffs constants. Therefore, these
7
8 The Gantry-Tau
00.20.40.60.811.21.41.61.82
−0.5
0
0.5
1
0
0.2
0.4
0.6
0.8
1
1.2
1.4
1.6
XY
Z
AB
CD
EF
q2
q1
q3
q4
q5
CED
AB
F
L2
L1
L3
L3
TCP = (X,Y,Z)’
Figure 3.1. A plot of the Gantry-Tau using Matlab. The single arrows defines thepositive directions for the base actuators while the double arrows shows the location ofthe telescopic links (positive direction is an extension of the arm).
Point X Y ZA q2 Y2a Z2a
B q2 Y2b Z2b
C q3 Y3c Z3c
D q3 Y3d Z3d
E q3 Y3e Z3e
F q1 Y1f Z1f
Table 3.1. The notations for the actuatorpoints.
Point X Y ZA Xa Ya Za
B Xb Yb Zb
C Xc Yc Zc
D Xd Yd Zd
E Xe Ye Ze
F Xf Yf Zf
TCP X Y Z
Table 3.2. The notations for the platformpoints.
3.3 Platform 9
X
Y
Z
Actuator 2
B
A
X
Z
Y
Zoffs
q1
F
Yoffs
C
Actuator 3
D
E
q3
Yoffs
Yoffs
q2
Yoffs
Yoffs
Zoffs
Actuator 1 X
Y
Zoffs
Z
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 qi wherei = 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 TolloLinear AB. In the 3-DOF case we have these arms locked to the lengths L2 andL3, respectively. In the 5-DOF case these lengths are variable and will be denotedby q4 and q5, 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 forthe platform points that will be used throughout this thesis. All the lengths andradii of the platform are known. Therefore all the platform points A-F can beexpressed in terms of these elementary lengths in the TCP-coordinate frame. Tobe able to calculate the inverse kinematics, we will also introduce the platformpoint 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 testingof the machine, the application intended rotational spindle has been integratedinto the platform.
Chapter 4
Kinematics
This chapter will describe how the inverse kinematics for the Gantry-Tau wasderived as described in the papers [3] and [10]. The ease and the difficulties offinding an analytical solution for the Gantry-Tau will be shown, at least for the3-DOF machine. A completely analytical solution for the 5-DOF parallel machineis not possible and only a numerical solution for the platform rotation angle rY
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 withoutany consideration of the forces and torques, i.e. calculating the positions, velocitiesand accelerations of the various elements of the robot. There are two separatekinematic problems:
Forward kinematics describes the position, velocity and acceleration of theplatform parameters as a function of the actuator variables. This problem is ingeneral 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 thepositions, velocities and accelerations of the actuator variables as a function of theplatform parameters. Also all the values of the passive and redundant variables arederived. This problem is usually a lot easier than the forward kinematics problemand an analytical solution can often be found even for parallel machines with manydegrees of freedom.
A good introduction to the kinematics problem, and to robotics in general, isgiven in [12]. This book focuses mostly on serial robots and to get a feel for theparallel machines an introduction is given in [5]. The notations are presented inChapter 3.
11
12 Kinematics
4.2 Forward kinematics
As told in the introduction above, forward kinematics for PKMs is in generalhard to derive. This is also the case for the triangular link Gantry-Tau wherean 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 theTCP coordinates accurately, we need to calculate them from the measurementsof the actuators given by the built-in decoders. Secondly, it would be good to beable to verify the equations of the inverse kinematics.
The problem is stated as a nonlinear least squares (NLS) with the loss functionV = zT z whose components are given by
z1 =[(q2 − (X + Xa))2 + (Y2a − (Y + Ya))2 + (Z2a − (Z + Za))2
]− L2
2 (4.1a)
z2 =[(q2 − (X + Xb))
2 + (Y2b − (Y + Yb))2 + (Z2b − (Z + Zb))
2]− q2
4 (4.1b)
z3 =[(q3 − (X + Xc))
2 + (Y3c − (Y + Yc))2 + (Y3c − (Z + Zc))
2]− L2
3 (4.1c)
z4 =[(q3 − (X + Xd))
2 + (Y3d − (Y + Yd))2 + (Z3d − (Z + Zd))
2]− q2
5 (4.1d)
z5 =[(q3 − (X + Xe))
2 + (Y3e − (Y + Ye))2 + (Z3e − (Z + Ze))
2]− L2
3 (4.1e)
z6 =[(q1 − (X + Xf))2 + (Y1f − (Y + Yf ))2 + (Z1f − (Z + Zf))2
]− L2
1 (4.1f)
where the unknowns are the TCP coordinates (X, Y, Z)T and orientation (rX , rY , rZ)T
which is hidden in the expressions for the platform points (Xi, Yi, Zi)T . The ex-
pressions inside the square brackets are the distances between the actuator andplatform points A, B, . . . , F , respectively, and shall equal the length of the adher-ent arm which is subtracted. The platform points are rotated with rX , rY and rZ
bounded by
−π
6≤ rX ≤
π
6, −
π
2≤ rY ≤ 0, −
π
6≤ rZ ≤
π
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 pro-vides numerical values for the TCP coordinates and the platform rotations giventhe values for the actuator positions. Since the solution is based on numericaloptimization it is very slow, even with good starting values, and thus not suitablefor real-time calculations.
The solution presented is for the general 5-DOF model and in the 3-DOF caseit is given by substituting q4 and q5 with L2 and L3, respectively.
4.3 Inverse kinematics
The inverse kinematics for the triangular Gantry-Tau was presented in the pa-pers [3] and [10]. These papers developed the inverse kinematics according tothe former prototype and in this report, the results are applied and verified for
4.3 Inverse kinematics 13
the present machine. The equations are still valid, only the basic geometry waschanged, and will be presented here to give a deeper understanding for the Gantry-Tau and kinematics of PKMs.
4.3.1 3-DOF Gantry-Tau
According to [3] we need to start by finding the redundant platform rotation rY .The rotation angles rX and rZ are equal to zero in the 3-DOF case, which is dueto the triangular link and the precise lengths of the arms [3]. To solve the inversekinematic problem, the coordinates for the TCP are specified by the user and theobjective is to express the actuator positions, the redundant rotation rY and allthe passive joint variables in those coordinates.
Z
X
L3m
L3
L3
L4
L2
XTCP
ZTCP
(Xm, Zm)
TCP = (X, Z)
rYpositive rYdirection
(q3, Z3c,e)
(q2, Z2a,b)
(Xc, Zc)
(Xe, Ze)
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 Fig-ures 2.2 and 3.1. We see that
cos rY =−Zm
L4(4.4)
where L4 is given by the Z-coordinate distance between the TCP and the platformpoints C, E and M , which is constant and is easy to calculate from the platform ge-ometry, see Section 3.3. To derive an expression for Zm in only the given variables,
14 Kinematics
we move to the global frame and note that
cos rY =Z + Zm − Z3m
√
(X + Xm − q3)2 + (Z + Zm − Z3m)2(4.5)
Since
L3m =√
(X + Xm − q3)2 + (Y + Ym − Y3m)2 + (Z + Zm − Z3m)2 (4.6)
equation (4.5) can be written as
cos rY =Z + Zm − Z3m
√
L23m − (Y + Ym − Y3m)2
(4.7)
Combining equation (4.4) and (4.7) yields
Zm =L4(Z3m − Z)
√
L23m − (Y + Ym − Y3m)2 + L4
(4.8)
The above expression is free from unknown parameters (the TCP-coordinates(X, Y, Z)T are specified by the user, Ym is known since the rotation rY is aroundthe Y -axis and L3m is constant since it is specified in the (X, Z) plane as the lengthfrom actuator three to the midpoint M) which makes it possible to calculate therotation rY according to (4.4)
rY = − arccos
(−Zm
L4
)
(4.9)
where the numerator on the right-hand side of the equation is given by (4.8) andthus expressed only in known coordinates. Hence an analytical expression for rY
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 rY around the global Y -axis. This is done by multi-plying the platform points with the rotational matrix
RY =
cos rY 0 sin rY
0 1 0− sin rY 0 cos rY
(4.10)
Now the expressions are quite easy to find
q1 = X + Xf +√
L21 − (Y + Yf − Y1f )2 − (Z + Zf − Z1f )2 (4.11)
q2 = X + Xa +√
L22 − (Y + Ya − Y2a)2 − (Z + Za − Z2a)2 (4.12)
q3 = X + Xc +√
L23 − (Y + Yc − Y3c)2 − (Z + Zc − Z3c)2 (4.13)
which are analytical as well.
4.3 Inverse kinematics 15
q2b
q2
X
q2a
L2
L2
q1a, q1b
[X + Xa, Y + Ya, Z + Za]
[X + Xb, Y + Yb, Z + Zb]
[X + Xc, Y + Yc, Z + Zc]
[X + Xe, Y + Ye, Z + Ze]
[X + Xd, Y + Yd, Z + Zd]
L3
L3
L3
q1c, q1d, q1e
q2c, q2e
q2d
X
q3
X
q1
[X + Xf , Y + Yf , Z + Zf ]Y
q2f
q1f
L1
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. Thisfigure shows the actuators, represented by rectangular blocks, and the passivejoint angles and their orientation, represented by cylinders. The robot arms arethe lines connected to the platform points, represented by dots. For the interestedreader, the equations for the joint variables are presented (without derivation) inAppendix 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 q1i around the global Y and then rotating with q2i 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 problemfor the 3-DOF machine. This enables the use of symbolic software packages, suchas 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 thechain rule, as analytical expressions in terms of the TCP-coordinates and its timederivatives.
16 Kinematics
α
β
Figure 4.3. The physical joint angles.
4.3.2 5-DOF Gantry-Tau
The solution for the 5-DOF machine presented in [10] follows a similar approachas above. The calculation of rY becomes a bit more complicated as we shall see.We start as before by determining the redundant angle rY . In the 5-DOF case, inaddition to the TCP coordinates, the rotations rX and rZ are also specified. Therotational matrices are given by
RX =
1 0 00 cos rX − sin rX
0 sin rX cos rX
, RZ =
cos rZ − sin rZ 0sin rZ cos rZ 0
0 0 1
(4.14)
and Ry is given by (4.10). The link lengths L3c and L3e, both equal to L3, can bedescribed as
L3c =√
(q3 − (X + Xc))2 + (Y3c − (Y + Yc))2 + (Z3c − (Z + Zc))2 (4.15)
L3e =√
(q3 − (X + Xe))2 + (Y3e − (Y + Ye))2 + (Z3e − (Z + Ze))2 (4.16)
When the platform points C and E are rotated by the sequence RXRZRY (theorder of multiplications is significant) and we end up with two equations with twounknowns (q3, rY ). Introduce the variables
S = X + Xc (4.17)
T = (Y3c − (Y + Yc))2 + (Z3c − (Z + Zc))
2 (4.18)
U = X + Xe (4.19)
V = (Y3e − (Y + Ye))2 + (Z3e − (Z + Ze))
2 (4.20)
Then (4.15) and (4.16) can be written as
L3c =√
(q3 − S)2 + T (4.21)
L3e =√
(q3 − U)2 + V (4.22)
Keeping in mind that L3c = L3e = L3 an expression for q3 can be found
q3 =U2 − S2 + V − T
2(U − S)(4.23)
4.3 Inverse kinematics 17
Substitution into (4.21) yields
0 =
(U2 − S2 + V − T
2(U − S)− S
)2
+ T − L23 (4.24)
This equation contains terms of sin rY and cos rY . Hence the Weierstrass substi-tution W = tan (rY /2) is in order, which can be written as
sin rY =2W
1 + W 2, cos rY =
1 − W 2
1 + W 2(4.25)
With these expressions for sin rY and cos rY , (4.24) turns out to be, in the generalcase, a sixth order polynomial equation in W . Since there is no general solutionfor such an high order equation, an analytical solution for W does not exist. Thusan analytical solution for rY = 2 atan (W ) is not possible.
With the assumption that the arms C and E are connected at the same point onthe actuators, the equation reduces to a fourth order polynomial equation. Thereis 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. Thealgorithms found use some sort of test to find out if a quantity is complex or inother 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 rY .Therefore only a numerical solution for the sixth order polynomial equation canbe used, which will turn out to be a small inconvenience when calculating theJacobians 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 asthe length from the platform points to the adherent actuator point.
q4 =√
(q2 − (X + Xb))2 + (Y2b − (Y + Yb))2 + (Z2b − (Z + Zb))2 (4.26)
q5 =√
(q3 − (X + Xd))2 + (Y3d − (Y + Yd))2 + (Z3d − (Z + Zd))2 (4.27)
The equations for the passive joint variables are valid as well if the lengthsL2 and L3 are substituted with q4 and q5, respectively, in equations (A.1) whereappropriate.
Since we were not able to deduce an analytical solution for the 5-DOF machineanother way to calculate the velocities and accelerations are needed. The waythat was chosen, to minimize the use of numerical differentiation, is to considerthe variable rY as given, which can be calculated with the help of the numericalforward kinematics, and in the Jacobians presented later consider rY as a userdefined variable.
Chapter 5
Jacobians
This chapter will explain and derive the necessary Jacobian matrices which are usedto calculate different velocities and accelerations for the robot. These Jacobiansare used in the inverse dynamic model of the manipulator as described in the nextchapter.
5.1 Introduction
We introduce the notation
q =
q1
q2
q3
q4
q5
, X =
XYZrX
rZ
, Xk =
Xk
Yk
Zk
(5.1)
where q denotes the actuator positions, X the TCP position and Xk the platformpoints, where k = A, B, . . . , F .
The most common Jacobian, which is also used in modeling of serial manipu-lators, is the robot Jacobian Jr. This Jacobian describes the relation between theactuator velocity q and the TCP velocity X as
X = Jrq, [Jr]ij =∂Xi
∂qj
(5.2)
The TCP X to platform points Xk Jacobian is defined by
Xk =
(δXk
δX
)
X,
(δXk
δX
)
ij
=∂(Xi + Xki
)
∂Xj
(5.3)
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 Jaco-
bians 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
20 Jacobians
link position if present) velocities and the velocities of the platform points. Forexample, the arm 3d has the link variables Q3d = (q3, q1d, q2d, q5)
T in the 5-DOFcase and
Xd = J′3dQ3d, J′
3d =δXd
δQ3d
(5.4)
5.2 3-DOF Gantry-Tau
In the 3-DOF case all equations for the inverse kinematics are analytically ex-pressed in the user specified TCP coordinates, and hence it is quite easy to calcu-late the Jacobians with symbolic software.
5.2.1 The robot Jacobian
By differentiating the equations (4.11)–(4.13) we end up with the inverse robotJacobian J−1
r which is a square matrix. Hence, the robot Jacobian is derivedsimply by inverting the result.
5.2.2 The platform point Jacobians
Since the coordinates of the platform points are easily expressed in terms of theTCP variables, see definition (5.3), the platform point Jacobians are readily cal-culated by differenting these equtions.
5.2.3 The inverse leg Jacobians
The final set of Jacobians needed in the dynamics model (6.1) are the inverse legJacobians J′−1. When calculating the leg Jacobian one shall disconnect the linkfrom the platform which has been done in Figure 5.1. Here the link variables are
(Q1, Y0, Z0)T
Q = (Q1, Q2, Q3)T
c
m
L
Figure 5.1. The link variables for the leg Jacobian.
defined, where Y0 and Z0 are the constant actuator coordinates Y and Z, m is themass 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 qi,the passive joints angles q1i and q2i, defined in Section 4.3.1, respectively.
5.3 5-DOF Gantry-Tau 21
Rotate the link into place by multiplication of the matrix RxRy where
Rx =
1 0 00 cos (Q3) − sin (Q3)0 sin (Q3) cos (Q3)
, Ry =
cos (Q2) 0 sin (Q2)0 1 0
− sin (Q2) 0 cos (Q2)
(5.5)
which yields
c =
Q1
Y0
Z0
+ RxRy
00L
=
Q1 + sin (Q2)LY0 − sin (Q3) cos (Q2)LZ0 + cos (Q3) cos (Q2)L
(5.6)
By differentiating (5.6) with respect to Q we obtain
J′ =
1 cos (Q3)L 00 sin (Q3) sin (Q2)L − cos (Q3) cos (Q2)L0 − cos (Q3) sin (Q2)L − sin (Q3) cos (Q2)L
(5.7)
Hence, we end up with an invertable square matrix, so to derive the inverse legJacobian J′−1
i we only need to invert J′ in (5.7).
5.3 5-DOF Gantry-Tau
Since we could not find an analytical expression for the redundant angle rY , thiswill give us some problem in the 5-DOF case. To solve this problem it will beassumed, in some cases, that the rotation rY is specified by the user, i.e. as anindependent variable. All the Jacobians are easily verified by their definitions inSection 5.1.
5.3.1 The robot Jacobian
To find the robot Jacobian Jr we will use the method presented in [1] and appliedin [4]. Let Fa = (Fa, Fb, . . . , Ff )T represent the arm forces and F = (Fx, Fy, Fz)
T
be the external forces acting on the TCP. Denote by M = (Mx, My, Mz)T the
external torques acting on the TCP. Then we can write
F =
6∑
i=1
Fiui, M =
6∑
i=1
Fi(ri × ui) (5.8)
where ui is a unit vector in the direction of link i and ri is a vector pointing fromthe TCP to the platform point i. Equation (5.8) can be written as
(F
M
)
= HFa (5.9)
where
H =
(u1 u2 . . . u6
r1 × u1 r2 × u2 . . . r6 × u6
)
(5.10)
22 Jacobians
The paper [1] showed the duality between the statics and the link Jacobian, i.e.
H−1 = JrT (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,
q1
F
Actuator 1 X
Y
ZActuator 2
B
A
X
Z
Y
τ2 = (Faua + Fbub) • (1, 0, 0)T
q2
Faua
Fbub
E
X
Y
Z
C
Actuator 3τ3 = (Fcuc + Fdud + Feue) • (1, 0, 0)T
D
q3
Fdud
Feue
Fcuc
τ1 = Ffuf • (1, 0, 0)T
Ffuf
Figure 5.2. The forces acting on the base actuators.
the robot Jacobian for the 5-DOF Gantry-Tau can be calculated as
Jr = (H2H−1)T (5.12)
where H is given in (5.10) and
H2 =
0 0 0 0 0 (uf )x
(ua)x (ub)x 0 0 0 00 0 (uc)x (ud)x (ue)x 00 1 0 0 0 00 0 0 1 0 0
(5.13)
5.3.2 The platform points Jacobians
The definition of the platform Jacobians(
δXk
δX
)would easily be used if we had an
analytical expression for the redundant angle rY . Since this is not the case for the5-DOF machine, we need to come up with a way to handle this difficulty. Oneway is to see rY as user defined, i.e. an independent variable, and add an extra
5.3 5-DOF Gantry-Tau 23
column to the Jacobian. Thus, for k = A, B, . . . , F , we have
(δXk
δX
)
=
∂(X+Xk)∂X
∂(X+Xk)∂Y
∂(X+Xk)∂Z
∂(X+Xk)∂rX
∂(X+Xk)∂rY
∂(X+Xk)∂rZ
∂(Y +Yk)∂X
∂(Y +Yk)∂Y
∂(Y +Yk)∂Z
∂(Y +Yk)∂rX
∂(Y +Yk)∂rY
∂(Y +Yk)∂rZ
∂(Z+Zk)∂X
∂(Z+Zk)∂Y
∂(Z+Zk)∂Z
∂(Z+Zk)∂rX
∂(Z+Zk)∂rY
∂(Z+Zk)∂rZ
(5.14)
5.3.3 The inverse leg Jacobians
In the dynamics model (6.1) we need the inverse of the leg Jacobian which turnsout to be quite difficult. First we try to calculate it by manipulating the expres-sion (5.6) and later describe a solution using the singularity-robust inverse whichin our case turns out to coincide with the pseudo inverse since the trajectories arechosen to avoid any collisions (singularities). Introduce the same notation as inSection 5.2.3 with the difference that L may vary. Then (5.6) is still valid
cx = Q1 + sin (Q2)L (5.15)
cy = Y0 − sin (Q3) cos (Q2)L (5.16)
cz = Z0 + cos (Q3) cos (Q2)L (5.17)
To derive the inverse leg Jacobian we need to express the four link variables ve-locities in terms of three Cartesian velocities. For this a fourth equation is added
L2 = (cx − Q1)2 + (cy − Y0)
2 + (cz − Z0)2 (5.18)
Differentiation of (5.15)– (5.18) with respect to time, keeping in mind that Y0 andZ0 are constants, yields the following equations
cx = Q1 + L cos (Q2)Q2 + sin (Q2)L (5.19)
cy = L sin (Q2) sin (Q3)Q2 − L cos (Q2) cos (Q3)Q3 − cos (Q2) sin (Q3)L (5.20)
cz = −L sin (Q2) cos (Q3)Q2 − L cos (Q2) sin (Q3)Q3 + cos (Q2) cos (Q3)L (5.21)
L =(
2(cx − Q1)(cx − Q1) + 2(cy − Y0)cy + 2(cz − Z0)cz
)
/(2L) (5.22)
Substituting (5.22) into the remaining equations yields a system on the form
A
cx
cy
cz
+ B
Q1
Q2
Q3
= 0 (5.23)
which gives the obvious solution
J′−1 =
(−A−1B
C
)
(5.24)
24 Jacobians
where C is derived from (5.22) with Q1 substituted with the solution from (5.23).Thus C = (0, C2, C3) where
C2 = −(Q1 − cx) cos (Q2) sin (Q3) + (Y0 − cy) sin (Q2)
Q1 + sin (Q2)L − cx
(5.25)
C3 = −(Q1 − cx) cos (Q2) cos (Q3) + (cz − Z0) sin (Q2)
Q1 + sin (Q2)L − cx
(5.26)
The ideal solution to the problem of finding the inverse leg Jacobian would be aleft inverse but the solution given by (5.24) turns out to be a right inverse, i.e.J′J′−1 = I3. 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
J′−1J′ =
1 ∗ ∗ ∗0 ∗ ∗ ∗0 0 1 00 ∗ ∗ ∗
(5.27)
from which we can draw the conclusion that the reconstruction of Q3 should begood. 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
6000 6200 6400 6600 6800 7000 7200 7400 7600 7800 8000
−0.2
−0.1
0
0.1
0.2
Link variable Q1
Vel
ocity
6000 6200 6400 6600 6800 7000 7200 7400 7600 7800 8000
−0.2
−0.1
0
0.1
0.2
0.3
Link variable Q2
Vel
ocity
6000 6200 6400 6600 6800 7000 7200 7400 7600 7800 8000−0.5
0
0.5
Link variable Q3
Vel
ocity
6000 6200 6400 6600 6800 7000 7200 7400 7600 7800 8000
−0.2
−0.1
0
0.1
0.2
0.3Link variable L
Vel
ocity
Figure 5.3. The thick solid line represents the true output, the thin grey line the outputgenerated from the analytic solution and the thin black line is calculated with the pseudoinverse. The third velocity is reconstructed perfectly.
5.3 5-DOF Gantry-Tau 25
seen quite poor. Especially for Q1 and L. This is due to the fact that no meansto minimize the norm of the error
∥∥∥∥
(δc − J′δQ
δQ
)∥∥∥∥
W
(5.28)
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 ma-
nipulator which minimize the error (5.28) is presented. It is a generalization ofthe pseudo inverse called the singularity-robust inverse (SR-inverse). By using asingular value decomposition J′ = UΣV one can calculate the SR-inverse
J′† , VΣ†UT (5.29)
where
Σ† =
(Σl 0
0 0
)
(5.30)
and Σl , diag(σi/(σ2
i + k))
for all the singular values i = 1, 2, . . . , l. The scalefactor k can be chosen in different ways and determines the size of the neighbor-hoods containing the singularities where one should be cautious. One suggestionis to use a variable scale factor
k =
{
k0(1 − w/w0) for w < w0
0 for w ≥ w0
(5.31)
where w is a measure of the manipulability
w ,
√
det {J′J′T } (5.32)
and w0 a threshold. This enables large k near singularities which counteracts largevalues of δQ near singularities. For theoretical motivation of the results, see [11].
In our case, where the trajectories do not contain any singularities, the SRand 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 thereconstruction of Q1 and L but it is still not satisfactory. This is due to thefact that the pseudo inverse also yields a right inverse. The pseudo inverse isimplemented in Matlab through the command pinv. In the future one shouldtry to improve this inverse to enhance the results of the dynamics model.
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 dy-namics problem was first derived. The dynamics of the 3-DOF Gantry-Tau waspresented in [3] and an early attempt of identification was made in [2]. The workin [2] led to a reconstruction of the Gantry-Tau prototype and we will here presentthe results. In addition, for the first time, we will present a dynamic model for the5-DOF Gantry-Tau machine.
6.1 General inverse dynamics
In this section we will present the equations for the general inverse dynamicsmodel presented in [6]. These equations were used in [3] for dynamic modeling ofthe 3-DOF Gantry-Tau. All the Jacobians used in this section are presented inChapter 5. The general inverse dynamics model of a parallel machine is given by
Γ = JrT
[
Fp +
m∑
k=1
(δXk
δX
)T
J′−Tk Hk
]
(6.1)
where Γ is the actuator forces, Jr is the direct robot Jacobian, Fp is the platformforces (see Section 6.1.1), Xk 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 linkvariables to platform point Jacobian of link i and Hk (see Section 6.1.2) is theinverse dynamic model of link i. Both J′
k and Hk are calculated with the platformdisconnected from the links. Below we will describe the different equation elements,except for the Jacobians who are given in Chapter 5.
27
28 Inverse Dynamics
6.1.1 Platform forces
The platform forces are calculated, as described in [6], through the followingNewton-Euler equation
Fp = γpvp +
(ωp × (ωp × MSp)
ωp × (Ipωp)
)
−
(mpI3
MSp
)
g (6.2)
where ωp = (rX , rY , rZ)T is the angular velocity of the platform, mp is the platformmass, Ip is the moment of inertia matrix of the platform, I3 is the 3 × 3 identitymatrix, g = (0, 0,−g)T is the gravity vector and MSp = mpc is the vector offirst moments, where c = (cx, cy, cz)
T is the platform’s center of mass. The skew
symmetric matrix MSp associated with MSp is given by
MSp =
0 −mpcz mpcy
mpcz 0 −mpcx
−mpcy mpcx 0
(6.3)
The spatial inertia matrix γp can now be calculated as
γp =
(
mpI3 −MST
p
MSp Ip
)
(6.4)
and the platform spatial acceleration can be expressed as
vp =
(
X
ωp
)
(6.5)
For further information about the derivation of these equations, see [6].
6.1.2 Link dynamics
For the derivation of the link inverse dynamics, we will follow the presentationgiven 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 dependentinertia matrix of the link, C(Q, Q) is the Coriolis matrix and g(Q) is the grav-ity vector. Equation (6.6) is based on analytical mechanics, the Euler-Lagrangeequation, where the first two terms on the right hand side are derived from the ex-pression 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 platformwhich has been done in Figure 6.1. In this figure the arm variables are defined,where Y0 and Z0 are the constant actuator coordinates Y and Z, m is the mass of
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 thecenter of mass (which is variable for the telescopic links) and Q are the passivejoint variables whose elements consist of the actuator position qk, the passive jointsangles q1k and q2k, defined in Section 4.3.1, respectively. In [12] it is shown that
(Q1, Y0, Z0)T
Q = (Q1, Q2, Q3)T
L c
m
Figure 6.1. The link variables for the leg dynamics.
M = mJvTJv + Jω
T RIRJω (6.7)
where Jv is the linear velocity Jacobian, Jω is the rotational velocity Jacobian, Ris the rotational transformation from the link attached frame to the global frameand I is the moment of inertia matrix of the link. Hence, we need to find theJacobians Jv and Jω.
We start by finding Jv. First, rotate the link into place by multiplication ofthe matrix RxRy where
Rx =
1 0 00 cos (Q3) − sin (Q3)0 sin (Q3) cos (Q3)
, Ry =
cos (Q2) 0 sin (Q2)0 1 0
− sin (Q2) 0 cos (Q2)
(6.8)
which yields
c =
Q1
Y0
Z0
+ RxRy
00L
=
Q1 + sin (Q2)LY0 − sin (Q3) cos (Q2)LZ0 + cos (Q3) cos (Q2)L
(6.9)
By differentiating (6.9) with respect to Q and L, Jv is obtained as
Jv =
1 cos (Q3)L 0 sin (Q2)0 sin (Q3) sin (Q2)L − cos (Q3) cos (Q2)L − sin (Q3) cos (Q2)0 − cos (Q3) sin (Q2)L − sin (Q3) cos (Q2)L cos (Q3) cos (Q2)
(6.10)
In the 3-DOF case we can remove the last column of the Jacobian above, sincethe telescopic arms are fixed and thus L = 0.
To find the Jacobian Jω we note the following. A pure translation and a pureextension of the link, Q2 = Q3 = 0, does not yield any rotations and thereforethe first and fourth columns in Jω must equal zero. By the definitions of the
30 Inverse Dynamics
passive joint variables, Q2 = q1i only gives rotations around the global Y andtherefore the second column in Jω must equal (0, 1, 0)T and Q3 = q2i yield ro-tation around X ′ = cos (Q2)X − sin (Q2)Z, hence the third column must equal
((cos (Q2), 0,− sin (Q2))T
. Thus
Jω =
0 0 cos (Q2) 00 1 0 00 0 − sin (Q2) 0
(6.11)
In the 3-DOF case we can remove the last column of Jω .By substitution of (6.10) and (6.11) with R = (RxRy)−1 into equation (6.7)
we get an expression for M whose elements are denoted by mij .
Coriolis matrix
According to [12], the elements of the Coriolis matrix can be calculated from theelements of the matrix M presented above
cijk =1
2
{∂mkj
∂Qi
+∂mki
∂Qj
+∂mij
∂Qk
}
(6.12)
These matrix components cijk are known as Christoffel symbols. Note that cijk =cjik 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(Z0 + cos (Q3) cos (Q2)L) (6.13)
where g is the gravitational constant. According to [12] we get the gravitationalvector by differentiating (6.13) with respect to Q and L
g =
0−mg cos (Q3) sin (Q2)L−mg sin (Q3) cos (Q2)Lmg cos (Q3) cos (Q2)
(6.14)
not to be confused with g in (6.2). In the 3-DOF case we can remove the last rowin (6.14) since the telescopic arms are fixed and thus no change in potential energyis invoked by L.
6.2 Validation
As a quality measure of the model we used the model fit, defined in [7] as
fit = 100
1 −
√∑N
t=1(y(t) − y(t))2√∑N
t=1(y(t) − y)2
(6.15)
6.3 3-DOF Gantry-Tau 31
where y(t) is the measured output, y(t) is the predicted output and y is the meanvalue of y(t). The model fit tells how well the model predicts the output of thesystem.
6.3 3-DOF Gantry-Tau
The dynamics for the 3-DOF Gantry-Tau were first derived in [3]. A brief descrip-tion 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
Γ︸︷︷︸
(3×1)
= JrT
︸︷︷︸
(3×3)
Fp︸︷︷︸
(3×1)
+
m∑
i=1
(3×3)︷ ︸︸ ︷(
δXk
δX
)T
J′−Tk︸ ︷︷ ︸
(3×3)
Hk︸︷︷︸
(3×1)
(6.16)
where the Jacobians in (6.16) have been explained in Chapter 5 and the linkdynamics Hk in the section above. The platform forces Fp need some adaptationthough. Since we were able to express rY in terms of X we do not need to considerthe platform torques [3], i.e. the terms dependent of Ip and MSp can be removedfrom (6.2) and we end up with
Fp = mp(X − g) + ωp × (ωp × MSp) (6.17)
where g = (0, 0,−g)T is the gravity constant and ωp = (0, rY , 0)T .
6.3.1 Simulations
The paper [3] presented simulations of the former prototype. Here we will presenta 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 implementationof the model have been made and will probably improve the results further. Allthe simulation results presented in this report are based on experimental data.
Figure 6.2 shows the result when the signals
q1(t) =
4∑
k=1
(sin (2kπw1(t + ϕ1)/4) + sin (2kπw1(t + ϕ1)/4)) (6.18a)
q2(t) =
4∑
k=1
(sin (2kπw2(t + ϕ2)/4) + sin (2kπw2(t + ϕ2)/4)) (6.18b)
q3(t) =
4∑
k=1
(sin (2kπw3(t + ϕ3)/4) + sin (2kπw3(t + ϕ3)/4)) (6.18c)
where w1 = 1.7, ϕ1 = 0.2, w2 = 2.3, ϕ2 = 0.15, w3 = 2.5 and ϕ3 = −0.20,have been normalized and applied to the base actuators. The measured signalsat the actuators are the positions and torques. Since the model only considers
32 Inverse Dynamics
6000 6200 6400 6600 6800 7000 7200 7400 7600 7800 8000−500
−400
−300
−200
−100
0
100
200
300
400
Actuator 1T
orqu
e
6000 6200 6400 6600 6800 7000 7200 7400 7600 7800 8000−300
−200
−100
0
100
200
300
400
500
600Actuator 2
Tor
que
6000 6200 6400 6600 6800 7000 7200 7400 7600 7800 8000
−600
−400
−200
0
200
400
Actuator 3
Tor
que
Figure 6.2. The simulation results when the actuator trajectories in (6.18) are applied.The thick line shows the measurement of the system when the Tau structure is connectedand the thin line shows the sum of the disconnected actuator torques and the modeloutput.
the dynamics of the Tau (arm) structure, we had to do two measurements: onewith the structure connected and one when it was disconnected. Thus Figure 6.2shows the measured total torque (thick line) when the Tau structure is connected,and when we added the disconnected actuator torques to the model output (thinline). In addition a three-axis accelerometer has been placed at the TCP. To getthe accelerations of the actuators we had to move the accelerometer, since we onlyhad one available, several times to complete one experiment set.
The different masses of the Tau structure, the links and the platform, havebeen measured as well and the moment of inertia matrices are approximated asdiagonal, since the links and the platform are approximately symmetrical cylinderswith different lengths and radii. The fit values for the simulation above are given
Act 1 Act 2 Act 3fit 44.48 25.71 55.27
Table 6.1. The fit values for the simulation of the actuator positions given in (6.18).
in Table 6.1 Even though the model fit values are not that high these are the
6.4 5-DOF Gantry-Tau 33
best values up to today. They are even as high as the fit produced by systemidentification in [2]. This is mainly due to the reconstruction of the robot andthe corrected errors in the model implementation. The reconstruction aimed tominimize the flexibilities in the structure to avoid backlash and hysteresis. We canthus conclude that the reconstruction was successful.
6.4 5-DOF Gantry-Tau
Here we will, for the first time, present a solution to the inverse dynamic problemfor the 5-DOF triangular Gantry-Tau. The general model presented in the begin-ning of this chapter will be used. We start, as before, with a dimension analysisof the general inverse dynamics equation (6.1)
Γ︸︷︷︸
(5×1)
= JrT
︸︷︷︸
(5×6)
Fp︸︷︷︸
(6×1)
+
m∑
i=1
(6×3)︷ ︸︸ ︷(
δXk
δX
)T
J′−Tk︸ ︷︷ ︸
(3×4)
Hk︸︷︷︸
(4×1)
(6.19)
where the Jacobians are presented in Section 5.3, the platform forces in Sec-tion 6.1.1 and the link dynamics in Section 6.1.2.
6.4.1 Simulations
Here we will use the same measurement methods as described in Section 6.3.1.First, a simulation of the same motion as in Section 6.3.1 is presented and theresults will be discussed. Later a general motion will be analysed.
3-DOF motion
We start by simulating the same trajectories as in (6.18), keeping the telescopiclinks fixed to the original lengths L2 and L3, respectively. The result is given inFigure 6.3 and the fit values are given in Table 6.2. For the actuators four and five
Act 1 Act 2 Act 3fit 66.63 4.02 31.09
Table 6.2. The fit values for the simulation of the actuator positions given in (6.18) forthe 5-DOF model.
only the model outputs are presented. They are centered around -200 N and 200 N,respectively. This is predictable since they have to compensate for the gravitationof the platform and one telescopic link whose added masses totals approximately20 kg. The variations are due to the movement of the remaining actuators.
Despite the poor result when simulating the inverse leg Jacobian J′−1 the dy-namics model turns out quite well for this simple motion. The low model fit for the
34 Inverse Dynamics
6000 6200 6400 6600 6800 7000 7200 7400 7600 7800 8000
−400
−200
0
200
400
Actuator 1T
orqu
e
6000 6200 6400 6600 6800 7000 7200 7400 7600 7800 8000
−200
0
200
400
600Actuator 2
Tor
que
6000 6200 6400 6600 6800 7000 7200 7400 7600 7800 8000
−600
−400
−200
0
200
400
Actuator 3
Tor
que
6000 6200 6400 6600 6800 7000 7200 7400 7600 7800 8000
−600
−400
−200
0
200
400
Actuator 4
Tor
que
6000 6200 6400 6600 6800 7000 7200 7400 7600 7800 8000
−600
−400
−200
0
200
400
Actuator 5
Tor
que
Figure 6.3. The simulation of the system when the actuator trajectories in (6.18) areapplied for the 5-DOF model. The thick line shows the measurement of the systemwhen the Tau structure is connected and the thin line shows the sum of the disconnectedactuator torques and the model output.
second and third actuators originates from the offset of the curves and is difficultto explain. One possible explanation is that the weight of the Tau structure mightchange the friction of the base actuators, which might decrease/increase when theweight is pulling/pushing, which is not yet tested or analyzed, but this does notexplain why the same effect is not affecting the model fit in the same way in the3-DOF case.
Otherwise the simulation shows a similar behavior as the one given in Fig-ure 6.2. This indicates that the generalization of the 3-DOF model is successful.
General motion
By applying sine waves to the telescopic actuators
q4(t) = 10 sin (0.125 · 2πt) (6.20a)
q5(t) = 10 sin (0.125 · 2πt) (6.20b)
and keeping the same base trajectories given in (6.18) we get a complete 5-DOFmotion. The result of the simulation is given in Figure 6.4 and Table 6.3. Someof the fit values are negative which is not good. It means that the mean value doesa better job than the model. This contradicts Figure 6.4 where the model seems
6.4 5-DOF Gantry-Tau 35
Act 1 Act 2 Act 3fit -93.59 20.53 -78.02
Table 6.3. The fit values for the simulation of the actuator positions given in (6.18)with a sine wave on the telescopic links.
6000 6200 6400 6600 6800 7000 7200 7400 7600 7800 8000
−400
−200
0
200
400
Actuator 1
Tor
que
6000 6200 6400 6600 6800 7000 7200 7400 7600 7800 8000
−200
0
200
400
600Actuator 2
Tor
que
6000 6200 6400 6600 6800 7000 7200 7400 7600 7800 8000
−600
−400
−200
0
200
400
Actuator 3
Tor
que
6000 6200 6400 6600 6800 7000 7200 7400 7600 7800 8000
−600
−400
−200
0
200
400
Actuator 4
Tor
que
6000 6200 6400 6600 6800 7000 7200 7400 7600 7800 8000
−600
−400
−200
0
200
400
Actuator 5
Tor
que
Figure 6.4. The simulation of the system when the actuator trajectories in (6.18) areapplied with sine waves on the telescopic links. The thick line shows the measurement ofthe system when the Tau structure is connected and the thin line shows the sum of thedisconnected actuator torques and the model output.
to give a good estimate. It looks almost as good as when the telescopic links werefixed in Figure 6.3. The fact that the second actuator gets a better fit value ishard to explain.
Overall, the model seems to predict the output quite nicely despite, as men-tioned before, the poor results of the inverse leg Jacobian. In the future one couldprobably improve the results with a better method to calculate the inverse legJacobian and look into how the weights of the Tau structure affects the saddlefrictions of the base actuators.
Chapter 7
Identification
In this chapter, identification of the parameters in the inverse dynamics model iscovered. First a brief presentation of the fundamentals of system identificationwill be given. Then we apply these techniques to the 3-DOF model and discuss theresult. For this purpose we use the modified version of the newly found 5-DOFmodel to be able to run experiments with different fixed lengths of the telescopiclinks. This will lead to different platform orientations and hopefully yield moreinformation about the system.
7.1 Introduction
The simulations of the models presented in Chapter 6 were based on approxima-tions of the moment of inertias for the links. To get the best possible values, withrespect to some well chosen measure, we use system identification. The identifica-tion of parameters in a physical model is called grey-box identification, comparedto black-box identification where parameters do not have any explicit physicalinterpretation.
A good introduction to identification is given in [8]. A more complete descrip-tion of the subject can be found in [7]. The book [5] gives an introduction to theidentification of serial robots.
7.1.1 Experiments
In system identification it is important to excite the system as much as possible inorder to get the maximal amount of information. We will use multisines, sum ofsine functions with different frequencies and phase, for this purpose. The paper [13]presents a way to calculate the optimal trajectories for identification. This wasused on the previous prototype in [2], but the difference in result between theoptimal method and only choosing a few frequencies was minimal. Therefore, sincecalculating the optimal trajectories is very time consuming, the frequencies werechosen ad-hoc to get a powerful arm and platform motion without any movementin the frame.
37
38 Identification
Since we now have a complete model for the inverse dynamics of the 5-DOFmachine we will make use of this by running the base actuator trajectories withdifferent lengths of the telescopic links. This will hopefully give more informationof the system and yield a successful identification.
7.1.2 Validation
To get information about how well the model describes the system, the fit value de-fined in (6.15) will be used. It is important to use different data sets for estimationand validation which is called cross validation.
7.1.3 Linear regression
It turns out that the models presented in Chapter 6 can be written as linearregressions. The model is given by
Γ = ϕT θ (7.1)
where Γ is the measured torques, ϕ is the regressor containing information aboutpast behavior and θ contains the parameters, such as moments of inertias, we wishto identify. From a data set of N samples one gets
Γ(t1)Γ(t2)
...Γ(tN )
︸ ︷︷ ︸
YN
=
ϕT (t1)ϕT (t2)
...ϕT (tN )
︸ ︷︷ ︸
ΦN
θ (7.2)
The solution to a least squares criterion is then given by
θls = Φ†NYN (7.3)
where Φ†N is the pseudoinverse
Φ†N = (ΦT
NΦN )−1ΦTN (7.4)
7.2 3-DOF Gantry-Tau
An early attempt to identify the parameters in the inverse dynamics model ofthe 3-DOF Gantry-Tau was made in [2]. Unfortunately, this was not successfuland gave unrealistic parameter values, such as negative moments of inertias. Onereason for this might be the flexibilities of the former prototype. These problemswere considered when building the new prototype. Another reason given was thatthere might be some unmodelled behavior. Hopefully it will turn out that theunmodelled behavior was a part of the unwanted flexibilities.
7.2 3-DOF Gantry-Tau 39
7.2.1 Linear regression
In Chapter 6 we found a model for the Gantry-Tau on the form
Γ = JrT
[
Fp +
m∑
k=1
(δXk
δX
)T
J′−Tk Hk
]
(7.5)
where the notation are given in Section 6.1. The Jacobians used are the onesgiven in the 5-DOF model where we have removed rows four and six, containingthe dynamics inflicted by rX , rX , rZ and rZ , since these equal zero when thetelescopic links are fixed. The inverse leg Jacobian J′T
k and the leg dynamics Hk
are the same as in the 3-DOF case.The three Jacobians present are independent of masses and inertias and there-
fore we focus on rewriting the platform forces Fp and the link dynamics Hk. Theplatform forces Fp for the 5-DOF case is given in (6.2). Evaluating this equationand removing rows four and six yields
Fp =
mpX − mpcxr2Y − mpcz rY
mpY
mpZ + mpcxrY − mpcz r2Y + mpg
mpczX − mpcxZ + rY Ipyy− mpg
(7.6)
which can be written as a linear regression
Fp =
X −r2Y −rY 0
Y 0 0 0
Z + g rY −r2Y 0
0 −Z − g X rY
mp
mpcx
mpcz
Ipyy
, Fpθp (7.7)
The parameters in θp do not occur anywhere else in the model, i.e. all other termsin Γ are independent of these parameters, so the continued rewriting will concernmasses and inertias of the links.
A similar analysis of the leg dynamics Hk, derived in Section 6.1.2, showsa linear dependence of the mass and the moment of inertias of the link. TheGantry-Tau has three different types of links: a shorter stiff link (index s), threelonger stiff links (index l) and two telescopic links (index t). These can be writtenindividually as linear regressions
Hk , Hkθk (7.8)
where θk =(mk, Ikxx
, Ikyy, Ikzz
)T. The total model for the inverse dynamics can
thus be written as a linear regression
Γ = JrT(Fp Fs Fl Ft
)
θp
θs
θl
θt
, ϕT θ (7.9)
40 Identification
Act 1 Act 2 Act 3fit 68.23 66.08 65.61
Table 7.1. The fit values for the simulation of the actuator positions given in (6.18) forthe 5-DOF model.
mp mpcx mpcz Ipyy
2.36 (± 0.32) −0.38 (± 0.17) 0.89 (± 0.22) 10.76 (± 0.41)mt Itxx
ItyyItzz
1.47 (± 0.61) −11.46 (± 1.23) 1.27 (± 1.83) −0.80 (± 0.98)ml Ilxx
IlyyIlzz
7.99 (± 0.41) 2.27 (± 0.85) −0.03 (± 0.89) 4.70 (± 0.68)ms Isxx
IsyyIszz
5.32 (± 0.41) −18.44 (± 8.86) −3.43 (± 1.89) −4.10 (± 0.70)
Table 7.2. The estimated parameters for the linear regression presented in Section 7.2.1with the parameter standard deviation within parentheses.
where Fp is given in (7.7) and
Fs =
(δXF
δX
)T
J′−TF Hs
Fl =
((δXA
δX
)T
J′−TA +
(δXC
δX
)T
J′−TC +
(δXE
δX
)T
J′−TE
)
Hl
Ft =
((δXB
δX
)T
J′−TB +
(δXD
δX
)T
J′−TD
)
Ht
(7.10)
7.2.2 Experiments
The trajectories used for the identification are the ones given in (6.18), wherewe shift the phases ϕi and the lengths of the telescopic links for the differentexperiment sets.
7.2.3 Results
A simulation of the least square parameter model for the actuator trajectoriesgiven in (6.18) is presented in Figure 7.1 where the telescopic links are fixed tothe original lengths L2 and L3 respectively. As can be seen, the model predictsthe output quite well which is confirmed by the model fit presented in Table 7.1.
Unfortunately, the estimated parameter values, which are given in Table 7.2together with their standard deviations, are not physical. Some of the momentof inertias are negative and the masses are far from the correct values which aregiven in Table 7.3.
7.2 3-DOF Gantry-Tau 41
3000 3200 3400 3600 3800 4000 4200 4400 4600 4800 5000−400
−300
−200
−100
0
100
200
300
400
3000 3200 3400 3600 3800 4000 4200 4400 4600 4800 5000−200
−100
0
100
200
300
400
500
600
700
3000 3200 3400 3600 3800 4000 4200 4400 4600 4800 5000−700
−600
−500
−400
−300
−200
−100
0
100
200
300
Figure 7.1. The simulation of the system with the least square parameters when theactuator trajectories 6.18 are applied. The thick line shows the measurement of thesystem when the Tau structure is connected and the thin line shows the sum of thedisconnected actuator torques and the model output.
mp mpcx mpcz Ipyymt Itxx
ItyyItzz
11.790 0 0 0.0614 10.370 1.3503 1.3503 0.0187ml Ilxx
IlyyIlzz
ms IsxxIsyy
Iszz
4.220 0.5495 0.5495 0.0021 3.970 0.3931 0.3931 0.0020
Table 7.3. The nominal parameter values used for the simulation in Section 6.3.1.
42 Identification
We can also see that the negative inertias have huge standard deviations, whichimplies that these parameters were not properly excited and thus not correctlyestimated.
A difference between the new prototype and the former, where a parameterestimation attempt was presented in [2], are the masses of the links and the plat-form. The total mass of the Tau-structure in the former prototype was 10.08 kgand now it is 49.16 kg. Hence, the actuator accelerations are more limited thanbefore. The trajectories chosen were close to the limit of the allowed accelerationsand one could clearly feel the vibrations in the frame of the manipulator. Theamplitude of the actuator trajectories were chosen small (10 mm) due to concernsthat the friction of the base actuators would vary significantly over the tracks. Inthe future one should allow a larger span of the actuator positions over the tracksto get a wider motion, but with the same maximal accelerations. One could alsotry the method presented in [13] to calculate the optimal trajectories for the linearregression. One obvious question is if the chosen parameters are identifiable, whichshould be compared with the method of finding the base parameters in rigid bodyidentification of industrial robots. Finally, one should try to use a wider range offrequencies in the trajectories.
Chapter 8
Conclusions
This chapter will summarize the report by restating the results from the previouschapters. Some ideas for future work will be given.
8.1 Inverse dynamics
An implementation of the inverse kinematics and dynamics model for the 3-DOFGantry-Tau presented in [3], to fit the new Gantry-Tau prototype has been made.The model has been evaluated against the new prototype and high model fit wasachieved. This is due to the new mechanical construction, which has reduced thebacklash and hysteresis in the system, and the corrected errors in the implemen-tation.
We have also presented, for the first time, a dynamic model for the new 5-DOF Gantry-Tau manipulator. Due to redundancy, the inverse leg Jacobian hadto be calculated with the pseudo inverse. Surprisingly, this turned out quite wellcontradicting the poor results when simulating the derived Jacobian.
8.2 Identification
The inverse dynamics model was rewritten as a linear regression and the leastsquares method was applied to estimate the unknown parameters. The fact thatwe were able to express the model as a linear regression is due to the nice propertiesof the general solution presented in [6]. In this model the terms describing theplatform forces and the leg dynamics are linear in the unknown parameters. Eventhough we made use of the newly found 5-DOF model to enable different platformorientations, the estimation was not successful. High model fit was achieved butthe estimated parameters did not have physical values, since some inertias werenegative. This is thought mainly to be due to the chosen trajectories.
43
44 Conclusions
8.3 Future work
For the dynamics modeling one should try to model the difference in the actua-tor frictions when the Tau-structure is connected and disconnected. This wouldprobably lead to a better model, especially for the second and third actuator.
Also, recently the author found an explicit closed form solution to a quarticequation, see [9]. One should, by simulations, see if one of these four solutionsconsistently give the correct value for the rotation rY . Thus having a completeanalytical solution for the 5-DOF Gantry-Tau. The model could be improved sig-nificantly if one could find an alternative way to calculate the inverse leg Jacobianfor the 5-DOF manipulator.
For a successful system identification one should try wider motions containinga larger span of frequencies. The method presented in [13] could give a suggestionin the choice of frequencies.
Bibliography
[1] C. Gosselin. Stiffness mapping for parallel manipulators. In IEEE Transac-
tions on Robotics and Automation, volume 6, pages 377–382, June 1990.
[2] J. Gunnar. Dynamical analysis and system identification of the gantry-tauparallel manipulator. Master thesis, Linköpings Universitet, 2005.
[3] G. Hovland, T. Brogårdh, and M. Murray. Analytical kinematics and dy-namics of the 3-dof gantry-tau parallel manipulator. Manuscript, 8 pages,2006.
[4] G. Hovland, M. Choux, M. Murray, and T. Brogårdh. Benchmark of the 3-dofgantry-tau parallel kinematic machine. Manuscript, 6 pages, 2006.
[5] W. Khalil and E. Dombre. Modeling, Identification & Control of Robots.Kogan Page Science, 2002. ISBN 1-9039-9666-X.
[6] W. Khalil and O. Ibrahim. General solution for the dynamic modeling ofparallel robots. In Proceedings of the 2004 IEEE International Conference on
Robotics & Automation, pages 3665–3670, April 2004.
[7] L. Ljung. System Identification, Theory for the User. Prentice Hall, 2ndedition, 1999. ISBN 0-13-656695-4.
[8] L. Ljung and T. Glad. Modeling of Dynamic Systems. Prentice Hall, 1994.ISBN 0-13-597097-0.
[9] Planet Math. http://planetmath.org/encyclopedia/QuarticFormula.html.
[10] M. Murray, G. Hovland, and T. Brogårdh. Collision-free workspace design ofthe 5-axis gantry-tau parallel kinematic machine. Manuscript, 6 pages, 2006.
[11] Y. Nakamura. Advanced Robotics, Redundancy and Optimazation. Addison-Wesley, 1st edition, 1991. ISBN 0-201-15198-7.
[12] M. Spong, S. Hutchinson, and M. Vidyasagar. Robot Modeling and Control.John Wiley & Sons, 2006. ISBN 0-471-64990-2.
[13] J. Swevers, C. Ganseman, D.B. Tükel, and J. De Schutter. Optimal robotexcitation and identification. In IEEE Transactions on Robotics and Automa-
tion, volume 13, pages 730–740, October 1997.
45
Appendix A
Joint angles
The passive joint variables are defined in Figure 4.2. We present the equations forthese variables without derivation
q1a = −π − asin
(X + Xa − q2
L2
)
(A.1a)
q2a = atan2 (Y + YA − Y2a,−(Z + Za − Z2a)) (A.1b)
q1b = −π − asin
(X + Xb − q3
L2
)
(A.1c)
q2b = atan2 ((Y + Yb − Y2b),−(Z + Zb − Z2b)) (A.1d)
q1c = asin
(X + Xc − q3
L3
)
(A.1e)
q2c = atan2 (−(Y + Yc − Y3c), (Z + Zc − Z3c)) (A.1f)
q1d = asin
(X + Xd − q3
L3
)
(A.1g)
q2d = atan2 (−(Y + Yd − Y3d), (Z + Zd − Z3d)) (A.1h)
q1e = asin
(X + Xe − q3
L3
)
(A.1i)
q2e = atan2 (−(Y + Ye − Y3e), (Z + Ze − Z3e)) (A.1j)
q1f = asin
(X + Xf − q1
L1
)
(A.1k)
q2f = atan2 (−(Y + Yf − Y3f ), (Z + Zf − Z3f )) (A.1l)
where the notation is given in Chapter 3.
47
På svenska Detta dokument hålls tillgängligt på Internet – eller dess framtida ersättare – under en längre tid från publiceringsdatum under förutsättning att inga extra-ordinära omständigheter uppstår.
Tillgång till dokumentet innebär tillstånd för var och en att läsa, ladda ner, skriva ut enstaka kopior för enskilt bruk och att använda det oförändrat för ickekommersiell forskning och för undervisning. Överföring av upphovsrätten vid en senare tidpunkt kan inte upphäva detta tillstånd. All annan användning av dokumentet kräver upphovsmannens medgivande. För att garantera äktheten, säkerheten och tillgängligheten finns det lösningar av teknisk och administrativ art.
Upphovsmannens ideella rätt innefattar rätt att bli nämnd som upphovsman i den omfattning som god sed kräver vid användning av dokumentet på ovan beskrivna sätt samt skydd mot att dokumentet ändras eller presenteras i sådan form eller i sådant sammanhang som är kränkande för upphovsmannens litterära eller konstnärliga anseende eller egenart.
För ytterligare information om Linköping University Electronic Press se förlagets hemsida http://www.ep.liu.se/ In English The publishers will keep this document online on the Internet - or its possible replacement - for a considerable time from the date of publication barring exceptional circumstances.
The online availability of the document implies a permanent permission for anyone to read, to download, to print out single copies for your own use and to use it unchanged for any non-commercial research and educational purpose. Subsequent transfers of copyright cannot revoke this permission. All other uses of the document are conditional on the consent of the copyright owner. The publisher has taken technical and administrative measures to assure authenticity, security and accessibility.
According to intellectual property law the author has the right to be mentioned when his/her work is accessed as described above and to be protected against infringement.
For additional information about the Linköping University Electronic Press and its procedures for publication and for assurance of document integrity, please refer to its WWW home page: http://www.ep.liu.se/ © Christian Lyzell