cinemática directa

CINEMÁTICA DIRECTA C O N T E N I D O

•  Introducción •  Representación de un punto •  Representación de la orientación •  Ejercicios 1 en Matlab® •  Matriz de transformación homogénea •  Relación entre marcos •  Matrices de transformación de un robot •  Ejercicios 2 en Matlab® •  Cinemática Directa de un Robot •  Ejemplo numérico de la Cinemática Directa

Por: Dr. Juan Antonio Rojas Estrada

INTRODUCCIÓN PROBLEMA DE CINEMÁTICA

DIRECTA

PROBLEMA DE CONTROL DE

FUERZA

PROBLEMA DE CINEMÁTICA

INVERSA

CONTROL DE POSICIÓN

GENERACIÓN DE TRAYECTORIAS

SIMULACIÓN

PROGRAMACIÓN DE ROBOTS

DINÁMICA

INTRODUCCIÓN ARTICULACIÓN, VARIABLE DE

UNIÓN

ACTUADOR

HERRAMIENTA O ELEMENTO TERMINAL

UNIÓN DE LA BASE

ESLABÓN

UNIÓN REVOLUTA

ESLABÓN

ACTUADOR

INTRODUCCIÓN

Uniones prismáticas, variable, distancia

Uniones revolutas, variable, ángulo

TIPOS DE UNIÓN

INTRODUCCIÓN

•  Cinemática. Rama de la física que estudia el movimiento sin tomar en cuenta qué lo produce

•  Dinámica. Rama de la física que estudia el movimiento tomando en cuenta qué lo produce

•  Control de fuerza. Además de controlar la posición del elemento terminal, se busca controlar la fuerza que sujeta o se ejerce sobre la herramienta o cuerpo rígido

•  Control de posición. Tarea esencial de un robot, llevar al elemento final a una posición en el espacio

•  Problema de Cinemática Directa. Dados los valores de las variables de unión, determina la posición en el espacio del elemento terminal

•  Problema de Cinemática Inversa. Establecido un punto a alcanzar en el espacio, determinar los posibles conjunto de valore de las variables de unión

•  Grados de Libertad (g.d.l.) El número de variables necesarias para posicionar un robot

REPRESENTACIÓN DE UN PUNTO

�

Punto en el espacio

P(x, y, z)

Sistema de ejes coordenados, Marco de referencia {A}

{A}

AP =xyz

⎡

⎣

⎢⎢⎢

⎤

⎦

⎥⎥⎥

Vector de posición

Representación matemática del punto en el espacio

AP

CUERPOS RÍGIDOS

Sistema de ejes coordenados, Marco de referencia {A}

{A}

Sistemas de ejes coordenados asignados a los cuerpos rígidos Marcos {B}, {C}, {D} y {E}

{B}

{C}

{D}

{E}

POSICIÓN DE CUERPOS RÍGIDOS

Sistema de ejes coordenados, Marco de referencia

{A}

Vectores de posición

Sistemas de ejes coordenados asignados a los cuerpos rígidos

{B}

{C}

{D}

{E} {E}

DEFINICIÓN DE PRODUCTO PUNTO

A i B = A B cosθ {A}

Proyección de v sobre u

Significado: La proyección indica “que cantidad” de v apunta en la dirección de u

Si las magnitudes de A y B son la unidad, entonces el producto punto es simplemente

A i B = cosθ

A = a1 a2 a3⎡⎣

⎤⎦T

A = a12 + a2

2 + a32

A =a1A

a2A

a3A

⎡

⎣⎢⎢

⎤

⎦⎥⎥

T

 indica vector unitario, esto es, la magnitud es 1. El superíndice T, Indica traspuesta

REPRESENTACIÓN DE LA ORIENTACIÓN

{C}

AP

{A} ZA

YA

XA

ZC

YC

XC

El sistema de ejes coordenados o marco de la herramienta {C}, esta descrito en referencia al marco {A}. Se observa que {C} no tiene la misma orientación que {A}

REPRESENTACIÓN DE LA ORIENTACIÓN

