robot de tipo articulado de 3 grados de libertad

INSTITUTO POLITÉCNICO NACIONAL

Escuela Superior de Ingeniería Mecánica y Eléctrica Unidad Azcapotzalco

ROBOT ARTICULADO DE 3 GRADOS DE LIBERTAD

Alumnos: Vargas Cortes Giovanni Yarid

Lara del Prado Aarón

Arreguin Jasso Eduardo

Profesor: Javier Ramírez Gordillo

Grupo: 8RV2



ÍNDICE GENERAL

CAPITULO I GENERALIDADES..................................................................................................3

1.1 OBJETIVO.............................................................................................................................3

1.1.1 OBJETIVOS ESPECÍFICOS..................................................................................3

1.2 JUSTIFICACIÓN.............................................................................................................3

1.3 INTRODUCCIÓN..................................................................................................................4

1.4 MARCO TEÓRICO...............................................................................................................5

1.4.1 ALGORITMO.................................................................................................................5

1.4.2 PLANEACIÓN DE MOVIMIENTO...............................................................................7

1.4.3 ESPACIO DE LAS CONFIGURACIONES................................................................8

1.4.4 CAMPO DE POTENCIAL ARTIFICIAL......................................................................8

CAPITULO II FUNDAMENTOS TEÓRICOS.............................................................................10

2.1 CONFIGURACIÓN DE TIPO ARTICULADO.................................................................10

2.2 MOVIMIENTO DEL ROBOT.............................................................................................10

2.3 CINEMÁTICA DEL ROBOT..............................................................................................11

2.3.1 CINEMÁTICA DIRECTA............................................................................................11

2.3.1.1 CINEMÁTICA DIRECTA CON TRANSFORMACIONES HOMOGÉNEAS.....12

2.3.2 CINEMÁTICA INVERSA............................................................................................12

2.4 DINÁMICA DEL ROBOT...................................................................................................13

CAPITULO III DESARROLLO DE INGENIERÍA......................................................................14

3.1 PIEZAS, MODELADO Y ENSAMBLADO DEL BRAZO ROBÓTICO........................14

3.2 SERVOMOTORES.............................................................................................................16

3.3 COMUNICACIÓN SOFTWARE-HARDWARE...............................................................17

3.4 DIAGRAMA ELECTRÓNICO...........................................................................................18

3.5 PROGRAMACION DEL ARDUINO.................................................................................19

3.6 PROGRAMACIÓN EN MATLAB.....................................................................................20

3.6.1 PROGRAMA PARA LA CINEMÁTICA DIRECTA.................................................20

3.6.2 PROGRAMA PARA LA CINEMÁTICA INVERSA.................................................22

3.6.3 PROGRAMA PARA LA DINÁMICA.........................................................................28

3.7 MODELO DINÁMICO........................................................................................................32

1


Escuela Superior de Ingeniería Mecánica y Eléctrica Unidad AzcapotzalcoCAPITULO IV CONCLUSIONES, REFERENCIAS Y ANEXOS............................................33

4.1 CONCLUSIONES GENERALES......................................................................................33

4.2 ANEXOS..............................................................................................................................33

4.2.1 ANEXO (A1).................................................................................................................33

4.2.2 ANEXO (A2).................................................................................................................33

4.2.3 ANEXO (A3).................................................................................................................33

4.2.4 ANEXO (A4).................................................................................................................33

4.2.5 ANEXO (A5).................................................................................................................33

4.2.6 ANEXO (A6).................................................................................................................33

4.2.7 ANEXO (A7).................................................................................................................34

2



CAPITULO I GENERALIDADES

1.1 OBJETIVO

El objetivo principal del proyecto, es diseñar una estrategia a través de un algoritmo adecuado para la generación de trayectorias con un brazo robótico con una configuración de tipo articulado de 3 grados de libertad.

1.1.1 OBJETIVOS ESPECÍFICOS

Para cumplir con el objetivo principal de este proyecto, se necesitan satisfacer los siguientes objetivos específicos:

Diseñar y construir la configuración mecánica del robot articulado

Diseñar y construir la configuración eléctrica-electrónica

Elaborar una interface de comunicación lógica-física

Integrar cada una de las configuraciones diseñadas

Desarrollar un algoritmo para generar trayectorias

Diseñar el control ON/OFF y PI para la generación de las trayectorias

1.2 JUSTIFICACIÓN

Ya que los robots operan en entornos reales y son capaces de interactuar en un entorno dinámico donde también se encuentran otros objetos, personas y en general con otros agentes, se presentan impresionantes resultados en su mecánica, dinámica y en su control de movimientos.

Los robots son capaces de desarrollar diferentes tipos de tareas, así mismo, con respecto al mundo dinámico que lo rodea, la planificación de futuras acciones y las tareas resultantes a evaluar son características importantes para una planeación.

3



Por lo tanto el diseño de un robot conlleva a desarrollar estrategias para una planeación de un buen control y así pueda interactuar con su entorno sin que se vea interrumpido con algún tipo de obstáculo que se le llegase a presentar, por ello se debe diseñar el algoritmo adecuado para la generación de sus trayectorias y así poder aplicar un control sobre él.

1.3 INTRODUCCIÓN

La planeación de trayectorias desempeña un papel muy importante en varios campos de la investigación. En su forma general, el objetivo es planear trayectorias para un sistema robótico entre una configuración de inicio y una posición final sobre un entorno incierto.

Hasta el día de hoy la planeación de movimientos en manipuladores robóticos para la realización de tareas específicas ha dado lugar a líneas de investigación que buscan explotar y maximizar las capacidades de un sistema robotizado. La planeación de movimientos consiste en guiar automáticamente a un robot alrededor de un entorno con obstáculos, esta define trayectorias libres de colisiones.

Dado un robot con (n) grados de libertad en un ambiente con un número de (k) obstáculos, busca una ruta libre de colisión que conecte la configuración actual (de inicio) del robot con una configuración final deseada (la meta), como es señalado en Canny (1987); Hwang et al. (1992).

