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 :

- 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 :

J(q)

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.

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

(11

31

3-

the given algorithm can be used also to carry out the inverse
kinematic position solution of redundant robots, see
appendix.

Decomoosltlon of the

In this paper, for simplicity, we will consider only regular

positions, i.e the rank of J is equal to m.

In this case , J can be written as the product of two full

rank matrices [14] :
J=JmC
where :
Jm : regular (m x m) submatrix of J.
C : (m x m) matrix,

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. The first m joints will be
called primary joints and the other joints will be called
secondary joints.

jk

and C' and then use (8) and (9).

which gives:

From (3) it can be proved that [ 141 :

-1

JA

Using the pseudo inverse relation of a partitioned matrix

[16], C+ can be written as :
t
-1 t
h~1(In-m+C
1Ci) C1

Without loss of generality, we will assume that Jm is the

(m x m) left submatrix of J, in fact for sake of numerical

with ck=+.,

- Derlvatlon of the Dseudo Inverse solutIon

(5)

,1 : ( m x m ) identity matrix
jk :the kth column of J.
C1 : (m x n-m) matrix

where Oi,j is the (i x j) null matrix.

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.
The optimization starts by getting.the particular solution
-1
qp = J, X ; and then we look for the solution with minimum

The physical meaning of this decomposition is :

-1 .
q p = JmX

norm.

gives a particular solution of (1) using primary joints.

q = c+qp

(9)

,
since Jm is regular then qp = [ 1

: Cl] q

(13)

the secondary joints allow to get q satisfying the

minimum norm criteria
Remarks:
1- If the robot is in a singular position all the resul!s
given in sections 4, 5, and 6 can also be applied, only X
should be replaced by X a the nearest controllable vector to

with

qm = the first m components of q

qc

X [14].

= the

last (n-m) components of q

Using (14) and provided that qp has been determined, we

2- 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.

38

Let

For a pseudo inverse solution, 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,dlP

The matrix (In-m

Thus (20) becomes :

=o

+ C1 C1)

(16)
is positive definite SO, the
which gives :

optimum solution of qc is :

4,

t
t
(1n-m + C, c 1j1
(C+ : -1n-m) (Q Z)

(22)

Thus, the complete solution can be obtained as follows:

1 - Calculate qp using (8),

Thus the complete solution is given by the following steps:

2- Calculate qc by the use of (22),

3- Calculate qm by the use of (14).

1- Calculate qp using (8), a direct solution without
inverting explicitly Jm is in general more efficient.

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.

2- Calculate qc by the use of (17)

3- Calculate qm by the use of (14).

This solution is direct and general, the calculation of J+

is not to be realized explicitly.

The symbolic calculation of the proposed solution is

very efficient from the number of operations point of view [14].
If the degree of redundancy (m-n) is equal to one, the relation
t
-1
( In-m + C1 C1) will be just a scalar.

s - Use 0f

others crlteria

In this section we will study different criteria, which have

been proposed in the literature to verify that the proposed
method can be adapted for them .

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),(6)

into (2) we get :

6.1 Increasing the Manipulability :

The concept of manipulability mesure has been defined
by Yoshikama [18]. This is a measure of manipulating ability
of robotic mechanisms in positionning and orienting the end
effectors. 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,
increasing w will increase the dexterity of the system and will
assure avoiding "avoidable" singularities.

(19)

This criterion can be maximised by the use of the

homogeneous solution. 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.

using (11) and (19), Equation (18) can be rewritten as:

Cl

On-m,m

I-[

cl ](In.,+C1C1) t
-In - rn

-1 CIJmX
t -1

6.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

If the degree of redundancy (n-m) is one, a simpler

solution can be derived. As un example, for a three joint
planar robot the evolution of the criterion (27) as function of
qc, can be represented for a given position as shown in

physical limitations [9,15,21]. In the following we will present

two methods to optimize this criterion.
criterion
If qi is the center position of the ith joint, and the
maximum one sided excursion is ei, then the criterion
function C to minimize can be taken as:

e=i=l
c[

(qi-"ql2
ei2

Figure l.
The solution must be on the intersection of
representing the functions (

(23)

ql; I'
I

two lines

) which is easy to get.

The homogenous solution can be used [9,15] as given

in section 6, 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, 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,
A q = q(k) -;i

solution qc

Thus: C = (Aq 4 z )t W (Aq + q )~

(24)
All vectors and matrices will be partitionned into two parts :

Fiaurel.
6.3 Welghted Pseudo Inverse :

C = (Aqm +cimz 1' Wm (mm+cimz +

(4c

+tic 7 )t

wc (A% .dc 2 1

The pseudo inverse may also be weighted to provide

an additionnal mechanism for realizing desired performance
characteristics. The generalized inverse J+w that
instantaneously minimizes the cost qt Wq is given as [19,20]:

Using (14) and differentiating with respect to qc we obtain :

t
t
(Wc + q w m C1) T 2Cic=C1 Wm (4,
t
,
where (Wc + C1 W

.si>7 ) z - W c AqCz

J+,

(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/

- 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 . In this case the

(26)

4 m=b-C1ilc

criterion to be minimized is a quadratic function of q. It can be

easily minimized, thus:

c r i m

The least-squares norm is used in many problems, not

because it is truly desired, but because it is tractable. The
ideal criterion to distribute joint variables equitably would be
to minimize [8,22] ;

C=$Wtj
partitionning W into four matrices :

'

q i - 'i I
fori=l, ...,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. Whitney [19] proposed an

alternative method for enforcing high and low priority of
hand variables through appropriate selection of weights.
Konstantinov, Markov, and Nenchev [21] used weights to
avoid joint limits.

matrix.

b) Minimax

qi(k+l) = qi(k) + qpi z + C1iq'c T

where C1 j is the i th row of C1.

So this problem represents linear criterion with linear

constraints which can be solved by linear programming
techniques such as the simplex method.

40

we can write (38) as :

l- = l-'

+ A(In - J'J)

where :
r ' = A J + ( X - J q)+B(q,q)
For the optimum value of qc, the derivative of (31) with

r ' is independant of Z
Let : rmid , the midpoint of joint torque,
= (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 - rmid )I

(411

The solution is given as :

If W is diagonal (33) becomes :

t
(34)
ilC=(wcc+ ~ w m m c 1 ) - 1 c ~ w m m i P
The complete solution can be obtained as previously using
(14).

Z = [A(In - 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.

where

r = ri+ r
r ' = A J+( X -J
r = A (In - 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,
the proposed method needs only the inversion of a (m-n x m-1
n) matrix, beside the inversion of .J
,

q) + B(q&
[(A (In - J+J)]+ ( rmid - r ' ) (43)

The computation of the previous equation by the use of

J+ will be very complicated.The calculation of (43) can be

achieved more efficiently, as follows :
Let

6.4 Mlnlmlzatlon of the joint torques

H = A ,I(
The homogeneous solution can be used also to place
the joint torques closest to the midpoint of joint torque limits.
In this section we present at first the classical solution, and
then a new solution which needs less calculations to carry
out.

- J+J)

From (3),(6),(19) we get :

By differentiating (1) we get :

X=Jq+Jq

(35)

H= UV
where

solving for q we obtain :

..

q =J+( X - J q ) + ( I n - J + J ) Z
The motor torques
model as [I]:

(36)

Amcc1

are calculated by the use of the dynamic

T = A q +B(q ,q)

-Act
t

, U is a (n x n-rn) matrix

-1

I( 1n-m + CI
~ 1 )C, : - (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, (n-m), so:

where :
A is the inertia matrix of the robot

H+ = V+U+,

B(q , 4) represents the centrifugal, Coriolis and

gravity torques.

t h u s HH+ = UV V+ U+ = UU+
(45)

From (36) and (37) we get :

r = AJ+( X

-J q)+ A(1n- J'J)

+ B(q A)

Calculating

(38)

41

"

by (45) is more efficient than by (43)

Co.

If the redundancy is of order 1, then U will be a vector

and the solution will be given as :

[la] T. Yoshikawa , "Manipulability of robotics mechanisms" - 28me

ISRR, TOyOkO,l982, pp.91-98.

[19] D.E. Whitney , "Resolved motion rate control of manipulators
and human Prostheses", IEEE Trans. Man. Machine Systems.
MMS10, pp.47-53., 1969
[20] D.E. Whitney , "The mathematics of coordinated control of
prosthetic arms and manipulators," ASME J. Dynamic Systems,
Meas., Control, 1972, pp.303-309.
[21] M.S. Konstantinov , M.D. Markov, D.N. Nenchev, "Kinematic
control of redundant manipulators",Proc. 11th Int. Symp. on
Industrial Robots, 1981, Tokyo, Japan, pp.561-568
[22] C. Chevallereau, "Commande de robots manipulateurs dans
I'espace cartesien", These de doctorat, Nantes, 1988, to appear.

r N = m d - r ' )

llUIl*
7

Concluslon

This paper presents a new procedure for the solution of

the inverse kinematics of redundant robots. The method has
the advantage of yielding efficient solutions from the number
of operations point of view. The null space solution is given
by a computational cost comparable to that of the pseudo
inverse solution. The method can be used to provide most of
the optimization criteria presented in the literature, in some of
them an exact solution has been presented. The method may
be used also to yield symbolic solution.

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, while
the c other joints being fixed, so we have :
qm=go(,qc)
(46)

References

A particular solution q corresponding to a given X is:

qp : qm calculed by (46)

[ l ] W. Khalil. E. Dombre, "ModBlisation et comrnande de robots

manipulateurs", Editions Hermbs, Paris 1988. to appear.
[2]J. Baillieul, "Kinematic programming alternatives for redundant
manipulators", Proc. IEEE Conf. on Robotics and Automation ,St
Louis March 25-28, 1985, pp. 722-728.
[3]J. Baillieul, J. Hollerbach , R. Brockett, "Programming and control
of kinematically redundant manipulators", Proc. of 23d Conf. on
decision and control, 1984.
[4] J.Y.S. Luh, Y.L.Gu, "Industrial robots with seven joints" , Proc.
IEEE Conf. on Robotics and Automation, 1985, pp. 1010-1015.
[5]C.A. Klein , "Use of redundancy in the design of robotic
Systems", 2Bme Inter. Symp. on Robotic Research", Toyoko 1984,
pp.58-65.
[6]A.A. Maciejewski, C.A. Klein, "Obstacle avoidance for
kinematically redundant manipulators in dynamically varying
environments", Int. J Robotics Research, Vol. 4, N0.3, Fall 1985 ,
pp.109-117.
[7] J. Baillieul, "Avoiding Obstacles and resolving kinematic
redundancy", Proc. IEEE Conf. on Robotics and Automation,
1986, pp.1698-1704.
[8] C.A. Klein, C. Huang, "Review of pseudo inverse control for use
with kinematically redundant manipulators", IEEE Trans. on Syst..
Man, and Cybernetics. Vol. SMC-13, 1983, pp. 245-250.
[g] A.LiBgeois , " Automatique supervisory control O t tne
configurationand behavior of multi-body mechanisms", IEEETrans.
on Syst., Man, and Cybernetics. Vol. SMC-7(12),1977, pp. 868871,
[1O]P.H. Chang , "A closed form solution for the control of
manipulators with kinematic redundancy", Proc. IEEE Conf. On
Robotics and Automation, San francisco, 1986, pp. 9-14.
[ll]T. Yoshikawa, "Analysis and control of robot manipulatorswith
redundancy". Robotics research : The First Inter. Symp., MIT press,
1983, pp. 735-748.
[12]J.M. Hollerbach, K.C. Suh, "Redundancy resolution of
manipulators through torque optimization", Proc. IEEE Conf. on
Robotics and Automation, 1985, pp.1016-1021.
[13]M. Benoit, M.Briot, H. Donnarel, A. Liegeois, M.A. Meyer, M.
Renaud, "Synthese de la commande dynamique d'un
t6lBopBrateur redondant", Revue RAIRO, no Mai 1975-J-2, pp. 89103.
[14]C. Chevallereau , W. Khalil , " Efficient method for the
calculation of the pseudo inverse kinematic problem", Proc. IEEE
Conf. on Robotics and Automation, Raleigh, 1987 , pp. 1842-1848.
(15lA. Fournier , "GBnBration de mouvements en robotique
application des inverses gBnBralisBes et des pseudos inverses" ,
These dUtat, Montpellier , 1980.
(16lT.L. Bouillon , P.L. Odell , "Generalized inverse matrices ", New
York Willey Intersciences , 1971.
[17] A. Ben-Israel, T.N.E .Greville , " Generalized inverses :Theory
and applications" , New York, 1980, Robert E. Krieger publishing

qc having the same value as for the previous

point.
For a redundant robot an infinite number of solution exists for
the same position in Cartesian space. Let qp and (qp + dq)
be two solutions :
(qp)
f(qp+dq)

=X
=X,

where f is the direct geometric model.

so

f(qp)

- 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 [ I, : C1 ( qp ) ] dq = 0
The linearized solution around qp can be represented by :

where dqc is arbitrary.

This solution looks like that given by (14), so all the methods
proposed to calculate dqc = qc z , which optimizes a desired
criterion can be applied.

42