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