Implementacion Sistema Robotico - Scara

8/19/2019 Implementacion Sistema Robotico - Scara

1/12

Cálculo del Modelo Dinámico del Robot SCARA

Dr. Emmanuel Dean León.U.P.I.I.T.A-IPN.

Resumen

En este documento se muestra paso a paso la metodoloǵıa para obtener el modelo dinámico de un

sistema robótico usando como herramienta los parámetros y matrices de transformación calculados con

el método de Denavit-Hartenberg directo.

1. Parametrización del Sistema

Suponga el sistema robótico mostrado en la Figura 1, el primer paso que debemos realizar para poder

calcular el modelo dinámico de dicho robot es definir los ejes de movimiento del sistema y los parámetrosde los eslabones, como se muestra en las figuras (2) y (3), respectivamente.

Figura 1: En la imagen se puede observar la disposición de los eslabones que conforman la arquitecturaSCARA. El sistema cuenta con 3 grados de libertad, de tipo R-R-P.

Figura 2: En la figura se muestran los ejes de movimiento de los eslabones, los cuales son importantes de

definir, ya que, con ellos, se estableceran los ejes zi del sistema.

1


2/12

Una vez definidos los ejes de movimiento definiremos la pose (posici ón y orientación) de los marcoscoordenados Oi con i = 0, 1, 2, 3.

Para este fin, colocaremos los marcos coordenados tomando en cuenta las siguientes reglas:

El origen Oi debe ser colocado en el eje de movimiento de eslab ón i.

El eje xi debe ser paralelo e intersectar al eje zi−1.

Tomando en cuenta las anteriores restricciones, los marcos coordenados tienen la siguiente disposici ón,ver Figura 3.

z0

x0

z1

x1

z2

x2

z3

x3

h1

l1l2

q1

q2

h2cm1cm1 cm2

cm2

Figura 3: En la figura se observa finalmente la disposici ón de los marcos coordenados que definen loseslabones del sistema robótico, aśı como el movimiento que realizan. La base del sistema está definidacomo O0.

Una vez definidos los marcos coordenados del robot manipulador, calcularemos la tabla de parámetrosde Denavit-Hartenberg (D-H).

2. Tabla de Parámetros de D-H

Para definir los parámetros de la tabla de D-H, debemos tomar en cuenta las siguientes definiciones:

Variable Definición

θi El ángulo medido sobre el eje zi−1 para colocar el eje xi−i en dirección y sentido de xi.di La distancia en dirección de zi−1 para colocar el origen de xi−1 sobre el origen de xi.ai La distancia en dirección de xi para colocar el origen de zi−1 sobre el origen de zi.αi El ángulo medido sobre el eje xi para colocar el eje zi−i en dirección y sentido de zi.

Dadas las anteriores definiciones, la tabla de parámetros de Denavith-Hartenberg para los marcoscoordenados de los ejes de rotación y del efector final es (ver Figura 3):

i θi di ai α1

1 q 1 h1 l1 02 q 2 h2 l2 03 0 −q 3 0 0

Cuadro 1: Tabla de parámetros de las articulaciones del robot y del efector final.

Mientras que, la tabla de parámetros de Denavith-Hartenberg para los marcos coordenados de loscentros de masa es (ver Figura 3):

2


3/12

icm θi di ai α1

1cm q 1 h1 l11 02cm q 2 h2 l21 03cm 0 −q 3 0 0

Cuadro 2: Tabla de parámetros de los centros de masa del robot.

3. Transformaciones Homogéneas

El siguiente paso es calcular las transformaciones homogéneas entre marcos, para este fin, usaremos lamatriz de transformación homogénea de D-H, la cual se puede obtener de la siguiente forma:

T ii−1 = T Rz,θiT tz,diT tx,aiT Rx,αi

esto es,

T ii−1 =

cos θi − sin θi 0 0sin θi cos θi 0 0

0 0 1 00 0 0 1

1 0 0 00 1 0 00 0 1 d

i0 0 0 1

1 0 0 ai0 1 0 00 0 1 00 0 0 1

1 0 0 00 cos αi − sin αi 00 sin α

i cos α

i 0

0 0 0 1

T ii−1 =

cos θi − sin θi cos αi sin θi sin αi ai cos θisin θi cos θi cos αi − cos θi sin αi ai sin θi

