Cinematic A Directa e Inversa

Resumen - En el presente ensayo se explicara el desarrollo matemático para hallar las ecuaciones de la cinemática directa e inversa de nuestro brazo robótico. Se obtendrá 6 ecuaciones para la localización espacial del robot utilizando para ello los algoritmos de Denavit - Hartenberg

Abstract - In this rehearsal to explain the mathematical development to find the equations of direct and inverse kinematics of our robot arm. It will get six equations for the spatial location of the robot using the algorithms of Denavit - Hartenberg

Palabras Claves: cinematica, posición, variables articulares, algoritmo.

I. INTRODUCCIÓN

os estudios de la cinemática se basan en analizar el movimiento del robot con respecto a un sistema de

referencia cualesquiera. La cinemática describe el movimiento espacial del robot utilizando para ello las relaciones entre la posición y orientación del extremo final del robot. Existen dos tipos de análisis cinemáticos relacionados entre ellos, estos análisis son: cinemática directa y cinemática inversa

L

Cinemática directa nos permite determinar la posición P(x, y, z) del extremo final del robot en función de los valores de las variables articulares (θ1 , θ2 ,θ3). El análisis cinemático depende la geometría del brazo.Cinemática inversa nos permite determinar los valores de las variables articulares para conseguir una posición deseada.

Figura 1. Diagrama de relación entre cinemática directa e inversa [1]

II. ALGORITMO DE DENAVIT - HARTENBERG

En 1955 Denavit - Hartenberg desarrollaron un algoritmo que describe y representa la geografía espacial de los elementos de una cadena cinemática con respecto al sistema de referencia elegido. El algoritmo se base en el desarrollo de una matriz de transformación homogénea para la descripción de la relación espacial entre dos elementos adyacentes, al utilizar esto, se reduce el problema cinemático directo a encontrar una matriz de transformación homogénea de 4 x 4, que nos sirve para la

localización espacial del extremo del robot con respecto al sistema de coordenadas de su base.Para calcular la cinemática directa se plantean 4 transformaciones básicas

1. Debería rotar alrededor del eje O z un ángulo ϴ 2. Debería trasladar el sistema de coordenadas (u, v, w )

a lo largo de O zuna distancia d3. Trasladar el sistema de coordenadas (u, v, w) a lo

largo de O x una distancia a

4. Rotar alrededor del eje O x un ángulo α.

La matriz de traslación homogénea resultante es:

i-1

Ai=[cosθ i −cos αi senθ i senα i senθi

senθ i cos αi cosθ i −senα i senθi

0 senα i cosα i

¿¿0 0 01](1)

III. OBTENCIÓN DE LOS PARÁMETROS PARA EL ALGORITMO DE DENAVIT - HARTENBERG

En la figura 2 se observa la disposición del brazo robótico, en la misma figura también se observa cómo se colocaron los ejes de referencia para cada articulación y la numeración que se le da a cada articulación, esta misma numeración será tomada en cuenta para los cálculos de las matrices de transformación homogénea de Denavit - Hartenberg, cabe señalar que el eje Z de cada articulación está en función del eje de giro de dicha articulación.

[1] Tomado de la referencia 1

Realizado por: Jefferson Gutama, Gustavo Parra, Jaime Vásquez.Revisado por: Ing. Eduardo Calle Universidad Politécnica Salesiana

Carrera de Ingeniería Eléctrica

Análisis cinemático directo e inverso del brazo robótico

1

Figura 2. Disposición de los eslabones para el cálculo de los parámetros de Denavit - Hartenberg

Luego de aplicado todos los pasos que se nos indicaron en clase para la obtención de los parámetros de este algoritmo los resultados obtenidos fueron:

Tabla 1. Parámetros de Denavit - HartenbergArticulación ϴ d a α

1 90+ϴ1

0.064 0 90

2 90+ϴ2

0 0.08 0

3 ϴ3 0 0.095 04 ϴ4 0 0.032 0

Los valores de d y a fueron tomados de las dimensiones propias del brazo.