Planear trayectorias seguras implica en identificar rutas y jerarquizarlas en un espacio abstracto parta obtener modelos simples, caracterizar el espacio vacío, realizar esquemas sobre la planeación de movimientos con obstáculos y reducir la complejidad del problema.

Cualquiera que sea el panorama, la planeación considera el hecho de que el robot interactúa con sus componentes mecánicos y con elementos que obstaculizan la realización de tareas, ya sea que su desempeño este en entornos estáticos, dinámicos y/o geométricamente restringidos por maquinas u otros robots.

4



1.4 MARCO TEÓRICO

1.4.1 ALGORITMO

En matemáticas, ciencias de la computación y disciplinas relacionadas, la palabra algoritmo proviene del latín, dixit algorithmus y éste a su vez del matemático persa Muhammad ibn Musa al-Jwarizmi. Un algoritmo es una lista definida, ordenada y finita de operaciones que permite hallar la solución a un problema. Dado un estado inicial y una entrada, con pasos sucesivos y definidos, se llega a un estado final obteniendo una solución. Los algoritmos son objetos de estudio de la algoritmia y su definición queda formalizada por el modelo computacional de la máquina de Turing.

Su importancia radica en mostrar la manera de llevar a cabo procesos y resolver sistemáticamente problemas matemáticos o de otro aspecto. Sin embargo, para que un algoritmo pueda ser considerado como tal, éste requiere ser determinista, eficiente, tener un número finito de instrucciones y un criterio de término. Por determinista se entiende que al seguir el mismo proceso más de una vez, se llega siempre al mismo resultado; por eficiente se entiende que el consumo de tiempo debe estar cercano a cero y ocupar en lo menos posible los recursos de memoria. El concepto de algoritmo, no debe confundirse con el concepto de programa, mientras el primero es la especificación de un conjunto de pasos en operaciones, instrucciones, órdenes, etc., orientados a la resolución de un problema; el segundo es un conjunto de operaciones específicas de un determinado lenguaje de programación, susceptible de ser ejecutado. Un algoritmo, estrictamente hablando, no puede ejecutarse hasta que no se implementa y se decide un procedimiento de finalización.

En términos de la teoría de la complejidad computacional que estudia los recursos requeridos durante el cálculo para resolver un problema. Los recursos comúnmente estudiados son el tiempo (número de pasos de ejecución de un algoritmo) y el espacio (cantidad de memoria utilizada), esto genera problemas que tienen una solución con orden de complejidad lineal y que se resuelven en un tiempo que se relaciona linealmente con su tamaño.

5



Hoy en día las máquinas resuelven problemas mediante algoritmos que tienen un máximo de complejidad o costo computacional relacionado entre el tamaño del problema y su tiempo de ejecución. Estos problemas como es analizado en Cortéz (2004), se agrupan en dos conjunto: P y NP; Los problemas con un costo factorial o combinatorio están agrupados en NP y de los cuales no tienen una solución práctica en un tiempo razonable. La complejidad P es el conjunto de los problemas de decisión que pueden ser resueltos en una máquina determinista en tiempo polinómico, lo que corresponde intuitivamente a problemas que pueden ser resueltos aún en el peor de sus casos. La complejidad NP es el conjunto de los problemas de decisión que pueden ser resueltos por una máquina no determinista en tiempo polinómico, todos los problemas de esta clase tienen la propiedad de que su solución puede ser verificada efectivamente.

En Baeza (2002), se mencionan algunos tipos de algoritmos que pueden ser divididos según su función como algoritmos de ordenamiento ó algoritmos de búsqueda, que a su vez pueden implementarse en técnicas de diseño como: Algoritmos voraces.- Seleccionan los elementos más prometedores de un conjunto de candidatos hasta encontrar una solución. En la mayoría de los casos la solución no es óptima.

Algoritmos paralelos.- Permiten la división de un problema en sub-problemas de forma que se puedan ejecutar de manera simultánea en varios procesadores.

Algoritmos probabilísticos.- Algunos de los pasos de este tipo de algoritmos están en función de valores pseudo-aleatorios.

Algoritmos determinísticos.- Sus pasos están definidos y aportan una solución exacta.

Algoritmos de programación dinámica.- Intenta resolver problemas disminuyendo su costo computacional pero aumentando el costo espacial.

Algoritmos Metaheurísticos.- Encuentran soluciones aproximadas a problemas basándose en un conocimiento anterior de los mismos.

6



1.4.2 PLANEACIÓN DE MOVIMIENTO

Los investigadores trabajan en mejorar a los robots para la realización de tareas sumamente complejas con la mínima intervención del hombre, dando lugar a la necesidad de resolver el problema que implica la planeación de movimientos, que durante las tres últimas décadas sigue considerándose como un área muy importante y productiva de la investigación en la robótica.

Consecuentemente, la planeación de movimiento es un problema multimodal no lineal por naturaleza, debido a la relación cinemática que describe las capacidades de movimiento y restricciones de los elementos que componen a un manipulador robótico. Esto implica analizar el tema desde una forma general hasta un aspecto particular.

Además se consideran dos aspectos que pueden definir el problema a estudiar, el primero toma como base la construcción física del entorno y el tipo del robot; el segundo está relacionado con los algoritmos que se emplearán para solucionar el problema de cuerdo al anterior. Dentro del primero los diferentes tipos de ambientes en los que cualquier tipo de robot puede operar (trátese de robot manipulador, robot con restricciones, o de un ambiente con varios tipos de robots), pueden ser: estacionario, variante o invariante en tiempo, adaptable o no adaptable y de objetos móviles. El segundo aspecto que define al problema se relaciona con los algoritmos a emplear y que puede derivarse en situaciones como: la complejidad computacional y el alcance que tienen con métodos de planeación de movimiento global o local, su implementación se hace para obtener resultados de forma heurística, exacta, completa, estocástica o que el algoritmo opere en el espacio cartesiano, el espacio de las configuraciones.

Otra de las características importantes sobre el tipo de espacio a operar, están relacionadas directamente con la caracterización de obstáculos y representación de objetos, que son fundamentales en la planeación de movimiento.

