Desarrollo de Un Entorno de Simulacion Para Un Robot Indust

8/8/2019 Desarrollo de Un Entorno de Simulacion Para Un Robot Indust

1/76

1

INTRODUCCION

Con el desarrollo de los computadores desde el siglo pasado hasta nuestros das,

muchas de las actividades humanas han sido notablemente influenciadas por la

sistematizacin. La ciencia, la ingeniera y las comunicaciones han sido motores

de desarrollo de este campo ya que son las reas mas beneficiadas por sus

logros.

Es muy comn encontrar que muchas tareas se realizan con el uso de

computadores, por ejemplo: transacciones bancarias, facturas, cartas, problemas

numricos complejos, dibujo de planos, clculo de estructuras, etc. Cada tarea

requiere de una aplicacin especfica que se encarga de hacer que el computador

responda de la manera requerida.

La necesidad cada vez ms apremiante de aumentar la productividad y conseguir

productos acabados de calidad uniforme, hace que la industria gire cada vez ms

hacia la automatizacin apoyada en computador. En el momento actual, la

mayora las tareas de fabricacin automatizadas se realizan mediante mquinas

de uso especial para realizar funciones en un proceso de manufactura.

Generalmente la inflexibilidad y el alto costo de estas mquinas, llamadas a

menudo sistemas de automatizacin duros, ha llevado a un inters creciente en el

uso de robots capaces de efectuar una variedad de funciones de fabricacin en un

entorno de trabajo ms flexible y a un coste menor de produccin.


2/76

2

Las universidades estn llamadas a ser las pioneras en introducir estas

tecnologas al mercado nacional como es el caso de la Universidad del Valle que

esta haciendo avances en el grupo de robtica con un proyecto a largo plazo con

miras a construir una celda de manufactura flexible algunos de tales avances son:

el trabajo acerca del robot industrial de tipo SCARA1, el trabajo que trata del

modelo dinmico del robot CERMA UNIVALLE2, el trabajo acerca de la

planeacin dinmica de procesos de manufactura en celdas flexibles3, y el trabajo

sobre el seguimiento de trayectorias predefinidas en el desplazamiento de robots.

Durante el ao 2000 el grupo de robtica de la Universidad del Valle trabajo en un

proyecto de investigacin destinado al diseo del robot articulado tipo industrial

que hace parte de una celda flexible de manufactura preconcebida, trabajo

realizado por el Profesor Jos Isidro Garca como tesis de la maestra en

automtica, este trabajo de grado pretende ser un complemento y ampliacin de el

trabajo de maestra mencionado.

Un robot industrial en un manipulador de uso general controlado por computador

que se compone de elementos rgidos conectados en serie mediante

articulaciones prismticas o de revolucin. El final de la cadena est fijo a una

1

ABADIA N. JOSE ANTONIO. Informe cientfico final del proyecto de investigacin, Especificaciny diseo, construccin y operacin y mantenimiento de un robot industrial de tipo scara con unaarquitectura de pilotaje, tiempo real y multiprocesador, etapa IV Santiago de Cali, Diciembre 1996. 2TOMAS BUITRAGO, Modelo dinmico del robot CERMA UIVALLE, proyecto de investigacin.

Maestra en automtica, programa de postgrado en ingeniera elctrica y electrnica y de lacomputacin, Santiago de Cali 1998.3 ELIOT MOTATO ESCOBAR, Planeacin Dinmica de procesos de manufactura en celdas

flexibles, proyecto de investigacin. Maestra en automtica, programa de postgrado en ingenieraelctrica y electrnica y de la computacin, Santiago de Cali 1999


3/76

3

base soporte, mientras el otro extremo est libre y equipado con herramienta para

manipular objetos o realizar tareas de montaje. El movimiento de las

articulaciones resulta en, o produce, un movimiento relativo de los distintos

elementos. Mecnicamente, un robot se compone de un brazo y de una mueca

ms una herramienta. Se disea para alcanzar una pieza de trabajo localizada

dentro de su volumen de trabajo. El volumen de trabajo es la esfera de influencia

de los robots, cuyo brazo puede colocar el sub-montaje de la mueca en cualquier

punto de la esfera. El brazo generalmente se puede mover con 3 grados de

libertad. La combinacin de los movimientos posiciona la mueca sobre la pieza

de trabajo. La mueca normalmente consta de tres movimientos giratorios. La

combinacin de estos movimientos orienta la pieza de acuerdo a la configuracin

del objeto para facilitar su alcance. Estos tres ltimos movimientos se denominan

a menudo elevacin, desviacin y giro (Pitch, Yaw y Roll). Por tanto, para un robot

con seis articulaciones, el brazo es el mecanismo posicionamiento, mientras que

la mueca es el mecanismo de orientacin.

Hoy da vemos la robtica con un campo de trabajo mucho ms amplio que el que

se tena el pasado, tratando con investigaciones y desarrollos en una serie de

reas interdisciplinarias, que incluyen cinemtica, dinmica, planificacin de

sistemas, control, sensores, lenguajes de programacin e inteligencia artificial.


4/76

4

OBJETIVOS

Objetivo General.

Crear una aplicacin

4

que se ejecute en la plataforma MicrosoftWindows para asistir en el anlisis cinemtico y dinmico de un Robot

industrial que es parte de una celda de manufactura flexible, proyecto

del laboratorio de robtica de la Universidad del Valle, en el que han

estado trabajando en los ltimos aos.

Objetivos Especficos

Simular grficamente en tiempo real el funcionamiento del robot, luego

de haberle programado una trayectoria.

Permitir al usuario modificar o tomar decisiones sobre los resultados

basndose en su criterio y experiencia.

Estar en capacidad de reajustar la simulacin.

Interesar al usuario mediante una interfase fcil de emplear. Elaborar un informe que explique los fundamentos de la aplicacin y sus

restricciones.

4La aplicacin se denomino S2A2RD v 0.0, acronimo en ingles de Software of Simulation and

Anlisis for Aided Robots Design.


5/76

5

JUSTIFICACIN

La ingeniera desarrolla un papel preponderante en el desarrollo de la industria, y

en ella el rea de la robtica industrial aplicada es un factor importante en la

optimizacin de los procesos de produccin, pero debido a los altos costos de su

tecnologa no ha sido adoptada en nuestro medio y en donde se ha hecho, no ha

sido integrado completamente en el proceso productivo.

Nuestro medio se ve afectado por el descuido tecnolgico a que se ha sometido el

sector de la manufactura de productos, esto se debe a que se han obtenido

resultados aceptables sin desarrollar tecnologas avanzadas; tecnologas que

como la robtica de ser implantada de forma adecuada, dara un gran impulso a

la industria hacindola mas competitiva a nivel internacional.

En este trabajo se seala que es el momento propicio para aprovechar la

tecnologa que se desarrolla actualmente en torno a la grafica computacional y el

desarrollo de software en el rea de ingeniera, tecnologa con la que el autor de

este documento ha estado trabajando en los ltimos 4 aos; y el trabajo que en los

ltimos aos se ha realizado en la Universidad del Valle por parte docentes en el

rea de robtica. La implantacin de la robtica en nuestro medio depende de

una disminucin de sus costos, no tanto en el hardware, sino en la tecnologa de

control y diseo de robots, tecnologa fundamentada en buena parte en el

entendimiento y manejo de los modelos cinemticos y dinmicos, entendimiento y

manejo sobre los que gira este trabajo.


6/76

6

1. CARACTERISTICAS DEL MANIPULADOR

Como se menciono anteriormente en la introduccin este manipulador5 fue

diseado en el Laboratorio de Robtica de la Universidad del Valle por el Profesor

Jos Isidro Garca como parte de una celda de manufactura flexible, las siguientes

caractersticas son tomadas y adaptadas de su trabajo.

Estructura: Robot articulado vertical de 5 ejes con desplazamiento

Cargas: 20 Kg.

Precisin: Exactitud = 0.5 mm

Repetitividad = 0.02 mm

Velocidad: Resolucin = Controlado por software

Velocidad de la herramienta: Controlado por software

Aceleraciones y desaceleraciones: Controlados por software

Alcance: Eje Z0: Traslacin 1.05 m

Eje Z1: Rotacin 180

Eje Z2: Hombro -100/35

Eje Z3: Codo 70

Eje Z4: Pitch 90

Eje Z5: Roll 180

Peso: 24 Kg

5 Los trminos manipulador y robot industrial son utilizados como sinnimos en este documento.

Ilustracin 1.MARCOS DE REFERENCIA


7/76

7

Para la obtencin de los parmetros fsicos6 de las diferentes partes que

conforman el manipulador se utiliz el programa comercial MECHANICAL

DESKTOP, cuyos resultados se enuncian a continuacin:

PARTE 1: Translacin

Masa 4.5102 Kg.

Volumen 1.664 x 10-3 mm3

Centro de Masa X: 0 m

Y: -0.0533 m

Z: 0 m

Momentos de inercia X: 0.0255 Kg. m2

Y: 0.0196 Kg. m2

Z: 0.0291 Kg. m2

Productos de inercia XY: 0 Kg. m2

XZ: 0 Kg. m2

6 Los centros de masa, momentos y productos de inercia estn calculados respecto al marco de

referencia local de la Parte-Elemento.

Ilustracin 2. UBICACIN DE LOS EJES

Ilustracin 3.TRANSLACION


8/76

8

YZ: 0 Kg. m2

Parte 2: Rotacin

Masa: 16.4456 Kg.

Volumen: 6.068 x 10-3 m3

Centro de Masa: X: 0.6545 m

Y: -0.544 m

Z: -0.9267 m

Momentos de inercia: X: 19.3295 Kg. m2

Y: 21.3357 Kg. m2

Z: 12.2623 Kg. m2

Productos de inercia XY: -5.8478 Kg. m2

