Kinematics of Robots
Alba Perez Gracia c Draft date September 9, 2011 24 Chapter 2
Robot Kinematics Using Matrix Algebra
2.1 Overview
In Chapter 1 we characterized the motion using vector and matrix algebra. In this chapter we introduce the application of 4 × 4 homogeneous transforms in traditional robot analysis, where the motion of a robot is described as a 4 × 4 homogoeneous matrix representing the position of the end-effector. This matrix is constructed as the composition of a series of local displacements along the chain.
In this chapter we will learn how to calculate the forward and inverse kinematics of serial robots by using the Denavit-Hartenberg parameters to create the 4 × 4 homogeneous matrices.
2.2 Robot Kinematics
From a kinematics point of view, a robot can be defined as a mechanical system that can be programmed to perform a number of tasks involving movement under automatic control. The main characteristic of a robot is its capability of movement in a six-dimensional space that includes translational and rotational coordinates.
We model any mechanical system, including robots, as a series of rigid links connected by joints. The joints restrict the relative movement of adjacent links, and are generally powered and equipped with systems to sense this movement.
25 26 CHAPTER 2. ROBOT KINEMATICS USING MATRIX ALGEBRA
The degrees of freedom of the robot, also called mobility, are defined as the number of independent parameters needed to specify the positions of all members of the system relative to a base frame. The most commonly used criteria to find the mobility of mechanisms is the Kutzbach-Gruebler formula. For a robot with n links (counting the base) and j joints, in which each joint i allows fi degrees of freedom, we compute the mobility as j X M = 6(n − 1) − (6 − fi). (2.1) i=1
It is important to remark that this formula does not give the correct answer for several types of closed, or parallel, robots. For a serial robot, the degrees of freedom are equal to the number of joints multiplied by the mobility allowed by each joint.
The mobility indicates how many of the joints of the robot need to be actuated, also called active joints. In a serial robot, all joints are active.
Once the mobility is defined and the active joints are identified, the two main problems of kinematic analysis of robots are the direct kinematics and the inverse kinematics. In the direct kinematics, also called forward kinematics, we define the position of the end-effector as a matrix which is a function of the angles or slides at each joint. If the joint variables are known, the position of the end-effector is completely specified. The inverse kinematics problem consists of finding the joint angles to reach a given position. For serial robots, the inverse kinematics problem is harder than the forward kinematics, while it is the opposite for parallel robots.
The solutions of the direct kinematics define the workspace of the robot. This can be defined as the set of all possible positions of the end-effector, constructed using all possible values of the joint variables within their range. Remember that position means location plus orientation; the workspace of the robot is a six-dimensional subset of the six-dimensional space of rotations and translations.
Because of the difficulty in visualizing the workspace, several subspaces have been defined [13].
• Reachable workspace (WR): Set of all locations of the origin of the end-effector frame that the robot can reach. It is a three-dimensional subset of the workspace.
• Dextrous workspace (WD): Set of all locations of the origin of the end-effector frame that the robot can reach with any orientation. It is useful because the robot has full dexterity in this subspace, which ensures that any task can be performed within it.
• Workspace with constant orientation (Wθ): Set of all locations of the origin of the end-effector frame that the robot can reach with a specified orientation for the end-effector. 2.3. BASIC JOINTS 27
Notice that [ \ Wθ = WR , Wθ = WD (2.2) θ∈range θ∈range
Figure 2.1 shows the typical sketch of the reachable workspace for a commercial robot.
Figure 2.1: Reachable workspace, Adept Viper m650 robot (Adept Technologies, Inc.)
2.3 Basic joints
Releaux [19] identified six basic types of joints that continuously share a surface of contact, that he called lower pairs. His classification is still used today in most robotics and mechanisms books. He defined revolute (R), cylindric (C), prismatic (P), spherical (S), helical (H) and plane (E) joints. In our research, we only consider the basic types of revolute and prismatic joints; any of the others can be obtained by combining the two basic types with certain additional constraints. In addition to these, we refer to the universal joint (T), which can be modeled as two revolute joints at right angles. See Figure 2.2.
The revolute joint is a one-degree-of-freedom joint that allows a rotation of angle θ about an arbitrary located joint axis G = g + g0. The prismatic joint is also a one-degree-of-freedom joint, which allows translation along the direction h of the axis H. It is interesting to notice that the location of the prismatic axis does not matter for the movement of the prismatic joint. The cylindric joint is a general screw motion, in which the rotation about and the translation along the screw axis S are nonzero and independent. It can be constructed as the composition of a revolute joint 28 CHAPTER 2. ROBOT KINEMATICS USING MATRIX ALGEBRA
Figure 2.2: Common types of joints and a prismatic joint with same direction. In a similar fashion, the universal (T) joint and the spherical (S) joint can be constructed from individual rotations.
2.4 Forward Kinematics Equations
Following the traditional matrix kinematics equations, we will represent local transformations from joint to joint using 4 × 4 homogeneous matrices. The composition of these displacements will give as a result the displacement from the base to the end-effector.
Angles will be measured usually taking the ”extended” position as the origin. However, this is an arbitrary choice and any reference configuration can be used as the zero for the joint variables.
In order to create the local transformations along the chain, we identify the lines locating the joint axes, Si, and we compute the common normal lines Ai,i+1 between joint Si and joint Si+1. We consider that the link joining two consecutive joints extends along the common normal line, regardless of its real shape.
We assign local coordinate frames with the Z axis along the joint axis and the X axis along the com- mon normal line. Local displacements will take us from local coordinate frame to local coordinate frame, from the base to the end-effector, see Figure 2.3.
2.4.1 Denavit-Hartenberg parameters
In 1955, Denavit and Hartenberg introduced the methodology of 4 × 4 homogeneous matrix trans- formations to analyze robotic systems, which became a standard tool in robotics. They shaped the local transformations along the chain as screw displacements about the Z and X axes, de- pendent on only four parameters. With the pass of the years, slightly different versions of the Denavit-Hartenberg parameters have been developed. Our notation is similar to Craig [11]. 2.4. FORWARD KINEMATICS EQUATIONS 29
Figure 2.3: Local transformations along the links of a robot.
We locate the first local frame with the Z axis along the first joint axis S1 and the X axis along the common normal A12; the origin of the frame is located at the intersection of these two lines. Assume that a general displacement [G] locates this first local frame with respect to the fixed frame, as shown in Figure 2.3.
The second local frame has the same X axis but now the Z axis is aligned with S2, with its origin at the intersection point of these two lines, while the third local frame has same Z axis as the second and the line A23 as its X axis. From the local frame attached to the last joint axis to the moving frame attached to the gripper we define a general displacement [H].
Only four parameters are needed to transform from local frame to local frame: two correspond to the transformation along the link, which is an X-screw displacement, and two more to perform a Z-screw displacement about the joint. These parameters are:
• Twist angle αi−1,i: Angle between joint axes Si−1 and Si measured about the common normal
line Ai−1,i.
• Link length ai−1,i: Distance between joint axes Si−1 and Si measured along the common
normal line Ai−1,i.
• Joint angle θi: Angle between previous common normal line Ai−1,i and next common normal
line Ai,i+1, measured about joint axes Si. Notice that if joint Si is a revolute joint, the joint angle will include the variable value of the joint rotation. 30 CHAPTER 2. ROBOT KINEMATICS USING MATRIX ALGEBRA
Joint αi−1,i ai−1,i di θi
1 0 0 d1 θ1
2 α12 a12 d2 θ2
3 α23 a23 d3 θ3
Table 2.1: Denavit-Hartenberg parameters.
• Offset di: Distance between previous common normal line Ai−1,i and next common normal
line Ai,i+1, measured along joint axes Si. Notice that if joint Si is a prismatic joint, the offset will include the variable value of the slide.
The Denavit-Hartenberg parameters describe in a simple way the geometry of the robot. They are usually arranged on a table. For the robot of Figure 2.3 we create the corresponding Table 2.1, where we have not specified whether the joints are revolute or prismatic.
The D-H parameters are used to create the screw displacements,
1 0 0 ai−1,i cos θi − sin θi 0 0 0 cos αi−1,i − sin αi−1,i 0 sin θi cos θi 0 0 [X(αi−1,i, ai−1,i)] = [Z(θi, di)] = . 0 sin α cos α 0 0 0 1 d i−1,i i−1,i i 0 0 0 1 0 0 0 1 (2.3)
2.4.2 Kinematics Equations
The local screw displacements created using the D-H parameters, together with the initial and final displacements [G] and [H], are multiplied together to obtain the displacement from the fixed frame to the frame attached to the end-effector,
[D] = [G][X(α01, a01)][Z(θ1, d1)][X(α12, a12)][Z(θ2, d2)] ... [X(αn−1,n, an−1,n)][Z(θn, dn)][H]. (2.4)
The 4 × 4 transform [D] is called the forward kinematics or simply the kinematics equations. It provides with all possible positions of the end-effector as a function of the joint variables. The kinematics equations solve the direct kinematics problem; for a given set of joint variables, the position of the end-effector is immediately calculated. 2.5. FORWARD KINEMATICS EXAMPLE 31
2.5 Forward Kinematics Example
Let us apply the technique described above to calculate the forward kinematics equations of the robot presented in Figure 2.4.
S1(q1)
{F} {M}
S (q ) S2(q2) 3 3
Figure 2.4: Example: a three-jointed serial robot.
The transformation from the fixed frame to the first local frame is a translation along the Z axis, given by the matrix 1 0 0 0 0 1 0 0 [G] = (2.5) 0 0 1 80 0 0 0 1
The local transformations from the first joint axis to the last can be calculated using the Denavit- Hartenberg parameters of the robot, presented in Table 2.2.
Joint αi−1,i ai−1,i di θi
1 0 0 0 θ1
2 π/2 70 0 θ2
3 0 65 0 θ3
Table 2.2: Denavit-Hartenberg parameters for the robot of Figure 2.4. 32 CHAPTER 2. ROBOT KINEMATICS USING MATRIX ALGEBRA
The screw displacements about the X axis and Z axis are created using the rows of Table 2.2, cos θ1 − sin θ1 0 0 1 0 0 70 sin θ1 cos θ1 0 0 0 0 −1 0 [Z1] = [Z(θ1, 0)] = , [X12] = [X(π/2, 70)] = , 0 0 1 0 0 1 0 0 0 0 0 1 0 0 0 1 cos θ2 − sin θ2 0 0 1 0 0 65 sin θ2 cos θ2 0 0 0 1 0 0 [Z2] = [Z(θ2, 0)] = , [X23] = [X(0, 65)] = , 0 0 1 0 0 0 1 0 0 0 0 1 0 0 0 1 cos θ3 − sin θ3 0 0 sin θ3 cos θ3 0 0 [Z3] = [Z(θ3, 0)] = . (2.6) 0 0 1 0 0 0 0 1
The last local displacement, from the third joint axis to the end-effector frame, is calculated to be 1 0 0 35 0 0 1 −15 [H] = . (2.7) 0 −1 0 0 0 0 0 1
The kinematics equations are constructed by multiplying the local transformations,
[D] = [G][Z1][X12][Z2][X23][Z3][H] = cos θ1 cos(θ2 + θ3) − sin θ1 − cos θ1 sin(θ2 + θ3) 5 cos θ1(14 + 13 cos θ2 + 7 cos(θ2 + θ3) + 3 sin(θ2 + θ3) sin θ cos(θ + θ ) cos θ − sin θ sin(θ + θ ) 5 sin θ (14 + 13 cos θ + 7 cos(θ + θ ) + 3 sin(θ + θ ) 1 2 3 1 1 2 3 1 2 2 3 2 3 sin(θ2 + θ3) 0 cos(θ2 + θ3) 80 − 15 cos(θ2 + θ3) + 65 sin θ2 + 35 sin(θ2 + θ3) 0 0 0 1 (2.8)
2.6 Inverse Kinematics
The inverse kinematics consists of finding expressions for the joint variables as a function of the desired position of the end-effector. Given the matrix formulation for the kinematics equations, usually these formulas are obtained by equating elements of the matrix to the elements of the desired task position and manipulating the expressions. For a good collection of results, see [24].
In general, for a robot with n joints and joint variables qi, i = 1 . . . n, we state the inverse kinematics 2.7. INVERSE KINEMATICS EXAMPLE 33 problem by equating the kinematics equations [D] to a desired position of the end-effector [P ],
[D(q1, q2, . . . qn)] = [P ]. (2.9)
There is no general formulation to solve for the joint variables in this equation; each case presents different techniques to isolate the joint variables qi.
Most of the times we obtain several solutions. For robots with more than six degrees of freedom, and for some lower-dof robots, we may obtain infinite solutions, and additional strategies must be used to decide the best set of joint variables.
2.7 Inverse Kinematics Example
To illustrate the somewhat vague description of previous section, we will solve the inverse kinematics for the example presented in Section 2.5.
Take the matrix from Eq.(2.8) and equate it to a desired task position [P ] = [pij]. Notice that we can solve for θ1 directly in elements (1, 2) and (2, 2); we obtain
θ1 = arctan(−p12, p22). (2.10)
Next we can solve for the sum of the angles θ2 + θ3 for instance using elements (3, 1) and (3, 3). Again we obtain
θ2 + θ3 = arctan(p31, p33). (2.11)
Finally, we can solve for θ2 in elements (2, 4) and (3, 4),
1 1 p24 θ2 = arctan( (p34 − 80 + 15p33 − 35p31), − ( + 14 + 7p33 − 3p31)), (2.12) 65 13 5p12 and θ3 = (θ2 + θ3) − θ2. In this case there are no multiple solutions and the solution is fairly easy to obtain. 34 CHAPTER 2. ROBOT KINEMATICS USING MATRIX ALGEBRA
Homework 2
Chapter 2
Use Maple to create the forward kinematics equations of the robot shown in Figure 2.5. Take the given configuration of the robot as the reference configuration.
Figure 2.5: Welding Robot (Fanuc Robotics)
1. Identify the joint axes and the common normal lines on the drawing or sketch.
2. Write the Pl¨ucker coordinates of the joint axes.
3. Create the D-H table.
4. Indicate the location of the world frame and the end-effector frame and construct the first and last transformations [G] and [H]. 2.7. INVERSE KINEMATICS EXAMPLE 35
5. Write the matrices of the local transformations and multiply them together to obtain the forward kinematics matrix.
6. What are the coordinates of the origin of the end-effector frame as a function of the joint variables?
7. Draw the trajectory of the origin of the end-effector frame for the following sequence of joint o o o o o o values: θ1 from 0 to 90 , θ2, θ3 from 0 to 45 , θ4, θ5 and θ6 from 0 to 180 .
8. (Optional): Create a simulation of the robot using Maple. Bibliography
[1] Ablamowicz, R. and Sobczyk, G., Lectures on Clifford (Geometric) Algebras and Applicatons, Birkhauser, Boston 2004.
[2] Artin, M., Algebra, Prentice-Hall, New Jersey, 1991.
[3] Bayro Corrochano, E., Daniilidis, K., and Sommer, G., “Motor Algebra for 3D Kinematics: The Case of Hand-Eye Calibration”, Journal of Mathematical Imaging and Vision, 13:79-100, 2000.
[4] Bayro Corrochano, E., Geometric Computing for Perception Action Systems, Springer-Verlag, New York, 2001.
[5] Bayro Corrochano, E., Handbook of Geometric Computing, Springer-Verlag, Berlin, 2005.
[6] Bayro-Corrochano, E., and Zamora-Esquivel, J., “Differential and Inverse Kinematics of Robot Devices Using Conformal Geometric Algebra”, Robotica, 25:43-61, 2007.
[7] Bottema, O., and Roth, B., Theoretical Kinematics, North Holland Press, NY, 1979.
[8] Browne, J., Grassmann Algebra: Exploring applications of extended vector algebra with Mathematica, Swinburne University of Technology, Melbourne, Australia, 2001. (Draft from http://www.ses.swin.edu.au/homes/browne/grassmannalgebra/book/index.htm).
[9] Cohn, P.M., Algebra, 2nd. edition, John Wiley & Sons, 1989.
[10] Ge, Q.J., 2002, “Projective convexity in computational kinematic geometry”, Proceedings of the ASME Design Engineering Technical Conference, pages 715-723, Montreal, Canada, 2002.
[11] Craig, J. J., Introduction to Robotics, Mechanics and Control, Addison Wesley Publ. Co., 1989.
[12] Gosselin, C.M., St-Pierre, E. and Gagne, M., 1996, “On the development of the agile eye: me- chanical design, control issues and experimentation”, IEEE Robotics and Automation Society Magazine, 3(4):29-37.
[13] Kumar, A., and Waldron, K.J., 1981, “The Workspace of a Mechanical Manipulator,” ASME J. Mech. Design, 103: 665-672.
73 74 BIBLIOGRAPHY
[14] McCarthy, j.m., Introduction to Theoretical Kinematics, The MIT Press, 1990.
[15] McCarthy, J.M., Geometric Design of Linkages, Springer, New York 2000.
[16] Murray, R., Li, Z. and Sastry, S., A Mathematical Introduction to Robotic Manipulation, CRC Press, Boca Raton, 1994.
[17] Perez, A. and McCarthy, J. M., 2005, “Clifford Algebra Exponentials and Planar Linkage Synthesis Equations”,ASME Journal of Mechanical Design, 127(5): 931-940.
[18] Porteous, I. R.,Clifford Algebras and the Classical Groups, Cambridge Studies on Advanced Mathematics, 50, Cambridge University Press, Cambridge, 1995.
[19] Releaux, F., 1875, The Kinematics of Machinery: Outlines of a Theory of Machines, Dover Publications, New York, translation of 1963.
[20] Selig, J.M., Geometrical Methods in Robotics, Springer, London 1996.
[21] Selig, J.M., Geometric Fundamentals of Robotics, Springer, London, 2005.
[22] Sommer, G., Geometric Computing with Clifford Algebras, Chapter1, Springer, Berlin, 2001.
[23] Sugimoto, K., 1987, “Kinematic and Dynamic Analysis of Parallel Manipulators by Means of Motor Algebra”, ASME Journal of Mechanisms, Transmissions and Automation in Design, 109(1):3-5.
[24] Tsai, L. W., 1999, Robot Analysis: The Mechanics of Serial and Parallel Manipulators, John Wiley and Sons, New York, NY.