CRC Press - Robotics and Automation Handbook Episode 1 Part 2 potx

Existence and Uniqueness of Solutions • Notation and NomenclatureThis chapter presents results related to the inverse kinematics problem for robotic manipulators.. As presented elsewhere

sinβ sin γ cosγ 0

sinβ cos γ − sin γ 0

2.4.3 Infinitesimal Rigid-Body Motions

As with pure rotations, the matrix exponential can be used to describe general rigid-body motions [2,14].For “small” motions the matrix exponential description is approximated well when truncated at the firsttwo terms:

Here = − Tand vect() = ω describe the rotational part of the displacement Since the second term

in Equation (2.24) consists mostly of zeros, it is common to extract the information necessary to describethe motion as

This six-dimensional vector is called an infinitessimal screw motion or infinitessimal twist.

Given a homogeneous transform

2-12 Robotics and Automation Handbook

The matrix B is skew-symmetric, and vect(B )= b.

Note that when the rotations are parametrized as R = R(q1, q2, q3) and the translations are parametrized

using Cartesian coordinates b(q4, q5, q6)= [q4, q5, q6]T, one finds that

This chapter has provided a brief overview of rigid-body kinematics A number of excellent textbooks

on kinematics and robotics including [1–4,8,16–18] treat this material in greater depth Other classicworks from a number of fields in which rotations are described include [5,7,9–11] The interested reader

is encouraged to review these materials

[3] Angeles, J., Rational Kinematics, Springer-Verlag, New York, 1988.

[4] Bottema, O and Roth, B., Theoretical Kinematics, Dover Publications, New York, reprinted 1990 [5] Cayley, A., On the motion of rotation of a solid Body, Cam Math J., 3, 224–232, 1843.

[6] Chirikjian, G.S and Kyatkin, A.B., Engineering Applications of Noncommutative Harmonic Analysis,

CRC Press, Boca Raton, 2001

[7] Goldstein, H., Classical Mechanics, 2nd ed., Addison-Wesley, Reading, MA, 1980.

[8] McCarthy, J.M., Introduction to Theoretical Kinematics, MIT Press, Cambridge, MA, 1990 [9] Rooney, J., A survey of representations of spatial rotation about a fixed point, Environ Plann., B4,

185–210, 1977

[10] Shuster, M.D., A survey of attitude representations, J Astron Sci., 41, 4, 439–517, 1993.

[11] Stuelpnagel, J.H., On the parameterization of the three-dimensional rotation group, SIAM Rev., 6,

422–430, 1964