XZ:-10.0106 Kg. m2

YZ: 8.2816 Kg. m2

Parte 3: Hombro

Masa: 1.5271 Kg.

Volumen: 0.563 x 10-3 m3


Y: 0 m

Z: 0 m


Y: 0.0672 Kg. m2

Z: 0.0638 Kg. m2

Productos de inercia: XY: 0 Kg. m2

XZ: 0 Kg. m2

Ilustracin 4.ROTACION

Ilustracin 5.HOMBRO


9/76

9

YZ: 0 Kg. m2

Parte 4: Codo

Masa: 1.3727275 Kg.

Volumen: 0.506 x 10-3 m3


Y: 0 m

Z: 0 m


Y: 0.0185 Kg. m2

Z: 0.0174 Kg. m2


XZ: 0 Kg. m2

YZ: 0 Kg. m2

Parte 5: Pitch

Masa: 0.0967 Kg.

Volumen: 0.0357 x 10-3 m3

Centro de Masa: X: 0

Y: 0

Z: 0

Momento de inercia: X: 0 Kg. m2

Y: 0 Kg. m2

Z: 0 Kg. m2


XZ: 0 Kg m2

Ilustracin 6. CODO

Ilustracin 7. PITCH


10/76

10

YZ: 0 Kg m2

Parte 6: Roll

Masa: 0.381 Kg.

Volumen: 0.1408 x 10-3 m3

Centro de Masa: X: 0 m

Y: 0 m

Z: 0.0778 m

Momento de inercia: X: 0.0026 Kg. m2

Y: 0.025 Kg. m2

Z: 0.003 Kg. m2

Producto de inercia: XY: 0 Kg. m2

XZ: 0 Kg. m2

YZ: 0

2. MARCO TEORICO

2.1 Matrices de rotacin

Una matriz de rotacin 3x3 se define como una matriz de transformacin que

opera sobre un vector de posicin y transforma sus coordenadas expresadas en

un sistema de coordenadas rotado Ouvw sobre un sistema de coordenadas de

referencia Oxyz. Fsicamente, se puede considerar que Ouvw es un sistema ligado

al cuerpo. Esto significa que esta permanente y convenientemente unido al cuerpo

rgido movindose con l.

Ilustracin 8. ROLL


11/76

11

Sean (ix, jy, kz) y (iu, jv, kw) los vectores unitarios a lo largo de los ejes de

coordenadas de los sistemas Oxyz y Ouvw, respectivamente. El vector de posicin

P se puede representar por sus coordenadas con respecto a ambos sistemas.

Para facilitar el anlisis, se supone que P est en reposo y fijo respecto a Ouvw.

Entonces el vector P se puede representar respecto a Ouvw y Oxyz,

respectivamente, como:

Puvw = (pu, pv, pw)T y Pxyz = (px, py, pz)

T

Ecuacin 1.

Donde Puvw y Pxyz representan el mismo vector P en el espacio en diferentes

sistemas de coordenadas y el superndice T denota la operacin transpuesta en

vectores y matrices.

Ilustracin 9. SISTEMAS DE COORDENADAS DE REFERENCIA YLIGADO AL CUERPO


12/76

12

Nuestro inters es la matriz R 3x3 que transforma Pxyz en Puvw, despus de que

OUVW ha sido girado. Esto es:

Pxyz = RPuvw

Ecuacin 2.La matriz R est dada por:

Ecuacin 3.

Anlogamente se puede obtener

Puvw = QPxyz

Ecuacin 4.

Como las matrices de rotacin son ortogonales y los productos escalares son

conmutativos, entonces:

Q = R-1 = RT

Ecuacin 5.


13/76

13

2.2 Matriz de transformacin.

La matriz de transformacin es una matriz 4x4 que transforma un vector de

posicin desde un sistema coordenado a otro. Una matriz de este tipo se puede

considerar que consiste de cuatro submatrices:

Ecuacin 6. Matriz de transformacin.

La submatriz 3x3 superior izquierda representa la matriz de rotacin; la submatriz

superior derecha 3x1 representa el vector de posicin del origen del sistema

rotado con respecto al sistema de referencia; la submatriz inferior izquierda de 1x3

representa la transformacin de perspectiva (en simulacin es nulo); y el cuarto

elemento diagonal es el factor de escala global (para nuestro fin es siempre 1).

2.3 Cinemtica

La cinemtica estudia la posicin y orientacin de la herramienta del robot (efector

final) a partir de la posicin y orientacin de las articulaciones.

Todo robot esta formado de articulaciones rotacionales y/o prismticas, cada una

con un grado de libertad (DOF), cuando la articulacin es rotacional, la variable de


14/76

14

articulacin es un ngulo y si por el contrario es prismtica, la variable de

articulacin es una distancia.

2.4 Formulacin de Denavit Hartenberg

Los robots industriales son modelados por un mtodo matricial desarrollado en

1955 por Denavit y Hartenberg, este mtodo establece en forma sistemtica un

sistema coordenado para cada elemento de la cadena cinemtica. La

representacin de Denavit Hartenberg (D-H) da como resultado una matriz de

transformacin homognea 4x4 que representa cada uno de los sistemas

coordenados de los elementos en la articulacin con respecto al sistema de

coordenadas del elemento previo. As, mediante transformaciones secunciales, el

efector final se puede expresar en las coordenadas de la base que constituye el

sistema inercial.

La representacin D-H de un elemento rgido depende de 4 parmetros

geomtricos asociados a cada elemento. Estos parmetros se definen as:

Ilustracin 10. SISTEMASCOORDENADOS


15/76

15

i: Es el ngulo de la articulacin del eje X i-1 al eje Xi respecto al eje Zi-1 (usando la

regla de la mano derecha).

di: Es la distancia desde el origen del sistema coordenado (i-1) hasta la

interseccin del eje Zi-1 con el eje Xi a lo largo del eje Zi-1.

ai : Es la distancia de separacin desde la interseccin del eje Z i-1 con el eje Xi

hasta el origen del sistema i-simo a lo largo del eje Xi.

i : Es el ngulo de separacin del eje Z i-1 al eje Zi respecto al eje Xi (usando la

regla de la mano derecha).

Para una articulacin giratoria (rotacional), di, ai y i son los parmetros de

articulacin permaneciendo constantes, mientras que i es la variable de

articulacin y cambia cuando el elemento i gira respecto al elemento i-1. Para una

articulacin prismtica, i, ai y i son los parmetros y di es la variable que cambia

cuando el elemento se mueve.

Con los parmetros se define la matriz de transformacin en cada articulacin i.

Para obtener la posicin y orientacin del efector final respecto a la base, se post-

multiplican las matrices de cada articulacin desde la base hasta la articulacin n.


16/76

16

0A11A2

2A3i-2Ai-1

i-1Ain-1An=A : Cada

i-1Ai contiene una incgnita bien sea i o di

Como resultado, el problema cinemtico inverso corresponde al clculo de las

incgnitas que satisfagan la anterior ecuacin matricial.

2.5 Dinmica

La dinmica de robot, por otra parte, trata con la formulacin matemtica de las

ecuaciones del movimiento del brazo. Las ecuaciones dinmicas del movimientode un manipulador son un conjunto de ecuaciones matemticas que escriben la

conducta dinmica del manipulador. Tales ecuaciones de movimiento son tiles

para la simulacin por computador. Este modelo dinmico se obtiene de las leyes

fsicas conocidas. Lo que conduce a la obtencin de las ecuaciones dinmicas

para las distintas articulaciones del manipulador en trminos de los parmetros

geomtricos en inerciales especificados en los distintos elementos.

2.6 Formulacin de Lagrange-Euler7

Las ecuaciones de movimiento que genera un manipulador se pueden expresar

convenientemente mediante la aplicacin directa de la formulacin de Lagrange-

Euler a sistemas no conservativos. Muchos investigadores utilizan la

representacin de Denavit Hartenberg para describir el desplazamiento espacial

7La explicacin de esta formulacin fue adaptada del libro de Lee, Gonzlez, Fu. Robtica de

1990.


17/76

17

entre los sistemas de coordenadas de elementos vecinos para obtener la

informacin cinemtica del elemento, y emplean la tcnica dinmica lagrangiana

para deducir las ecuaciones dinmicas del manipulador. La aplicacin directa de

esta formulacin, junto con la representacin de coordenadas conveniente y

compacta de las ecuaciones de movimiento del manipulador. El mtodo se

expresamente mediante operaciones matriciales y facilita tanto su anlisis como

su implementacin en un computador.

La derivacin de las ecuaciones dinmicas de un manipulador con n grados de

libertad se basa en la comprensin de:

1. La matriz de transformacin de coordenadas homogneas 4x4, i-1Ai, describe la

relacin espacial entre los sistemas de coordenadas del elemento i-simo y el

elemento (i-1)-simo. Relacin a un punto fijado en el elemento i prestaban

coordenadas homogneas con respecto al sistema e coordenadas i-simo en el

sistema de coordenadas (i-1)-simo.

2. La ecuacin de Lagrange-Euler

Ecuacin 7. Ecuacin de Lagrange-Euler

Donde

L = funcin lagrangiana = energa cintica K energa potencial P;

K = energa cintica total del brazo;

P = Energa potencial total del brazo;

qi = coordenada generalizada el brazo;

vi = primera derivada respecto al tiempo de qi ;


18/76

18

i = fuerza (o par) generalizado aplicado al sistema en la articulacin i para mover

mover el elemento i.

En las ecuaciones de Lagrange-Euler se requiere escoger adecuadamente un

conjunto de coordenadas generalizadas para describir el sistema. Estas

coordenadas describen completamente la localizacin (posicin orientacin) de un

sistema respecto a otro de referencia. En general para los manipuladores existe

una correspondencia natural entre las coordenadas generalizadas y las variables

