See discussions, stats, and author profiles for this publication at: https://www.researchgate.

net/publication/224729781

A Complete Generalized Solution to the Inverse Kinematics of Robots

Article  in  IEEE Journal on Robotics and Automation · April 1985

DOI: 10.1109/JRA.1985.1086995 · Source: IEEE Xplore

CITATIONS READS

265 2,905

3 authors, including:

Andrew A. Goldenberg
University of Toronto
322 PUBLICATIONS   6,466 CITATIONS   

SEE PROFILE

Some of the authors of this publication are also working on these related projects:

Mobile Robotics View project

Pediatric Surgical Robot View project

All content following this page was uploaded by Andrew A. Goldenberg on 03 February 2014.

The user has requested enhancement of the downloaded file.

14
www.DownloadPaper.ir IEEE JOURNAL OF ROBOTICS AND AUTOMATION, VOL. R A - 1 , NO. 1 , MARCH 1985

A Complete Generalized Solution to the

Inverse Kinematics of Robots
ANDREW A. GOLDENBERG, MEMBER, IEEE, B. BENHABIB, AND ROBERT G. FENTON

Abstract-The kinematic transformationbetween task space andjoint considers the kinematic model as a whole and determines all
configuration coordinates is nonlinear and configuration dependent. A joint variables using a rapidly converging iterative procedure.
solution to theinversekinematics is avector of joint configuration In a three-dimensional space a manipulator must have at
coordinates that corresponds to a set of task space coordinates. For a
class of robots closed form solutions always exist, but constraintson joint least six DOF in order to be able to attain any arbitrary end
displacements cannot be systematicallyincorporatedintheprocess of effector position and orientation. If the manipulator has six
obtaining a solution. An iterative solution is presented thatis suitable for DOF, a system of sixnonlinear equations has to be solved for
any class of robots having rotary or prismatic joints, with any arbitrary the six joint variables. However, if the manipulator has more
number of degrees of freedom, including both standard and kinematically than six DOF, the systemof six nonlinear equations is
redundant robots. The solution can be obtained subject to specified
constraints andbased on certainperformancecriteria.The solution is
underdetermined. For such cases an optimization procedure
based on a new rapidly convergent constrained nonlinear optimization may be used to determine the best set of solutions subject to a
algorithm which uses a modified Newton-Raphson technique for solving given objective function [4], [9].
a system nonlinear equations. The algorithm is illustrated using as an A number of iterative methods for the analysis of spatial
example a kinematically redundant robot. mechanisms presented in earlier works use matrix algebra to
formulate the kinematic relations 111, [24].
In literature the most commonalgorithms proposed to solve
I. INTRODUCTION
the nonlinear kinematic equations useNewton’smethods

FORADVANCEDCONTROLofrobot manipulators,
necessary capabilities are offline programming of the end
basedonsimultaneous successive linear interpolation of
nonlinear equations 121, 131. Since Newton-like algorithms
effector path and control in terms of Cartesian coordinates. are considered ‘local methods, which require an initial close
Although control of Cartesian trajectory of the end effector is a estimate to the exact solution, a proper step size control is
basic requirement for many industrial applications, most robot necessary to avoid divergence due to a “bad” initial estimate
manipulators lack this ability. ~41.
The “inverse kinematics” control of a robot manipulator This work consists of five’sections. Section presents a
requires the transformationofend effector Cartesian task general formulation of the inverse kinematics problem of n
space coordinates into corresponding joint configuration space DOF robot arms. A review ofsolutions of nonlinear equations
coordinates. The common approachfor solving this problem is is presented in Section 111. An efficient and general iterative
to obtain a closed-form solution to the inverse transformation technique is discussed in Section IV for both kinematically
[l]. However, only certain classes of robots (e.g., spherical determinate (standard) and redundant robots. The technique is
wrist manipulators, such as thePUMA560 robot) allow basedonamodifiedNewton-Raphsonmethod for solving
closed-form inverse kinematics solutions. The problem be- nonlinear equations. A numerical example of a seven degree
comes more critical for kinematically redundant robots, for of freedom robot illustrates the method in Section V.
which the number of degrees of freedom (DOF) exceeds the
11. PROBLEM FORMULATION
required six coordinates necessary to attain arbitrary locations
in the three-dimensional work space [2], [4]. The motion of anarticulated arm (robot) can be analysed by
A second approach for solving the inverse transformation assigning certain coordinate frames to each link [15], in order
problemof n DOF robots is basedontheuseof iterative to obtaina transformation that relates joint to task space
procedures for solving a system of nonlinear equations [3], coordinates. The task space coordinates are the position and
[6]. Alternatively, the kinematicsmodel of the orientation of the end effector with respect to a reference
manipulatorcan be divided into subsystemssuch that an (base) frame. The relative position and orientation of two link
iterative procedure can be applied to determine some of the coordinate frames are expressed in terms of transformation
joint variables, while the other variables are obtainedbya matrices A i R 4 x 4 13 defined as follows
closed-form solution [8]. Themethodpresented herein

