Robot Dynamic Calibration: Optimal Excitation Trajectories ... · Advanced robot control schemes require an accurate knowledge of the dynamic ... is solved here using a genetic algorithm

Robot DynamicCalibration: Optimal

Excitation Trajectoriesand Experimental

Parameter Estimation� � � � � � � � � � � � � � � � � � � � � � � � � � � � � � � � � � �

G. Calafiore, M. Indri,* and B. BonaDipartimento di Automatica e InformaticaPolitecnico di TorinoCorso Duca degli Abruzzi, 24-10129 Torino, Italye-mail: [email protected]

Received 17 April 1999; revised 13 October 2000

Advanced robot control schemes require an accurate knowledge of the dynamicparameters of the manipulator. This article examines various issues related to robotdynamic calibration, from generation of optimal excitation trajectories to data acquisi-tion and filtering, and experimental inertial and friction parameter estimation. Inparticular, a new method is developed for the determination of optimal joint trajecto-ries for the calibration experiment, which is based on evolutionary optimizationtechniques. A genetic algorithm is used to determine excitation trajectories thatminimize either the condition number of the regression matrix or the logarithmicdeterminant of the Fisher information matrix. All the calibration steps have beencarried out on a SCARA two-link planar manipulator, and the experimental results arediscussed. � 2001 John Wiley & Sons, Inc.

1. INTRODUCTION

In robotics, the implementation of advanced controlalgorithms that take into account the manipulator

Ž .nonlinearities like the inverse dynamics methodrequires a good knowledge of the dynamic model ofthe robot. While the structure of the motion differ-ential equations of a manipulator is well known, thecorrect values of the involved parameters are notalways available, because they are often not directlymeasurable. In particular, the determination of the

To whom correspondence should be addressed.

dynamic parameters can be achieved only by meansof a proper calibration procedure, which estimatestheir values from input and output data providedby sensors and internal measurement devices, orreconstructed via software.

Two different calibration procedures are com-monly used to estimate the dynamic parameters ofa manipulator: the first one is based on the recur-sive formulation of the Newton�Euler motion equa-tions1,2 the second one is based on the Lagrangianenergy approach.3 Both approaches lead to dynamicequations that are linear in a minimally reduced set

Ž .of identifiable parameters the base parameter set ,

( ) ( )Journal of Robotic Systems 18 2 , 55�68 2001� 2001 by John Wiley & Sons, Inc.

� Journal of Robotic Systems—200156

which can be determined by applying the reductionprocedure proposed by Gautier and Khalil.4 Thepreliminary aspects related to manipulator model-ing for dynamic calibration are recalled in Section 2.

Estimation of the identifiable parameters is usu-ally achieved by application of a least-squares algo-rithm to the motion or the energy equations of therobot. The validity of the provided estimate stronglydepends on the quality of the available input�out-put signals and on the choice of the imposed refer-ence trajectories, which must properly ‘‘excite’’ therobot dynamics. The quality of the used input�out-put signals can be enhanced by using filters and�or

Ž .observers see, e.g., refs. 5 and 29 .The problem of finding exciting trajectories for

the identification has been discussed in several pa-pers. The work presented in Armstrong7 is amongthe oldest and it proposed a functional approach, inwhich the calculus of variations is used to solve anonlinear path optimization problem: An optimalacceleration sequence is determined, and then ve-locities and positions are computed by numericalintegration. The weak points of such an approachare the difficulty of including trajectory feasibilityconstraints in the optimization procedure and highcomputational burden due to the large number ofdecision variables required when the method isdiscretized.

The methods proposed in refs. 8 and 9 are bothbased on the determination of a sequence of optimaljoint position�velocity couples. A continuoussmooth ‘‘optimal’’ trajectory is then found by inter-polating them via fixed order polynomials; trajec-tory constraints are only checked at the end. Thenumber of decision variables and, consequently, thecomputational effort in this case are dependent onthe number of samples along the trajectory: Gautierand Khalil9 reported numerical intractability of theoptimization problem in their test cases for a num-ber of samples greater than 50.

An alternative solution to the above-describedŽmethods, which are gradient based and thus re-

quire time-consuming computations to exactly de-termine or estimate the gradient of the objective

.function , is proposed in ref. 10, where a randomsearch approach is used to determine the set ofoptimal points to be interpolated.

The method proposed in this article is based onthe parameterization of the class of joint positiontime functions. The key idea underlying this methodis to restrict attention to some parameterized family

of possible trajectories, thus reducing the originalinfinite-dimensional problem to a more tractablefinitely parameterized optimization problem. Thisis, of course, done at the expense that the solutionwill be optimal in the selected class, i.e., suboptimalwith respect to the original problem. However, whatis of interest in practice is to determine a ‘‘good’’Ž .albeit not globally optimal excitation trajectory thatleads to an informative identification experiment. Inthis respect, the above drawback does not seem tobe critical in an experimental or industrial context,as long as the family of allowable trajectories ischosen to be ‘‘rich enough,’’ according to engineer-ing judgment and experimental trials. In this article,in particular, we consider the class of harmonicfunctions with a fixed, finite number of harmonicterms. The introduced parameterization takes intoaccount the frequency band constraints on the inputexcitation, and the number of decision parametersof the resulting optimization problem is indepen-dent of the number of time samples along the trajec-tory. Harmonic parameterization also was used inthis context by Swevers et al.11

The trajectory parameterization reduces theproblem to a nonlinear parametric optimization that

Ž .is solved here using a genetic algorithm GA . Theconvergence of the GAs to the global optimum isnot, in general, guaranteed, but in practice suchalgorithms have proved to be particularly efficientand robust in the global search of solutions of non-linear and multimodal optimization problems.12,13