7



1.4.3 ESPACIO DE LAS CONFIGURACIONES

El espacio de las configuraciones en un sistema mecánico, es un espacio puntual de dimensiones igual al número de grados de libertad, donde los puntos tienen como coordenadas generalizadas, aquellas que se han considerado en la descripción de la configuración del sistema; un sub-espacio de las configuraciones, son aquellos puntos que cumplen las ecuaciones de enlace geométrico y que forman el sub-espacio de configuraciones accesibles de acuerdo con Cardona et al. (2001a).

La planeación de trayectorias de manipuladores robóticos en un espacio de trabajo no homogéneo donde los obstáculos existen y con los cuales éste pueda colisionar es descrito en Udupa (1977), donde se analiza el problema de planificar trayectorias seguras para un manipulador mediante la caracterización de obstáculos asociados a una trayectoria libre de colisiones. Sin embargo en Berthold et al. (1977b), propone un método espectral que calcula los torques requeridos de las articulaciones, señalando que el método del espacio de las configuraciones proporciona un equilibrio óptimo entre el almacenaje y los costos de cómputo que son altos cuando se aplica al control de espacio de estados como un sistema relativamente sencillo.

Por otro lado, en Raibert (1978) señala una de tabulación de las ecuaciones del movimiento en tiempo real (conjuntamente con una memoria del espacio de las configuraciones organizada, por variables posiciónales y contenidas en la memoria pre-procesada solamente una vez para cada manipulador) para usarse en todos los movimientos posibles. Así, cada punto en el espacio que corresponden a una configuración del sistema son valores para los elementos del vector de la posición del brazo y que están relacionados con los coeficientes de la ecuación dinámica.

1.4.4 CAMPO DE POTENCIAL ARTIFICIAL

El método del campo de potencial artificial se asocia a manipuladores robóticos articulados, su aplicación pretende que el manipulador alcance desde una posición inicial un punto final sin colisionar con los obstáculos que se encuentran en el espacio de trabajo. Este método se basa en implementar un planeador, el cual toma como referencia el punto inicial dada una configuración particular del manipulador y que busca el punto final generado por una trayectoria libre de obstáculos, evitando así la colisión del mecanismo articulado con sus elementos en una serie de configuraciones coordinadas.

8



El método de campos de potencial artificial desarrollado por Khatib (1986), es empleado desde entonces y hasta la actualidad como base para el desarrollo de diversos algoritmos aplicados en Khosla et al. (1988b); Borenstein et al. (1989); Hussien (1989); Warren (1989b); Warren (1990b); Canny et al. (1990); Rimon et al. (1990a); Borenstein et al. (1991); Latombe (1991); Rimon et al. (1992a); Kim et al. (1992b); Ko et al. (1996); Chuang et al. (1998b); Merchán-Cruz (2005b). Es importante resaltar que este método ha sido modificado durante la solución de algunos problemas, debido a que su mayor desventaja es la existencia de mínimos locales entre ciertas configuraciones del objetivo y los obstáculos.

El análisis matemático es simple cuando se trata con obstáculos en estado estacionario en torno a una solución dinámica, como se establece en Ge et al. (2004) y son usados generalmente para la planeación de trayectorias de robots móviles en Khosla et al. (1988a); Rimon et al. (1992b); Barisic et al. (2006). El concepto básico del método de campo del potencial es colocar sobre el espacio de trabajo del robot, un potencial de atracción sobre la meta y un campo de repulsión sobre los obstáculos como puntos restringidos, de tal manera, que el planeador del robot sigue la fuerza de atracción hacia la meta, como se detalla en Latombe (1991).

Por otro lado, el método aplicado a robots móviles en ambientes dinámicos donde la meta y los obstáculos se mueven, tiene un trato más complejo como se observa en Ge et al. (2002); Conner et al. (2003). No obstante, el problema se reduce en Savage et al. (2000) cuando se toman imágenes y son analizadas automáticamente e incluso, la aplicación de acciones animadas y particularmente en los juegos dinámicos, se hacen más sencillas a través de la computación evolutiva tal como es desarrollado en Abbott et al. (2004).

9



CAPITULO II FUNDAMENTOS TEÓRICOS

2.1 CONFIGURACIÓN DE TIPO ARTICULADO

Volumen de trabajo es esférico. Estos tipos de robot se parecen al brazo humano, con una cintura, el hombro, el codo, la muñeca. Presenta una articulación con movimiento rotacional y dos angulares. Aunque el brazo articulado puede realizar el movimiento llamado interpolación lineal (para lo cual requiere mover simultáneamente dos o tres de sus articulaciones), el movimiento natural es el de interpolación por articulación, tanto rotacional como angular.

En el caso de nuestro proyecto elegimos la estructura de brazo articulado debido a su accesibilidad en diseño y armado, además de que presenta una configuración casi idéntica a los brazos robóticos convencionales usados en la industria por lo que puede ayudar en la mejor compresión de los sistemas que los componen y de su comportamiento mecánico.

2.2 MOVIMIENTO DEL ROBOT

La configuración de un sólido es la descripción estática del objeto en el espacio, relativo a un sistema de coordenadas de referencia. En el caso de un cuerpo rígido sin restricciones, es posible especificar su posición y orientación al menos por seis coordenadas generalizadas. Para poder describir completamente la relación espacial entre los sistemas, es necesario incluir su representación como una componente que relaciona el vector de posición y orientación entre los orígenes del sistema de coordenadas con movimientos relativos, este análisis de movimiento se describe como cinemática y dinámica. La configuración básica de un manipulador robótico está constituida por una cadena cinemática articulada con movimientos relativos y su estudio analítico del movimiento formula modelos factibles de su cinemática y dinámica, principalmente cuando se analiza el comportamiento como resultado de las fuerzas aplicadas por los sistemas de control.

10



2.3 CINEMÁTICA DEL ROBOT