CAR =

r11 r12 r13r21 r22 r23r31 r32 r33

⎡

⎣

⎢⎢⎢

⎤

⎦

⎥⎥⎥=

XC i XA YC i XA ZC i XA

XC i YA YC i YA ZC i YAXC i ZA YC i ZA ZC i ZA

⎡

⎣

⎢⎢⎢⎢

⎤

⎦

⎥⎥⎥⎥

(1)

En palabras: La orientación del objeto cuyo sistema de ejes coordenados o marco es {C},respecto al marco de referencia {A}, se expresa mediante una matriz de rotación expresada por: que indica la orientación de {C} respecto a {A}, cada elemento de la matriz representa un producto punto de los ejes de {C}, con los de {A}

CAR

INFORMACIÓN COMPLETA EN ROBÓTICA

Representación de un punto

Representación de la orientación de un cuerpo rígido

Vector

Matriz

C{ } = CAR, APCORG{ }

En palabras: El objeto, cuyo marco es {C}, esta descrito por (orientación) y el vector de posición. El vector localiza el origen de {C} con respecto a {A}

CAR APCORG

APCORG

EJERCICIOS 1 EN MATLAB 1.  Introduzca en Matlab los vectores:

2.   Obtenga los vectores unitarios

3.   Introduzca en Matlab la matriz

4.   Evalúe la matriz para un valor de θ=30°

5.   Compruebe la matriz de rotación entre los macos {A} y {B} para θ=60°

6.   Obtenga el determinante y traspuesta de la matriz rotación anterior

v1 = 1 −1 2⎡⎣ ⎤⎦T

v2 = −1 1 0⎡⎣ ⎤⎦T

v3 = 1 −2 1⎡⎣ ⎤⎦T

v1, v2, v3

BAR =

cosθ −sinθ 0sinθ cosθ 00 0 1

⎛

⎝

⎜⎜

⎞

⎠

⎟⎟

{A}

YA

XB

ZB

YB

ZA

XA

{B}

θ

BAR =

1/ 2 − 3 / 2 03 / 2 1 / 2 00 0 1

⎛

⎝

⎜⎜⎜

⎞

⎠

⎟⎟⎟

MATRICES DE TRANSFORMACIÓN HOMOGÉNEA

BAR APBORG

10 0 0BAT =

La matriz de transformación homogénea, contiene la información de la posición y orientación de un marco referido a otro marco

(2)

MATRICES DE TRANSFORMACIÓN HOMOGÉNEA

cosθ −sinθ 0sinθ cosθ 00 0 1

APBORG

10 0 0BAT =

La matriz de transformación homogénea, matriz de 4X4 que da la posición y orientación del marco {B} referido al marco {A}

MATRICES DE TRANSFORMACIÓN HOMOGÉNEA

L1

L2

Robot de uniones revolutas de 4 g.d.l.

MATRICES DE TRANSFORMACIÓN HOMOGÉNEA

{A}

YA

XB

ZB

YBZA

XA

{B}

θ

Indica que los vectores son paralelos

APBORG

Marco {A} asignado al primer eslabón móvil L1 Marco {B} asignado al segundo eslabón L2

RELACIÓN ENTRE MARCOS DE REFERENCIA

{A}

YA

XB

ZB

YBZA

XA

{B}

θ

APBORG

APBORG =L100

⎛

⎝

⎜⎜⎜

⎞

⎠

⎟⎟⎟

•  El marco {B} esta desplazado respecto al marco {A} sobre el eje por el vector una distancia del valor de L1

•  El marco {B} esta rotado sobre el eje un valor de θ grados.

•  El marco {A} es el de referencia

XAAPBORG

ZA

OBSERVACIONES:

Coordenadas del vector de posición

Usando (1) para obtener la Matriz de rotación

BAR

RELACIÓN ENTRE MARCOS DE REFERENCIA

BAR =

XB i XA YB i XA ZB i XA

XB i YA YB i YA ZB i YAXB i ZA YB i ZA ZB i ZA

