Upload
pako-alb
View
225
Download
4
Embed Size (px)
DESCRIPTION
Dinamica de robots
Citation preview
2. Modelos Clasicos
Dr. Martn Velasco Villa
Seccion de Mecatronica
Marzo de 2015
Manipulador Cartesiano
Coordenadas generalizadas: q1, q2
Fuerzas aplicadas: f1, f2
Masas de los eslabones: m1, m2
Energa cinetica: k = 12 qTD (q) q
Energa potencial: P = P (q)
Matriz de Inercia:
D (q) =
[m
i=1
{miJ
Tvi(q) Jvi (q) + J
Twi
(q)Ri (q) Iii R
Ti (q) Jwi (q)
}]
I Como solo se tienen uniones prismaticas: Jwi = 0
Parametros DH
Parametros D HEslabon a1 i di i
1 0 90 d1 02 0 0 d2 0
Relaciones basicas para la obtencion del Jacobiano
Recuerdese quev0n = Jv q, w
0n = Jw q
donde el Jacobiano del manipulador se obtiene a partir de
J (q) =
[JvJw
]=
[Jv1 Jv2Jw1 Jw2
]=
[Z0 Z10 0
]con Jv , Jw R3n.A partir de la cinematica del manipulador Jv y Jw se obtienen considerando:
Jvi =
{Zi1 (On Oi1) Uniones RotacionalesZi1 Uniones Prismaticas
y
Jwi =
{Zi1 Uniones Rotacionales0 Uniones Prismaticas
donde,
Z0i1 = R0i1k, k =
[0 0 1
]T.
Relaciones basicas para la obtencion del Jacobiano
Recuerdese que el Jacobiano del manipulador se encuentra a traves de las siguientesrelaciones,
R ij = Rii+1 . . .R
jij
O ij = Oij1 + R
ij1O
j1j .
Las transformaciones homogeneas Ai , Tij estan dadas por,
Ai =
[R i1i O
i1i
0 1
], T ij
que se relacionan entre si mediante,
T ij = Ai+1Ai+2 . . .Aj1Aj =[
R ij Oij
0 1
], (i < j) .
En el caso particular del manipulador prismatico,
T 01 = A1, T02 = A1A2
A1 =
[R01 O
01
0 1
], A2 =
[R12 O
12
0 1
]R02 = R
01R
12 .
Las matrices Ai tienen la forma general,
Ai =
Ci SiCi Si Si aiCiSi CiCi Ci Si aiSi0 Si Ci di0 0 0 1
.A partir de los parametros D H se obtiene directamente,
A1 =
1 0 0 00 0 1 00 1 0 d10 0 0 1
, A2 =
1 0 0 00 1 0 00 0 1 d20 0 0 1
,con lo cual,
A1A2 =
1 0 0 00 0 1 d20 1 0 d10 0 0 1
.
De las matrices, A1, A1A2 se obtienen directamente las posiciones de los orgenes conrespecto al marco inercial,
O0 =
000
, O1 = 00
d1
, O2 = 0d2
d1
.El Jacobiano que relaciona la velocidad del efector final con las velocidades de lasarticulaciones se obtiene entonces al considerar,
J (q) =
[Z0 Z10 0
],
donde
Z0 =
001
, Z1 = R01Z0 = 1 0 00 0 1
0 1 0
001
= 01
0
.
De los desarrollos anteriores,
J (q) =
[JvJw
], Jw = 0 Jv =
0 00 11 0
,esto es, la velocidad del efector final resulta,
v0n (t) =
0 00 11 0
[ q1q2
].
Observacion. El Jacobiano anterior se obtiene tambien facilmente al considerardirectamente la evolucion temporal del efector final dada por el punto P0n .
P0n =
0d2d1
, Pn (t) = P0nq
q =
0 00 11 0
[ q1q2
],
donde q1 = d1 y q2 = d2.
Modelo Dinamico
Para obtener el modelo dinamico del manipulador se tienen que considerar losjacobianos relacionados con lo centros de masa de cada uno de los eslabones, esto es,se requieren relaciones de la forma,
vc1 = Jvc1 q, vc2 = Jvc2 q
donde en este caso,
Jvc1 =[
Z0 0]=
0 00 01 0
, Jvc2 = [ Z0 Z1 ] = 0 00 1
1 0
.De lo anterior, la energa cinetica se obtiene en la forma,
k =1
2qT{m1J
Tvc1
Jvc1 +m2JTvc2
Jvc2
}q
=1
2qT{m1
[1 00 0
]+m2
[1 00 1
]}q
=1
2qT[
m1 +m2 00 m2
]q
=1
2qTDq.
La energa potencial del i-esimo eslabon se obtiene como,
Pi = migT rci
I mi : Masa del i-esimo eslabon.
I gi : Vector de gravedad.
I rci : Coordenadas del centro de masa.
Entonces,
P =n
i=1
migT rci
P = m1[
0 0 g] 00
d1
+m1 [ 0 0 g ] 0d2
d1
= m1gd1 +m2gd1
= g (m1 +m2) q1.
I Como la matriz de inercia D es constante, los smbolos de Christoffel son cero.
Los componentes gk , generados por el vector de gravedad resultan
g1 =P
q1= g (m1 +m2) , g2 =
P
q2= 0.
Finalmente, el modelo dinamico tiene la forma general
D (q) q + C (q, q) q + g (q) =
Equivalentemente,[m1 +m2 0
0 m2
]q +
[g (m1 +m2)
0
]=
[f1f2
]esto es,
(m1 +m2) q1 + g (m1 +m2) = f1
m2q2 = f2.
Manipulador Planar Rotacional
Considere un manipulador de la forma,
Parametros D HEslabon ai i di i
1 a1 0 0 1
2 a2 0 0 2
Con lo cual el Jacobiano relacionado con el efector final se obtiene a partir de
J (q) =
[JvJw
]=
[Jv1 Jv2Jw1 Jw2
]=
[Z0 (O2 O0) Z1 (O2 O1)
Z0 Z1
]
Recuerdese que la velocidad angular del i-esimo eslabon w i1i esta dada por larotacion del i-esimo eslabon, expresada relativa al marco Oi1xi1yi1zi1. En elmarco i 1 esta velocidad se expresa localmente como
w i1i = qiZi1i1 = qik
donde k es el vector unitario k =[
0 0 1]T
.Por lo tanto, considerando que
Z0i1 = R0i1k, k =
[0 0 1
]T.
la velocidad angular total del efector final se obtiene como
w0n =n
i=1
i qiz0i1 = 1q1k + 2q2R
01k + + nqnR0n1k
con i = 1 si la union es rotacional y i = 0 si es prismatica.
Ademas se tiene que,
R ij = Rii+j R
j1j , O
ij = O
ij1 + R
ij1O
j1j , (i < j)
donde las transformaciones homogeneas Ai , Tij se obtienen como,
Ai =
[R i1i O
i1i
0 1
]T ij = Ai+1Ai+2 . . .Aj =
[R ij O
ij
0 1
], (i < j)
En nuestro caso particular,T 01 = A1, T
02 = A1A2
Ai =
Ci SiCi Si Si aiCiSi CiCi Ci Si aiSi0 Si Ci di0 0 0 1
.
Entonces,
A1 =
C1 S1 0 a1C1S1 C1 0 a1S10 0 1 00 0 0 1
= [ R01 O010 1]= T 01
R01 =
C1 S1 0S1 C1 00 0 1
, O01 = a1C1a1S1
0
,y
A2 =
C2 S2 0 a2C2S2 C2 0 a2S20 0 1 00 0 0 1
= [ R12 O120 1]= T 12
R12 =
C2 S2 0S2 C2 00 0 1
, O12 = a2C2a2S2
0
.
Por lo tanto,
A1A2 =
[R01R
12 O
01 + R
01O
12
0 1
]=
C12 S12 0 a1C1 + a2C12S12 C12 0 a1S1 + a2S120 0 1 00 0 0 1
= T 02 .Dado que,
J (q) =
[Z0 (O2 O0) Z1 (O2 O1)
Z0 Z1
],
Entonces,
Z0 =
001
, Z1 = R01k = R01 00
1
= 00
1
O0 =
000
, O1 = a1C1a1S1
0
, O2 = a1C1 + a2C12a1S1 + a2S12
0
.
Notese tambien que de manera alterna,
O02 = O01 + R
01O
12 ,
entonces,
O02 =
a1C1a1S10
+ C1 S1 0S1 C1 0
0 0 1
a2C2a2S20
=
a1C1a1S10
+ a2 (C1C2 S1S2)a2 (S1C2 + C1S2)
0
=
a1C1 + a2C12a1S1 + a2S120
.
i j k0 0 1m1 m2 m3
= i 0 1m2 m3
j 0 1m1 m3+ k 0 0m1 m2
= m2i +m1j + 0k
De lo anterior,
Z0 (O2 O0) =
001
a1C1 + a2C12a1S1 + a2S12
0
= a1S1 a2S12a1C1 + a2C12
0
Z0 (O2 O1) =
001
a1C1 + a2C12 a1C1a1S1 + a2S12 a1S1
0
=
001
a2C12a2S12
0
= a2S12a2C12
0
.
El Jacobiano toma entonces la forma = J (q) q,
J (q) =
a1S1 + a2S12 a2S12a1C1 + a2C12 a2C12
0 00 00 01 1
, =
vxvyvzwxwywz
, q =[
q1q2
]
Ecuaciones dinamicas o de movimiento
li = a1 l2 = a2q1 = 1 q2 = 2
I Recuerdese que las ecuaciones de movimiento se obtienen considerando lasenergas cinetica y potencial de cada eslabon referenciadas a sus centros demasa.Oc1 , Oc2 .
I Entonces las velocidades lineales y angulares pueden expresarse en funcion dematrices Jacobianas apropiadas Jvi , Jwi , como,
vi = Jvi (q) q, wi = Jwi (q) q.
Energa Cinetica
La energa cinetica tiene la forma,
k =1
2qT
[n
i=1
{miJ
Tvi(q) Jvi (q) + J
Twi
(q)Ri (q) IiRTi (q) Jwi (q)
}]q
=1
2qTD (q) q.
Las matrices Jvi , Jwi para i = c1, c2 se obtiene a partir de sus respectivos Jacobianosdados por,
Jc1 =
[z0 (Oc1 O0) 0
z0 0
], Jc2 =
[z0 (Oc2 O0) z1 (Oc2 O1)
z0 z1
]
Jc1 =
[Jvc1Jwc1
], Jc2 =
[Jvc2Jwc2
].
Tomando en cuenta en principio los efectos traslacionales,
Jvc1 =[
z0 (Oc1 O0) 0]
, Jvc2 =[
z0 (Oc2 O0) z1 (Oc2 O1)]
O0 =
000
, Oc1 = lc1C1lc1S1
0
, O1 = l1C1l1S1
0
, O0c2 = O01 + R01O1c2O1c2 =
lc2C2lc2S20
.Entonces,
O0c2 =
l1C1l1S10
+ C1 S1 0S1 C1 0
0 0 1
lc2C2lc2S20
=
l1C1l1S10
+ lc2 (C1C2 S1S2)lc2 (S1C2 C1S2)
0
= l1C1 + lc2C12l1S1 + lc2S12
0
.
Por otra parte,
z0 =
001
, z1 = R01k = 00
1
con lo cual,
z0 (Oc2 O0) =
001
l1C1 + lc2C12l1S1 + lc2S12
0
= l1S1 lc2S12l1C1 + lc2C12
0
z0 (Oc1 O0) =
001
lc1C1lc1S1
0
= lc1S1lc1C1
0
z1 (Oc2 O1) =
001
lc2C12lc2S12
0
= lc2S12lc2C12
0
.
De lo anterior se obtiene entonces,
Jvc1 =
lc1S1 0lc1C1 00 0
, Jvc2 = l1S1 lc2S12 lc2S12l1C1 + lc2C12 lc2C12
0 0
.La energa cinetica traslacional resulta,
kT =1
2qT{m1J
Tvc1
Jvc1 +m2JTvc2
Jvc2
}q
donde,
JTvc1Jvc1 =
[l2c1(S21 + C
21
)0
0 0
]=
[l2c1 00 0
]JTvc2
Jvc2 =
[l21 + l
2c2+ 2l1lc2C2 l
2c2+ l1lc2C2
l2c2 + l1lc2C2 l2c2
].
Por su parte, la energa cinetica rotacional se obtiene a partir de
kRi =1
2qT{JTwi (q)Ri (q) IiR
Ti (q) Jwi (q)
}q
donde,Jw1 =
[z0 0
], Jw2 =
[z0 z1
]como
z1 = R01k = R
01
001
= 00
1
con lo cual,
Jw1 =
0 00 01 0
, Jw2 = 0 00 0
1 1
.Por otra parte,
R01 =
C1 S1 0S1 C1 00 0 1
, R02 = R01R12 = C12 S12 0S12 C12 0
0 0 1
.
Considerando elementos (eslabones) simetricos, los tensores de inercia resultanmatrices diagonales en la forma,
I1 =
I1xx 0 00 I1yy 00 0 I1zz
, I2 = I2xx 0 00 I2yy 0
0 0 I2zz
.Considerando que,
R1I1RT1 =
I1xxC21 + I1yyS21 I1xxC1S1 I1yyC1S1 0I1xxC1S1 I1yyC1S1 I1yyC21 + I1xxS21 00 0 Izz
R2I2R
T2 =
I2xxC212 + I2yyS212 I2xxC12S12 I2yyC12S12 0I2xxC12S12 I2yyC12S21 I2yyC212 + I2xxS212 00 0 Izz
.
Se obtiene entonces,
JTw1R1I1RT1 Jw1 =
[I1zz 00 0
]= I1zz
[1 00 0
]JTw2R2I2R
T2 Jw2 =
[I2zz I2zzI2zz I2zz
]= I2zz
[1 11 1
].
Produciendo,
KR =1
2qT{I1zz
[1 00 0
]+ I2zz
[1 11 1
]}q,
con lo cual la energa cinetica total K = 12 qTD (q) q toma la forma,
K =1
2qT{m1
[l2c1 00 0
]+m2
[l21 + l
2c2+ 2l1lc2C2 l
2c2+ l1lc2C2
l2c2 + l1lc2C2 l2c2
]+I1zz
[1 00 0
]+ I2zz
[1 11 1
]}q.
Por lo tanto,
D (q) =
[m1l
2c1+m2
(l21 + l
2c2+ 2l1lc2 cos q2
)+ I1zz + I2zz m2
(l2c2 + l1lc2 cos q2
)+ I2zz
m2(l2c2 + l1lc2 cos q2
)+ I2zz m2l2c2 + I2zz
].
La matriz de Coriolis y fuerzas centrifugas se obtiene a partir de los smbolos deChristoffel en la forma,
Cijk =1
2
{dkjqi
+dkiqj
dijqk
}.
Por lo tanto,
C111 =1
2
d11q1
= 0
C121 = C211 =1
2
{d12q1
+d11q2 d12
q1
}=
1
2
{d11q2
}= 1
22m2l1lc2 sin q2 = m2l1lc2 sin q2
C221 =1
2
{d12q2
+d12q2 d22
q1
}=
d12q2
= m2l1lc2 sin q2
C112 =1
2
{d21q1
+d21q1 d11
q2
}= 1
2
d11q2
= m2l1lc2 sin q2
C122 = C212 =1
2
{d22q1
+d21q2 d12
q2
}=
1
2
{d12q2 d12
q2
}= 0
C222 =1
2
{d22q2
+d22q2 d22
q2
}=
1
2
{d22q2
}= 0.
Considerando que,
Ckj =n
i=1
Cijk qi ,
se obtiene,
C11 =2
i=1
Ci11qi = C111q1 + C211q2 = m2l1lc2 sin q2q2
C22 =2
i=1
Ci22qi = C122q1 + C222q2 = 0
C12 =2
i=1
Ci21qi = C121q1 + C221q2 = m2l1lc2 sin q2q1 m2l1lc2 sin q2q2
C21 =2
i=1
Ci12qi = C112q1 + C212q2 = m2l1lc2 sin q2q1.
Produciendo,
C (q, q) =
[m2l1lc2 sin q2q1 m2l1lc2 sin q2q1 m2l1lc2 sin q2q2m2l1lc2 sin q2q1 0
].
Considerando h (q2) = m2l1lc2 sin q2 la matriz anterior puede escribirse como,
C (q1q) =
[hq2 h (q1 + q2)hq1 0
].
Finalmente, la energa potencial de cada eslabon resulta,
Pi = migTOci
esto es,
P =n
i=1
Pi =n
i=1
migTOci .
Entonces,
P1 = m1g[
0 1 0] lc1C1lc1S1
0
= m1glc1 sin q1P2 = m2g
[0 1 0
] lc1C1 + lc2C12lc1S1 + lc2S120
= m2g (l1S1 + lc2S12) ,por lo tanto
P = m1glc1 sin q1 +m2g (l1 sin q1 + lc2 sin (q1 + q2))
= m1glc1S1 +m2g (l1S1 + lc2S12) .
De lo anterior,
g1 =P
q1= m1glc1C1 +m2g (l1C1 + lc2C12)
= (m1lc1 +m2l1) g cos q1 +m2glc2 cos (q1 + q2)
g2 =P
q2= m2glc2C12 = m2lc2g cos (q1 + q2) .
Obteniendose el modelo dinamico,
D (q) q + C (q1q) q +
[g1g2
]=
[12
].
Notese que
C (q1q) q =
[hq2 h (q1 + q2)hq1 0
] [q1q2
]=
[h (q1q2) h (q1 + q2) q2
hq21
]=
[hq1q2 hq1q2 hq22
hq21
]=
[2hq1q2 hq22
hq21
].
Manipulador CartesianoManipulador Planar Rotacional