El análisis cinemático, estudia el movimiento de un cuerpo sin tomar en cuenta las fuerzas que lo producen, consecuentemente la cinemática de un manipulador robótico estudia la posición, velocidad y aceleración angular enfocadas a las propiedades geométricas, concretamente a los el movimiento resultante de los eslabones que conforman al robot, como se observa en Bartlett (2001). Sin embargo, éste análisis es inherente a los dos problemas que enfrenta, la cinemática directa e inversa. El primero determinar la posición y orientación de la herramienta del manipulador, debido al cambio en la configuración de los eslabones que conforman la cadena cinemática, el segundo define los valores angulares de las articulaciones, configuración resultante que satisface la orientación/posición deseada de la herramienta en su espacio de trabajo.

2.3.1 CINEMÁTICA DIRECTA

La cinemática directa es sencilla y no hay complejidad al obtener las ecuaciones, por lo tanto, hay siempre una solución directa para determina la posición y orientación de la herramienta o efector final. En general, la representación matemática del sistema se realiza mediante ecuaciones diferenciales ordinarias y el modelo resultante se obtiene a través de dos técnicas como la analítica y la experimental señalada en Kelly et al. (2005), las cuales se derivan sobre el cambio de configuración en el sistema de eslabones que conforman la cadena cinemática y que describen lo siguiente:

Analítico: Este procedimiento se basa en las leyes de la física del movimiento del sistema. Esta metodología tiene la ventaja de producir un modelo matemático tan preciso como se requiera.

Experimental: Este requiere un cierto grado de experimentación para obtener los datos propios del sistema. Normalmente se examina el comportamiento mediante determinadas señales de entrada. El modelo obtenido en general, es más impreciso que el modelo analítico, ya que depende en gran medida de los recursos integrados para su funcionamiento. No obstante, en muchos casos tiene la ventaja de ser más sencillo y rápido de obtener.

11



2.3.1.1 CINEMÁTICA DIRECTA CON TRANSFORMACIONES HOMOGÉNEAS

La convención descrita por Denavit y Hartenberg utiliza expresiones del algebra matricial para restringir la posición/orientación de un marco móvil respecto a un marco de referencia fijo. Por lo general, ésta metodología clásica establece un sistema de coordenadas para cada elemento del robot articulado, caracterizando su movimiento por medio de matrices de transformación homogénea i−1Ai, basado en el producto de cuatro transformaciones Homogéneas básicas, respectivamente, las cuales permiten obtener una matriz compuesta final 0Ti de posición/orientación como se detalla en Denavit et al. (1955).

FIG. 2.1.- Matriz de transformación homogénea

2.3.2 CINEMÁTICA INVERSA

La cinemática inversa es un problema más difícil, la solución a éste es computacionalmente expansiva y generalmente tarda mucho para el control de tiempo real sobre el manipulador. Las singularidades y no linealidades hacen que el problema sea complejo de solucionar. Por lo tanto, únicamente para una clase muy pequeña de los manipuladores, la cinemática inversa es simple (manipuladores con orientación de Euler) y obtiene soluciones analíticas completas como se apuntan en Kucuk et al. (2007).

Las técnicas conocidas para solucionar el problema de la cinemática inversa, es a través de métodos analíticos y el métodos numéricos. En el primer caso las variables comunes se solucionan analíticamente según los datos de la configuración dada.

12



Para el segundo la solución se obtiene cuando las variables comunes son evaluadas por algún método numérico. Para el método analítico las herramientas principales son las soluciones geométricas y algebraicas, la solución geométrica se aplica a estructuras simples del robot, tales como los manipuladores menos de dos grados de libertad o con ejes articulados paralelos.

Para los manipuladores con más de dos eslabones y los cuales se extienden en el espacio, la geometría y su desarrollo puede ser un poco tediosa. En ese caso, la solución algebraica es más bondadosa para hallar una respuesta a la cinemática inversa. Sin embargo, cuando las ecuaciones se encuentran acopladas incrementan su complejidad, debido a que hay múltiples soluciones y las singularidades existen en mayor grado, además, alguna de las soluciones matemáticas no siempre corresponde a las soluciones físicas, porque depende específicamente de la estructura del robot.

2.4 DINÁMICA DEL ROBOT

El análisis de movimiento de los cuerpos bajo la acción de las fuerzas que lo producen, se describe en términos de ecuaciones que relacionan el tiempo con el cambio de configuración del robot, obedeciendo a la segunda ley de Newton, donde la razón de cambio en el momento de una partícula es igual a la fuerza aplicada a ésta.

El análisis se realiza sobre un conjunto de partículas fijas hacia otras que tienen grados de libertad finitos y es necesario describir estas interconexiones, introduciendo restricciones entre las posiciones de las partículas, tal que la ecuación de restricción sea satisfecha y estas fuerzas de restricción son siempre normales a ésta superficie.

13



CAPITULO III DESARROLLO DE INGENIERÍA

3.1 PIEZAS, MODELADO Y ENSAMBLADO DEL BRAZO ROBÓTICO

Para este proyecto se construyó un robot usando cintra con una configuración articulada para la construcción de nuestro brazo robótico. Se utilizó el software de solidworks para el diseño, modelado y ensamble.

FIG 3.1.- Modelado de la base del brazo realizada en solidworks

14



FIG 3.2.- Modelado del primer eslabón del brazo realizado en solidworks

FIG. 3.3.- Modelado del segundo eslabón del brazo realizado en solidworks

15



FIG. 3.4.- Ensamblado de la base y los 2 eslabones

Las medidas de las piezas mostradas en las figuras 3.1, 3.2 Y 3.3 pueden ser visualizadas en los planos que vienen en el anexo (A1).

3.2 SERVOMOTORES

Se utilizó un servomotor MG995 “metal gear dual ball bearing servo” de alta velocidad por cada grado de libertad, utilizamos un total de 3, para más información acerca de sus especificaciones técnicas consultar el anexo (A2).

16



FIG. 3.5.- Servomotor

3.3 COMUNICACIÓN SOFTWARE-HARDWARE

