Documente Academic
Documente Profesional
Documente Cultură
-
D 2 3 = m3[(a x + a a ) c + a Z s +
2 3 2 3 3 2 3 3
- L
2a3x3 + a3 + k L 1
3YY
3 3 2
D
112
= m ( k
2k2yy
2xx
- - a'
2
- 2 a x )c s +
2 2 2 2
3 3 ( c s+ c s - 2 s s s ) +
m3[k3xx 2 2 3 3 2 3 23
2
3 3 k3zz(2s2s3s23 - c2s2 - c3s3) +
-
~ ( - 2 a c +s 4 a s s s +
3 2 2 2 3 3 2 3 2 3
The e x p l i c i tf o r mo fd y n a m i cp r o j e c t i o nf u n c -
t i o n s , D i , D i j a n dD i ' k ,i n c l u d i n gt h ev a l u e so ft h e a s- 2 a c s- 2 a c s ) +
2 3 3 2 2 3 3 3
g e o m e t r i ca n di n e r t i a ?p a r a m e t e r si nt h e s ef u n c t i o n s
f o rt h e PUMA 560 a r el i s t e db e l o w .N o t et h a t D i -
i s t h ei n e r t i a ll o a dp r o j e c t i o nf u n c t i o nt oj o i n 2 z (a c c
3 2 2 23 - a2S2S23
+ 2a c 2
3 23
- a,) +
r e l a t e dt oa c c e l e r a t i o na tj o i n t "j", Dijk(q) i s
c e n t r i p e t a l( j = k )o rC o r i o l i s( j # k )f o r c ep r o j e c t i o n
f u n c t i o n t o j o i n t "i" r e l a t e d t o v e l o c i t i e s a n d j o i n t s a2a3s3 - 2a a c s
2 3 2 23 - 2
a2C2S2
+
I 1j l l and l l k l l , a n dD i ( q ) i s t h e g r a v i t y l o a d a t j o i n t
"i". The g e n e r a lf u n c t i o nd e f i n i t i o n so ft h e Di, Dij 2 2
and D i j k d y n a m i cp r o j e c t i o nf u n c t i o n sc a nb ef o u n d In 2a s s s
3 2 3 2 3
- a ( C s + c3s3)l
3 2 2
[ 9 and l o ] . The s p e c i f i cf o r mo ft h e s ef u n c t i o n sf o r
t h e PUMA 560 r o b o t arm w i t h i t s m o t i o n r e s t r i c t e d t o
t h ef i r s tt h r e ej o i n t s i s a sf o l l o w s ,q u o t e df r o m [11
a n d1 2 ] : * ) D = m [ k( 2c s + c s - 2 s s s ) +
113 3 3xx 2 2 3 3 2 3 23
D =mkL +
11 1 lyy k2 ( 2 s s s - c s - s c ) +
3zz 2 3 2 3 2 2 3 3
x -
m 2 ( k i x x s i + k;yyci + a i c i + 2a 2 2 c 2 + x ( 4 a3 s 2s 3s 2 3 - 2 a ~ - ~2 a c s
3 2 3 3 3 3
-
- 2
a c s ) + z3(2a3cZ3 + a c c
2 2 23 2 2 2 3
- a )
3
+
2 2 2
2 a a c c
2 3 2 2 3
+ 2 x ( a c c
3 2 2 2 3
+ a c 2 ) +
323 2a3s2s3sZ3 - a 2 a 3 c 2 s Z 3 - a3c2s2 - a c s 1
3 3 3
2Y3d3 + 2 z 3 ( a 3 c Z 3 s z 3 + a2c2sZ3)1
D122 = m a Z c +
2 2 2 2
- -
D12=maFs + m [d s + (d3x3 + a3y3 + a d ) c 1
2 2 2 2 3 3 3 2 3 3 3 23
m [ ( d+ xa d ) s + -
3 3 3 + a3Y3 3 3 23 D = m [d s + (d3y3 + a3y3 + a3d3)cZ31
123 3 3 3 23
(a2y3 + a2d3)s2 - d3 z3 c2 3 1
-
d3z3sZ3 + (d3x3
- +
-
a3y3 + a d ) c 1
3 3 23
- *
- D133 = m3[
D13 = m3 [ (y3d3 + a3y3 a d )s
3 3 23 - Z3d3C231
DZl3 = 0 ( b e c a u s eo fg e n e r a l PUMA 560 geometry)
* ) N o t e t h a t we u s e t h e f o l l o w i n g s h o r t h a n d n o t a t i o n -
s i = sin ( q i ) , s ' . = sin ( q i + q j ) D223 = m3'
(-a2X3 - a2a3)s3 + a z c 1
2 3 3
c i = c o s ( q i ) , C f ! = c o s ':qi + q .)
11 3
1681
-
D~~~ = m3[ (-a2x3- a2 a31 s3 + a2Z3 c 3 1 Table 1. PUMA 560 Geometric Parameters
(For first three joints/links.)
Dl = 0
1 1 1 I 1 r:) I
D2 = m g(x2 + a ) c
2 2 2
-
17.N 0.75 5.925 17.05
m 3 g(X c
3 23 323 + a3C23 + a2C2)
+ 43.18 1.91 15.05 43.31
-
D3 = -yg(x3cz3 + z3sZ3 + a3cZ3)
Diji = 0 for
i 1. j (6)
REFLECTED
MOTOR
CENTER OF M A S S MASS RADIUS OF GYRATION INERTIA
LINK - - -
i zi(cm) g.s2/cm xi(cm)
yi(cm) k2xx.(cm2) (cm2)
k2 k2zz.(cm2)
I YYi I Iai(g.cm.s21
1 0 3.89 30.88
13.21 1816.3 151.93 1811.13 7916.66
1682
x2
x3
x4
x5 a3c1c23 + d4c1s23 + a2C1C2 - d3S1
6' a s c+ d s s
3 1 23 4 1 23 + a2S1C2 + d3C1
~3 3 I w = a(x) + E(X)V
DRIVE TRAIN - JOINT 2 DRIVE TRAIN - JOINT 3 DRIVE TRAIN - JTS 4,5,6
J O l h T 5 BEVEL
JOlhl615~1
J O I N T 515501
- 1
where
1
~ 0 1 0 0 0 0
where Jh is theJacobian matrix of h(21,22,23), 0 0 0 0 0 0
D ( % ) and p(x) are defined previously, and where we
introduced the following notations: z3! 0 0 0 1 0 0
z =
0 0 0 0 0 0
0 0 0 0 0 1
z5
6
' I
' A =
~ 0 0 0 0 0
i 0
lX61
with
---
a J l l a J 1 2 aJ13
axi axi axi
-aJh
= ---
a J 2 1 a J 2 2 aJ23
i = 1, 2, 3
B =
axi
here a more direct proof. We first find the required denotes the differential of the C" mapping T,
(a, 6, T) via the algorithm proposedin Chen's an; " o " denotes the composition of mappings.) The
dissertation [ 6 1 : verification is done by carrying out the following
straightforward computations:
and
dhl
dLfhl
dh2
dLfh2
dh3
dLfh 3
1684
i2h
f l 1 L2h
f l 1 where r 1
0 1 y = cz (17)
i o 0 Thus, the new system (L 6 D block in Figure 3 ) can be
1 considered to be three independent subsystems. Each
LO O has the following secondorder form:
Iz2i-11
,-. yi = [ 1 01 1 for i = 1,2,3
So both < = T,(gB) o T
-1
and h = h 0 T
-1
hold.
more details on this direct proof see [ 1 4 ] .
For Lz2i J
(19)
Thus, the nonlinear feedback and diffeomorphic It is noted that the damping ratio5 and natural fre-
transformation, Eqs. ( 1 0 and 111, apply to the PUMA quency % of this second order system are s o that
560 robot arm, and convert the nonlinear dynamic sys- ,'u = fil, 2cwn = fi2.
tem, Eq. ( 8 ) , with task, Eq. ( 9 1 , into the Brunovsky
canonical form, Eq. (12), with simultaneous output
decoupling, Eq. (13). We can consider Eqs. (18-19) as the new mathematical
model of the realsystem which is now externally (or
As a matter of fact, the system described by Eqs. exactly) linearized, output decoupled and stabilized. Let
(12 and 13) consists of three (i=1,2,3) independent the output errorei for each decoupled subsystem be
subsystems of the following form denoted as follows:
(14)
(20)
yi = [ 1 01 z i (15)
1685
Figure 3 . New Dynamic Control Method: Nonlinear Feedback with Optimal Error
Correcting Loop
where superscript "d" indicates "desired"or model From the well-known optimal linear control theory
quantities; the same variables without superscript "d" the optimal correction is
are the real (or measured) variables.
-1
Avi = -R biP(t)ei(t), where
From Eqs. (18-20) we then have:
r -l
1 1 + f12 ($.-$.)
A(vi-vf) = (y.-yd) 1 d 1 + fil(yi-yi).
d
p 1 2 p221
That is is a positive solution of the Riccati equation:
v. = bi2 + Fi2ei2 + fileil P(t) = -P(t)Ai - AiP(t) + P(t)biR-'biP(t) - Q
or with P(T) = S.
We consiaer the steady state (t + m) solution o t
the Riccati equation. This gives:
J(AV~) = jo AviRbvidt +
It contains three major steps.
6. Computer Simulations
1688