0 sin αi cos αi di0 0 0 1

(1)

3.1. Transformaciones para marcos de articulaciones y efector final

Usando los parémetros definidos en el Cuadro 1 y sustituyendolos en la matriz de la ecuación (1)obtenemos las transformaciones homogéneas entre los marcos coordenados O0 − O1, O1 − O2 y O2 − O3,ecuaciones (2), (3) y (4), respectivamente.

T 10 =

cos q 1 − sin q 1 0 l1 cos q 1sin q 1 cos q 1 0 l1 sin q 1

0 0 1 h10 0 0 1

(2)

T 21 =


0 0 1 h20 0 0 1

(3)

T 32 =

1 0 0 00 1 0 00 0 1 −q 30 0 0 1

(4)

Sin embargo, se debe obtener la matriz de transformación de cada articulación respecto a la base delsistema (en este caso O0), por lo que, usando las ecuaciones (2), (3) y (4), calcularemos las transformacioneshomogéneas entre los marcos O0 − O2 y O0 − O3. Usando la regla de concatenaci´ on de transformaciones entre marcos m´ oviles , es fácil ver que la transformación homogénea de O0 − O2 se puede obtener como:

T 20 = T 10T 21 =

cos(q 1 + q 2) − sin(q 1 + q 2) 0 l2 cos (q 1 + q 2) + l1 cos q 1sin(q 1 + q 2) cos (q 1 + q 2) 0 l2 sin (q 1 + q 2) + l1 sin q 1

0 0 1 h1 + h20 0 0 1

T 20 = cos(q 1 + q 2) − sin(q 1 + q 2) 0 l2 cos (q 1 + q 2) + l1 cos q 1

sin(q 1 + q 2) cos (q 1 + q 2) 0 l2 sin (q 1 + q 2) + l1 sin q 10 0 1 h1 + h20 0 0 1

(5)

3


4/12

De igual forma, la transformación homogénea O0 − O3 se obtiene como:

T 30 = T 20T 32 =


0 0 1 h1 + h2 − q 30 0 0 1

T 30 =


0 0 1 h1 + h2 − q 30 0 0 1

(6)

3.2. Transformaciones de los centros de masa

Aplicando el mismo criterio, podemos calcular las matrices de transformación homogénea de cada centrode masa y referidas respecto a la base del sistema O0. Estas transformaciones son útiles para el cálculo dela dinámica del robot manipulador. Ahora, sustituyendo los parámetros del Cuadro 2, en la ecuación (1)podremos calcular las matrices de transformación homogénea.

T 101 =


0 0 1 h10 0 0 1

(7)

T 211 =


0 0 1 h20 0 0 1

T 321 =

1 0 0 00 1 0 00 0 1 −q 30 0 0 1

T 201 = T 10T 211 =


0 0 1 h1 + h20 0 0 1

T 201 =


0 0 1 h1 + h20 0 0 1

(8)

T 301 = T 20T 321 =


0 0 1 h1 + h2−

q 30 0 0 1

(9)

Donde T i01, i = 1, 2, 3, representa la matriz de transformación homogénea de la base O0 al centro demasa i.

4. Cálculo de Jacobianos para centros de masa

Ahora, para modelar la dinámica del robot SCARA, necesitamos calcular los Jacobianos de cada centrode masa. La definición de Jacobiano se puede expresar mediante la siguiente ecuación:

4


5/12

Ẋ = J (q ) q̇ (10)

ẋ

ẏ

ż

ωxωyωz

= J v (q )

J ω (q )

q̇ 1q̇ 2.

.

.

q̇ n

(11)

donde, J v (q ) ∈ ℜ3×n representa el Jacobiano de velocidades lineales y J ω (q ) ∈ ℜ

3×n es el Jacobiano de velocidades angulares . En otras palabras, el Jacobiano J (q ) ∈ ℜ6×n es una transformación que relacionael espacio de velocidades articulares q̇ ∈ ℜn con el espacio de velocidades operacionales (o cartesianas)ẋ ∈ ℜ3. Para poder calcular el Jacobiano de cada centro de masa del robot manipulador debemos definirlas siguientes variables:

Ejes de movimiento de cada centro de masa

Z 0 = 00

1 , Z 1 = 00

1 , Z 3 = 00

1 (12)Posición de cada centro de masa respecto a la base O0