Para la comunicación lógica-física se utilizó un arduino UNO, con el microcontrolador ATMEL ATMEGA 328P-PU este es una interfaz que comunica mediante un sistemas RS232 via USB con un puerto serial virtual en la computadora (COM3). Por otra parte dependiendo de variables angulares el microcontrolador envía pulsos a los servomotores mediante el PWM.

Para más información acerca del Arduino, se puede consultar el Datasheet en el anexo (A3).

17


Escuela Superior de Ingeniería Mecánica y Eléctrica Unidad Azcapotzalco3.4 DIAGRAMA ELECTRÓNICO

18



19



3.5 PROGRAMACION DEL ARDUINO

Para este proyecto se utilizó un programa creado por mathworks para el control de servomotores atraves de varias plataformas de la familia arduino llamese (Arduino minipro, Arduino Mega y Arduino 1) en el cual se programó con el compilador de fábrica de Arduino para el control del PWM y la interface de comunicación lógica-física. Ir al programa (Se pude consultar el Datasheet del pic visualizando el anexo A4).

20



3.6 PROGRAMACIÓN EN MATLAB

Se utilizó el software de Matlab para la elaboración del diseño algebraico de la cinemática directa, la cinemática inversa y la dinámica de nuestro brazo robótico.

Tomando los parámetros de Denavit-Hartenberg de nuestro brazo robótico:

i Ө di αi ai

1 t1=0° 2.3 -90° 0

2 t1=0° 0 0° 9.5

3 t1=0° 0 0° 4.35

Tabla 3.1 Parametros de Denavit-Hartenberg de nuestro robot articulado de 3 grados de libertad

3.6.1 PROGRAMA PARA LA CINEMÁTICA DIRECTA

clear;clc;clf; %limpiar todo%% declarar los parámetros Denavit-hartenberg para robots articulados/prismáticos%%[theta alpha a d sigma]dh =[ 0 -90 0 4.58 0 0 0 7.61 0 0 0 0 5.68 0 0 ];%% genera las ecuaciones para la cinemática directa%% Generar las ecuaciones para resolver la cinemática directaap=sym(dh(:,2)*(pi/180));a=dh(:,3);k=(dh(:,5)==0);T=eye(4,4);v='(t(i)*(k(i))+dh(i,1)*(1-k(i)))*((pi/180)*(1-k(i))+k(i))';for i=1:length(dh(:,1)) eval(['t(',num2str(i),')=[sym(''t',num2str(i),''')*sym(',num2str(k(i)),')+sym(''d',num2str(i),''')*sym(1-',num2str(k(i)),')];'... 'A',num2str(i-1),num2str(i),' = [cos(',v,'),-cos(ap(i))*sin(',v,'), sin(ap(i))*sin(',v,'),a(i)*cos(',v,');'... 'sin(',v,'), cos(ap(i))*cos(',v,'),-sin(ap(i))*cos(',v,'),a(i)*sin(',v,');'... '0,sin(ap(i)),cos(ap(i)),dh(i,4)*(k(i))+t(i)*(1-k(i));0 0 0 1];'... 'A',num2str(0),num2str(i),' =T*A',num2str(i-1),num2str(i),';'... 'T = A',num2str(0),num2str(i),';']); Trob(:,:,i)=T(1:3,:);End

21



GRAF. 3.1 Posición inicial del brazo robótico

En la gráfica 3.7 se puede apreciar la posición inicial de nuestro brazo robótico cuando el ángulo de la base, del primer eslabón y del segundo eslabón son igual con 0. Para facilitar nuestra parametrización de Denavit-Hartenberg usamos ángulos complementarios para que nuestra posición inicial del robot quedara como se observa en la gráfica 3.8 y así poder aplicar la cinemática inversa y empezar a realizar su trayectoria.

GRAF. 3.2 Posición inicial del brazo robótico con ángulos complementarios

22



3.6.2 PROGRAMA PARA LA CINEMÁTICA INVERSA

clear all;clc;clear;clc;clf; %limpiar todo%% declarar los parámetros Denavit-hartenberg para robots articulados/prismáticos%%[theta alpha a d sigma]dh =[ 0 -90 0 2.3 0 0 0 9.5 0 0 0 0 4.35 0 0 ];%% genera las ecuaciones para la cinemática directaKD(dh);%% variables angulares/linealest=[0 0 0];%% gráfico de cinemática directagrafrobot(t);VS=linspace(0,1,100)';V=VS*t;for i=1:length(VS) grafrobot(V(i,:)); end %Cinematica inversa%% %%cargar arduino%%%%arduino=arduino('COM3');%%%%arduino.servoAttach(9);%%%%arduino.servoAttach(10);%%%%arduino.servoAttach(11);%%%X=1;Y=5;Z=5;X0=1;Y0=5;Z0=5; d=2.3;L2=9.5;L3=4.35;cont=0;%%primera linea%%while X<4.0px=X;py=Y;

pz=Z;b=pz-d;h=sqrt(px^2+py^2);h2=sqrt(b^2+h^2);alfa=atan(b/h);beta=acos((L2^2+h2^2-L3^2)/(2*L2*h2));q2=-(alfa+beta);q1= atan(py/px);q3=acos((L2^2-h2^2+L3^2)/(2*L2*L3));q1=(q1*180)/pi;q2=(q2*180)/pi;q3=(q3*180)/pi;q3=-(q3-180); q4=round(q1);q5=round(q2);q6=round(q3); q5=q5*-1; %arduino.servoWrite(9,q4)%arduino.servoWrite(10,q5)%arduino.servoWrite(11,q6)t=[q1 q2 q3];lg=max(size(t));l=hsv;k=fix(64/lg);s=5;M='Marker';C='Color';LW='LineWidth';clf;line([0 s],[0 0],[0 0],M,'.',C,'r',LW,1);line([0 0],[0 s],[0 0],M,'.',C,'g',LW,1);line([0 0],[0 0],[0 s],M,'.',C,'b',LW,1);evalA=Trob(t);pox=0;poy=0;poz=0;for i=1:lg line([pox evalA(10,i)],[poy evalA(11,i)],[poz evalA(12,i)],'MarkerEdgeColor',[0 0 0],M,'.',C,l(i*k,:),LW,3); pox=evalA(10,i);poy=evalA(11,i);poz=evalA(12,i); line([pox evalA(1,i)+pox],[poy evalA(2,i)+poy],[poz evalA(3,i)+poz],M,'.',C,'r',LW,1);