de articulacin definidas en las matrices de transformacin. As, en el caso de q i

giratoria qi =

i; mientras que si qi es prismtica qi = di.

La siguiente deduccin de las ecuaciones de movimiento de un manipulador de n

grados de libertad se basa en la matrices de transformacin tal y como de

desarrollaron en 2.2.

2.6.1 Velocidades de las articulaciones del robot

Para realizar la formulacin de Lagrange-Euler se debe conocer la energa cintica

del sistema y para ello se requiere saber la velocidad de cada articulacin. Se

deducir la velocidad de un punto fijo en el elemento i y los efectos del movimiento

de las otras articulaciones en todos los puntos del elemento i.


19/76

19

Respecto ala figura 11, seai

ri un punto fijo y en reposo en el elemento i respecto alsistema coordenado del elemento i-simo.

iri = (xi,yi,zi,1)T

Ecuacin 8.

Sea 0ri el mismo puntoiri

8con respecto al sistema coordenado de la base, i-1Ai la

matriz de transformacin que relaciona el desplazamiento espacial del sistema de

coordenadas del elemento i-simo respecto al (i-1)-simo y 0Ai la matriz de

transformacin que relaciona el sistema de coordenadas i-simo con el sistema de

la base; entonces

0ri =0Ai

iri Ecuacin 9.donde

0Ai =0A1

1A22A3

i-2Ai-1i-1Ai

Ecuacin 10.

8En el vector iri se incluye el cuarto trmino 1, por razones de compatibilidad con la matriz de

transformacin.

Ilustracin 11. Punto iri en el elemento i


20/76

20

Si la articulacin i es de revolucin, forma general de i-1Ai esta dada por:

i-1Ai =

Ecuacin 11.

Si la articulacin i es prismtica, la forma general de i-1Ai es:

i-1Ai =

Ecuacin 12.

Con el fin de deducir las ecuaciones de movimiento que son aplicables a ambos

tipos de articulacin, usaremos la variable qi para representar la coordenada

generalizada de la articulacin i.

Como el punto iri esta en reposo en el elemento i, y suponiendo su movimiento

como de cuerpo rgido, la velocidad de iri expresada en el sistema coordenado de

la base se puede expresar como

Ecuacin 13.


21/76

21

Esta forma compacta se obtiene porque las derivadas de iri son cero. La derivada

parcial de 0Ai con respecto a qj se puede calcular con facilidad gracias a la matriz

Qi que para una articulacin de revolucin de define como

Qi =

Ecuacin 14.

Y, para una articulacin prismtica como

Qi =

Ecuacin 15.

Por lo cual

Ecuacin 16.

Conviene anotar que para la derivada parcial de i-1Ai respecto a qi resulta una

matriz que no es de transformacin. Al premultiplicar por Qi la matriz i-1Ai se realiza

en esta ltima una operacin equivalente a intercambiar sus dos primeras filas,

negando la primera y anulando la tercera y cuarta; esto si i-1Ai es una matriz de

rotacin. Para una articulacin prismtica, el efecto es sustituir la tercera fila de


22/76

22

i-1Ai por la cuarta y anular las otras. La ventaja de usar las matrices Q i es que

permiten seguir utilizando las matrices i-1Ai y aplicar las operaciones que las

preceden.

De la aplicacin de la ecuacin anterior se tiene, para i = 1,2,, n

0A11A2

j-2Aj-1Qjj-1Aj

i-1Ai para ji

0 para ji

Ecuacin 17.

Esta ecuacin se puede interpretar como el efecto del movimiento de la

articulacin j sobre todos los puntos de en el elemento i. Para simplificar las

notaciones, se define Uij = 0Ai/qj, la anterior ecuacin se puede escribir como:

0Aj-1Qjj-1Ai para ji

0 para j>i

Ecuacin 18.

Utilizando esta notacin, Vi9se puede expresar como

Ecuacin 19. Velocidad de iri respecto a la base

9No se debe confundir el vector de velocidad del elemento V i con el escalar vi que es simplemente

la derivada respecto al tiempo de q i.


23/76

23

A continuacin los efectos de la interaccin entre las articulaciones se escriben

como Uijk = Uij/qk, de la siguiente manera:

0Aj-1Qjj-1Ak-1Qk

k-1Ai ikj

Uijk =0Ak-1Qk

k-1Aj-1Qjj-1Ai ijk

0 i < j o i < k

Ecuacin 20.

2.6.2 Energa cintica del manipulador

Sea Ki la energa cintica del elemento i, expresada en el sistema de coordenadas

de la base, y sea dKi la energa cintica de una partcula de masa diferencial dm

en el elemento i entonces

dKi = traza(ViVit)dm = Tr(ViVi

t)dm

Ecuacin 21.

En esta ecuacin se utiliza la operacin traza10 en lugar del producto escalar de

vectores para poder formar el tensor del cual se puede obtener la matriz de inercia

del elemento i (o matriz de pseudo inercia) Ji. Sustituyendo Vi de la Ecuacin 19, la

energa cintica de la masa diferencial es:

10La traza es la suma de los elementos de la diagonal de la matriz.


24/76

24

Ecuacin 22.

La matriz Uij es la velocidad de cambio de los puntos (iri) sobre el elemento i

relativo al sistema de coordenadas de la base cuando qi cambia. Es constante

para todos los puntos en el elemento i e independiente de la distribucin de masa

del elemento. Tambin los vi son independientes de la distribucin de masa, as

que integrando respecto ala masa los trminos iri se obtiene:

Ecuacin 23.

El termino integral en el interior del corchete es la inercia (J i) de todos los puntos

en el elemento i. para obtener la energa cintica total del robot sumamos la de

todos sus elementos.


25/76

25

Ecuacin 24. Energa cintica total del manipulador

K es una cantidad escalar. Ji es dependiente de la distribucin de masa del

elemento i y no de su posicin o velocidad y se expresa en el sistema i-simo. Por

tanto, cada Ji se necesita calcular solo una vez para calcular la energa cintica de

un robot.

2.6.3 Energa potencial de un manipulador

Sea P la energa potencial total del robot y sea Pi la energa potencial del

elemento i:

Ecuacin 25.

Y la energa potencial total del brazo se puede obtener sumando las de cada

elemento:


26/76

26

Ecuacin 26. Energa potencial total del manipulador

Donde g = (gx, gy, gz, 0) es un vector fila de gravedad expresado en el sistema de

coordenadas de la base. Para un sistema a nivel, g (0, 0,-|g|,0) y g es la constante

gravitacional (g = 9.8062 m/s2).

2.6.4 Ecuaciones de movimiento de un manipulador

De las ecuaciones Ecuacin 24y Ecuacin 26, la funcin Lagrangiana L = K P

esta dada por

Ecuacin 27.

Aplicando la formulacin de Lagrange-Euler (Ecuacin 7) a esta ecuacin seobtiene la ecuacin del par generalizado i requerido para que el actuador de la

articulacin i nueva el elemento i-simo del manipulador,


27/76

27

Ecuacin 28.

para i = 1,2,,n. La ecuacin anterior se puede expresar de modo mas simple en

notacin matricial como

Ecuacin 29.

o en forma matricial como

(t) = D(q(t))a(t) + h(q(t),v(t)) + c(q(t))

Ecuacin 30.

donde

(t) = vector n x 1 que representa el par generalizado en las articulaciones.

q (t) = vector n x 1 de las variables de articulacin del robot.

v (t) = vector n x 1 que contiene las velocidades de las articulaciones.

a (t) = vector nx 1 de las aceleraciones de las articulaciones.

D (q(t)) = matriz simtrica inercial n x n relacionada con la aceleracin.


28/76

28

Ecuacin 31.

h(q(t),v(t)) = vector n x 1 que representa la fuerza de coriolis y centrfuga no lineal.

h(q(t),v(t)) = (h1,h2,,hn)T

Ecuacin 32.

donde

Ecuacin 33.

y

Ecuacin 34.

c(q(t)) = vector n x1 que representa la fuerza gravitatoria.

c(q(t)) = (c1,c2,,cn)T Ecuacin 35.


29/76

29

donde

Ecuacin 36.

Los coeficientes ci, Dik, hikm son funciones de las variables de articulacin y los

parmetros inerciales del manipulador y son llamados coeficientes dinmicos. El

significado fsico de estos coeficientes es fcil de observar:

El coeficiente ci representa los trminos de la fuerza gravitatoria en el

elemento i debido a las partes del manipulador superiores en la cadena

cinemtica.

El coeficiente Dik se relaciona con la aceleracin de las variables de

articulacin. En particular, para i = k, D ii esta relacionado con la aceleracin

de la articulacin i donde el par generalizado i acta, para i k, Dik se

relaciona con el par generalizado de reaccin inducido por la aceleracin de

la articulacin k y que acta en la articulacin i o viceversa11.

El coeficiente hikm se relaciona con la velocidad de las variables de

articulacin. Los ltimos ndices, km, estn relacionados con las

velocidades de las articulaciones k y m, de cuya interrelacin dinmica se

induce un par generalizado en la articulacin i. En particular, para k = m, h ikk

est relacionada con la fuerza centrifuga generada por la velocidad angular

11La matriz de inercia es simtrica y Tr(A) = Tr(AT), por eso Dik = Dki.


30/76

30

de la articulacin k12 y que afecta ala articulacin i, mientras que si k m,

hikm esta relacionada con la fuerza de Coriolis generalizada por las

velocidades de las articulaciones k y m que afectan a la articulacin i. por

razonamientos fsicos resulta obvio que hikm = himk.

Al evaluar estos coeficientes, es conveniente observar que algunos de ellos

pueden ser nulos por las siguientes razones:

Un diseo cinemtico particular del manipulador puede eliminar algn

acoplo dinmico

13

entre los movimientos de las articulaciones. Alguno de los coeficientes dinmicos relacionados con la velocidad tienen

