Professional Documents
Culture Documents
Direct Estimate of Closed-Form Dynamic Equation Parameters of A Robot: Tools and Methods For Designing Experiments
Direct Estimate of Closed-Form Dynamic Equation Parameters of A Robot: Tools and Methods For Designing Experiments
(
(
(
(
1
2
(6)
computed at N random position, velocity and accelera-
tion points.
The dynamic equation of each joint is complete
with the addition of friction torque term. We consider
the friction in the form:
i f i i i i d
q sign q t u u
u
= + ( ) (7)
where
i
u
u
and
i d
u are the viscous and dynamic fric-
tion coefficients of the joint i. This friction model does
not take in consideration the stiction process and some
other components that could be or not, depending on
devices that we are considering in our experimental
setup. One of these is the position dependent friction
torque in harmonic drive gear
[6]
. All these approxima-
tion are to be considered in error analysis.
Phase II: Optimization of the Condition Number of
Regressors Matrix
It has been shown
[3],[4]
the importance of low condition
number of the regressor matrix
i
u for increasing noise
immunity of the estimation procedure. The first step to
obtain good exciting trajectories is similar to proce-
dure followed for the energy model-based
[4]
except for
inclusion of acceleration trajectories. A set of position,
velocity and acceleration points (OPVA) has been
computed as solution of non-linear optimization prob-
lem. The second step is to interpolate OPVA points to
obtain smooth and continuous position, velocity and
acceleration exciting trajectories. We consider an
OPVA point as a triple of vectors (q(t),q(t),q(t) ) at the
same time t. The optimum points of position, velocity
and acceleration are computed solving the following
non-linear constrained optimization problem:
min ( )
; ; ;
q,q,q
mo Mo Mo Mo
q q q q q q q
Cond
i
u
s s s s
(8)
where q
mo
, q
Mo
, q
Mo
, q
Mo
are the vectors of joint po-
sition, velocity and acceleration limits for the optimi-
zation problem. Their values must be smaller than the
actual kinematics bounds of the robots. If the con-
straints are too close to physical limits, the interpolat-
ing trajectories between the OPVA points will exceed
more easily allowed values of position, velocity and
acceleration.
The constrained optimization problem can be
transformed into another one without constraints, con-
sidering the auxiliary vectors x x x
q q q
, with the fol-
lowing changes of variables:
q q q q x
q q x
q q x
= +
=
=
m M M q
M q
M q
sin
sin
sin
( ) ( );
( );
( );
2
(9)
Computation times depend on the complexity of
the axis dynamic equation. We can simplify the ex-
pressions of the regressor functions reducing the num-
ber of the kinematic variables, when this does not im-
ply loss of identifiability. Practically the situation cor-
responds to block one or more joints but, of course,
that one involved in the identification procedure. Nev-
ertheless, it must be stressed that not always we have a
lower condition number of the regressor matrix as a
result of the complexity reduction. Another factor af-
fecting the length of computation times is the number
of regressors function constituting the equation of
each axis. According with analogous procedures
[4]
, in-
to the optimization algorithm we must consider a
number of OPVA points equal, at least, to the double
of the unknown cfe parameters. Hence, the dimension
of the regressor matrix is 2mm, with m equal to the
numbers of unknown parameters into the axis dynamic
equation.
In our experimental setup the identification has
been performed only on the first three joints of the ro-
bot. Obtained condition numbers are 64 for identifica-
tion procedure associated with the axis 1 dynamic
equations (30 OPVA points, no braked joint), 46 for
axis 2 (32 OPVA points, no braked joint), 26.5 for axis
3 (18 OPVA points, joint 2 braked). They requires
from 1530 minutes of CPU time, using routine
FMINU of MATLAB OPTIMIZATION TOOLBOX
over a PC 486, 66 MHz.
Phase III: Interpolation of Exciting Trajectories
A seventh order polynomial is used to interpolate op-
timum position points. Its first and second derivatives
are used for interpolating optimum velocities and ac-
celerations points. For reasons given in a previous
work
[7]
, this choice allows to obtain smoother and
lower-rate acceleration trajectories. This ensures high
repetitivity in acceleration trajectories too, further re-
ducing the variation of kinematic variables and, as a
consequence, perturbations of regressors matrix.
To this aim we extend the general criteria used by
Gautier-Khalil
[4]
with restrictions on the third deriva-
tive of the position (jerk). The following notations are
used:
- u
f(i)
is the time interval between two consecutive
OPVA points, (q,q,q )
a(i)
and (q,q,q )
b(i)
:
u t t
f(i) b(i) a(i)
= ;
- u
(i)
is the relative time on interval (i):
u t t u
(i) a(i) f(i)
[ , ] = e 0
-
The interpolating polynomials for every joint in
any interval (omitting subscripts i and (i) ) are:
q u a a u a u a u a u a u a u a u ( ) = + + + + + + +
0 1 2
2
3
3
4
4
5
5
6
6
7
7
( ) q u a a u a u a u a u a u a u = + + + + + +
1 2 3
2
4
3
5
4
6
5
7
6
2 3 4 5 6 7
( ) q u a a u a u a u a u a u = + + + + + 2 6 12 20 30 42
2 3 4
2
5
3
6
4
7
5
(10)
The coefficients a
k
are computed to satisfy the
following condition:
q q q u q
q q q u q
q q q u q
( ( )
( ( )
( ( )
a f b
a f b
a f b
0)
0)
0)
= =
= =
= =
(11)
For the jerk we impose:
a(i) b(i 1)
q q =
( ) [ ( ) ]
[ , ]
b(i)
q sign RND q q q RND
RND
m M m
= +
e 11
(12)
( ( )
a f b
q q q u q 0) = =
where RND is a normally distributed random number
and q
m
, q
M
can be tuned to avoid too fast rate and to
facilitate trajectory interpolation.
Phase IV: Acquisition and Pretreatment of Data,
Parameter Estimation
For lack of acceleration and velocity sensors on robot
actuators in our experimental setup, we have used es-
timation algorithms based on LS techniques
[8]
to eval-
uate actual acceleration and velocity trajectories start-
ing from position acquisitions. Butterworth filters are
used for the pretreatment of the data, especially in or-
der to polish current acquisitions from high frequen-
cies disturbances (Fig. 2). Fig. 3 shows a detail of
torque, acceleration, velocity and position filtered tra-
jectories for ten acquisitions. Unfortunately, lower
frequency disturbances do not allow good torque fil-
tering. As a consequence, high perturbations due to
the torque vector can occur in LS estimation if the
number of OPVA points is equal to the double of the
Fig. 2
N w
3 0 3 2 3 4 3 6 3 8 4 0
- 2 0 0
- 1 0 0
0
1 0 0
2 0 0
[ N w m ]
O v e r l a p o f f i l t e r e d t r a j e c t o r i e s ( 1 0 a c q u i s i t i o n s , a x i s 1 )
T o r q u e
3 0 3 2 3 4 3 6 3 8 4 0
- 2
- 1
0
1
2
[ r a d / s e c ]
A c c e l e r a t i o n
3 0 3 2 3 4 3 6 3 8 4 0
- 2
- 1
0
1
2
[ r a d / s]
3 0 3 2 3 4 3 6 3 8 4 0
- 2
0
2
4
T im e [ s]
[ ra d ]
V e lo c it y
P o sit io n
Max. variations in trajectories (evaluated for 10 acquisitions)
torque 13 Nwm
acceleration 0.0012 rad/s
2
velocity 210
-4
rad/s
position 210
-5
rad
Fig. 3
unknown parameters. We need much more points to
have more reliable results. For this reason an exten-
sion of regressor matrix is made, considering the in-
tervals around each OPVA point on the exciting trajec-
tories. The following notations are used:
- z is the vector of the sample times corresponding
to OPVA vector on exciting trajectories:
Time [s]
0 5 10 15 20 25 30 35 40
-200
0
200
0 5 10 15 20 25 30 35 40
-200
0
200
[Nwm]
Comparison between actual and filtered torque (axis 1)
z = [ , , , ] z z z
N 1 2
N is the number of OPVA points.
- Az
a
is the vector of interval widths to the left of
OPVA points.
- Az
b
is the vector of interval widths to the right of
OPVA points.
-
With these notations OPVA regressor matrix is:
i
i
i
i N
z
z
z
u
z
=
(
(
(
( )
( )
( )
1
2
(13)
and the extended regressor matrix can be expressed in
the form:
i e
i
i
i
a
a
b
u
u
u
u
u
u
=
(
(
(
(
(
(
(
+
z z
z z
z
z+1
z z
A
A
A
1
(14)
It is possible to find
i e
u with low condition num-
ber solving another optimization problem:
min ( )
,
A A
u
z z
a b
cond
i e
(15)
The resulting piece-wise identification trajectories
consist of thousands of points and strongly reduce the
influence of torque perturbations on the estimation
problem solution. We have obtained piece-wise trajec-
tories of 3070 points for axis 1 estimation algorithm,
3280 points for axis 2 and 1850 for axis 3. Variation
of the regressor matrix condition numbers goes re-
spectively, from 64 to 66.3, from 46 to 48.5 and from
26.5 to 29. In spite of this little increase of the condi-
tion numbers, the upper bound of the estimation meth-
od error
[4][4]
strongly decrease. The worst case corre-
spond to axis 1 identification procedure: the upper
bound goes from 189% for the estimation with 30
OPVA points to 65% for the estimation with piece-
wise trajectories.
MATLAB routines are used to perform estimation
and validation of the dynamic model. Trajectories ob-
tained by interpolation of random position, velocity
and acceleration points are used for the dynamic mod-
el cross-validation. Fig. 5 shows the comparison be-
tween actual (filtered) and estimated torque, resulting
from the execution of this kind of trajectories. The lit-
tle discontinuities on the curve of the estimated torque
come out from the sign variation of the friction torque
contribution, as in (7). It does not appear in actual
torque because of filtering.
Estimated values of the parameters are in Table 1,
together with their general expression and standard
deviations. For the parameters which are in more than
one axis dynamic equation the best estimation is con-
sidered.
Fig. 4
0 5 10 15 20 25 30
-200
0
200
Time [s]
[Nwm]
Torque cross validation (axis 1): actual, - - - estimated
Fig. 5
EXPERIMENTAL SETUP
The parameter estimation has been performed on the
six-degree-of-freedom manipulator COMAU SMART
3S with C3G 9000 controller, but only the first three
joints are considered in our identification procedure.
The arm is equipped with a brushless motor on each
axis, shaft absolute resolver as position sensor, har-
monic drive gears. The open version of C3G 9000
0 5 10 15 20 25 30 35 40
-200
-100
0
100
200
Torque: actual (filtered) piece-wise, o OPVA relating points (axis 1)
[Nwm]
0 5 10 15 20 25 30 35 40
-200
-100
0
100
200
Torque: actual (filtered), - - - estimated (axis 1)
Time [s]
Paramet
~
u
1
0.3(
~
u
2
2(J
xy
~
u
3
0.3c
~
u
4
1.22
~
u
5
2(J
xy
~
u
6
0.3c
~
u
7
J
xx2
-
~
u
8
J
xx3
-
~
u
9
J
yz2
-
~
u
10
J
yz3
-
~
u
11
J
xz2
-
~
u
12
J
xz3
-
~
u
13
(J
xx2
+2c
y
+0.4
+2 c
~
u
14
J
zz2
+
+c
y3
~
u
15
J
zz3
+
1
~
u
v
1
~
u
d
2
~
u
v
2
~
u
d
3
~
u
v
3
~
u
d
makes po
MHz CPU
tion is re
Adapter B
memory
10 ms a
makes po
exchange
ble to ac
points for
the motor
gy, on m
constants
joint torq
Robot C
ter obtained from
(LS e
(c
x2
m
2
+0.61m
3
)
y2
-c
x2
c
y2
m
2
)
c
x3
m
3
2c
y3
m
3
y3
-c
x3
c
y3
m
3
)
c
y2
m
2
-J
yy2
-c
x2
2
m
2
+c
y2
2
m
-J
yy3
-c
x3
2
m
3
+c
y3
2
m
-c
y2
c
z2
m
2
-c
y3
c
z3
m
3
-c
x2
c
z2
m
2
-0.61c
z3
m
-c
x3
c
z3
m
3
2
+J
xx3
+J
yy2
+J
yy3
+2
y1
2
m
1
+0.045m
2
+c
417m
3
+c
x3
2
m3+c
y
c
z3
2
m
3
)/2
+J
zz3
+c
x2
2
m
2
+c
y2
2
m
3
2
m
3
+c
x3
2
m3+c
y3
2
m
3
ossible the con
U with the co
ealized with a
Board and da
physically loc
an interrupt
ossible the sy
e of data can o
cquire shaft m
r each joint w
rs are controll
multiplying cu
s and gear re
ques. In doing
COMAU SMART
Fig. 6
m identifications o
estimation)
m
2
-0.3721m
3
m
3
m
3
2J
zz1
+2c
x1
2
m
1
c
x2
2
m
2
+c
y2
2
m
2
+2c
y3
2
m
3
m
2
+0.3721m
3
+c
x3
Table 1
nnection of a
ontroller VME
a pair of BIT3
ata exchange
cated on the
signal genera
ynchronization
occur. In such
motor position
with a sample
led with orien
urrent values
duction ratios
g so we intro
T 3S
on axis 1, 2, 3.
10.1
-6.3
7.0
6.33
2.1
8.5
-25
-1.2
-0.6
-0.115
-2.5
-0.36
z2
2
m
2
34.4
3
2
m3 46.3
10.7
82.0
10.5
85.0
12.3
73.4
8.0
PC with a 48
E bus. The con
3 Communica
occurs in a sh
robot CPU. E
ated by cont
n with the PC
h a way it is p
ns and curren
time of 10 m
nted-field tech
for motor to
s we can eva
duce two app
std.
dev
.
0.3
0.2
0.3
0.04
0.1
0.5
0.4
0.3
0.1
0.05
0.1
0.05
0.5
0.3
0.7
0.3
0.3
0.5
0.5
0.2
0.2
86-66
nnec-
ations
hared
Every
troller
C and
possi-
nt set
ms. As
hnolo-
orque
aluate
proxi-
ma
lar
ren
rat
An
clo
ula
go
tio
me
ter
of
co
tin
fer
to
[1]
[2]
[3]
[4]
[5]
[6]
[7]
[8]
ations: the ba
rge enough to
nt values are f
tion region of
n identificatio
osed-form dyn
ator has been
ood results fo
oned, even in
easurements, t
rs for the data
f regressor ma
mputation tim
nes. However,
rent arms and
confirm the q
] J.J.Slotine,
nipulator I
64, 1985
] F.Caccavale
Identificatio
ventional In
Fourth IFA
ly, pp.619-6
] B.Armstron
for Identifi
with Nonlin
vol.8, no.6,
] M.Gautier
for the Iden
of Robots,
pp.362-375
] L.Ljung, S
User, Pren
1987
] S.C.P.Gome
ling and Fri
Manipulato
Robot. and
1992
] F.Milella,
Equation P
tor, Proc. o
Manufactur
] R.H.Brown
Analysis o
from Discre
Trans. on
1992
andwidth of
o follow curren
far from their
the motors.
CONCLU
on framework
namic equatio
introduced in
r the experim
the presence
thanks to an a
a pretreatment
atrices are easi
me using MA
, further tests
with differen
quality of the i
REFERE
The Robust
Int. J. Robotics
e and P.Chi
on of Dynami
ndustrial Mani
C Symp. on R
624, 1994
ng, On Findi
cation Experi
near Dynamic
pp.28-48, 198
and W.Khali
ntification of
, Int. J. of Ro
, 1992
System Identif
ntice Hall, Inc
es and J.P.Ch
iction Compen
r Joint, Proc
Autom., Nice
Estimation o
arameters on
of IASTED Int
ring, Cancn
, S.C.Schn
of Algorithms
ete Position V
Ind. Electr.,
the power a
ent set-points
r limits, i.e. fa
USION
k for the para
on of an indus
n this paper.
mental setup
e of high nois
accurate desig
t. Low conditi
ily obtained w
ATLAB optim
might be do
nt experimenta
implemented m
ENCES
t Control of
cs Res., vol.4,
iacchio, En
mic Parameters
ipulator, Prep
Robot Control
ing Exciting
iments Involv
cs, Int. J. Ro
89
il, Exciting
Base Inertial
obotics Res., v
ification: The
c., Englewood
hrtien, Dyna
nsated Contro
c. of IEEE In
e, France, pp
of Closed-For
n an Industria
t. Conf. on R
n, Mexico, 199
neider, M.
s for Velocity
Versus Time D
vol.39, no.1
amplifiers is
and the cur-
ar from satu-
ameters of a
strial manip-
It has given
above men-
se in torque
gn of the fil-
ion numbers
with no high
mization rou-
ne over dif-
al conditions
methods.
Robots Ma-
no.1, pp.49-
ergy Based
s for a Con-
eprints of the
l, Capri, Ita-
Trajectories
ving System
obotics Res.,
Trajectories
l Parameters
vol.11, no.4,
eory for the
d Cliffs, NJ,
amic Model-
ol of a Robot
nt. Conf. on
p.1429-1435,
rm Dynamic
al Manipula-
Robotics and
95
G.Mulligan,
y Estimation
Data, IEEE
1, pp.11-19,