ManuscriptreceivedSeptember 26, 1984; revisedFebruary1985. This

work was supportedin part by the National Science and Engineering Research
Council under Grant A-4664.
The authorsare with the Robotics and Automation Laboratory, Department
of Mechanical Engineering, University of Toronto, Toronto, ON, Canada. where ai, ai, di, Oi are the link length, twist, distance, and joint

.OO 0 1985 IEEE

Authorized licensd use limted to: IE Xplore. Downlade on May 10,2 at 18:570 UTC from IE Xplore. Restricon aply.
GOLDENBERG et
www.DownloadPaper.ir
GENERALIZED SOLUTION TO THE INVERSE KINEMATICS OF ROBOTS 15

variable, respectively. For simplicityweassumethatonly r,= 1/2(a"

rotary joints are considered. Using ( l ) ,the transformation T,
representing the position and orientation of the end effector re= 1/2(na a t - n t
with respect to the base frame is
r$=1/2(oa n f - o f n3 (7)
Thesolution to the target transformation is obtained
when the actual end effector transformation Tnais coincident
where n , are the orientation vectors, p is the position with thetarget transformation T n f Such
. a solution exists if and
vector, and is the configuration space vector consisting of only if i.e.
joint variables Oi, member R".
The inverse kinematics problem is the determination of a r 0
vector which corresponds to a given end effector target
Thealgorithmpresented herein generates suchthat ( 8 )
transformation T k . The approach, first introduced in [ 5 ] , is
holds with (actual configuration) being the initial estimate
based on a residual vector definition r member ' R 6 which
of
represents the residual position and orientation between Tnf
andtheactual (present) endeffectortransformation Tna. III. NUMERICAL METHODS FOR NONLINEAR
Clearly r r ( q ) is defined as follows KINEMATIC EQUATIONS
r=@Jyrzr,ror$) (3) A . Determination of the Jacobian
where (rxryrz)and (r,ror,) represent the residual position and The most common methods for solving nonlinear equations
the residual orientation, respectively. The elements of residual approximate the nonlinear system by a linear one such as (8),
position are defined as [16] then
solve the problem iteratively [17], [18], [19]. For
example, the Newton-Raphson method 181 takes into account
rx=na @ ' - p a ) only the first order terms of the Taylor series expansion of
r(q) in (3). A solution for this system of equations is
ry=oa ( p t - p ? determinedusing,in an iterative manner, the following
approximation 121
q(k+ q(k) 6Ck)
(9)
where p' and p a are the position vector of the target and the
actual frames with respectto thebase frame, respectively, and wheresolvesthe linear systemand
(naoaau)are the orientation unit vectors of the actualend
effector frame with respect to the base frame.
For the elements of the residual orientation vector one can i=
useany suitable set of rotationangleswith a predefined
sequence of rotations. For example, for Euler anglesthe where Jji is defined as
elements are defined as J..( arj/aqi],=,(k), i = l , n; j = l , 6. 1)
(1
',=a tan at), a')] r,=r,+ 180"
Thefunction rj is continuously differentiable and Jji is
nonsingular in the neighborhood of the solution
re=a tan 2[((na cos r,+ sin r,),
The manipulator Jacobianmatrix, which is requiredto solve
a91 (lo), has to be determinedanalytically or numerically.
Analytical expressions for the Jacobian matrix corresponding
r $ = a tan 2[(-(na n f ) sin r,+(oa n') cos r,), to the residual vector r(q) given by (4)and (7) are shown in
Appendix I 101. Since the computations are lengthy, the user
(na sin r,+ ot) cos r,)] may decide either to compute the Jacobian onlyafter every m
iterations, or approximateitusing the followingfunction
For yaw-pitch-roll angles the elements are' defined as values 191
' , = a tan nt), (na nt)] r,=r,+ 180" [d(k+ ( 4 [Dl (k) (12)
re=a tan 2[-(aa nt), where
((n" n') cos r,+(oa nt) sin r,)]

' $ = a tan 2[((na sin r,- cos r,),

(-(na sin r,+ cos r,)] andvectorwhich is chosen to beorthogonal to 6

is determined using the Gram-Schmidt orthogonaliza-

For a set of y z rotation axes the elements are tion process, so that

Authorized licensd use limted to: IE Xplore. Downlade on May 10,2 at 18:570 UTC from IE Xplore. Restricon aply.
 16
www.DownloadPaper.ir IEEE JOURNAL OF ROBOTICS AND AUTOMATION, VOL. RA-I, NO. 1 , MARCH 1985

[13](k)(6('))=0, l ~ k - i ~ n . Values of S R obtained from (19), corresponding to certain

6" values, can now be substituted into any objective function
Following the determination of the Jacobian matrix, the next
of the form
task is to invert it for solving (IO).
B. Inverse of the Jacobian min Z = f ( q )
Case I: For manipulator arms with n 6 DOF, the (6 andtheoptimumcanbedeterminedusing an (n 6)
n) Jacobian matrixis nonsquare, so that an inverse can be only dimensional optimization routine.
obtained using generalized inverses [17] as follows B. Constraints
G=[Jl+=([J1T[4)-1[Jl= (15) The system of nonlinear equations (8), r r ( q ) ,is always
The inverse J + is unique, minimizes the Euclideannorm of subject to constraints imposed on the solution such as
and satisfies the following relations [I71 (22)
JJ+ (164 where and are the lower and upper limits on the joint
variable displacements qi, i n.
JJf J+ J+ 16b) In general, for Newton-Raphson methods, when the gener-
alized inverse is used to obtain the inverse Jacobian matrix, no
J)T= (J+ 17a) control is exercised on the computed correction vector at
iteration step k. It is possible, that q(k+l)
determined from
JJ+ (JJ+)T= 17b) (9)doesnot satisfy (22) and the algorithmconverges to a
(nonfeasible) solution outside the permissible joint ranges of
Case For manipulator arms with n 6 DOF, the
the robot (outside the work space). Under such circumstances
Jacobianmatrix is squareanditcan be inverted if it is
there may betwo strategies to preventconvergence to a
nonsingular. Then J + J - (the ordinary inverse) in (15).
nonfeasible solution: 1) Check the solution q ( k + lagainst
) the
Case 3: For kinematicallyredundant robots with n 6
limits and correct those variable values to their nearest limit
DOF, the (6 n) Jacobian matrix is nonsquare andits inverse
which are out-of-range, or 2) Reduce the step size of Newton-
canbeobtainedusingpseudoinverses [4]. Although, the
Raphson iteration, 6 ~ ~ ( ~if) al ljoint
, exceeds its limits. This
pseudoinverseof the (6 n) Jacobian matrix, n 6, can
strategy will be discussed in detail in Section IV-C.
easily be obtained, it can only beused if the objective function
For the modified Newton's method presented inSection IV-
is the minimum Euclidean norm. For more general objective
A, for n 6 , joint limits on the free variables, are
functions a newtype of Jacobian inversion technique is
determinedfrom constraints equation (22) specified for all
presented in the next section.
joints prior to every algorithm step. Let qi@)be the current
IV. SOLUTION TO THE INVERSE KINEMATICS value of the ith joint variable. Then, using (19) and (22), the
A . The Modified Newton-Raphson Method for n 6 constraints become
DOF q ; ' - q ; ' k ' = ~ I. ~- < ~I . ( k ) ~ ~ i u = q i U - q i ( k ) (23)
The purpose of this method is to solve the nonlinear system
of equations (8) subject to arbitrary objective functions and
(optimization criteria). Since n 6 in (lo), the (6 n) [b](Q(6A)(q ( 6 R ) U . (24)
Jacobian matrix can be partitioned into a (6 6 ) and a (6
(n 6 ) ) matrix as in If the totalnumberof variables is n 7, the constraint
equations will simplify as follows,
(r(q'Q)) [J"]' Q ( 6 R ) ( k ) [J"] ( k ) ( b A ) ( k ) 0 (1 8)
(6")L 5 (&A)
where [ J R ]is the (6 6) reduced Jacobian, evaluated at
[J"] is the (6 (n 6)) matrix corresponding to where
obtainedbyexcluding (n 6) columnsfrom [4,(liR)ck) max [(aA)[,(aj (6iR)?/bj; 1, 61
(qR)@+ (qR)(@ are the six joint correction variables, and
(aA)@) ( q A ) ( k + l ) ( q A ) ( @are the (n 6) free joint ( ~ 5 ~ ) ~ = r n[(dA), i n (aj-(SjR)")/bj;j = 1, 61 (26)
correction variables.
The only condition in choosing the free variables 6" is to where aj and bj are the elements of ( a )and ( b ) ,respectively.
guarantee the invertibility of the reduced Jacobian matrix J R ] C. Step Size Control
which is necessary to obtain the following equations,
The classical Newton-Raphson iteration often fails to
(6R, [bI@A ) (19) converge whenthe initial estimate is not sufficientlyclose,
in the Euclidean sense, to the solution of the system (8). A
where
number of useful modifications havebeendeveloped to
[JR] l(r) Newton's method, which provide reliable convergence, even
when aclose initial estimate is not available 171, 181, 191. In
[b] (20)
[JR] '[JA]. 181, Eq. (9) is replaced by the expression

Authorized licensd use limted to: IE Xplore. Downlade on May 10,2 at 18:570 UTC from IE Xplore. Restricon aply.
GOLDENBERG et ul.: GENERALIZED SOLUTION TO THE INVERSE KINEMATICS OF ROBOTS
www.DownloadPaper.ir 17

(27) G ( @ + l ) ) from (29) is compared with a specified threshold

value E for convergence check. The algorithm consists of the
where X(") is a positive number chosen to satisfy the following following step-by-step procedure.
inequality,
Step I : Determine the Jacobianmatrix [Jl(see ( 1 1 ) )
G(q(k+l))<G(q(k)) corresponding to the residual(28)vector r (see (3))at q(k).
where Step 2: Evaluate the full Newton-Raphson correction 6NR(k)
as follows.
Case I (for arbitrary n DOF):
j= I a) using generalized inverses (see (15)-(17)) obtain
Equation (28) is expected to hold [18], since if [Jl
(k) is
[Jl
b)determine the correction vector BNR(Q (see
nonsingular, then
(10)).
G(q("

unless q(k)is already a solution of (8).

1
X=O
2G(q("))<0
Case 2 (for n 6 DOF):
a) partition the Jacobian matrix into [ J R ]and [ P I (see
18))
b) determine the constants and [b] (see (20))
A different modification to the classical Newton-Raphson c)determine the feasible domainof the free variables
iteration, which is known as the Levenberg-Marquardt itera- (see (23)-(26))
tion [20], [21],replaces (9) by d) determine an optimal solution for agiven
(k+ q objective function (see (21))
(31 ) e) determine the optimal correction vector tSNR@) (see
where solves the linear system (19)).
([JJT[~+y(k)[ll)q(k)+[JJT(r(q(k)))=O (32) Step 3: Evaluate the gradient ( g ) @ )as follows [22],
where is a positive number.
A common strategy to both methods is to retain direction,
but to restrict step length if necessary. If (28) is satisfied, the
full Newton-Raphson correction 6NR(k)is maintainedby Step 4: Checkif the solution is converging to a local
setting X(") 1 in (27) and d k ) 0 in (32). minimum using
Asmentionedin Section IV-B, step size control canbe
effectively used to prevent nonfeasible solutions. If sucha G(q(k92tIlg'k'll
situation is encounteredduringthe iterative procedure, the
step size is restricted using a newalgorithmpresented in where t: is usually set to anoverestimate of the distance to
Section IV-D the solution [22].If (34)holds, i.e., there is convergence to
It is noticed that the iterative procedure of the Levenberg- a local minimum, then terminate the process, else proceed to
Marquardt method (see Appendix 11) [20] is a least squares the next step.
solution, and can not be easily applied for n 6 DOF robots Step Restrict the step size by the following procedure, if
if the modified Newton's method of Section IV-A is used. On necessary.
the other hand, the step size restriction procedure using (27), 1 ) If
as it will be explained inSection IV-D, is moresuitable for the
newalgorithmof Section IV-A, since the correction is SACk) (35)
restricted only after the full Newton-Raphson correction
6NR(k) is computed. and

D. Algorithm of the Iterative Procedure

The algorithm proposed inthis paper seeks a solution to then set
the problem defined by (8) subject to the constraints of (22).
This algorithm generalizes the algorithm presented earlier for (k)f 6NR,(k)
n 6 DOF (redundant) robots [ 6 ] . In Steps 1 and 2 the
manipulatorJacobianmatrix is computedand inverted to where A(") is the maximum allowable step size initially
obtain the full Newton-Raphson correction vector 6NR(k). The set to AC0) max (DSTEP, min [DMAX, p11g111); and p
user has the option to use either generalized inverses or the ~ ~ g ( " ) ~ ~ 2;
/ ~DSTEP
~ J g ( is
k )chosen
~ / to avoid computer
inversion procedure described in Section IV-A. In Steps 3-6 roundoff errors.
the step length, defined as may be restricted to prevent 2) If equality (36) holds and (35) does not, then
nonfeasible solutions. The constant in (27)is computed as 6(k)=A(k)g(k)/llg(k)ll (37)
a multiplier of the Newton-Raphson correction vector
that was determined in the earlier steps. In Step 7 the value of 3 ) If both inequalities (35) and (36) do not hold,

Authorized licensd use limted to: IE Xplore. Downlade on May 10,2 at 18:570 UTC from IE Xplore. Restricon aply.
18
www.DownloadPaper.ir IEEE JOURNAL OF ROBOTICS AND AUTOMATION, VOL. RA-I, NO. 1, MARCH 1985

6 (1 y)pg (k) y 6 N R (k) (38) TABLE r

KINEMATIC VARIABLES
such that the positive number y satisfies
Joint
=A(k). (39)
Maximum Velocity
Step 6: Verify whether both inequalities (23) and (28) hold (1 /s) 2 2 2 2 10 10
(check (28) only if Case 2 of algorithm Step 2 is used). Maximum Acceleration
10 10 10 25 25 25
1) If they do not hold, reduce A(" as follows and return to Lower Limit
Step 5, (rad)
Upper Limit
ACk) max
(40) DSTEP) (rad)
Mid-Range
where 0.0 0 (rad) 0 0 0 0 0 0 0
2) If they hold, then set
(9)
7
Step Verify the convergence to final solution as follows, A42 (AB;) (47)
i= I
(41)
where E
1) If (41) holds, then
0.0 is a threshold value supplied by the user.
q ( k + lis) the optimal solution of
M~ x.(e;-
7

i= I
eiM)2 (48)

(8)
2) If (41) doesnot hold, let k k 1 and return to where 7;is the motiontimeof ith joint, and BiM is the
algorithm Step 1. operational midrange value of thejoint angle of the ith joint.
The kinematic variables corresponding to each of the joints
The proofs of convergence of the algorithm described above
are shown in Table I. The optimal solution obtained for the
can be found in [18].
objective function (45) is
V. NUMERICAL EXAMPLE
A seven DOF, all rotary joints, twoelbowrobotwitha Oi(')=(O.3533, 0.2841, 0.9001, 0.0966, -0.8976,
maximumreach 3.0 is given. The link lengths (defined (49)
0.2368, 0.9067)
according to [lo]) areal 0, a2 1, a3 1, 1,
0, 0, 0. The residual vector r(q),expressed by (4) where
and (7), and the correspondingJacobianmatrix(using the
formulation given in Appendix I) are utilized in the algorithm
1 10-10 0.5
described in Section IV-D.
The robot is positioned initially at eico)corresponding to task
space T7(0),and it is required to relocate its end effector to a DSTEP 0.00001DMAX 20.0
task space T7(I), where
8i(O)=(0.2172, 0.3914, 0.1651, 0.4032, 1.0723, A(o)= 0.9967

0.4137, 0.0601) (42) and the number of iterations required for this calculation was
13.
0.522 -0.467 0.714 1.688
T7@) -0.821 0.047 0.569 0.373 VI. CONCLUSIONS
-0.232 0.883 -0.408 2.347 (43)
0.000 0.000 0.000 1.000 This paper presents a complete generalized solution to the
inverse kinematics of robots with arbitrary number of degrees
0.943 0.207 0.261 2.231 offreedomusing the concept of residuals. The solution is
T7(') -0.292 0.894 0.352 0.749 robot independent andis obtained usingan iterative procedure.
-0.160 -0.395 0.899 1.923 (44)
The procedure is amodifiedNewton-Raphsonalgorithm
0.000 0.000 o.Oo0 1.000 for which the step length is only restricted to avoid nonfeasible
solutions. It is however importantto define a sufficiently large
The optimization criterion used in this example is a weighted
initial maximumallowable step length A(o). Theprocedure
average of three criteria and it is defined as
also considers constraints on variables as well as an
min
+0.3M2+0.2M,
Z=0.5MI (45) objective function to be minimized. The procedure requires
that the Jacobian is either analytically or numerically com-
where
puted. In the case of analytically determined Jacobian therate
M I max [7;(Aei); i=
(446) 71 of convergence is considerably improved.

Authorized licensd use limted to: IE Xplore. Downlade on May 10,2 at 18:570 UTC from IE Xplore. Restricon aply.
GOLDENBERG et al.: GENERALIZED SOLUTION TO THE INVERSE KINEMATICS
OF
www.DownloadPaper.ir ROBOTS 19

Compute I ) ( Y ( ~ - ~ ) ) and from (29) as APPENDIX I

THE MANIPULATOR JACOBIAN follows:
a) If G(k+l)(y(k-I)/E) 5 let
Each column ofthe manipulator Jacobian matrix[qcan be b) If G(k+l)/(y(k-l)/E) and G(k+l)(y(k-l))5
obtained from aT,/dO;. In order to determine the columns of let
the (6 n ) Jacobian matrix with respect to the end effector
frame, the T,, i n transformations have to be 3) Solve (A8) for the normalized correction vector’
developed as follows ([Jp+ (A81
i-lTn=(AiAi+l A,), j=l, , n
4) Solve (A9) for correction vector 6,
where A iare the (4 4) transformation matrices between
frames (i 1) and (i). (A9)
The Jacobian matrix is denoted as
The constant is selected in order to satisfy inequality (28).
The normalization procedure is sometimes referred as “scal-
ing” and widely used in linear least squares problemsas a way
of improving the performance of an algorithm [20], [23].
REFERENCES
[l] R. P. Paul, B. Shimano, and G. E. Mayer, “Kinematic control
equations for simple manipulators,” ZEEE Trans. Syst., Man,
Cybern., vol. SMC-11, no. 6, pp. 456-460, 1981.
[2]M.Benati, P. Morasso, andV. Togliasco, “The inverse kinematic
problem of anthropomorphic manipulator arms,” ASME J . Dynamic
Syst., Meas., Contr., vol. 104, pp. 110-113,1982.
[3] A.Goldenbergand B. Benhabib,“End effector optimalpath
generation,” IEEE Trans. Syst., Man, Cybern., submitted for
publication.
[4] R. G . Fenton, B. Benhabib, and A. A. Goldenberg, “Optimal point to
6j=n,i+o,j+azk point control of robots with redundant degrees of freedom,” in Proc.
ASME Winter Ann. Meeting, New Orleans, PED-Vol. 13, pp. 93-
and if the joint is prismaic 109; ASME J. Eng. Ind. to appear.
[5] A. A. Goldenberg and D. L. Lawrence, “A generalized solution to the
dj=nzi+ozj+azk inversekinematics of robotic manipulators,” ASME J. Dynamic
Syst., Meas., Contr., vol. 107,pp.103-106, Mar. 1985.
6;=Oi+Oj+Ok 645) [6] B. Benhabib, A. A . Goldenberg, and R. G. Fenton, “A solution to the
inverse kinematics of redundant manipulators,” ASME J. Dynamic
where n, and p are the column vectors of T, Syst., Meas., Contr., submitted for publication; Proc. ACC, 1985.
[7] V. 3 . Lumelsky, “Iterative coordinate transformation procedure for
APPENDIX 11 one class of robots,” ZEEE Trans. Syst., Man, Cybern., vol. SMC-
14, no. 3, pp. 500-505, 1984.
THE LEVENBERG-MARQUARDT ITERATIVE [SI R. Featherstone, “Position and velocity transformations between robot
PROCEDURE end effector coordinates and joint angles,” ZnL J. Robot. Res., vol. 3,
no. 2, pp. 35-45, 1983.
The algorithmof the Levenberg-Marquardtprocedure [9] A. Liegeois, “Automatic supervisory control of the configuration and
given by (31)-(32) solves the nonlinear set of equations (3) behaviour of multibody mechanisms,” ZEEE Trans. Syst., Man,
Cybern., vol.SMC-7,no.12,pp.868-871,1977.
[lo] R. P. Paul, Robot Manipulators, Mathematics, Programming and
r= r,, r,, r,, re, r d = r ( q ) (3 Control. Cambridge, MA: Mass. Znst. Tech., 1981.
[ l l ] J. J. Uicker, J. Denavit, andR. S. Hartenberg, “An iterative method
1) for the displacement analysis ofspatial mechanisms,” ASME J. App.
Mech., pp.309-314,1964.
where r] solves the linear system [12] J. M. Ortega and W. C. Rheinboldt, Iterative Solution of Non-linear
Equations in Several Variables. New York Academic, 1970.
([J1T[JJ+y(k)[11)r](k)+[JJT(r(q(k)))=O.O. (32’) [13] J. F. Traub, Iterative Methods for the Solution of Equations.
Englewood Cliffs, NJ: Prentice-Hall, 1964.
The iterative procedure is as follows. [14]D. D. Whitney, “Optimum step size control for Newton-Raphson
1) Normalize the Jacobian matrix [JJ and the gradient (g), solution of non-linearvector equations,” ZEEE Trans. Automat.
Contr., pp. 572-574, 1969.
computed at as [15] J. Denavit, and R. S.Hartenberg, “A kinematic notation for lower-pair
mechanisms based on matrices,” ASME J. Appl. Mech., pp. 215-
221, 1955.
[16] P. R. Pauland C. H. WU, “Resolved motion force control ofrobot
manipulators,” IEEE Trans. Syst., Man, Cybern., vol. SMC-12, no.
3, pp. 226-275, 1982.
R. Fletcher, “Generalized inverse methods for the best least squares
047) solution of systems.of non-linear equations,” Comput. J., no. 10, pp.
392-399,1968.
2) Determine d k ) as follows. [18] P. Rabinowitz, Numerical Methods for Non-linear Algebraic Equa-
tions. New York: Gordon and Breach, 1970, pp. 87-114.
Let 1.0, and denote the valueof v from the [19] J. G . P. Barnes, “An algorithm for solving non-linear equations based
previous iteration; initially let v(O) 2. on the secont method,” Comput. J., no. 8, pp. 66-72, 1965.

Authorized licensd use limted to: IE Xplore. Downlade on May 10,2 at 18:570 UTC from IE Xplore. Restricon aply.
 www.DownloadPaper.ir IEEE JOURNAL OF ROBOTICS AND AUTOMATION, VOL. RA-I, NO. 1, MARCH 1985

D. W. Marquardt, “An algorithm for least-squares estimation ofnon- Engineers, the Society of Manufacturing Engineers, and the Association of
linear parameters,” J. Soc. Zndust. Appl. Math., vol. 11, no. 2, pp. Professional Engineers of Ontario.
431-441,1963.
K. Levenberg, “A method for thesolution of certain non-linear
problems in least squares,” Quart. Appl. Math., no. 2, pp. 164-168,
1944.
P. Rabinowitz, Numerical Methods for Non-linear AlgebraicEqua-
tions. New York:Gordon and Breach, 1970, pp.115-161.
J. More, et al., User guide for Minpack-I. Argonne, IL: krgonne
National Laboratory, 1980.
Y . Oh, D. Orin, and M. Bech, “An inverse kinematic solution for
kinematically redundant robot manipulators,” J. Robot. Syst., vol. 1,
no. 3, pp. 235-249, 1984.

View publication stats Authorized licensd use limted to: IE Xplore. Downlade on May 10,2 at 18:570 UTC from IE Xplore. Restricon aply.