12th IFToMM World Congress, Besançon, June 18-21, t [ω T ṗ T ] T 2 R 3,

Transcription

1 Inverse Kinematics of Functionally-Redundant Serial Manipulators: A Comparaison Study Luc Baron and Liguo Huo École Polytechnique, P.O. Box 679, succ. CV Montréal, Québec, Canada H3C 3A7 Abstract Kinematic redundancy of a pair of manipulator-task can be classified into intrinsic and functional redundancies. This paper compares two different approaches to solve the inverse kinematics of functionally-redundant serial manipulators. First, the augmented approach add a virtual joint to the manipulator in order to render the Jacobian matrice underdetermined and project a secondary task onto the null space of theaugmented Jacobian, while the twist-decomposition approach rather decompose the instantenous twist into two orthogonal subspaces of the tool frame. The numerical simulatution of an end-milling operation shown that the twist decomposition is more effective in termes of computation time and numerical robustness then the augmented approach. Keywords:Robotic, kinematics, redundancy-resolution, manipulators I. Introduction The inverse kinematics of serial robotic manipulators has indeeply been studied for decades at both displacement and velocity levels. It is well known that the former is highly nonlinear and deserve dedicated solution procedures, while the latter is linear and requires the inversion of the Jacobian matrix of the manipulator. For a general 6-degrees-offreedom (DOF) task, Many research works has reported algorithms (e.g., [], [], [3], [], [5], [6] and [7]) allowing to choose an optimal solution when the manipulator has more joints than the corresponding degrees-of-freedom (DOF) of the end-effector (EE). All of them use the generalized inverse of a redundant Jacobian matrix, namely J, together with the projection of secondary task onto the null space of J in order to choose the best solution among the infinitely many that exist. The concept of kinematic redundancy is also related to the definition of the task instead of being only an intrinsic property of the manipulator. Although this fact is still not well understood in practice, it has been recognized by several researchers. Samson [8] clearly presented that the redundancy depends on the task and may change with time. Siciliano [9] said that the manipulator can be functionally redundant when only a number of components of its operational space are concerned with a specific task, even if the Luc.Baron@polymtl.ca Liguo.Huo@polymtl.ca

2 tial ill-conditioning of the augmented J, and the additional computation cost required to solve this augmented J. Alternatively, Huo and Baron [] proposed the twist decomposition algorithms (TWA) to solve the functional redundancy. Instead of using the projection onto the null space of J, the TWA is based on the orthogonal decomposition of the required twist into two orthogonal subspaces in the instantaneous tool frame. The TWA has great difference with the task-decomposition approach proposed by Nakamura [] [3] on the theoretical base, although both of them consider the order of task priority. First, the TWA classifies the order of task priority in instantaneous tool frame instead of in the robot base frame. Second, the TWA is directly developed from the minimum-norm solution without considering the projection onto the null space of J, while the development of the task-decomposition approach is basing on the general equation using the null space of J. In this paper, the solutions for the two kinds of redundancy are reviewed, particularly the TWA for the functional redundancy, then the numerical analysis of the TWA is studied with application to end-milling. II. Background on Kinematic Redundancy Before formulating the problem, let us briefly recall the two basic sources of kinematic redundancy of a robotic manipulator with respect to a given task. A. Basic Definitions Let J denote, the joint space of a robotic manipulator having n +rigid bodies serially connected by n joints, either revolute R or prismatic P. The posture of the manipulator in J is given by the n-dimensional vector, namely θ, and hence, n =dim(j )=dim(θ). Moreover, let O denote, the operational space of the EE of the robotic manipulator resulting from the joint space J. Since any free-moving rigid body in space can have at most six degrees-of-freedom (DOFs), the dimension of O is also at most six, and hence, o =dim(o) 6. Furthermore, let T denote, the task space such as required by the functional mobility of the EE, independently of the manipulator s architecture and hence, t =dim(t ) 6. Now, let us recall the following three definitions: Definition : Intrinsic redundancy A serial manipulator is said to be intrinsically redundant when the dimension of the joint space J, denoted by n = dim(j ), is greater than the dimension of the resulting operational space O of the EE, denoted by o =dim(o) 6, i.e., when n>o. The degree of intrinsic redundancy of a serial manipulator, namely r I, is computed as Definition : Functional redundancy r I = n o. () This definition of functional redundancy of serial manipulator-task is expandable to other types of manipulators such as parallel and hybrid ones. A pair of serial manipulator-task is said to be functionally redundant when the dimension of the operational space O of the EE, denoted by o =dim(o) 6, is greater than the dimension of the task space T of the EE, denoted by t = dim(t ) 6, while the task space being totally included into the operation space of the manipulator, i.e., T O, and hence, o>t. The degree of functional redundancy of a pair of serial manipulator-task, namely r F, is computed as r F = o t. () Definition 3: Kinematic redundancy A pair of serial manipulator-task is said to be kinematically redundant when the dimension of the joint space J, denoted by n =dim(j ), is greater than the dimension of the task space T of the EE, denoted by t =dim(t ) 6, while the task space being totally included into the resulting operation space of the manipulator, i.e., T O, and hence, n>t. The degree of kinematic redundancy of a pair of serial manipulator-task, namely r K, is computed as r K = n t. (3) Upon substitution of eqs. () and () into eq.(3), it becomes apparent that the kinematic redundancy come from two sources, the intrinsic and functional redundancies, i.e., r K = r I + r F. () In the literature, most of the research works on redundancyresolution of serial manipulators suppose that r F =, and thus, study r K = r I. In this paper, the opposite case is studied, i.e., supposing r I =, and thus, study r K = r F. B. Problem Formulation The inverse kinematics of serial manipulators is usually based on the linear relationship between the EE velocity, called twist and denoted t, and the joint velocities, denoted θ, given by t = J θ, (5) with t and θ defined as t [ω T ṗ T ] T R 3, θ [ θ θ n ] T R n, (6) and the Jacobian matrix J being defined as [ ] A J, A, B R B 3 n, (7) where ω R 3 is the angular velocity vector of the EE, ṗ R 3 is the translation velocity vector of a point on the EE, while θ i is the velocity of joint i. It is noteworthy that t is not defined as a vector of R 6, but rather as a set of two vectors of R 3 casted into a column array, and hence, t R 3 R 6. This distinction is important and will be further used in section 3. For the sake of numerical computation,