solo una existencia formal en las ecuaciones de hi y hikm; esto es, son

fsicamente inexistentes14.

Debido a variaciones particulares en la configuracin del elemento durante

el movimiento, algunos coeficientes dinmicos se pueden anular en algunos

instantes particulares de tiempo.

12Esta fuerza existe solo si la articulacin K es rotacional.

13Coeficientes Dij, hikm.

14Por ejemplo, la fuerza centrifuga no interaccionara con el movimiento de la articulacin que lo

genere, esto es hiii = 0 siempre; sin embargo, puede interactuar con el movimiento de otrasarticulaciones en la cadena cinemtica, por lo que podemos tener hjii 0.


31/76

31

3. CALCULO DE LA CINEMTICA.

3.1 Cinemtica directa

El clculo de la cinemtica directa se hace utilizando la formulacin de Denavit-

Hartenberg, el sistema de marcos de referencia mostrados en laIlustracin 1y las

dimensiones generales dadas en la Ilustracin 2. Informacin con la que se

obtienen los parmetros de Denavit-Hartenberg. Parmetros dados en la Tabla 1

(pagina 31). Luego con estos parmetros se forman la matriz de transformacin

homognea de cada articulacin, multiplicando estas matrices se obtiene la matriz

que representa la posicin-orientacin del efector final15 del manipulador.

Articulacin d (mt) a (mt)

1 0 d1 0 -90

2 2 0.183 0 -90

3 3 0 0.324 0

4 4 0 0.22 0

5 5 0 0 -90

6 6 0.08 0 0

Tabla 1. Parmetros de Denavit-Hartenberg

15 Efector final es la herramienta ubicada al final de la cadena cinemtica del brazo del

manipulador, para nuestro caso la posicin-orientacin del efector final esta en el centro de lamano del robot.


32/76

32

A continuacin se muestran las matrices de transformacin homognea de cada

articulacin y su producto.

0A1= 1A2=

2A3=

3A4=

4A5=5A6=

A =

En la matriz A16 se utilizo para simplificar su escritura la siguiente notacin:

Ci = Cosi

Si = Sin

iLa matriz 5A6 se toma de esta forma y no como debiera ser, porque el movimiento

de la mueca que representa17, no afecta la posicin del centro de la mano del

robot.

16 A = 0A1

1A22A3

i-2Ai-1i-1Ai

n-1An n = 6


33/76

33

3.2 Cinemtica Inversa

Para el calculo de la cinemtica inversa se implementaron una serie de decisionesque debe tomar el manipulador para saber como y cuanto debe mover cada una

de sus articulaciones para alcanzar la posicin deseada. Los criterios de toma de

estas decisiones se explicaran a continuacin.

Una vez conocida la posicin deseada de la mano del manipulador18 (X, Y, Z), se

chequea si sus coordenadas en el plano XY19 estn o no en el interior del cuerpo

principal del robot20 o si esta posicin puede causar interferencia entre los

elementos del brazo, para este fin se defini en el plano XY un rea rectangular de

seguridad, si la posicin deseada resulta estar en el interior se genera el caso1, si

no, se genera el caso 2.

17Giro o roll.

18Representada en la Ilustracin 12 como una pequea esfera.

19Respecto al sistema coordenado de la base (X0, Y0, Z0).

20Partes 1 y 2.


34/76

34

Ilustracin 12. Caso 1 (izquierda) y caso 2 (derecha).

Para explicar los procedimientos, en el caso 1 se usara la posicin (0.2, 0.2, 0.2)

mt, para el caso 2 la posicin (0.2, 0.4, 0.2) mts.

Con estos procedimientos se pretende hallar siempre una solucin nica para la

cinemtica inversa.

3.2.1 Caso 1

3.2.1.1 Parte 1: translacin

En este caso, se debe decidir cuanto y hacia que lado se mover esta parte. La

parte se ubicara a una distancia de 0.312 mt21 de la posicin final deseada, esta

distancia se mide sobre el eje Z0, lo cual nos da dos posibles posiciones sobre

este eje, la posicin seleccionada ser la de menor valor absoluto.

21Esta es la mitad de la longitud til del brazo. Este valor en el software desarrollado es una de las

variables que controlan el comportamiento del manipulador.


35/76

35

Ilustracin 13. Caso 1 articulacin 1.

Para el caso especfico de la posicin (0.2, 0.2, 0.2) mts, aplicando los criterios de

decisin que acaban de definirse, se obtiene un movimiento de 0.112 mts hacia el

sentido negativo del eje Z0 (d1 = -0.112 mts).

3.2.1.2 Parte 2: rotacin

La misin de esta parte es alinear el brazo del manipulador con el objetivo, para lo

cual debe girar un ngulo determinado (2).


36/76

36


3.2.1.3 Movimiento del Brazo

La misin de las dos primeras partes (articulaciones 1 y 2) es ubicar el brazo en

una posicin adecuada para que pueda alcanzar el objetivo. Ahora el manipulador

debe decidir cuantas y cuales articulaciones del brazo debe usar para alcanzar la

posicin del objetivo, adems de cuanto y en que sentido mover cada una de

ellas. Inicialmente el manipulador calcula la posicin del objetivo respecto al

sistema coordenado X2Y2Z2 y su distancia al mismo, para usar esta distancia

como parmetro de decisin, lo cual genera cuatro posibles opciones, la opcin en

la que la distancia al objetivo este entre 0.130 y 0.312 mts no es posible debido a

que las decisiones tomadas para las primeras articulaciones descartan la

presencia de la posicin objetivo en esa zona, a pesar de que fsicamente el

manipulador puede llegar a ella.


37/76

37

Ilustracin 15. Subdivisin del espacio de trabajo para el caso 1

3.2.1.3.1 Distancia al objetivo mayor a 0.624 mts

En este caso el software generara un mensaje de error que indica que el objetivo

esta fuera de alcance, si esto sucede se aborta el calculo y ninguna articulacin se

mueve, porque la aplicacin22 se diseo para enviar los datos nicamente cuando

se haya calculado completamente el vector de articulaciones.

3.2.1.3.2 Distancia al objetivo igual a 0.624 mts

En este caso la tercera articulacin (hombro) se alineara con el objetivo, y las

dems articulaciones tomaran el valor de la posicin inicial, lo cual significa que

4=0, 5=0, 6=0, ver Ilustracin 16.

22Los trminos software, aplicacin, programa se utilizan indistintamente en este trabajo como

sinnimos.


38/76

38

Ilustracin 16. Distancia al objetivo igual a 0.624 mts

3.2.1.3.3 Distancia al objetivo entre 0.544 y 0.624 mts

En este caso el programa calcula la interseccin de dos circunferencias, la primera

con centro en el origen del sistema coordenado X2Y2Z2 sobre el plano X2Y2 y con

un radio igual a 0.324 mts que es la longitud til de la parte 323 (hombro), la

segunda circunferencia tiene centro en la posicin del objetivo y su radio es igual a

0.3 mts que es la suma de las longitudes tiles de la cuarta y quinta articulacin. Al

alinear la tercera articulacin con estas dos intersecciones se obtienen dos valores

para 3 se escoge el de menor valor absoluto. Tomar este valor para 3 garantiza

que al alinear la cuarta articulacin con el objetivo la posicin del efector final

coincidir con el, y las restantes articulaciones tomaran la posicin inicial.

23Igual a parmetro D-H a3.


39/76

39

Ilustracin 17. Distancia al objetivo entre 0.544 y 0.624 mts


Para este caso el manipulador usara de la tercera a la quinta articulacin para

alcanzar la posicin deseada, para ello calculara primero la interseccin entre dos

circunferencias, la primera con centro en el origen del sistema coordenado X2Y2Z2

sobre el plano X2Y2 y con un radio igual a 0.324 mts y la segunda con centro en la

posicin del objetivo y su radio es igual a 0.22 mts que es la longitud til de la

cuarta articulacin (codo), al alinear la tercera articulacin con estas dos

intersecciones se obtienen dos valores para 3 seleccionando el de menor valor

absoluto. En segunda instancia se calculara la interseccin entre dos



posicin del objetivo y su radio es igual a 0.08 mts, al alinear la cuarta articulacin

con estas dos intersecciones se obtienen dos valores para 4 se escoge el de


40/76

40

menor valor absoluto; tomar estos valores para 3 y 4 garantiza que al alinear la

quinta articulacin con el objetivo la posicin del efector final coincidir con el.


Para la posicin (0.2, 0.2, 0.2) mts, aplicando los criterios de decisin que acaban

de definirse, se obtiene 3=32.053957, 4= -75.2323, 5= -100.4756.

3.2.2 Caso 2

3.2.2.1 Parte 1: translacin

En este caso la parte se alineara con la posicin objetivo, esto significa en otras

palabras que la variable de articulacin d1 tomara el valor Z de la coordenada de la


41/76

41

posicin objetivo, colocando al manipulador de frente y alineado con la posicin a

alcanzar.


3.2.2.2 Parte 2: rotacin

Dado que la alineacin con el objetivo ya se completo con la primera articulacin,

no se requiere que la segunda ejecute algn movimiento. Esto significa que 2 = 0

siempre para el caso 2.

3.2.2.3 Movimiento del Brazo


42/76

42

Al igual que el caso 1, lo primero que el manipulador hace es calcular la posicin

del objetivo respecto al sistema coordenado X2Y2Z2 y su distancia al mismo, para

usar esta distancia como parmetro de decisin, lo cual genera cinco posibles

opciones.

Ilustracin 20. Subdivisin del espacio de trabajo para el caso 2

3.2.2.3.1 Distancia al objetivo mayor a 0.624 mts

En este caso el software generara un mensaje de error que indica que el objetivo

esta fuera de alcance, si esto sucede se aborta el calculo y ninguna articulacin se

mueve.