IV. CINEMÁTICA DIRECTA

Ahora aplicando la formula 1 para cada eslabón tendríamos las siguientes matrices:

0 A1=[−sen θ1 0 cosθ1

cosθ1 0 senθ1

0 1 0

¿¿0 0 01](2)

1 A2=[−sen θ2 −cosθ2 0cosθ2 −senθ2 0

0 0 1

¿¿ 0 00 1](3)

2 A3=[cosθ3 −senθ3 0sen θ3 cosθ3 0

0 0 1

¿¿0 001](4)

3 A4=[cosθ4 −senθ4 0senθ4 cosθ4 0

0 0 1

¿¿0 0 01](5)

Encontramos la matriz de transformación homogénea propuesta por Denavit – Hartenberg que no es más que:

T=¿0 A1 ∙1 A2 ∙2 A3 ∙3 A4(6 a)

Por cuestiones de espacio físico se describirá elemento por elemento de la matriz T

T 12=0.5 sen (−θ4−θ3+θ1−θ2)+0.5 sen ( θ4+θ3+θ1+θ2 ) T 13=cos (θ1) T 14=0.016 cos (−θ4−θ3+θ1−θ2 )−0.016 cos (θ4+θ3+θ1+θ2 )+0.0475 cos (−θ3+θ1−θ2)−0.0475 cos (θ3+θ1+θ2 )+0.04 cos (θ1−θ2 )−0.04 cos (θ1+θ2 )

T 21=−0.5 sen ( θ4+θ3+θ1+θ2 )+0.5 sen (−θ4−θ3+θ1−θ2 ) T 22=−0.5 cos (θ4+θ3+θ1+θ2)−0.5 cos (−θ4−θ3+θ1−θ2 ) T 23=¿ sen (θ1)T 24=−0.016 sen (θ4+θ3+θ1+θ2 )+0.016 sen (−θ4−θ3+θ1−θ2 )−0.0475 cos (θ3+θ1+θ2 )+0.0475 cos (−θ3+θ1−θ2 )−0.04cos (θ1+θ2 )+0.04 cos (θ1−θ2 )

T 31=cos (θ2+θ3+θ4 ) T 32=−sen (θ2+θ3+θ4 ) T 33=¿ 0T 34=0.032 cos (θ2+θ3+θ4 )+0.064+0.095 cos (θ2+θ3 )+0.08 cos (θ2 )

T 41=0

T 42=0

T 43=¿ 0T 44=1

Con lo que la matriz deseada sería la siguiente:

T=[T 11 T 12 T 13

T 21 T 22 T 23

T 31 T 32 T 33

¿¿

T 41T 42T 43T 44](6 b)

Como sabemos la matriz de transformación homogénea está formada por 4 matrices que son: Rotación (R), traslación (P), perspectiva (f) y escalado (W).

MTH=[R3 x3 P3 x 1

R1 x 3 W 1 x 1](7)

Para la robótica esta matriz tiene unos cambios que se tallan a continuación:

MTH=[ R3 x3 P3 x 1

[0,0,0] [1] ](8)

Como para la cinematica directa nos interesa únicamente la traslación que nos servirán para determinar la posición P(x, y, z) del extremo final del robot, entonces solo tomaremos de la matriz los elementos de traslación que son: T 14, T 24 y T 34 que corresponderían a x, y y z respectivamente por lo que:

x=0.016 cos (−θ4−θ3+θ1−θ2 )−0.016 cos (θ4+θ3+θ1+θ2 )+0.0475 cos (−θ3+θ1−θ2 )−0.0475 cos (θ3+θ1+θ2 )+0.04 cos (θ1−θ2 )−0.04 cos (θ1+θ2)

y=−0.016 sen ( θ4+θ3+θ1+θ2 )+0.016 sen (−θ4−θ3+θ1−θ2 )−0.0475 cos (θ3+θ1+θ2 )+0.0475 cos (−θ3+θ1−θ2 )−0.04 cos (θ1+θ2)+0.04 cos (θ1−θ2 )