23



line([pox evalA(4,i)+pox],[poy evalA(5,i)+poy],[poz evalA(6,i)+poz],M,'.',C,'g',LW,1); line([pox evalA(7,i)+pox],[poy evalA(8,i)+poy],[poz evalA(9,i)+poz],M,'.',C,'b',LW,1);endline([X0 X],[Y0 Y],[Z0 Z],M,'.',C,'k',LW,1);view(3);pause(1e-300); Qt=[q4;q5;q6] X=X+0.1 ;pause(0.2);end X1=X;Y1=Y;Z1=Z; %%SEGUNDA linea%%while Y<8.0px=X;py=Y;pz=Z;b=pz-d;h=sqrt(px^2+py^2);h2=sqrt(b^2+h^2);alfa=atan(b/h);beta=acos((L2^2+h2^2-L3^2)/(2*L2*h2));q2=-(alfa+beta);q1= atan(py/px);q3=acos((L2^2-h2^2+L3^2)/(2*L2*L3));q1=(q1*180)/pi;q2=(q2*180)/pi;q3=(q3*180)/pi;q3=-(q3-180); q4=round(q1);q5=round(q2);q6=round(q3); q5=q5*-1;

%arduino.servoWrite(9,q4)

%arduino.servoWrite(10,q5)%arduino.servoWrite(11,q6)t=[q1 q2 q3];lg=max(size(t));l=hsv;k=fix(64/lg);s=5;M='Marker';C='Color';LW='LineWidth';clf;line([0 s],[0 0],[0 0],M,'.',C,'r',LW,1);line([0 0],[0 s],[0 0],M,'.',C,'g',LW,1);line([0 0],[0 0],[0 s],M,'.',C,'b',LW,1);evalA=Trob(t);pox=0;poy=0;poz=0;for i=1:lg line([pox evalA(10,i)],[poy evalA(11,i)],[poz evalA(12,i)],'MarkerEdgeColor',[0 0 0],M,'.',C,l(i*k,:),LW,3); pox=evalA(10,i);poy=evalA(11,i);poz=evalA(12,i); line([pox evalA(1,i)+pox],[poy evalA(2,i)+poy],[poz evalA(3,i)+poz],M,'.',C,'r',LW,1); line([pox evalA(4,i)+pox],[poy evalA(5,i)+poy],[poz evalA(6,i)+poz],M,'.',C,'g',LW,1); line([pox evalA(7,i)+pox],[poy evalA(8,i)+poy],[poz evalA(9,i)+poz],M,'.',C,'b',LW,1);endline([X0 X1],[Y0 Y1],[Z0 Z1],M,'.',C,'k',LW,1);line([X1 X],[Y1 Y],[Z1 Z],M,'.',C,'k',LW,1);view(3);pause(1e-300); Qt=[q4;q5;q6] Y=Y+0.1 ;pause(0.2);endX2=X;Y2=Y;

Z2=Z;%%tercera linea%%

24


Escuela Superior de Ingeniería Mecánica y Eléctrica Unidad Azcapotzalcowhile X>1px=X;py=Y;pz=Z;b=pz-d;h=sqrt(px^2+py^2);h2=sqrt(b^2+h^2);alfa=atan(b/h);beta=acos((L2^2+h2^2-L3^2)/(2*L2*h2));q2=-(alfa+beta);q1= atan(py/px);q3=acos((L2^2-h2^2+L3^2)/(2*L2*L3));q1=(q1*180)/pi;q2=(q2*180)/pi;q3=(q3*180)/pi;q3=-(q3-180); q4=round(q1);q5=round(q2);q6=round(q3); q5=q5*-1; %arduino.servoWrite(9,q4)%arduino.servoWrite(10,q5)%arduino.servoWrite(11,q6)t=[q1 q2 q3];lg=max(size(t));l=hsv;k=fix(64/lg);s=5;M='Marker';C='Color';LW='LineWidth';clf;line([0 s],[0 0],[0 0],M,'.',C,'r',LW,1);line([0 0],[0 s],[0 0],M,'.',C,'g',LW,1);line([0 0],[0 0],[0 s],M,'.',C,'b',LW,1);evalA=Trob(t);pox=0;poy=0;poz=0;for i=1:lg line([pox evalA(10,i)],[poy evalA(11,i)],[poz evalA(12,i)],'MarkerEdgeColor',[0 0 0],M,'.',C,l(i*k,:),LW,3); pox=evalA(10,i);poy=evalA(11,i);poz=evalA(12,i); line([pox evalA(1,i)+pox],[poy evalA(2,i)+poy],[poz

evalA(3,i)+poz],M,'.',C,'r',LW,1);

line([pox evalA(4,i)+pox],[poy evalA(5,i)+poy],[poz evalA(6,i)+poz],M,'.',C,'g',LW,1); line([pox evalA(7,i)+pox],[poy evalA(8,i)+poy],[poz evalA(9,i)+poz],M,'.',C,'b',LW,1);endline([X0 X1],[Y0 Y1],[Z0 Z1],M,'.',C,'k',LW,1);line([X1 X2],[Y1 Y2],[Z1 Z2],M,'.',C,'k',LW,1);line([X2 X],[Y2 Y],[Z2 Z],M,'.',C,'k',LW,1);view(3);pause(1e-300); Qt=[q4;q5;q6] X=X-0.1 ;pause(0.2);end X3=X;Y3=Y;Z3=Z; %%SEGUNDA linea%%while Y>5px=X;py=Y;pz=Z;b=pz-d;h=sqrt(px^2+py^2);h2=sqrt(b^2+h^2);alfa=atan(b/h);beta=acos((L2^2+h2^2-L3^2)/(2*L2*h2));q2=-(alfa+beta);q1= atan(py/px);q3=acos((L2^2-h2^2+L3^2)/(2*L2*L3));q1=(q1*180)/pi;q2=(q2*180)/pi;q3=(q3*180)/pi;q3=-(q3-180);