3.2.2.3.2 Distancia al objetivo igual a 0.624 mts


43/76

43

En este caso la tercera articulacin se alineara con el objetivo, y las dems

tomaran el valor de la posicin inicial, lo cual significa que 4=0, 5=0, 6=0.

Ilustracin 21. Distancia al objetivo igual a 0.624 mts


Este caso se trata igual que el tratado en el numeral 3.2.1.3.3. El programa

calcula la interseccin de dos circunferencias, la primera con centro en el origen

del sistema coordenado X2Y2Z2 sobre el plano X2Y2 y con un radio igual a 0.324

mts que es la longitud til de la parte 3 la segunda circunferencia tiene centro en

la posicin del objetivo y su radio es igual a 0.3 mts que es la suma de las

longitudes tiles de la cuarta y quinta articulacin. Al alinear la tercera articulacin

con estas dos intersecciones se obtienen dos valores para 3 se escoge el de

menor valor absoluto, al alinear la cuarta articulacin con el objetivo la posicin del


44/76

44

efector final coincide con el, y las restantes articulaciones tomaran la posicin

inicial.



Para este caso el manipulador utilizara de la tercera a la quinta articulacin para

alcanzar la posicin deseada, para ello calculara primero la interseccin entre dos



posicin del objetivo y su radio es igual a 0.22 mts que es la longitud til de lacuarta articulacin, al alinear la tercera articulacin con estas dos intersecciones

se obtienen dos valores para 3 se escoge el de menor valor absoluto. En segunda

instancia se calculara la interseccin entre dos circunferencias, la primera con


45/76

45

centro en el origen del sistema coordenado X3Y3Z3 sobre el plano X3Y3 y con un

radio igual a 0.22 mts y la segunda con centro en la posicin del objetivo y su radio

es igual a 0.08 mts, al alinear la cuarta articulacin con estas dos intersecciones

se obtienen dos valores para 4 se escoge el de menor valor absoluto; tomar estos

valores para 3 y 4 garantiza que al alinear la quinta articulacin con el objetivo la

posicin del efector final coincidir con el.


Para la posicin (0.2, 0.4, 0.2) mts, aplicando los criterios de decisin que acaban

de definirse, se obtiene 3=-6.03, 4= -96.75, 5= -100.47.

3.2.2.3.5 Distancia al objetivo menor a 0.240 mts

En este caso el software generara un mensaje de advertencia que indica que el

objetivo aunque no esta fuera de alcance, esta en una zona de peligro de

interferencia, si esto sucede se aborta el clculo y ninguna articulacin se mueve.

4. Calculo dinmico


46/76

46

El calculo de las ecuaciones dinmicas del robot se realizo, mediante el uso de la

formulacin de Lagrange-Euler24, los parmetros D-H y la cinemtica directa

calculada anteriormente, debido a la gran extensin del calculo dinmico, este se

encuentra en forma detallada en el anexo 3.

5. Implementacin

5.1 Enfoque y estructuras de programacin

El enfoque de programacin que se esta utilizando es la programacin orientada a

objetos (OOP, por sus siglas en ingles).

La programacin orientada a objetos hace modelos de los objetos del mundo real

mediante sus contrapartes en software. Aprovecha las relaciones de clase, en

donde objetos de cierto tipo, tienes las mismas caractersticas. Aprovecha adems

las relaciones de herencia, en la que ciertos objetos descienden de otros.

La OOP proporciona una forma ms natural e intuitiva de observar el proceso de

programacin, es decir haciendo manejo de objetos del mundo real, de sus

atributos y comportamientos, OOP tambin hace modelos de comunicacin entre

objetos.

24Vista en la seccin 2.6 de este documento.


47/76

47

OOP encapsula datos (atributos) y funciones (comportamientos) en paquetes

llamados objetos; los datos y funciones de un objeto estn muy unidos. Los

objetos tienen la propiedad de ocultar la informacin. Esto significa que aunque los

objetos puedan saber como comunicarse unos con otros mediante interfaces bien

definidas, a los objetos no se les esta permitido saber como funcionan otros

objetos, los detalles de la puesta en practica (implementacin) quedan ocultas

dentro de los objetos mismos.

Para la programacin de la aplicacin se selecciono el lenguaje C++, por ser un

lenguaje de programacin orientado a objetos. En C++, la unidad de programacin

es la clase a partir de la cual eventualmente los objetos son creados.

Como plataforma para la aplicacin se selecciono Windows por ser una

plataforma cuya arquitectura es orientada a objetos y proporciona toda la

infraestructura grfica, lo cual permite concentrar los esfuerzos en la creacin de

las estructuras propias de la aplicacin. Adems la arquitectura Windows es

compatible con tecnologas grficas como OpenGL que permite el manejo y

creacin de grficas en 3D.

Como entorno de desarrollo la opcin obvia fue usar Visual C++ 6.0

, por ser

orientado visualmente, diseado especficamente para la programacin de

aplicaciones Windowsen C++. MFC (Microsoft Fundation class) es la principal

forma de escribir aplicaciones Windows en Visual C++. MFC es un conjunto de


48/76

48

clases en C++ que comprenden una aplicacin genrica del funcionamiento de

Windows lista para ser personalizada.

La tecnologa seleccionada para la visualizacin 3D fue OpenGL, la cual se ha

convertido en un estndar para las aplicaciones 3D, con esta tecnologa los PCs

disponen de las mismas capacidades grficas 3D que una estacin de trabajo.

En OpenGL los objetos 3D son manejados por matrices de transformacin

homogneas 4x4 iguales alas utilizadas en robtica, con las cuales se maneja el

comportamiento de los objetos, OpenGL utiliza un objeto llamado modelview que

consta de una pila de matrices de 32 niveles, lo cual es muy til en el manejo de la

cinemtica.

La pila de matrices modelview tiene una gran utilidad cuando se trabaja con

modelos que guardan jerarquas entre sus diferentes elementos.

En OpenGL las transformaciones que crean una imagen bidimensional de un

objeto tridimensional se manejan mediante transformaciones matriciales estas

transformaciones incluyen transformaciones de modelo, de visualizacin y de

proyeccin.

5.2 Modelado del problema en objetos.


49/76

49

Como se menciono en el apartado anterior OOP es una forma de representar en

software el mundo real, por tanto debemos analizar la estructura del robot para

modelarla en objetos.

Un robot industrial (tambin llamado en la literatura tcnica manipulador mecnico,

serial manipulator, etc.) consiste en una secuencia de cuerpos rgidos, llamados

elementos, conectados mediante articulaciones prismticas o de revolucin. Cada

articulacin elemento constituye un grado de libertad.

De aqu que para un robot con n grados de libertad haya n pares articulacin

elemento con una unin (que no se toma como parte del robot) unido a una base

donde se establece un sistema de coordenadas inercial, y el ultimo elemento esta

unido al efector final. Las articulaciones y elementos se enumeran hacia fuera

desde la base; as cada elemento se conecta, a lo ms, a otros dos, evitando de

esta manera la formacin de lazos cerrados.

Como el robot es un arreglo de articulaciones, la forma natural de crearlo es que

este definido en su estructura principal como arreglos de las diferentes

caractersticas de sus articulaciones en cada posicin de tiempo, el objeto robot

se defini con los siguientes campos:

est estado[600]; //matriz 600x29

vectorTH g; //matriz 4x1

matrizTH A,A1,A2,A3,A4,A5,A6; //matrices de transformacin homognea 4x4

matrizTH Q1,Q2; //matrices auxiliares Qi


50/76

50

matrizTH I; //matriz auxiliar identidad

derivadaU U; // matriz 600x6x6x4x4 de matrices Uij

seudoinercias J; //matriz 6x4x4 de matrices de seudo inercias

matrizD D[600]; //matriz 600x6x6 de matrices dinmicas

derivadasU u; //matriz 600x6x6x6x4x4 de matrices Uijk

matrizH h[600]; //matriz 600x6x6x6 de trminos hijk

vectorD H[600]; //matriz 600x6 con los componentes centrfugos y de coriolis

vectorD G[600]; //matriz 600x6 con los componentes de la fuerza de gravedad

vectorD Fza[600]; // matriz 600x6 con los componentes dinmicos

El campo estado es una matriz de 600x29 donde se guardan posiciones,

velocidades, aceleraciones y momentos generalizados de todas las articulaciones

para 600 posiciones de tiempo, adems de la posicin, velocidad y aceleracin del

efector final para las mismas 600 posiciones de tiempo. Las dems matrices

contienen las matrices de transformacin y todas las otras utilizadas en el clculo

dinmico para cada posicin de tiempo.

Los mtodos diseados para el objeto robot son los siguientes:

double dot(double a[],double b[],int din);

void matxvecTH(double a[4][4], double b[],double c[]);

void matxvecD(double a[6][6], double b[],double c[]);

void mxmTH(double a[4][4],double b[4][4],double c[4][4]);

void matxmatD(double a[6][6],double b[6][6],double c[6][6]);


51/76

51

void mproTH(matrizTH A,matrizTH B,matrizTH C,matrizTH D,matrizTH E,matrizTH F,matrizTH

G,matrizTH H,matrizTH R);

void transpuestaTH(double a[4][4],double b[4][4]);

void transpuestaD(double a[6][6],double b[6][6]);

double trazaTH(double a[4][4]);

double trazaD(double a[6][6]);

void nulaTH(double a[4][4]);

double Dik(int pos,int i,int k);

double hikm(int pos,int i, int k,int m);

void directa(int pos);

void caso2(int pos);

void caso1(int pos);

double angulo(double r1, double h2, double k2, double r2);

bool esta_entre(double h,double k,double inf,double sup,bool z);

bool zona_interna(bool z,int pos);

void dinamica(int pos);

Estos mtodos realizan los clculos matriciales de cinemtica inversa, directa y el

