A NEW METHOD FOR THE SOLUTION OF THE

INVERSE KINEMATICS OF REDUNDANT ROBOTS

CCHEVALLEREAU and W.KHALIL
Laboratoire d'Automatique de Nantes UA C.N.R.S 4.823
1 Rue de la noe
E.N.S.M. 44072 NANTES CEDEX
FRANCE

Abstract

where:
J : the jacobian matrix of dimension (m x n)
n : the number of degrees of freedom of the robot
m :the number of degrees of freedom of the task, the
number of redundancy is thus equal to n-m

This paper presents a new algorithm for the solution of
inverse kinematic problem of redundant robots. The method
has the advantage of generating efficient symbolic solution
from the number of operations point of view, it gives the null
space solution with a computational cost comparable to that
of the pseudo inverse solution. The given method can be
used to provide most of the optimization criteria used in the
literature, an exact solution is proposed for most of them.

The general solution of (1) can be given as :

q=J+X +(In-J+J)Z
where :

1

- Introduction

A kinematically redundant robot is a robot that has more
degrees of freedom than the number of task space
coordinates.
The extra degrees of freedom may be used to satisfy
many supplementary criteria [l],such as:
singularity avoidance [2,3,4],
obstacles avoidance [5,6,7],
keeping the joint variables within their physical limitations
[8,9],providing greater dexterity [10,11],placing the joint
torques closest to the midpoint of joint torque limits [3,12],
and minimization of kinetic energy [13].
Most of these criteria can be achieved by the use of the
pseudo inverse solution combined with the homogeneous
solution (null space solution).