⎛

⎝

⎜⎜⎜⎜

⎞

⎠

⎟⎟⎟⎟

BAR =

cosθ cos(θ + 90) cos90

cos(90 −θ ) cosθ cos90

cos90 cos90 cos0

⎛

⎝

⎜⎜⎜

⎞

⎠

⎟⎟⎟

BAR =

cosθ −senθ 0senθ cosθ 00 0 1

⎛

⎝

⎜⎜

⎞

⎠

⎟⎟

{A}

YA

XB

ZB

XA

θ

{B} ZA

Usando las identidades:cos(A + B) = cosAcosB − senA senB y sen(A + B) = senAcosB + cosA senBcon: cos90 = 0 y sen90 = 1cos(90 −θ ) = cos90 cosθ + sen90senθcos(90 −θ ) = senθ ; cos(90 +θ ) = −senθ ;

NOTA: Dado que lo que interesa aquí es la orientación, se elimina el vector de posición y los marcos se empalman para mejor visualización de los ángulos

YBθ

RELACIÓN ENTRE MARCOS DE REFERENCIA

Usando (2), la matriz de transformación del marco {B} al marco {A} queda:

BAT =

BAR APBORG⎡⎣ ⎤⎦

0 0 0⎡⎣ ⎤⎦ 1[ ]

⎛

⎝

⎜⎜

⎞

⎠

⎟⎟ =

cosθ −senθ 0 L1senθ cosθ 0 00 0 1 00 0 0 1

⎛

⎝

⎜⎜⎜⎜

⎞

⎠

⎟⎟⎟⎟

MATRICES DE TRANSFORMACIÓN DE UN ROBOT

{2}

Y2

X3

Z3

Y3Z2

X2

{3}

θ3

Indica que los vectores son paralelos

{0},

X0,1

Z0,1Y0,1

{1}

{4}

Y4

X4

Z4

θ4

θ1

θiθ1θ2θ3θ4

θ2

di0

ai−1 ai−1i

1234

0

−90

0

0

00L1L2

000

Tabla DH del robot

DH = Denavit-Hartenberg

MATRICES DE TRANSFORMACIÓN DE UN ROBOT

Eje i-1

Eje i

Eje i+1

Zi−1Yi−1

Xi−1

Xi

YiZi

θi

ai

di

α i−1

ai−1

Eslabón i-1

Eslabón i

Indican vectores paralelos y

PARÁMETROS = Torsión de eslabón = Longitud de eslabón = Corrimiento de eslabón = Ángulo de unión

α i−1

ai−1diθi

DEFINICIÓN DE PARÁMETROS = Ángulo de a alrededor de

= Distancia a a lo largo de

= Distancia a a lo largo de

= Ángulo de a alrededor de

α i−1

ai−1diθi

Zi−1 Zi Xi−1

Zi−1 Zi Xi−1

Xi−1 Xi Zi

Xi−1 Xi Zi

MATRICES DE TRANSFORMACIÓN DE UN ROBOT

Matriz de transformación de la unión i-1 a la unión i

ii−1T =

cθi −sθi 0 ai−1sθicα i−1 cθicα i−1 −sα i−1 −sα i−1disθisα i−1 cθisα i−1 cα i−1 cα i−1di0 0 0 1

⎛

⎝

⎜⎜⎜⎜

⎞

⎠

⎟⎟⎟⎟

Las matrices de transformación, del caso del robot, de acuerdo a la tabla DH

10T =

cθ1 −sθ1 0 0sθ1 cθ1 0 00 0 1 00 0 0 1

⎛

⎝

⎜⎜⎜⎜

⎞

⎠

⎟⎟⎟⎟

21T =

cθ2 −sθ2 0 00 0 1 0

−sθ2 −cθ2 0 00 0 0 1

⎛

⎝

⎜⎜⎜⎜

⎞

⎠

⎟⎟⎟⎟

32T =

cθ3 −sθ3 0 L1sθ3 cθ3 0 00 0 1 00 0 0 1

⎛

⎝