z=0.032 cos (θ2+θ3+θ4 )+0.064+0.095 cos (θ2+θ3 )+0.08 cos (θ2 ) Debido a cuestiones de simulaciones en computadora (Matlab 7.0) para verificar que las ecuaciones sean las correctas, se procedió a hacer un cambio en las ecuaciones de x, y y z


2

porque Simulink de Matlab 7.0 cambia la disposición de los ejes, el cambio que se hace es:

x=0.016 cos (−θ4−θ3+θ1−θ2 )−0.016 cos (θ4+θ3+θ1+θ2 )+0.0475 cos (−θ3+θ1−θ2 )−0.0475 cos (θ3+θ1+θ2 )+0.04 cos (θ1−θ2 )−0.04 cos (θ1+θ2) (9)

y=0.032cos (θ2+θ3+θ4 )+0.064+0.095 cos (θ2+θ3 )+0.08 cos (θ2) (10)

z=−[−0.016 sen ( θ4+θ3+θ1+θ2 )+0.016 sen (−θ4−θ3+θ1−θ2 )−0.0475 cos (θ3+θ1+θ2 )+0.0475 cos (−θ3+θ1−θ2 )−0.04 cos (θ1+θ2)+0.04 cos (θ1−θ2 )](11)

Las ecuaciones (9), (10) y (11) son las que utilizaremos para el análisis cinemático directo

V. CINEMÁTICA INVERSA

Para encontrar los valores el valor de las variables articulares (θ1 , θ2 ,θ3 y θ4) requiere plantear nuevamente la matriz de traslación homogénea de la forma básica la cual se detalla a continuación:

T=[nx ox ax

n y o y a y

nz oz az

¿¿0 0 01] (12)

Con lo que ya se expuso en la ecuación (6a) se procede de la siguiente manera para encontrar los valores de las variables articulares:

a) Para encontrar θ1se despeja 0 A1 de la ecuación (6a) con lo que se tendría la siguiente ecuación