In robotics, genetic algorithms have been used, forinstance, for motion planning of mobile manipula-tors14 and for estimation of kinematic parameters.15

In our case, the use of GAs in conjunction with theintroduced trajectory parameterization allows us totake into account the physical constraints on jointpositions, velocities, and accelerations directly inthe decoding phase of the genotype of the individu-als, i.e., of the strings containing the binary codes ofthe decision variables, so that the resulting trajecto-ries are certainly feasible.

In the experimental part of this article, the com-plete calibration procedure is performed on a

ŽSCARA two-link manipulator produced by IMI In-.tegrated Motions Inc., Berkeley, CA , starting from

trajectories generation according to the proposedmethod, through issues related to data acquisitionand filtering, to the estimation of the inertial andfriction parameters of the manipulator and modelvalidation.

�Calafiore, Indri, and Bona: Robotic Dynamic Calibration 57

2. ROBOT CALIBRATION MODEL

Ž .Consider an n degrees-of-freedom dof manipula-tor, having only revolute joints for the sake of sim-

Žplicity the subsequent developments can be easily.extended to the most general case . Let the kinemat-

ics of the robot be described by introducing De-navit�Hartenberg notation to define local framesattached to the links. According to such notation,frame RR , i�1, . . . , n, is the reference frame lo-i�1cated in correspondence to joint i, with the z axiscoinciding with the rotational axis of the respective

Ž .joint RR is the inertial frame , whereas RR is the0 nframe attached to the end effector. Matrix R i�1 isithe rotation matrix from frame RR to frame RR . Ini�1 iaddition, let q be the joint position vector, whereeach coordinate q represents the angular position ofijoint i, defined according to Denavit�Hartenbergrules.

The motion equations of the manipulator can bedetermined following the recursive Newton�Eulerformulation, by using two transformation laws: thefirst one is represented by the forward transforma-tion law of the robot kinematics from the base to theend effector, while the second one is given by thebackward computation of the joint torques from theend effector to the base. For a manipulator that hasonly revolute joints, the recursive Newton�Euler

Ž .equations are dependence on time is omitted

i�1� � Ž .� �R � �z q , 1a˙i�1 i i i i�1

i�1� Ž .� Ž .� �R � �z q �� z q , 1b˙ ˙ ¨ ˙i�1 i i i i�1 i 1 i�1

i�1 Ž .v �R v �� p �� p ,˙ ˙ ˙i�1 i i i�1 i�1 i�1 i�1 i�1

Ž .1c

i Ž .f �R f �m v �� m s �� m s ,˙ ˙i i�1 i�1 i i i i i i i i i

Ž .2a

n �R i n �p �f �� ˙i i�1 i�1 i i i i i

Ž . Ž .� � � �m s �v , 2b˙i i i i i

T i Ž .� �n R z �� , 2ci i i�1 i�1 f i

where � is the angular velocity of joint i, v is thei ilinear velocity of the origin of frame RR , f and ni i iare, respectively, the force and the moment appliedto link i by link i�1, � is the torque applied toijoint i, � is the friction torque acting on joint i, zf i idenotes the z axis of frame RR expressed in its owni

Ž � �T .frame i.e., z � 0 0 1 for all i , p representsi ithe position of the origin of frame RR with respecti

to RR , m is the mass of link i, s is the positioni�1 i ivector of the center of mass of link i, and � is theiinertia tensor with respect to frame RR . For nota-itional convenience, vectors with subscript i in

Ž . Ž .Eqs. 1 and 2 are represented in the referenceframe RR .i

The friction torques � can be expressed accord-f iing to one of several models proposed in the litera-ture, taking into account the different frictioncomponents, e.g., stiction, Coulomb, and viscousfriction.16 In general, the friction torque vector � fcan be written as

Ž . Ž .� �F q, q � , 3˙f f

Ž .where the expression of the function F q, q and the˙choice of the parameter vector � �� n f depend onfthe considered friction model. Note that if onlyCoulomb and viscous friction are included in the

Ž .model, then F q, q is a function of velocity q only.˙ ˙By introducing vector � , containing the n

Ž . Ž .torques applied to the joints, Eqs. 1 and 2 can berearranged in the form

Ž . Ž .��D q, q, q � , 4˙ ¨ m

which is linear with respect to the vector � �� mm

which contains all the inertial parameters of themanipulator, i.e., the masses m , the inertia mo-i

Ž .ments six for each link , and the first-order mo-Ž . 3,17ments three for each link of the robot links and

Ž .the n friction parameters. D q, q, q , which is a˙ ¨fnonlinear function of joint position, velocity, andacceleration vectors, is then an n�m matrix withm�10n�n .f

It is well known that only some of the inertialparameters of the robot really affect its dynamicsand are then identifiable by calibration procedures.It is possible to determine the set of minimum numberof inertial parameters, i.e., the base parameter set con-taining only the p identifiable dynamic parameterscollected in � �� p, by means of the reductionpprocedure proposed by Gautier and Khalil,4 based

Ž .on the energy model of the robot. Equation 4 canbe then rewritten as

Ž . Ž .��D q, q, q � , 5˙ ¨

� T T �T n pin which �� , with n �p�n , andp f p fŽ . Ž .matrix D q, q, q is properly defined. Relation 5 ,˙ ¨

which is linear with respect to �, can be used for theestimation of � by collecting the values of � , q, q,˙

Žand q by measurements and�or reconstructed by¨


. Ž .observers at n time instants with n n�n dur-m m ping the execution of a task, thus obtaining the equa-tion

Ž .y�H��v 6a

� Ž . Ž .�Twith y� � t �� t and1 nm

Ž Ž . Ž . Ž ..D q t , q t , q t˙ ¨1 1 1.. Ž .H� , 6b.

Ž . Ž . Ž .D q t , q t , q t˙ ¨Ž .n n nm m m