clculo de momentos generalizados.

5.3 Trayectorias.

Ya que obtener todas las soluciones al problema cinemtico inverso para una

posicin orientacin dada del efector final (que pueden ser hasta 16 para un robot

6 DOF), no es de gran ayuda en cuanto al manejo de que de las trayectorias har

el robot. Esta se har de la siguiente manera:


52/76

52

Ilustracin 24. Entrada de datos.

El usuario dar entrada de los puntos clave de la trayectoria (posicin,

orientacin, tiempo y tipo de interpolacin25 del efector final). El programa calcula

la cinemtica inversa de los puntos clave como en 3.2, luego se calculan los

puntos intermedios mediante interpolacin entre los puntos clave y luego se

calcula para cada punto obtenido del efector la cinemtica directa, as obtengo

todos los puntos de la trayectoria del efector y los vectores de articulacin para

todas las posiciones.

25Se implementaron 3 tipos de interpolacin: con velocidad constante, uniformemente acelerado y

uniformemente desacelerado.


53/76

53

Para poder realizar los pasos anteriores se programo toda la matemtica

requerida tanto de geometra vectorial como de lgebra lineal.

Cabe anotar que la aplicacin grfica la trayectoria y el movimiento del robot en

tiempo real y en 3D (hasta donde las capacidades del computador en donde se

instale lo permitan).

Con este procedimiento se asegura la no ocurrencia de singularidades. Una

singularidad ocurre cuando el efector final cae sobre una lnea que intercepta

todos los ejes de articulacin. En este caso el movimiento instantneo a lo largo

de esa lnea es imposible. Esta singularidad genera los limites externos del

espacio de trabajo, a lo largo de una superficie cerrada, es decir que las

singularidades posicinales generan superficies en el espacio tridimensional, que

aparecen en el interior del espacio de trabajo o en sus limites, estas superficies

aparecen en condiciones geomtricas particulares.

Desde el punto de vista de control la ocurrencia de singularidades es un serio

problema, por eso la aplicacin obtiene la cinemtica inversa mediante una

variante26 del mtodo de aproximacin lineal hacia delante (mtodo utilizado por

muchos programas de animacin 3D) ya que las soluciones obtenidas de esta

forma toman en cuenta los limites de articulacin, evitando as las singularidades.

26La variante de este mtodo es la descrita en 3.2.


54/76

54

La aproximacin lineal en cuestin consiste en mover el efector final utilizando la

primera articulacin de la cadena cinemtica hasta que la distancia entre el efector

y la posicin deseada sea mnima, dentro del rango de la articulacin (las dems

articulaciones quedan estticas), luego esta articulacin permanece esttica y se

mueve la articulacin siguiente repitiendo el proceso de aproximacin, hasta

alcanzar la posicin deseada (esta posicin se debe alcanzar con mximo las 6

articulaciones si se encuentra en el espacio de trabajo). Segn la articulacin sea

Rotacional o prismtica, la trayectoria del efector final ser un segmento de lnea

dirigido o un arco y sobre esta trayectoria se busca el punto que este mas cerca

de la posicin deseada.

El programa no realiza un anlisis de las superficies generadas por las

singularidades ya que no es posible estudiar y sistematizar todos los casos

particulares y los mltiples campos de posturas que se generan en cada paso

particular, dado que las singularidades forman hiper-superficies en el espacio de

articulaciones que para esta caso es 6D. Todo lo anterior hara que para efectos

prcticos, aun en forma simplificada el anlisis de las singularidades disminuira el

rendimiento de la aplicacin a niveles inaceptables.


55/76

55

5.4 Grafica computacional e interfaz de usuario.

Como se puede observar fcilmente no se halla en el objeto robot definido

anteriormente ningn vestigio de comportamientos grficos del mismo. Bien, esto

se debe ala arquitectura documento/vista de Windows, las aplicaciones en esta

plataforma son por lo general objetos ventana y en su estructura se halla un objeto

especial el cual es el encargado de la visualizacin de la aplicacin y todo el

despliegue de informacin de la misma. Este objeto es la clase27 vista, y es en ella

donde se crean los comportamientos grficos de la aplicacin, especficamente

estos comportamientos se programan o son invocados desde el mtodo OnDraw

que es el encargado de dibujar y mantener actualizada el rea del cliente28.

Aunque Windows posee su propia Interfaz de Dispositivo de Grficos (GDI), esta

interfaz es inadecuada para el desarrollo de software grafico tridimensional, ya que

es un sistema de dibujo bidimensional. Por esta razn se uso como contexto de

dispositivo a OpenGL para lo cual debi reestructurar la clase vista, tanto para

usar el nuevo contexto de dispositivo como para ser una interfaz adecuada entre

el objeto robot y su visualizacin, ya que OpenGL tiene sus propios tipos de

datos numricos, que aunque son homlogos a los de C++ con frecuencia

requieren adecuacin en los mtodos implementados para la visualizacin de los

diferentes tipos de informacin generados por la aplicacin.

27En programacin los trminos clase y Objeto son sinnimos.

28 rea del cliente es el trmino utilizado para denominar la zona principal de despliegue de

informacin.


56/76

56

La idea principal que hasta aqu debe quedar clara es que Windows es el

encargado de crear y mantener la ventana de la aplicacin, y el programador es el

encargado de crear, modificar y mantener su contenido a travs de los objetos a

visualizar, su modificacin y actualizacin, para lo cual usa la arquitectura

Windows para modificar y actualizar la informacin creando una interfaz de

usuario29,y visualizando esta informacin en el rea de cliente utilizando OpenGL

para la generar lo grficos en 2D y 3D, su animacin30 y modo de visualizacin.

El desarrollo de la interfaz de usuario se realizo usando la arquitectura bsica de

Windows la explicacin de su estructura y uso se encuentra en el anexo 5 de

este documento.

La implementacin de la parte de grafica computacional en OpenGL para el

manejo del rea de cliente se realizo en varias etapas:

Diagramacin del rea de cliente.

Programacin de las sub-divisiones del rea de cliente.

Programacin de las interacciones de los grficos con el objeto Robot y con

el usuario.

29Mens, sub-mens, cajas de dialogo, tool bars, shorcuts y eventos del Mouse entre otros.

30Cambio de estado en el tiempo.


57/76

57

5.4.1 Diagramacin del rea de cliente.

En el diseo de la diagramacin del rea de cliente se tomo como criterio

primordial la capacidad de mostrar toda la informacin susceptible de ser

visualizada grficamente y que es generada por el objeto robot en cada instante

de tiempo. Se decidi que deberan haber grficos 3D para visualizar los cambios

de posturas del robot en el tiempo y la trayectoria del efector final, se realizaron

pruebas que mostraron la necesidad de visualizar en robot tanto en modo de

malla31 como de modo sombreado. El modo malla es necesario para obtener una

adecuada visualizacin de la interaccin entre las articulaciones, el modo

sombreado se requiere para visualizar en forma mas realista el robot y para que

este tenga un mejor contraste con la grafica de la trayectoria del efector final, para

la correcta visualizacin de la trayectoria se determino que se requera la

capacidad de zoom y Orbit32 en el modo sombreado. Tambin se hallo la

necesidad de poder graficar contra el tiempo y ver como es la variacin de

diversas variables33 de forma simultanea al movimiento del robot en los modos de

malla y sombreado.

El anterior anlisis dio como resultado la necesidad de dividir el rea de cliente en

3 zonas: zona de modelo en malla, zona de modelo sombreado y zona de

graficacin de variables vs. Tiempo.

31Tambin conocido como estructura de alambre.

32Zoom es la capacidad de acercarse y alejarse del modelo, esta capacidad es anloga al zoom de

las cmaras. Orbit es la capacidad de orbitar libremente alrededor del modelo.33Las componentes de los vectores posicin, velocidad y aceleracin del efector final. Adems de

las componentes del vector de articulaciones, sus velocidades, aceleraciones y momentosgeneralizados.


58/76

58

5.4.2 Programacin de las sub-divisiones del rea de cliente.

Ilustracin 25. Sub-divisiones del rea de cliente

5.4.2.1 Modelo en malla.

La programacin de las graficas de esta zona, al igual que la de las otras dos se

realizo en OpenGL, utilizando para esta zona los objetos tipo malla: cilindro y

caja, el objeto cilindro se crea siempre con el centro de su tapa inferior en el origen

de coordenadas activo, el objeto caja sita su centro geomtrico tambin en el

origen de coordenadas activo, lo cual hace necesario la reubicacin continua del

sistema coordenado para la correcta ubicacin y orientacin de las articulaciones


59/76

59

del manipulador. Los pasos para la realizacin de esta grafica se especifican en el

siguiente cdigo:

glBegin(GL_LINES);glColor3f(0,0,1);

glVertex3d(-0.08,-0.116,-1.05);glVertex3d(-0.08,-0.116,1.05);glVertex3d(0.08,-0.116,-1.05);glVertex3d(0.08,-0.116,1.05);//rielesglColor3f(1.0f, 0, 0);glVertex3d(0,0,0);glVertex3d(0.2,0,0);glColor3f(1, 0, 0);glVertex3d(0,0,0);glVertex3d(0,0.2,0);glColor3f(1.0f, 0, 0);glVertex3d(0,0,0);glVertex3d(0,0,0.2);//XoYoZo

glEnd();

glTranslated(0,-1,0);glColor3f(0, 0, 0);glTranslated(0,0,ik.estado[pos][3]);auxWireCylinder(0.0725,0.0885);//translacin

glRotated(ik.estado[pos][4]*57.29577951,0,1,0);glTranslated(0,1.116,0);auxWireBox(0.3,0.232,0.174);//Rotacin

glTranslated(0,0.06716,0);

glRotated(ik.estado[pos][5]*-57.29577951,0,0,1);glTranslated(0.162,0,0);auxWireBox(0.324,0.08,0.083);//Codo