q4=round(q1);q5=round(q2);q6=round(q3);

25


Escuela Superior de Ingeniería Mecánica y Eléctrica Unidad Azcapotzalcoq5=q5*-1; %arduino.servoWrite(9,q4)%arduino.servoWrite(10,q5)%arduino.servoWrite(11,q6)

t=[q1 q2 q3];lg=max(size(t));l=hsv;k=fix(64/lg);s=5;M='Marker';C='Color';LW='LineWidth';clf;line([0 s],[0 0],[0 0],M,'.',C,'r',LW,1);line([0 0],[0 s],[0 0],M,'.',C,'g',LW,1);line([0 0],[0 0],[0 s],M,'.',C,'b',LW,1);evalA=Trob(t);pox=0;poy=0;poz=0;for i=1:lg line([pox evalA(10,i)],[poy evalA(11,i)],[poz evalA(12,i)],'MarkerEdgeColor',[0 0 0],M,'.',C,l(i*k,:),LW,3); pox=evalA(10,i);poy=evalA(11,i);poz=evalA(12,i); line([pox evalA(1,i)+pox],[poy evalA(2,i)+poy],[poz

evalA(3,i)+poz],M,'.',C,'r',LW,1);line([pox evalA(4,i)+pox],[poy evalA(5,i)+poy],[poz evalA(6,i)+poz],M,'.',C,'g',LW,1); line([pox evalA(7,i)+pox],[poy evalA(8,i)+poy],[poz evalA(9,i)+poz],M,'.',C,'b',LW,1);endline([X0 X1],[Y0 Y1],[Z0 Z1],M,'.',C,'k',LW,1);line([X1 X2],[Y1 Y2],[Z1 Z2],M,'.',C,'k',LW,1);line([X2 X3],[Y2 Y3],[Z2 Z3],M,'.',C,'k',LW,1);line([X3 X],[Y3 Y],[Z3 Z],M,'.',C,'k',LW,1);view(3);pause(1e-300); Qt=[q4;q5;q6]Y=Y-0.1 ;pause(0.2);end

26



3.6.3 PROGRAMA PARA LA DINÁMICA

clear;clc;clf; %limpiar todo%% declarar los parámetros Denavit-hartenberg para robots articulados/prismáticos%%[theta alpha a d sigma]dh =[ 0 -90 0 4.58 0 0 0 7.61 0 0 0 0 5.68 0 0 ];%% genera las ecuaciones para la cinemática directa%% Generar las ecuaciones para resolver la cinemática directaap=sym(dh(:,2)*(pi/180));a=dh(:,3);k=(dh(:,5)==0);T=eye(4,4);v='(t(i)*(k(i))+dh(i,1)*(1-k(i)))*((pi/180)*(1-k(i))+k(i))';for i=1:length(dh(:,1)) eval(['t(',num2str(i),')=[sym(''t',num2str(i),''')*sym(',num2str(k(i)),')+sym(''d',num2str(i),''')*sym(1-',num2str(k(i)),')];'... 'A',num2str(i-1),num2str(i),' = [cos(',v,'),-cos(ap(i))*sin(',v,'), sin(ap(i))*sin(',v,'),a(i)*cos(',v,');'... 'sin(',v,'), cos(ap(i))*cos(',v,'),-sin(ap(i))*cos(',v,'),a(i)*sin(',v,');'... '0,sin(ap(i)),cos(ap(i)),dh(i,4)*(k(i))+t(i)*(1-k(i));0 0 0 1];'... 'A',num2str(0),num2str(i),' =T*A',num2str(i-1),num2str(i),';'... 'T = A',num2str(0),num2str(i),';']); Trob(:,:,i)=T(1:3,:);end

%%A00=eye(4);A11=A00;A22=A00;A33=A00;Q1=[0 -1 0 0;1 0 0 0;0 0 0 0;0 0 0 0];Q2=Q1;Q3=Q1;A13=A12*A23;

for i=1:length(dh(:,1)) for j=1:length(dh(:,1)) if j<=i eval(['U',num2str(i),num2str(j),' =A',num2str(0),num2str(j-1),'*Q',num2str(j),'*A',num2str(j-1),num2str(i),';']); else eval(['U',num2str(i),num2str(j),' =zeros(4);']); end end

27



endfor i=1:length(dh(:,1)) for j=1:length(dh(:,1)) for k=1:length(dh(:,1)) if k>i | j>i eval(['U',num2str(i),num2str(j),num2str(k),' =zeros(4);']); else if i>=j & j>=k eval(['U',num2str(i),num2str(j),num2str(k),' =A',num2str(0),num2str(k-1),'*Q',num2str(k),'*A',num2str(k-1),num2str(j-1),'*Q',num2str(j),'*A',num2str(j-1),num2str(i),';']); else if i>=k & k>=j eval(['U',num2str(i),num2str(j),num2str(k),' =A',num2str(0),num2str(j-1),'*Q',num2str(j),'*A',num2str(j-1),num2str(k-1),'*Q',num2str(k),'*A',num2str(k-1),num2str(i),';']); 7 end end end end endendsyms m1 m2 L1 L2 L3 m3

J1=[0 0 0 0;0 0 0 0;0 0 0 0;0 0 0 m1];J2=[m2*L2^2 0 0 m2*L2;0 0 0 0;0 0 0 0;m2*L2 0 0 m2];J3=[m3*L3^2 0 0 m3*L3;0 0 0 0;0 0 0 0;m3*L3 0 0 m3];

disp('6.- Cálculo de las matrices de inercia : D = [dij]')