where v is assumed to be the zero mean measure-ment noise vector, uncorrelated from �, and H is

Žthe data matrix, assumed to be deterministic theeffects of uncertainty on the data matrix H are

.discussed in ref. 7 . The base parameter vector � canŽ .be estimated from the regression equation 6a us-

ing a least-squares algorithm as discussed in thefollowing subsection.

We note that the energy-based calibration ap-proach3,17 also leads to a standard least-squaresidentification problem, which may be cast in the

Ž .form 6a . Therefore, all the subsequent develop-ments can be applied equally to the energy-basedcalibration method.

2.1. Least-Squares Parameter Estimation

Ž .We recall the regression equation 6a ,

y�H��v,

where y�� nm is the vector of measurements, �� n p is the parameter vector to be identified, H�

nm , n p Ž� is the regression matrix assumed to be de-. nmterministic , and v�� is a zero mean random

vector representing measurement errors. Vector v isassumed to be uncorrelated from �, and the auto-correlation matrix R �0 of v is assumed to bevknown. A priori information about the unknownparameter vector is also included in the problem via

Žthe mean value � of � for instance, � may be taken.as the vector of nominal parameters and the ‘‘confi-

dence’’ on the nominal parameters, expressed by agiven covariance matrix R �0. It is then well�

known18 that the linear least-mean-squares estima-tor of �, given y, is

�1�1 T �1 T �1ˆ Ž . Ž . Ž .�� R �H R H H R y�H� , 7� v v

and the corresponding error covariance matrix isgiven by

�1�1 T �1Ž . Ž .P � R �H R H . 8� � v

The above formulas refer to the batch solution ofŽ .the least-squares LS problem for the accumulated

measurements y. In the experimental tests, the ac-tual estimate will be computed using the following

Žrecursive formulation of the LS algorithm see ref.. 1, n p18 . Let H �� and y �� represent the ith rowi i

of the regression matrix and the ith measurement,ˆrespectively. Let � and P be defined as the esti-i � , i

mate of the parameter vector �, given the measure-ments up to i, and the relative covariance, respec-tively. The estimate can then be recursively updatedusing the recursions

ˆ ˆ ˆ ˆ� �� K y �H � , � �� ,Ž .i�1 i i�1 i�1 i�1 i 0

�1T TŽ .K �P H R �H P H ,i�1 � , i i�1 v i�1 � , i i�1

P �P �K H P , P �R .� , i�1 � , i i�1 i�1 � , i � , 0 �

We recall that the solution obtained from the recur-sive algorithm for i�n coincides with the batchm

ˆ ˆsolution, i.e., � ��; P �P . The recursive ver-n � , n �m m

sion is, therefore, a computationally efficient way toŽ .solve 7 , especially when a large number of mea-

surements is to be processed.If a priori knowledge is neglected, then R ��,�

Ž .and it follows from 8 that the ‘‘size’’ of the estima-tion error depends on the normal equations matrix��HT R�1 H. A frequently used scalar measurerv

Ž .on � is given by J � log det � . Maximization ofdJ with respect to the input excitation is referred todas D optimality,19 and it is desirable in order to getestimates with minimal uncertainty bounds. An-other significant index is derived from the condition

Ž .number of �: J �cond � . A small J is desirablek kto minimize the bias of the estimate due to unmod-

Želed dynamics errors which are correlated with the.regressors , to enhance the convergence rate of the

algorithm, and to minimize the sensitivity of thesolution to variations of the measurements vector yŽ .see refs. 19 and 20 .

3. TRAJECTORY OPTIMIZATION

The previously discussed optimality indices dependon the kinematic structure of the manipulator, whichis fixed for a given type of manipulator, and on the


time history of the joint reference positions, veloci-ties, and accelerations. Denoting with QQ a class ofcontinuous functions of the time variable t�� ,

� �defined over the interval TT� t , t , the prob-min maxlem of determining the most ‘‘exciting’’ reference

Ž .trajectory q* t with respect to the generic index Jcan be formulated as

Ž . Ž Ž .. Ž .q* t �arg min J q t 9a

subject to

Ž . Ž .q t �QQ � i , 9bi

� Ž . � Ž .q t �q � i , t , 9ci i , max

� Ž . � Ž .q t �v � i , t , 9di i , max

� Ž . � Ž .q t �� i , t , 9ei i , max

where q , v , and � represent physicali, max i, max i, maxconstraints on the ith joint position, velocity, and

Ž .acceleration, respectively. The problem 9 is a non-linear functional optimization problem, for whichdifferent solutions have been presented in literature,as discussed in section 1.

The method that we propose herein is based ona parameterization of the class QQ of joint positiontime functions. Formally, we consider that the classQQ is of the form

Ž .QQ�QQ p , p�PP,

where p is a vector of real-valued parameters andPP represents the set of physical constraints on thevalues of the parameters. The number of decisionparameters is, therefore, independent of the numberof time samples along the trajectory.

The two most popular optimality criteria, amongŽthe various ones proposed in the literature see, e.g.,

.refs. 7, 9, and 11 are considered: the conditionnumber J of the regression matrix, as a measure ofkthe disturbance influence on the parameter esti-

Ž Ž ..mates and the scalar measure log det � of theŽ .Fisher information matrix i.e., the J index to guar-d

antee minimal uncertainty of the parameter esti-mates.

The resulting finite-dimensional optimizationproblem is then solved using the paradigm of ge-netic evolution. The genetic algorithm used in the

Ž .solution is objective fitness function based anddoes not require the computation of gradients andhessians. Also, the GA is oriented toward the identi-fication of the global optimum, as is discussed insome detail in the following subsections.

3.1. Parameterization of the Joints Trajectories

Though the method outlined in this article is con-Ž .ceptually valid for any parametric family QQ p , we