V O(q) : the gradient of (D(q).
: a positive constant number if Q(q) has to be
a
maximized or a negatif number if O(q) has to
be minimized.

Several methods have been proposed to get J+
numerically [15,16,17]
but they need prohibitive number of
operations. Even with a simple expression for J+, its use in
(2) will lead to complicated calculations.
This paper presents a direct and symbolic solution for
(2)without calculating J+ explicitly.

General solut Ion of the klnemat ic inverse model
The relation between the joint velocities q a n d

Cartesian velocities X is given by :

X

=

J(q)

4

CH2555-1/88/oooO/0037$01.000 1988 IEEE

To optimize a function (D(q), then Z is to be taken as [9,15]:

z = aVO(q),

The aim of this paper is to extend the pseudo inverse
kinematic solution to take into account other criteria and such
that the number of operations be reasonable for on line
control applications.

-

The first term of (2)represents the solution of minimum norm
joint velocity vector ; the second term uses a projection
operator in the space of homogeneous solution of (1) and
therefore can be used for optimization purpose.

where:

In 1141 we have presented a method which permits to
calculate a symbolic solution for the pseudo inverse problem
with a reasonable number of operations. This solution yields
locally a minimum norm joint velocity.

2

J+ : pseudo inverse of J
I n : (n x n) identity matrix
2 : an arbitrary vector.

(11

31

gives a particular solution of (1) using primary joints. q = c+qp (9) . 4 In this case . . see appendix.e the rank of J is equal to m. which gives: From (3) it can be proved that [ 141 : -1 JA Using the pseudo inverse relation of a partitioned matrix [16]. The first m joints will be called primary joints and the other joints will be called secondary joints. we will assume that Jm is the (m x m) left submatrix of J.3- the given algorithm can be used also to carry out the inverse kinematic position solution of redundant robots. in fact for sake of numerical with ck=+. and 6 can also be applied.If the particular solution of the primary joint qp can be obtained by a closed form expression such as qp = g(X). can calculate qm for an arbitrary qc.the particular solution -1 qp = J. X . we 2. The vector q of (7) can be obtained by means of one of the following methods: (3) i) Find accuracy we choose the submatrix with the biggest determinant in absolute value. C : (m x m) matrix. we will consider only regular positions. only X should be replaced by X a the nearest controllable vector to with qm = the first m components of q qc X [14].If the robot is in a singular position all the resul!s given in sections 4. i. C+ can be written as : t -1 t h~1(In-m+C 1Ci) C1 Without loss of generality. J can be written as the product of two full rank matrices [14] : J=JmC where : Jm : regular (m x m) submatrix of J.. q p = JmX norm.Derlvatlon of the Dseudo Inverse solutIon (5) . 38 . C1 : (m x n-m) matrix where Oi. = the last (n-m) components of q Using (14) and provided that qp has been determined. for simplicity. The optimization starts by getting.j is the (i x j) null matrix. Decomoosltlon of the In this paper. Thus the complete solution is given as : This decomposition into two full rank matrices leads to the following pseudo inverse solution : ii) The calculation of q can be considered as an optimization problem. jk and C' and then use (8) and (9).1 : ( m x m ) identity matrix jk :the kth column of J. since Jm is regular then qp = [ 1 : Cl] q (13) the secondary joints allow to get q satisfying the minimum norm criteria Remarks: 1. 5. and then we look for the solution with minimum The physical meaning of this decomposition is : -1 .

We calculate symbolically det (JJt) and its derivatives with respect to the joint variables vector in order to get Z=aV (det (JJt) ) and we apply the algorithm given in section 5. 1. The manipulability of the robot is given by : From (10) we obtain: w =ddet (JJt) Since w is equal to zero at the singular positions. the which gives : optimum solution of qc is : 4. which have been proposed in the literature to verify that the proposed method can be adapted for them . the complete solution can be obtained as follows: 1 . Thus the complete solution is given by the following steps: 2. we have to minimize the Q be defined as : criterion : c = 11 (ill2 The derivative of (15) with respect to qc gives : t t c 1 Clilc+dlc-C. the relation t -1 ( In-m + C1 C1) will be just a scalar.+C1C1) t -In . using (11) and (19).(6) into (2) we get : 6.dlP The matrix (In-m t Thus (20) becomes : =o + C1 C1) (16) is positive definite SO. From (17) and (22) we conclude that the vector q which optimizes a desired criterion with a gradient method is obtained in almost the same number of operations as the solution of pseudo inverse problem.rn -1 CIJmX t -1 6. c 1j1 (C+ : -1n-m) (Q Z) - (22) Thus.2 Maximum Avallabillty of joints A realistic criterion for a redundant manipulator may be to search for a solution such that each variable be within its 39 .Calculate qp using (8).m I-[ cl ](In. This solution is direct and general.Calculate qm by the use of (14).Let For a pseudo inverse solution..1 Increasing the Manipulability : The concept of manipulability mesure has been defined by Yoshikama [18].Calculate qp using (8). a direct solution without inverting explicitly Jm is in general more efficient. = t t (1n-m + C. This is a measure of manipulating ability of robotic mechanisms in positionning and orienting the end effectors. D e r b m l m of the oseudo inverse c o m b b e d wit0 Substituting the expressions of J and J+ given in (3). Equation (18) can be rewritten as: Cl = [ On-m.Calculate qc by the use of (17) 3. If the degree of redundancy (m-n) is equal to one. The symbolic calculation of the proposed solution is very efficient from the number of operations point of view [14]. 2. (19) This criterion can be maximised by the use of the homogeneous solution.Calculate qm by the use of (14). the calculation of J+ is not to be realized explicitly. increasing w will increase the dexterity of the system and will assure avoiding "avoidable" singularities.Calculate qc by the use of (22). 3. 5 - - s .Use 0f others crlteria In this section we will study different criteria.

. . The generalized inverse J+w that instantaneously minimizes the cost qt Wq is given as [19. The solution must be on the intersection of representing the functions ( (23) I ql. t .Wc(Aqc/T 11 PI (28) We propose to calculate the optimum value of q by a method Similar to method (ii) of section 3 . As un example. matrix. Markov.i solution qc Thus: C = (Aq 4 z )t W (Aq + q )~ (24) All vectors and matrices will be partitionned into two parts : Fiaurel. but because it is tractable.dc 2 1 The pseudo inverse may also be weighted to provide an additionnal mechanism for realizing desired performance characteristics. thus the next position will be given as : q(k+l) = q(k) + q T where z is the sampling time 1 let : W = diag b) e. In the following we will present two methods to optimize this criterion.21]. So this problem represents linear criterion with linear constraints which can be solved by linear programming techniques such as the simplex method. criterion If qi is the center position of the ith joint. (25) C1) is a (n-m x n-m) positive definite Thus q will be calculated as : t t ir c = (wc + Wm clyl { q w m [ ( ~ q mT/ . but an exact solution can be obtained by the use of a method similar to the second method of section 4 by replacing the criterion (15) by that of (23) Let q(k) be the current position of the joints. b) Minimax I I qi(k+l) = qi(k) + qpi z + C1iq'c T where C1 j is the i th row of C1.4 If the degree of redundancy (n-m) is one.20]: Using (14) and differentiating with respect to qc we obtain : t t (Wc + q w m C1) T 2Cic=C1 Wm (4. 6. and Nenchev [21] used weights to avoid joint limits. The homogenous solution can be used [9.3 Welghted Pseudo Inverse : C = (Aqm +cimz 1' Wm (mm+cimz + (4c +tic 7 )t wc (A% .. a simpler solution can be derived.15] as given in section 6.si>7 ) z .W c AqCz J+. not because it is truly desired. then the criterion function C to minimize can be taken as: n e=i=l c[ (qi-"ql2 ei2 Figure l. can be represented for a given position as shown in physical limitations [9. 40 . Whitney [19] proposed an alternative method for enforcing high and low priority of hand variables through appropriate selection of weights.n (27) ei Equation (14) shows that all the variables qi can be considered as linear functions of qc C=max( = W-l Jt (J W1 Jt)-l where W is a matrix of weights. thus: c r i m The least-squares norm is used in many problems. In this case the (26) 4 m=b-C1ilc criterion to be minimized is a quadratic function of q. and the maximum one sided excursion is ei.22] . I' I two lines ) which is easy to get. Konstantinov.. where (Wc + C1 W .15. C=$Wtj partitionning W into four matrices : ' q i . A q = q(k) -. It can be easily minimized. for a three joint planar robot the evolution of the criterion (27) as function of qc. The ideal criterion to distribute joint variables equitably would be to minimize [8.'i I fori=l.

q) = -Act t 1 . and then a new solution which needs less calculations to carry out. Z = [A(In .J'J) Z + B(q A) Calculating (38) 41 " by (45) is more efficient than by (43) .J+J)]+ ( rmid . the midpoint of joint torque.I( The homogeneous solution can be used also to place the joint torques closest to the midpoint of joint torque limits.= l-' + A(In . : .J'J) 2 where : r ' = A J + ( X . so: where : A is the inertia matrix of the robot H+ = V+U+.we can write (38) as : l.1 c ~ w m m i P The complete solution can be obtained as previously using (14). .J'J) " From (28) and (33) we see that the classical solution needs the inversion of a (m x m) matrix and a (n x n) matrix. In this section we present at first the classical solution.4 Mlnlmlzatlon of the joint torques H = A . B(q . where r = ri+ r r ' = A J+( X -J r = A (In .r ' ) (43) The computation of the previous equation by the use of J+ will be very complicated.J + J ) Z The motor torques model as [I]: (36) Amcc1 v are calculated by the use of the dynamic T = A q +B(q .(1n-m + ~1 c 1j1 1 V is a (n-m x n) matrix (37) We can prove that U and V are full rank. q) + B(q& [(A (In . t h u s HH+ = UV V+ U+ = UU+ (45) From (36) and (37) we get : r = AJ+( X -J q)+ A(1n. q =J+( X . the derivative of (31) with r ' is independant of Z Let : rmid . (n-m).J q ) + ( I n .q) For the optimum value of qc.J .(6). = (rmax + rmin)Q where: rmax the maximum value of joint torques rmin the minimum value of joint torques The criterion to be minimized is : e = 1) ' + A(1n(33) J+J)z . as follows : Let 6.(19) we get : By differentiating (1) we get : X=Jq+Jq (35) H= UV where solving for q we obtain : . U is a (n x n-rn) matrix -1 t t I( 1n-m + CI ~ 1 )C.The calculation of (43) can be achieved more efficiently.J q)+B(q.J+J) From (3). Coriolis and gravity torques. beside the inversion of .J+J)]+ ( rmid - (42) ') The joint torques will be calculated by the following expression: To minimize the kinematic energy. . the matrix W is to be taken as the inertia matrix of the robot. 4) represents the centrifugal.rmid )I (411 The solution is given as : If W is diagonal (33) becomes : t (34) ilC=(wcc+ ~ w m m c 1 ) .. the proposed method needs only the inversion of a (m-n x m-1 n) matrix.

Huang. Montpellier .58-65. A m e n dI X Let us suppose available a closed form solution for the inverse geometric model by the use of the first m joints. pp. Klein. Editions Hermbs. MIT press. Meyer. Renaud.1016-1021. Proc. IEEE Conf. 735-748. and Cybernetics.109-117. on Robotics and Automation. Dynamic Systems. and Cybernetics. Konstantinov . Hollerbach . 245-250. "ModBlisation et comrnande de robots manipulateurs". H. Odell . Symp. so all the methods proposed to calculate dqc = qc z . Control. Int. : C1 ( qp ) ] dq = 0 The linearized solution around qp can be represented by : where dqc is arbitrary. [4] J.Y. Yoshikawa.303-309. San francisco. Paris 1988. pp. The null space solution is given by a computational cost comparable to that of the pseudo inverse solution. Khalil .Gu. MMS10. Benoit. [5]C. T. Whitney .91-98. Chang . C. Fournier ." ASME J. 9-14.A.3. Machine Systems.C. E.. where f is the direct geometric model. Vol. Meas.N. Baillieul. Man. Symp. pp.Co. r N = m d . 868871. Hollerbach. Vol. 1010-1015. "Manipulability of robotics mechanisms" . [ll]T. [21] M. [14]C. Markov. [12]J. 1972. Chevallereau.1977. pp. 42 . pp. " Automatique supervisory control O t tne configurationand behavior of multi-body mechanisms". (15lA. pp. while the c other joints being fixed. "Industrial robots with seven joints" . Tokyo.Proc.L. [13]M.E. Let qp and (qp + dq) be two solutions : (qp) f(qp+dq) f =X =X. on Syst.28me ISRR. "Obstacle avoidance for kinematically redundant manipulators in dynamically varying environments". IEEE Conf.S. to appear. A.S. "Kinematic programming alternatives for redundant manipulators".Briot.L. "Synthese de la commande dynamique d'un t6lBopBrateur redondant". " Efficient method for the calculation of the pseudo inverse kinematic problem". These dUtat. [6]A. 2Bme Inter. 1985. (16lT. Toyoko 1984. [2]J. Proc. D. on decision and control. IEEE Conf. 1983. Fall 1985 . Symp. pp. IEEE Trans. J. [8] C. which optimizes a desired criterion can be applied. of 23d Conf. 1983. [19] D.f(qP+dq)=O Using a first order linearization we obtain : J ( q P 1 dq = o (47) Equation (3) permits to write J m ( q P ) [ l m : CI ( q ~ ) l d q = O AS Jm ( qp ) is regular.. so f(qp) . on Syst. 1988. on Robotics and Automation. so [ I.H.1698-1704. On Robotics and Automation. Chevallereau . on Robotics and Automation.qc) (46) References A particular solution q corresponding to a given X is: qp : qm calculed by (46) [ l ] W. 1981. IEEE Trans. Man. "The mathematics of coordinated control of prosthetic arms and manipulators. Klein . P. [1O]P.Greville .St Louis March 25-28.47-53. For a redundant robot an infinite number of solution exists for the same position in Cartesian space. M. pp. "Commande de robots manipulateurs dans I'espace cartesien". J Robotics Research. 1986. no Mai 1975-J-2. [3]J. pp. on Industrial Robots. Donnarel. Yoshikawa . so we have : qm=go(. Nenchev. New York. 4. Klein. 11th Int.M. Whitney . Proc. pp. "Redundancy resolution of manipulators through torque optimization". IEEE Conf. "Avoiding Obstacles and resolving kinematic redundancy". N0.561-568 [22] C. K. M. "Review of pseudo inverse control for use with kinematically redundant manipulators". 1980. pp. SMC-13. IEEE Conf.E . Ben-Israel. Raleigh.E. Japan. IEEE Conf. Suh. C.A.N. Brockett. Krieger publishing qc having the same value as for the previous point. R.L. 1980. If the redundancy is of order 1. 89103. "Resolved motion rate control of manipulators and human Prostheses". in some of them an exact solution has been presented.LiBgeois . 1985. 1984. Nantes. 722-728. 1842-1848.A. pp. 1986. Khalil. [g] A. W. on Robotics and Automation . "Programming and control of kinematically redundant manipulators". Bouillon . pp. 1969 [20] D. pp. 1987 . " Generalized inverses :Theory and applications" .A. M. Revue RAIRO. The method can be used to provide most of the optimization criteria presented in the literature.l982. on Robotics and Automation. "Use of redundancy in the design of robotic Systems". [17] A. IEEETrans. Baillieul.. Proc. to appear. Robotics research : The First Inter. pp. These de doctorat. [7] J. SMC-7(12). The method has the advantage of yielding efficient solutions from the number of operations point of view. pp. New York Willey Intersciences .A. Luh. This solution looks like that given by (14). Vol. TOyOkO. "A closed form solution for the control of manipulators with kinematic redundancy". Proc. Man. Proc. The method may be used also to yield symbolic solution. Y. on Robotic Research". Robert E. M. 1985. Liegeois. "Analysis and control of robot manipulatorswith redundancy". "Generalized inverse matrices ". "GBnBration de mouvements en robotique application des inverses gBnBralisBes et des pseudos inverses" . Proc.. 1971. Baillieul. Maciejewski.. then U will be a vector and the solution will be given as : [la] T.r ' ) llUIl* 7 - Concluslon This paper presents a new procedure for the solution of the inverse kinematics of redundant robots.D. "Kinematic control of redundant manipulators". Dombre.