⎜⎜⎜⎜

⎞

⎠

⎟⎟⎟⎟

43T =

cθ4 −sθ4 0 L2sθ4 cθ4 0 00 0 1 00 0 0 1

⎛

⎝

⎜⎜⎜⎜

⎞

⎠

⎟⎟⎟⎟

EJERCICIOS 2 EN MATLAB

1.  Obtenga la matriz de transformación :

2.   Dada la descripción DH de un robot

Obtenga las matrices de transformación de

Cada eslabón. Use Matlab

BAR =

cosθ −sinθ 0sinθ cosθ 00 0 1

⎛

⎝

⎜⎜

⎞

⎠

⎟⎟

{A}

YA

XB

ZB

YB

ZA

XA

{B}

θ

BAT

APBORG

θiθ10θ3

di0

ai−1 ai−1i

123

0

90

0

000

d2L2

Tabla DH del robot

θ = 30

CINEMÁTICA DIRECTA DE UN ROBOT

La transformación total desde el último eslabón hasta el eslabón inmóvil Se obtiene multiplicando las todas las matrices de transformación, esto es, N0T = 1

0T i 21T ii N

N−1T

Para el caso del robot analizado 40T = 1

0T i 21T i 3

2T i 43T

40T =

cθ1 −sθ1 0 0sθ1 cθ1 0 00 0 1 00 0 0 1

⎛

⎝

⎜⎜⎜⎜

⎞

⎠

⎟⎟⎟⎟

i

cθ2 −sθ2 0 00 0 1 0

−sθ2 −cθ2 0 00 0 0 1

⎛

⎝

⎜⎜⎜⎜

⎞

⎠

⎟⎟⎟⎟

i

cθ3 −sθ3 0 L1sθ3 cθ3 0 00 0 1 00 0 0 1

⎛

⎝

⎜⎜⎜⎜

⎞

⎠

⎟⎟⎟⎟

i

cθ4 −sθ4 0 L2sθ4 cθ4 0 00 0 1 00 0 0 1

⎛

⎝

⎜⎜⎜⎜

⎞

⎠

⎟⎟⎟⎟

43T =

r11 r12 r13 r14r21 r22 r23 r24r31 r32 r33 r34r41 r42 r43 r44

⎛

⎝

⎜⎜⎜⎜⎜

⎞

⎠

⎟⎟⎟⎟⎟

cθ = cosθ; sθ = senθ

CINEMÁTICA DIRECTA DE UN ROBOT r11 = −cθ4 cθ1sθ2sθ3 − cθ1cθ2cθ3( )− sθ4 cθ1cθ2sθ3 + cθ1cθ3sθ2( )r12 = −cθ4 cθ1cθ2sθ3 + cθ1cθ3sθ2( ) + sθ4 cθ1sθ2sθ3 − cθ1cθ2cθ3( )r13 = −sθ1r14 = L1cθ1cθ2 − L2 cθ1sθ2sθ3 − cθ1cθ2cθ3( )r21 = cθ4 cθ3cθ2sθ1 − sθ1sθ2sθ3( )− sθ4 cθ2sθ1sθ3 + cθ3sθ1sθ2( )r22 = −cθ4 cθ3sθ1sθ2 + cθ2sθ1sθ3( )− sθ4 cθ2cθ3sθ1 − sθ1sθ2sθ3( )r23 = cθ1r24 = L2 cθ2cθ3sθ1 − sθ1sθ2sθ3( ) + L1cθ2sθ1r31 = −cθ4 cθ2sθ3 + cθ3sθ2( )− sθ4 cθ2cθ3 − sθ2sθ3( )r32 = sθ4 cθ2sθ3 + cθ3sθ2( )− cθ4 cθ2cθ3 − sθ2sθ3( )r33 = 0r34 = −L1sθ2 − L2 cθ2sθ3 + cθ3sθ2( )r41 = 0r42 = 0r43 = 0r44 = 1

Donde las submatrices

40R =

r11 r12 r13r21 r22 r23r31 r32 r33

⎛