d11=simple(simplify(trace(U11*J1*U11.')))+simple(simplify(trace(U21*J2*U21.')))+simple(simplify(trace(U31*J3*U31.')));d12=simple(simplify(trace(U22*J2*U21.')))+simple(simplify(trace(U32*J3*U31.')));d13=simple(simplify(trace(U33*J3*U31.')));d21=simple(simplify(trace(U21*J2*U22.')))+simple(simplify(trace(U31*J3*U32.')));d22=simple(simplify(trace(U22*J2*U22.')))+simple(simplify(trace(U32*J3*U32.')));d23=simple(simplify(trace(U33*J3*U32.')));

d31=simple(simplify(trace(U31*J3*U33.')));d32=simple(simplify(trace(U32*J3*U33.')));d33=simple(simplify(trace(U33*J3*U33.')));D = [d11 d12 d13;d21 d22 d23;d31 d32 d33];%%for i=1:length(dh(:,1)) %%for j=1:length(dh(:,1))

28



%%eval(['D',num2str(i),num2str(j),'=symsum(k,(simplify(trace(U11*J1*U11.'))))+simple(simplify(trace(U21*J2*U21.'))))']); %% end%%end

disp('7.- Cálculo del vector hikm')

h111=simplify(trace(U111*J1*U11.'))+simplify(trace(U211*J2*U21.'))+simplify(trace(U311*J3*U31.'));h112=simplify(trace(U212*J2*U21.'));h113=simplify(trace(U313*J3*U31.'));h121=simplify(trace(U221*J2*U21.'));h122=simplify(trace(U222*J2*U21.'));h123=simplify(trace(U323*J3*U31.'));h131=simplify(trace(U331*J3*U31.'));h132=simplify(trace(U332*J3*U31.'));h133=simplify(trace(U333*J3*U31.'));h211=simplify(trace(U211*J2*U22.'));h212=simplify(trace(U212*J2*U22.'));h213=simplify(trace(U313*J3*U32.'));h221=simplify(trace(U221*J2*U22.'));h222=simplify(trace(U222*J2*U22.'));h223=simplify(trace(U323*J3*U32.'));h231=simplify(trace(U331*J3*U32.'));h232=simplify(trace(U332*J3*U32.'));h233=simplify(trace(U333*J3*U32.'));h311=simplify(trace(U311*J3*U33.'));h312=simplify(trace(U312*J3*U33.'));h313=simplify(trace(U313*J3*U33.'));h321=simplify(trace(U321*J3*U33.'));h322=simplify(trace(U322*J3*U33.'));h323=simplify(trace(U323*J3*U33.'));h331=simplify(trace(U331*J3*U33.'));h332=simplify(trace(U332*J3*U33.'));h333=simplify(trace(U333*J3*U33.'));

disp('8.- Cálculo del vector de fuerza centrífuga y de coriolis:H=[hi]')

syms t1 t2 t3 dt1 dt2 dt3

29



syms gh1=h111*t1^2+h112*t1*t2+h113*t1*t3+h121*t2*t1+h122*t2^2+h123*t2*t3+h131*t3*t1+h132*t3*t2+h133*t3^2;h2=h211*t1^2+h212*t1*t2+h213*t1*t3+h221*t2*t1+h222*t2^2+h223*t2*t3+h231*t3*t1+h232*t3*t2+h233*t3^2;h3=h311*t1^2+h312*t1*t2+h313*t1*t3+h321*t2*t1+h322*t2^2+h323*t2*t3+h331*t3*t1+h332*t3*t2+h333*t3^2;

H=[h1;h2;h3];

disp('9.- Cálculo de la matriz columna de fuerzas de gravedad:C=[ci]')disp('--------------------------------------------------------------------')g1=[g 0 0 0]; r11=[0;0;0;1]; % CG para el eslabon 1r22=[0;L2;0;1]; % CG para el eslabon 2r33=[0;0;0;1];c1=-m1*g1*U11*r11 - m2*g1*U21*r22 - m3*g1*U31*r33;c2=-m1*g1*U12*r11 - m2*g1*U22*r22 - m3*g1*U32*r33;c3=-m1*g1*U13*r11 - m2*g1*U23*r22 - m3*g1*U33*r33;C=[c1;c2;c3];

disp('10.- Ecuación Dinámica del Robot')

tau=simplify(D*[dt1;dt2;dt3]+H+C);disp('El torque o par para la primera articulación revoluta: ')

T1 = tau(1) % Juntura Revoluta

disp('La fuerza para al articulación prismática:')

F2 = tau(2) % Juntura Prismática

F3 = tau(3) % Juntura Prismática

disp('Ecuación Dinámica del Robot Manipulador RP:')

tau=[tau(1); tau(2)]

30



3.7 MODELO DINÁMICO

De acuerdo a nuestro programa de dinámica y de los parámetros de Denavit-Hartenberg modificado se logró sacar el modelo dinámico de nuestro brazo robótico. Para observar el modelo dinámico del brazo articulado de 3 grados de libertad, consultar el anexo A5.

31



CAPITULO IV CONCLUSIONES, REFERENCIAS Y ANEXOS

4.1 CONCLUSIONES GENERALES

4.2 ANEXOS

En esta sección se colocaran los vínculos hacia los anexos correspondientes puesto que la mayoría son extensos y requieren ser visualizados por completo.

4.2.1 ANEXO (A1)

Planos de cada pieza realizada para la construcción del brazo articulado de 3 grados de libertad.

PLANOS DE PIEZAS\ESLABON 1.PDF (Plano del eslabón 1).

PLANOS DE PIEZAS\UNION DE ESLABON 1 Y 2.PDF (Pieza de unión de eslabones).

4.2.2 ANEXO (A2)

SERVOMOTOR NEO-X 3010 (Hoja técnica del servomotor)SERVOMOTOR NEO-X MINI 2240 (Hoja técnica del servomotor)

4.2.5 ANEXO (A3)PROGRAMA SERVO MATLAB

4.2.6 ANEXO (A4)

ATMEL82-71.PDF (Datasheet Del AVR)

4.2.7 ANEXO (A5)

MODELO DINAMICO\DINAMICAL-E.DOCX (Modelo dinamico del robot articulado de 3 grados de libertad)

32

robot de tipo articulado de 3 grados de libertad

Documents