glTranslated(0.162,0,0);glRotated(ik.estado[pos][6]*-57.29577951,0,0,1);glTranslated(0.11,0,0);auxWireBox(0.22,0.08,0.08);//Brazo

glTranslated(0.11,0,0);glRotated(ik.estado[pos][7]*-57.29577951,0,0,1);glTranslated(0.04,0,0);auxWireBox(0.08,0.02,0.02);//Mano

se dibujan los ejes coordenados del sistema cero, luego el sistema se traslada a

una posicin adecuada para colocar el objeto cilindro y luego se traslada el

sistema ala posicin del sistema coordenado de la primera articulacin donde, se


60/76

60

lee el valor de desplazamiento de esta articulacin en el objeto robot, luego de lo

cual se realiza el desplazamiento. A continuacin el sistema coordenado se ubica

en la posicin requerida para colocar el siguiente objeto caja, luego el sistema es

trasladado ala posicin del sistema coordenado de la articulacin, se lee el valor

de la variable de esta articulacin y se realiza el desplazamiento correspondiente.

El proceso se repite para las dems articulaciones hasta llegar ala ltima,

obtenindose al final el modelo del robot en la posicin dada por el vector de

articulaciones para un tiempo determinado.

Este procedimiento se repite recorriendo todas las posiciones de tiempo

calculadas, la frecuencia de repeticin de este procedimiento en un computador

con procesador Athlon1800+, 512 MB de memoria RAM y una tarjeta de grficos

NVidia TNT 2 64 de 32 MB de memoria es de aproximadamente 20 veces por

segundo.

5.4.2.2 Modelo sombreado.

Para la programacin grafica de esta zona se utilizo bsicamente el mismo

procedimiento que para la zona de malla, salvo la utilizacin de objetos tipo slido,

la implementacin de iluminacin y de la grafica de la trayectoria en 3D. La

graficacin de la trayectoria del efector final se realizo trazando lneas entre todas

las posiciones consecutivas en el tiempo del efector final tomadas del objeto

robot, accin que se puede interpretar ms fcilmente en el siguiente cdigo

simplificado:


61/76

61

For (i=0;i=599;i++){glVertex3d(posicin(i));glVertex3d(posicin(i+1));}

Aunque este no fue el cdigo usado ya que este tipo de automatizacin no fueposible de implementar en el cdigo OpenGL muestra claramente la forma en

que se programo la grafica de la trayectoria. Para ayudar al mejor

aprovechamiento de la grafica 3D de la trayectoria se implementaron

procedimientos de zoom y orbit, que se explican mas adelante junto a la

explicacin de la comunicacin entre el objeto robot el usuario y los mtodos

grficos del rea de cliente.

5.4.2.3 rea de grficos.

Para la programacin grafica de esta zona se utilizo programacin tipo lnea,

introduciendo los vrtices por parejas y trazando lneas entre ellos. Procedimiento

visualizado en el siguiente cdigo:

For (i=0;i=599;i++){glVertex3d(posicin(var,i));glVertex3d(posicin(var,i+1));}

Como se puede ver el procedimiento traza lneas entre posiciones consecutivas de

una variable en el tiempo. Adems se agrego una lnea mvil cuya posicin estadeterminada por la posicin del tiempo. Los grficos de las tres subdivisiones del

rea de cliente estn sincronizados, ejecutndose ala misma frecuencia de 20Hz.

Se implemento adems control de la escala vertical.


62/76

62

5.4.3 Programacin de interacciones.

La programacin de las interacciones consiste en la definicin de las maneras en

que reaccionan recprocamente los diferentes objetos de la aplicacin entre si y

con el usuario. Para mostrar las interacciones estas se explicaran a medida que

aparezcan en el desarrollo de una ejecucin tpica de la aplicacin.

En esta ejecucin del programa introduciremos una trayectoria cclica con 5

segmentos (6 puntos clave), indicando para cada segmento si las articulaciones se

movern con velocidad constante, aceleracin o desaceleracin constante. La

trayectoria se encuentra tabulada a continuacin.

Punto Clave Posicin del efector Tiempo (Seg.) Tipo de

movimiento/Segmento

1 0.624,0.174,0 0 Desacelerado/1-2

2 0,0,1 15 Acelerado/2-3

3 -0.624,0.174,0 30 Vel. Constante/3-4

4 -0.312,0.3,-0.5 37.5 Desacelerado/4-5

5 0,0.5,-1 45 Acelerado/5-6

6 0.624,0.174,0 60

Para la introduccin de un segmento se debe ir al men Trayectorias IK y

dinmica y seleccionar el tipo de movimiento para el segmento. Cada opcin en

un men posee un identificador el cual genera un mensaje, Windows toma el

mensaje y busca si la aplicacin tiene un manejador para el, este manejador es un

mtodo creado en alguno de los objetos del programa, para este caso los mtodos


63/76

63

pertenecen ala clase vista, si el mtodo debe tomar informacin del usuario utiliza

a un objeto tipo caja de dialogo programado especialmente para ese fin, este

objeto caja de dialogo tiene asignado a cada caja de edicin34 una variable, la cual

sirve de intermediaria entre el usuario y el objeto en el cual debe ser depositada la

informacin. Una vez seleccionado el tipo de movimiento debe aparecer la

siguiente caja de dialogo:

Ilustracin 26. Ingreso de segmento de trayectoria.

En esta caja de dialogo se ingresa el primer segmento de la trayectoria, al hacer

click en Aceptar el mtodo en la clase vista guarda los datos ingresados en el

objeto robot y llama a los mtodos de este encargados de calcular la cinemtica

inversa35 para que la calcule para las posiciones extremas del segmento, luego de

lo cual el mtodo en la clase vista interpola y guarda en el objeto robot todas las

posiciones intermedias del vector de articulaciones

36

, una vez hecho esto se llamade nuevo a un mtodo de la clase robot, el mtodo encargado de calcular la

cinemtica directa para calcular y guardar todas las posiciones intermedias del

34Sitio en el que el usuario ingresa informacin.

35Se describen en el anexo 1.

36Se describe en el anexo 2.


64/76

64

efector en el intervalo. Despus de hacer esto finalmente se llama para cada

posicin del intervalo el mtodo de la clase robot que calcula y guarda la

dinmica.

Despus de ingresar todos los segmentos de la trayectoria el rea de cliente debe

verse as:

Ilustracin 27. rea de cliente.

Donde se pueden apreciar las tres zonas del rea de cliente, en el modelo

sombreado de ve la trayectoria completa del efector final, en la zona de graficas

por defecto aparece la grafica de posicin de la primera articulacin vs. Tiempo.


65/76

65

Si se desea ver alguna otra grafica se debe ir al men Graficas e ir ala opcin

Graficas vs. Tiempo esto har aparecer la siguiente caja de dialogo:

Ilustracin 28. Seleccin de tipo de grafica.

Aqu se introduce el cdigo de la grafica que se desea visualizar, si se elige lagrafica 24 Momento generalizado 2 veremos lo siguiente.

Ilustracin 29. Grafica de momento generalizado 2.


66/76

66

Esta grafica no permite visualizar correctamente el comportamiento del momento,

as que se requiere aumentar la escala vertical, esto se hace desde el men

Graficas con la opcin Escala esto llama una caja de dialogo que permite

modificar la escala vertical, si damos a esta escala el valor 10 obtendremos el

siguiente cambio:

Ilustracin 30. Grafico con factor de escala 10.

Si al analizar las graficas deseamos chequear para un tiempo determinado la

posicin o la dinmica debe ir al men Trayectorias IK y dinmica y seleccionar

bien sea Chequear Posicin o Chequear Dinmica y luego de indicar la posicin

de tiempo a revisar aparecern respectivamente las siguientes cajas de dialogo:

Ilustracin 31. Chequeos.


67/76

67

Todas las anteriores cajas de dialogo son invocadas desde el objeto vista y solo

tienen acceso de lectura a la clase robot, al igual que las cajas de dialogo que son

llamadas desde el men Matrices y Coeficientes. Estas cajas de dialogo son en

su mayora de la siguiente forma:

Ilustracin 32. Visualizacin de la matriz D.

Finalmente puede que la perspectiva que por defecto de coloca en el modo

sombreado no sea muy clara por eso se implementaron las opciones zoom y orbiten el objeto vista como mtodos manejadores del Mouse, el zoom se activa

haciendo click sostenido en el botn derecho mientras se mueve, la opcin orbit se

activa haciendo click sostenido en el botn izquierdo mientras este se mueve, por

defecto el orbit se ejecuta como una rotacin al rededor del eje Y0 ,haciendo un

orbit horizontal de debe ir al men Graficas y en la opcin Orbit activar Vertical.

La combinacin del zoom y el orbit permiten la completa visualizacin en 3D de la

trayectoria y el robot en cualquier instante de tiempo.


68/76

68

Ilustracin 33. Visualizacin con zoom y orbit.


69/76

69

6. Validacin de resultados.

7. Conclusiones.

8. Bibliografa.

INSTITUTO COLOMBIANO DE NORMAS TCNICAS Y CERTIFICACIN.

Normas Colombianas para la presentacin de trabajos escritos y tesis de grado.

Santa Fe de Bogot: ICONTEC, 1999.

D. MANOCHA, J.F. CANNY. Real time inverse Kinematics of general 6R

manipulator. In Proceedings of IEEE conference on robotics and automation, pg

383-389 1992.

B. PADEN, S. SASTRY. Optimal kinematic design of 6R manipulator. International

Journal of Robotic Research, 7(2): 43-61, 1988.

W.K. VOISCHEGGER, C. WU. A method for calibrating and compensating robot

kinematic errors. In IEEE conference on robotics and automation, pg 39-43, 1987.

