A N ITERATIVE GENERAL INVERSE KINEMATICS SOLUTION WITH VARIABLE DAMPING by STEPHEN K.C. CHAN A THESIS SUBMITTED IN PARTIAL FULFILMENT OF THE REQUIREMENTS FOR THE DEGREE OF MASTER OF APPLIED SCIENCE in THE FACULTY OF GRADUATE STUDIES Department of Electrical Engineering We a^ccept this thesis as conforming to the required standard THE UNIVERSITY OF BRITISH COLUMBIA April 1987 © Stephen K.C. Chan, 1987 In presenting this thesis in partial fulfilment of the requirements for an advanced degree at The University of British Columbia, I agree that the Library shall make it freely available for reference and study. I 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. Department of Electrical Engineering The University of British Columbia 2075 Wesbrook Place Vancouver, Canada V6T 1W5 Date: Apri l 1987 ABSTRACT Currently, there is much interest in the field of robotics in researching methods of obtaining inverse kinematics solutions for arbitrary manipulators. Simple closed-form inverse kinematics equations can be obtained for a few joint configurations using geometric methods. However, there exist many manipulators which were not originally designed for kinematic control which do not have simple closed-form inverse kinematics equations. An efficient and stable iterative method is investigated in this thesis which solves the general inverse kinematics problem without detailed analysis of the manipulator's structure. The proposed iterative inverse kinematics algorithm combines a calibration procedure to estimate the manipulator's Denavit-Hartenberg parameters with an iterative method using the Jacobian and damped joint corrections. The kinematics control algorithm parameters are selected with a computer graphics simulation of the manipulator. The proposed inverse kinematics algorithm is tested with a simulation of an industrial manipulator arm which does not have a closed-form solution, RSI Robotic Systems International's Kodiak arm, and exhibits stability in all regions of operation and fast convergence over most regions of operation. ii TABLE OF CONTENTS Abstract ii Acknowledgements v» Chapter 1. Introduction 1 Chapter 2. INVERSE KINEMATICS 9 2.1. Manipulator Regions 9 2.2. Approximation by a Multinomial 14 2.3. Transform Methods 17 2.4. Iterative Schemes 22 Chapter 3. PROPOSED KINEMATIC CONTROL METHOD 28 3.1. Generating the Jacobian 28 3.2. Damping in a Single Link Manipulator 33 3.3. Damping in the Multi-Jointed Manipulator 39 3.4. The Positioning Problem 40 3.5. The Orientation Problem 50 3.6. Six Degrees of Freedom Manipulators 55 3.6.1. Iterative Algorithm with Closed-Form Orientation 57 3.6.2. Partitioned Iterative Method 64 3.6.3. Full Six Degrees of Freedom Iterative Method ... 64 3.7. Evaluation of the Proposed Inverse Kinematic Algorithm ... 76 Chapter 4. PARAMETER ESTIMATION 82 4.1. NLSQ Methods 84 4.2. Vector Method 86 4.3. Evaluation of the Proposed Calibration Method 89 4.4. Data Collection 90 Conclusions 93 Suggestions for Further Research 96 References 98 Appendix A 102 Appendix B 104 Appendix C 106 Appendix D 108 i i i List of Figures 1.1 Master/Slave control 2 1.2 Denavit-Hartenberg representation for a) revolute and b) prismatic joints 4 1.3 Typical industrial machine with joint controls 7 2.1 Manipulator regions for a two degree of manipulator 11 2.2 Kinematics equations for the elbow manipulator 14 2.3 Inverse kinematics equations for the elbow manipulator 15 2.4 Closed loop mechanism with hypothetical seventh joint 20 3.1 Damped and undamped inverse Jacobian for the one jointed manipulator ...34 3.2 Newton's method to solve the arccosine function for a) good target b) singular target 38 3.3 Two link two jointed and three jointed manipulators 41 3.4 Mulitple solutions in the workspace for two jointed arm 43 3.5 Convergence path of two jointed arm in the jointspace 44 3.6 Higher position error when the elbow oscillates between attitudes 46 3.7 Effect of different starting positions on attitude 48 3.8 Convergence to the target in the jointspace with joint locking 49 3.9 Two types of manipulator wrists 52 3.10 Primary and Secondary workspaces 56 3.11 RSI's RT3 arm and Kodiak arm 60 3.12 Convergence with closed-form orientation and partitioning, from joint vector (0,1,1,0,1,0) to (1,0,1,0,1,0) for the Kodiak 61 3.13 Convergence to a singular wrist for closed-form orientation, from joint vector (0,1,1,0,1,0) to (1,0,1,0,0,0) for the Kodiak 62 3.14 Ten thousand random test points in jointspace with closed-form orientation for the Kodiak 63 3.15 Convergence to a singular point with partitioning, from joint vector (0,1,1,0,1,0) to (1,0,1,0,0,0) for the Kodiak 65 3.16 Convergence to a good point with partitioning from joint vector (0,1,1,0,1,0) to (1,0,1,0,1,0) for the Kodiak 66 3.17 Ten thousand random test points in the jointspace with partitioning for the Kodiak 67 3.18 Convergence to singular elbow, from joint vector (1,-1,1,0,1,1) to (0,0,0,0,1,1) for the Kodiak 70 3.19 Convergence to singular wrist, from joint vector (0,0,1,0,1,0) to (-1,-1,1,1,0,0) for the Kodiak 71 3.20 One hundred thousand random test points for the Kodiak arm without partitioning 77 4.1 Projection of the target onto the joint axes 88 D. 1 Convergence to a singular elbow and wrist, from joint vector (-1,1,-1,1,1,0) to (0,0,-1.57,0,0,0) for the RT3 arm 109 D.2 Convergence to a singular elbow, from joint vector (-1,1,-1,1,3,0) to (0,0,-1.57,1,2,0) for the RT3 arm 110 D.3 Convergence to a singular wrist, from joint vector (-1,1,-1,1,1,0) to (-2,0.5,-1,1,0,0) for the RT3 arm I l l D.4 Convergence to a singular elbow and wrist from joint vector (-1,1,-1,1,1,0) to (0,0,-1.57,0,0,0) for the Puma 560 arm 112 D.5 Convergence to a singular elbow, from joint vector (-1,1,-1,1,3,0) to (0,0,-1.57,1,2,0) for the Puma 560 arm 113 iv D.6 Convergence to a singular wrist, from joint vector (-1,1,-1,1,1,0) to (-2,0.5,-1,0,0) for the Puma 560 arm 114 v List of Tables 3.1 Kodiak Denavit-Hartenberg Parameters ..58 4.1 Parameter errors for measurements of 0.01m accuracy with 8 samples per joint 91 4.2 Parameter errors for measurements of 0.001m accuracy with 8 samples per joint 91 vi ACKNOWLEDGEMENTS I would like to thank my supervisor, Dr. Peter D. Lawrence, for his encourangement and support. I would also like to thank my fellow students, Joseph Poon and Derek Hutchinson, for their helpful discussions. I dedicate this thesis to my parents. vii C H A P T E R 1. I N T R O D U C T I O N At present, industrial manipulators are controlled in the jointspace. The manipulator controller requires joint value inputs to place the manipulator end effector at a desired position and orientation in space. The joint values required for any given position and orientation of the end effector in Cartesian space change with the joint configuration of the manipulator. However, the interaction of the manipulator with objects in the world is more easily specified in world coordinates instead of joint coordinates. Currently, there is much interest in manipulator inverse kinematics. Manipulator inverse kinematics equations describes the relation between the Cartesian position and orientation of the end effector and the manipulator's joint values. The transformation is nonlinear and can be very difficult to obtain. Manipulator programming and operation would be simplified if tasks can be specified in Cartesian coordinates instead of joint coordinates. Present methods of programming and kinematic control are inadequate and they represent a major obstacle to the expanded used of manipulators. One common solution to the inverse kinematics problem is to use a master arm to teach or guide the manipulator through a desired trajectory. Figure 1.1 is an example of a master/slave arm combination. An approximate model of the manipulator is physically guided along the desired trajectory by an operator. Master controllers are position control devices whereby the position of the end effector on the master is transformed to the position of the slave arm through the joint sensors on the master. Inaccuracies in modelling the slave arm degrades the accuracy of the transformation. Master arms are also difficult to use because the arm links interfere with the operator's arms. Master/slave control is very 1 Introduction / 2 1.1 Master/Slave control Introduction / 3 time consuming, inefficient and its low accuracy limits its applications. A general controller using velocity control which accepts incremental changes would have a greater range of motion and be easier to operate. A general inverse kinematics algorithm is necessary to transform incremental changes in manipulator end effector position and orientation to incremental changes in the manipulator joints. Manipulator operation would be simplified by an efficient and accurate general inverse kinematics algorithm. Many inverse kinematics algorithms have been proposed for general, specific, redundant and nonredundant manipulators. [ANGEL85, ANGEL86, FEA83, GOLD84, HOLL83, PEIPER69, WAMPLER86, WHIT69] This thesis will investigate inverse kinematics algorithms for nonredundant manipulators which are general and which are valid for all regions of the manipulator operation. The proposed algorithm is an iterative inverse kinematics solution. The linearized relationship between joint changes and position and orientation changes is used. Most iteration methods are not stable around certain regions of manipulator operation called singular points. A modified damping procedure is used to stabilize the solution and to expand the range of permissible input targets. The proposed inverse kinematics algorithm is compared with previous implementations and the advantages and disadvantages are discussed. A few assumptions are made in the investigation into the inverse kinematics control of a manipulator. The manipulator is assumed to be accurately represented with only the basic Denavit-Hartenberg notation. The Denavit-Hartenberg notation models the manipulator as an open serial chain Introduction / 4 Joint n Joint n+l Fig. 1.2 Denavit-Hartenberg representation for a) revolute and b) prismatic joints. Introduction / 5 consisting of ideal joints connected by rigid links. The assumptions of ideal joints and rigid links are valid for most industrial manipulators and adequate for the accuracy required in their use. The joints have only a single degree of freedom and are either revolute or prismatic. The joint axes are lines in space and only four variables are required to describe the relationship between two adjacent joint axes. Figure 1.2 shows how the parameters are used to describe the relationship between adjacent revolute and prismatic joints. A coordinate frame is attached to each joint. Thus the motion of the coordinate frame from one joint to another can be performed with a transformation matrix. Using the Denavit-Hartenberg notation, the forward kinematics equations describing the manipulator end effector position and orientation can be easily derived with transformation matrices. Paul[PAUL81] describes a method of representing any manipulator using a four by four homogeneous transformation matrix, the A matrix and the Denavit-Hartenberg parameters. A = COS0 sine? 0 0 - s i n 0 c o s 0 COSf?COS0 sinf? 0 s i n 0 s i n # -cos0sin<6 COS0 0 OCOS0 asinf? d 1 (1.1) A reduced number of parameters is required over a general transformation. Only three parameters and the joint variable are required in the transformation for one joint instead of the six parameters required in the general case. The parameters consist of one rotation, the twist value <f>, and two translations, the offset c and displacement d. The joint variable is -a translation for a prismatic Introduction / 6 joint and a rotation for a revolute joint. The target machines for the inverse kinematics algorithm consist of industrial manipulators and industrial machines. Industrial machines such as construction or logging equipment can benefit from a simplified control mechanism. The control inputs of industrial machines operate on individual joints through joint actuators. The machine controls vary with individual machines. Figure 1.3 shows a typical industrial machine with its joint controls. A common control instrument which can specify motion in Cartesian coordinates would reduce the training time required to master a specific machine and increase the productivity of the operator. For industrial machines, the inverse kinematics algorithm must be able to operate in real-time. The algorithm must be fast enough to operate with present available technology. The lag between control inputs and joint value outputs to the joint controller must be unnoticeable. The inverse kinematic algorithm must also be stable, returning valid joint values for all possible control inputs. The algorithm must not limit the range of motion of the manipulator and thus must be stable for all possible control inputs to the manipulator. Modifications to the machines are to be mmimized. Therefore, the algorithm must be adaptable to the machine. The different approaches to the general inverse kinematics problem are discussed in Chapter 2. Various inverse kinematics algorithms for general solutions have been considered and the results are discussed. The proposed algorithm is presented in Chapter 3. An efficient method for generating the Jacobian required by the algorithm is given in Chapter 3.1. The basis for the algorithm strategy is presented in Chapter 3.2 with a single link one jointed manipulator. Chapter Introduction / 7 S l i d i n g boom i n B o o m u o Fig. 1.3 Typical industrial machine with joint controls 3.3 extends the idea to a multi-jointed manipulator. In Chapter 3.4 and Chapter 3.5, the more complex problem of three degrees of freedom positioning and orientation are investigated. The solution for the general six degrees of freedom manipulator is presented in Chapter 3.6 with different variations. The performance of the six degree of freedom inverse kinematics algorithm is discussed in Chapter 3.7. Chapter 4 discusses a method for estimating the Denavit-Hartenberg parameters required by the algorithm. Vectors and matrices are used frequently. In general, matrices and vectors are Introduction / 8 represented by boldfaced upper case and lower case characters respectively. Scalar values are represented with Greek or italic characters. CHAPTER 2. INVERSE KINEMATICS 2.1. MANIPULATOR REGIONS There are two sets of variables in describing the kinematics of a robot manipulator. The joint coordinates of a manipulator is a set containing the joint variables. The work coordinates of a manipulator is the set consisting of all end effector positions and orientations. There are many ways of specifying the end effector position and orientation in work coordinates. The position can be given in either Cartesian, cylindrical or spherical coordinates while the orientation can be given in Euler angles, Row-Pitch-Yaw angles, a coordinate frame[PAUL81], or quaternions[KOREN80]. The transformation between different work coordinates is simple. See the Appendix C for details. When the variables are limited in then-range, the subsets are the jointspace and workspace. The jointspace is very simple and is determined solely by joint constraints. However, joint limits are not investigated in detail in this thesis. In the simulations, all revolute joints are assumed to have the full range of motion, from it to -it. Joint limits are only investigated on joints which determine the discrete solution if more than one existed. For those joints, the range is restricted to it. The workspace of an arbitrary manipulator can be very complex. It is determined both by joint constraints and by the configuration of the manipulator[YANG84]. The workspace is the region consisting of all positions and orientations which can be reached by the manipulator. The target posture in work coordinates for a manipulator can be broken into three regions in space with respect to the difficulty of the inverse kinematics problem. Figure 2.1 shows 9 INVERSE KINEMATICS / 10 the three regions for a two degree of freedom manipulator and the transformation to the jointspace. The first region is the high manipulability region or good region. The second region consists of singular points and the area around them. The third region is rarely considered in inverse kinematics and it consist of all postures which are unattainable by the manipulator. The boundaries of the high manipulability region are subjective. Manipulability is a term describing the quality of the relationship between incremental workspace changes and jointspace changes. In a high manipulability region, the ratio between workspace changes and jointspace changes is high. A manipulator is easiest to control in a high manipulability region. The region of high manipulability is identified by many metrics with the determinant of the Jacobian being the simplest to compute. The Jacobian matrix is the linear relationship between mfinitesimal changes in posture and changes in individual joint values. v = J.d6 (2.1) A large determinant for the Jacobian matrix generally means that the manipulability is high in that position. The Jacobian matrix itself does not change greatly with small changes in the joint values in high manipulability regions. The condition number of the Jacobian matrix is also used as a manipulability measure but it requires much more computation. The condition number of the Jacobian matrix is the ratio of its largest eigenvalue over the smallest eigenvalue. It has the advantage of being independent of the scale of the manipulator. A manipulator which is scaled larger will have a larger determinant yet the manipulability is not improved. A high manipulability posture would have a condition number near unity. Owens [OWENS] gives a more INVERSE KINEMATICS / 11 Fig. 2.1 Manipulator regions for a two degree of manipulator INVERSE KINEMATICS / 12 comprehensive discussion on manipulability and describes other possible measures for determining manipulability. The second region consist of singular points and the regions around them. Singular points are those combinations of joint values which result in a zero for the determinant of the Jacobian matrix[HOL83]. A Jacobian matrix with the origin of the coordinate system on any joint axis can be used to determine the singular points of a manipulator arm. The proper selection of the origin will simplify the analytic study of singular points for the manipulator[LIPKIN85]. Singular points can be divided into two types. When the elbow of an anthropomorphic manipulator is fully extended, it loses the mobility in the radial outward direction and is at a stationary point. All joint values can be uniquely determined at a stationary point. The second type of singular point, the uncertainty point, occurs when an infinite range of joint values are possible. This occurs in cases where two revolute joints becomes colinear in space or when more than three joint axes intersect at a point. In all singular points, the manipulator experiences a reduction in the number of degrees of freedom and a corresponding increase in redundancy. The singular region is an area in the neighborhood of the singular point and it is separated from the high manipulability region with the help of manipulability measures. Thus, . the boundaries of the singular region are subjective. The controllability of a manipulator in the singular region is limited because small motions in certain directions require very large joint value changes. The third region consists of all points outside the workspace. The response of INVERSE KINEMATICS / 13 inverse kinematics solutions to unreachable targets in this region is important. Most inverse kinematics solutions do not handle this problem directly. These points must generally be identified by a separate routine. The task of identifying whether a target for the end effector is inside a workspace becomes more difficult when the manipulator contains offsets in the shoulder and wrist. The workspace may be quite complex and cannot be accurately modelled by simple geometric shapes such as cylinders, spheres and boxes especially if limits in orientation are considered. This region is important when workspace limits are not built into the controlling mechanism. There is no explicit way of determining when the manipulator is asked to reach a posture outside its workspace with a general controller. In master/slave position control with a model of the slave arm, it is not possible for the target to be placed outside the workspace. A desired response for the kinematics control algorithm for unreachable targets would have the manipulator move to the outer limits of the workspace to a posture where the least squares position and orientation errors are minimized. Forward kinematics solutions are quite easy to generate. They can be easily obtained using Paul's A matrices and the equations are unique[PAUL81]. Figure 2.2 shows the kinematics equations for the elbow manipulator[PAUL81]. Given any point in the jointspace there exists only one corresponding point in the workspace. In contrast, the inverse kinematics equations are not unique. Multiple solutions are common in many cases. In the solutions for the elbow manipulator, there are two possible solutions for joints one, three and five giving a total of eight possible sets of joint values for one position and orientation. Figure 2.3 shows the complexity of the closed-form inverse kinematics equations for the I N V E R S E K I N E M A T I C S / 14 *5'z6 T 6 = n* ox o, px Hy Oy Oy PV n» Oi <h Pz 0 0 0 1 Ox = — Ci[Cj34<3A + S»4Ce] + SiSsSt Oy — — SilCiiiCiSa + S234C&] — CiSiSa o, — —SxiiPsSt + CXMCB Ox = C1C2345. + S\Ci Oy = SiCfc^ — C1C5 a» = $34$> Px = Ci [C234OA + Cj3«3 + C ^ ] Py = [C234CI4 -|- + Op?] ft = Sl34«4 + Sl3«3 + 5l<*2 Fig . 2.2 Kinematics equations for the elbow manipulator elbow manipulator. Many methods exist for obtaining the inverse kinematics solution for manipulators of different configurations. The solutions vary in their response to target postures in the three work coordinate regions. 2.2. APPROXIMATION BY A MULTINOMIAL The inverse kinematics equations may be approximated by general functions. The simplest general function is a polynomial which has good characteristics in approximating functions. A multinomial is necessary because each joint is a function of many variables which describes the posture of the target. The set of INVERSE KINEMATICS / 15 = +180° C i O j - r - b i O y Px = C i 7>» + Si py — C234 04 P'u = ft — S234O4 _ 1 (C 3 a 3 + a^)Tfv — S3 Q 3 P , ft ~ U n (C 3 o3 + 0 3 ) ^ + S 3 o3ryy 04 = 6*234 — ft — ft _ . C 2 3 4 ( C | O, + S| Oy) + S234O, ft; = tan — ^ — ft = t a n - 1 ( - C 5 l C 2 3 4 ( C l 0 T + Si Op) 4- S 2 M O , l -T- S S(S t Ox - C t O y A ^ — S 2 3 4 (Ci oz - f S, o„) + C2340, ) Fig. 2.3 Inverse kinematics equations for the elbow manipulator sample points required to solve for the coefficients consists of posture points as a function of the joint values. The selection of the sample points is important in determining the maximum error in the approximation over the region of the approximation[RICE69]. Various searching algorithms may have to be used to select the best points for the approximation. INVERSE KINEMATICS / 16 Attempts were made to fit the coefficients using least squares and a learning algorithm. The learning identification algorithm suggested by Kevin Huscroft[HUS84] gave a very good approximation in the high manipulability regions. It was used to estimate the inverse kinematics equations for a simple two link two degrees of freedom manipulator and a two link three degrees of freedom manipulator. Many iterations were required in the multidimensional case although it tended to approximate a minimax solution. Millions of iterations were needed for the three degrees of freedom case. The algorithm adjusted the coefficients on a point by point basis so that memory storage was not a problem. A disadvantage of the algorithm was its sensitivity to bad learning points. New points are weighted much more than past points. A poor learning point tended to erase the learning of many previous points. If the weighting was changed to emphasize past points, the convergence suffered. Least squares gave a fairly good approximation to the function but many points were needed to obtain a good approximation over the region of interest. To fit a multinomial to the inverse kinematics equations, enough sample points to solve for the coefficients of the multinomial are needed. A large amount of memory, was needed to solve the least squares problem. The algorithm suffers from numerical problems with the number of points required for the six degrees of freedom case. Both learning and least squares methods have problems with singularities and the regions near them. Multinomials were not able to approximate the rapidly changing joint values in the singular region. The inverse kinematics functions are INVERSE KINEMATICS / 17 not well behaved around singular points. Thus function approximations are possible only in the high manipulability regions in the manipulator workspace. Rational functions, such as a ratio of two polynomials have better characteristics in approximating functions with singularities but the errors are still large near singularities[ACTON70]. Although the terms of a multinomial are simple and easy to calculate, the number of terms increases to the power of the number of joints. The number of coefficients and terms may be quite high to achieve good accuracy. For a six degree of freedom manipulator, 5000 coefficients might be necessary to give a rough approximation for one joint variable over a limited workspace region. Therefore, more than 30000 multiplications and additions may be necessary to calculate all the joint values. Even though the functions are simple, the number of operations required in the multidimensional case diminishes the attractiveness of using function approximations. In addition, approximation functions do not handle the situation in which the target posture is outside the workspace. 2.3. TRANSFORM METHODS Analytic solutions for inverse kinematics can be obtained in several ways. Closed-form solutions express the joint variables explicitly in terms of the workspace variables. More complex joint solutions express the joint variables as a function of the roots of a homogeneous polynomial. With analytic solutions, it is theoretically possible to determine all possible solutions for a given target posture. However, analytic solutions do not give valid values for postures which are outside the workspace and are usually not accurate near singularities. A separate INVERSE KINEMATICS / 18 routine is necessary to screen the target postures before a solution is attempted. Special care is necessary in deriving the solution for the case of targets in the singular region. Three methods for deriving analytic solutions were presented by Pieper, Paul, and Duffy. The methods of Paul and Duffy were systematic and general. Pieper[PIEPER69] investigated many approaches to the inverse kinematics problem. For closed-form solutions he essentially calculated the solutions for all possible combinations of revolute and prismatic joints for various existing robot structures. He paid particular attention to geometries with three intersecting revolute joints. Closed-form solutions were obtained in almost all cases by trial and error. The most difficult solution required solving for the roots of a fourth order polynomial in the joint variable. Closed-form solutions for common joint configurations are also given by Colson[COL83] and Milenkovic[MIL83]. Systematic methods can be used to describe the transformation from jointspace to workspace, the forward kinematics equations. The forward kinematics equations can be generated very easily given the manipulator Denavit-Hartenberg parameters. Given these relationships, Paul[PAUL81] and Duffy [DUFFY80] expressed the jointspace variables explicitly in terms of the workspace variables through manipulation of these relationships. They used similar methods but with different initial sets of equations. Paul generates equations by rotating the transformation matrices, his A matrices, to obtain the end effector position and orientation in terms of the jointspace INVERSE KINEMATICS / 19 variables in different coordinate systems. T 6 = A , - A 2 « A 3 - A » - A 5 « A 6 (2 .2) The T 6 matrix contains the end effector position and orientation with respect to a base coordinate system. The reference coordinate system is shifted to the next joint to generate more equations. A , " 1 • T 6 = A 2 - A 3 - A f l - A 5 . A 6 (2 .3) Geometrically, the process transforms the workspace variables into the coordinate system of another joint. In the above equation, the manipulator end effector is given with respect to the coordinate system of joint two. For certain special configurations, simplification occurs and equations with only one unknown joint variable can easily be obtained. These simplifications are possible with many industrial manipulators which were designed to have simple inverse kinematics solutions. They commonly have twist values which are multiples of \v. If a simple relation could not be found, selected sets of equations were solved simultaneously to eliminate as many unknown variables as possible. The method requires much intuition and is fruitful for only a few special cases. Duffy introduced an extra step in converting the manipulator into a closed-loop mechanism by the addition of a hypothetical seventh joint with its own offset and displacement as is shown in Figure 2.4. He suggested a more systematic method of obtaining equations for the joints values as a function of the workspace variables. The closure equations describe the transformation from the base forward along the links and back to the base. I = A , - A 2 - A 3 - A , - A 5 - A 6 - A 7 (2.4) Where I is the identity matrix. Using his methods, it is theoretically possible to INVERSE KINEMATICS / 20 Fig . 2.4 Closed loop mechanism with hypothetical seventh joint obtain analytic solutions for any arbitrary manipulator, although the equations may be very complex. Extra Denavit-Hartenberg parameters for the seventh joint are obtained as functions of the workspace variables. Sets of equations are derived by equating the elements of the product of the A matrix with those of the identity matrix. More equations results from using a different joint as the base. For example, all points can be referenced to the origin of joint two. A , " 1 • I = A 2. A 3 « A,« A 5- A 6- A 7 (2.5) Equations with the fewest number of variables are selected. If more than one unknown variable exists in the simplest equation, as many independent equations INVERSE KINEMATICS / 21 as necessary are used to eliminate the unwanted unknowns. In the case with two unknown revolute joint variables, the set of equations must be solved simultaneously subject to the constraint on the joint values. The result is a fourth order polynomial in the sines and cosines of one unknown revolute joint variable. The difficult cases are with revolute joints. Prismatic joints give rise to linear terms which can be easily eliminated. In the worst case, the solution for a six degree of freedom manipulator would be a homogeneous polynomial of 32nd order. In some cases, multiple roots are present which reduces the actual number of possible solutions. The number of possible solutions for a given position is approximately equal to the order of the polynomial equation. Unique closed-form solutions are only possible for equations of order one in one unknown joint variable. For equations of second order, two solutions will exist which may be quite complex involving square roots and inverse tangents. For equations of higher order, an iterative root finding algorithm is required to solve the homogeneous polynomial equation. Duffy's method is similar to Paul's method in requiring much analysis and trial and error. A solution is theoretically possible for all manipulators although an iterative method may be required in some cases. The equations are not valid for all regions of the manipulator workspace because of difficulties near singularities. The solutions are not valid for unreachable targets. INVERSE KINEMATICS / 22 2.4. ITERATIVE SCHEMES The primary objective of the many different iterative inverse kinematics solutions for nonredundant manipulators is to minimize the errors in the position and orientation of the tool on the end effector. In iterative methods, the position and orientation errors are minimized by performing small corrections to the joint values. Joint variables are changed in each iteration such that the objective of minimizing the distance and orientation errors is achieved at each iteration step. Two sets of information are needed in iteration schemes. The first is the direction in which the joint corrections are to be made for each joint variable. Secondly the amount of the joint correction must be determined. Because of the highly nonlinear nature of the kinematics equations, estimates of the direction and size of the corrections are usually not very accurate if the workspace changes are large and especially if the arm is in a singular region. An ideal arm is one with accurate closed-form solutions available for all joints. Most manipulator arms follow a basic configuration for an ideal arm, but the inverse kinematics equations are made complex because of small offsets in the arm. The inverse kinematics equations for the ideal arm can be used if the error introduced by the offsets can be subsequently eliminated. The joint values obtained with the inverse kinematics equations for an ideal arm, position the manipulator close to the target but in error by the offsets. To eliminate the errors, Hollerbach[HOL83] suggested adding the workspace error to the target to obtain a new target position. The joint values for the new target point using the inverse kinematics for the ideal arm should be closer to the desired target position. The idea was suggested but not implemented. The method would INVERSE KINEMATICS / 23 encounter problems if the new target position was outside the workspace of the ideal manipulator. The equations would fail to give an answer or would give joint angles which would be incorrect. This method is only possible in high manipulability regions. The inverse kinematics equations can be solved using well-known linear methods by obtaining a linear approximation of the relationship between jointspace and workspace variables. For a given position in space, a Taylor series expansion of the forward kinematics relationship x = K(0) (2.6) where x is a workspace vector and 0 is a jointspace vector and K is the forward kinematics transformation. From an initial starting position, 0,, the new target position is equal to the expansion terms multiplied by the required joint corrections, 9 0. K ( 0 i + 1 ) = R(t? i) + J(t?i)«3c?i+ H(6i ) • 9 20 ±+- • • (2.7) If terms of order higher than one are ignored, a linear relationship is obtained with the Jacobian. K ( e i + 1 ) - K(e.) = j(ei)-bei (2.8) v = J(0.)•90. l i where 0 i + 1 = ^ i + ^ ^ i (2.9) The required joint corrections 90^ can be obtained by solving the set of linear equations. From this relation, it is easily seen that the Jacobian would be a good approximation only if it is relatively large compared with the higher order terms. This is a good assumption in high manipulability regions. The assumption INVERSE KINEMATICS / 24 is also better for very small joint changes. The Newton-Raphson method uses this approximation to iteratively reach the desired target position. At each iteration step, the necessary joint corrections are obtained from the workspace error to move the manipulator closer to the target. The iteration continues until the maximum error is satisfied or until a maximum number of iterations is reached. The joint corrections are obtained from the workspace changes by inverting the Jacobian matrix. The inversion is possible only in nonsingular regions. Near a singularity, the condition number of the inverse Jacobian matrix becomes very large. Thus the workspace changes in some directions are multiplied by a very large value near singularities and by infinity at a singular point. Near singularities, the corrections are good only if the workspace changes are very small, and the joint corrections will generally be too large for motion in certain directions. Goldenberg[GOLD84,BEN84] and others searched for the right joint correction through scaling the joint corrections obtained using the Jacobian by a factor. 0 i + 1= 0±+ 30-e11 (2.10) Where e is a factor and n is the subiteration number. In each iteration, if the joint corrections do not move the manipulator closer to the target, the joint corrections are iteratively decreased until the manipulator is closer to the target or until a minimum joint correction is reached. There is another level of iteration within an iteration. The iteration is strictly descending in implementing corrections only if they move the manipulator closer to the target. This strategy assumes that the direction given by the Jacobian is reasonably correct. In fact this INVERSE KINEMATICS / 25 assumption fails near and at singularities because the manipulator and its Jacobian is limited in its direction of motion and the Jacobian changes greatly with small changes in the workspace. The algorithm requires too many iterations to determine the best value for the joint corrections. Goldenberg's inverse kinematics algorithm was intended for redundant manipulators which would use the redundancy to avoid some singularities. However, singularities still exist for redundant manipulators in such the cases as when the target is out of reach. The algorithm is not able to handle singularities except by scaling the joint corrections. Pieper[PIEPER69] suggested another approximation to the linearization of the inverse kinematics equations using a vector method. He suggested using screw theory to represent the changes in posture. The changes in position and orientation are equal to a single equivalent translation and rotation. The total change in position and orientation is equal to the sum of the changes in position and orientation by each individual joint if the changes are small. For large jointspace changes, only a fraction of the corrections are implemented. The method performs better than Peiper's implementation of the Newton-Raphson method in terms of the number of iterations needed to reach a target and in terms of the stability of the Jacobian. The method does have difficulty with singular positions and convergence can be very slow. His upper limit is 400 iterations for the maximum number of iterations. Many methods use the Moore-Penrose pseudoinverse to perform the inversion. The approach was more in the interest of being able to obtain a solution in the case INVERSE KINEMATICS / 26 of redundant manipulators. In Goldenberg's case [GOLD84], the method did not handle situations near singularities. Angeles[ANGEL86] used the pseudoinverse in the five degrees of freedom case to ensure that the inversion was possible. For a general six degrees of freedom manipulator, Angeles[ANGEL85] used a Jacobian matrix which is similar to Peiper's vector Jacobian to obtain a general inverse kinematics algorithm. Instead of using a single vector for the equivalent rotation vector, he separated the equivalent rotation vector into a unit vector and the magnitude of equivalent rotation vector. Pseudoinversion was necessary because the matrix was not square. He introduced condition number minimization to stabilize the algorithm near singularities but he did not address the problems near singularities. The large joint changes which occur near singularities with finite workspace errors can be reduced by using the Levenberg-Marquardt stabilization. A least squares method is applied to minimize the error in the solution for the joint corrections along with minimizing the deviation of the joint correctons from zero. ||J.30 - v | | 2 + a 2 - ||30||2 (2.11) Wampler[WAMPLER86] suggested using a fixed damping factor which would give a stable algorithm by driving all joint changes to zero when the target posture was near a singularity. The algorithm is made stable by its inability to move into the singular region. Convergence is also very slow to points near the singular region. Faster convergence can be obtained if the damping factor was varied with the position and orientation errors. This approach was chosen for investigation in this INVERSE KINEMATICS / 27 thesis because it increases the convergence rate near singularities but is still stable in the whole manipulator workspace for an arbitrary manipulator. It gives a solution minimizing the least squares posture errors when the target is outside the workspace. CHAPTER 3. PROPOSED KINEMATIC CONTROL METHOD 3.1. GENERATING THE JACOBIAN The proposed algorithm uses the Jacobian matrix. The Jacobian relates differential changes between workspace and jointspace. J-90 = v (3.1) It can be derived in many ways for either a general or a specific manipulator. This thesis will concentrate on methods which are general. The Jacobian can be calculated analytically. The matrix elements relating specific changes in workspace to changes in the jointspace are obtained by differentiating the forward kinematics equations. The equations for elements of the T 6 matrix are differentiated with respect to the joint variables. The method requires only knowledge of the Denavit-Hartenberg parameters. A symbolic manipulation program such as Reduce or MacSyma can perform the differentiation easily and automatically. For industrial manipulators with mainly parallel and orthogonal joints, many terms are common in the equations for the Jacobian elements. The equations can be simplified by collecting the common terms giving a computationally efficient representation for the Jacobian. One difficulty with the analytic Jacobian is with the representation for the orientation of the end effector. If the unit vectors of the coordinate system of the final link are used, there will be more equations than are actually required. At least two vectors are necessary to represent the three degrees of freedom in orientation. However, the two vectors will give six equations. If Euler angles are used, only three equations are necessary but discontinuities are present in the equations. 28 PROPOSED KINEMATIC CONTROL METHOD / 29 The Jacobian can also be derived from the physical relationship between jointspace changes and workspace changes using vector algebra. This is the same Jacobian used in Pieper's velocity control method. A efficient recursive method for calculating the elements of the vector Jacobian is given by Wampler[WAMPLER86] and Angeles[ANGEL85]. Again, only the Denavit-Hartenberg parameters are required. To obtain the column vector in the vector Jacobian relating the change in the workspace to changes in one specific joint, all other joints are assumed fixed. For a revolute joint, the changes in position V , the end effector velocity vector, can be determined from the joint axis vectors, z , and the radius vector from the joint axis to the end effector, p For a prismatic joint, the end effector velocity vector is equal to the joint rate along the joint axis. If the cross product is performed in the coordinate system of the revolute joint before the transformation to the base coordinate system, the number of operations is reduced. About two multiplications and two additions are saved over performing the cross product in the base coordinate system. The joint axis vector is [ 0, 0, 1 ] T and the radius vector is [ x^ , y^ , z^]T. The cross product is a simple rearrangement of the terms. The vectors then must be transformed to the coordinate system of the base by multiplication with a direction cosine matrix. v° = (z° x P ° ) .be. i i i i ( 3 . 2 ) ( 3 . 3 ) v.= [ -y., x., 0 ] T I J i ' I ' ( 3 . 4 ) v° = C° 1 c i PROPOSED KINEMATIC CONTROL METHOD / 30 ( 3 . 5 ) A small change in orientation is equivalent to a rotation around a joint axis. The partial angular velocity uector[WAMPLER86] which describes the direction of rotation as well as the amount of rotation is equal to the revolute joint axis vector multiplied by the amount of the rotation. i i 3c?. ( 3 . 6 ) w° = 0 v = Z n , v° i=l I The partial angular velocity generated by a prismatic joint is zero. ( 3 . 7 ) The equivalent partial velocity is equal to the sum of the individual partial velocity vectors[WAMPLER86]. Assuming that the position and orientation variables are not a function of time ( 3 . 8 ) For the orientation, the property that for small rotations the total rotation rate of the end effector is equal to the vector sum of individual rotations created by the joints is used. ( 3 . 9 ) The elements of the Jacobian matrix are equal to the individual partial velocity and rotation vectors. 2,Xp, Z 2 X p 2 Z 3 X p 3 Z » X P « Z 5 X P 5 Z 6 X p 6 Z t Z 2 Z 3 Z g Z 5 Z g ( 3 . 1 0 ) w = I. , w. i=l 1 V w The radius vectors from each joint to the end effector and the direction cosine matrices which transform the velocity vectors to the base coordinate system are PROPOSED KINEMATIC CONTROL METHOD / 31 obtained using a recursive technique. For the radius vectors, the recursion starts from the end effector and works towards the first joint. The radius vector for the last joint is equal to the offset. P 6= a 6 (3.11) The radius vector for the next joint is equal to the previous vector multiplied by the previous joint's rotation and twist values. P._1= 3 ^ + T ( ^ ) . C(6\)-{ P ±+ d.} (3.12) Where C ( 4J\ ) and T(«j>^) are three by three direction cosine matrices for the rotations by the joint angle and the twist value between joints. For the transformation matrices from joint coordinates to base coordinates, the recursion starts from the base and works it way to the last joint. The transformation for the first joint is only the rotation of the first joint around the joint axis. c1(e1) (3.13) C 1 = C i _ 1 i+1 i T U . ) . C . ( 0 . ) For the next joint, the transformation is multiplied by the twist to the next joint and the rotation respectively. (3.14) If the twist angles are a multiple of ^T T, the transformations are simple. The premultiplication by the twist matrix becomes a simple rearrangement of terms for the next rotation matrix. No floating point operations are required. The direction cosine matrix for the joint rotation when multiplied by the direction cosine matrix for twist angles which are multiples of ^ jr are: 0 = lit (3.15) cos0 - s i n 0 0 0 sine? 0 COS0 -1 0 4> = 0 cost? sin0 0 PROPOSED KINEMATIC CONTROL METHOD / 32 ( 3 . 1 6 ) -sine? 0 cos0 0 0 1 <t> = ~ COS0 0 - s i n 0 - s i n 0 0 0 1 -cos0 0 ( 3 . 1 7 ) These matrices require only four multiplications and two additions to transform a vector compared with the nine multiplications and six additions which are needed in the general case. Orthogonal and parallel joints are used in the simulations. The number of operations for calculating the vector Jacobian compares well with the number required by other methods. The analytic Jacobian obtained by differentiating the position and orientation equations with respect to the input variables generates the most complex equations unless simplifications are made to take advantage of special configurations and to reduce the number of terms. The analytic method produces a Jacobian with the fewest operations for manipulators with many parallel joints such as the Kodiak arm. Orin[ORIN84] has a comparison of different vector methods for calculating the Jacobian matrix. The many methods are different mainly in the the coordinate system in which the calculations for the Jacobian elements are performed. The PROPOSED KINEMATIC CONTROL METHOD / 33 method of Wampler[WAMPLER86] is the most efficient when compared with the many methods mentioned by Orin[ORIN84]. Wampler's advantage is obtained by taking the cross product in the coordinate system of the particular joint. Two multiplication and two additions are saved over performing the cross product in any other coordinate system[WAMPLER86]. Using vectors to represent the partial rotation is much more efficient because no more calculations are needed to generate the Jacobian elements. Another advantage is that the posture is obtained in the process, thus eliminating the need for a separate routine to perform forward kinematics. This is especially advantageous in iterative inverse kinematics algorithms. 3.2. DAMPING IN A SINGLE LINK MANIPULATOR The effect of damping is most easily visualized in the case of the single link one degree of freedom manipulator in the x-y plane. If only motion along the X axis is considered, the equation for the workspace variable is X = COSd (3.18) The Jacobian is -sine? and its inverse as a function of the joint angle is shown in Figure 3.1 as a solid line. The Jacobian is a very good approximation for the position near zero for the joint value. The inverse Jacobian is singular and is a very poor approximation at the limits of motion at n and -tr. Near the singular points, the inverse Jacobian is greater than the value required to take the manipulator to the target. The size of the inverse Jacobian is reduced by damping the joint changes[WAMPLER86]. The steps to obtain the pseudoinverse with damped joint PROPOSED KINEMATIC CONTROL METHOD / 34 - 3 - 2 - 1 0 1 2 3 Joint AngleCrad) Inverse Jacobian Damped Inverse Jacobian Fig. 3.1 Damped and undamped inverse Jacobian for the one jointed manipulator PROPOSED KINEMATIC CONTROL METHOD / 35 changes are as follows: ( 3 . 19 ) ( J T J + a 2 I ) . 3 0 = J T v ( 3 . 20 ) 30 = ( J T J + a 2 I ) - 1 J T v ( 3 . 21 ) 30 J*v ( 3 . 22 ) Where I is the identity matrix and J * is the pseudoinverse. For the one degree of freedom manipulator, the pseudoinverse is: The damped inverse Jacobian is stabilized by using a to force the pseudoinverse to zero instead of infinity at singular points as shown by the dashed line in Figure 3.1. With a fixed damping factor, the convergence rate is diminished near the singular point. Before the manipulator can reach points near singularities, the joint corrections from the damped inverse Jacobian will go to zero. To reach points near singularities, the actual value for the inverse Jacobian should vary according to the amount of the step change and the position of the manipulator. If the step change is large, large damping should be used to limit the size of the joint corrections to reasonable values especially if the starting position is near a singular point. If the step change is small, a large inverse Jacobian value is needed near singular points to help drive the manipulator to the target. In the limit, for mfinitesimally small workspace changes the joint rates should go to - s i n e J * ( 3 . 23 ) s in 20 + o2 PROPOSED KINEMATIC CONTROL METHOD / 36 infinity near singularities to completely eliminate the errors in posture. The damping factor can be varied to accomplish these goals. For the one degree of freedom manipulator the damping factor can vary with the size of the workspace correction or the expected joint change. By comparing the desired joint ratio with that obtained with damping, a good approximation value for the damping factor can be calculated. From comparing the values for different joint changes, a relationship between the damping factor and the joint changes is noticeable. Using an estimate of the joint changes to vary the damping factor sometimes produces oscillations. If the initial estimate is below the actual change, then the joint corrections would be too large. If the initial estimate is above the actual change, the converse is true. Thus if the error in the estimates is too large, oscillations between too large and too small a correction results. The iterations are more stable by making the damping factor proportional to the position error. For the one degree of freedom manipulator a conservative relation for the damping factor to obtain a good approximation to the inverse Jacobian is The double bar signifies the magnitude of the vector. The damping coefficient, f (a, v), is a function of the joint values and of the distance error. "When the manipulator is in a good region a fairly large coefficient is needed to stabilize large corrections. When the manipulator is near a singular point, a very small damping coefficient is required to eliminate small errors between the present position and the target. The damping coefficient function can be approximated by a constant, a = f (0,v) • |v|| ( 3 . 2 4 ) f ( 0 , v ) - K ( 3 . 2 5 ) PROPOSED KINEMATIC CONTROL METHOD / 37 A larger value is needed to prevent overshoot at singular points and for unreachable target points. See the Appendix A for details. The need to provide stability when the target is out of reach with a large damping value conflicts with the need for fast convergence and less damping when the target is near or at a singular point. The convergence rate for targets near and at singular points must be sacrificed for stability when the target is out of reach. The problem of using the Jacobian to approximate the arccosine function can be seen in using Newton's method for determining the root of a function. There are six possible combinations of starting and target values for the one link manipulator. They are: 1. singular point to singular point 2. good point to good point 3. good point to singular point 4. singular point to good point 5. good point to unreachable point 6. singular point to unreachable point From Figure 3.2, no difficulties are present with either choice of (2) or (3) if the target is inside the workspace. Problems occur in moving away from a singular point because the Jacobian gives a poor estimate of both direction for the joint correction and the amount of the correction. Joint corrections can be too large leading to overshoot. However, once the iteration lands in a good region near the target, convergence will be steady. The difficulties lies in obtaining a good starting point. Damping is needed to ensure that the inversion is possible. Oscillations will always occur when the target is unreachable using the Jacobian. • t PROPOSED KINEMATIC CONTROL METHOD / 38 Fig. 3.2 Newton's method to solve the arccosine function for a) good target b) singular target PROPOSED KINEMATIC CONTROL METHOD / 39 3.3. DAMPING IN THE MULTI-JOINTED MANIPULATOR The effect of damping can be seen in the multidimensional case by looking at the singular value decomposition of the Jacobian matrix. J = Q«E•Q T (3.26) The matrix Q contains orthonormal eigenvectors and the matrix E is a diagonal matrix containing the singular values. d i a g ( E ) = [ vy,v2, . . . ,v ] (3.27) The damped inverse Jacobian with a damping value of a is J * = Q T «E* «Q (3.28) Where E* is a diagonal matrix. d i a g ( E ) = [ a / U 2 + a 2 ) , . . . , a/( i> n 2 + a 2 ) ] (3.29) Each singular value of the pseudoinverse is equal to the arithmetic inverse of the singular value of the Jacobian. At singular points, one or more eigenvalues of the Jacobian goes to zero and the motions in the workspace along the corresponding eigenvectors of Q results in infinite joint corrections. With damping, the singular values are averaged with the damping factor in a similar way as in the one dimensional case so that the inverse goes to zero at singular points instead of infinity. The singular values for the inverse Jacobian is the ratio of workspace changes to jointspace changes. It is identical to the inverse Jacobian in the one jointed manipulator. In a functionally partitionable manipulator, the tasks of positioning and orienting the end effector is divided among the joints. The first three joints form the arm, whose purpose is to place the wrist near the target. The last three joints form the wrist, whose purpose is to orient the end effector. First, the problems of PROPOSED KINEMATIC CONTROL METHOD / 40 positioning and orientation will be presented separately. Then the combined problem will be investigated with different methods for the kinematics control of six degrees of freedom manipulators. 3.4. THE POSITIONING PROBLEM The effect of damping on the positioning aspect of the inverse kinematics problem will first be illustrated with examples of a two degrees of freedom and a three degrees of freedom idealized manipulator. The idealized manipulators consist of two links of unit length and either two or three revolute joints. Figure 3.3 shows the specifications and the notations for the two test manipulators. The examples concentrate on arms with only revolute joints because prismatic joints do not introduce more difficulties to the problem. The method should work just as well, if not better, with prismatic joints present. The effect of damping on the positioning problem is easily visualized with a two degrees of freedom manipulator. For the Cartesian position only, the analytic and vector Jacobians are identical. The two link two degrees of freedom manipulator has the following equations for the position of the end effector. x = cosfc 1, + cosie, + Bz) (3.30) y = s i n e , + s i n f e 3 , + 62) (3.31) The corresponding analytic Jacobian matrix, J, is - s i n f l T - s i n f f l i + f l z ) - s i n ( 6 , + 6 2 ) ] (3.32) COS0! + cos ( t 9 1 + t 9 2 ) cos{6,+e2) J The the determinant of the Jacobian is det(J) = s i n 0 2 (3.33) It shows that the manipulator is singular when the second joint angle, the PROPOSED KINEMATIC CONTROL METHOD / 41 Fig. 3.3 Two link two jointed and three jointed manipulators PROPOSED KINEMATIC CONTROL METHOD / 42 singular joint, is a multiple of rr. In this position, the manipulator has lost the degree of freedom along the outward radial direction of the arm. Only the rotation of the end point around the origin is possible. For each possible (x, y) position exclusive of the singular point, there are two possible discrete solutions for the joints. Figure 3.4 shows the elbow up and elbow down attitudes for the manipulator for a given target position. The convergence criterion for the two degree of freedom manipulator is either the maximum distance error, the maximum joint correction, or the maximum number of iterations. The manipulator reaches the target when the distance error is below a minimum. When joint corrections go to zero, the manipulator is at a singular point. The target is out of reach if the distance error is not negligible. The algorithm is considered to have failed if the number of iterations is above a limit. The damping factor is proportional to the square root of the distance error between the end effector and the target and works well in stabilizing convergence to singularities. The damping coefficient is set to a constant of one. A minimum value for the damping factor is needed to limit the size of the condition number for the inverse Jacobian matrix and to reduce numerical errors. The minimum damping factor is set equal to the positioning accuracy. A plot of the position error in the joint space will show two minima. It will be along the line 0 2 = -0, + C (3.34) PROPOSED KINEMATIC CONTROL METHOD / 43 Fig. 3.4 Mulitple solutions in the workspace for two jointed arm Without damping, the manipulator arm will generally converge to any point in the workspace except for points in the singular region. Without damping, the new estimate may not be closer to the target than the original estimate for large joint changes and hence the iterative algorithm using the Jacobian will not converge. With damping, the manipulator will converge in the same number of iterations or slightly more than without damping. The convergence rate usually increases as the manipulator nears its target. Near singular regions, the damped manipulator will converge with more iterations in contrast to the undamped case where it will continually oscillate about the target point. Depending on the storting position and the convergence path of the arm, it can converge to either minimum. PROPOSED KINEMATIC CONTROL METHOD / 44 * 2 Fig. 3.5 Convergence path of two jointed arm in the jointspace PROPOSED KINEMATIC CONTROL METHOD / 45 From the definition of damped least squares, the manipulator will generally converge to the closest minimum in terms of the sum of the squares of the joint changes. The workspace region can be roughly divided into two areas, separated by the second joint equal to zero, where the manipulator will tend to converge to its respective minima. The region is only approximate in determining the choice of minima. Figure 3.5 shows the convergence path in the jointspace to the two discrete solutions for different starting positions. It is desirable to restrict the attitude of the manipulator to be either elbow up or elbow down. Even when one discrete solution is closer to the starting point in the terms of the least squares of the joint changes, the position error between samples is much greater if the arm has to pass through a singularity to reach the solution. Figure 3.6 shows the sample points and the probable path of the manipulator between sample points when it passes through a singularity. With the modified damping proposed here, the arm has no difficulty in converging from a good region to a singular region. There is some difficulty in selecting the attitude of the arm in converging from a singular region to a good region because only the magnitude of the damping is affected, not the direction. When the starting position is a singular point, the direction in which the corrections will be made is not predictable. The manipulator has an equal chance of converging to either discrete solution. The desired solution may be selected by moving the starting position away from the present position to one which is more likely to be a starting position which will converge to the desired solution. For the two degrees of freedom manipulator, if the desired solution is elbow up, PROPOSED KINEMATIC CONTROL METHOD / 46 Fig. 3.6 Higher position error when the elbow oscillates between attitudes PROPOSED KINEMATIC CONTROL METHOD / 47 then the first joint should be moved counterclockwise and the elbow moved far from the singular position into an elbow up position. This is position 2 in Figure 3.7. The arm will tend to converge to an elbow up position if the approach path is clockwise. From position 1 in Figure 3.7 the manipulator may converge to an elbow down solution. The simple heuristic works in all cases for the two jointed manipulator. The method will converge for very large joint changes. Thus large differences between the starting and ending joint values are not a problem. The attitude can also be determined by locking the singular joint when it goes singular. The manipulator will converge to a point between the two possible solutions. The convergence criterion is the size of the joint changes which goes to zero as the end effector moves to the centerline between the two minima. Because the singular joint is locked inside one solution region, it is closer to one discrete solution than the other. When the singular joint is unlocked it will converge directly to the desired discrete solution. Figure 3.8 shows the iteration paths which result with locking of the elbow joint into an elbow down attitude and without locking. Moving from a two degrees of freedom manipulator to a three degrees of freedom manipulator introduces another singular point and two extra possible solutions for each target position. The two extra solutions correspond to a left shoulder/right shoulder arrangement. The Jacobian is singular when the target is over the shoulder. The equations for the two link three degrees of freedom manipulator are: PROPOSED KINEMATIC CONTROL METHOD / 48 Fig. 3.7 Effect of different starting positions on attitude X = COSBy{ COS0 2 + C O S ( 0 2 + '0 3) } y = s i n t ? ^ c o s 0 2 + c o s ( 0 2 + 0 3 ) } z = s i n 0 2 + s i n ( 0 2 + 0 3 ) (3.35) (3.36) (3.37) The corresponding analytic Jacobian is: -sine?, { c o s 0 2 + c o s ( 0 2 + 0 3 ) } c o s 0 2 { - s i n 0 2 - s i n ( 0 2 + 0 3 ) } - c o s 0 , { c o s 0 2 + c o s ( 0 2 + 0 3 ) } s i n 0 2 { - s i n 0 2 - s i n ( 0 2 + 0 3 ) } 0 c o s 0 2 + c o s ( 0 2 + 0 3 ) c o s 0 1 s i n ( 0 2 + 0 3 ) s i n 0 , s i n ( 0 2 + 0 3 ) c o s ( 0 2 + 0 3 ) (3.38) The deterrninant of the Jacobian is: det(J) = s i n 0 3 { c o s 0 2 + c o s ( 0 2 + 0 3 ) } (3.39) PROPOSED KINEMATIC CONTROL METHOD / 49 Fig. 3.8 Convergence to the target in the jointspace with joint locking PROPOSED KINEMATIC CONTROL METHOD / 50 The Jacobian is singular when the third joint is a multiple of 7T and when the projection of the arm on the x-y plane is zero. At the second singular position, the manipulator is not able to move in the y axis of the first joint without causing a change of £TT for the first joint. The determinant of the Jacobian for the three jointed arm is zero when the two jointed arm is singular multiplied by the new singular position. Some of the singular positions of the multi-jointed manipulator are determined by the singular positions of its subgroups. The singular points of a manipulator are in part determined by the values of critical joints, the singular joints. The three jointed arm experiences the same difficulties as the two jointed arm in maintaining the same attitude when the target is near a singularity. The task of holding the elbow to one attitude is more difficult with the introduction of another revolute joint variable. The simple heuristic of moving the elbow farther away from the singular point is not as effective. The elbow is more likely to move to another discrete solution if it has to travel farther in the jointspace. Joint locking for the elbow joint is necessary. 3.5. THE ORIENTATION PROBLEM The orientation of the end effector is specified in terms of its final coordinate frame with respect to the base coordinate frame. Three noncolinear points or two vectors on the end effector are necessary to specify the final coordinate frame uniquely. The two vectors used are the approach and orientation vectors which correspond to the z and y vectors respectively of the final coordinate frame. The test case for the orientation problem consists of three consecutively orthogonal PROPOSED KINEMATIC CONTROL METHOD / 51 joints. This corresponds to the two types of wrists which are used in industrial manipulators. Figure 3.9 shows the two common arrangements of revolute joints which are common in industry. The joint configuration in Figure 3.9a is equivalent to a spherical wrist and is found in RSI Robotics Systems International's RT3 arm and the Puma 560 arm. The configuration in Figure 3.9b is found on the Kodiak arm. The rotations are performed in the following order starting with the first joint: rotate about the z axis, then rotate about the new y axis, and finally rotate about the new z axis. The process of moving the joint angles of the wrist to align the end effector with that of the target orientation is done with the use of equivalent rotation vectors. The sum of consecutive rotations along different axes, where the amount and direction of rotation is specified by the direction of the vector and its magnitude, is equal to the vector sum of those rotations for small values of rotation. The method seems to work for very large rotations with the algorithm because of its iterative nature. The total transformation from one coordinate frame to another is equivalent to a rotation along a single common vector, the partial angular velocity vector. The convergence is achieved when the magnitude of the partial angular velocity vector is below a minimum. The result of a series of rotations about different axis vectors depend on the order of the rotation. However, for small rotations the order is not important since the rotated coordinate frame do not differ much from the original and rotations are approximately commutative as are translations in space. Using partial angular rotation vectors, it is not possible for a target frame to be PROPOSED KINEMATIC CONTROL METHOD / 52 Fig. 3.9 Two types of manipulator wrists PROPOSED KINEMATIC CONTROL METHOD / 53 unreachable with three revolute joints in the wrist and no joint limiting. The magnitude of the rotations can be limited to the range ir and - 7 T . The partial angular velocity vector is derived from the initial, C^ , and target, , coordinate frames. Wampler obtained his partial angular velocity vector from the direction cosine matrix, C, for the transformation from the initial to the target coordinate frame. C° = C° -C ( 3 . 4 0 ) C = C° T C ° ( 3 . 4 1 ) Twenty seven multiplications and eighteen additions are required for the direction cosine matrix alone. More operations are required to obtain the partial angular velocity vector. More than one set of equations is necessary because the equations are not valid for the whole jointspace. Simple vector operations such as cross products and dot products can give a reasonable approximation for the partial angular velocity vectors from the end effector coordinate frame. The cross product of two vectors results in a vector perpendicular to the plane of the two vectors with its magnitude equal to the sine of the angle between the vectors. It is a good approximation for the partial angular velocity vector for small rotations. At least two vectors of the initial and target coordinate frames are necessary thus requiring the calculation of consecutive rotations. The rotations due to the orientation vectors, w , are ' o projected onto the plane perpendicular to the rotations due to the approach vectors, w where: a w a= a . x a t ( 3 . 4 2 ) PROPOSED KINEMATIC CONTROL METHOD / 54 ( 3 . 4 3 ) Thus the equivalent paritial angular velocity vector becomes: ) w a a ( 3 . 4 4 ) This partial angular velocity vector requires only eighteen multiplications and twelve additions. It is fairly accurate for rotations from 0 to iit. For the simulations, the damping method is similar to that used for the one jointed manipulator. The damping factor is proportional to the magnitude of the orientation error. The damping coefficient, K, is set equal to one. For the desired accuracy of 0.001 radians, a fixed value of one for the damping coefficient is adequate. As with the positioning case, the minimum damping factor is set equal to the accuracy of the orientation. The problems with orientation are similar to those of position. Two solutions exist for each orientation. The discrete solution depends on the value of the second joint. The singular point occurs when the second joint is equal to zero or it and the first and third joints are aligned in space. Without damping, the Jacobian matrix becomes singular at these points. At the singular point, the first and third joints are redundant and the final values of joints one and three depend on the starting values. Because the joints are redundant, only the sum of the values is significant at the singular point. The problem of indeterminacy of the joint values also occurs with using iterative methods with redundant a 2 = K | W | | ( 3 . 4 5 ) PROPOSED KINEMATIC CONTROL METHOD / 55 manipulators[HOLL85]. With only orientation to contend with, the choice of solution is easier because the solutions are far apart in the jointspace. 3.6. SIX DEGREES OF FREEDOM MANIPULATORS For six degrees of freedom manipulators, the workspace is divided into a primary and a secondary workspace[YAN84]. The primary workspace consists of all end effector positions in space where any arbitrary orientation is possible. The secondary workspace consists of end effector positions where the possible orientations are limited. The total workspace is the sum of the primary and secondary workspaces. It is desirable to have a large primary workspace to increase the effectiveness of the manipulator. A large primary workspace is achieved by having small hand dimensions relative to the rest of the arm. Depending on the relative size of the hand links versus the size of the manipulator, various iterative inverse kinematics algorithms are possible with modified damping. Nonredundant industrial robots have their joints dedicated to performing the separate tasks of positioning and orienting the manipulator end effector. Ideally, the actions of the positioning joints should not affect the orientation of the end effector greatly and vice versa. This is the case for manipulators with very small hand dimensions and a large primary workspace. In actual practice, there are some interactions between positioning and orientation joints because the hand size is not negligible. If the interactions are small, the manipulator can be partitioned into two three degrees of freedom problems. The inverse kinematics solutions for the position and orientation can be performed independently. PROPOSED KINEMATIC CONTROL METHOD / 56 Fig. 3.10 Primary and Secondary workspaces PROPOSED KINEMATIC CONTROL METHOD / 57 Partitioning decreases the number of computations for an iterative scheme because in general the number of operations is proportional to the square of the number of joints. For the proposed inverse kinematics algorithm, the total number of operations depends also on the matrix and vector calculations which are functions only of the number of joints. Separating a six degrees of freedom manipulator into two three degrees of freedom problems actually only reduces the number of floating point operations by less than one half. The savings come from solving for the pseudoinverse of two three by six matrices instead of one six by twelve matrix. Most of the simulations were performed with RSI's Kodiak arm. The Kodiak arm was not designed for simple inverse kinematics control as is evident by the size of the offsets on the wrist and the size of the hand relative to the arm as shown in Figure 3.11. The Kodiak arm was designed to be operated in master/slave mode. The parameters of the Kodiak arm are given in Table 3.1. 3.6.1. Iterative Algorithm with Closed-Form Orientation The orientation of the manipulator tool is only a function of rotation and is not affected by any offsets or displacements. The solution for the orientation joints can be separated from that of the positioning joints. If the wrist contained consecutively orthogonal revolute joints, then closed-form solutions for the joint angles of the wrist can be calculated solely from the desired orientation in space and the values of the positioning revolute joints. Thus offsets and displacements do not enter into the equations. The rotational matrix for the orientation joints R 5R 6 is: PROPOSED K I N E M A T I C C O N T R O L M E T H O D / 58 Table 3.1 Kodiak Denavit-Hartenberg Parameters Twist(rad) Displacement(m) Offset(m) it 0.0000 0.0000 -** 0.0000 0.0000 0.0000 0.0000 0.0000 -0.0750 0.4500 0.1500 0.8400 0.4575 0.2250 0.0750 0.0000 C 5 C 6 - c s s 6 S 5 C 6 - S s S j S6 C 6 S 5 - c 5 0 (3.46) The terms for the sines and cosines of joints five and six are isolated in equation 3.46 and can be expressed in terms of the final coordinate frame with respect to the coordinate frame of joint four. A,-A5>A6 •= (A,- Aj.AaJ-'-Ts (3.47) The equations for the orientation joints are: 0. = atan2( a* f a« ) (3.48) y x 0 5 = acos( a«) (3.49) Z 0 6 = atan2( o* , -n* ) (3.50) z z The vectors are in the coordinate system of joint four. A n iterative method can be used for the positioning joints to compensate for offsets in the wrist. However, the end effector orientation is also a function of the positioning revolute joints. A n iteration scheme for alternately correcting the position and the orientation of the end effector is necessary. Partitioning works well i f the hand size is small with respect to the size of the arm such as in the case of the Stanford or Puma arms. Partitioning does not work as well on manipulators with PROPOSED KINEMATIC CONTROL METHOD / 59 large hand dimensions. Examples of manipulators with large dimensions in the wrist are RSI's RT3 and KODIAK arm. If the hand size is large, oscillations in the iteration will occur which will reduce the convergence rate. Depending on the relative magnitudes of the wrist offsets, interactions between the positioning and orienting joints may cause the algorithm to converge slowly or not to converge at all. The actions of the positioning joints in correcting for the position error cause by the wrist offsets will change the orientation. The interactions slows down the convergence rate and reduces some of the advantages of partitioning. It is possible for the position and orientation corrections to exactly match each other and cause the solution to continually oscillate without convergence. In high manipulability regions the closed-form solutions are accurate and convergence is monotonic as shown by the simulation on the Kodiak arm in Figure 3.12. The form of the equations is similar to Paul's equations for the last three joints of the elbow manipulator. The accuracy of the closed-form orientation equations is limited near singularities because the equations contain transcendental functions which are sensitive to numerical errors. The amount of errors in the closed-form solution is shown by the convergence to a singular wrist position in Figure 3.13. The algorithm was tested with random target points in jointspace. The joint values were varied by a maximum of plus or minus 0.2 radians for ten thousand test points. Figure 3.14 shows that there were many target points for which the algorithm failed to converge after fifty iterations. PROPOSED KINEMATIC CONTROL METHOD / 60 Fig. 3.11 RSI's RT3 arm and Kodiak arm PROPOSED KDNEMATIC CONTROL METHOD / 61 " r i i i n u n IIIIIIIIIII i n i | i i i i i i i i i | i i i i i i i i n i i i i i i i i i | i i i i i i i i i | i n i i i i i i i i i i i i i i i i i i i i n i i i i * 2 3 4 5 6 7 8 8 « tl Iteration Fig. 3.12 Convergence with closed-form orientation and partitioning, from joint vector (0,1,1,0,1,0) to (1,0,1,0,1,0) for the Kodiak PROPOSED KINEMATIC CONTROL METHOD / 62 T — I 1 1 I 1 1 1 1 1 r——i 1 1 I I I I 5 10 « 20 Iteration Fig. 3.13 Convergence to a singular wrist for closed-form orientation, from joint vector (0,1,1,0,1,0) to (1,0,1,0,0,0) for the Kodiak PROPOSED KINEMATIC CONTROL METHOD / 63 Fig. 3.14 Ten thousand random test points in jointspace with closed-form orientation for the Kodiak PROPOSED KINEMATIC CONTROL METHOD / 64 3.6.2. Partitioned Iterative Method An iterative scheme may be used for both the positioning and orienting groups. Higher orientation accuracy can be achieved over using closed-form solutions for the orientation joints. Because iterative schemes have some difficulty moving away from singularities, convergence may become unstable during the iteration. Overshoots do occur during the iteration when either the arm or the wrist lands near a singular position and the posture error is large. With the Kodiak arm, oscillations are visible in convergence to a singular wrist in Figure 3.15 and they are still visible in convergence to a good target posture in Figure 3.16. With the random test point routine, the convergence was inferior to the closed-form orientation method. In general, more iterations were required for convergence to the target points and the algorithm failed to converge for many cases as is shown in Figure 3.17. Oscillations can cause either the elbow or the wrist to pass through singularities to an alternate discrete solution. Internal subiterations must be used to compensate for the small instability near singularities for manipulators with large offsets. This will increase the number of iterations and further eliminate some of the advantages of partitioning. A trajectory which approaches the target monotonically would be ideal. Oscillations create unpredictability in the final solution. 3.6.3. Full Six Degrees of Freedom Iterative Method For a more general solution, the manipulator must be visualized as a full six degree of freedom manipulator. If the hand size is large relative to the size of the manipulator, the effect of the orientation joint movements on the end effector position is significant and vice versa. • The effect of all joints on both the position PROPOSED KINEMATIC CONTROL METHOD / 65 Fig. 3.15 Convergence to a singular point with partitioning, from joint vector (0,1,1,0,1,0) to (1,0,1,0,0,0) for the Kodiak PROPOSED KINEMATIC CONTROL METHOD / 66 Fig. 3.16 Convergence to a good point with partitioning from joint vector (0,1,1,0,1,0) to (1,0,1,0,1,0) for the Kodiak \ 1 PROPOSED KINEMATIC CONTROL METHOD / 67 Fig. 3.17 Ten thousand random test points in the jointspace with partitioning for the Kodiak PROPOSED KINEMATIC CONTROL METHOD / 68 and orientation of the effector must be taken into account. Convergence is more monotonic in terms of the joint corrections and the decrease in errors when the iteration is performed without partitioning. For a six by six Jacobian, every joint contributes to both the position and orientation errors. The partial angular velocity vectors in the Jacobian matrix consist of unit axis vectors and are independent of the dimensions of the manipulator. The partial angular velocity vectors in the Jacobian matrix for the positioning joints vary with the size of the manipulator. The direction and magnitude of the joint corrections depend on the relative magnitudes of the partial velocity and partial angular velocity vectors. The magnitude of the partial velocity vectors must be greater than unity for the positioning joints for the algorithm to converge. Therefore, for small manipulators the magnitude of the velocity vectors in the Jacobian due to the positioning joints must be greater than the unit vectors of the joint axis. The manipulator parameters and the partial velocities of the end effector must be scaled by an equal amount such that the magnitude of the column elements of the Jacobian in Figure 3.10 for positioning is greater than unity for the positioning joints. The scaling of the manipulator and the velocity inputs can also be used if the accuracies of positioning and orienting are different in magnitude. v'.= X v . ( 3 . 5 1 ) Where X is an arbitrary scaling constant. Scaling is not used for the Kodiak arm because its dimensions in meters is large compared with the unit axis vectors. Scaling is necessary for the Puma560 arm if its dimensions are measured in meters. PROPOSED KINEMATIC CONTROL METHOD / 69 For the Kodiak arm different damping schemes were evaluated. The square of the damping factor for the positioning or orientation joints was made proportional to either the distance or angular error. o| = i e | v | | (3.52) a 2 = K f l w | | (3.53) Si The damping factor was also made proportional to both the distance and angular errors. a2 = K | V + W | | (3.54) In the simulations, there was no visible difference between the two damping methods. The damping factors in equations 3.52 and 3.53 were used in the results. The minimum damping factor was set equal to the minimum distance and angular errors for convergence. The damping coefficient was set to one. An accuracy of 1.0mm and 0.001 radians was used. For the Kodiak arm, the singular points are easily identified. The arm is singular when the elbow or joint three is straightened, the fifth joint is a multiple of it, and when joint six is colinear with the first joint. In most cases the manipulability of the arm is evident from the positions of singular joints three and five. These are the same singularities in the three degrees of freedom positioning and orienting cases. The arm is in general in a high manipulability region when joints three and five are far from zero or it. On the Kodiak arm, the proposed inverse kinematics algorithm will converge to a solution in about half the number of iterations if partitioning is not used. Figures 3.18 and 3.19 shows the convergence of the Kodiak arm to a singular elbow PROPOSED KINEMATIC CONTROL METHOD / 70 Iteration Iteration Fig. 3.18 Convergence to singular elbow, from joint vector (1,-1,1,0,1,1) to (0,0,0,0,1,1) for the Kodiak Fig. 3.19 Convergence to singular wrist, from joint vector (0,0,1,0,1,0) to (-1,-1,1,1,0,0) for the Kodiak PROPOSED KINEMATIC CONTROL METHOD / 72 and a singular wrist respectively. The distance error as well as the magnitude of the partial angular velocity vector is plotted as a function of the iteration number. The corresponding joint corrections from the kinematic control algorithm is also plotted. The advantages of partitioning are eliminated for arms with relatively small primary workspaces. The solution is also more stable in most regions of the workspace and there is less tendency for the joints to oscillate. The joints still oscillate initially for large workspace changes until the end effector is close to the target posture. A large workspace change is more than 0.1 meters and 0.1 radians. There is some difficulty in being able to determine the discrete solution as for the three degrees of freedom cases. The arm can converge to an elbow up/elbow down, or left shoulder/right shoulder, or flip/no flip wrist. The direction of the joint changes depends on the target position and orientation with respect to the starting position and orientation. The discrete solution can be influenced by moving the starting posture from a singular point into a high manipulability region. The new starting posture must be closer in the least squares sense for the joint changes to the desired solution than for any other possible solutions. For example, if an elbow up solution is desired, then the manipulator elbow must be moved far away from an extended elbow position into an elbow up position for the start of the iteration. The same principle applies to the wrist in choosing a flip or no flip wrist. The starting posture for the arm is moved away from a singular elbow and wrist position by shifting the joints away from jointspace boundaries of one discrete solution. 6 . = 6 - 6 ( 3 . 5 5 ) start max PROPOSED KINEMATIC CONTROL METHOD / 73 6 ^ = 6 . + 6 ( 3 . 5 6 ) s t a r t mm The heuristics do increase the number of iterations required by potentially moving the arm away from the target. Fewer problems arise in selecting the discrete solution when the changes in the nonsingular joints are small. If the changes in the nonsingular joints are large and the elbow or wrist is close to singular, then movement to an alternate solution is more likely. The discrete solution can also be determined by restricting the motions of some of the joints to prevent them from moving into the singular region. This can be accomplished by removing a column for that joint from the Jacobian matrix. The contributions due to a joint is removed by either setting the corresponding column in the Jacobian matrix to zero or increasing the damping value for that joint. Increasing the damping value for a particular joint increases the condition number for the inverse Jacobian and increases the numerical errors in the pseudoinversion. Increasing the damping factor is not effective at limiting joint corrections. Setting a column of the Jacobian to zero results in a joint change of zero for that particular joint without increasing the condition number greatly. The algorithm proceeds with the locked singular joint until the joint changes are very small. At this point, the arm is midway between the alternate solutions in jointspace. The arm is closer to the desired discrete solution than the alternate solution. When the singular joint is unlocked, the arm will converge to the desired solution. The algorithm was implemented with a graphics routine in a C program on the HP9050 computer. The proposed kinematics control algorithm converged to a PROPOSED KINEMATIC CONTROL METHOD / 74 target in the workspace in all cases when no limits were placed on the joint values. When the manipulator was viewed graphically, it was evident that singular joints were oscillating between solutions when the arm was in a singular region even when the end effector followed the trajectory exactly at the sample points. Thus limiting the range of the joint values for the solution is necessary to prevent oscillations of singular joints between sample points. The oscillations in the elbow will increase the trajectory error tremendously between the sample points. The rate of convergence to singular points in the six degree of freedom case is slow. About twenty iterations are necessary for points in a singular region. The rate of convergence can be increased by increasing the rate at which damping is decreased. The number of iterations is halved when the damping factor is made proportional to the error instead of proportional to the square root of the error. a 2 = K |v | | 2 (3.57) However, the algorithm becomes unstable for unreachable targets and for some singular points. Singular joints tend to oscillate for unreachable targets. Some stability is sacrificed for speed. Three separate strategies are needed to handle the three target regions. The algorithm does not have difficulty with targets in the high manipulability region and with targets in the singular region. There are some difficulties when the arm is folded into itself such as when the end effector is near the origin of the base. It will converge to other targets in the singular region and to the correct attitude by shifting the starting posture and by the application of joint locking. PROPOSED KINEMATIC CONTROL METHOD / 75 For targets out of reach, a fairly large damping value is required for the joint rates to die to zero quickly. A heuristic rule was developed to guess the region of the target posture. When oscillations occur, the target posture is either a singular or unreachable point. First the target is assumed to be reachable. If oscillations still persist, then the target must be unreachable a locked elbow and a large damping coefficient is tried. Part of the problem with singularities is due to the convergence criterion of looking only at the least squares error. Near a singular point, the manipulator is nearly redundant and small movements in the workspace are accomplished by large joint movements. For example, with the single link one degree of freedom manipulator a motion of 0.5mm near a high manipulability position such as \"K requires only a rotation of 0.0005 radians. At the singular point, a movement of 0.5mm requires a rotation of 0.0316 radians. If 0.5mm is the tolerance for positioning error, then the convergence region is not a point in jointspace but a region whose size depends on the location of the target point. For a given convergence volume in the workspace, the volume in the jointspace grows as the manipulator approaches a singular point. When the manipulator is very close to a singular position the convergence regions merge making it difficult to select the solution. The algorithm will terminate anywhere inside the convergence region. This is evident in the six degrees of freedom case. PROPOSED KINEMATIC CONTROL METHOD / 76 3.7. EVALUATION OF THE PROPOSED INVERSE KINEMATIC ALGORITHM The real time performance of the inverse kinematics algorithm was tested with a graphics program on the HP9050 workstation driving an HP98710 graphics display. The input to the program was through a knob box. To test for all possible target postures, the inputs consisted of three Cartesian values for the position and three Euler angles for the orientation. To test for only targets in the workspace, the inputs were joint values which were transformed to target postures for the inverse kinematic algorithm. The arm converged in under twenty iterations in most cases. A few problems arose because joint limiting was not implemented. The algorithm had difficulty in converging to some physically unrealizable positions near the base. With large posture changes, the arm wrapped into itself and had difficulty moving out of the position. At this position, the value of either joint three or five is n. Even when a target was reachable, the arm cannot move out of the wrapped position easily and was therefore in a local minimum for the inverse kinematics algorithm. The distance and angular errors did not decrease for small movements of the joints. The position is physically unrealizable in the actual manipulator and is not a valid target position. The algorithm was also tested with a random target generation routine. Test runs were performed whereby the target posture was moved randomly by changing all joint angles by a maximum of plus or minus 0.2 radians. Figure 3.20 shows the distribution of number of iterations required to reach different PROPOSED KINEMATIC CONTROL METHOD / 77 Fig. 3.20 One hundred thousand random test points for the Kodiak arm without partitioning PROPOSED KINEMATIC CONTROL METHOD / 78 target postures in the workspace. The cost of iterative kinematics solutions is usually measured in the number of floating point operations because the execution times for these operations are usually much greater than the execution times for any other operation. For the proposed kinematic control algorithm using Gaussian elimination in the pseudo inversion, the Kodiak arm required 768 floating point operations. The operations consisted of basically equal numbers of additions and multiplications with some divisions. The algorithm was developed in the C programming language on a HP9050 computer. Timing results were only available using the system's 60 Hz clock. For the Kodiak arm, each iteration took about 1.3 clock cycles or about 21 ms. Assuming a maximum of twenty iterations for convergence, the sampling rate is about 2.5 Hz. Timing tests were performed to estimate the amount of time needed to perform a single floating point operation on two array elements and storing the result into a third element in main memory. The operation with multiplications and additions took about 15 us each which would translate to a time of 11 ms per iteration for the algorithm. There was an overhead of approximately two times the amount of time needed to perform the floating point operations alone. The increased time can be the result of the amount of memory needed for the test program. The whole program did not fit into high speed cache memory. Therefore, lines were constantly being swapped between cache and main memory. Many array indexing operations are needed, and the total number of integer operations can be significant. In addition, the overhead can be significant in PROPOSED KINEMATIC CONTROL METHOD / 79 performing subroutine calls. The execution times will be lower with fewer subroutine calls and by writing some sections in assembly language. These results were used to extrapolate expected performances onto other computer systems. A Motorola 68020 system with a 68881 math coprocessor running at 16MHz can perform a multiplication or addition in approximately 6 jts. With a potential few megabytes of no wait state high speed ram available, the memory access times should be negligible. A projected time for each iteration is therefore: 7 6 8 * 6 = 4 . 6 m s ( 3 . 5 8 ) With a maximum of 20 iterations per target posture the sampling frequency will be approximately 10 Hz. Faster systems can be constructed with high speed adders and multipliers. Floating point multipliers are available which can perform a multiplication in 400ns. At this speed and taking a conservative estimate of double the number of floating point operations, the sampling frequency will be about 90 Hz. The sampling rate can be further increased by using multiprocessor architectures. The sample points could be pipelined. Increasing the number of processors by n would increase the sampling rate by n. The delay between input and output will remain constant. Multiplication and additions can be performed in parallel, therefore increasing the sampling rate by two. The sampling frequency for control systems must be a least double the natural resonant frequency of the arm. For the Stanford arm, the average natural resonant frequency of the links is about 20Hz. Thus the sampling frequency for PROPOSED KINEMATIC CONTROL METHOD / 80 the control system must be at least 40Hz. The kinematics control algorithm does not have to run as fast. The kinematics control algorithm provides target posture points or knot points[TAY79] along the trajectory of the manipulator for the control algorithm. The sampling rate at which actual target points are passed to the control algorithm depends on the desired response time of the system and the accuracy of the end effector in following the desired trajectory. The points which are passed to the control algorithm could be interpolated from the knot points to increase trajectory accuracy. For a teleoperator system, the manipulator end effector speeds are generally low and trajectory accuracy is not as important as posture accuracy. Sampling rates of 10 to 20 Hz for the kinematics control algorithm may be adequate. The least squares solution for the damped joint changes can be obtained using a variety of methods. The factors which determine which method should be used are the numbers of operations required, the type of operations required, the numerical stability, and the condition number. Gaussian elimination was used for the operation count above, but either Cholesky decomposition or QR decomposition could have been used. Cholesky decomposiition gives the fewest number of floating point operations. However, square root function calls which might not be available or which can take excessive computation time are required. Least squares solution with QR decomposition gives the most operations, about twice the number with Gaussian elimination, but the method is more stable with very high posture accuracy. Errors in the partial velocities and partial angular velocities are amplified by the PROPOSED KINEMATIC CONTROL METHOD / 81 condition number. In both Cholesky and Gaussian methods, the condition number is the square of that for QR decomposition. With high posture accuracy, double precision arithmetic may be necessary for Gaussian elimination and Cholesky decomposition. Gaussian elimination is capable of 1mm positioning accuracy for the Kodiak arm and is adequate for 0.1mm positioning accuracy in some cases with single precision arithmetic. This assumes that six significant digits are available with single precision floating point numbers. The smallest damping factor required is equal to the square of the distance or angular errors. This restriction may be relaxed but convergence near singularities may suffer. Double precision arithmetic may be used for higher accuracy but a single double precision floating point operation is equivalent to four single precision floating point operations. Thus for high posture precision, QR decomposition should be used. CHAPTER 4. PARAMETER ESTIMATION The proposed iterative kinematics control method requires only the Denavit-Hartenberg parameters for the kinematics representation. Many alternatives have been proposed for estimating the Denavit-Hartenberg parameters depending on the accuracy required, the generality of the algorithm, the availability of test points and the assumptions made for the manipulator. The parameters may be measured directly from production drawings of the manipulators or from the manipulators themselves. However, differences do exist between the parameters for the actual manipulators and those obtained from production plans. The parameters for the links are different from actual link shapes and dimensions. A kinematician must analyze the manipulator to determine an equivalent Denavit-Hartenberg representation. Production drawings also do not account for the differences that exist with specific manipulators. Manufacturing tolerances are usually much less than the positioning accuracy of the manipulator. Actual measurements on the machines will be necessary for good positioning accuracy. Damage to the machinery may cause joints to be out of alignment when compared with production plans. A more accurate estimate of the parameters may be obtained through a calibration process. Besides errors in the Denavit-Hartenberg parameters, many nonlinear effects will reduce the accuracy of the representation in actual manipulators. A few factors which contribute to kinematic errors are: 1. base motion 2. encoder offset 3. noncircular gears 4. gear deadzones or backlash 82 PARAMETER ESTIMATION / 83 5. gravity weighting 6. joint shaft wobble 7. joint compliance 8. link flexion 9. thermal expansion 10. cross coupling of joints Many of the above factors are due to the transmission between the electric motors and the joints. Mounting the feedback sensors on the motors reduces costs but introduces many nonlinear errors through the transmission mechanism. Many calibration methods introduce additional parameters to account for the errors caused by the transmission mechanism. The errors caused by backlash, noncircular gears and joint compliance can be eliminated by placing higher resolution feedback sensors directly on the joint axes. High resolution joint position sensors such as optical encoders, although very expensive, are easily available and eliminate the need to calibrate the encoders. Other errors such as gravity weighting, joint shaft wobble, link flex, and thermal expansion are usually too small to have significant effect. If the errors due to link flex and gravity weighting were significant, the dynamics of the manipulator must be included. Encoder offset can be eliminated by calibration. Base motion can be partially eliminated by securing the manipulator to the laboratory floor. The cross coupling of joints is a problem which has to be dealt with by the controller. The inverse kinematics algorithm only specifies the joint values which are required to reach a desired posture and not the actuator inputs that are required. Assuming most of these errors are negligble compared with the desired accuracy, the basic Denavit-Hartenberg representation should give an accurate model of the manipulator over its workspace. Most calibration methods assume that equations already exist for the configuration PARAMETER ESTIMATION / 84 of the manipulator. Correction parameters are added to the a priori knowledge to account for the small differences that exist in individual manipulators. These methods are tailored to specific classes of manipulators. For the proposed inverse kinematics algorithm, a general method is required which is applicable to a wide range of manipulators. Extra parameters would increase the amount of computation required to calculate the Jacobian matrix and perform the inversion. The calibration methods differ in the type of sample points and the number required for accurate estimation. Methods which do not assume a priori knowledge of the manipulator configuration are presented and their advantages and disadvantages discussed. 4.1. NLSQ METHODS The forward kinematics equations are nonlinear in the revolute joint variables and in the twist values between joints. The sine and cosine of the revolute joint variables and of the twist values appear in the equations for the position and orientation of the end effector. Assuming that the errors due to the gear transmission can be ignored then the unknowns are the twist values, displacement and offsets which can be obtained by solving the set of nonlinear forward kinematics equations[CHEN86] using nonlinear least squares. The nonlinear least squares method is iterative and a good initial estimate of the parameters is required. A diverse set of sample points can be used to estimate the parameters which will minimize the position and orientation errors. Many points are required to obtain a good representation of the manipulator over its workspace. The test PARAMETER ESTIMATION / 85 data consist of the position and the orientation data with the corresponding joint values. These methods do not put any restrictions on the test data. The test points can be known target postures on a jig[MOOR84] or a calibration cube[FOU84]. The joint angles can be easily recorded for the end effector in different target positions. This method will only be valid for manipulators of similar size and configuration. The process is severely affected by errors in the joint sensors and an offset in the joint sensors will affect the results significantly[WHI84]. Another method[LIS85] looks only at differential relationships between joint movements and the movements of the end effector. A general set of relationships between the differential joint values and posture variables are derived as a set of nonlinear equations in the manipulator parameters. The method is general and can be used with any manipulator. Relative changes in position and orientation values are required instead of absolute changes in position and orientation values. The method is not affected by joint encoder offsets since only differential changes appears in the equations. However, because the method requires the measurement of differential position and orientation values, very accurate measurements have to be made. The changes in orientation are derived from changes in the n, O, and a vectors of the final coordinate frame which increases the number of measurement points considerably. PARAMETER ESTIMATION / 86 4.2. VECTOR METHOD The calibration routine proposed here uses a vector approach to identify the relationship between the end effector movement and joint movement and is similar in concept to the method suggested by Barker[BARKER83]. Only the Cartesian values of a single target point on the end effector is required. No a priori knowledge of the manipulator configuration is needed. The calibration process takes advantage of the assumptions in the Denavit-Hartenberg representation of manipulators. With the manipulator fixed at a posture in space, the offsets, displacements, and twist values can be determined from knowledge of the direction and position of the joint axes. The direction and position of the individual joint axes are determined by moving each joint individually while holding the other joints at fixed reference values and following the trajectory of the target point. For a revolute joint with joint angle 8, the target draws a circular path in space with the equation e«sint? + f « c o s 0 + g = p ( 4 . 1 ) Where g is the projection of the target onto the joint axis. The vectors e and f are the y and x vectors of the coordinate frame respectively. The vector p is the Cartesian position of the target. For a prismatic joint, the target draws a line in space h - 0 + 1 = 0 ( 4 . 2 ) The coefficients are solved with a least squares fit of the equation with the test points for the trajectory of the target. The direction of the joint axis z for a revolute joint is given by (exf ) z = (4.3) Bexf 1 PARAMETER ESTIMATION / 87 For a prismatic joint, the direction is given by the vector h. h z = ( 4 . 4 ) A minimum of three test points are required for each revolute joint and two points are required for each prismatic joint. This is an improvement over Barker's equations which are more complex, requiring a minimum of four points to solve for the joint axis vector. The a vector between two joint axes is defined to be perpendicular to both axes and can be obtained by taking the cross product of two adjacent revolute axis. a. = z. Xz. x, ( 4 . 5 ) l l i+l If the joint axes are parallel, the difference between the projection of the target on the joint axes is the direction of the a vector. a i = p i + r Pi ( 4- 6 ) The relationship between different joints and the parameters can be calculated from the Denavit-Hartenberg definition. The magnitude of the twist between two adjacent joint axis is given by the magnitudes of the dot product and cross product of the joint axis vectors. tan<> = ( 4 . 7 ) z.«z. , I i+l Given two known points on each axis, the translations required to move from the point on one joint axes to a point on the second joint axis can be obtained by solving a set of simultaneous equations P 2 _Pi = [ J2 j i a, ] d ( 4 . 8 ) d T = [ <V , al , <£.' ] T ( 4 . 9 ) If the points p 1 and p 2 are the origins of the coordinate systems for joints one PARAMETER ESTIMATION / 88 x 0 Fig. 4.1 Projection of the target onto the joint axes PARAMETER ESTIMATION / 89 and two, The origin of the coordinate systems are determined by the projections of the target onto the joint axis vector and the distance to the a vector. 0.^= p. ..- d . n ' (4.10) l+l * i + l l+l To obtain a good set of test points, the target must move with motion of the joints. An extension arm equivalent to an offset in the last link may have to be attached to the end effector to translate changes in orientation to changes in position in space. The end effector orientation does not have to be explicitly determined. A large extension will be necessary for manipulators with small hand dimensions because the number of significant digits in the coefficients depend on the range of values in the test points. The method is not affected by offsets in the joint sensors. It can be used to determine the zero position for the joint sensors for add-on equipment by fitting points to the equation 6' = 6 + e (4.11) where 6 is the actual joint value and 6' is the corrected joint value. This method was simulated on the HP9050 computer. Higher accuracy may be obtained by using different reference values and performing an averaging of the Denavit-Hartenberg parameters. 4,3. EVALUATION OF THE PROPOSED CALIBRATION METHOD The method performs quite well except when low accuracy measurements are given. With low accuracy data, the twist values become very sensitive to errors in the test data. Transcendental functions are required to determine the twist values and are very inaccurate when measurement errors are large. Parallel joints are common for most manipulators. Joints which appear to be nearly PARAMETER ESTIMATION / 90 parallel results in very large values for the other parameters. A minimum magnitude for the twist values is needed before adjacent axes are considered parallel. For three measurement points per joint, the displacement and offset values are in general three times the errors in the data measurements. The error can be decreased by taking more data points. For example, Table 4.1 and 4.2 shows the errors for the Kodiak for measurement devices of 10mm and 1mm accuracy using eight sample points for each joint. The error in the joint sensors is assumed negligible compared with the errors in the target position. This assumes that the circle generated by the rotation of each joint is sufficiently large enough to give coefficients with reasonably high accuracy. A 0.5m extension was added to the end effector of the Kodiak arm. With more sample points the error in the parameters can be decreased with least squares averaging. An average of the parameters obtained with different reference points can decrease the error in the parameters throught the workspace. 4.4. DATA COLLECTION The calibration routines require fairly accurate test data. Some of the methods which are available are: 1. video camera 2. large area photodiodes 3. theodolites 4. measuring tape Video cameras are usually CCD devices which are limited in their array size. PARAMETER ESTIMATION / 91 Table 4.1 Parameter errors for measurements of 0.01m accuracy with 8 samples per joint Twist(rad) Displacement(m) Offset(m) •0.00244 - 0 . 0 0 1 0 5 0.000297 •0.00188 -0 .00381 0.00230 •0.00345 0.00000 - 0 . 0 0 4 0 3 0.00472 0.00000 - 0 . 0 0 3 0 5 0.00075 - 0 . 0 0 2 4 5 - 0 . 00392 0.00000 0 .00460 0.00294 Table 4.2 Parameter errors for measurements of 0.001m accuracy with 8 samples per joint Twist(rad) Displacement(m) Offset(m) •0.00024 - 0 . 00010 0.00030 •0.00019 - 0 . 0 0 0 3 8 0 .00023 0.00035 0 .00000 - 0 . 00040 0.00048 0 .00000 -0 .00031 0.00007 - 0 . 00024 - 0 . 0 0 0 3 9 0.00000 0.00046 0 .00029 The cameras usually have good characteristics and are quite linear over the the whole viewframe. The transformations from the object to the camera can be easily calculated. Image processing may be necessary to locate the target point on the arm extension. Common CCD camera have 512 by 512 pixel arrays which translate into an accuracy of only ±0.1% over the field of view. Photodiodes depend on the presence of a concentrated light source in a dark PARAMETER ESTIMATION / 92 background. The target is usually a high intensity infrared diode and is focussed onto the diode surface by a lens. Precise focusing is not required as long as all of the light spot hits the photodiode. The photodiodes have good linearity except near the perimeter of the diodes. Photodiodes are used in a commercial remote robotics measurement system called RobotCheck. Photodiodes give the x-y coordinates of the target in the viewframe directly so that calculations are minimized. With calibration of the device, their accuracy could be increased to about ±0.025% of the viewwidth[DAINIS85]. Multiple targets are possible with time multiplexing of the diodes. The information may be easily fed directly to a computer. Their performance is affected by stray reflected infrared light off the manipulator or the surroundings. However, it is very accurate in a laboratory setting. Theodolites are surveying equipment which require a human operator to align the sight onto the target and to record the readings. The target is the center of a spherical ball. Two or more devices may be required and triangulation is used to obtain three dimensional values. They can obtain very high accuracy as exhibited by the measurements taken at the National Bureau of Standards[CHEN86]. An accuracy of ±0.02 mm accuracy per meter of target travel was claimed. If a rough estimate of the offset and displacement parameters are needed, the end effector position may be estimated by a simple triangulation device using tape measurements. The measurements from three noncolinear points is necessary to determine the target position in space. CONCLUSIONS Various algorithms have been investigated for the purpose of obtaining a general real-time inverse kinematics solution with minimal analysis of the manipulator. An objective for the solution was a large allowable workspace region which included the singular region. Many previous methods did not handle singularities directly or require too much detailed analysis. Among the methods investigated, it was: found that multinomial approximation of the inverse kinematics function was accurate only in the high manipulability region. Multinomials required too many terms for the multi-jointed case to be computationally efficient. Closed-form solutions and solutions in terms of the roots of a polynomial were found to require much analysis, were not accurate near singularities, and did not accept unreachable targets as inputs. Efficient closed-form solutions were found to be possible only in a very limited number of configurations. Many iterative algorithms were investigated for their stability and efficiency. Partitioning has been frequently used as a strategy for reducing the number of operations in iterative methods. Six degrees of freedom manipulators can be functionally partitioned into a positioning and an orientation group. Partitioning was attempted with a closed-form solution for the orientation group and with the iterative algorithm for both the positioning and orientation groups. Partitioning reduces the number of operations per iteration by approximately one half. It was shown that partitioning did not lower the amount of time required for 93 / 94 convergence because the number of iterations required is approximately doubled that of the nonpartitioned case. The iterative algorithm was also unstable in the singular region. An efficient iterative algorithm was proposed and investigated which considered the contributions of all joints to both the position and orientation of the end effector. For the manipulators which were tested, the algorithm converged in under ten iterations on average and in about twenty for difficult target postures. The iterations of the proposed inverse kinematic algorithm were stabilized with a modified damping scheme for the joint corrections. The algorithm was stable in all regions of the manipulator operation and for target postures which are not reachable. The iteration parameters required are: 1. 6", the shifting value for singular joints 2. K, the damping coefficient 3. x> the scaling factor for the manipulator dimensions and velocity inputs 4. the jointspace for one discrete solution The iteration parameters are significant only in stationary singular points and are chosen by the algorithm's performance in converging to these points. The selection of the parameters can be made with a computer simulation of the target arm. A general program, arm, has been written to perform the simulation for any set of Denavit Hartenberg parameters. An investigation into joint limiting was made for joints which determined the discrete solution when alternate solutions are adjacent in the jointspace. (e.g. elbow up/elbow down) When the jointspace is unrestricted, the algorithm did not / 95 have difficulty converging to all points in the jointspace. However, it is advantageous to have some control over the final discrete solution. ( e.g. joint angle limits ) Oscillations between discrete solutions increased the error between sample points. In the case of the elbow joint, it must pass through the fully extended position before it can reach the alternate discrete solution. The discrete solution can be chosen by moving the starting position of the arm away from the jointspace boundaries of the discrete solution. It was found that for large joint changes, locking of the singular joint was sometimes necessary during the iteration for convergence to the desired solution. A general calibration routine was proposed to obtain the basic Denavit-Hartenberg parameters required in the kinematic control algorithm. The calibration algorithm was simplier and more accurate than the one proposed by Barker[BARKER84]. A minimum of three test points for each joint was required and fewer operations were needed to estimate the parameters. S U G G E S T I O N S F O R F U R T H E R R E S E A R C H The algorithm can be easily extended to include redundant manipulators to handle singular conditions. With redundant manipulators, extra requirements may be needed to add additional contraints to the system. The basic assumption of the proposed inverse kinematics algorithm is to solve for the closest least squares solution for the desired target end effector posture. Additional requirements will be necessary to limit the allowable attitudes of the redundant arm to prevent it from becoming too twisted. A problem with using iterative methods with redundant arms is in the ^determinancy of the position of the links and the final joint values. The arm can terminate in very undesireable attitudes if only restrictions on the end effector is used. The arm should always try to stay in the high manipulability region. An application of redundancy with industrial machines would be to include the locomotion of the machines to give extra redundant degrees of freedom. Joint limiting on all joints can be further investigated in detail for various manipulators. The proposed kinematics control algorithm convergences monotonically to the target posture when the arm is very close to the target and thus makes the application of a single joint limit very simple. There are some slight oscillations in the joints for large translations and rotations. A large value for translation and rotation is about 0.1 meters and 0.1 radians for the Kodiak arm. More heuristics and strategies need to be investigated if many joint limits are applied simultaneouly to ensure that the correct solution is reached. An efficient singular value decomposition algorithm will increase the effectiveness 96 / 97 of the inverse kinematics algorithm. Damping will only be necessary in singular regions and for small posture errors which is evident from the singular values of the Jacobian matrix. Convergence to a target posture is faster without damping when the target posture is in a high manipulability region. Additional heuristics such as varying the damping coefficient could be applied when the arm is in a singular region to increase the convergence rate. REFERENCES [ACTON70] F.S. Acton, Numerical Methods that Work, Harper & Row, 1970. [AHMAD85] S. Ahmad, "Second Order Nonlinear Kinematic Effects, and Their Compensation", IEEE Conference on Robotics and Automation, 1985, pp.307-314. [ANGLE85] J. Angeles, "On the Numerical Solution of the Inverse Kinematic Problem", The International Journal of Robotics Research, Vol. 4, No. 2, Summer 1985, pp.21-37. [ANGEL86] J. Angeles, "Iterative Kinematic Inversion of General Five-Axis Robot Manipulators", The International Journal of Robotics Research, Vol. 4, No. 4, Winter 1986, pp.59-70. [BARKER83] K.L. Barker, "Vector-Algebra Approach to Extract Denavit-Hartenberg Parameters of Assembled Robot Arms", NASA Technical Paper 2191, 1983. [BEN84] B. Benhabib, A.A. Goldenberg, R.G. Fenton, "A Solution to the Inverse Kinematics of Redundant Manipulators", ASME Journal of Dynamic System, Measurement, and Control, 1985, pp.368-374. [BAZ84] A. Bazerghi, A.A. Goldenberg, J. Apkarian, "An Exact Kinematic Model of PUMA 600 Manipulator", IEEE Transactions on Systems, Man and Cybernetics, Vol. SMC-14, No. 3, May/June 1984, pp.483-487. [CHA086] L.M. Chao and J.C.S. Yang, "Development and Implementation of a Kinematic Parameter Identification Technique to Improve the Positioning Accuracy of Robots", Robots 10, pp.11.69-11.81. [CHEN86] J. Chen and L.M. Chao, "Positioning Error Analysis for Robot Manipulators with All Rotary Joints", IEEE Conference on Robotics and Automation, 1986, pp.1011-1016. [COL83] J.C. Colson and N.D. Perreira, "Kinematic Arrangements Used in Industrial Robots", IEEE Computer Society Tutorial on Robotics, pp. 73-90. [CR084] W.J. Crochetiere, "Locating the Wrist of an Elbow-Type Manipulator", IEEE Transactions on Systems, Man, and Cybernetics, Vol. SMC-14, No. 3, May/June 1984, pp.497-499. [DAINIS85] A. Dainis and M. Juberta, "Accurate Remote Measurement of Robot Trajectory Motion", IEEE Conference on Robotics and Automation, 1985, pp.92-99. [DUFFY80] J. Duffy, Analysis of Mechanisms and Robot Manipulators, Edward Arnold, 1980. [ELG84] S. Elgazzar, "Efficient Solution for the Kinematic Positions for the Puma 560 Robot", National Research Council Canada, December 1984, ERB-973, NRC No.23952. 98 / 99 [ERSU84] E. Ersu and D. Nungesser, "A Numerical Solution of the General Kinematic Problem", IEEE Conference on Robotics and Automation, 1984, pp.162-168. [FEA83] R. Featherstone, "Position and Velocity Transformations Between Robot EndEffector Coordinates and Joint Angles", The International Journal of Robotics Research, Vol. 2, No. 2, Summer 1983, pp.35-45. [FEA83a] R. Featherstone, "Calculation of Robot Joint Rates and Actuator Torques from End Effector Velocities and Applied Forces", Mechanisms and Machine Theory, Vol. 18, No. 3, 1983, pp. 193-198. [FOU86] L.P. Foulloy and R.B. Kelley, "Improving the Precision of a Robot", IEEE Conference on Robotics and Automation, 1984, pp.62-67. [GUPTA85] K.C. Gupta and K. Kazerounian, "Improved Numerical Solutions of Inverse Kinematics of Robots", IEEE Conference on Robotics and Automation, 1985, pp.743-747. [HOLL83] J.M. Hollerbach and G. Sahar, "Wrist-Partitioned, Inverse Kinematic Accelerations and Manipulator Dynamics", The International Journal of Robotics Research, Vol. 2, No. 4, Winter 1983, pp.61-76. [HOLL85] J.M. Hollerback and K.C. Suh, "Redundancy Resolution of Manipulators through Torque Optimization", IEEE Conference on Robotics and Automation, 1985, pl016-1021. [HUS84] CK. Huscroft, Learning Algorithms for Manipulator Control, UBC MASc Thesis, October 1984. [JEN84] A. Jennings, Matrix Computation for Engineers and Scientists, John Wiley & Sons, 1984. [KELLY84] F.A. Kelly, and M.P. Deisenroth, "Mechanics and the Graphical Off-Line Programming of Robots", Conference on Computers in Engineering, 1984, pp. 101-106. [KLEIN83] CA. Klein and CH. Huang, "Review of Pseudoinverse Control for Use with Kinematically Redundant Manipulators", IEEE Transactions on Systems, Man, and Cybernetics, Vol. SMC-13, No. 3, March/April 1983, pp.245-250. [KLETT79] R.D. Klett, A Cerebellum-Like Learning Machine, UBC MASc Thesis, 1979. [KOREN85] Koren, Yoram, Robotics for Engineers, McGraw Hill, New York, 1985. [LEE82] G.C.S. Lee, "Robot Arm Kinematics, Dynamics and Control", IEEE Computer, December 1982, pp.62-80. / 100 [LIPKIN85] H. Lipkin and J. Duffy, "A Vector Analysis of Robot Manipulators", Recent Advances in Robotics, Geraldo Beni and Susan Hackwood (ed), John Wiley and Sons, 1985, pp. 175-214. [LIS85] R. Liscano, H. El-Zorkany, and I. Mufti, "Identification of the Kinematic Model Parameters of an Industrial Robot", COMPINT 85, pp.477-480. [LnJ86] C.H. Liu and Y.M. Chen, "Multiprocessor-Based Cartesian Space Control Techiniques for Mechanical Manipulator", IEEE Conference on Robotics and Automation, 1986, pp.823-827. [LUM84] V.J. Lumelsky, "Iterative Coordinate Transformation Procedure for One Class of Robots", IEEE Transactions on Systems, Man, and Cybernetics, Vol. SMC-14, No. 3, May/June 1984, pp.500-505. [MIL83] V. Milenkovic and B. Huang, "Kinematics of Major Robot Linkage", Conference Proceedings of the 13 th International Symposium on Industrial Robots/Robots 7, April 17-21, Chicago, Illinois, Vol. 2, pp.16.21-16.47. [MOOR84] B.W. Mooring and G.R. Tang, "An Improved Method for Identifying the Kinematic Parameters in a Six Axis Robot", Conference on Computers in Engineering, 1984, pp.79-84. [MORE77] J.J. More, "The Levenberg-Marquardt Algorithm: Implementation and Theory", Numerical Analysis - Lecture Notes in Mathematics 630, G.A. Watson(ed), Springer-Verlag, 1977. [OPP84] I.J. Oppenheim, "Kinematics and Control for construction Robots", Conference on Computers in Engineering, 1984, pp.236-241. [ORIN84] D.E. Orin and W.W. Schrader, "Efficient Computation of the Jacobian for Robot Manipulators", The International Journal of Robotics Research, Vol. 3, No. 4, Winter 1984, pp.66-75. [OWENS] R.A. Owens and P.D. Kovesi, "Robot Manipulability", unpublished, University of Western Australia, 14pp. [PAUL81] R.P. Paul, Robot Manipulators: Mathematics, Programming and ControlThe Computer Control of Robot Manipulators, MIT Press, 1981. [PAUL81a] R.P. Paul, B. Shimano, and G.E. Mayer, "Kinematic Control Equations for Simple Manipulators", IEEE Transactions on Systems, Man,. and Cybernetics, Vol. SMC-11, No. 6, June 1981, pp.449-455. [PAUL81b] R.P. Paul, B. Shimano, and G.E. Mayer, "Differential Kinematic Control Equations for Simple Manipulators", IEEE Transactions on Systems, Man, and Cybernetics, Vol. SMC-11, No. 6, June 1981, pp.456-460. [PEIPER69] D.L. • Peiper, The kinematics of manipulators under computer control, Ph.D. dissertation, Computer Science Department, Stanford University, 1969. / 101 [RICE69] J.R. Rice, The Approximation of Functions: Vol. 2, Nonlinear and Multvariant Theory, Addison-Wesley, 1969. [SCALES85] L.E. Scales, Introduction to Non-Linear Optimization, Springer-Verlag, New York Inc., 1985. [SCH82] B. Scheffer, "Geometric Control and Calibration Method of an Industrial Robot", Proceedings of the 12th International Symposium on Industrial Robots, June 9th to 11th, 1982, Paris, France, pp.331-339. [SCHMIDT82] R. Schmidt, A.V. Balakrishnan, and M. Thoma (ed), Lecture Notes in Control and Information Sciences - Advances in Nonlinear Parameter Optimization, Springer-Verlag, 1982. [STRANG80] G. Strang, Linear Algebra and Its Applications, 2nd Ed., Academic Press, 1980. [TAY79] R.H. Taylor, "Planning and Execution of Straight Line Manipulator Trajectories", IEEE Computer Society, Tutorial on Robotics, 1983, pp. 152-164. [THAKOR86] V.N. Thakor and M. McNeela, "Application of Dynamic Programming to Robot Kinematics", IEEE Conference on Robotics and Automation, 1986, pp.679-683. [VEITS86] V.K. Veitschegger and C.H. Wu, "Robot Accuracy Analysis Based on Kinematics", IEEE Journal of Robotics and Automation, Vol. RA-2, No. 3, September 1986, pp.171-179. [WAMPLER86] C.W. Wampler TJ, "Manipulator Inverse Kinematic Solutions Based on Vector Formulations and Damped Least-Squares Methods", IEEE Transactions on Systems, Man, and Cybernetics, Vol. SMC-16, No. 1, January/Febuary 1986, pp.93-101. [WAMPLER85] C.W. Wampler II, Computer Methods in manipulator kinematics, dynamics, and control: A comparative study, Ph.D. dissertation, Mechanical Engineering Department, Stanford University, 1985. [WHIT84] D.E. Whitney, CA. Lozinski, and J.M. Rourke, "Industrial Robot Calibration Method and Results", Conference on Computers in Engineering, 1984, pp.92-100. [YANG84] D.C.H. Yang and T.W. Lee, "Heuristic Combinatorial Optimization in the Design of Manipulator Workspace", IEEE Transactions on Systems, Man, and Cybernetics, Vol. SMC-14, No. 4, July/August 1984, pp.571-580. i APPENDIX A An approximate relationship can be derived between the damping factor and the Jacobian required for stability in the case of the single link one degree of freedom revolute jointed manipulator. A n explicit relationship between the damping factor and the damped inverse Jacobian exist in the one dimensional case. For stability in the manipulator, the calculated joint corrections must be less than the actual joint corrections required to move the end effector to the target. Two difficult cases exist in the iterative solution for the joint values. First is the movement from a good region to a singular point. Second is the movement from a good region to an unattainable position. For the movement from a good region to a singular point, the modified damped Jacobian must be less than the ratio required to move the arm past the singularity to an alternate solution. The equation for the modified damped inverse Jacobian is -sinf?! sin 2©, + a 2 Where - s i n 6, is the Jacobian value at the starting point. The exact inverse Jacobian is equal to the change in joint value to reach the singularity over the change in position. The change in joint value is equal to the present joint value which is approximately equal to the sine of the present joint value since the joint value is zero at the singularity. The change in position is equal to the distance error between the current position and the target, V . Near the singularity, 102 / 103 -sin0, 90 < — sin 20, + a2 v The joint corrections required to reach the singularity, 90 is approximately equal to s i n 0,. Therefore, the damping factor is equal to o 2 ^ v - sin2©. In the case where the target is out of reach, the joint corrections from the modified damped Jacobian must be less than that required to move the arm to the singular point -without overshooting. The joint corrections from the modified damped Jacobian for the distance error should be less or equal to the joint corrections from the Jacobian for the position change to the singular point. J T v £ J " 1 ( 1 - cosff,) sin20.,v < ( sin 20, + a 2) -(1 - cos0,) a 2 £ (1 - cos0,) - 1-sin 20,«v - sin 20, a 2 £ (1+COS0,) «v - s i n 2 0 . The damping factor is still proportional to the distance error but the damping coefficient must be greater when the target is out of reach to ensure stability. j APPENDIX B Many learning algorithms were suggested by Huscroft[HUS84] to adaptively approximate a target function with a multinomial given only the knowledge of the inputs and outputs of the target function. The appoximation function is a sum of the product of the input ariables. The approximation function is of the form f = w T«p Where p is a vector consisting of the products of the workspace variables and w is a vector of coefficients, n approximating the inverse kinematics functions, the input variables are the position and orientation values. If (z 0 , z, , z 2) were a set of workspace variables, one possible set of terms would be ( z 0 , z, , z 2 , Z o Z 1 , Z Q Z 2 , Z 1 Z 2 / Z o Z , Z 2) • The coefficients of the multinomial are adjusted according to the error in approximating the target function. With learning identification, one of the learning algorithms suggested by Huscroft, the coefficient corrections are: Aw •p T Af = P T ' P A scaling factor can be used to reduce the weighting of the correction for each point. Af = u«Aw«p T The input vector for a sample point can be transform to other coordinate systems to satisfy different criterions of minimization. The method works well with well behaved functions and with a low number of 104 / 105 input variables. Such is the case with the forward kinematics equations for simple manipulators. In the one dimensional case, many iterations of the learning identification algorithm results in a minimax approximation of the target function. A disadvantage of the method is the limitations of using multinomials as the approximation function for the inverse kinematics functions. Multinomials do not represent functions with singularities well. For a six degree of freedom manipulator, at least six independent variables are required in the approximation function. The complexity of the approximation function, in terms of the number of coefficients, grows exponentially with the order of the approximation. Partitioning can be used to reduce the order, but some stability is sacrifice in the singular regions as shown in the thesis. Only the high manipulability region of the workspace can be adequately approximated with a multinomial. APPENDIX C The position of the end effector can be represented using Cartesian, cylindrical or spherical coordinates. The kinematics control algorithm accepts changes in Cartesian coordinates. Thus if other coordinate systems are used, the position must be transformed to the Cartesian coordinate system. For the transformation from cylindrical coordinate, ( z, r, 6 ), to Cartesian coordinates, ( x, y, z ), the position vector is equal to rcost? rsinf) z For the transformation from spherical coordinates, ( 6, <f>, r ), to Cartesian coordinates rcost?sin0 rsinflsint/) rCOS<f> The changes in orientation is represented by the partial angular velocity vector in the kinematics control algorithm. An approximation can be obtained when the orientation is given as the coordinate frame of the end effector. For orientation given in Euler angles ( $, 6, \j/ ), the direction cosine matrix transforming the base to the end effector coordinate frame is 106 / 107 cos0cost?cos»// -sin^sini// -cos^cosdsin^ -sin^cos^ sin0cost?cos^ +cos0siru// -sin0cost?sini/> + 0 0 5 0 0 0 5 ^ -sint?cosi// sinflsin^ cos0sint9 sin0sint? cost? The transformation from roll, pitch, yaw ( 0, 6, ^ ), to a direction cosine matrix is COS0COS0 sin0cost? -sine? c c ^ s i n f l s i n ^ -sin0cos\r' sin0sine?sini/> +cos0cosi// cosflsin^ cos0sint?cos\r> +sin0sini/> sin0sin6,cos\/' -cos0sini/> cost?cos\// APPENDIX D Some simulations were performed with kinematics parameters for RSI Robotics Systems International's RT3 Excaliber arm and the PUMA 560 arm. The test points were entered as starting and target joint angles in radians. The test points consist of the following difficult movements 1. high manipulability to singular elbow and wrist 2. high manipulability to singular elbow 3. high manipulability to singular wrist The movements in all joints were large. Therefore, overshooting in the elbow joint occurred. The elbow joint was locked in case 1 and 2 for the Puma arm. 108 / 109 1 2 3 4 5 B 7 B Iteration Iteration Fig. D.l Convergence to a singular elbow and wrist, from joint vector (-1,1,-1,1,1,0) to (0,0,-1.57,0,0,0) for the RT3 arm i / 110 1 2 3 4 5 6 7 6 Iteration Fig. D.2 Convergence to a singular elbow, from joint vector (-1,1,-1,1,3,0) to (0,0,-1.57,1,2,0) for the RT3 arm / 111 co 10 c_ o c 4-3 to : Q 1e-005—1 i i i i i i i i i I i i i i i i i i i I i i i i i i i « i I i i i i i i i i i 1 2 3 4 5 Iteration 1 2 3 4 5 Iteration Fig. D.3 Convergence to a singular wrist, from joint vector (-1,1,-1,1,1,0) to (-2,0.5,-1,1,0,0) for the RT3 arm Fig. D.4 Convergence to a singular elbow and wrist from joint vector (-1,1,-1,1,1,0) to (0,0,-1.57,0,0,0) for the Puma 560 arm i / 113 Fig. D.5 Convergence to a singular elbow, from joint vector (-1 1-113 0) (0,0,-1.57,1,2,0) for the Puma 560 arm Fig. D.6 Convergence to a singular wrist, from joint vector (-1,1,-1,1,1,0) to (-2,0.5,-1,0,0) for the Puma 560 arm
- Library Home /
- Search Collections /
- Open Collections /
- Browse Collections /
- UBC Theses and Dissertations /
- An iterative general inverse kinematics solution with...
Open Collections
UBC Theses and Dissertations
Featured Collection
UBC Theses and Dissertations
An iterative general inverse kinematics solution with variable damping Chan, Stephen K. C. 1987
pdf
Page Metadata
Item Metadata
Title | An iterative general inverse kinematics solution with variable damping |
Creator |
Chan, Stephen K. C. |
Publisher | University of British Columbia |
Date Issued | 1987 |
Description | Currently, there is much interest in the field of robotics in researching methods of obtaining inverse kinematics solutions for arbitrary manipulators. Simple closed-form inverse kinematics equations can be obtained for a few joint configurations using geometric methods. However, there exist many manipulators which were not originally designed for kinematic control which do not have simple closed-form inverse kinematics equations. An efficient and stable iterative method is investigated in this thesis which solves the general inverse kinematics problem without detailed analysis of the manipulator's structure. The proposed iterative inverse kinematics algorithm combines a calibration procedure to estimate the manipulator's Denavit-Hartenberg parameters with an iterative method using the Jacobian and damped joint corrections. The kinematics control algorithm parameters are selected with a computer graphics simulation of the manipulator. The proposed inverse kinematics algorithm is tested with a simulation of an industrial manipulator arm which does not have a closed-form solution, RSI Robotic Systems International's Kodiak arm, and exhibits stability in all regions of operation and fast convergence over most regions of operation. |
Subject |
Machinery, Kinematics of Robotics |
Genre |
Thesis/Dissertation |
Type |
Text |
Language | eng |
Date Available | 2010-07-20 |
Provider | Vancouver : University of British Columbia Library |
Rights | For non-commercial purposes only, such as research, private study and education. Additional conditions apply, see Terms of Use https://open.library.ubc.ca/terms_of_use. |
DOI | 10.14288/1.0065545 |
URI | http://hdl.handle.net/2429/26684 |
Degree |
Master of Applied Science - MASc |
Program |
Electrical and Computer Engineering |
Affiliation |
Applied Science, Faculty of Electrical and Computer Engineering, Department of |
Degree Grantor | University of British Columbia |
Campus |
UBCV |
Scholarly Level | Graduate |
AggregatedSourceRepository | DSpace |
Download
- Media
- 831-UBC_1987_A7 C42_6.pdf [ 4.84MB ]
- Metadata
- JSON: 831-1.0065545.json
- JSON-LD: 831-1.0065545-ld.json
- RDF/XML (Pretty): 831-1.0065545-rdf.xml
- RDF/JSON: 831-1.0065545-rdf.json
- Turtle: 831-1.0065545-turtle.txt
- N-Triples: 831-1.0065545-rdf-ntriples.txt
- Original Record: 831-1.0065545-source.json
- Full Text
- 831-1.0065545-fulltext.txt
- Citation
- 831-1.0065545.ris
Full Text
Cite
Citation Scheme:
Usage Statistics
Share
Embed
Customize your widget with the following options, then copy and paste the code below into the HTML
of your page to embed this item in your website.
<div id="ubcOpenCollectionsWidgetDisplay">
<script id="ubcOpenCollectionsWidget"
src="{[{embed.src}]}"
data-item="{[{embed.item}]}"
data-collection="{[{embed.collection}]}"
data-metadata="{[{embed.showMetadata}]}"
data-width="{[{embed.width}]}"
async >
</script>
</div>
Our image viewer uses the IIIF 2.0 standard.
To load this item in other compatible viewers, use this url:
http://iiif.library.ubc.ca/presentation/dsp.831.1-0065545/manifest