⎝

⎜⎜⎜

⎞

⎠

⎟⎟⎟y

0P =r14r24r34

⎛

⎝

⎜⎜⎜

⎞

⎠

⎟⎟⎟

Denotan respectivamente la orientación y la posición en la Muñeca del robot

El conjunto de las ecuaciones de arriba denotan las ecuaciones de la cinemática directa del robot

APLICACIÓN DE LA CINEMÁTICA DIRECTA DE UN ROBOT

{2}

Y2

X3

Z3

Y3Z2

X2

{3}

θ3

Indica que los vectores son paralelos

{0},

X0,1

Z0,1

Y0,1

{1}

{4}

X4

Z4

θ1

θ2

H

H es la distancia del origen del marco {4} Al punto entre la tenaza a lo largo del eje X4

4P =H00

⎛

⎝

⎜⎜

⎞

⎠

⎟⎟

El punto tiene las coordenadas respecto al marco {4}

4P

APLICACIÓN DE LA CINEMÁTICA DIRECTA DE UN ROBOT

0PX = L1cθ1cθ2 − H cθ4 cθ1sθ2sθ3 − cθ1cθ2cθ3( ) + sθ4 cθ1cθ2sθ3 + cθ1cθ3sθ2( )⎡⎣ ⎤⎦ − L2 cθ1sθ2sθ3 − cθ1cθ2cθ3( )0PY = L1cθ2sθ1 − H cθ4 sθ1sθ2sθ3 − cθ2cθ3sθ1( ) + sθ4 cθ2sθ1sθ3 + cθ3sθ1sθ2( )⎡⎣ ⎤⎦ − L2 sθ1sθ2sθ3 − cθ2cθ3sθ1( )0PZ = −L2 cθ2sθ3 + cθ3sθ2( )− L1sθ2 − H cθ4 cθ2sθ3 + cθ3sθ2( ) + sθ4 cθ2cθ3 − sθ2sθ3( )⎡⎣ ⎤⎦

A partir de la matriz de transformación general, se obtiene cualquier posición y orientación de una herramienta referida al marco de referencia, marco {0}, la posición del punto entre la tenaza descrita por el vector se describe referido al marco de referencia por el vector como

4P0P

0P = 40T 4P

0P =

r11 r12 r13 r14r21 r22 r23 r24r31 r32 r33 r34r41 r42 r43 r44

⎛

⎝

⎜⎜⎜⎜⎜

⎞

⎠

⎟⎟⎟⎟⎟

H001

⎛

⎝

⎜⎜⎜⎜

⎞

⎠

⎟⎟⎟⎟

APLICACIÓN DE LA CINEMÁTICA DIRECTA DE UN ROBOT

Primer caso: L1 = 1, L2 =1 y H = 1, θ1 = θ2 =θ3 =θ4 =0, El vector es 0P

>> [px,py,pz,T,TN] = robotejPN(0,0,0,0,1,1) px = 2 py = 0 pz = 0

TN = 1 0 0 2 0 0 1 0 0 -1 0 0 0 0 0 1 >> P0=TN*P4 P0 = H + 2 0 0 1 >> eval(P0) ans = 3 0 0

Ejemplos numéricos

APLICACIÓN DE LA CINEMÁTICA DIRECTA DE UN ROBOT

Segundo caso: L1 = 1, L2 =1 y H = 1, θ2 = -90° ; θ1 = θ3 =θ4 = 0 El vector es

0P

>> [px,py,pz,T,TN] = robotejPN(0,-90,0,0,1,1) px = -0.0084 py = 0 pz = 2.0000 TN = -0.0042 1.0000 0 -0.0084 0 0 1.0000 0 1.0000 0.0042 0 2.0000 0 0 0 1.0000

>> P0=TN*P4 P0 = - (1211622740714113*h)/288230376151711744 - 1211622740714113/144115188075855872 0 (2251779918085443*h)/2251799813685248 + 2251779918085443/1125899906842624 1 >> eval(P0) ans = -0.0126 0 3.0000 1.0000