treat in detail the class of harmonic functions with apredefined and fixed number W of harmonic terms.This class of trajectories was also suggested in ref.21 and, independently, in ref. 11.

Ž .Given W typically a small integer, W�3, 4, . . . ,the reference trajectory for the jth joint is assumedto be

W

Ž . Ž . Ž .q t � a sin � t , 10Ýj j , k j , kk�1

where a and � , j�1, . . . , n, k�1, . . . , W, arej, k j, know the decision variables of the optimization prob-

Ž .lem 9 . The number of decision variables n �2Wnvis independent of the number of time samples alongthe trajectory. As

W W

� Ž . � Ž . � � Ž .q t � a sin � t � a , 11Ý Ýj j , k j , k j , kk�1 k�1

Ž .the constraint 9c may be conservatively substi-tuted by

qj , max� �a � � j, k .j , k W

Similarly, we obtain that

W Wqj , max� Ž . � Ž . � �q t � a � cos � t � �˙ Ý Ýj j , k j , k j , k j , kWk�1 k�1

Ž .and the constraint 9d is conservatively substitutedby

vj , max� �� j, k .j , k qj , max

Also

W Wqj , max2 2� Ž . � Ž . � �q t � a � sin � t � �¨ Ý Ýj j , k j , k j , k j , kWk�1 k�1

Ž .and the constraint 9e is conservatively substitutedby