O11 =

l11 cos q 1l11 sin q 1

h1

, O21 =

l21 cos (q 1 + q 2) + l1 cos q 1l21 sin (q 1 + q 2) + l1 sin q 1

h1 + h2

, O31 =

l2 cos (q 1 + q 2) + l1 cos q 1l2 sin (q 1 + q 2) + l1 sin q 1

h1 + h2 − q 3

(13)

Centro de giro del eslabón 2

O1 =

l1 cos q 1l1 sin q 1

h1

4.1. Jacobiano para el centro de masa 1 (cm1)

Entonces, para el primer centro de masa tenemos el siguiente Jacobiano:

J 1 =

J 11J 12

,

donde, J i, representa el Jacobiano del centro de masa i, J i1 representa el Jacobiano de velocidades lineales del centro de masa i, y J i2 es el Jacobiano de velocidades angulares del centro de masa i.

Debido a que la primera articulación es de tipo revoluta , tenemos la siguiente expresión para los Jaco-bianos del centro de masa 1.

J 11 =

Z 0 × (O11 − O0) 0 0

Revoluta No afecta No afecta

J 12 =

Z 0 0 0

Revoluta No afecta No afecta

donde,

Z 0 × (O11) =

−l11 sin q 1l11 cos q 1

0

por lo que los Jacobianos de velocidad lineal y angular son:

5


6/12

J 11 =

−l11 sin q 1 0 0l11 cos q 1 0 0

0 0 0

(14)

J 12

= 0 0 0

0 0 01 0 0 (15)


Realizando el mismo procedimiento anterior, podemos calcular el Jacobiano del centro de masa 2.

J 2 =

J 21J 22

J 21 =

Z 0 × (O21 − O0)

Z 1 × (O21 − O1)

0

Revoluta Revoluta No afecta

J 22 = Z 0 Z 1 0

Revoluta Revoluta No afecta

donde,

Z 0 × (O21) =

−l21 sin (q 1 + q 2) − l1 sin q 1l21 cos (q 1 + q 2) + l1 cos q 1

0

Z 1 × (O21 − O1) =

−l21 sin (q 1 + q 2)l21 cos (q 1 + q 2)

0

por lo que los Jacobianos del centro de masa 2 son:

J 21 =

−l21 sin (q 1 + q 2) − l1 sin q 1 −l21 sin (q 1 + q 2) 0l21 cos (q 1 + q 2) + l1 cos q 1 l21 cos (q 1 + q 2) 0

0 0 0

(16)

J 22 =

0 0 00 0 0

1 1 0

(17)


Nuevamente usando el mismo procedimiento, calculamos el Jacobiano del centro de masa 3 de la si-guiente forma:

J 3 =

J 31J 32

J 31 =

Z 0 × (O31 − O0) Z 1 × (O31 − O1) Z 2

Revoluta Revoluta Prismático

J 32 =

Z 0 Z 1 0

Revoluta Revoluta Prismático

donde,

6


7/12

Z 0 × (O31) =

−l2 sin (q 1 + q 2) − l1 sin q 1l2 cos (q 1 + q 2) + l1 cos q 1

0

Z 1 ×(

O31 − O1) =

−l2 sin (q 1 + q 2)l2

cos (q 1

+q 2

)0

por lo que, finalmente, los Jacobianos del centro de masa 3 son:

J 31 =


0 0 1

(18)

J 32 =

0 0 00 0 0

1 1 0

(19)

Tomando las ecuaciones (14-19) obtenemos los Jacobianos de los centros de masa como:

J 1 =

−l11 sin q 1 0 0l11 cos q 1 0 0

0 0 00 0 00 0 01 0 0

J 2 =


0 0 00 0 0

0 0 01 1 0

J 3 =


0 0 10 0 00 0 01 1 0

donde, J i ∈ ℜ6×3 representa el Jacobiano del centro de masa i respecto a la base O0.

5. Matriz de inercia H (q )Los Jacobianos de velocidad lineal y angular de los centros de masa serán útiles para el cálculo de la

matriz de inercia H (q ) por lo que los definiremos nuevamente:Jacobiano del centro de masa 1:

J 11 =

−l11 sin q 1 0 0l11 cos q 1 0 0

0 0 0

, J 12 =

0 0 00 0 0

1 0 0

