Iterative Inverse Kinematics with Manipulator Configuration Control and Proof of Convergence
Gregory Z. Grudid
B.A.Sc. University of British Columbia, 1987
A THESIS SUBMITTED IN PARTIAL FULFILLMENT OF
THE REQUIREMENTS FOR THE DEGREE OF
MASTER OF APPUED SCIENCE
in
THE FACULTY OF GRADUATE STUDIES
DEPARTMENT OF ELECTRICAL ENGINEERING
We accept this thesis as conforming
to the required standard
THE UNIVERSITY OF BRITISH COLUMBIA
August 1990
© Gregory Z. Grudid, 1990 in presenting this thesis in partial fulfilment of the requirements for an advanced degree at the University of British Columbia, 1 agree that the Library shall make it freely available for reference and study. 1 further agree that permission for extensive copying of this thesis for scholarly purposes may be granted by the head of my department or by his or her representatives. It is understood that copying or publication of this thesis for financial gain shall not be allowed without my written permission.
(Signature)
Department of
The University of British Columbia Vancouver, Canada
Date Aua. 20, 1110.
DE-6 (2/88) Abstract
A complete solution to the inverse kinematics problem for a large class of practical manipulators, which includes manipulators with no closed form inverse kinematics equations, is presented in this thesis. A complete solution to the inverse kinematics problem of a manipulator is defined as a method for obtaining the required joint variable values to establish the desired endpoint position, endpoint orientation, and manipulator configuration; the only requirement being that the desired solution
exists. For all manipulator geometries that satisfy a set of conditions (THEOREM I), an algorithm
is presented that is theoretically guaranteed to always converge to the desired solution (if it exists).
The algorithm is extensively tested on two complex 6 degree of freedom manipulators which have no
known closed form inverse kinematics equations. It is shown that the algorithm can be used in real
time manipulator control. Applications of the method to other 6 DOF manipulator geometries and to
redundant manipulators are discussed.
ii Table of Contents
Abstract 11
List of Tables v
List of Figures vi
Acknowledgments vii
1 Introduction 1 1.1 Current Approaches to the Inverse Kinematics Problem 3 1.2 Thesis Organization 8
2 The Theoretical Development 9 2.1 Summary of the Approach 9 2.2 Forward Kinematics Representation : 9
2.3 The Theoretical Framework 11 2.4 Application to 6 DOF Manipulators with Non-Intersecting, Non-Parallel Rotational Wrist Joints 14
3 The Nonlinear Equation Solver 16 3.1 The Main Algorithm 17 3.2 The Fixed Point Algorithm 18 3.3 The Search Algorithm 20 3.4 The Modified Powell's Algorithm 24 3.5 Numerical Approximation of the Jacobian 28
4 Numerical Examples " 31 4.1 The Spherical Manipulator 31 4.1.1 Evaluation of the Algorithm Over 100,000 Uniformly Generated Random Points . 35 4.1.2 Rate of Convergence Comparison with an Existing Algorithm 37
iii 4.2 The Modified PUMA Manipulator 39 4.2.1 Evaluation of the Algorithm Over 1,000,000 Uniformly Generated Random Points 4.2.2 Evaluation of the Algorithm On Various Wrist Offset Magnitudes 44
47 5 Discussion 5.1 Analysis of Results 47 5.2 Improving Convergence Near Singularity Regions and Workspace Boundaries 48 5.3 Application to Other Manipulator Geometries 48 5.4 Application to Redundant Manipulators 49
6 Conclusions 51 6.1 Contributions 51 6.2 Further Research
References
Appendix A Proof of Theorems 56
Appendix B Pseudo-code Descriptions of the Procedure and Algorithms 65
Appendix C The Inverse Kinematics Equations of the Spherical Model Manipulator 76
Appendix D The Inverse Kinematics Equations of the Modified PUMA Model Manipulator 78
iv List of Tables
1.1 Summary of Current Inverse Kinematics Methods 5
4.1 The Denavit-Hartenberg Parameters of the "Real" Spherical Manipulator 33
4.2 The Denavit-Hartenberg Parameters of the "Model" Spherical Manipulator 33
4.3 Computational Requirements of The Spherical Manipulator 37
4.4 The Denavit-Hartenberg Parameters of the "Real" Modified PUMA Manipulator 40
4.5 The Denavit-Hartenberg Parameters of the "Model" Modified PUMA Manipulator .... 40
4.6 Simulation Results of the Modified PUMA Manipulator 43
4.7 Computational Requirements for the Modified PUMA Manipulator 44
4.8 Simulation Results on Various Wrist Offset Magnitudes: Analytical Jacobian 45
4.9 Simulation Results on Various Wrist Offset Magnitudes: Numerically Approximated
Jacobian ^
V List of Figures
1.1 The Modified PUMA Manipulator
1.2 Manipulator Configurations
3.1 Hierarchy of Algorithms
3.2 Search Grid 4.1 The Spherical Manipulator
A.l Relative Positioning of Manipulator Endpoint Positions
vi Acknowledgments
Foremost, I would like to thank my supervisor, Dr. Lawrence, for his continued support throughout the course of my thesis. Many of the ideas presented in this thesis have resulted directiy from discussions with him.
I would also like to thank Jane Mulligan, Kevin O'Donnell, and The Iceman for proof reading earlier versions of thesis.
vii Chapter 1: Introduction
Chapter 1 Introduction
A complete solution to the inverse kinematics problem for a robot manipulator, as defined in this thesis, is a method of obtaining the required manipulator joint variable values for any desired endpoint position, endpoint orientation (see Figure 1.1), and manipulator configuration (i.e. elbow up/elbow
down, right arm/left arm, etc.; see Figure 1.2); the only requirement being that the desired solution
exists. For 6 degree of freedom (DOF) manipulators, a complete solution to the inverse kinematics
problem is known when any three adjacent joints of the manipulator are either intersecting [1] or
parallel to one another [2]. Because of this, most industrial manipulators are built with the last
three joints having intersecting axes. Most 6 DOF manipulators which fall into this restricted class
have closed form inverse kinematics equations. Many iterative algorithms have been developed
which attempt to solve the complete inverse kinematics problem for manipulators with no closed
form inverse kinematics equations. The author is not aware of any existing algorithms which can
theoretically guarantee a complete solution to this problem.
In this thesis, an iterative method is presented which gives a complete solution to the inverse
kinematics problem for a large class of manipulators that may or may not have closed form inverse
kinematics equations. The method is applicable to any manipulator geometry that satisfies a specific
set of conditions (the conditions of THEOREM I), and it is shown that these conditions include a
class of 6 DOF manipulators which do not have three adjacent joints that are either intersecting or
parallel. It is advantageous to have a complete solution to the inverse kinematics problem for such
manipulators because, in general, these manipulators are easier to build and maintain, and they can
be made more dextrous [3] than those manipulators which fall into more restrictive classes. Subsea
and heavy equipment manipulators are often built which do not have three adjacent joints that are
1 Chapter 1: Introduction
The inverse kinematics problem is one of calculating the required joint variable values (?i, «•>, • • •, ?o) in order to
achieve the desired endpoint position (pd) and orienting (defined by the 3 unit vectors nd, sd, &d). Note that the desired
endpoint position and orientation can usually be achieved at more than one manipulator configuration (see Figure 1.2).
Figure 1.1: The Modified PUMA Manipulator
either intersecting or parallel. Applications of the method to other 6 DOF manipulator geometries
and to redundant manipulators (manipulators with more than 6 DOF) are also discussed in this thesis.
The method is thoroughly tested on two complex 6 DOF manipulators which have no known
closed form inverse kinematics equations. It is shown that the method never fails and is applicable
to real time manipulator control (defined below).
2 Chapter 1: Introduction
b) Left Arm, Elbow Down. a) Left Arm, Elbow UP.
d) Right Arm, Elbow Down. c) Right Arm, Elbow UP.
Figure 1.2: Manipulator Configurations
1.1 Current Approaches to the Inverse Kinematics Problem
A summary of the characteristics of some current inverse kinematics methods is given Table 1.1.
A brief description of each of these methods is presented here. The characteristics of the inverse
kinematics methods are compared in the following categories:
1. Convergence conditions given. If convergence conditions for a particular inverse kinematics
method are not given, then one can never be sure that the algorithm being used will converge
to the desired solution.
2. Guaranteed convergence if solution exists. Ideally, the inverse kinematics algorithm should
always be able to converge to the desired solution if it exists. Also, the algorithm should be
able to detect when a solution does not exist
3 Chapter 1: Introduction
3. Allows real time manipulator control. It is desirable to have an algorithm which is fast enough
to be used in a real time manipulator control scheme. For the puiposes of this thesis, a loose
definition for an inverse kinematics algorithm that is suitable to a real time control scheme,
is one for which the convergence time in most of the manipulator's workspace (independent
of the initial position of the manipulator) is less than 1 second. The time of 1 second was
chosen in order to meet the real time claims of [5-17], Convergence time is defined as the
required time for the algorithm to converge to the desired solution, to within an accuracy that
is equal to the positioning repeatability accuracy of the manipulator.
4. Allows direct control over manipulator configuration. In order to avoid obstacles in the manip-
ulator's workspace, it is often essential to have control over the manipulator's configuration.
5. Extendable to all 6 DOF manipulators. The algorithm should ideally be applicable to all
practical 6 DOF geometries.
6. Extendable to redundant manipulators. The algorithm should also be applicable to manipu-
lators with more than 6 degrees of freedom.
Tsai and Morgan [4] developed a method which is applicable to general six- and five-DOF
manipulators and which can generate all possible solutions for a desired endpoint position and
orientation. This was done by converting the problem into a system of 8 second order equations
in 8 unknowns and then solving this system for all possible solutions, by continuation methods [25].
Manseur and Doty [5] developed a fast algorithm which can be used with 6 DOF manipulators
that have revolute joints at the first and last links, and for which one is able to derive closed form
equations for joint variables 2 through 6 as a function of joint variable 1. Under these conditions, it is
shown that a single nonlinear equation can be generated with the only unknown being joint variable
1. This equation is then solved using the Newton-Raphson method.
Tourassis and Ang [6] formulated a fast method which is applicable to general 6 DOF geometries.
The method is based on creating a system of 3 nonlinear equations in 3 unknowns using the known
4 Chapter 1: Introduction
Table 1.1: Summary of Current Inverse Kinematics Methods
[7][8] [18][19]
[9] [10] [20] [21] Reference —»• [4] [5] [6] [14] [15] [16] [17] [11][12] [22] [23]
[13] [24]
Convergence No No Yes No No No No No No conditions given. Guaranteed
convergence if No No No No No No No No No
solution exists. Allows real time
manipulator No Yes Yes Yes Yes Yes Yes Yes Yes
control. Allows direct
control over Yes No Yes No No No No No Yes
configuration. Extendable to all
6 DOF Yes No Yes Yes Yes Yes Yes No Yes
manipulators. Extendable to
redundant ? ? ? Yes Yes Yes Yes Yes Yes
manipulators.
forward kinematics and inverse kinematics of the first three joints and the last three joints separately.
Fixed Point Theory [26] is used to solve for the 3 unknowns which represent the Cartesian position
of the wrist Conditions under which the method will converge are given and the desired manipulator
configuration can be easily specified. The algorithm is also applicable for real time use.
5 Chapter 1: Introduction
Most other methods which have been extensively used to solve the inverse kinematics problem are based on Newton-type algorithms which are be used to solve systems of nonlinear equations
[7][8][9][10][11][12][13]. The nonlinear equations are usually derived from expressions of the type where the desired endpoint position and orientation are functions of the manipulator's variable
parameters. The method works by linearizing the system of equations using a Taylor's series
expansion around the manipulator's initial position (i.e. calculating the Jacobian of the forward
kinematics equations). This gives a system of linear equations which, when solved, give an
approximation of the joint variables which will give the desired endpoint position and orientation.
Newton-type methods are generally applicable to all manipulator geometries, including redundant
manipulators, and are usually suitable for real time use.
Some Newton-based algorithms use damped least-squares methods to compensate for problems
that are associated with Jacobian singularities [8][27][28][29][30][31]. Nakamura [11] made the
damping factor variable by increasing it as the manipulator approached a Jacobian singularity region.
Chan [12] made the damping factor a function of residual error and showed that this gave good inverse
kinematics control of manipulators near singularity regions. Hutchinson [13] showed that a least-
squares approximated Jacobian used in a Newton-Raphson type algorithm has similar convergence
properties to the damped least-squares method.
Novakovi<5 and Nemec [14] developed an algorithm based on sliding mode control and Lyapunov
theory. The algorithm is tested on a 4 axis manipulator which has no known closed form solutions
to the inverse kinematics problem. The method is shown to be computationally more efficient than
Newton-type methods and is not prone to singularity problems.
Powell [32] developed a hybrid algorithm for solving systems of linear equations which uses both
the Newton-Raphson and the gradient descent methods. The combination of these three algorithms
give Powell's hybrid algorithm, in general, better convergence properties than most other nonlinear
6 Chapter 1: Introduction equation solving algorithms. Powell's algorithm was applied to the inverse kinematics problem by
Goldenbeig [15]. This inverse kinematics formulation is applicable to general manipulator geometries,
allows real time control, and has better convergence properties then methods that are based solely
on the Newton-Raphson approach.
Another approach to the inverse kinematics problem involves the integration of joint velocities
which are obtained using inverse Jacobian techniques. Tsai and Orin [16] used a variation of this
approach along with a special-purpose inverse kinematics processor to obtain a system that works
in real time. This method has good convergence properties in that it usually only fails to converge
at manipulator singularity regions.
Poon [17] used a novel iterative inverse kinematics approach where the function of each joint
variable is divided into either endpoint orientation or position control. At each iteration, the joint
variables are updated based on the error associated with the functions that each joint is intended to
control. The algorithm can be implemented in parallel and is therefore fast.
Other inverse kinematics approaches use a functional approximation approach to formulate
functions that approximate the manipulator's inverse kinematics functions [33]. In most functional
approximation methods, the inverse kinematics functions are learned by repeatedly moving the
manipulator throughout its workspace, while observing the joint variables values as a function of
the endpoint position, endpoint orientation, and manipulator configuratioa Albus [18] and Marr [19]
used a look-up table approach which is based on the human cerebellum. Miller [20] has recently
applied these methods to real industrial manipulators. The disadvantage of look-up table functional
approximators is that they require large amounts of memory in order to obtain satisfactory positioning
accuracy (the memory requirements increase exponentially with the number of dimensions of the
function being approximated).
Klett [21] and Huscroft [22] used multidimensional polynomials (multinomials) to approximate
7 Chapter 1: Introduction inverse kinematics functions. The disadvantage of this approach is that inverse kinematics functions contain discontinuities, and therefore an infinite number of polynomial terms are required to approx- imate an entire inverse kinematics function (using a single multinomial) at any given manipulator configuration.
Neural Networks have also been used to approximate inverse kinematics functions [23] [24].
The basic problem with using Neural Networks is that no theoretical framework exists on the type
of network architectures required to approximate inverse kinematics functions. Because of this,
acceptable positioning accuracy throughout the entire workspace of a given manipulator has yet to
be demonstrated using Neural Networks.
None of the above methods guarantee convergence if the desired solution exists and only one
of them gives convergence conditions [6], Also, the three types of methods that allow direct control
over manipulator configuration are either too slow for real time use [4], are not easily extendable
to redundant manipulators [6], or have poor accuracy [ 18][ 19][20][21 ][22][23][24]. The inverse
kinematics approach presented in this thesis is intended to resolve these difficulties.
1.2 Thesis Organization
In Chapter 2, a new theoretical framework for solving the inverse kinematics problem is given. In
Chapter 3, a numerical inverse kinematics method, based on the theory developed in Chapter 2, is
presented. In Chapter 4, the method is applied to two 6 DOF manipulators that have no known
closed form inverse kinematics equations. In Chapter 5, the simulation results are discussed along
with the application of the method to other manipulator geometries and to redundant manipulators.
The conclusions of the thesis are given in Chapter 6.
8 Chapter 2: The Theoretical Development
Chapter 2 The Theoretical Development
2.1 Summary of the Approach
The manipulator for which the inverse kinematics solution is required is termed the real manipulator.
A model manipulator is selected which only differs from the real manipulator by the magnitude of the link lengths (i.e. the values of the Denavit-Hartenberg parameters [34] a, and d{). There must
exist a complete solution to the inverse kinematics problem of the model manipulator. For example,
for a 6 DOF real manipulator which does not have a complete inverse kinematics solution, one can
select a model manipulator (which has a complete solution to its inverse kinematics problem) by
setting three consecutive link lengths (a, and d,) to zero [1]. The inverse kinematics solution of the
model manipulator and the difference between the forward kinematics of the model manipulator and
the real manipulator, are used to construct a system of 3 equations in 3 unknowns. The solution of
this system of equations yields a complete solution to the inverse kinematics problem for the real
manipulator. An algorithm for solving this system of equations is introduced in Chapter 3. If the
desired solution to the inverse kinematics problem is within the real manipulator's workspace, the
algorithm is theoretically guaranteed to find this solution to within a specified maximum endpoint
position error. If the desired solution does not exist, then the algorithm will stop iterating and set a
flag to indicate that the desired solution is outside the manipulator's workspace.
2.2 Forward Kinematics Representation
The Denavit-Hartenberg representation [34] is used to formulate the forward kinematics equations
for robot manipulators. This representation is based on the four geometric parameters at, dt, at
and 9{. For a complete definition of these parameters see [35, pp. 36^4]. The Denavit-Hartenberg
parameters are used in homogeneous transformation matrices to describe the geometric relationship
between link (t - 1) and link i.
9 Chapter 2: The Theoretical Development
The forward kinematics equations for the manipulators presented in this thesis are given in the form of a single 12 element column vector:
k(q) = [n(q) s(q) a(q) p(q)]T (2.1)
The vectors n = [nx ny nz], s = [sx sy sz] and a = [ax ay az] are orthonormal vectors which uniquely define the orientation of the manipulator's distal link (the hand) and are
respectively termed the normal vector, the sliding vector, and the approach vector of the hand. The
vector p = [px py pz] defines the Cartesian endpoint position of the manipulator. The vector
q = [ qi
the joint variable values of the manipulator. For a prismatic joint i the joint variable is d, (i.e. # - d{),
and for a rotational joint i the joint variable is 0{ (i.e. qt = 0,). The equations which give the vectors
n, s, a and p are derived using homogeneous transformation matrices 0_1A;) [35]: -nT(q) sT(q) aT(q) pT(q)- 3 N 1 : "A/A3 A3 • •• ~ An (2.2) 0 0 0 where, for a rotational joint i " cos 9{ — cos oti sin sin a; sin 0,- cos Oi"
sin Oi cos a; cos - sin cos 6t - cos a,- sin Oi sin a,- sin Oi 0 sin Oi cos ai cos Oi - sin ai cos 9, 0 (2.4) 0 sin a,- cos a,- di 0 0 0 1 The forward kinematics of the real manipulator, referred to by the subscript r, are given by the following equations: xrj = [ nr„ sTi ar, prj ] (2.5) = kr(qr) = [nr(qr) sr(qr) ar(qr) pr(qr)] 10 Chapter 2: The Theoretical Development yiTl is a column vector containing 12 scalars which define the desired endpoint position and orientation of the real manipulator. kr(qr) is a column vector containing the 12 forward kinematics functions of the real manipulator. The Denavit-Hartenberg parameters of the real manipulator are designated by aTi, aTi, dTi, and 9Ti (for i = 1,2, • • • ,iV). The forward kinematics equations of the model manipulator, referred to by the subscript m, are given by iT* = f nm sm am pm J (2.6) = ^(qm) = [nm(qm) sm(qm) am(qm) pm(qm)]T- ^ is a column vector containing 12 scalars which define the endpoint position and orientation of the model manipulator. km(qm) is a column vector containing the 12 forward kinematics functions of the model manipulator. The Denavit-Hartenberg parameters of the model manipulator are designated by amt, ami, dm<, and 0m, (for i = 1,2 23 The Theoretical Framework The complete inverse kinematics solution for the real manipulator is obtained by letting the joint variable values of the real manipulator be equal to those of the model manipulator. This allows one to formulate a set of delta kinematics equations by subtracting the forward kinematics equations of the real manipulator from the forward kinematics equations of the model manipulator. The inverse kinematics solution for the model manipulator and the delta kinematics equations are then used to construct a nonlinear system of 3 equations in 3 unknowns. The numerical solution of this system of equations gives a complete solution to the inverse kinematics problem for the real manipulator. The inverse kinematics of the model manipulator is given by q^k^Hw). (2.7) The vector c contains the desired configuration (i.e. elbow up or elbow down, left arm or right arm, etc) that both the real manipulator and model manipulator are required to have. If the real 11 Chapter 2: The Theoretical Development manipulator is a redundant manipulator, then c must also contain a enough variables to completely specify all redundancies. Note that (2.7) simply represents a theoretically guaranteed method for finding the complete inverse kinematics solution for the model manipulator and need not represent closed form equations. Let the joint variable values of the real manipulator be equal to the joint variable values of the model manipulator: i.e. 2 8 q = qm = qr- ( - ) The delta kinematics equations (Ak(q)), which give the difference between the forward kinematics equations of the model manipulator and the forward kinematics equations of the real manipulator, are derived as follows: Ak(q) = Xm - xri = km(q)-kr(q) nm(q) - iv (q)" • 0 - Sm(q) - Sr(q) 0 aro(q) -«r(q) 0 Pm(q) - Pr(q)- -Ap(q). The first 9 entries in the column vector Ak(q) are always zero because the real manipulator and the model manipulator always have identical endpoint orientations (see the proof of THEOREM II in Appendix A). Therefore [nm sm am]T = [nT<1 sr„. a^f. (2.10) By substituting (2.7) into the system (2.9), the following system of 3 equations in 3 unknowns is obtained: pm = prj + Ap(k-1([nr, sri arj pm]T,c)). (2.11) 12 Chapter 2: The Theoretical Development Therefore the only unknown in (2.11) is the 3 element row vector pm = [pmi pmy pm, ]. The unknown vector (pm) represents the endpoint position of the model manipulator when the real manipulator and the model manipulator have identical joint variable values (i.e. qm = qr), and when the real manipulator is at the desired endpoint position (pr sr , and arj), and manipulator configuration (c). Therefore, since a complete solution to the inverse kinematics problem of the model manipulator is known (2.7), numerically solving the system (2.11) for pm allows one to solve for the joint variable values of the model manipulator, and therefore for the real manipulator as well. Because the system (2.11) implicitly contains information about the endpoint position, endpoint orientation, and manipulator configuration of the real manipulator, the solution of the equations in system (2.11) represents a complete solution to the inverse problem for the real manipulator. The conditions under which the above theoretical framework gives the complete solution to the inverse kinematics problem of the real manipulator, are given in THEOREM I. The proof of this theorem can be found in Appendix A. Note that ||x|| indicates the L2 norm of the vector x. THEOREM I: Given a real manipulator and a model manipulator, assume that the following conditions are true: CI. The only difference between the model manipulator and the real manipulator are the values of the Denavit-Hartenberg parameters a,- and di. C2. There is a complete solution (as defined in this thesis) to the inverse kinematics problem for the model manipulator; i.e. for any fixed configuration c, the function qm = k^/fx^c) is continuous (with respect to x^j, and, j/(xm, c) £ Um (Um defines the singularity region of the model manipulator) the function is also one to one. C3. q = qm — 13 Chapter 2: The Theoretical Development C4. The configuration of model manipulator is always identical to the configuration of real manipulator; i.e. qm = qr cm = cT, where cm is the configuration of the model manipulator, and cT is the configuration of the real manipulator. C5. For every endpoint position, endpoint orientation and manipulator configuration which lies in a singularity region of the model manipulator, there exists another endpoint position which is arbitrarily close to the previous endpoint position, butwhich is not in a singularity region; i.e. if all the previous conditions of the theorem are met, and i/([ tVj sr ],C) E Um then there exists an p2m € ^ such that - < e, where s is an arbitrarily small positive real number, and ([ nr„ sr, ar„ ], c) £ Um. C6. The desired inverse kinematics solution exists. Then, with the real manipulator at the desired endpoint position, endpoint orientation (xr<1 = T [rv, sTi arj pr, ] ), and manipulator configuration (c), for any Emax > 0 (Ernax e Kj, there exists an approximation of the endpoint position of the model manipulator, pm<> € such that (rewriting system (2.11)) 1 ||pm. - Ap(k- (xm„,c)) - PrJ < Emax, (2.12) where xm„ — [ nrii STJ B.Tj Pm, J • 2.4 Application to 6 DOF Manipulators with Non-Intersecting, Non-Parallel Rotational Wrist Joints An algorithm has been developed which gives (by applying THEOREM I) a complete solution to the inverse kinematics problem for a large class of practical general purpose 6 DOF manipulators. This class includes manipulators that do not have closed form inverse kinematics equations. The conditions of THEOREM II define this class of manipulators. The proof of THEOREM II is given in Appendix A. 14 I Chapter 2: The Theoretical Development THEOREM II: Manipulators that satisfy all of the following conditions, satisfy the conditions of THEOREM I: CI. The real manipulator has 6 serial degrees of freedom. C2. Joint variables 1,2, and 3 give three degrees of freedom for the Cartesian positioning of joint 4. C3. Joints 4, 5, and 6 of the real manipulator are all a. rotational. b. non-parallel (i.e. aTi ± 0° or ± 180° and an ± 0° or ± 180°). c. non-intersecting (i.e. not all of the au, aT. and dr- are zero). C4. The model manipulator has been created using Procedure 1 of Appendix B. C5. qm = qr. C6. The desired inverse kinematics solution exists. Condition C2 ensures that the manipulator endpoint will have three degrees of freedom in Cartesian space. Condition C3 ensures that the wrist joints give 3 degrees of freedom in hand orientation. Condition C3 also excludes manipulators that have been shown by Pieper to have a complete solution to the inverse kinematics problem [1]. The model manipulator, for the above class of real manipulators, is created by changing the Denavit-Hartenberg parameters of the real manipulator to give the last three joints rotational intersecting axes. This is done by setting all of ami, am, and dm. of the model manipulator to zero, and making the rest of the model manipulator identical to the real manipulator, this construction is fully outlined in Procedure 1 of Appendix B. Procedure 1 ensures that the model manipulator has a complete solution to its inverse kinematics problem. At most one of the joint variables is obtained by solving for the roots of a fourth order polynomial [36], and the rest of the joint variables have closed form inverse kinematics equations [1]. 15 Chapter 3: The Nonlinear Equation Solver Main Algorithm ^ Fixed Point * Search Algorithm Algorithm [37] | Modified Powell's Algorithm [32] Figure 3.1: Hierarchy of Algorithms Chapter 3 The Nonlinear Equation Solver Two types of nonlinear equation solvers are used to solve the system of equations (2.11). The first, called the Fixed. Point Algorithm [37], is based on Banach Fixed Point Theory [26]. The main feature of this algorithm is that it is computationally efficient and terminates quickly when it cannot find the solution. The second algorithm, called the Modified Powell's Algorithm, is based on an algorithm developed by Powell [32]. It is more computationally intensive than the Fixed Point Algorithm, however, the closer the starting point of the algorithm is to the desired solution, the more likely it is that the algorithm will converge to this solution. These two algorithms are controlled through the Main Algorithm. The Modified Powell's Algorithm is accessed via the Search Algorithm. The Search Algorithm preforms a 3 dimensional search in a bounded region that is known to contain the desired solution. The values generated by the Search Algorithm serve as inputs to the Modified Powell's Algorithm. It is the Search Algorithm which theoretically guarantees that if the Fixed Point Algorithm fails to find the solution to the system (2.11), then the solution, if it exists, can always be found using the Modified Powell's Algorithm. The organization of the nonlinear equation solving algorithms is summarized in Figure 3.1. 16 Chapter 3: The Nonlinear Equation Solver 3.1 The Main Algorithm The Main Inverse Kinematics Algorithm is used to access the Fixed Point Algorithm and the Search Algorithm. A pseudo-code description of the Main Algorithm can be found in Algorithm 1 of Appendix B. The algorithm begins by reading the desired endpoint position and endpoint orientation (Xrd), and manipulator configuration (c) of the of the real manipulator. Next the initial position and orientation of the model manipulator (xTO>) is set equal to the desired endpoint position and orientation of the real manipulator, i.e. xm, = x^. If pm = [pmi Pmv Pmz ] (the initial endpoint position of the model manipulator), is outside the workspace of the model manipulator, then this initial endpoint position is recalculated so that it is close to the desired endpoint position of the real manipulator, while still being within the model manipulator's workspace. This calculation is manipulator dependent (for an example see the Numerical Example Section). In this thesis, the model manipulator's workspace is defined by the symbol Sm C which defines the region of all possible endpoint positions of the model manipulator. Next the initial joint variables values (qt) and the initial endpoint position of the real manipulator (pr.) are calculated using the following relationships: q. = km(xm,>c)> (3.1) Pr, = Pm, - Ap(q,"). The initial value for the endpoint orientation of the real manipulator is the desired endpoint orientation of the real manipulator and therefore x^ = [nr„ sTd ar„ pr, ]T. The initial approximation of the desired solution is then set equal to these calculated initial positions; i.e. pr, = pr,, pm _ [pm pm pm ) = Pm,. ^ qa = qi- These initial approximations are passed to the Fixed Point Algorithm. If the Fixed Point Algorithm fails to converge to within the desired maximum error (Emax), then its best guess and the initial starting position are both passed to the Search Algorithm. 17 Chapter 3: The Nonlinear Equation Solver If both the Fixed Point Algorithm and the Search Algorithm fail to converge, then the desired solution is not within the real manipulator's workspace (flag point beyond reach is set to 1). The algorithm also checks whether joint limits have been exceeded. The maximum possible value of joint variable j is defined by and the minimum possible value of joint j is given by <7jm>n. If any joint limits have been exceeded then the approximate joint variable values are set to those limits (flag joint_limit_reached is set to 1). 3.2 The Fixed Point Algorithm The fixed point algorithm formulated in [37] is used in this thesis. The algorithm is fully described in pseudo-code in Algorithm 2 of Appendix B. The version of the fixed point algorithm presented in this thesis differs only notationally from the version found in [37], Let p*. p^ and qh respectively represent the approximation of pr„, pTO, and q at the kth iteration step. Starting with the initial approximations from the Main Algorithm p° = pr„, p^ = pm„ and q° = qa, the subsequent estimates of pr„, pm, and q are given by the following recursion: P^i=Pr,+Ap(qfc) P*+1=P*m-AP(qfc) (3-2) qfc+1 = k"1 nTj sri arj p^+1]r,c). The algorithm is terminated when any of the following conditions are met: +1 CI. ||p* -PrJ < Emax, where Emax is the predefined maximum allowable error. This condition indicates that a solution has been found and the algorithm exits with the flag successful = 1. C2. g Sm. This condition indicates that the next approximation is outside the model manipulator's workspace and therefore outside the real manipulators workspace. Thus the iteration is unsuccessful and the algorithm exits with the flag successful = 0. C3. ||pfc+1 -PrJ| > • ||Pr _ PrJI' where 0 < < 1. This condition indicates that the algorithm is not converging at the desired convergence rate which is predefined by 18 Chapter 3: The Nonlinear Equation Solver the constant (3p. Thus the iteration is unsuccessful and the algorithm exits with the flag successful = 0. C4. k > kmaXF, where kmaXF is a predefined constant indicating the maximum number of allowable fixed point iteration steps. This indicates that a solution has not been found and the algorithm exits with the flag successful = 0. The conditions under which the Fixed Point Algorithm will converge to the desired solution are given in THEOREM III. The proof for this theorem can be found in [26]. THEOREM III: By definition, let Sm C ^ be the set of all points such that pm e Sm; i.e. Sm defines the workspace of the model manipulator. Let x^ = [ nr (Emax € K) be an arbitrarily small maximum allowable error in the real manipulator's desired endpoint position. Given that the following conditions are met: CI. For all k = 0,1,2,..., oo; ifpkm € Sm then p*+1 = prt + Ap(k-! (x^, c)) 6 Sm. C2. For all k = 0,1,2,..oo; if pkm € Sm then < pF < 1. Here |H|N N indicates the norm of a matrix. Then it can be concluded that Algorithm 2 will converge in a finite number of iterations to within an error of Emax of the desired endpoint position of the real manipulator. The number of iterations is log e""('-M bounded by + 1. 19 Chapter 3: The Nonlinear Equation Solver 33 The Search Algorithm System (2.11) can be written as f(Pm)=[/l(Pm) h( Pm) /3(Pm)f, = pm - Ap (K~ 1 ([ n,« sr, ar< pm ]T, c)) - pr4, (3.3) = 0. The goal of the Search Algorithm is to find pm„ such that £mas>£=||f(PmJ||. (3.4) The algorithm works by passing a series of different possible starting approximations of pm to the Modified Powell's Algorithm. The Modified Powell's Algorithm then attempts to solve system (3.3) from these initial approximations. The Modified Powell's Algorithm will always iterate to a local minimum. If this local minimum is within Emax of ||f(pm.)|| = 0, where pm„ is the current approximation of pm> then the Modified Powell's Algorithm has succeeded in finding the solution. If, however, the algorithm cannot find this solution, then the Search Algorithm passes it a new starting approximation for pm. This process continues until either the Modified Powell's Algorithm converges to the desired solution, or the Search Algorithm has generated a point which is within Emax of the desired solution. The Search Algorithm is designed so that it does not pass the same initial guess for pro to the Modified Powell's algorithm more than once. The Search Algorithm is fully described in pseudo-code in Algorithm 3 of Appendix B. It begins by passing pm. (as calculated by the Fixed Point Algorithm), to the Modified Powell's Algorithm. If this initial position fails to allow the Modified Powell's Algorithm to converge to the required solution, then the algorithm resets pm„ = pm>. and then this initial approximation is also passed to the Modified Powell's Algorithm. If this initial value also fails to give convergence, then a more extensive search of all possible solutions is performed. The more extensive search is termed the 20 Chapter 3: The Nonlinear Equation Solver second part of the Search Algorithm and it begins at STEP 2 of Algorithm 3. It should be noted that in most instances the Modified Powell's Algorithm will have converged to the desired solution without the need for this extensive search. A set of all values of pm„ which will allow possible convergence to the desired solution is contained within the region V where V = {pm. € K3 such that pm. e Sm AND ||pm. - pm,|| < Rs}, (3.5) and Rs is chosen so that £s>||Pm-Pm,||. (3-6) Note that V does not contain all possible values of pm„ which will allow convergence to the desired solution, but only a very small well-contained subset of all possible values. It can be shown that, for real manipulators that meet the conditions of THEOREM II, the maximum distance between the endpoint position of the real manipulator and the endpoint position of the model manipulator is given by ||Pri-Pm|| Therefore, Rs is calculated as follows: Rs = kJ + + < + IIPm. - Pr, II- (3-8) The second part of the search procedure is done in distinct stages, and each stage is carried out in 2 separate phases. Let n be an integer which indicates the current stage of the Search Algorithm. Tire first phase of each stage involves scanning of the region defined by (3.5) for the local minimum which gives the minimum value of the function E = ||f(pm. )||. The point at which this best approximation of pm is found is designated as pm,ra = [PmIln Pm.,„ ]• The corresponding joint variable 21 Chapter 3: The Nonlinear Equation Solver © © © © © © © © © © pmi * © © © © © © © © © © The centers of the circles indicate the points generated in the first phase of the search algorithm for the case n=2 and n=3. The numbers inside the circle indicate the stage at which the points axe generated. The grid is here viewed along the z-axis and thus only two dimensions are shown. Figure 3.2: Search Grid values and real manipulator endpoint positions are given by qim and prim respectively. The scan is always centered at pmi and moves out in a 3 dimensional Cartesian grid which has coordinate axes that are parallel to the base coordinate axes1. At stage n, the grid has a spacing of along each coordinate axis, where i is an odd integer such that |i| < 2n (see Figure 3.2). The first phase of any stage of the Search Algorithm always generates new starting values of pm>. Each point generated in the above grid is passed to the Modified Powell's Algorithm. The first phase ends when either the Modified Powell's Algorithm has found a Pm. which satisfies equation (3.4), or all points of the grid have been passed to the Modified Powell's Algorithm. If the desired solution is not found in the first phase, then the second phase is executed. 1 The base coordinate frame is the one in which the desired endpoint position and orientation of the real manipulator are defined. 22 Chapter 3: The Nonlinear Equation Solver The function of the second phase of each stage of the Search Algorithm is to search around the best approximation of pm discovered in the first phase. The second phase is not required to in order to guarantee convergence (see Appendix A); its only function is to improve the rate of convergence. The second phase of the Search Algorithm is centered at pmim and moves out in a 3 dimensional Cartesian grid which also has coordinate axes that are parallel to the base coordinate axes. At stage n, the grid has a spacing of i • ^ • along each coordinate axis, where i is an odd integer such that |t| < 2n. Note that this search is similar to the one done in the first phase except that it is restricted to a much smaller volume; i.e. volume = (^Rs f. As in the first phase, each point generated by this search is passed to the Modified Powell's Algorithm. If at any time during this search a new local minimum is found, the search is continued around this new local minimum. The second phase ends when either the Modified Powell's Algorithm finds the desired solution or all the grid points have been passed to the Modified Powell's Algorithm. If the solution has not been found in the second phase of stage n, then n is incremented and the first phase is executed once more. The Search Algorithm ends if either of the following two conditions are met: CI. The Modified Powell's Algorithm has set the flag successful = 1. This indicates that the desired solution has been found. C2. n > nmax. This condition indicates that the Search Algorithm has generated a sufficiently dense 3 dimensional Cartesian grid to completely cover the region of all possible solutions to within a maximum real manipulator endpoint position error of Emax (see THEOREM III). If this condition is met, then it is concluded that the desired solution is outside the real manipulator's workspace, and the Search Algorithm exits with the flag successful = 0. Note that nmax must be determined in advance either theoretically or experimentally. The conditions under which the Search Algorithm gives convergence to the desired solution are given in THEOREM IV. The proof for this theorem is given in Appendix A. 23 Chapter 3: The Nonlinear Equation Solver THEOREM TV: Given that the following conditions are true: CI. The desired inverse kinematics solution for the real manipulator exists. C2. R, > ||pmi-pm||. Then there exists a finite positive integer nmax such that the Search Algorithm (Algorithm 3) will generate an initial starting approximation of pm that will allow the Modified Powell's algorithm to find a pm. which will satisfy equation (3.4). 3.4 The Modified Powell's Algorithm The Modified Powell's Algorithm uses 3 types of iterative nonlinear techniques for solving systems of nonlinear equations. These are the Newton-Raphson method, the Gradient Descent method, and a linear combination of the Newton-Raphson and Gradient Descent methods. The Modified Powell's Algorithm is based on an algorithm developed by Powell [32] and the modifications to the algorithm that are suggested in this thesis make it more suitable for solving the nonlinear system (2.11). For this reason, the Modified Powell's algorithm is fully explained in this thesis. Note that the theoretical framework of the algorithm presented here is identical to Powell's Algorithm and therefore all convergence proofs given in [32] are applicable here. A pseudo-code description of the Modified Powell's Algorithm is given in Algorithm 4 of Appendix B. The goal of the algorithm is to find pm. such that 2 2 2 (error) = F(pm.) = ||f(Pm.)|| < E max, (3.9) where f(-) is defined in equation (3.3). Let pkT, pym and qk respectively represent the approximation th of Pri, pm, and q at the k iteration. Starting with the initial guesses (from the Search Algorithm) _o _ _ „o = n Qo _ q the subsequent estimations of pTi, pm, and q are calculated Fr rra t Fm trma n 24 Chapter 3: The Nonlinear Equation Solver using the following recursion: q^k-^p^c), (3.10) p^1 = P^-AP(qfc+1). In Powell's original algorithm, 6 € is calculated at each iteration using only one of the Newton- Raphson method, the gradient descent method , or the linear combination of these two methods. Powell used a set of tests to determine which calculation of §_ was valid. These tests proved inadequate for solving the nonlinear system (2.11) because it was found that they often eliminated valid calculations of 6. As a result, in the Modified Powell's Algorithm, all three possible 6_'s may be calculated; the first one that reduces the error by an amount which satisfies F (p^ + S) < /3P • F (p^) (where 0 < (3p < 1 is a predefined constant that determines the minimum allowable reduction in the total error per iteration step) is used. The Modified Powell's Algorithm first calculates 6 using the Newton-Raphson method. This is done by solving the following system of 3 linear equations in 3 unknowns: f(p5.)+J_1£ = 0> (3.11) where J is the 3 by 3 Jacobian matrix of f(p^); i.e. 'd(f(pkm)) J = (3.12) The magnitude of the step 6 is limited to a maximum value defined by Amox. Initially this is set to Amax = y/F( Pi). (3.13) If ||i|| > A max then 6 is recalculated as 6 = AmoX|||| (note that Powell's original algorithm does not do this recalculation; it simply ignores the Newton-Raphson calculation of 6 if its magnitude exceeds Amax)- 25 Chapter 3: The Nonlinear Equation Solver If the Newton-Raphson calculation of 6 fails to reduce the error by the required amount, then the gradient descent method is used. The gradient of F(pjjj is given by and <5 is calculated as « = « • (3.15) Finally, if both the Newton-Raphson method and the gradient descent method fail to reduce the error by the required amount, then a linear combination of these two methods is used. This involves finding 7 such that = Amax. (3.16) pgr If such a 7 is a positive real number between 0 and 1 (i.e. 7 € & such that 0 < 7 < 1), then 6 is calculated as follows: ^ = (l-7)*g + 7J-1 If such a 7 does not exist, then the linear combination of the Newton-Raphson method and the gradient descent method has not succeeded in calculating a S (the flag NRG error is set to 1). At each iteration, all calculated 6's are put through a boundary test These tests are required to ensure that the algorithm is not iterating to points that are outside of the manipulator's workspace (note that Powell's original algorithm does not do boundary tests). A boundary test is done by checking to see whether (p^ +1) is within the region of all possible solutions; i.e. (p^ +1) G Sm such that ||p^ + ^ - Pm, || < Rs- If it is not, then the magnitude of 6 is limited to a distance which is less than the minimum distance between the boundary of all possible solutions and p^. This new maximum value of M|l is assigned to the variable A. The calculation of A is manipulator dependent 26 Chapter 3: The Nonlinear Equation Solver and an example of it is given in the Numerical Example Section. If A > Amin, where Am,-n is a predefined minimum step size that is used to account for computer round off errors, then 6_ is calculated as 6 — Ajjfjj. If A < Am,-n then the direction of 6 is such that it gives points that outside the region of all possible solutions. This error is referred to as a model manipulator boundary error. If it occurs in the Newton-Raphson calculation then the flag NR_error is set to 1; if it occurs in the gradient descent calculation then the flag Grad_error is set to 1; and, if it occurs in the calculation of the linear combination of the two methods, then the flag NR error is set to 1. If all calculated S's do not reduce the error by the required amount, but at least one of them does not produce a model manipulator boundary error, then the algorithm sets Amax = and the above steps are repeated starting with the Newton-Raphson method. If a successful 6 has been calculated, then the next iteration of the algorithm begins once the recursion (3.10) is done, the maximum step size is recalculated as A max — y/F{p^+1), and k is incremented by 1. The Modified Powell's Algorithm terminates under the following conditions fe C^I • F(o\K m) J < — E 2 mar This indicates that a solution has been found and the algorithm exits with the flag successful = 1. C2. A max < &min- This indicates that a local minimum has been found and the algorithm exits with the flag successful = 0. C3. (NR error = 1) AND (Grad_error = 1) AND (NRG error = 1). This condition indicates that a local minimum has been found which is on the region of all possible solutions: i.e. the region defined by p^ € Sm such that ||p^ - pm. || < Rt. Thus the algorithm has been unsuccessful and exits with the flag successful = 0. C4. k > kmaxp. This indicates that the Jacobian matrix has been calculated more then the maximum number of allowable times (determined by the predefined integer kmaXp). The algorithm exits with the flag successful = 0. Note that Powell's original algorithm terminates when the test F(pkm) > M||g|[ is true. M is a 27 Chapter 3: The Nonlinear Equation Solver positive real number which gives an over-estimate of the of the distance from p^ to the desired solution. This test was found to be inadequate for the nonlinear system (2.11) because it often stopped the algorithm when it was iterating to the right solution. 3.5 Numerical Approximation of the Jacobian The Jacobian, J, used in the Modified Powell's Algorithm is defined as follows: df(pl)=[dfi(pkm) df2(j>km) dh(vkm)}T, = J dpi, Vii Jl2 Jul r^Ll'dpi/ (3-18) •hi hi Jn dpi, . Jn Jn Jxi .dpi This Jacobian can either be evaluated using closed-form analytic equations, or it can be numerically approximated. A numerical approximation is necessary only if the inverse kinematics equations of the model manipulator (i.e. equation (2.7)) are not closed form equations. In this section, a method for numerically approximating the Jacobian will be presented. Each row (denoted by n) of the Jacobian is approximated using a least squares estimation over t points as follows; Jni T _1 T (3.19) [Jnl Jn2 Jut}2 Jn2 = (A A) A b Jn.1 where j^ + A pXl ^y + Apyi + APz k k p mi + A?x2 V mv + A Py2 pi, + A pz A = (3.20) ,^+A Pxt pi+Apyt j&.+Ap,,. f/n(Pm) + A/n /n(P^)+A/„ (3.21) L/n(P^)+A/niJ 28 Chapter 3: The Nonlinear Equation Solver and Afi- = [A/i, A/2I A/3I], (3.22) = f (pi + Ap.) - f (p^), for i = 1,2, • • • ,t. Each time the Jacobian is calculated, the points Apt- = [ ApVi ApZi} (for % = 1,2, • • •, f) are generated using a uniform random distribution centered at 0 with a width of Wj. Wj is chosen small enough to ensure that the Jacobian is approximately linear in this range. The inverse Jacobian is approximated using the same procedure; i.e. the inverse Jacobian is defined as • jinv rinv rinv Jn Jn 0/iR) (3.23) rinv Am jim k "11 12 23 dfi(l> m) rinv rinv Tinv k V3I -'32 "33 . Ldh(p m). The nth row of the inverse Jacobian is approximated as rinvi 711 iinv T 1 r (3.24) [./r jt J™? = Jn2 = (D D)~ D h, Tinv L-Ai3 - where k k h(p m) + A /i, /2(pi) + A/2, h(v m)+A /n, /i(Pfn) + A/lj /2(Pi) + A/22 /3(p*)+A h D = (3.25) /i(Pm) + A/i, h(pkm)+Afi, /n(pi) + A/J + APc(n), A* + Aj>c(n)2 h = (3.26) + APc(n), - and Apc(i), = Apr,, (3.27) APc( 2), = APyt, Apc(3)i = ApZi, for i - 1,2, • • •, t. 29 Chapter 3: The Nonlinear Equation Solver Ap, and Af; are defined above. Chapter 4: Numerical Examples Chapter 4 Numerical Examples The inverse kinematics algorithm developed in this thesis has been tested on two 6 DOF manipulators that have no known closed form inverse kinematics equations. The first manipulator, called the Spherical manipulator (see Figure 4.1), is taken form a paper by Tourassis and Ang [6]. Tourassis and Ang used this manipulator to demonstrate their inverse kinematics algorithm which is intended to work on manipulators that have no closed form inverse kinematics equations. Tourassis and Ang tested their algorithm on 24 desired inverse kinematics solutions for this manipulator; their algorithm failed to converge on 4 of these desired solutions. In contrast, the algorithm presented here was always able to find the desired inverse kinematics solution (if it existed). The second manipulator on which the inverse kinematics algorithm has been tested is called the Modified PUMA manipulator (see Figure 1.1). The Modified PUMA manipulator is identical to the PUMA 560 except that all wrist link lengths (a4, a5, d;-,, and a6) have been set non-zero values. Because of these wrist offsets, the Modified PUMA has no known closed form inverse kinematic equations. The inverse kinematics algorithm presented here has been tested with various magnitudes of the Modified PUMA's wrist offsets. Also, tests were carried out with the Jacobian (of the Modified Powell's Algorithm) being both numerically approximated and analytically calculated. 4.1 The Spherical Manipulator The Denavit-Hartenberg Parameters of the "real" Spherical manipulator [6] (see Figure 4.1) are given in Table 4.1. The Denavit-Hartenberg Parameters of the corresponding model manipulator (generated using Procedure 1) are given in Table 4.2. 31 Chapter 4: Numerical Examples Figure 4.1: The Spherical Manipulator One should note that in [6], the forward kinematics of the Spherical manipulator have been defined as [nT(q) sT(q) aT(q) pT(q)l a 3 u, 4 5 = "A/A3 A3 Ati; A4 As A6, (4.1) .0 0 0 1 . 3 where the homogenous transformation matrix, Aw, is defined (as indicated in Tables 4.1 and 4.2) by setting the Denavit-Hartenberg parameters to a = 0.0, d = 0.0, a - -90.0°, and 0 = -90.0°; i.e. 0 0 1 01 -10 0 0 3 A = (4.2) 0-100 L 0 0 0 1J Therefore, the forward kinematics equations (k(q) = [n(q) s(q) a(q) p(q)]T) for both the model Spherical manipulator and the real Spherical manipulator must be calculated using equation (4.1). 32 Chapter 4: Numerical Examples Table 4.1: The Denavit-Hartenberg Parameters of the "Real" Spherical Manipulator a Joint i 9Ti r. aTi du Joint Range 1 9i 90° 0 100.0 mm 0° < 91 < 360° 2 <12 -90° 15.0 mm 0 0° < q2 < 360° 3 0° 0° 0 93 -OO < §3 < CO — -90° -90° 0 0 — 4 94 90° 0 0 0° < q4 < 360° 5 9o 90° 0 20.0 mm 0° < 55 < 360° 6 96 0° 20.0 mm 0 0° Table 4.2: The Denavit-Hartenberg Parameters of the "Model" Spherical Manipulator Joint Range Joint i Q-m i ami dmi 0 1 91 90° 0 100.0 mm 0° < qi < 360 2 92 -90° 15.0 mm 0 0° < q2 < 360° 3 0° 0° 0 93 -00 < < 00 — -90° -90° 0 0 — 4 94 90° 0 0 0° < q4 < 360° 5 95 90° 0 0 0° < g5 < 360° 0° < e 360° 6 96 0° 20.0 mm 0 q < The delta kinematics equations are Ap(q) = Pm(q) - Pr(q), ~ drs{ClSlSA - C1C2C4) (4.3) dr-XSlSlSi - S1C2C4) dr-X~CiSi - S2C4) The notation C; = cos®, Si = sing,, CX3 = cosfo + q3), and StJ = sin(9i + q3) is used in this thesis. The closed form inverse kinematics equations of the model Spherical manipulator are given in Appendix C. These equations were obtained from [6], The 8 possible manipulator configurations are 33 Chapter 4: Numerical Examples defined by ARM = ±1, ELBOW = ±1, and WRIST = ±1. The Jacobian used in the Modified Powell's Algorithm was calculated using numerically using a least squares approximation (see Section 3.5). The workspace of the model manipulator is given by Pm 6 Sm = {[px py pz} = Pm - am6Hm, such that (4.4) AKM-{pl+p2y) + (pz-dri?>R2}, where 2 R = c?m2 + buffer. (4.5) The user defined constant "buffer" prevents the initial model manipulator endpoint position from being located exactly on the model manipulator's boundary, and is chosen to be much less than the maximum allowable endpoint position error (i.e. 0 < buffer < If, in the Main Algorithm, the desired endpoint position of the real manipulator is not in the model manipulator's workspace ( i.e. pm = Prd Sm) then the initial position of the model manipulator is chosen so that it lies approximately a distance of AB away from the edge of the boundary. AB is a positive real number which is defined in advance, and ensures that the initial position is sufficiently within the manipulator's workspace boundary. The initial position of the model manipulator is calculated as follows: STEP 1. Pm, = pr„- STEP 2. [px Py Pz] = Pm, - amenrd. STEP 3. IF (ARM • {pi + p2y) + (pz - dmif < R2) a. IF ipz - di > 0) • pz = di + sJ&B + < - ARM • + fy). ELSE • pz — di - J&B + al,-ARM-(pl + p>). STEP 4. pm, = [px Py Pz] + amsnrj. The value of A in the Modified Powell's Algorithm is calculated so that it gives the minimum distance, along the line defined be the vector 6 = [6X 6y 6Z ], between the present approximation 34 Chapter 4: Numerical Examples of the endpoint position of the model manipulator (x^), and the model manipulators' workspace boundary. The following procedure is used to calculate A: k STEP 1. [px Py Pz] = p m - amsnr STEP 2. a = 62x + ^ + STEP 3. b = 2(pjx + py6y + pzSz - 6zdmi). STEP 4. c - -2pzdmi - a2mi + (p* + fi + £ + d?mi) - buffer. STEP 5. Ai = ~6+^~4ac- b V iaC STEP 6. A2 = ~ ~ 2a' - STEP 7. IF ((Ai > 0) AND (A2 < 0)) a. A = Ai. ELSE a. IF ((Ai < 0) AND (A2 > 0)) • A = A2. ELSE . IF ((Ai > 0) AND (A2 > 0)) • IF (Ai < A2) . A = Ai. ELSE A = A2. STEP 8. A = Ay/a. 4.1.1 Evaluation of the Algorithm Over 100,000 Uniformly Generated Random Points The user defined parameters of the inverse kinematics algorithm where set to the following values: Emax = 0.1 mm (this value was chosen because most industrial manipulators have positioning repeatability accuracy that is approximately this magnitude or less), 0F = 0.99, i3P = 0.9999, 2 kmax = 50, Ai? = 10.0 mm, buffer = (0.01mm) , kmaXp - 500, nmax = 11, and Amm = 35 Chapter 4: Numerical Examples 0.0025 mm. The Jacobian (Modified Powell's Algorithm) was approximated and the approximation parameters were set as follows: t — 10, Wj — 0.0002. The algorithm was written in C code and was tested on over 100,000 randomly selected desired real manipulator endpoint positions, endpoint orientations and configurations. The test points were generated using a uniform random distribution in joint space (joint variable 3 was limited to -500 mm < < 500 mm ). The algorithm always converged to the desired solution. A manipulator configuration error never occurred and the orientation error, which was calculated as 8 11^ _ nra || + ||srii - Sra II + ||arj - ara ||, was always less then 10" . The real manipulator endpoint position error was always less than the maximum allowable error Emax = 0.1 mm. The Fixed Point Algorithm found the required solution 99.948% of the time and the Search Algorithm found the solution the other 0.052% of the time. The algorithm was implemented on a SUN SPARC station 1. For 99.952% of the points the convergence time never exceeded 27 milliseconds, with an average time of 1.59 milliseconds. Only 0.048% of the points (i.e. 48 points out of 100,000) required more than 27 milliseconds to converge to the desired solution. Note that one iteration of the Fixed point algorithm corresponds to approximately 0.87 milliseconds and one iteration of the Modified Powell's Algorithm corresponds to approximately 11.71 milliseconds. Slow convergence was always associated with the desired solution being very close to a model manipulator singularity region. A more extensive analysis of the algorithm's performance near singularity regions is given in Section 4.2.1. It should be noted that neither the Inverse Kinematics Algorithm nor the C code were optimized, and therefore it may be possible to significantly improve on these reported times. Almost all of the computation time of the algorithm is involved with computing the delta kinematics functions Ap(-)- A non-optimal list of the number of multiplies, divides, additions, and calls to functions (trigonometric, square root and power functions) that are required for this 36 Chapter 4: Numerical Examples Table 4.3: Computational Requirements of The Spherical Manipulator i Function Number of Number of Number of Number of Multiplies Divides Adds/ Library Subtracts Function Calls Ap(-) 50 2 34 22 computations is given in Table 4.3. 4.1.2 Rate of Convergence Comparison with an Existing Algorithm Tourassis and Ang [6, pp. 565-567] applied their inverse kinematics algorithm to the Spherical manipulator and tested it on 24 desired inverse kinematics solutions. These desired solutions are defined by the following 3 desired endpoint positions and orientations (presented in Denavit- Hartenberg homogeneous transformation matrix form): -0.807207 -0.229473 -0.543838 -245.137035mm -0.566297 0.041162 0.823173 -143.535049mm desired pos./orien.#l (4.6) -0.166510 0.972444 -0.163176 419.569791mm 0 0 0 -0.788453 0.140895 -0.598741 -105.126303mm' -0.555469 0.254994 0.791475 -62.699816mm desired pos./orien.#2 (4.7) 0.264190 0.956623 -0.122788 509.678558mm 0 0 0 -0.758285 0.386192 -0.525224 -25.793805mm! -0.457945 0.257867 0.850759 -15.295045mm (4.8) desired pos./orien.#3 0.463994 0.885641 0.018682 527.139402mm 0 0 0 1 Each of the above three desired endpoint positions and orientations is to be achieved at all 8 possible configurations (this gives the desired 24 desired inverse kinematics solutions). 37 Chapter 4: Numerical Examples Tourassis and Ang's algorithm was not able to converge for 4 of the above desired solutions. The algorithm was written in C and was run on a SUN 3/60 workstation (Motorola 68020 with a 68881 floating point processor running at 20 MHz). The average execution time of their algorithm was 42.58 milliseconds with a maximum of 65 milliseconds. The algorithm presented in this thesis was also tested on the above 24 desired solutions. The user defined parameters of the inverse kinematics algorithm where identical to those given in Section 8 4.1.1, except that the maximum allowable error was set to Emax = 1.0 x 10~ mm. This error was chosen to be of the same magnitude as that found in [6]. Unlike with Tourassis and Ang's algorithm, the algorithm presented here was always able to converge to the desired solution. The fixed point algorithm found the desired solution in all cases (i.e. the Search Algorithm was not requited). On a SUN 3/60 workstation (Motorola 68020 with a 68881 floating point processor running at 20 MHz), the average time of convergence was 14.17 milliseconds with a maximum time of 20.0 milliseconds. As compared to Tourassis and Ang's algorithm, this is a 66.7% decrease in the average time and a 69.2% decrease in the maximum time. 38 Chapter 4: Numerical Examples 42 The Modified PUMA Manipulator The Denavit-Hartenberg Parameters of the "real" Modified PUMA manipulator (see Figure 1.1) are given in Table 4.4. This manipulator is identical to the PUMA 560 except that all of the wrist offsets have nonzero values. No known closed form inverse kinematics equations exist for this manipulator. The Denavit-Hartenberg Parameters of the corresponding model manipulator (generated using Procedure 1) are given in Table 4.5. The model manipulator differs from the PUMA manipulator by only the single nonzero wrist offset am( (ams is set to a nonzero value only for the sake of completely filling Table 4.4 with nonzero wrist offset values; its magnitude has no affect on the results). The delta kinematics equations are Ap(q) = pm(q)-pr(q), 'ariPll + Gr:J>12 + dr.pu (4.9) - auj>2i + aTip22 + dTip2i _aT,VM + «rj>32 + dr-PM _ where pn = SiSi - C1C23C4, P12 — Ci(S2nSj - C-aCiC;-,) + S1S4C';-, Pit = S1C4 + C1C2354, Pii = -C1S4 - S1C23C4, (4.10) P22 = Si{SnSr, - C23C4C5) _ C1S4C0 J>23 = -C1C4 + 5iCM54, Pni = SnC4, P32 = + CraS-t,'O! P33 = -S2HS4. 39 Chapter 4: Numerical Examples Table 4.4: The Denavit-Hartenberg Parameters of the "Real" Modified PUMA Manipulator Joint i aTi Joint Range 1 ?1 -90° 0 0 -160° < qi < 160° 2 92 0° 431.8 mm 149.09 mm -225° < q2 < 45° 3 93 90° -20.32 mm 0 -45° < ^ < 225° 4 94 -90° On 433.07 mm -175° < qi < 175° 5 9o 90° «r3 dTi -160° < < 160° 6 96 0° Or. 56.25 mm -266° < q6 < 266° Table 4.5: The Denavit-Hartenberg Parameters of the "Model" Modified PUMA Manipulator Joint Range Joint i 0rrij otmi a*m. dmi 1 11 -90° 0 0 -160° < qi < 160° 2 <12 0° 431.8 mm 149.09 mm -225° 4 94 -90° 0 433.07 mm -175° 6 96 0° 20.0 mm 56.25 mm -266° < 96 < 266° The closed form inverse kinematics equations of the model Modified PUMA manipulator are given in Appendix D. These equations are identical to those of the PUMA 560 except that the position of the wrist (pt,,) is determined by the equation pw = pm - dmtsam - amsnm (this is because an has been arbitrarily set to a nonzero value in the real manipulator). A derivation of the inverse kinematics equations can be found in [35]. The 16 possible manipulator configurations are defined by ARM = ±1, ELBOW = ±1, WRIST = ±1, and FLIP = ±1. The Jacobian used in the Modified Powell's Algorithm was calculated using closed form equations. The equations were generated using the symbolic manipulation package maple and are presented here. The workspace of the model manipulator is given by pm e sm = {[px Py Pz} = Pm- ~ ^.Dm, (4.11) such that p2x + p2y> Riin AND p\ + p] + p\ < R2max}, 40 Chapter 4: Numerical Examples RLn = 4, + buffer, , , ,2 (4.12) Rlaz = <&, + («m, + + - buffer- As with the Spherical manipulator, the user defined constant "buffer" prevents the initial model manipulator endpoint position from being located exactly on the model manipulator's boundary, and is chosen to be less then the maximum allowable endpoint position error (i.e. 0 < buffer < E^ax). Also, if, in the Main Algorithm, the desired endpoint position of the real manipulator is not in the model manipulator's workspace ( i.e. pm, = pr„ £ Sm) then the initial position of the model manipulator is chosen so that it lies approximately a distance of AB away from the edge of the boundary. AB is a positive real number which is, as with the Spherical manipulator (Section 4.1), defined in advance. The initial position of the model manipulator is calculated as follows: STEP 1. Pmi = pr<. STEP 2. [px py pz] = pm> - dmtar„ - amtnrd. 2 STEP 3. IF (pl+fi < R min) a- Px = vfe^™"+AB)- b. Py = 7^v(Rmm + AB). 2 C. IF (p2 + iy + £ > R maX) • Pz = ^Rlax-^Bf-iti+rt)- 2 2 STEP 4. IF (p£ + p y + pi > R m„*) b- + ^ goto step 3 d. IF (pPx + < iO then d STEP 5. pm, = [Px Py Pz] + mtar, + drnAr 41 Chapter 4: Numerical Examples The value of A in the Modified Powell's Algorithm is calculated as follows: 2 A = min{ Jpi+fy- R min + buffer, Jfi+tf+pB?max - buffer, Y y (4.13) ||Pm. - xm,|| - Rs} ~ ^buffer. 4.2.1 Evaluation of the Algorithm Over 1,000,000 Uniformly Generated Random Points The user defined parameters of the inverse kinematics algorithm where set to the following values: Emax = 0.1 mm (this is approximately the positioning repeatability of the PUMA 560 manipulator), 2 fa = 0.99, I3P = 0.9999, kmaXF = 50, AB = 10.0 mm, buffer = (0.01mm) , kmaXp = 500, nmax = 11, and Amin = 0.0025 mm. The wrist Denavit-Hartenberg parameters were set to ar, = 20.0mm, aT- = 20.0mm, dT. = 20.0mm, and aTt = 20.0mm. The algorithm was written in C code (except for the Jacobian used in the Modified Powell's Algorithm which was coded in Fortran that was generated by the symbolic manipulation package maple) and was tested on over 1 million randomly selected desired real manipulator endpoint positions, endpoint orientations and configurations. The test points were generated using a uniform random distribution in joint space. The desired solutions spanned the entire workspace of the real manipulator and the algorithm always converged to the right solution. A manipulator configuration error never occurred and the orientation error, which was calculated as Hn^ - nr J + ||srj - sr J + ||arj - ar J, was always less then 10"8. The real manipulator endpoint position error was always less than the maximum allowable error Emax = 0.1 mm. The Fixed Point Algorithm found the required solution 97.57% of the time and the Search Algorithm found the solution the other 2.43% of the time. The algorithm was implemented on a SUN 4/330. Detailed simulation results are given in Table 4.6 (LT. stands for iteration time). The table gives convergence times as a function of the distance to a singularity regions. The distance to a singularity region is calculated as being the minimum of the distance to the condition wrist singularity of the model manipulator), and the distance to the boundary of the real manipulator's 42 Chapter 4: Numerical Examples Table 4.6: Simulation Results of the Modified PUMA Manipulator (The wrist Denavit-Hartenberg parameters were set to ar, = 20.0mm, ar. = 20.0mm, dri " 20.0mm, and ar, = 20.0mm.) (I.T. = iteration time) Distance No. of No. of Pts. No. of Pts. No. of Pts. No. of Pts. to a pts. tested with I.T. < with I.T. > 60 with I.T. > with I.T. > Singularity 60 ms ms 100 ms 670 ms 0 to 0.1 mm 230 223 154 470 240 (48.9%) (47.4%) (32.8%) 0.1 to 1 mm 2,406 1,201 412 4,224 1,818 (57.0%) (28.4%) (9.8%) 321 1.0 to 2.5 mm 5,026 1,004 7,047 2,021 (71.3%) (14.2%) (4.6%) 304 2.5 to 5 mm 9,313 1,241 11,745 2,432 (79.3%) (10.6%) (2.6%) 172 5.0 to 10 mm 20,270 1,454 23,511 3,241 (86.2%) (6.2%) (0.73%) 10 to 20 mm 46,372 359 31 47,201 829 (98.2%) (0.76%) (0.066%) > 20 mm 905,494 83 905,802 308 0 (99.97%) (0.0092%) workspace. One can see that for 98.91% of the points tested, the algorithm required less than 60 milliseconds to find the desired solution. The average convergence time for these points was 2.82 milliseconds. For 99.44% of the points the convergence time never exceeded 100 milliseconds, with the average time of 3.22 milliseconds. For 99.86% of the points the convergence time never exceeded 670 milliseconds, with the average time of 4.09 milliseconds. Note that one iteration of the Fixed 43 Chapter 4: Numerical Examples Table 4.7: Computational Requirements for the Modified PUMA Manipulator Function Number of Number of Number of Number of Multiplies Divides Adds/ Library Subtracts Function Calls Ap(-) 122 6 76 33 T _ [8(f(pS.))l J - I 754 70 461 28 point algorithm corresponds to approximately 0.77 milliseconds and one iteration of the Modified Powell's Algorithm corresponds to approximately 1.75 milliseconds. It should be noted that neither the Inverse Kinematics Algorithm nor the C code were optimized, and therefore it may be possible to significantly improve on these reported times. Almost all of the computation time of the algorithm is involved with computing the delta A non-optimal list of the number of kinematics functions Ap(-) and the Jacobian J = dpi, multiplies, divides, additions, and calls to functions (trigonometric, square root and power functions) that are required for these computations is given in Table 4.7. 4.2.2 Evaluation of the Algorithm On Various Wrist Offset Magnitudes In this section, the performance of the inverse kinematics algorithm is evaluated on the Modified PUMA manipulator with various magnitudes of wrist offsets and with both a numerically approxi- mated and analytically calculated Jacobian. The user defined parameters of the inverse kinematics algorithm where set to the values given in Section 4.2.1. For the approximated Jacobian simulations, the approximation parameters were set as follows: t = 5, Wj = 0.0002. For each set of wrist offset magnitudes, the inverse kinematics algorithm was tested on 100,000 random points that spanned the workspace of the manipulator. The points were generated using a uniform random distribution in joint space. The tests were repeated for both an analytical and 44 Chapter 4: Numerical Examples numerically approximated Jacobian. The results of the tests for the analytical Jacobian case are given in Table 4.8. The numerically approximated Jacobian results are given in Table 4.9. Table 4.8: Simulation Results on Various Wrist Offset Magnitudes: Analytical Jacobian (LT. = iteration time) Wrist Offset Magnitudes ave. I.T. for pts with I.T. < 60 % of pts. with I.T. < 60 ms. in mm. ms. ar„ = 20.0, ar, = 20.0, 98.91% 2.82 ms dr, = 20.0 or„ = 20.0 an = 40.0, ar, = 40.0, 92.56% 4.21 ms dr, = 40.0 ari = 40.0 Or, = 32.0, Or, = 63.0, 91.12% 5.13 ms dT, = 55.0 ar„ = 20.0mm ar, = 60.0, Or, = 60.0, 92.94% 3.83 ms dTi = 60.0 ars = 60.0 aTi = 80.0, aTs = 80.0, 83.27% 5.51 ms dT, = 80.0 ar6 = 80.0 ar, = 100.0, ar, = 100.0, 77.05% 6.83 ms dT, = 100.0 ar„ = 100.0 45 Chapter 4: Numerical Examples Table 4.9: Simulation Results on Various Wrist Offset Magnitudes: Numerically Approximated Jacobian (LT. = iteration time) Wrist Offset Magnitudes ave. I.T. for pts with I.T. < 60 % of pts. with I.T. < 60 ms. in mm. rns. au = 20.0, ar, = 20.0, 95.42% 3.26 ms dr- = 20.0 ar5 = 20.0 au = 40.0, aTi = 40.0, 91.43% 4.14 ms dr, = 40.0 ars = 40.0 au = 32.0, Or, = 63.0, 89.52% 4.38 ms dT- = 55.0 aTt = 20.0mm ^ = 60.0, ar, = 60.0, 87.03% 4.83 ms dT- = 60.0 aTs = 60.0 au = 80.0, aT- = 80.0, 80.80% 5.52 ms dr, = 80.0 aT6 = 80.0 ^ = 100.0, ar5 = 100.0, 73.92% 6.28 ms dr, = 100.0 ar6 = 100.0 46 Chapter 5: Discussion Chapter 5 Discussion 5.1 Analysis of Results From the results obtained in Chapter 4, the following comments can be made: 1. As predicted by theory, if the desired inverse kinematics solution was attainable by the manipulator, then the algorithm was always able to find it. This is true for all of the manipulator geometries tested. 2. The algorithm is relatively fast. As compared with the inverse kinematics algorithm developed by Tourassis and Ang [6], the algorithm presented in this thesis was 66.7% faster (see Section 4.1.2). Also, the algorithm is suitable for use in a real time control scheme, as defined in this thesis (see Section 1.1, page 4), for both the Spherical manipulator (see Section 4.1.1), and the Modified PUMA manipulator (see Table 4.8). 3. The algorithm can have slow convergence near model manipulator singularity regions (see Table 4.6). As an example, for the Modified PUMA manipulator with wrist offsets ar, = 20.0mm, an = 20.0mm, dT:> = 20.0mm, and an = 20.0mm, and with an analytically calculated Jacobian (see Section 4.2), in approximately 0.56% of the manipulator's workspace (as measured in joint space), the algorithm required more than 100 milliseconds to converge to the desired solutioa 4. The rate of convergence of the algorithm depended on the magnitude of the wrist offsets au, ar , and dT. (see Section 4.2.2). In general, an increase in the magnitude of these offsets, gives an increase in the average convergence times of the algorithm. For example, when the wrist offset magnitudes (ari, an, and dT-) of the Modified PUMA manipulator were increased from 20 mm. to 100 mm., the number of points for which the algorithm converged to in less than 60 milliseconds dropped from 98.91% to 77.05% (see Table 4.8). 47 Chapter 5: Discussion 5. When the numerically approximated Jacobian was used, the algorithm was generally slower. For example, for the Modified PUMA manipulator, when the Modified Powell's algorithm used a numerically approximated Jacobian, convergence in less than 60 milliseconds was found on 3% fewer points then when compared to the analytically calculated Jacobian (see Table 4.9). 5.2 Improving Convergence Near Singularity Regions and Workspace Boundaries An obvious way of improving the rate of convergence is to implement the Search Algorithm in parallel. This can be done by having more than one Modified Powell's Algorithm process going at once, with each process starting at a different initial approximation of pm. Another way of improving the rate of convergence is to modify the algorithm so that it searches for intermediate solutions that lie on a straight line between the starting endpoint position of the real manipulator, and its desired endpoint position. This allows the search radius (Rs) to be significantly smaller and therefore the convergence near singularity points can be significantly improved. The one disadvantage of this modification is that if any point lying on the line between the initial endpoint position and the desired endpoint position of the real manipulator, is not within the workspace of the real manipulator, then the algorithm may not converge to the desired endpoint position. 53 Application to Other Manipulator Geometries The essence of the inverse kinematics method presented in this thesis is to take the real manipulator and systematically reduce offsets to zero until it is possible to formulate a complete inverse kinematics solution for this newly formulated model manipulator. For 6 degree of freedom (DOF) manipulators, a complete solution to the inverse kinematics problem is known when any three adjacent joints of the manipulator are intersecting [1]. Therefore if the real manipulator is reducible to a model manipulator which falls into the above class, then it follows that the complete solution to the inverse kinematics problem for the real manipulator can be obtained using the theoretical framework presented here. This 48 Chapter 5: Discussion makes the algorithm presented in this thesis extendable to almost any kind of 6 DOF manipulator. The only requirement being that the conditions of THEOREM I are satisfied. When applying the proposed algorithm to other manipulator geometries, the following modifi- cations need to be made: 1. The procedure for selecting the model manipulator (see Procedure 1 of Appendix B) will need modifications. 2. The model manipulator boundary tests (see the Modified Powell's Algorithm) will need to be determined. 3. The search radius, R„ of the Search Algorithm will need to be calculated. 5.4 Application to Redundant Manipulators The complete inverse kinematics solutions for model manipulators with more than 6 degrees of freedom require additional specifications in order to define all possible solutions. For an N DOF redundant manipulator, the (N-6) redundancies can be specified using appropriately defined (N-6) real variables; these will here be termed redundancy variable. Using redundancy variables, it is possible to write closed form inverse kinematics equations for many types of redundant manipulators. The desired redundancy variables values can be considered to be part of the configuration vector c. Thus, if one can reduce a redundant real manipulator to a redundant model manipulator which satisfies the conditions of THEOREM I, then one can formulate the complete solution to the inverse kinematics problem for the real manipulator. Therefore, as long as the real manipulator/model manipulator relationship is defined by the conditions of THEOREM I, then, no matter how many degrees of freedom the real manipulator has, the theoretical framework presented in this thesis can be used to solve the complete inverse kinematics problem of the. real manipulator. The advantages of this approach to the solution of the inverse kinematics problem for a redundant manipulator are: 49 Chapter 5: Discussion 1. Complete control over the endpoint position, endpoint orientation, manipulator configuration, and all redundancy variables is obtained. Such control is essential when the manipulators workspace is cluttered with obstacles that must be avoided. 2. The complexity of the problem never goes above the need to solve a system of 3 equations in 3 unknowns. This can allow for real time inverse kinematics calculations for redundant manipulators. 3. Complete control over redundancy variables allows manipulator singularities to be avoided. 50 Chapter 6: Conclusions Chapter 6 Conclusions A new theoretical framework has been developed for solving the inverse kinematics problem. Based on this theory, an inverse kinematics algorithm has been formulated which is theoretically guaranteed to give a complete inverse kinematics solution, as defined in this thesis, for any manipulator (of any number of degrees of freedom) that satisfies the conditions of THEOREM I. In particular, it has been shown that all 6 DOF manipulators that have non-parallel, non- intersecting wrist joints, and that have 3 degrees of freedom in the Cartesian positioning of the wrist, satisfy these conditions (THEOREM II). The algorithm has been experimentally tested on two different 6 DOF manipulator geometries which have non-parallel, non-intersecting wrist joints, and, which have no known closed form inverse kinematics equations. The following can be concluded from these tests: 1. If the desired inverse kinematics solution existed, the algorithm was always able to converge to it. 2. In general, the algorithm can be applied to real time manipulator control (as defined in this thesis, Section 1.1, page 4). 3. Although the algorithm does not fail even at singularities, it was observed that slow conver- gence was always associated with the desired solution being close to a singularity. 4. The rate of convergence was dependent on the total magnitude of the manipulator's wrist offsets. In general, the greater the total magnitude of the wrist offsets, the longer algorithm took to converge to the desired solution. 6.1 Contributions The main contribution of this thesis is the development of an iterative inverse kinematics algorithm which gives direct control over the manipulator's configuration, and which has theoretically proven 51 Chapter 6: Conclusions convergence conditions (THEOREM I). For any manipulator geometry that meets the conditions of THEOREM I, the inverse kinematics algorithm developed in this thesis is theoretically guaranteed to give a complete solution (as defined in this thesis) to the inverse kinematics problem of that manipulator. This contribution can be summarized as follows: 1. The development of a new theoretical framework for solving the robot inverse kinematics problem. This new theoretical framework reduces the N degree of freedom manipulator inverse kinematics problem, to a bounded 3 dimensional problem. 2. The development of a fast algorithm which, based on the above theoretical framework, theoretically guarantees a complete solution to the inverse kinematics problem of a large class of manipulators. 6.2 Further Research Topics for further research include the following: 1. Improving the convergence rate near singularity regions. Suggestions on how this may be done are given in Section 5.2. 2. Applying the algorithm to redundant manipulators. This is discussed in Section 5.4. 3. A more complete theoretical analysis of how the theoretical framework developed in this thesis can be applied to more general manipulator geometries. 52 References D. L. Pieper. The Kinematics of Manipulators Under Computer Control. PhD thesis, Comput. Sci. Dep. Stanford Univ., Oct 1968. J. Duffy. Analysis of Mechanisms and Robot Manipulators. Edward Arnold, London, 1980. Jr. M.H. Ang. and V.D. Tourassis. Flexible manufacturing using modular robotic wrists. In Int. Conf. on CAD/CAM, Robotics, and Manufacturing of the Future, Southfield, MI, Aug 1988. L. W. Tsai and A. P. Morgan. Solving the kinematics of the most general six- and five-degree-of- freedom manipulators by continuation methods. Trans. ASME J. Mech. Transmiss., Automat. Des., 107(2): 189-200, June 1985. R. Manseur and K. L. Doty. A fast algorithm for inverse kinematics analysis of robot manipulators. Int. J. Robot. Res., 7(3):52-63, June 1988. V. D. Tourassis and JR. M. H. Ang. A modular architecture for inverse robot kinematics. IEEE Trans. Robotics and Automat., 5(5):555-568, Oct 1989. J. Angeles. On the numerical solution of the inverse kinematics problem. Int. J. Robot. Res., 4(2):21- 37, Summer 1985. C. W. Wampler. Manipulator inverse kinematics solution based on vector formulation and damped least squares. Syst., Man, Cybern., SMC-16(1):93-100, Jan./Feb. 1986. Chi-Haur Wu and Kuu-Young Young. An efficient solution to the differential inverse kinematics problem for wrist partitioned robots. IEEE Trans. Robotics and Automat., 6(1): 117-123, Feb. 1990. D. E. Orin and W. W. Schrader. Efficient computation of the jacobian for robot manipulators. Int. J. Robotics Res., 3(4):66-75, Winter 1984. Y. Nakamura. Kinematical Studies on the Trajectory Control of Robot Manipulators. PhD thesis, Kuoto, Japan, June 1985. S K C. Chan. An iterative general inverse solution with variable damping. Master's thesis, Department of Electrical Engineering, University of British Columbia, April 1987. D. Hutchinson. Manipulator inverse kinematics based on recursive least squares estimation. Master's thesis, Department of Electrical Engineering, University of British Columbia, December 1988. Z R. Novankovid and Bojan Nemec. A solution to the inverse kinematics problem using the sliding mode. IEEE Trans. Robotics and Automat., 6(2):247-252, April 1990. A A. Goldenberg, B. Benhabib, and R. G. Fenton. A complete generalized solution to the inverse kinematics of robots. IEEE Trans. Robotics and Automat., RA-l(l): 14-20, March 1985. 53 Y. T. Tsai and D. E. Orin. A strictly convergent real-time solution for inverse kinematics of robot manipulators. J. Robotic Syst., 4(4):477-501, Aug. 1987. J. K.-S. Poon. Multiprocessor-Compatible Inverse Kinematics and Path Planning for Robots. PhD thesis, Department of Electrical Engineering, University of British Columbia, May 1988. J. S. Albus. Theoretical and Experimental Aspects of a Cerebellar Model. PhD thesis, University of Maryland, December 1972. D. Marr. A theory of the cerebellar cortex. /. Physiol. Land., 202:437, 1969. HI W.T. Miller, R. P. Hewes, F. H. Glanz, and III L. G. Kraft Real-time dynamic control of an industrial manipulator using a neural-network-based learning controller. IEEE Trans. Robotics and Automat., 6(l):l-9, Feb. 1990. D. Klett A cerebellum-like learning machine. Master's thesis, Department of Electrical Engineering, University of British Columbia, July 1979. C. K. Huscroft. Learning algorithms for manipulator control. Master's thesis, Department of Electrical Engineering, University of British Columbia, October 1984. J. Barhen, S. Gulati, and M. Zak. Neural learning of constrained nonlinear transformations. IEEE Computer, pages 67-76, June 1989. T. M. Martinetz, H. J. Ritter, and K. J. Schulten. Three dimensional neural net for learning visuomotor coordination of a robot arm. IEEE Transactions on Neural Networks, 1(1): 131-137, March 1990. A. P. Morgan. A method for computing all solutions to systems of polynomial equations. ACM Trans. Math. Software, 9(1): 1-17, March 1983. E. Zeidler. Nonlinear Functional Analysis and Its Applications I. Fixed Point Theorems. Springer- Verlag, New York, NY, 1986. F. L. Litvin and J. Tan. Sigularities in motion and displacement functions of constrained mechanical systems. Int. J. Robotics Res., 8(2):30-43, April 1989. F. L. Litvin, P. Fanghella, J. Tan, and Yi Zhang. Singularities in motion and displacement functions of spatial linkages. Trans. ASMEJ. Mech. Transmiss., Automat. Des., 108(4):516-523, 1986. F. L. Litvin, V. Parenti Castelli, C. Innocenti, and Yi Zhang. Singularities, configurations, and displacement functions of manipulators. Int. J. Robotics Res., 5(2):52-65, 1986. K. J. Waldron, S. L. Wang, and S. J. Bolin. A study of the jacobian matrix of serial manipulators. Trans. ASME J. Mech. Transmiss., Automat. Des., 107(2):230-237, 1985. K. Sugimoto, J. Duffy, and K. H. Hunt. Special configurations of spatial mechanism and robot arms. Mechanism and Machine Theory, 17(2): 119-132, 1982. M. J. D. Powell. A hybrid method for nonlinear equations. In P. Rabinowitz, editor, Numerical Methods for Nonlinear Algebraic Equations, pages 87-114. London, UK: Gordon and Breach, 1970. 54 [33] C. G. Atkeson. Learning arm kinematics and dynamics. Ann. Rev. Neurosci., 12:157-183, 1989. [34] J. Denavit and R. S. Hartenberg. A kinematic notation for lower-pair mechanisms based on matrices. J. App. Mech., 77:215-221, 1955. [35] K. S. Fu, R. C. Gonzalez, and C .S. G. Lee. Robotics: Control, Sensing, Vision, and Intelligence. McGraw-Hill, New Yak, NY, 1987. [36] W. H. Press, B. P. Flannery, S. A. Teukolsky, and W. T. Vetterling. Numerical Recipes in C: The Art of Scientific Computing. Cambridge University Press, New York NY, 1988. [37] J. M. Ortega and W.C. Rheinboldt. Iterative Solutions of Nonlinear Equations in Several Variables. Academic Press, New York, NY, 1970. 55 Appendix A Proof of Theorems THEOREM /: Given a real manipulator and a model manipulator, assume that the following conditions are true: CI. The only difference between the model manipulator and the real manipulator are the values of the Denavit-Hartenberg parameters ai and di. C2. There is a complete solution (as defined in this thesis) to the inverse kinematics problem for the model manipulator; i.e. for any fixed configuration c, the function qm = k^!(xm, c) is continuous (with respect to xmJ, and, i/(xm,c) £ Um (Um defines the singularity region of the model manipulator) the function is also one to one. C3. q = qm = qr C4. The configuration of model manipulator is always identical to the configuration of real manipulator; i.e. qro = qr cm = cr, where cm is the configuration of the model manipulator, and Cr is the configuration of the real manipulator. C5. For every endpoint position, endpoint orientation and manipulator configuration which lies in a singularity region of the model manipulator, there exists another endpoint position which is arbitrarily close to the previous endpoint position, butwhich is not in a singularity region; i.e. if all the previous conditions of the theorem are met, and if([ nrj sri arj pi],c) 6 then there exists an p2m e & such that - plm}\ < e, where £ is an arbitrarily small positive real number, and ([xiri sTi a^ p„],c) £ Um. C6. The desired inverse kinematics solution exists. Then, with the real manipulator at the desired endpoint position, endpoint orientation (xTd = [nr sr ar, pTd f). and manipulator configuration (c), for any Emax > 0 (Emax € there exists an approximation of the endpoint position of the model manipulator, pm„ G such that 56 (rewriting system (2.11)) 1 ||Pm. - Ap(k^ (xm.,c)) - prJ < Emax, (A.1) T where Xm. = [nr„ srj a.Ti pm„ ] - Proof of THEOREM I Throughout the proof, condition C6 is assumed valid. First, because the mapping qm = k-'(xm,c) is continuous w.r.t. xm (condition C2), and because the forward kinematics function Ap(-) is also continuous (this is true because all forward 1 kinematics functions are continuous), then the mapping pm = pr, + Ap(k^ (xm,c)) must also be continuous. The theorem will first be proven for the case where the desired solution is not in a singularity region of the model manipulator. A singularity region, Um, is defined as the set of all (xm, c) e Um, 1 r such that if for one set of joint variables, q , one can write (xm,c) = k(q ) (k(-) is a function that gives both the forward kinematics and the configuration of the manipulator), then there exists a 2 2 1 2 second set of joint variables, q , where q f q and (xm,c) = k(q ) [27][28][29][30][31]. Because the model manipulator is not in a singularity region, then, by condition C2, the inverse kinematics of the model manipulator is one to one; i.e. the mapping qm = k-'(xm,c) is one to one. Because the model manipulator differs from the real manipulator by only the values of the Denavit-Hartenberg parameters a,- and dx (condition CI), the hand (endpoint) orientation of the model manipulator is always identical to the hand orientation of the real manipulator, i.e. if condition C3 holds (q = qm = qr) then [ n™ sm am ] = [ nr„ sr„ arj ]. This can be shown using homogeneous 1 transformation matrices. Thus the equation pm = pr, + Ap(k- (xm, c)) must completely define the relationship between the model manipulator and the real manipulator. Because the model manipulator and real manipulator always have the same manipulator configurations (condition C4), for all unique 57 values of (x^,c) there is one and only one corresponding value for pm. This is true because all possible solutions to any desired inverse kinematics problem (when there is no singularity condition) 1 ) 1 can always be distinguished by configuration; i.e. for all unique x™ and q , if (xm,c ) = k(q ), 1 2 2 1 and if c ^ c , then (x™, c ) f k(q ). This proves that the value of pm is unique when the model manipulator is not in a singularity region. 1 Because the mapping pm = pu + Ap(k~ (xm,c)) is continuous and is one to one in the nonsingularity case, then by the definition of continuous functions, it is concluded that there exists a 3 pm. 6 ft which is sufficiently close to pm, such that for ||Pm. - , c)) - prJ - Emax T (where v = [iv, sTi arj pm„ ] ), as Emax 0, Pm. pm. This proves the theorem for the case where there is no model manipulator singularity condition. By condition C5, if the model manipulator is in a singularity region then there exists a p^ e ^ such that ||p^ - Pm|| < £> where e is an arbitrarily small positive real number, and ([nr4 srj ar 1 in a singularity region, must give a mapping pm. = prj + Ap(k- (xm„, c)) which is one to one. 3 Therefore, as already proven above, there exists a pm. € R which is sufficiently close to pm> 1 such that for ||pm. - Ap (k~ (x™,, c)) - pr J = Emax (where x^ = [ nrd sr, ar, Pm„ f) as Emax 0, pm Pm (the value of pm must once more be unique). However, under these conditions, Emax can be arbitrarily small but never zero, and Pm„ can be arbitrarily close to the unique value pm, but never equal to it Q.E.D. 58 THEOREM II: Manipulators that satisfy all of the following conditions, satisfy the conditions of THEOREM I : CI. The real manipulator has 6 serial degrees of freedom. C2. Joint variables 1, 2, and 3 give three degrees of freedom for the Cartesian positioning of joint 4. C3. Joints 4, 5, and 6 of the real manipulator are all a. rotational. b. non-parallel (i.e. aTi ^ 00 or ± 180° and aT, ± 0° or ± 180°). c. non-intersecting (i.e. not all of the aTi, ar. and dT. are zero). C4. The model manipulator has been created using Procedure 1 of Appendix B. C5. qm = qr C6. The desired inverse kinematics solution exists. Proof of THEOREM H Condition C6 of THEOREM II is identical to condition C6 of THEOREM I, and, Condition C5 of THEOREM II is identical to condition C3 of THEOREM I. Thus it only remains to be proven that conditions CI, C2, C3, and C4 of THEOREM II imply conditions CI, C2, C4, and C5 of THEOREM I. Satisfying Condition CI, of THEOREM I. This condition is explicitly satisfied by Procedure 1; i.e. by condition C4 of THEOREM II, only the Denavit-Hartenberg parameters aTl, ar. and dr:> are modified when creating the model manipulator. Satisfying Condition C2, of THEOREM I. By condition C4 of THEOREM II, am, = 0, am - 0 and dm- = 0. Therefore, the last three links of the model manipulator are intersecting and thus a complete solution to the inverse kinematics problem for the model manipulator must exist [1]. Thus condition C2 of THEOREM I is satisfied. 59 Satisfying Condition C4, of THEOREM I. By conditions C4 and C5 of THEOREM II, the model manipulator and the real manipulator are identical from the base to the wrist (i.e. the first 3 joints), and therefore the configurations which are associated with the first 3 joint variables of both manipulators, must be identical between the two manipulators. The hand configuration of the model manipulator can be defined to be dependent only on the desired orientation of the hand and the position and orientation of the wrist coordinate system. Let the wrist coor- dinate system (i.e. the coordinate system of joint 4) be given by °A3 = F(gi,g2,?,i)- °A3 can be obtained using homogeneous transformation matrices [35, pp. 35-44]. Because the wrist joints all intersect, one can write (gi, 92,93) = k^'(pm - dm6arj - amtnr„,c) [1], and therefore °A3 = F^x™). Because °A3 = Fi(xm) is a homogeneous transformation ma- T trix, one can write [n£ s w a I 0] = °A3[n£ a£ 0], where [n£ s I aI 0] is the desired orientation of the hand in the coordinate frame of the wrist. The hand con- figurations, , can now always be defined as c = ch h 0]), and therefore ch - f2(°A3[n^ sjd 0]) (see [35, pp. 74-75] for an example of this). Because this function is independent of wrist offsets, it is the same for both the model manipulator and the real manipulator. Because the model manipulator and the real manipulator have identical hand orientations and wrist coordinate systems, it can be concluded that they also have identical hand configurations. Thus condition C4 of THEOREM I is satisfied. Satisfying Condition C5, of THEOREM I. This condition will be shown to be true by prov- ing it for 2 separate cases. Case 1: Assume that the model manipulator is in a singularity region which is associated with the first 3 joint variables; i.e. for the desired manipulator configuration and hand (end- point) position and orientation (x^,c) (where x^ = [xv< srd arj p^ ]), there are more than one set of values for the joint variables which give the required wrist posi- tion defined by pi, = P^ - dmi arj - amsnrj. Then, by condition C2 of THEOREM II, and because plw = is a continuous forward kinematics function which gives 60 the position of the wrist, one can find a set of joint variable values (q{, q%, qj) such that || Wiill^l) ~ II < £ (where e G U is an arbitrarily small positive number), and 2 Vi = P® («i > ih 4) ± Pi (where p^ = p + dmta^ + amen^, x^ = [ nr Case 2: Now assume that the singularity is a result of the desired hand orientation; i.e. for the desired hand configuration, orientation and position, (x^, c), there is more than one set of valid joint variable values (q\,ql,ql). Because the last 3 joints are all rotational, a hand orientation singularity can occur if and only if any two of the last 3 joints becomes parallel (and thus give the same motion in orientation space). By condition C3 of THEOREM II, the last three joints give 3 degrees of freedom to the orientation of the hand, and thus joint 4 is never parallel to joint 5, and joint 5 is never parallel to joint 6. Therefore, a hand orientation singularity will occur if and only if joint 4 becomes parallel to joint 6. As will be proven in the following lemma, this can only occur at specific values of the joint variable q5 (for example, a wrist singularity of the PUMA 560 manipulator occurs when <75, = 0°). LEMMA: By conditions CI, C2, and C3 of THEOREM II, ifq\ = a, where o G 8 gives a hand ori- entation singularity, corresponds to the Cartesian wrist position pi,, and if 0 < ||p2 - pi|| < £ (e is an arbitrarily small positive real number) and p2w does not produce a singularity condition in the first 3 joints of the manipulator, then it can be concluded that q\ ^ o, and p% does not correspond to a hand orientation singularity. Proof of lemma (by contradiction): Assume that pi is also in a hand orientation singularity T region. Let °A3 = [n I s w a I p£] (see [35, pp. 40-44]) be the homogeneous transform from the base coordinate system to the coordinate system of joint 4. In this coordinate frame, the desired hand orientation is given by [ n^ s* a^ 0]=°A3[n£ 0], If we move form plw to pi, then the joint variables (q{,q^q^) must change to (q^q^qfy such that ||(g?>?2>93) ~ < £ (where £ € R is an arbitrarily small positive number). There- 61 fore, [n£ s£ a£ 0] = a^ 0] must change to [ s£ a£ 0] = ° A|[ &Jd 0 ]. By conditions C2 and C3 of THEOREM II, and because pi does not produce a singularity condition in the first 3 joints of the manipulator, it must be the case that f ± *£*> and f However, p^ is in a hand orientation singularity condition and therefore joint 4 is parallel to joint 6. Thus changes in q\ and q^ cause rotation about the a^ (this is true because, by the definition of the homogeneous transformation matrix [35, pp. 36-44], at pi, the z-axis of the wrist coordinate frame is parallel to a^, and a change in q\, which is parallel to q^ causes rotation about the z-axis). Thus changes in qlA and qfr can only cause changes in and Therefore, if pi is in a hand orientation singularity condition, then = a^. This is a contradiction and therefore q5 must change to a value such that q\ ± o - qi, and p2w cannot be in a hand orientation singularity condition. This completes the proof of the lemma. Therefore, because the position of the coordinate system of joint 4 can be defined by pi, = 1 nMrm - ^7716d a Td- a ™ TfX^n. , bTjy th» eJ above lemma, condition C5 of THEOREM I is satisfied when the singularity is associated with the last 3 joint variables. Thus condition C5 of THEOREM I is satisfied. Q.E.D. 62 THEOREM TV: Given that the following conditions are true: CI. The desired inverse kinematics solution for the real manipulator exists. C2. Rs > ||Pm, -pm||. Then there exists a finite positive integer nmax such that the Search Algorithm (Algorithm 3) will generate an initial starting approximation of pm that will allow the Modified Powell's algorithm to find a pm„ which will satisfy equation (3.4). Proof of THEOREM IV The Search Algorithm searches an area which is defined by a sphere of radius Rs that is centered at pmi- Therefore if the conditions CI and C2 of this theorem are met, then the solution must be within the search area of the algorithm. By THEOREM I, there always exists a pm„ which is a finite nonzero distance away from pm, and which satisfies ||pOT„ - Ap(k~1(xma, c)) - pr J < Emax where Emax > 0 is an arbitrarily small real number and xm„ = [ nr„ sTd arj pm. f. For any such pm„, let $ = ||pma - pm|| (see Figure A.l). At stage n of the Search Algorithm, the maximum spacing between any two adjacent points generated by the algorithm is • ^ • Rs. Therefore the Search Algorithm must generate a pm„ which satisfies ||pm„ - Ap(k by stage nmax, where nmax is a positive integer that satisfies the equation nmax > log2 ^^ (this is obtained from the following equation: $ = >/3 • £ • Rs)- This nmax must exist and be finite because $ is nonzero and finite. Therefore, because the Modified Powell's Algorithm tests whether the initial estimate which it receives from the Search Algorithm is within error tolerances (Emax), the search for the solution will end when n > nmax- Q.E.D. 63 The dotted lines indicate corresponding model manipulator and real manipulator endpoint positions. Note that the circle represents a sphere of radius R,; the real manipulator endpoint positions prd and pr„, may or may not lie within this sphere. Figure A.1: Relative Positioning of Manipulator Endpoint Positions 64 Appendix B Pseudo-code Descriptions of the Procedure and Algorithms Procedure 1: Generating the model manipulator. STEP 1. FOR i = 1,2,3,6. a. ami = Or; b. «m, = c. dm, = dTi d. Qrm = STEP 2. FOR i = 4,5. a. b. ami = 0 i = 4 c. dm. = 0 i = 5 d. Algorithm 1 The Main Algorithm STEP 1. READ xTi, c. STEP 2. xm, = xr„. STEP 3. IF (pm, i Sm) a. Chose a pm which is close to pr, but which is still within the model manipulator's workspace (defined by Sm). 65 STEP 4. q, = k^(xm.,c). STEP 5. pri = pm, - Ap(qi). STEP 6. Xrj = [llrd Sri &rd prif. STEP 7. xm. = Xm'.. STEP 8. Xr. = Xrr STEP 9. qa = q, . STEP 10. EXECUTE Fixed Point Algorithm. STEP 11. IF ( successful = 1 ) GOTO STEP 16. STEP 12. EXECUTE Search Algorithm. STEP 13. IF ( successful = 1 ) GOTO STEP 16. STEP 14. point_beyond_reach = 1. STEP 15. GOTO STEP 17. STEP 16. point_beyond_reach = 0. STEP 17. IF (ftm,„ < ft. < for all j) THEN a. joint_limit_reached = 0. ELSE a. joint_limit_reached = 1. b. ft. = max {ftm,„, ft.}, for all j. c. ft. = min {ftm„, ft.}, for all j. STEP 18. OUTPUT q0> point beyond reach, jointjimit reached. STEP 19. GOTO STEP 1. Algorithm 2 The Fixed Point Algorithm STEP 1. k = 0. STEP 2. p£ = Pr.- 66 STEP 3. p^ = pm.- STEP 4. q* = qa. STEP 5. Ek = ||Prj -Prfc||. STEP 6. IF (Ek < Emax) a. successful = 1. b. EXIT. STEP 7. IF (k > kmaXF) a. successful = 0. b. EXIT. fc STEP 8. = pri + Ap(q ). +1 STEP 9. IF (p^ i Sm) a. successful = 0. b. EXIT. STEP 10. p*+1 = pJk - Ap(qfc). k+1 +1 STEP 11. E = ||Prj -p^ ||. STEP 12. IF ((£fc+1 > P • Ek) OR (k ± 0)) a. successful = 0. b. EXIT. STEP 13. k = k + 1. STEP 14. Pr„ = p*. k STEP 15. pm„ = P m- 1 T STEP 16. = 1Q ([nTj sTi arj p£j ,c). k STEP 17. qa = q . STEP 18. GOTO STEP 6. 67 Algorithm 3 The Search Algorithm STEP 1. IF (pm> = pm.) a. EXECUTE Modified Powell's Algorithm. IF (successful = 1) • EXIT. ELSE • GOTO STEP 2. ELSE a. EXECUTE Modified Powell's Algorithm. b. IF (successful = 1) • EXIT. ELSE • Pr„ = Pr.- Pm, = Pm,- • qa = q.- EXECUTE Modified Powell's Algorithm. IF (successful = 1) . EXIT. ELSE . GOTO STEP 2. STEP 2. IF (||pm. - Pm,|| < R,) a. Elm = ||Pr. - PrJ|- b. Pr,„ = Pr.- c- Pmim = Pm.- d. q im = q»- 68 ELSE a. Elm = ||pr> - prJ. b. Prim = Pr,- C- Pmim = Pm;- d. q;m = q,- STEP 3. n = 0. STEP 4. n = n + 1. STEP 5. IF (n > nmax) a. successful = 0. b. EXIT. STEP 6. search_local_min = 0. STEP 7. ix = 1. STEP 8. signx = 1. STEP 9. iy = 1. STEP 10. sign,, = 1. STEP 11. ix = 1. STEP 12. sign, = 1. STEP 13. IF (searchlocalmin = a- frn,. = PmIi + ix • signx " b. Pmya = Pm+ iy • signy • c. Pm.. = + iz • sign2 • ELSE a. Pm.. = Pm.,m + ix • signx b. Pmy. = Prn>lm + iy • Sig^ c. pm.. = Pm,,,. + iz • sign2 n STEP 14. IF ((pTO. i Sm) OR (||pm. - Pm,|| > R,) OR (ix > 2 )) a. ix = 1. b. IF (signx = 1) • signx = -1. • GOTO STEP 13 ELSE signx = 1. iy = iy + 2. . IF (iy > 2n) • = • IF (signy = 1) • signv = -1. • GOTO STEP 13 ELSE • signy = 1. • iz = iz + 2. . IF (iz > 2n) • IF (signz = 1) • iz = 1. • sign2 = -1. . GOTO STEP 13 ELSE IF (search local min = 0) searchlocalmin = 1. • GOTO STEP 7 70 ELSE • GOTO STEP 4. ELSE • GOTO STEP 13 ELSE • GOTO STEP 13 STEP 15. qa = k^Hxm^c). STEP 16. pr. = pm„ - Ap(qa). STEP 17. EXECUTE Modified Powell's Algorithm. STEP 18. IF (successful = 1) a. EXIT. STEP 19. E = ||pr. -PrJ. STEP 20. IF (E < Elm) a. Elm = E. b- Pr,„ = P r.- c = - PmIro Pm« • d. q im = STEP 21. ix = ix + 2. STEP 22. GOTO STEP 13. Algorithm 4 The Modified Powell's Algorithm STEP 1. jfe = 0. STEP 2. p* = pr.- STEP 3. p^ = Pm.- fc STEP 4. q = qa 71 k 1 STEPS. f(Pm) = [/l(Pm) h(p m) h (Pm) f = Pm. ~ Ap (k^ , c))-prj, where = srj &T, pif. STEP 6. F(p*J = E [/.'(Pm)]2- i= 1 STEP 7. A max — STEP 8. IF (A max — Ejjiax) a. successful = 1. b. EXIT. (k > STEP 9. IF kmaxp ) a. successful = 0. b. EXIT. STEP 10. Calculate the Jacobian Matrix: J = . STEP 11. Calculate the inverse Jacobian Matrix: J-1 = STEP 12. Calculate the Newton-Raphson estimate of £ as follows: a. 6 = -J-lf(pkm). b. IF (||i|| > ATOOx) THEN £ = AmaX|j!|j. c. IF «(p* + I) t Sm) OR (||p^ + 6 - Pm> || > R.)) Calculate A such that A < the minimum distance between the boundary of all possible solutions and p^. • IF (A < Amin) NR_error = 1. • GOTO STEP 13. ELSE NR_error = 0. • i = A • gjy d. IF(F(pi+A) < /3F(p^)) valid delta = 1. 72 • GOTO STEP 16. ELSE valid_delta = 0. STEP 13. Calculate the Gradient Descent estimate of i as follows: - „ - r 9F(p~) dF(PS.) dFto) f b. 6_ = — Amox|j^jj. c. IF (((p^ + 6) £ Sm) OR (||p^ + 6 - pmi || > R.)) Calculate A such that A < the minimum distance between the boundary of all possible solutions and p^. • IF (A < Amin) Grad_error = 1. • GOTO STEP 14. ELSE Graderror = 0. • $ =A • A- d. W{F{pkm + 6) < (3F{pkm)) • valid_delta = 1. • GOTO STEP 16. ELSE valid_delta = 0. STEP 14. Calculate an estimate of S_ which is a linear combination of the Gradient Descent estimate and the Newton-Raphson estimate as follows: a. Find 7 such that |(1 - 7)$fg + 1J lf(Pm) = A•max- b. IF (7 does not exist) NRG_error = 1. . GOTO STEP 15. 73 ELSE • NRGerror = 0. c. 6 = (l-T^g + TJ-1^). d. IF (((pi + £)# Sm) OR (||pi + 6 - Pm, || > R,)) • Calculate A such that A < the minimum distance between the boundary of all possible solutions and pi- • IF (A < Am.'n) • NRG_error = 1. • GOTO STEP 15. ELSE • NRG_error = 0. • = A • A- e. W(F(pkm + 6) < l3F{pkm)) valid_delta = 1. ELSE valid_delta = 0. f. GOTO STEP 16. STEP 15. IF ((NRerror = 1) AND (Grad_error = 1) AND (NRG_error = 1)) a. successful = 0. b. EXIT. STEP 16. IF (valid delta = 1) a. p^1 = pi + L +1 b. pm. = Pi - c. qa = k-HPm^c). d. Pr. = Pm. - Ap(qa). e. k = k + 1. 74 f. Pr = Pr.- g- q* = qa- 1 T h. f(Pm) = Pm. - Ap(k- ([nrj srj arj p^ ] ,c)) - pr,. i. F(Pi) = biHpi)]2. »=i j. Am ax = k. IF (A max — Fmax) successful = 1. • EXIT. ELSE • GOTO STEP 9. ELSE 3,. Amaa; — 2^max« b. IF (A max A min) • successful = 0. • EXIT. ELSE GOTO STEP 12. 75 Appendix C The Inverse Kinematics Equations of the Spherical Model Manipulator AR.M • py" j! = atan2 ARM -pxy am2b + ELBOW •gA/o2 + fc2-a„ q2 = atan2 am2a - ELBOW • b^/a1 4- 62 - om2 cob»9 2o ' if cos ** r «3 : — sin 92 otherwise ' WRIST •V g4 = atan2 .WRIST •Az_ WRIST -Al = atan2 Vi -WRIST Oz qc, = atan2 WRIST • Nz where Px — PmI — «m|»>nIi Py = Pmv - am6«my, Pz = Pm, - Omj'»m, > a = px co6+ pj,smgi, b = pz - dml, Ax = (- sin ?! cce^)ar ,4y = (- cos 91 cos q-i)aTii + (- sin cos «2)arjy + B'n ftK^ , Az = (sin qi)arix + (-cos , ' O, = (sin ?i)«rjr + (- cos , Nz = (sin qi)nTix + (- cos «i)nrji:, 76 The function atan2 is defined in [35, p55] as follows: 0° < q < 90° for + xand + y y 90° < q < 180° for - xand + y q = atan2 - (C.3) -180° < q < -90° for - xand - y „ -90° < q < 0° for + xand - y Note that the configuration vector is c = [ARM ELBOW WRIST]. (C.4) The manipulator singularities are handled as follows: 611 1. If px = Py = 0' th qi retains its previous value and the rest of the joint variables are calculated as defined above. 2. If, in addition to the above singularity, pz - drni, then both qi and q2 retain their previous values and the rest of the joint variables are calculated as defined above. 3. If Ax = Ay = 0, then retains its previous value and (C.5) qh = 180°, g6 = -qi + atan2 or (C.6) where Ox = (-smtLco6 92)er^ + (~sinqiemq2)sriji + (coeq2)srjz, Nx = (- sin 51 coe «2)nrji + (- sin sin q-i)nrjy + (coe ?a)>irji • 77 Appendix D The Inverse Kinematics Equations of the Modified PUMA Model Manipulator _ , T f ~ARM • Vy • r - Vi • qi~ —AR.M 'Px 'T + Py • d-m? . SaCp + (ARM • ELBOWQCgS, q2 = atan2 - (ARM • ELBOW)SaS> -SpCJgr - y - v r(jpS ( q3 z= atan2 cpc„+spsffy u WRISWHIMT -• [(-•|a (Ci m,u -ai „ r {CiCnCi - 5154)om. + (ftCaA + CiftK,, - C4.?gom, atan22[ CiS^a-mI , +i Sc.L Snamy + Cztam, J [FLIP • ((—5i<74 -frCaflQwm. + ((7t(74 - S^aft)"™, + S4SanTO.) 9f, = atan2 [FLIP • ((-5! (74 - C,(7235r,)smi + (<7,<74 - Si<7aS4)«mi, + where Pi = Pm* — dmtsamx — ameiimx, n Py Pmy — dmeamii — °m» m,, Pi = Pm, ~ dmiamz - am„nm,, i?.= ijpi + P$ + Pi - Pz Sa = R' AR.M • r c„ = - i?. ' (-a — 2amj if. = ^C}, <7, .9P = ARM • ELBOW0. - Cj, C.= a >/ 78 The function atan2 is defined in Appendix C. Note that the configuration vector is c = [ARM ELBOW WRIST FLIP]. (D.3) The wrist singularity of q5 = 0 is handled by letting g6 as defined above. 79