� j , max� �� j, k .j , k ( qj , max


Ž .The optimization problem 9 specializes now to

Ž Ž .. Ž .minimize J q t 12aa , � ; � j , kj, k j , k

subject, � j, k, to the constraints

Ž . Ž .q t �QQ , 12bj

qj , max� � Ž .a �a � , 12cj , k j , max W

v �j , max j , max Ž .� �� min , . 12dj , k j , max ½ 5(q qj , max j , max

3.2. GA Coding

Ž .The optimization problem 12 is solved using a GAwith binary coding.12 In the language of geneticprogramming, each candidate solution is repre-sented by a binary string of n digits which isbreferred to as an individual. The GA works with apopulation of N individuals and emulates thepparadigm of natural selection and evolution. Ateach generation, some members of the populationare selected for reproduction into the next genera-tion. The choice of the individuals to be reproducedis guided by the fitness value of the individuals andby a random component. The reproduction steptakes place by emulating the genetic operators of

Žcrossover and mutation on the genotypes the bit.strings of the individuals. The evolution from one

generation to the next is very likely to enhance theaverage and best fitness of the individuals in thepopulation. After a certain number of generations,the GA converges to an individual which is the bestsuited for the given fitness function. The conver-gence of the GA to the global optimum is not, ingeneral, guaranteed, but in practice the GA hasproved to be extremely efficient in solving nonlinearand multimodal optimization problems.12,13 The GAturns out to be particularly efficient and robust inthe global search of solutions, while usual gradient-based methods are efficient in the local solutionsearch.

In the problem at hand, each decision variablea and � is coded as a binary integer A andj, k j, k j, k of nŽa. and nŽ� . digits, respectively, and anj, k j, k j, kindividual is represented by a binary string X ob-tained by connecting all the binary strings headto tail relative to the decision variables: X�� A , . . . , A , A , . . . , A , . . . , A , . . . , A ,1, 1 1, W 2, 1 2, W n, 1 n, W

� , . . . , , , . . . , , . . . , , . . . , .1, 1 1, W 2, 1 2, W n, 1 n, W

Ž . Ž .The constraints 12c and 12d are directly takeninto account in the decoding phase of the genotypeX. Prior to evaluation of the fitness function, eachelement of X is decoded to its real value in themanner

� �A 1j , k 10 Ž .a � � 2 a � j, k , 13Ž a.j , k j , maxnž /j ,k 22 �1

� � j , k 10 Ž .� � � � j, k , 14Ž a.j , k j , maxn j ,k2 �1

� � � �where A and are the decimal valuesj, k 10 j, k 10of the binary integers A and . The bounds onj, k j, kthe decision variables are, therefore, implicitly in-cluded in the decoding step, so that the resultingproblem is unconstrained in the binary string X.The GA has been implemented in a MATLAB pack-age developed by the authors and is available onrequest.

4. APPLICATION TO A PLANARMANIPULATOR

The effectiveness of the method is validated byexperimental tests for the dynamic calibration of theSCARA two-link planar manipulator, which is pro-duced by IMI and is shown in Figure 1. The consid-ered manipulator has two revolute joints equipped

Ž .with brushless direct-drive motors produced byŽ .NSK Nippon Seiko K. K., Tokyo, Japan , incorporat-

ing resolvers for position measurements. The con-trol unit of the robot is constituted by a Pentium-90PC with a TMS320C30 Digital Signal ProcessingŽ .DSP board, and the control algorithms are imple-mented in C. The joint coordinates are defined ac-cording to Denavit�Hartenberg notation and col-

� �Tlected in vector q� q q , where q represents1 2 ithe angular position of joint i. The command torque

� �Tvector is defined as �� , where � is the1 2 itorque applied to joint i, and the lengths of the linksare, respectively, given by l �0.359 m and l �1 20.24 m.

4.1. Dynamic Model of the Planar Manipulator

The application of the reduction procedure devel-oped by Gautier and Khalil4 leads to a base param-eter set constituted by four inertial parameters givenby the inertia moments of the links with respect to

Žthe z axis i.e., the axis perpendicular to the motion0.plane and the first-order moments m s , m s of2 2 x 2 2 y


Figure 1. Photograph and schematic model of the IMI two-link manipulator.

the second link. The vector � of the identifiablepinertial parameters is then given by

T2� � �m l m s m s .p 1 z 2 1 2 2 x 2 2 y 2 z

Due to the small number of identifiable parame-ters for our 2 dof robot, no further reduction of theparameter vector is needed, while an approximate,

Žsimplified model which neglects, if possible, the.effects of some of the identifiable parameters could

be usefully employed for greater manipulators, assuggested in ref. 22.

In the considered case, only Coulomb and vis-cous friction are modeled, under the assumption

Žthat other components e.g., hysteresis effects.and�or stiction can be considered as negligible.

The friction torque acting on each joint is thendescribed as

Ž . Ž . Ž .� q �� sgn q �� q , 15˙ ˙ ˙f , i i s , i i v , i i

where the parameters � and � represents, i v, iCoulomb dynamic friction and the viscous frictioncoefficient, respectively. Note that this model cancorrectly describe friction only at ‘‘sufficiently high’’velocities; otherwise, a specified model for stiction

Ž .must be introduced. According to 15 , the frictionparameters of the robot can be collected in a vector

4 �� , which is defined as � � � , � , � ,f f s, 1 v, 1 s, 2�T� . The complete parameter vector � is thenv, 2

� T T �T 8given by �� . Following the New-p fton�Euler approach, the planar robot dynamic

Ž .model is expressed as in 5 , where

D �q ,¨1, 1 1

Ž Ž . Ž ..D � l c 2 q �q �s q 2 q �q ,¨ ¨ ˙ ˙ ˙1, 2 1 2 1 2 2 2 1 2

Ž Ž . Ž ..D � l �s 2 q �q �c q 2 q �q ,¨ ¨ ˙ ˙ ˙1, 3 1 2 1 2 2 2 1 2

D �q �q ,¨ ¨1, 4 1 2

Ž .D �sgn q ,˙1, 5 1

D �q ,˙1, 6 1

D �D �0,1, 7 1, 8

D �0,2, 1

Ž .16

Ž 2 .D � l c q �s q ,¨ ˙2, 2 1 2 1 2 1

Ž 2 .D � l �s q �c q ,¨ ˙2, 3 1 2 1 2 1

D �q �q ,¨ ¨2, 4 1 2

D �D �0,2, 5 2, 6

Ž .D �sgn q ,˙2, 7 2

D � q2, 8 2

with s �sin q and c �cos q .i i i i

4.2. Robot Control and Physical Constraints

The control of the two-link manipulator is per-formed by an independent joint feedback controllaw, which is implemented by inserting an observerfor the estimation of the joint velocities, becauseonly position measurements are available. Unsatis-factory results have been obtained by simply deriv-


ing the joint velocities at discrete time from theposition measurements. The high-gain observer de-veloped in ref. 23 were chosen after a comparisonamong different solutions proposed in literatureŽ .e.g., refs. 24�27 because it seems to offer, in ourcase, a good compromise in terms of tracking error

Žand quality of the reconstructed signals with refer-ence in particular to velocities, which must be usedboth for the control and for the parameter identifica-

.tion procedure . The implemented controller�ob-server scheme is then described by

1˙ Ž . Ž .x �x � H q�x , 17aˆ ˆ ˆ1 2 p 1�

1˙ Ž . Ž .x � H q�x , 17bˆ ˆ2 v 12�

Ž . Ž . Ž .��K q �q �K q �x , 17c˙ ˆp d d d 2

where the observer state variables x and x , respec-ˆ ˆ1 2tively, denote the dynamic estimates of the jointpositions and velocities, q is the vector of the jointpositions measured by the resolvers incorporated in

Ž . Ž .the motors, q t and q t are the assigned refer-˙d d

ence position and velocity vectors, obtained by theoptimization procedure, and � is the vector of the

Ž .applied torques in 5 . The observer gain matricesŽ . Ž .H �diag h and H �diag h are chosen sop p, i v v, i

that the spectra of the characteristic polynomialsŽ . 2p � �h �h , i�1, 2, are in the open lefti p, i v, i

half-plane, whereas � is a small positive parameter.The controller gain matrices K and K are diago-p d

nal, positive definite matrices that are selected tosatisfy the desired closed-loop specifications. In thetests carried out, the controller�observer scheme isdesigned at continuous time to obtain a closed-loopsystem with a bandwidth of about 60 rad�s and amaximum peak�4 dB, and then implemented at

Ždiscrete time with sampling time T�1 ms sample.rate � �2��T6280 rad�s . The selected observerc

Ž . Ž .gains are H �diag 7, 6.2 , H �diag 2, 4 , and ��p v

0.002.The reference joint trajectories are assumed to

belong to the class QQ of harmonic functions definedŽ . Ž . Ž .in 10 , with the physical bounds 9c � 9e on the

angular positions, velocities, and accelerations givenby q �1.5 rad, v �8 rad�s, and � �5max max max

2 Ž .rad�s for both joints. From 12d it follows that thefrequency content of the reference signals is alwaysbelow � 1.825 rad�s for all joints.max

4.3. Data Acquisition and Processing

The main problems to be solved for the acquisitionof the data that are required for the calibrationprocedures can be summarized as follows:

1. The need to guarantee a good quality of themeasured or observed signals to be used forthe parameter estimation.

2. Computation of the signals not directly avail-able: joint accelerations and command

Žtorques because they are not directly mea-.surable in our case .

3. Characterization of the torque measurementerrors to be used in the estimation algorithm.

The joint velocities reconstructed by the observer�Ž . Ž .�17a and 17b and directly used for the robotcontrol need to be filtered before the computation ofthe acceleration signal and the parameter estima-tion. This filtering should be performed off-line bymeans of a noncausal zero phase shift filter to avoiddistortion in the dynamic regressor, as discussed inref. 6. However, due to our hardware limitationsand conflicting interrupt problems during the mo-tion of the robot, it was not possible for us toacquire samples at the control rate � and then filtercand decimate the data off-line. Therefore, we pre-ferred to filter the velocity signal on-line using the

Ž .following discrete time filter T�2�� 1 mscthat has cutoff frequency � 14 rad�s,f

Ž . Ž . ŽŽ . .q kT �� x kT �� q k�1 T˙ ˆ ˙fil 1 2 2 fil

ŽŽ . .�� q k�2 T ,˙3 fil

with � �1.95550858, � ��0.95599748, and � �12 3 1�� . Note that this filter introduces negligible2 3phase shift for the reference signals used, whichhave frequency content below � . The joint accel-maxerations are then computed on-line at the controlrate � from the filtered velocity by means of accentral difference algorithm, as discussed in refs. 6and 28.

The torques are reconstructed by their expres-Ž .sion 17c and then filtered on-line at the control

rate, using the same discrete time filter employedfor the velocities. The filtered torque vector is thengiven by

Ž . Ž . ŽŽ . .� kT �� kT �� k�1 Tfil 1 2 fil

ŽŽ . .�� k�2 T .3 fil


For each reference trajectory, the following proce-dure is used to estimate the mean-square errorŽ .sample variance of the torque measurements. Thegiven trajectory is repeated by the manipulator forM�20 times and n data samples are collectedmover each repetition of the trajectory. The samplevariance of the torque measurements for the ithjoint is computed as

n Mm1 22 Ž . Ž .� � � k �� k ,Ž .Ý Ýi i j iŽ .n M�1m k�1 j�1

Ž .where � k represents the k th measurement of thei jtorque at joint i during the jth trajectory repetition

Ž .and � k represents the average over the M repeti-itions of the value of the ith joint torque at the k thsample.

5. EXPERIMENTAL PARAMETER ESTIMATION

Optimal excitation trajectories are computed accord-ing to the two criteria previously discussed. Thereference trajectories so found are then executed bythe manipulator and a recursive LS algorithm isapplied to the acquired data to estimate the inertialand friction parameters.

The LS normal equation matrix is, in this case,T �1 Ž .given by ��H R H, where H is given by 6v

2 24and R �diag � , � is the diagonal covariancev 1 2matrix relative to the torque measurements. Whereasthe estimated sample variances on the torque mea-surements depend on the reference trajectory, it wasdecided to compute the optimal reference trajectoryassuming R �I and then to estimate the measure-vment variances on the computed trajectories for theapplication of the recursive parameter estimationalgorithm.

It was determined from simulations that param-Žeter resolution i.e., the number of binary digits

.used to encode each of the decision variable is notan issue in the problem at hand; the choice ofnŽa. �nŽ� .�4 � j, k is satisfactory for our purposes.j, k j, kNumerical simulations also showed that a numberof harmonics as low as W�3 is sufficient to deter-mine a good trajectory, because larger values of Wgave no significant improvement on the optimalobjective. The number of variables for the optimiza-tion problem is, in this case, n �2Wn�12, wherevn�2 is the number of joints of the manipulator and

Ž .each solution string genotype is represented by the

GA as a n �4n �48 digits binary string X. No-b v

tice that the number n of decision variables isv

independent of the number of time samples nm

along the trajectory, while it is proportional to theconsidered number W of harmonic terms. The lengthof the genotypic string X is proportional to n andv

Ž .to the selected resolution number of binary digitsto which the variables are encoded. The fitnessfunction is, in this case, given by �J or by J ,k d

because the GA is traditionally a maximization al-gorithm.

5.1. Estimation Using J Optimal Trajectoriesk

We first consider the optimality criterion based onŽ .the minimization of the condition number J � .k

The resulting three-harmonic trajectory is shown inŽ .Figure 2. The minimum value of J J �13.34 isk k

achieved by the GA after less than 20 generations,as shown in Figure 3. The order of the computa-tional time is 5 min on an ‘‘old’’ Intel 486�66 PC.

The computed optimal trajectory is generated at1 ms and passed to the manipulator that repeats it20 times. The torque measurement variances areestimated over the 20 repetitions as � 2 �3.069 N 2 �1

m2 and � 2 �0.157 N 2 �m2. The LS algorithm de-2

scribed in Section 2.1 is used for the off-line parame-ter estimation, collecting n �80 equally spacedm

samples for each of the first six trajectory repeti-tions. The evolution of the estimates is shown in

Figure 2. Optimal joint reference trajectories using indexJ . Solid line, first joint; dashed line, second joint.k


Figure 3. GA evolution. Crosses represent fitness valuesŽ .��J of the best individual in the generation. The solidkline represents the average fitness over the populationmembers at each generation.

Figure 4, and the final obtained estimates, which areconsistent with the physical meaning of the parame-ters, are given by

ˆ �� 3.923; 0.808; 0.328; 0.141; 2.61; 7.343;T�1.460; 0.787 ,

Figure 4. Evolution of the recursive LS parameter esti-mates.

Ž .Figure 5. Torque estimation residuals for the first topŽ .and second joints bottom ; six data batches.

whereas the diagonal elements of the obtained esti-mation covariance matrix are

�� 0.0054; 0.0028; 0.0032; 0.0004; 0.0225; 0.0473;�

T�0.0011; 0.0025 .

The values obtained from the manufacturer data forthe first four parameters are

� �3.689 kg �m2 ,nom , 1

� �0.97 kg �m,nom , 2

� �0 kg �m,nom , 3

� �0.275 kg �m2 ,nom , 4

but we remark that the goal of the applied LSmethod is to determine parameters that minimizethe torque residual on the basis of the assumeddynamic and friction model, without regard to thephysical meaning of each single parameter.

The estimated parameters are used to comparethe reconstructed torques with the measured ones.Figure 5 shows the torque estimation residuals,while the measured and reconstructed torques are

Ž .compared in Figure 6. The root-mean-squared rmsŽestimation residuals difference between the mea-

.sured and reconstructed torques are computed overthe estimation set as � �1.34 N �m and � �0.241 2N �m; the relative estimation errors, defined as theratio between the rms estimation error and the rmsvalue of the measured torque, are � �15% andr , 1� �11.7%.r , 2


Ž .Figure 6. Comparison of measured dashed and recon-Ž . Ž . Žstructed solid torques: first top and second joints bot-

.tom ; two data batches.

Estimate Validation

The obtained estimate is tested against two valida-tion data sets that differ from the data used forestimation. As a first validation set, we consideranother batch of six repetitions of the optimal refer-ence trajectory. The rms values of the residuals arein this case � �1.38 N �m and � �0.23 N �m, while1 2the relative rms errors are � �16.5% and � �r , 1 r , 212.2%. The second validation set is constructed uponmeasurements on six repetitions of the test trajec-tory shown in Figure 7. The resulting residual errorsare � �1.3 N �m, � �0.31 N �m, � �17.2%, and1 2 r , 1� �14.6%, and the torque reconstruction residu-r , 2als are shown in Figure 8.

5.2. Estimation Using J Optimal Trajectoriesd

Computation of the optimal three-harmonic refer-ence trajectory also was performed with respect tothe second optimality criterion, based on the index

Ž .J � log det � . The maximum obtained value is, indŽ .this case, J �17.9, corresponding to a det � valued

of about 8�1017. A plot of the reference trajectory isreported in Figure 9. The torque measurement vari-ances were estimated as previously explained,yielding the values � 2 �113.77 N 2 �m2 and � 2 �1 20.39 N 2 �m2. By applying the recursive LS algo-rithm, the parameter estimates

ˆ �� 3.445; 0.698; 0.012; 0.107; 0.500; 7.884;T�1.023; 0.883

Figure 7. Validation trajectory. Solid line, first joint;dashed line, second joint.

were obtained with the following values for thediagonal elements of the covariance matrix:

�� 0.0339; 0.0010; 0.0011; 0.0000; 1.0446; 0.5603;�

T�0.0035; 0.0019 .

The rms torque reconstruction residuals on the esti-mation data set are � �2.98 N �m, � �0.24 N �m,1 2� �20.3%, and � �9.3%. Figure 10 shows ther , 1 r , 2

Figure 8. Torque reconstruction residuals for the valida-Ž . Ž .tion trajectory: first top and second joints bottom ; six

data batches.


Figure 9. J -optimal reference trajectories. Solid line, firstdjoint; dashed line, second joint.

torque reconstruction residuals, while the measuredand reconstructed torques are compared in Fig-ure 11.

Estimate Validation

Using a first validation data set given by anotherbatch of six repetitions of the optimal referencetrajectory, the following rms residuals were ob-tained: � �3.65 N �m, � �0.31 N �m, � �26.6%,1 2 r , 1and � �13%. The larger error on the reconstruc-r , 2tion of the torque at the first joint is caused by thelarge mean-square error in the measurements of this

Ž .Figure 10. Torque reconstruction residuals: first topŽ .and second joints bottom ; six data batches.

Ž .Figure 11. Comparison of measured dashed and recon-Ž . Ž . Žstructed solid torques: first top and second joints bot-

.tom ; two data batches.

torque. Validation on the test trajectory of Figure 7yields � �1.78 N �m, � �0.4 N �m, � �23.8%,1 2 r , 1and � �19%.r , 2

5.3. Remarks

Trajectories optimized with respect to the J indexkprovided parameter estimates with the lowest rela-tive rms prediction errors. The obtained parameterestimates allowed reconstruction of the input

Žtorques that may be used for feedforward compen-.sation with about 20% rms error on both joints.

J -optimal trajectories tend to move the manipulatordfaster and result in higher measurement variance;therefore, the relative parameter estimates yieldhigher rms errors in the reconstructed torques.

Some of the parameter estimates obtained in theŽtwo cases i.e., when tracking a J - or a J -optimalk d

.trajectory are quite different. In particular, the esti-mates of the static friction � of the first joint are,s, 1respectively, given by 2.61 and 0.5 N �m. This factmay be partially due to the differences between the

Ž .imposed trajectories see Figures 2 and 9 : Becausethe first one is slower than the second one, the effectof the static friction is stronger in the first case, so

Žthat greater values of � are estimated compares, i.also the estimates of � , 1.460 and 1.023 N �m .s, 2

Therefore, a more accurate model of the frictionphenomena may be useful if direct friction compen-sation is required for control purposes. Notice alsothat the value of the static friction parameter for thefirst joint for the J -based experiment has little sig-d


nificance, since the relative variance is high com-pared to the nominal value of the parameter.

Finally, it must be stressed that the parameterestimates obtained using standard LS identificationmay sometimes lead to physically unfeasible nu-

Žmerical values of the parameters i.e., negative mo-.ments of inertia, etc. because no specific constraints

are imposed in the optimization problem. Futurework will be devoted to study the effect of theinclusion of physical constraints on the torque re-construction residuals.

6. CONCLUSIONS

In this article, an efficient method for the determina-tion of ‘‘exciting’’ trajectories for robot inertial pa-rameter estimation has been presented. The methodis based on harmonic parameterization of the jointtime functions and minimization of different suit-able measures on the LS normal equation matrixperformed by means of a binary coded genetic algo-rithm. The proposed framework may be applied toboth Newton�Euler and energy-based parameterestimation models.

Numerical simulations were carried out usingthe Newton�Euler model and showed that goodtrajectories can be found using low parameter reso-lution and a small number of harmonic terms. Thecomputed trajectories respect the frequency band-width constraints and the physical limits on jointposition, velocity, and acceleration.

The results of the experimental identification ofthe inertial and friction parameters of a SCARAtwo-link manipulator, using the optimal input exci-tations, also were presented and discussed.

REFERENCES

1. C.G. Atkeson, C.H. An, and J.M. Hollerbach, Estima-tion of inertial parameters of manipulator loads and

Ž .links, Int J Robotics Res 5 1986 , 101�119.2. C.P. Neuman and P.K. Khosla, Identification of robot

dynamics: An application of recursive estimation, Proc4th Yale Workshop on Applications of Adaptive Sys-tem Theory, Yale University, 1985, pp. 42�49.

3. S.-Y. Sheu and M.W. Walker, Estimating the essentialparameter space of the robot manipulator dynamics,Proc 28th Conf on Decision and Control, 1989, pp.2135�2140.

4. M. Gautier and W. Khalil, Direct calculation of mini-mum set of inertial parameters of serial robots, IEEE

Ž .Trans Robotics Automation 6 1990 , 368�373.

5. B. Bona and M. Indri, Experiments on the dynamiccalibration of a planar manipulator, Proc 27th IntSymp on Industrial Robots, Milan, Italy, 1996, pp.117�122.

6. M. Gautier, A comparison of filtered models for dy-namic identification of robots, Proc 35th CDC, 1996,pp. 875�880.

7. B. Armstrong, On finding exciting trajectories foridentification experiments involving systems withnon-linear dynamics, Proc IEEE Int Conf on Roboticsand Automation, 1987, pp. 1131�1139.

8. F. Caccavale and P. Chiacchio, Identification of dy-namic parameters and feedforward control for a con-ventional industrial manipulator, Control Eng Prac-

Ž .tice 2 1994 , 1039�1050.9. M. Gautier and W. Khalil, Exciting trajectories for the

identification of base inertial parameters of robots, IntŽ .J Robotics Res 11 1992 , 362�375.

10. G.W. Van der Linden and A.J.J. Van der Weiden,Practical rigid body parameter estimation, Proc 4thIFAC Symp on Robot Control, 1994, pp. 631�636.

11. J. Swevers, C. Ganseman, D.B. Tukel, J. de Schutter,H. Van Brussel, Optimal robot excitation and identifi-

Ž .cation, IEEE Trans Robotics Automation 13 1997 ,730�740.

12. D.E. Goldberg, Genetic algorithms in search, opti-mization, and machine learning, Addison-Wesley,Reading, MA, 1989.

13. Z. Michalewicz, Genetic algorithms�data structures�evolution programs, Springer-Verlag, New York,1992.

14. M.W. Chen and A.M.S. Zalzala, Dynamic modellingand genetic-based trajectory generation for non-holo-nomic mobile manipulators, Control Eng Practice 5Ž .1997 , 39�48.

15. H. Zhuang, J. Wu, and W. Huang, Optimal planningof robot calibration experiments by genetic algo-

Ž .rithms, J Robotic Syst 14 1997 , 741�752.16. B. Armstrong-Helouvry, P.E. Dupont, and C. Canudas´

de Wit, A survey of models, analysis tools and com-pensation methods for the control of machines with

Ž .friction, Automatica 30 1994 , 1083�1138.17. S.-Y. Sheu and M.W. Walker, Basis sets for manipula-

tor inertial parameters, Proc IEEE Int Conf on Roboticsand Automation, 1989, pp. 1517�1522.

18. T. Kailath, Lectures on Wiener and Kalman filtering,Springer-Verlag, New York, 1981.

19. L. Ljung, System identification, theory for the user,Prentice-Hall, Englewood Cliffs, NJ, 1987.

20. G.H. Golub and C.F. Van Loan, Matrix computations,The Johns Hopkins University Press, Baltimore, 1989.

21. G. Calafiore and M. Indri, Experiment design forrobot dynamic calibration, Proc 1998 IEEE Int Conf onRobotics and Automation, 1998, pp. 3303�3309.

22. A. Visioli and G. Legnani, Experiments on modelidentification and control of an industrial robot ma-nipulator, Proc 6th IFAC Symp on Robot ControlŽ .SYROCO’00 , 2000, Vol. 1, pp. 283�288.

23. S. Nicosia, A. Tornambe, and P. Valigi, Experimental`results in state estimation of industrial robots, Proc29th IEEE Conf on Decision and Control 1990, pp.360�365.


24. H. Berghuis and H. Nijmeijer, Robust control of robotsvia linear estimated state feedback, IEEE Trans Au-

Ž .tomat Control 39 1994 , 2159�2162.25. H. Berghuis and H. Nijmeijer, Global regulation of

robots using only position measurements, Syst Con-Ž .trol Lett 21 1993 , 289�293.

26. R. Kelly, A simple set-point robot controller by usingonly position measurements, Proc 12th IFAC WorldCongress, 1993, Vol. 6, pp. 173�176.

27. M.F. Khelfi, M. Zasadzinski, H. Rafaralahy, E. Richard,and M. Darouach, Reduced-order observer-based

point-to-point and trajectory controllers for robot ma-Ž .nipulators, Control Eng Practice 4 1996 , 991�1000.

28. P.R. Belanger, X. Meng, and P. Dobrovolny, Estima-tion of angular velocity and acceleration fromequally-spaced shaft encoder measurements, IEEE IntConf on Robotics and Automation, Nice, 1992.

29. B. Bona and M. Indri, Identification of the dynamicparameters of a planar manipulator: Simulations andexperiments, Proc 2nd World Automation CongressŽ .WAC 96 , Montpellier, France, 1996.

Documents

Robot Dynamic Calibration: Optimal Excitation Trajectories ... · Advanced robot control schemes require an accurate knowledge of the dynamic ... is solved here using a genetic algorithm