3 we replace velocities by small displacements in eq.(5), i.e., t Δt and θ Δθ. When J is square: The resolved motion-rate method proposed by Whitney [] can be used to compute Δθ as Δθ = J Δt, (8) where it is apparent that J must be non-singular. When J has more rows than columns: The mobility of the EE is less than 6-DOF, because the manipulator has either less than six joints and/or these joints do not allow to produce the full EE mobility. In both cases, the Δθ can be computed from Δt as Δθ = J Δt, (9) where J is the left generalized inverse of J such that J (J T J) J T. () This right generalized inverse is usually not required, because there always exist a time-invariant frame in which the constrained mobility of the EE are rows of zeros in J. Thus, the user can directly eliminate these rows from J in order to render it square and full rank. For example, planar and spherical serial manipulators belong to this case. When J has less rows than columns: For intrinsically-redundant serial manipulators, J always has more columns than rows, and hence, eq.(5) becomes an underdetermined linear algebraic system having infinitely many solutions. In this case, Liégeois[] proposed to compute Δθ as Δθ = (J )Δt + ( J J)h, () }{{}}{{} minimum norm solution homogeneous solution where J is defined as the right generalized inverse of J such that J J T (JJ T ), () and h is an arbitrary vector of J allowing to satisfy a secondary task. The first term in the right-hand side (RHS) of eq.() is known as the minimum-norm solution of eq.(5), i.e., the Δθ M that minimizes Δθ among all the Δθ that are solutions of eq.(5). The second part of the RHS of eq.() is known as the homogeneous solution of eq.(5), i.e., the Δθ H that produce Δt =, i.e., no displacement of the EE. This joint displacement Δθ H is also known as the self-motion of the manipulator. It is symbolically computed as the projection of an arbitrary vector h onto the null space of J with the orthogonal projector ( J J). Equation () is used to solve the kinematic inversion of intrinsically-redundant manipulators by many researchers (e.g., [], [7]), including those who payed a special attention in avoiding the squaring of the condition number while solving eq.(). Sometimes, it exist a time-invariant frame in which the functional redundancy are rows of J. In this case, the eq.() can be used to solve the inverse kinematics with these rows previously eliminated from J. For example, an arc-welding task may require the 3-D positining of the endpoint of the welding electrod at a constant vertical orientation. In this case, the redundant task is the orientation around the electrod axis, i.e., the vertical axis, which is time-invariant in the base frame. Most of the times, it is not possible to find such a time-invariant frame to express the functional redundancy of a task. For the general arcwelding, the electod axis needs to undergo arbitrary orientations in space, and hence, the electrod axis is time-varying in base frame. Therefore, eq.() can not be used. Below, a twist-decomposition approach to solve this problem is compared with the augmented approach.. III. Twist-Decomposition Approach Before proposing a functional-redundancy resolution algorithm, let us briefly review the orthogonal decomposition of vectors and twists. A. Orthogonal-Decomposition of Vectors Decomposing any vector ( ) of R 3 into two orthogonal parts, [ ] M, the component lying on the subspace, M, and [ ] M, the component lying in the orthogonal subspace, M, using the projector M and an orthogonal complement of M, namely M, as follows: ( )=[ ] M +[ ] M = M( )+M ( ). (3) It is apparent from eq.(3), that M and M are related by M+M = and MM = O, where and O are the 3 3 identity and zero matrices, respectively. The orthogonal complement of M thus defined, M, is therefore unique, and hence, both M and M are projectors that verify the following properties: Symmetry: [M] T = M, [M ] T = M Idempotency: [M] = M, [M ] = M Rankness: rank(m)+rank(m )=3 Completness: M M = R 3 The projector M projects vectors of R 3 onto the subspace M, while the orthogonal projector M projects those vectors onto the orthogonal subspace M. These projectors are given for the four possible dimensions i of subspaces of R 3 as: P M i = L O, M i = O L P i =3 3-Dtask i = -Dtask i = -Dtask i = -Dtask, () 3

4 where the plane and line projectors, P and L, respectively, are defined as: P L, L ee T, (5) in which e is a unit vector along the line L and normal to the plane P. The null-projector O is the 3 3 zero matrix that projects any vector of R 3 onto the null-subspace O, while the identity-projector is the 3 3 identity matrix that projects any vector of R 3 onto itself. B. Orthogonal-Decomposition of Twists Any twist array ( ) of R 3 can also be decomposed into two orthogonal parts, [ ] T, the component lying on the task subspace, T, and [ ] T, the component lying on the orthogonal task subspace (also designated as the redundant subspace), T, using the twist projector T and an orthogonal complement of T, namely T, as follows: ( )=[ ] T +[ ] T = T( )+T ( ) (6) It is apparent from eq.(6), that T and T are projectors of twists that must verify all the properties of projectors reviewed before. However, twists are not vectors of R 6, and hence, projectors of twists cannot be defined as in eqs. () and (5), e.g., T tt T, T tt T, (7) but must rather be defined as block diagonal matrices of projectors of R 3, i.e., [ ] Mω O T, (8) O M v [ ] T Mω O T =, (9) O M v where M ω and M v are projectors of R 3 defined in eqs. () and (5) which allow the projection of the angular and translational velocity vectors, respectively. It is noteworthy that the matrices of eq.(7) do not verify the properties of projectors, and hence, cannot be used for orthogonal decomposition. Finally, eq.(6) becomes t = t T + t T = Tt +( T)t. () C. Functional-Redundancy Resolution For functionally-redundant serial manipulators, it is possible to decompose the twist of the EE into two orthogonal parts, one lying into task subspace and another one lying into the redundant subspace. Substituting eq.() into eq.(8) yields Δθ = (J T)Δt + J ( T)Jh, () }{{}}{{} task displacement redundant displacement where h is an arbitrary vector of J allowing to satisfy a secondary task. Vector h is often chosen as the gradient of an objective function to minimize[]. For example, in the case of avoidance of the joint-limits, the objective function z can be written as to maintain the manipulator as close as possible to the mid-joint position θ, i.e., z = (θ θ) T W T W(θ θ) with θ and W being defined as min θ, () θ (θ max + θ min ), W Diag(/(θ max θ min )). (3) Vector h is thus chosen as minus the gradient of z, i.e., h = z. () The first part of the RHS of eq.() is the joint displacement required by the task, while the second part is the joint displacement in the redundant subspace (or irrelevant to the task). Clearly, eq.() does not require the projection onto the null-space of J as most of the redundancy-resolution algorithms do, but rather requires an orthogonal projection based on the instantaneous geometry of the task to be accomplished. As shown in Algorithm, eq.() is used within a resolved-motion rate method. At lines -3, the joint position θ and the desired EE pose {p d, Q d } are first initialized, then the actual EE pose {p, Q} is computed with the direct kinematic model DK(θ). At lines -6, an EE displacement Δt is computed from the difference between the desired and actual EE poses. The vect( ) at line 6 is the function transforming a 3 3 rotation matrix into an axial vector as defined in [5] (page 3). At lines 7-8, the instantaneous orthogonal twist projector T is computed from DK(θ). At line 9, the orthogonal decomposition method is used to compute the corresponding joint displacement Δθ. Finally, the algorithm is stop whenever the norm of Δθ is smaller than a certain threshold ɛ. Algorithm : Twist-Decomposition Algorithm θ initial joint position; {p d, Q d } desired EE pose; 3 {p, Q} DK(θ) ΔQ Q T Q d 5 Δp [ p d p ] Qvect(ΔQ) 6 Δt Δp 7 DK(θ) [ e M] ω, f M v, J Mω O 8 T, O M v 9 Δθ J TΔt + J ( T)Jh if Δθ <ɛthen stop; else θ θ +Δθ, and go to 3.

5 joint a i b i α i Min. Max π/ π/ π/ π/ unit m m rad. rad. rad. TABLE I. DH parameters of the Fanuc M6iB IV. Numerical Example When performing the end-milling operations, the milling tool has a symmetric axis, around which the milling tool may be rotated without interfering with the task to be performed. This axis describes the geometry of the functional redundancy (or the redundant subspace of twists). The unit vector e denote the direction of the symmetric axis of the tool. The projection of ω along e is the irrelevant component of ω, while its projection onto the plane normal to e is the relevant component of ω. For a general end-milling task around the symmetric axis e, the twist projector is defined as T mill T mill [ ( ee T ) [ ee T ], (5) ]. (6) Now, substituting eqs. (5) and (6) into eq.() yields Δθ = J T mill Δt + J [ ee T Ah ], (7) where A is the upper part of J as defined in eq.(6). Equation (7) can be used as line 9 of Algorithm in order to solve the inverse kinematics of serial manipulators while performing a general end-milling task. A Fanuc M6iB serial robot is used to perform a sphere-edge milling task. Its Denavit-Hartenberg parameters and joint limits are described in Table I. The milling tool has a transformation meth. a. Itera. No. = b. p e <.; o e <. Period A Period B Period A Itera. No. Aug TWA TABLE II. Computation time of the augmented(aug.) and twist decomposition approaches(twa). (a) fixing the iteration number as one; (b) reaching the same accuracy. All the unit of the time are in second. matrix A tool as A tool = cos β sin β.785 sin β cosβ.5, (8) with β =.398. The EE must perform consecutively the trajectory Λ (T = 5sec.). The graphic simulator and translation path of the task are shown in Figure. The symmetric axis of the milling tool is always normal to the sphere surface along the trajectory Λ. The velocity of the tool is changing according the sine curve ( from π/ to π/ ) along each edge. That is to say, EE velocity is zero at each corner of the path, and is accelerated smoothly to maximum at the middle point of the edge, then the EE velocity is decelerated smoothly till zero at the next corner. The path planning is done with the help of CATIA V5 machining module. The secondary task h is chosen to avoid the joint-limits such that: h = W(θ θ ), (9) where W is a positive-definite weighting matrix as in eq.(3) and θ the mid-joint position of the robotic manipulator, i.e., W Diag( ), (3) θ [ ] T. (3) Figure (a) shows the joint positions to perform twice the trajectory Λ as computed by the resolved-motion rate method without considering the functional redundancy, i.e., using eq.(8) at line 9 of Algorithm. Apparently, without taking advantage of the axis of symmetry of the tool, the manipulator is unable to perform the trajectory Λ. Figure (b) shows the joint positions to perform twice the trajectory Λ as computed by the augmented approach, i.e., using eq.() at line 9 of Algorithm 3.. Apparently, joint is out of the joint limit starting from 3 second. On the other hand, excessive joint velocities appear at every turn. Figure (c) shows the joint positions for two consecutive turns as computed by TWA, i.e., using eq.() at line 9 of Algorithm. Apparently, the manipulator is able to perform multiple consecutive turns without exceeding the joint limits shown in Table I. For the sake of the comparing the efficiency, two methods are used to analyze the computation time. The first method fixes the iteration number of resolve-motion rate as one rather than until a threshold is reached, then two period of the computation for the whole trajectory are counted. Period A includes the computation time from line 3 to in Algorithm; period B only counts the computation time of eq.() or eq.() at line 9 in Algorithm. The results are listed in column a of Table II. Apparently, the TWA 5

6 is much faster than the augmented approach, and the computation of eq.() is almost three and half times faster than eq.(). The second method uses the reached accuracy as the threshold of stopping the iteration loop, i.e., the iteration stops only if the norm of the position error (in meter) and the orientation error (in radian) are less than. and., respectively. As the two approaches implement with almost same iteration number to reach the required accuracy, the TWA has only spent 75% time of the augmented approach in the period A. Clearly, the computation of the pseudo inverse of a non-square jacobian matrix in the augmented approach is much more complicated and requires more time than the inverse of a square jacobian matrix in the TWA. That is the reason which lags the computation of augmented approach. The results of this method are listed in the column b of Table II. [] Huo, L., and Baron, L., Kinematic inversion of functionallyredundant serial manipulators: application to arc-welding, Trans. of the Canadian Society for Mech. Eng., 5. [] Nakamura, Y., Hanafusa, H. and Yoshikawa, T., Task-priority based redundancy control of robot manipulators, Int. J. Robotics Research, Vol. 6, No., 987. [3] Nakamura, Y., Advanced robotics redundancy and optimization, Addison-Wesley Pub. Co., Massachusetts, 337 pages, 99. [] Whitney, D.E., Resolved motion rate control of manipulators and human prostheses, IEEE Trans. Man-Machine Syst., vol., no., pp. 7 53, 969. [5] Angeles, J., Fundamental of Robotic Mechanical Systems: Theory, Methods, and Algorithms, second edition, Springer-Verlag, New York, 5 pages, 3. V. Conclusions Two different approaches have been compared to solve the functional redundancy of a serial robotic manipulator, while performing an end-milling operation. Our numerical implementation shown that the twist decomposition approach is more effective than the non-redundant or augmented approaches in terms computation time and numerical robustness. VI. Acknowledgements The financial support from the Natural Sciences and Engineering Research Council of Canada under grant OGPIN- 368 and the research contract CDT-P37 are gracefully acknowledged. References [] Liegeois, A., Automatic supervisory control of the configuration and behavior of multibody mechanisms, IEEE Trans. on System, Man, Cybern., vol. SMC-7, no., pp. 5 5, 977. [] Siciliano, B., Solving manipulator redundancy with the augmented task space method using the constraint Jacobian transpose, IEEE Int. Conf. on Robotics and Automation, vol. 5, pp. 8, 99. [3] Yoshikawa, T., Basic optimization methods of redundant manipulators, Lab. Robotics and Aut., vol. 8, No., pp. 9 6, 996. [] Whitney, D.E., The mathematics of coordinated control of prosthetic arms and manipulator, ASME J. Dynamics Syst., Measur. and Cont., vol. 9, no., pp , 97. [5] Klein, C.A. and Huang, C.-H., Review of pseudoinverse control for use with kinematically redundant manipulators, IEEE Trans. on Systems, Man, and Cyb., vol. SMC-3, no. 3, pp. 5 5, 983. [6] Angeles, J., Habib, M. and Lopez-Cajun, C.S., Efficient Algorithms for the Kinematic Inversion of redundant Robot Manipulators, Int. J. of Robotics and Automation, vol., no. 3, pp. 6 5, 987. [7] Arenson, N., Angles, J. and Slutski, L., Redundancy-resolution algorithms for isotropic robots, Advances in Robot Kinematics: Analysis and Control, pp. 5 3, Salzburg, 998. [8] Samson, C., Le Borgne, M. and Espiau, B., Robot control: the task function approach, Oxford [Angleterre]: Clarendon Press, 36 pages, 99. [9] Sciavicco, L. and Siciliano, B., Modelling and control of robot manipulators, Springer, London, 377 pages,. [] Baron, L., A joint-limits avoidance strategy for arc-welding robots, Int. Conf. on Integrated Design and Manufacturing in Mech. Eng., Montreal, Canada, May 6-9,. 6

7 st turn nd turn θ θ 3 θ θ 5 θ (rad.) 6 θ 8 θ (a) Time (sec.) st turn nd turn θ 3 θ θ (rad.) 3 θ θ 5 θ θ (b) Time (sec.) st turn nd turn 3 θ 6 θ 3 θ θ (rad.) θ θ 5 θ (c) Time (sec.) Fig.. Joint space trajectories of two consecutive turns of trajectory Λ as computed by the resolved-motion rate method with different approaches: a) No redundant-resolution approach; b) Augmented approach c) Twist decomposition approach 7