(0 A1 ¿−1 ∙T=¿1 A2 ∙2 A3 ∙3 A4(13 a)

Desarrollando por separado tendríamos:

(0 A1 ¿−1 ∙T=A=[A11 A12 A13

A21 A22 A23

A31 A32 A33

¿¿

A41 A42 A43 A44]

A11=−nx sen (θ1 )+ny cos (θ1 ) A12=−ox sen (θ1 )+o y cos (θ1 ) A13=−ax sen (θ1 )+a y cos (θ1 ) A14=−x sen (θ1 )+ y cos (θ1 )

A21=nz

A22=oz

A23=az

A24=−0.064+z

A31=nx cos (θ1 )+n y sen (θ1)

A32=ox cos (θ1)+oy sen (θ1 ) A33=ax cos (θ1)+ay sen (θ1 ) A34=xcos (θ1 )+ y sen (θ1 )

A21=0

A22=0

A23=0

A24=1

1 A2 ∙2 A3 ∙3 A4=B=[B11 B12 B13

B21 B22 B23

B31 B32 B33

¿¿

B41 B42B43 B44]

B11=−sen (θ2+θ3+θ4 ) B12=−cos (θ2+θ3+θ4 ) B13=0

B14=−0.032 sen (θ2+θ3+θ4 )−0.095 sen (θ2+θ3 )−0.08 sen (θ2 )

B21=cos (θ2+θ3+θ4 ) B22=−sen (θ2+θ3+θ4 ) B23=0

B24=0.032 cos ( θ2+θ3+θ4 )+0.095 cos (θ2+θ3 )+0.08 cos (θ2)

B31=0

B32=0

B33=1

B34=0

B41=0

B42=0

B43=0

B44=1

Al igualar las ecuaciones tendríamos:

[ A11 A12 A13

A21 A22 A23

A31 A32 A33

¿¿

A41 A42 A43 A44]=[B11 B12 B13

B21 B22 B23

B31 B32 B33

¿¿

B41B42 B43 B44](13 b)

Comparando término a término ambas matrices podemos ver que es más factible despejar θ1 igualando los términos A34

con B34 con lo que tendríamos:

xcos (θ1 )+ y sen (θ1)=0de donde


3

θ1=tan−1( xy)(14 )

b) Para encontrar θ2se despeja 1 A2 de la ecuación (13a) con lo que se tendría la siguiente ecuación

(1 A2 ¿−1 ∙ (0 A1 ¿−1 ∙T=¿2 A3 ∙3 A4(15 a)


(1 A2 ¿−1 ∙ (0 A1 ¿−1 ∙T=C=[C11 C12 C13

C21 C22 C23

C31 C32 C33

¿¿

C41C42C43C44]

C11=nx sen (θ1) sen (θ2 )−ny cos (θ1) sen (θ2 )+nz cos (θ2 ) C12=ox sen (θ1 ) sen (θ2 )−oy cos (θ1 ) sen (θ2 )+ozcos (θ2 ) C13=ax sen (θ1 ) sen (θ2 )−ay cos (θ1 ) sen (θ2 )+az cos (θ2 ) C14=x sen (θ1 ) sen (θ2 )− ycos (θ1 ) sen ( θ2 )+zcos (θ2 )−0.08−0.064 cos (θ2 )

C21=nx sen (θ1 ) cos (θ2 )−ny cos (θ1) cos (θ2 )−nz sin (θ2 ) C22=ox sen (θ1 ) cos (θ2 )−o y cos (θ1 ) cos (θ2 )−oz sin (θ2 ) C23=ax sen (θ1 ) cos (θ2 )−a y cos (θ1 ) cos (θ2)−az sin (θ2 ) C24=x sen (θ1) cos (θ2 )− y cos ( θ1 ) cos (θ2 )−zsin (θ2)+0.064 sin (θ2 )

C31=nx cos (θ1)+ny sen ( θ1 ) C32=ox cos ( θ1 )+o y sen (θ1) C33=ax cos ( θ1 )+a y sen (θ1 ) C34=xcos (θ1 )+ y sen (θ1 )

C21=0

C22=0

C23=0

C24=1

2 A3 ∙3 A4=D=[D11 D12 D13

D21 D22 D23

D31 D32 D33

¿¿

D 41 D42 D 43 D44]

D11=cos (θ3+θ4 ) D12=−sen (θ3+θ4 ) D13=0

D14=0.032cos (θ3+θ4 )+0.095 cos (θ3 )

D21=sen (θ3+θ4 )

D22=cos ( θ3+θ4 ) D23=0

D24=0.032 sen (θ3+θ4 )+0.095 sen (θ3 )

D31=0

D32=0

D33=1

D34=0

D41=0

D42=0

D43=0

D44=1


[C11 C12 C13

C21 C22 C23

C31 C32 C33

¿¿

C41C42C43C44]=[D11 D12 D13

D21 D22 D23

D31 D32 D33

¿¿

D 41 D42 D 43 D44](15 b)

Comparando término a término ambas matrices podemos ver que es más factible despejar θ2 igualando los términos C24

con D24 con lo que tendríamos:

x sen (θ1) cos (θ2 )− ycos ( θ1 ) cos (θ2 )−zsin (θ2) 0.064 sin (θ2 )=0.032 sen (θ3+θ4 )+0.095 sen (θ3 )(16)

Al despejar θ2 se presento el inconveniente de que la respuesta encontrada era muy extensa, por lo que se decidió adjuntar en el ANEXO 1 el valor de θ2. Cabe destacar que se

cambio θ1por q1 por facilidad de cálculo computacional.

c) Para encontrar θ3se despeja 2 A3 de la ecuación (13a) con lo que se tendría la siguiente ecuación

(2 A3 ¿−1 ∙ (1 A2 ¿−1 ∙ (0 A1 ¿−1 ∙T=¿3 A4(17 a)


(2 A3 ¿−1 ∙ (1 A2 ¿−1 ∙ (0 A1 ¿−1 ∙T=E=[E11 E12 E13

E21 E22 E23

E31 E32 E33

¿¿

E41 E42 E43 E44]

E11=0.5 nx cos (−θ3+θ1−θ2 )−0.5 nx cos (θ3 +θ1+θ2 )−0.5 n y sen (θ3 +θ1+θ2 )+0.5 n y sen (−θ3 +θ1−θ2 )+nz cos (θ2+θ3 ) E12=0.5 ox cos (−θ3+θ1−θ2 )−0.5 ox cos (θ3+θ1+θ2 )−0.5 o y sen (θ3+θ1 +θ2 )+0.5 oy sen (−θ3+θ1−θ2 )+oz cos (θ2+θ3 ) E13=0.5 ax cos (−θ3+θ1−θ2 )−0.5 ax cos (θ3+θ1+θ2 )−0.5 a y sen (θ3 +θ1+θ2 )+0.5 a y sen (−θ3 +θ1−θ2 )+az cos (θ2+θ3 )


4

E14=0.5 xcos (−θ3+θ1−θ2)−0.5 x cos (θ3 +θ1+θ2 )−0.5 y sen (θ3+θ1+θ2 )+0.5 ysen (−θ3+θ1−θ2 )+z cos (θ2+θ3 )−0.095−0.064 cos (θ2+θ3 )−0.08 cos (θ3)

E21=0.5 nx sen (θ3+θ1+θ2 )+0.5 nx sen (−θ3+θ1−θ2 )−0.5 ny cos (−θ3+θ1−θ2 )−0.5 ny cos (θ3+θ1 +θ2 ) sen (θ2 )−nz sen (θ2+θ3 ) E22=0.5 ox sen (θ3+θ1+θ2 )+0.5 ox sen (−θ3 +θ1−θ2 )−0.5 oy cos (−θ3+θ1−θ2 )−0.5 o y cos (θ3+θ1+θ2 ) sen (θ2 )−oz sen (θ2+θ3 ) E23=0.5 ax sen (θ3+θ1+θ2 )+0.5 ax sen (−θ3 +θ1−θ2 )−0.5 ay cos (−θ3 +θ1−θ2 )−0.5 ay cos (θ3 +θ1 +θ2 ) sen ( θ2 )−az sen (θ2+θ3 ) E24=0.5 x sen (θ3 +θ1+θ2 )+0.5 x sen (−θ3 +θ1−θ2 )−0.5 ycos (−θ3+θ1−θ2 )−0.5 ycos (θ3+θ1 +θ2 ) sen (θ2 )−z sen (θ2+θ3 )+0.064 sen (θ2+θ3 )+0.08 sen (θ3 )

E31=nx cos (θ1 )+n y sen (θ1) E32=ox cos (θ1)+o y sen (θ1 ) E33=ax cos (θ1)+ay sen (θ1 ) E34=xcos (θ1 )+ y sen (θ1 )

E21=0

E22=0

E23=0

E24=1

3 A4=[cosθ4 −senθ4 0senθ4 cosθ4 0

0 0 1

¿¿0 0 01]


[E11 E12 E13

E21 E22 E23

E31 E32 E33

¿¿

E41 E42 E43 E44]=[cos θ4 −senθ4 0

senθ4 cosθ4 00 0 1

¿¿0 0 01](17 b)

Comparando término a término ambas matrices podemos ver que es más factible despejar θ3 igualando los términos E24

con 0.032 ∙ senθ4 con lo que tendríamos:

0.5 x sen (θ3+θ1 +θ2 )+0.5 x sen (−θ3+θ1−θ2)−0.5 ycos (−θ3+θ1−θ2 )−0.5 ycos (θ3+θ1+θ2) sen (θ2 )−z sen (θ2+θ3 )+0.064 sen (θ2+θ3 )+0.08 sen (θ3 )=0.032 ∙ senθ4(18)

Al despejar θ3 se presento el inconveniente de que la respuesta encontrada era muy extensa, por lo que se decidió adjuntar en el ANEXO 2 el valor de θ3.

Al intentar encontrar el valor θ4nos encontramos con la dificultad de encontrar menos ecuaciones que variables para despejar, por lo que se intento utilizar el método de desacoplo cinemático pero los valores encontrados no fueron los deseados, por lo que por cuestiones de tiempo se decidió dejar el brazo robótico con 3 grados de libertad únicamente.

VI. CONCLUSIONES

Las principales conclusiones a las que se llego son:

Los parámetros encontrados para el algoritmo de Denavit – Hartenberg dependo en gran medida del posicionamiento que se le da al brazo, el colocar para nuestro caso el brazo de manera vertical (figura 1) nos ayuda primero a que tres de los cuatro ejes de rotación estén alineados y así se reduce el número de parámetros encontrados (Tabla 1) este a su veces tiene directa incidencia en la matriz de transformación homogénea encontrada (T) por dicho método. Por lo que nuestro análisis matemático no revea mayor complicación de la que ya se analizo en este ensayo

El análisis cinemático directo del brazo es sencillo de realizar ya que el procedimiento es más corto al usar únicamente la matriz de transformación homogénea para hallar las ecuaciones de x, y y z para el posicionamiento espacial final del brazo. Otro hecho que simplifica el análisis es que se igualan matrices de dimensiones, P3 x 1 al lado izquierdo de la

ecuación con una matriz de dimensiones T 4 x4, con lo que se tiene para el análisis únicamente una ecuación por cada variable.

A diferencia del análisis cinemático directo el análisis cinemático inverso tiene un mayor grado de complejidad debido a que para encontrar las cada una de las variables articulares es necesario realizar cambios en los procedimientos de cálculo. El encontrar ecuaciones para cada variable articular se convierte en un proceso complejo ya que se dispone en cada lado de la ecuación de 12 posibles soluciones. Basta con referirse a los ANEXOS 1 y 2 para notar las dimensiones que toma las soluciones tanto para θ2 yθ3. La complejidad de la cinemática inversa nos llevo a tomar la decisión de reducir los grados de libertad del brazo de cuatro grados de libertad a tres grados de libertad, debido a que hallar la ecuación para θ4 torno muy complejo e incluso se tuvo que recurrir a análisis complementarios a los que se vio en clases (método de desacoplo cinemático) para intentar hallar el valor de θ4 pero no se encontró la solución para esta variable que satisfaga nuestro análisis.

Si bien encontrar soluciones para las variables de posición y articulares pueden hallarse únicamente con un análisis matemático, el uso de simuladores de movimiento (Simulink) nos ayuda a comprobar que estas ecuaciones sean las que realmente describen el movimiento espacial del robot para situarse en una posición deseada, en base a este criterio cabe señalar que para la cinemática inversa para las ecuaciones de θ2 yθ3 se hallaron dos soluciones para cada una de ellas, entonces se procedió a elegir únicamente una solución para cada variable para dicha elección se utilizo el modelo de realidad virtual de nuestro brazo


5

robótico de Simulink, se cargo las soluciones encontradas y se procedió a simular, para las cuatro ecuaciones se llego a la posición deseada pero en dos las soluciones (una de cada variable) si bien se llego a la posición era físicamente imposible que el robot realice la trayectoria deseada, debido a que, la trayectoria tenia implícito que el brazo atraviese la base del robot por lo que se descarto estas dos soluciones y se tomo las soluciones que se detallan en el ensayo

VII. REFERENCIAS

[1] Barrientos Antonio, Peñin Luis, Balaguer Carlos, Aracil Rafael. Fundamentos de Robótica. Primera edición. España: McGraw-Hill, 1997.

[2] Gutama Jefferson, Parra Gustavo, Vásquez Jaime. Apuntes de Robótica. Carrera de Ingeniería Eléctrica. Universidad Politécnica Salesiana. Cuenca 2010


6

Cinematic A Directa e Inversa

Documents

Transcript of Cinematic A Directa e Inversa