Jacobiano del centro de masa 2:

J 21 = −l21 sin (q 1 + q 2) − l1 sin q 1 −l21 sin (q 1 + q 2) 0l21 cos (q 1 + q 2) + l1 cos q 1 l21 cos (q 1 + q 2) 0

0 0 0 , J 22 = 0 0 00 0 0

1 1 0 Jacobiano del centro de masa 3:

7


8/12


9/12

H 21 = m2J T 21J 21 =

m2l21 + 2m2 (cos q 2) l1l21 + m2l221 m2l221 + l1m2 (cos q 2) l21 0m2l221 + l1m2 (cos q 2) l21 m2l221 0

0 0 0

H 22

= J

T

22R2

I 2

R

T

2 J 22

= I 233 I 233 0

I 233

I 233

00 0 0

Matriz de inercia centro de masa 2:

H 31 = m3J T 31

J 31 =

m3l21 + 2m3 (cos q 2) l1l2 + m3l22 m3l22 + l1m3 (cos q 2) l2 0m3l22 + l1m3 (cos q 2) l2 l22m3 0

0 0 m3

H 32 = J T 32

R3I 3RT 3

J 32 =

I 333 I 333 0I 333 I 333 0

0 0 0

Finalmente la Matriz H (q ) es:

H (q ) =

h11 h12 h13h21 h22 h23

h31 h32 h33

(23)

con:

h11 = I 133 + I 233 + I 333 + l2

1m2 + l

2

1m3 + l

2

2m3 + m1l

2

11 + m2l

2

21 + 2l1l2m3 cos q 2 + 2l1m2l21 cos q 2

h12 = m3l2

2 + l1m3 (cos q 2) l2 + m2l

2

21 + l1m2 (cos q 2) l21 + I 233 + I 333

h13 = 0

h21 = h12

h22 = m3l22 + m2l221 + I 233 + I 333

h23 = 0

h31 = h13

h32 = h23

h33 = m3

Una vez definido los n ×n elementos de la matriz de inercia H (q ), usaremos estos términos para calcularla matriz de aceleraciones centŕıpetas y de Coroll ı́.

6. Matriz de aceleraciones centŕıpetas y de Corolĺı

Para calcular la matriz C (q, q̇ ) debemos calcular las derivadas parciales de la matriz de inercia H (q )respecto a las variables de posición articular q ∈ ℜn.

∂H

∂q 1=

0 0 00 0 0

0 0 0

∂H

∂q 2=

−2l1 (l2m3 + l21m2)sin q 2 −l1 (l2m3 + l21m2)sin q 2 0−l1 (l2m3 + l21m2)sin q 2 0 0

0 0 0

∂H

∂q 3

= 0 0 00 0 0

0 0 0 De lo anterior podemos definir la siguiente lista:

9


10/12


11/12

C 23 = 1

2 q̇ 1

∂h21

∂q 3−

∂ h13

∂q 2

+

1

2 q̇ 2

∂h23

∂q 2+

∂h22

∂q 3−

∂ h23

∂q 2

+

1

2 q̇ 3

∂h23

∂q 3−

∂h33

∂q 2

C 23 = 0

C 31 = 1

2 q̇ 1

∂h31

∂q 1−

∂ h11

∂q 3

+

1

2 q̇ 2

∂h31

∂q 2+

∂h32

∂q 1−

∂ h21

∂q 3

+

1

2 q̇ 3

∂h33

∂q 1−

∂h31

∂q 3

C 31 = 0

C 32 = 1

2 q̇ 1

∂h31

∂q 2−

∂ h12

∂q 3

+

1

2 q̇ 2

∂h32

∂q 2+

∂h32

∂q 2−

∂ h22

∂q 3

+

1

2 q̇ 3

∂h33

∂q 2−

∂h32

∂q 3

C 32 = 0

C 33 = 12

q̇ 1∂h31

∂q 3− ∂ h

13

∂q 3

+ 1

2 q̇ 2∂h33

∂q 2+ ∂h

32

∂q 3− ∂ h

23

∂q 3

+ 1

2 q̇ 3∂h33

∂q 3− ∂h

33

∂q 3

C 33 = 0

Por lo que la matriz C (q, q̇ ) tiene la siguiente forma:

C (q, q̇ ) =