D. MANOCHA, Y. ZHU. A fast algorithm and system for inverse kinematics of

general serial manipulators. In proceedings of IEEE conference on robotics and

automation, 1994.


70/76

70

K.S. FU, R.C. GONZLEZ, C.S. LEE. Robtica: Control, deteccin, visin e

inteligencia. Mc Graw Hill, 1990.

M. RAGHAVAN, B. ROTH. Kinematics analysis of the 6R manipulator of general

geometry. In international symposium on robotics research, pg 314-320, Tokio,

Japan. 1989.

BUITRAGO J.T. Modelo dinmico del robot CERMA UNIVALLE, proyecto de

investigacin. Maestra en automtica, programa de postgrado en ingeniera

elctrica y electrnica y de la computacin, Santiago de Cali 1998

ELIOT MOTATO ESCOBAR, Planeacin Dinmica de procesos de manufactura

en celdas flexibles, proyecto de investigacin. Maestra en automtica, programa

de postgrado en ingeniera elctrica y electrnica y de la computacin, Santiago

de Cali 1999

PINEDO J, C. R. Metodologa Para el seguimiento de trayectorias predefinidas enel desplazamiento de robots, proyecto de investigacin. Maestra en automtica,

programa de postgrado en ingeniera elctrica y electrnica y de la computacin,

Santiago de Cali 1998.

RON FOSNER. Creating a reusable 3D view class for MFC. Microsoft Press, 1998.

DALE ROGERSON. OpenGl I, OpenGl II, OpenGl III, OpenGl IV, OpenGl V,

OpenGl VI, OpenGl VII. Microsoft Developer Network Technology Group, 1995.


71/76

71

M. RAGHAVAN, B. ROTH. A general solution for the inverse kinematics of all

serial chains. Proc. Of the 8th CISM-IFTOMM symposium on robots and

manipulators, 1990.

ROBERTO LPEZ. El manejo de la pila de matrices en OpenGL. 3D World, 2(14),

1998.

CHUCK SPHAR. Learn Microsoft Visual C++ 6.0 Now. Mc Graw Hill, 1999.

H.M. DEITEL, P.J. Deitel. C/C++ How to Program. Mc Graw Hill, 1995.

ANDEEN GERRY. Robot Design Handbook. Mc Graw Hill, 1988.


72/76

72

9. Anexos.

9.1 Implementacin del clculo de la cinemtica inversa.

void caso1(){ //calculo de d1

d1=Z-0.312;temp1=-0.312;if (fabs(Z-0.312)1.05){

d1= Z-0.312;temp1=-0.312;}if (fabs(d1)>1.05){

if ((Z-0.312)1.05){d1= Z-0.312;temp1=-0.312;}

}//calculo de t2t2=atan2(temp1,X);

//calculo de t3h1=X*cos(t2)+((d1-Z)*sin(t2));k1=0.174-Y;

//if (esta_entre(h1,k1,0.2,0.6245,z)==false) printf("Punto fuera de alcance");

if (esta_entre(h1,k1,0.544,0.624,z)){

t3=angulo(0.324,h1,k1,0.3);//t4h=X*cos(t2)*cos(t3)+pow(cos(t2),2)*(-0.324*pow(cos(t3),2)+(0.174-Y- 0.324*sin(t3))*sin(t3))+sin(t2)*((d1-Z)*cos(t3)-0.324*pow(cos(t3),2)*sin(t2)+sin(t2)*(0.174-Y- 0.324*sin(t3))*sin(t3));

k=cos(t3)*(0.174-Y)-(X*cos(t2)+(d1-Z)*sin(t2))*sin(t3);

t4=atan2(k,h);//t5t5=0;

if (sqrt(pow(h1,2)+pow(k1,2))==0.624){

t3=atan2(k1,h1);//t4t4=0;

//t5t5=0;

}

}if (esta_entre(h1,k1,0.285,0.544,z)){t3=angulo(0.324,h1,k1,0.22);

//calculo de t4h=X*cos(t2)*cos(t3)+pow(cos(t2),2)*(-0.324*pow(cos(t3),2)+(0.174-Y-0.324*sin(t3))*sin(t3))

+sin(t2)*((d1-Z)*cos(t3)-0.324*pow(cos(t3),2)*sin(t2)+sin(t2)*(0.174-Y-0.324*sin(t3))*sin(t3));k=cos(t3)*(0.174-Y)-(X*cos(t2)+(d1-Z)*sin(t2))*sin(t3);

//if (sqrt(pow(h,2)+pow(k,2))!=0.22) k=correccion(h,k,0.22);t4=angulo(0.22,h,k,0.08);

//calculo de t5h=sqrt(pow(-d1+Z+cos(t3)*(0.324+0.22*cos(t4))*sin(t2)-0.22*sin(t2)*sin(t3)*sin(t4),2)


73/76

73

+pow(X+cos(t2)*(cos(t3)*(-0.324-0.22*cos(t4))+0.22*sin(t3)*sin(t4)),2));k=Y-(0.174-0.324*sin(t3)-0.22*cos(t4)*sin(t3)-0.22*cos(t3)*sin(t4));t5=atan2(-k,-h)-t4-t3;}

}

void caso2()

{ d1=Z;t2=0;

//calculo de t3h1=X;k1=0.174-Y;if (esta_entre(h1,k1,0.13,0.624,z)==false) printf("Punto fuera de alcance");

if (esta_entre(h1,k1,0.544,0.624,z)){

t3=angulo(0.324,h1,k1,0.3);//t4

/* h=X*cos(t3)-Y*sin(t3)-2*0.324*pow(cos(t3),2)+0.174*sin(t3);k=-Y*cos(t3)-X*sin(t3)+0.185*cos(t3);*/h=sqrt(pow(X-0.324*cos(t2)*cos(t3),2)+pow(d1-Z-0.324*cos(t3)*sin(t2),2));k=Y-(0.174-0.324*sin(t3));

//if (sqrt(pow(h,2)+pow(k,2))!=0.3) k=correccion(h,k,0.3);

t4=atan2(-k,h)-t3;//t5t5=0;

if (sqrt(pow(h1,2)+pow(k1,2))==0.624){

t3=atan2(k1,h1);//t4t4=0;

//t5t5=0;

}}if (esta_entre(h1,k1,0.285,0.544,z)){t3=angulo(0.324,h1,k1,0.22);

//t4h=X*cos(t3)-0.324*pow(cos(t3),2)+(0.174-Y-0.324*sin(t3))*sin(t3);k=-Y*cos(t3)-X*sin(t3)+0.174*cos(t3);//precalculo//if (sqrt(pow(h,2)+pow(k,2))!=0.22) k=correccion(h,k,0.22);t4=angulo(0.22,h,k,0.08);

//calculo de t5k=-Y*cos(t3+t4)+pow(cos(t3),2)*0.324*sin(t4)-0.174*sin(t3)*sin(t4)+0.324*pow(sin(t3),2)*sin(t4)

+cos(t3)*0.174*cos(t4)-X*sin(t3+t4);h= X*cos(t3+t4)+pow(cos(t3),2)*(-0.22-0.324*cos(t4))+cos(t3)*0.174*sin(t4)+sin(t3)*(cos(t4)*(0.174

-0.324*sin(t3))-0.22*pow(cos(t4),2)*sin(t3)-0.22*sin(t3)*pow(sin(t4),2))-Y*sin(t3+t4);t5=atan2(k,h);}

}


74/76

74

9.2 Implementacin de los mtodos de interpolacin.

Esta es la implementacin para velocidad constante.

if (subint>=599) subint=0;else subint=int(dlg.m_pos2);

i= int(dlg.m_pos);ik.estado[i][0]=dlg.m_X;ik.estado[i][1]=dlg.m_Y;ik.estado[i][2]=dlg.m_Z;

if (ik.zona_interna(z,i)) ik.caso1(i);else ik.caso2(i);

i= int(dlg.m_pos2);ik.estado[i][0]=dlg.m_X2;ik.estado[i][1]=dlg.m_Y2;ik.estado[i][2]=dlg.m_Z2;


i=int(dlg.m_pos+1);t=1;

while(i=599) subint=0;else subint=int(dlg.m_pos2);



i= int(dlg.m_pos2);ik.estado[i][0]=dlg.m_X2;ik.estado[i][1]=dlg.m_Y2;ik.estado[i][2]=dlg.m_Z2;


acceld1= 2*((ik.estado[int(dlg.m_pos2)][3]-ik.estado[int(dlg.m_pos)][3]))/(pow((dlg.m_pos2-dlg.m_pos),2));


75/76

75

accelt2= 2*((ik.estado[int(dlg.m_pos2)][4]-ik.estado[int(dlg.m_pos)][4]))/(pow((dlg.m_pos2-dlg.m_pos),2));




i=int(dlg.m_pos+1);t=1;

while(i=599) subint=0;else subint=int(dlg.m_pos2);



i= int(dlg.m_pos2);ik.estado[i][0]=dlg.m_X2;

ik.estado[i][1]=dlg.m_Y2;ik.estado[i][2]=dlg.m_Z2;


acceld1= 2*((ik.estado[int(dlg.m_pos2)][3]-ik.estado[int(dlg.m_pos)][3]))/(pow((dlg.m_pos2-dlg.m_pos),2));





i=int(dlg.m_pos+1);

t=1;

while(i


76/76

ik.estado[i][7]=ik.estado[int(dlg.m_pos)][7]+((ik.estado[int(dlg.m_pos2)][7]-ik.estado[int(dlg.m_pos)][7])-(0.5*accelt5*pow((dlg.m_pos2-dlg.m_pos)-t,2)));

ik.directa(i);

i=i+1;t=t+1;}

Desarrollo de Un Entorno de Simulacion Para Un Robot Indust

Documents

Transcript of Desarrollo de Un Entorno de Simulacion Para Un Robot Indust