[12] Chasles, M., Note sur les propri´et´es g´en´erales du syst´eme de deux corps semblables entr’eux et plac´esd’une mani`ere quelconque dans l’espace; et sur le d´esplacement fini ou infiniment petit d’un corps

solids libre F´erussac, Bulletin des Sciences Math´ematiques, 14, 321–326, 1830.

[13] Ball, R.S., A Treatise on the Theory of Screws, Cambridge University Press, Cambridge, England,

1900

[14] Brockett, R.W., Robotic manipulators and the product of exponentials formula, in Mathematical Theory of Networks and Systems (A Fuhrman, ed.), 120–129, Springer-Verlag, New York, 1984.

[15] Denavit, J and Hartenberg, R.S., A kinematic notation for lower-pair mechanisms based on matrices,

J Appl Mech., 22, 215–221, June 1955.

[16] Tsai, L.-W., Robot Analysis: The Mechanics of Serial and Parallel Manipulators, John Wiley & Sons,

Existence and Uniqueness of Solutions • Notation and Nomenclature

This chapter presents results related to the inverse kinematics problem for robotic manipulators As presented

elsewhere, the forward kinematics problem of a manipulator is to determine the configuration (positionand orientation) of the end effector of the manipulator as a function of the manipulator’s joint angles.The inverse problem of that, i.e., determining the joint angles given a desired end effector configuration,

is the inverse kinematics problem and the subject of this chapter This chapter will outline and provideexamples for two main categories of approaches to this problem; namely, closed-form analytical methodsand numerical approaches

The main difficulty of the inverse kinematics problem in general is that for some desired end effectorconfiguration, there may be no solutions, there may be a unique solution, or there may be multiplesolutions The advantage of a numerical approach is that it is relatively easy to implement As illustratedsubsequently, however, one drawback is that the method only leads to one solution for one set of startingvalues for what is fundamentally an iterative method Also, if no solutions exist, a numerical approachwill simply fail to converge, so care must be taken to distinguish between an attempted solution that willnever converge and one that is simply slow to converge The advantage of analytical approaches is that allsolutions can be found and if no solutions exist, it will be evident from the computations The disadvantage

is that they are generally algebraically cumbersome and involve many steps and computations Also, closedform solutions only exist for certain categories of manipulators, but fortunately, the kinematics associatedwith the most common manipulators generally seem to belong to the class of solvable systems

FIGURE 3.6 Elbow manipulator.

3.3.1.1 Inverse Kinematics for Two Examples via Subproblems

Consider the schematic illustration of the “Elbow Manipulator” in Figure 3.6 The link frame attachmentsare illustrated in Figure 3.7 With respect to the elbow manipulator, we can make the following observations:

r If0T desis specified for the manipulator in Figure 3.6, generally two values forθ3may be determinedsince the location of the common origin of frames 4, 5, and 6 is given by0T desand the distance from

3-6 Robotics and Automation Handbook

the common origin of each of the 0, 1, and 2 frames to the origins of any of the 4, 5, and 6 frames

Generically, this procedure utilized the two following subproblems:

1 Determining a rotation that produced a specified distance In the example, θ3 determined thedistance from the origins of the 1, 2, and 3 frames to the origins of the 4, 5, and 6 frames andsubsequently,θ2was determined by the height of the origins of the 4, 5, and 6 frames

2 Determining a rotation about a single axis that specified a particular point to be located in a desiredposition In the example,θ1was determined in this manner

Other subproblems are possible as well, such as determining two rotations, which, concatenated together,specify a particular point to be located in a particular position These concepts are presented with fullmathematical rigor in [6] and are further elaborated in [8]

As an additional example, consider the variation on the Stanford manipulator illustrated inFigure 3.8

The assumed frame configurations are illustrated inFigure 3.9.By analogous reasoning, we can computethe inverse kinematic solutions using the following procedure:

r Determine d3(the prismatic joint variable) from the distance between the location of0P 6ORGand

0P 0ORG

r Determineθ2from the height of 0P 6ORG

r Determineθ1from the location of 0P 6ORG

r Determineθ4,θ5, andθ6in the same manner as for the elbow manipulator

The presentation of these subproblems is intended to be a motivational conceptual introduction ratherthan a complete exposition As is clear from the examples, for some manipulators, the inverse kinematicsproblem may be solved using only one or two of the subproblems In contrast, some inverse kinematicsproblems cannot be solved in this manner The following section presents Pieper’s solution, which is amore mathematically complete solution technique, but one based fundamentally on such subproblems

however, this happens to be the case when the manipulator is equipped with a three-axis spherical wristfor joints four through six.

Assuming a six degree of freedom manipulator and assuming that axes four, five, and six intersect, thenthe point of intersection will be the origins of frames 4, 5, and 6 Thus, the problem of solving forθ1,θ2,andθ3simplifies to a three-link position problem, sinceθ4,θ5, andθ6do not affect the position of theircommon origins,0P 4ORG=0P 5ORG=0P 6ORG

Recall that the first three elements in the fourth column of Equation (3.1) give the position of the origin

of frame i expressed in frame i− 1 Thus, from3T ,

sinθ3cosα2 cosθ3cosα2 − sin α2 − sin α2d3

sinθ3sinα2 cosθ3sinα2 cosα2 cosα2d3

f1(θ3)= a2cosθ3+ d4sinα3sinθ3+ a2

f2(θ3)= a3sinθ3cosα2− d4sinα3cosα2cosθ3− d4sinα2cosα3− d3sinα2

f3(θ3)= a3sinα2sinθ3− d4sinα3sinα2cosθ3+ d4 cos α2cosα3+ d3cosα2







If frame 0 is specified in accordance with step four of the Denavit-Hartenberg frame assignment outlined

in Section 3.2.2, then ˆZ0will be parallel to ˆZ1, and henceα0= 0 Also, since the origins of frames 0 and 1 will

be coincident, a0= 0 and d1= 0 Thus, substituting these values into0T and expanding Equation (3.3),

sinθ2cosα1 cosθ icosα1 − sin α1 − sin α1d i

sinθ2sinα1 cosθ2sinα1 cosα1 cosα1d2

g2(θ2,θ3)= sin θ2cosα1f1(θ3)+ cos θ2cosα1f2(θ3)− sin α1f3− d2sinα1 (3.6)

g3(θ2,θ3)= sin θ2sinα1f1(θ3)+ cos θ2sinα1f2(θ3)+ cos α1f3(θ3)+ d2cosα1. (3.7)Hence, multiplying Equation (3.4),

g3(θ2,θ3)1







It is critical to note that the “height” (more specifically, the z coordinate of the center of the spherical

wrist expressed in frame 0) is the third element of the vector in Equation (3.8) and is independent ofθ1.Specifically,

z = sin θ2sinα1f1(θ3)+ cos θ2sinα1f2(θ3)+ cos α1f3(θ3)+ d2cosα1 (3.9)Furthermore, note that the distance from the origin of the 0 and 1 frames to the center of the sphericalwrist will also be independent ofθ1 The square of this distance, denoted by r2is simply the sum of thesquares of the first three elements of the vector in Equation (3.8); namely,

3.3.2.1 Simplifying Case Number 1: a1 = 0

Note that if a1= 0 (this will be the case when axes 1 and 2 intersect), then from Equation (3.10), thedistance from the origins of the 0 and 1 frames to the center of the spherical wrist (which is the origin of

frames 4, 5, and 6) is a function ofθ3only; namely,

r2= f2(θ3)+ f2(θ3)+ f2(θ3)+ a2+ d2+ 2d2f3(θ3) (3.11)Since it is much simpler in a numerical example, the details of the expansion of this expression will

be explored in the specific examples subsequently; however, at this point note that since the f i’s containtrigonometric function ofθ3, there will typically be two values ofθ3that satisfy Equation (3.11) Thus,given a desired configuration,

of the spherical wrist in the 0 frame is given by

Since one or more values ofθ3are known, Equation (3.13) will yield one value forθ2for each value ofθ3.Finally, returning to Equation (3.8), one value ofθ1can be computed for each pair of (θ2,θ3) which havealready been determined

Finding a solution for joints 4, 5, and 6 is much more straightforward First note that3R is determined

3.3.2.2 Simplifying Case Number 2: α1 = 0

Note that ifα1= 0, then, by Equation (3.5), the height of the spherical wrist center in the 0 frame will be

g3(θ2,θ3)= sin θ2sinα1f1(θ3)+ cos θ2sinα1f2(θ3)+ cos α1f3(θ3)+ d2cosα1

g3(θ3)= f3(θ3)+ d2,

so typically, two values can be determined forθ3 Then Equation (3.10), which represents the distancefrom the origin of the 0 and 1 frames to the spherical wrist center, is used to determine one value forθ2.Finally, returning to Equation (3.8) and considering the first two equations expressed in the system, onevalue ofθ1can be computed for each pair of (θ2,θ3) which have already been determined

3.3.2.3 General Case when a1= 0 and α1= 0

This case is slightly more difficult and less intuitive, but it is possible to combine Equation (3.7) andEquation (3.11) to eliminate theθ2dependence and obtain a fourth degree equation inθ3 For a few moredetails regarding this more complicated case, the reader is referred to [1]

3-12 Robotics and Automation Handbook

At this point, we have four combination of (θ2,θ3) solutions; namely

Then the six equations in Equation (3.20) may be, for example

3-16 Robotics and Automation Handbook

system can be utilized In this case

value ofθ iif the value ofθ ibecomes less than−π or greater than π, respectively Finally, implementing

the program for the same PUMA 560 example from Section 3.3.3, the following represents the iterativeevolution of theθ values for a typical program run where the initial conditions were picked randomly:

Iteration theta1 theta2 theta3 theta4 theta5 theta6

θ1− θ0 ≤ b and let

abc≤12

then

1 θ i defined by Equation (3.22) are uniquely defined as

θ n − θ0 ≤ 2b and

2 the iterates converge to some vector, α for which g(α) = 0 and

θ n − θ0 ≤ 2b

2n

Appendix B: Implementation of Newton’s Method

This appendix presents C code that implements Newton’s method for a six degree of freedom manipulator

It assumes that all the joints are revolute, i.e., the joint angles,θ i, are the variables to be determined It hasbeen written not with the goal of complete robustness or efficiency, but rather for a (hopefully) optimal

combination of readability, robustness, and efficiency, with emphasis on readability Most of the variables

that one may need to tweak are contained in the header file

The main file is “inversekinematics.c” which utilizes Newton’s method to numerically find a solution tothe inverse kinematics problem This file reads the Denavit-Hartenberg parameters for the manipulatorfrom the file “dh.dat,” reads the desired configuration for the sixth frame from the file “Tdes.dat,” readsthe initial values from the file “theta.dat.” The other files are:

r “computejacobian.c” which numerically approximates the Jacobian by individually varying thejoint angles and computing a finite approximation of each partial derivative;

r “forwardkinematics.c” which computes the forward homogeneous transformation matrix usingthe Denavit-Hartenberg parameters (including the joint angles,θ i);

r “homogeneoustransformation.c” which multiplies the six forward transformation matrices to termine the overall homogeneous transformation;

de-r “matrixinverse.c” which inverts a matrix;

r “matrixproduct.c” which multiplies two matrices;

r “dh.dat” which contains the Denavit-Hartenberg parametersα i−1, a i−1, and d i;

r “theta.dat” which contains the initial values for the last Denavit-Hartenberg parameter,θ i; and,

r “inversekinematics.h” which is a header file for the various C files.

On a Unix machine, move all the files to the same directory and compile the program, by typing

> gcc *.c -lm

at a command prompt To execute the program type

* This program numerically solves the inverse kinematics problem for

* an n degree of freedom robotic manipulator

*

* It reads the Denavit-Hartenberg parameters stored in a file named

* "dh.dat" The format of "dh.dat" is:

* The number of degrees of freedom is determined by the number of

* rows in "dh.dat" For this program, it is assumed that the number

* of degrees of freedom is six

*

* This program reads the desired configuration from the file

* "Tdes.dat" and stores it in the matrix Tdes[][]

*

* This program reads the initial values for Newton's iteration from

* the file "theta.dat" and stores them in the array theta[] The

* initial values are in degrees

*

* The convergence criterion is that the sum of the squares of the

* change in joint variables between two successive iterations is less

* that EPS, which is set in "inversekinematics.h"

*

* This program assumes that the d_i are fixed and the joint variables

* are the theta_i

*

* This program assumes a 6-by-6 Jacobian, where n is the number of

* degrees of freedom for the system The six elements utilized in

* the homogeneous transformation are the first three elements of the

* fourth column (the position of the origin of the sixth frame and

* elements (2,3), (3,3), and (3,2) of the rotation submatrix of the

2- < /b > 12 Robotics and Automation Handbook< /i>

The matrix B is skew-symmetric, and vect(B )= b.

Note... Equation (3 .20 ) may be, for example