−l1 (l2m3 + l21m2) q̇ 2 sin q 2 −l1 (l2m3 + l21m2) q̇ 1 sin q 2 − l1 (l2m3 + l21m2) q̇ 2 sin q 2 0l1 (l2m3 + l21m2) q̇ 1 sin q 2 0 0

0 0 0

(26)La matriz C (q, q̇ ) debe cumplir con la siguiente propiedad:

Definición 1 Sea H (q ) la matriz de inercia de un robot de n gdl, definiendo a C (q, q̇ ) en términos de los elementos de H (q ), de acuerdo a la ecuaci´ on (24) entonces, la matriz N (q, q̇ ) = Ḣ (q ) − 2C (q, q̇ ) es una matriz anti-simétrica, esto es, los componentes njk de N (q, q̇ ) satisfacen la relaci´ on njk = −nkj .

Para validar la Definición 1, calcularemos la matriz N (q, q̇ ).

N (q, q̇ ) = Ḣ − 2C (q )

N (q, q̇ ) =

0 l1 (sin q 2) (l2m3 + m2l21) ( 2q̇ 1 + q̇ 2) 0

−l1 (sin q 2) (l2m3 + m2l21) ( 2q̇ 1 + q̇ 2) 0 00 0 0

(27)

con

Ḣ =

−2l1 (l2m3 + l21m2)sin q 2 −l1 (l2m3 + l21m2)sin q 2 0−l1 (l2m3 + l21m2)sin q 2 0 0

0 0 0

Es evidente que la definición de N (q, q̇ ), ecuación (27), es una matriz anti-simétrica, ya que cumple connjk = −nkj .

7. Vector de pares gravitacionales

Los pares gravitacionales se obtienen a través de la enerǵıa potencial del sistema, la cual se puede

generar por dos elementos:

La enerǵıa potencial debido a la masa:

11


12/12

La enerǵıa potencial debido a resortes:

Para calcular la enerǵıa potencial de las masas, requerimos de la posición de cada centro de masa,dichas posiciones las definimos en la ecuación (13). Entonces la enerǵıa potencial de las masas será:

P 1 =n

i=1 miGT Oi1

P 1 = GT

3i=1

miOi1

con

3i=1

miOi =

l2m3 cos (q 1 + q 2) + m2l21 cos (q 1 + q 2) + l1m2 cos q 1 + l1m3 cos q 1 + m1l11 cos q 1l2m3 sin (q 1 + q 2) + m2l21 sin (q 1 + q 2) + l1m2 sin q 1 + l1m3 sin q 1 + m1l11 sin q 1

h1m1 + h1m2 + h1m3 + h2m2 + h2m3 − m3q 3

y

G = 00

g por lo que la energı́a potencial proporcionada por las masas es:

P 1 = gh1m1 + gh1m2 + gh1m3 + gh2m2 + gh2m3 − gm3q 3 (28)

Por otro lado, la energı́a potencial generada por los resortes es:

P 2 =ni=1

1

2K iq

2

i

P 2 = 1

2K 1q

2

1 + 1

2K 1q

2

2 + 1

2K 1q

2

3

La energı́a potencial total es la suma de las energı́as potenciales:

P = gh1m1 + gh1m2 + gh1m3 + gh2m2 + gh2m3 − gm3q 3 + 1

2K 1q

2

1 +

1

2K 1q

2

2 +

1

2K 1q

2

3

El vector de pares gravitacionales será entonces:

g =

∂P ∂q1∂P ∂q2∂P ∂q3

=

K 1q 1K 1q 2

K 1q 3 − gm3

(29)

8. Modelo dinámico del robot SCARA

Por último, el modelo dinámico del robot manipulador tipo SCARA tiene la siguiente forma:

H (q ) q̈ + C (q, q̇ ) q̇ + g (q ) = τ − Dq̇

donde H (q ) ∈ ℜ3×3 está definida por la ecuación (23), los términos de C (q, q̇ ) ∈ ℜ3×3 se pueden ver en laecuación (26), el vector g (q ) está expresado en la ecuación (29) y

τ =

τ 1τ 2

F 3

con τ i es el par de las articulaciones i y F 3 es la fuerza de entrada para el eslab ón 3. La matriz D defricciones viscosas es:

D = β 1 0 0

0 β 2 00 0 β 3

12

Documents

Implementacion Sistema Robotico - Scara