Embed Size (px)
Citation preview
OPTIMIZACIÓN DE UN ROBOT TIPO DELTA
DANIEL JARAMILLO HURTADO
UNIVERSIDAD DE LOS ANDES FACULTAD DE INGENIERÍA
DEPARTAMENTO DE INGENIERÍA MECÁNICA BOGOTÁ 2013
2
OPTIMIZACION DE UN ROBOT TIPO DELTA
DANIEL JARAMILLO HURTADO
Proyecto de grado para optar por el título de Ingeniero Mecánico
ASESOR: CARLOS FRANCISCO RODRÍGUEZ HERRERA, I.M., Ph.D
Profesor Asociado
UNIVERSIDAD DE LOS ANDES FACULTAD DE INGENIERÍA
DEPARTAMENTO DE INGENIERÍA MECÁNICA BOGOTÁ 2013
3
AGRADECIMIENTOS
Agradezco a mis padres por darme la oportunidad de llevar a cabo éste proyecto de vida que es la ingeniería mecánica. Agradezco al doctor Carlos Francisco Rodríguez por su liderazgo y motivación en el desarrollo del proyecto. Por último, a Jhon Sneider por su orientación y apoyo en cuanto a temas de conexión y programación en la plataforma YTERM.
4
TABLA DE CONTENIDOS
1. INTRODUCCIÓN: ............................................................................................................ 5 1.1 Motivación: ............................................................................................................................ 6 1.2 Objetivos: ............................................................................................................................... 6
2. CALIBRACIÓN: ................................................................................................................ 7 2.1 Montaje de Sensores Inductivos de proximidad: ..................................................... 7 2.2 Programación para rutina HOME: ................................................................................. 9 2.3 Medición de ángulos de entrada en posición HOME: ............................................ 10
3. OPTIMIZACIÓN: ........................................................................................................... 12 3.1 Trayectoria Característica: ............................................................................................ 12 3.2 Análisis de Posición: ........................................................................................................ 12 3.3 Planteamiento Dinámico: ............................................................................................... 13 3.4 Método Numérico de Optimización: ........................................................................... 15 3.5 Procedimiento: .................................................................................................................. 16 3.6 Resultados Teóricos: ........................................................................................................ 18
4. EXPERIMENTACIÓN: .................................................................................................. 21 4.1 Programación de trayectorias: De MATLAB a YTERM: ......................................... 21 4.2 Mediciones de Torque: .................................................................................................... 22 4.3 Resultados Experimentales: .......................................................................................... 24
5. CONCLUSIONES: .......................................................................................................... 30
6. BIBLIOGRAFÍA: ............................................................................................................ 31
7. ANEXOS: ......................................................................................................................... 32 Función para generar puntos por trayectoria lineal: .................................................. 32 Función para discretizar cinemática de un círculo: ..................................................... 33 Cinemática Inversa: ................................................................................................................. 34 Torque a partir del análisis Jacobiano: ............................................................................. 36 Torque a partir del análisis dinámico (planteamiento de Lagrange): ................... 38 Ejecutable para Optimización: ............................................................................................. 40 De ángulos a pulsos: ................................................................................................................ 42 Código YTERM para trayectoria característica con a=0.14m: .................................. 43
5
1. INTRODUCCIÓN: El robot Delta se encuentra clasificado entre los robots paralelos planares. Éstos se caracterizan por una plataforma móvil conectada a una base fija por medio de varios brazos, donde cada uno de los brazos cuenta con cierto número de barras. Cada uno de éstos es controlado por un actuador, el cual suele ser montado en la plataforma fija [Tsai, 1999]. El robot Delta cuenta con el mismo número de brazos que grados de libertad (3), el número de uniones en cada uno de los brazos está distribuido en el mismo orden y la ubicación de las barras accionadas es igual. Por ésta razón el robot cumple con todos los requisitos para ser considerado un robot simétrico. Éste tipo de robots paralelos se clasifica según el tipo de uniones que utilizan (Rotacionales o Prismáticas) y el orden en que se encuentran (desde la plataforma fija a la plataforma movil): RRR, RRP, RPR, PRR, RPP, PRP y PPR. El robot delta es un robot 3RPP. La siguiente figura presenta un esquema general del robot delta:
Figura 1: [Laribi, M.A; Romdhane, L; Zeghloul, S, 2005] Con el fin de definir los parámetros del robot delta se presenta la siguiente imagen:
Figura 2:[Tsai, 1999]
6
Como se puede apreciar en la figura anterior la sección OA se define como “r”, la sección AB como “a”, BC como “b” y CP como “h”. Los parámetros de entrada son los ángulos entre la plataforma fija y la barra “a”, también definidos como 𝜃!! (para i=1,2,3). A partir de éstos se pueden definir las coordenadas del punto “P” en el espacio [x y z]. Los ángulos 𝜃!!,𝜃!",𝜃!" son controlados por servomotores Yaskawa SGMAH-‐01AAF41 conectados a un amplificador “Legend 01” manipulados por un controlador Yaskawa SMC4040. Dicho controlador recibe comandos de pulsos de un computador por medio del software YTERM. En la sección de resultado se describe con mayor profundidad el procedimiento para programar trayectorias en YTERM y los factores de conversión necesarios.
1.1 Motivación: El sistema de control del robot permite definir la velocidad angular de entrada, de tal forma que el torque requerido en los servomotores es variable según las condiciones dinámicas de la trayectoria y la geometría del robot. En el presente trabajo se pretende optimizar la sección de barra AB (es decir la barra “a”) de tal forma que el robot exija el mínimo torque posible ante una trayectoria definida. Dicho estudio será evaluado de manera analítica para luego realizar una validación experimental, lo cual pondrá en evidencia la magnitud de las variables que no son consideradas en el modelo dinámico (tales como fricción en las uniones, elasticidad de partes, resistencia del aire, entre otras).
1.2 Objetivos: Minimizar las condiciones de torque de un robot tipo Delta a partir de cambios en la longitud de las barras que unen la plataforma móvil a la plataforma estacionaria.
-‐ Evaluación cinemática del robot Delta.
-‐ Análisis dinámico (Lagrange) para búsqueda del valor mínimo.
-‐ Manufactura de barras con longitudes determinadas por el análisis dinámico.
-‐ Implementación de la posición HOME.
-‐ Programación de trayectoria característica en YTERM.
-‐ Medición de torque para diferentes longitudes de barras.
-‐ Selección de proporciones óptimas según resultados experimentales.
7
2. CALIBRACIÓN: Debido a que los encoders de los servomotores son incrementales, no hay manera de conocer la posición angular absoluta del servomotor. Esto implica que no hay manera de solicitar una trayectoria definida, pues no se conoce la posición inicial. Por dicha razón se implementó una posición HOME, la cual se establece como la posición del robot Delta en el momento en que se active un sensor ubicado en algún punto de la trayectoria. Luego se toman mediciones de los ángulos en los cuales se encuentra la barra en la posición HOME para determinar la posición inicial para la trayectoria. Cabe resaltar la importancia de la calibración para el procedimiento experimental a seguir, pues a partir de ésta se asegura la repetibilidad de la trayectoria y por lo tanto se asegura que las diferencias en los resultados de torque se deban a cambios en la longitud de la barra “a”. Por otra parte, la calibración permite que la trayectoria se realice tal como fue programada y por lo tanto permite comparar los resultados experimentales con los teóricos por medio del modelo dinámico.
2.1 Montaje de Sensores Inductivos de proximidad: Los sensores utilizados para la implementación de la posición HOME se debían ubicar de tal manera que detectaran la proximidad de cada una de las 3 barras denominadas “a” (ver figura #2) en cierta posición angular. Por ésta razón se diseñó un soporte que permitiera ubicar los sensores para cumplir dicho fin pero que no interceptara el robot en ningún punto del volumen de trabajo. Con el fin de seleccionar un sensor adecuado para la implementación de HOME se consultó el catálogo del controlador Yaskawa SMC4040 ® para determinar las condiciones a las cuales debe trabajar dicho sensor:
Figura 3: Especificaciones para entradas dedicadas a posición HOME. Extraido del manual del controlador Yaskawa SMC4040 ®.
8
De la figura anterior se puede ver que para la entrada HOME, los sensores deben ser a 24 V DC con una entrada tipo “sinking”. Por otra parte, el catálogo presenta el siguiente diagrama de conexión para los sensores de HOME:
Figura # 4: Conexión de entradas para la posición HOME. Extraido del manual del controlador Yaskawa SMC 4040 ®. De la figura anterior se puede ver que la entrada HOME debe ir referenciada a tierra por lo cual el sensor a utilizar debe ser de tipo PNP (transistor de unión bipolar tipo PNP). Considerando las condiciones anteriores y el hecho que la barra a censar es metálica, se selecciona un sensor inductivo con la siguiente referencia:
-‐ AUTONICS PR18-‐8DP El cual tiene un diámetro de 18mm, alcance de 8mm, alimentado a 24 VDC, es normalmente abierto y tipo PNP. Luego de seleccionado el sensor se procede con el diseño del soporte, el cual se debe acomodar a las restricciones del montaje y del movimiento del robot. Para censar la barra “a” en su recorrido rotacional resulta ideal un soporte lateral del sensor, de tal manera que ésta nunca interceptará el soporte ni el sensor. Sin embargo, las barras “b” tienen 2 grados de libertad en rotación por lo cual el soporte se debe ubicar lo suficientemente alto de tal manera que ésta no tenga la posibilidad de colisionar contra el soporte. Considerando que el robot cuenta con una limitación de su volumen de trabajo causada por la inversión geométrica de la estructura (ocurre cuando las barras “b” se encuentran horizontales) se optó por diseñar el soporte de tal manera que la distancia entre la plataforma fija y las barras “b” en la condición límite (inversión geométrica) sea mayor que la distancia entre la plataforma fija y el extremo del soporte. De ésta manera las barras “b” nunca llegarán a interceptar el soporte de los sensores para HOME (el diseño se realiza a partir de una longitud de barra “a” de 24cm, la cual se encuentra montada inicialmente). El último criterio de diseño a considerar es que los sensores deben contar con la posibilidad de desplazarse a diferentes posiciones en los soportes. Esto se debe a
9
que la posición del sensor determinará la ubicación angular de la barra “a” en HOME y resulta conveniente ubicar éstos para lograr una posición determinada (Ej. si se quiere contar con la misma posición angular para todas las barras). Por último los sensores deben ser conectados como lo muestra la figura #4. Se debe asegurar de conectar cada sensor en la entrada correspondiente al servomotor que va a sensar. Es decir, el sensor que se encuentra ubicado para sensar la barra “a” del servomotor Y,Z o W se debe conectar en la entrada HOME destinada a Y,Z o W respectivamente:
Figura #5: PIN donde se debe conectar cada sensor HOME. Extraido de manual del controlador Yaskawa SMC 4040 ®. El sensor PR18-‐8DP cuenta con 3 cables de color: café, azul, negro. El cable café se debe conectar a la fuente de 24 V DC. El azul se debe conectar a tierra (0 V DC), y por último el negro se conecta al PIN correspondiente presentado en la figura # 4.
2.2 Programación para rutina HOME: Luego que los sensores inductivos se encuentran debidamente posicionados y conectados se procede con el código que establezca la rutina HOME. El software YTERM cuenta con una rutina de HOME establecida, de la cual se toma provecho para ésta aplicación. Se trata del comando “FE” (FindEdge) el cual activa los servomotores en la dirección definida por el comando “CN” (configure limitswitches) y a la velocidad solicitada hasta hallar un cambio de voltaje en la entrada dedicada a los sensores de HOME. Por lo tanto el algoritmo para implementar la rutina HOME resulta bastante sencillo y se presenta a continuación: #HOME SH CN -‐1,1,-‐1,0 FE YZW BG EN Descripción de comandos: #HOME: Se usa para darle nombre a la subrituna (en este caso se le llama “HOME”) y facilitar la llamada de ésta. Por ejemplo, en la ventana de comandos
10
puede escribir el comando “XQ#HOME,0” y correrá la rutina HOME anteriormente planteada en la memoria 0. SH: “Servo HERE” Éste comando le dice al controlador que use la posición actual de todos los servomotores (X,Y,Z,W) como la posición requerida y habilita el control del PID. CN -‐1,1,-‐1,0: “Configure limit switches”. Permite configurar 4 parámetros: CN m,n,o,p. El parámetro “n” permite configurar la dirección de rotación de los servomotores al comandar una rutina HOME. El valor de “n” en 1 girará el motor en dirección de las manillas del reloj mientras que el valor de “n” en -‐1 en el sentido contrario. Los demás parámetros (m, o, p) se dejan como se encuentran por Default (-‐1,-‐1,0 respectivamente). FE YZW: “Find Edge”. El commando FE YZW hace rotar los servomotores YZW en la dirección definida en CN (n=1=clockwise) hasta detectar un cambio en el voltaje de la entrada asignada al HOME de cada uno de los servomotores (ver figura #5). BG: “Begin”. Comando para comenzar el movimiento de lo programado anteriormente. EN: “End”. Fin de programa o subrutina.
2.3 Medición de ángulos de entrada en posición HOME: Después de implementar la posición HOME del robot Delta, se procede a determinar los ángulos de entrada para cada una de los brazos. Por cuestiones de simetría resulta idóneo que los ángulos sean los mismos en cada una de las tres barras, por lo cual se procede a mover el sensor dentro de la corredera del soporte hasta obtener el valor deseado. La calibración del robot permitió posicionar los sensores de tal manera que los ángulos de entrada en cada uno de los brazos sean:
𝜃!!𝜃!"𝜃!"
=45°45°45°
*Se recomienda revisar los ángulos de entrada para la posición HOME
antes de realizar pruebas. La posición angular inicial de las barras “a” (ángulo de entrada) se definen con las mediciones de la posición HOME, las cuales corresponden a la primera fila de la matriz de ángulos. Por otra parte los demás ángulos de cada una de las posiciones definidas en la trayectoria se determina por medio de la cinemática inversa que se encuentra implementada en el algoritmo “angulosEn(p)” donde “P” es el vector de posición en x,y,z del centro de la plataforma móvil. A continuación se presenta una imagen del robot Delta durante la calibración para la posición HOME:
11
Imagen #1. Robot Delta en posición HOME.
12
3. OPTIMIZACIÓN: Como se mencionó anteriormente, las condiciones de torque en los servomotores del robot Delta dependen tanto de la dinámica en la trayectoria planteada como en la geometría del robot. En el presente trabajo se realizará una aproximación hacia la optimización de la geometría a partir de una trayectoria definida. Para realizar dicha optimización se realizará una primera aproximación teórica a partir del modelo dinámico de Lagrange y un algoritmo genético para hallar la longitud de barra que logra el torque mínimo. Luego se tomarán mediciones experimentales de torque con diferentes longitudes de barra para hallar el valor óptimo y comparar con los resultados obtenidos de manera teórica. Para esto se requiere lo siguiente: Establecer una trayectoria característica y determinar la posición angular requerida para cada punto de dicha trayectoria (cinemática inversa). A partir de las posiciones angulares(𝜃!!), espaciales (P) y el vector de tiempo se pueden determinar numéricamente las condiciones de velocidad y aceleración durante la trayectoria. Dichos parámetros se utilizan para determinar las condiciones de torque a partir del modelo dinámico de Lagrange. Por último se corre algoritmo (genético) para determinar el valor óptimo para la longitud de barra “a”.
3.1 Trayectoria Característica: Una de las grandes falencias de optimizar con el procedimiento anteriormente descrito es que se requiere definir una trayectoria para la cual se hallará el valor óptimo y por lo tanto la optimización es restringida a dicha trayectoria. Para el caso de estudio se considera como trayectoria característica realizar círculos en las siguientes coordenadas:
𝑧 = 0.2 𝑚
𝑟 = 0.07 𝑚 Se consideró dicha trayectoria circular porque al no contar con cambios bruscos de dirección se puede llegar a valores de torque que se asemejen más al modelo teórico. Por otra parte, ésta trayectoria cuenta con la ventaja de otorgar resultados simétricos para cada uno de los servomotores.
3.2 Análisis de Posición: Con el fin de establecer la relación geométrica entre los parámetros de entrada y la posición de salida se puede realizar la cinemática inversa y directa del robot Delta. La cinemática inversa permite determinar los ángulos de entrada (𝜃!!,𝜃!",𝜃!" ) a partir de las coordenadas del punto “P”, mientras que la cinemática directa define los ángulos de entrada y calcula la posición del punto “P”.
13
Para el caso de estudio resulta relevante la cinemática inversa, pues se requiere una posición de la plataforma y se deben conocer los comandos que serán enviados al servomotor. Los resultado obtenidos por [Tsai, 1999] son los siguientes:
De tal forma que se pueden despejar los valores de los ángulos como:
Donde:
Finalmente los ángulos de entrada 𝜃!!no se pueden hallar analíticamente del sistema de ecuaciones presentado anteriormente por lo cual se debe hacer uso de métodos numéricos de solución.
𝑎 ∗ cos 𝜽𝟏𝒊 + 𝑏 ∗ sin 𝜃!! ∗ cos 𝜽𝟏𝒊 + 𝜃!!= 𝑐𝑜𝑠 𝜙! ∗ 𝑃! + sin 𝜙! ∗ 𝑃! + ℎ − 𝑟
Para esto de realizan iteraciones del valor de 𝜃!! hasta que el valor de la parte derecha de la ecuación anterior sea lo suficientemente cercano al valor de la izquierda. Ya que los algoritmos de solución requieren un valor de
adivinanza desde el cual inician se definió éste como 𝝅𝟒.
3.3 Planteamiento Dinámico: A partir de los resultados obtenidos en el análisis de posición y de métodos numéricos para determinar las condiciones de velocidad y aceleración se utiliza el siguiente modelo dinámico para determinar el torque en cada uno de los 3 servomotores para cada punto de la trayectoria.
14
En el presente estudio no se utiliza el análisis Jacobiano, pues éste resulta limitado cuando se requiere una expresión analítica para los valores de torque en un robot paralelo ya que no considera la masa, inercia y aceleraciones del sistema. Por ésta razón se plantea la dinámica de Lagrange para el robot Delta presentado en la sección 10.4.1 de [Tsai, 1999].Otros planteamientos de la dinámica de robots paralelos son el principio de trabajo virtual y la formulación de Newton-‐Euler. La dinámica de Lagrange parte de la siguiente ecuación para establecer los valores de torque en las barras accionadas del robot Delta:
Donde 𝑄! representa el torque en cada uno de los actuadores. Luego de un procedimiento matemático que puede ser consultado en [Tsai, 1999] (sección 10.4.1) se obtiene el siguiente sistema de ecuaciones para hallar los valores de 𝜆! (multiplicadores de Lagrange para i=1,2,3) :
Las tres ecuaciones anteriores generan un sistema de ecuaciones de 3x3 que se puede utilizar para despejar los valores de los multiplicadores de Lagrange. A partir de éstos se puede hallar el torque de los tres servomotores con las siguientes 3 ecuaciones:
15
A continuación se describe el significado de las nuevas variables introducidas (las demás se encuentran representadas en la Figura 2).
𝑚! = 𝑚𝑎𝑠𝑎 𝑑𝑒 𝑙𝑎 𝑝𝑙𝑎𝑡𝑎𝑓𝑜𝑟𝑚𝑎
𝑚! = 𝑚𝑎𝑠𝑎 𝑑𝑒 1 𝑑𝑒 𝑙𝑎𝑠 2 𝑏𝑎𝑟𝑟𝑎𝑠 𝑑𝑒 "b"
𝑚! = 𝑚𝑎𝑠𝑎 𝑑𝑒 𝑙𝑎 𝑏𝑎𝑟𝑟𝑎 "𝑎"
𝑓!" = 𝑓𝑢𝑒𝑟𝑧𝑎 𝑒𝑥𝑡𝑒𝑟𝑛𝑎 𝑒𝑛 𝑑𝑖𝑟𝑒𝑐𝑐𝑖𝑜𝑛 "𝑥" sobre el punto "P"
𝑓!" = 𝑓𝑢𝑒𝑟𝑧𝑎 𝑒𝑥𝑡𝑒𝑟𝑛𝑎 𝑒𝑛 𝑑𝑖𝑟𝑒𝑐𝑐𝑖𝑜𝑛 "𝑦" sobre el punto "P"
𝑓!" = 𝑓𝑢𝑒𝑟𝑧𝑎 𝑒𝑥𝑡𝑒𝑟𝑛𝑎 𝑒𝑛 𝑑𝑖𝑟𝑒𝑐𝑐𝑖𝑜𝑛 "𝑧" sobre el punto "P"
𝐼! = 𝑀𝑜𝑚𝑒𝑛𝑡𝑜 𝑑𝑒 𝐼𝑛𝑒𝑟𝑐𝑖𝑎 𝑎𝑥𝑖𝑎𝑙 𝑑𝑒𝑙 𝑟𝑜𝑡𝑜𝑟 𝑚𝑜𝑛𝑡𝑎𝑑𝑜 𝑒𝑛 𝑙𝑎 𝑏𝑎𝑟𝑟𝑎 "𝑖" Luego de resolver el sistema de ecuaciones 6x6 (multiplicadores de Lagrange y torques) presentado anteriormente se obtiene un modelo dinámico para determinar el torque en cada una de las barras accionadas y por lo tanto en los servomotores. Cabe resaltar que el robot Delta con el cual se está realizando el trabajo cuenta con un sistema de reducción, el cual debe ser considerado para hallar los torques en los servomotores.
3.4 Método Numérico de Optimización: Como se mencionó anteriormente, la función llamada “ejecutable(L_a)” está diseñana para ser utilizada en un método numérico de optimización. Sin embargo se tienen varios valores que pueden ser optimizados (torque promedio, desviación estándar del torque, torque máximo) y resulta difícil decidir cual de ellos es el más adecuado para el análisis. Por ésta razón se toma provecho de la “optimización multiobjetivo” con la cual se optimiza un vector (el cual contiene el torque promedio, desviación y máximo) y se obtienen varios resultados
16
óptimos con diferentes valores para la longitud de la barra “a”. A esto se le conoce como la frontera de Pareto. El método numérico para optimizar se toma de la biblioteca de MATLAB y se trata de un algoritmo genético multiobjetivo llamado “gamultiobj” que se encuentra en la biblioteca de la herramienta de optimización “optimtool”. Para realizar la optimización se llama a la herramienta de optimización al escribir “optimtool” en la ventana de comandos y se configuran los parámetros de la siguiente manera:
Figura #6: Configuración de Optimtool de Matlab para realizar optimización con algoritmo genético multiobjetivo (gamultiobj). Los parámetros para el algoritmo genético son:
-‐ Population type: Double Vector -‐ Population Size: 15 -‐ Selection function: Tournament -‐ Tournament size: 2 -‐ Crossover fraction: 0.8 -‐ Mutation function: Gaussian -‐ Crossover function: Intermediate (ratio=1.0) -‐ Migration: Forward (fraction=0.2; interval=20) -‐ Generations: 200
3.5 Procedimiento: Con el fin de generar curvas de torque teórico (a partir de la dinámica de Lagrange descrita anteriormente) para una trayectoria específica se realiza el siguiente procedimiento:
17
1.) -‐Se define una trayectoria para la plataforma móvil. En caso de tratarse de una serie de puntos de paso se debe utilizar la función de MATLAB llamada “function [puntos]=puntosIntermedios(cadaCuanto)” que se encuentra en el anexo “Función para generar puntos por trayectoria lineal”. Ésta función carga un archivo de texto (.txt) con las coordenadas en centímetros para los puntos de paso y a partir de éste genera una matriz con las coordenadas de una serie de puntos intermedios definidos por el parámetro de entrada “cadaCuanto”, el cual se refiere a la distancia que debe haber entre puntos. Éste código se obtuvo de [Romero, 2012] y se le realizaron pocas modificaciones. -‐Por otra parte si se requiere una trayectoria definida por una función matemática se debe realizar un algoritmo para discretizar la trayectoria a una serie de puntos y otorgue como salida una matriz de las coordenadas en diferentes puntos (matriz 3xn pues requiere coordenadas de x,y,z para los n puntos). Como ejemplo se presenta el código desarrollado para obtener valores de posición, velocidad y aceleración para una trayectoria circular llamado “function [posicion velocidad aceleración vector_t]=cinematicaCircular(R,z,numeroPuntos,numeroVueltas,TporVuelta)” el cual se encuentra en el anexo “función para discretizar la cinemática de un círculo”. Cabe resaltar que las matrices de velocidad y aceleración son necesarias para realizar la dinámica de Lagrange.
2.) Luego de contar con la matriz de coordenadas para cada punto se debe realizar la cinemática inversa con el fin de hallar los ángulos de entrada para cada una de los brazos. La función que realiza ésta tarea es “function [angulos]=angulosEn(p,l_a)” el cual se encuentra en los anexos en la sección “cinemática inversa”. Ya que éste código recibe como parámetro el vector de posición, éste se debe correr en un ciclo para realizar la cinemática inversa de cada punto de la matriz de posición. Éste código se obtuvo de [Romero, 2012] y se le realizaron pocas modificaciones.
3.) -‐Con la información obtenida de los numerales anteriores se puede proceder a calcular los valores de torque para cada servomotor en cada punto de la trayectoria. En caso de proceder por medio del ANÁLISIS JACOBIANO se utiliza la función “function [Torque]=torqueEn(p,vectorFuerza,l_a)” que se encuentra en el anexo “torque a partir del análisis Jacobiano”. Como se puede ver en los parámetros, éste también recibe el vector posición “P” por lo cual se debe llamar de manera iterativa con el fin de obtener el torque en cada posición. La salida de la función (Torque) es un vector que cuenta con el valor de cada uno de los 3 servomotores. Éste código se obtuvo de [Romero, 2012] y se le realizaron pocas modificaciones. -‐ Por otra parte si se requiere aplicar el ANÁLISIS DINÁMICO DE LAGRANGE se debe llamar a la función “function [Tau,T_promedio,NormDesvEst,T_max]=torqueDinamicoEn(l_a,posicion,aceleracion,angulos,alpha)”. A diferencia de la función “torqueEn(p)”, éste otorga como resultado la matriz de torque, el torque promedio y el torque máximo absoluto. Es decir, para éste no se requieren llamadas iterativas.
4.) Con el fin de utilizar métodos computacionales para hallar los valores de mínimo torque se desarrolló la función “function [vectorOpt]=ejecutable(l_a)”, la cual recibe como parámetro la longitud de
18
la barra “a” y entrega un vector con los valores de: torque promedio, desviación estándar del torque y torque máximo absoluto durante la trayectoria definida (en éste caso se define una trayectoria circular). La función se encuentra en el anexo “ejecutable para optimización”.
5.) A partir del código mencionado en el numeral anterior (“ejecutable(L_a)”) se utilizan métodos numérico de optimización para hallar la longitud de barra que otorga mejores resultados (ver sección “Método numérico de optimización”).
3.6 Resultados Teóricos: A continuación se presenta una curva típica del comportamiento del torque en los tres servomotores para la trayectoria característica:
Gráfica # 1: Comportamiento del torque durante la trayectoria característica para una longitud de barra “a” de 0.246 m. Para realizar la optimización del torque en función de la longitud de la barra “a” se debe establecer tanto un límite inferior como superior para la las longitudes de barra que se van a iterar. Para comenzar se optó por un panorama general que permitiera ver la tendencia del torque en un amplio rango de “a”:
0.05𝑚 < 𝑙𝑜𝑛𝑔𝑖𝑡𝑢𝑑 𝑏𝑎𝑟𝑟𝑎 "a" < 0.7𝑚 Al correr el algoritmo genético multiobjetivo con las limitantes mencionadas anteriormente (ver figura #6: lower and upper bounds) se obtiene la siguiente frontera de Pareto:
0
0.2
0.4
0.6
0.8
1
1.2
1.4
1.6
1.8
0 10 20 30 40 50 60 70 80
Torque [N*m
]
Tiempo [s]
Teo. Y
Teo. Z
Teo. W
Sol$Optima$# T_promedio$[N*m] Desv.$Est$[N*m] Torque$Max.$[N*m] Longitud$A$[m]1 814.58 1394.21 )60.72 0.422 0.00 0.97 1.88 0.593 0.18 0.15 0.40 0.054 681.43 1038.85 )60.22 0.425 0.00 0.97 1.88 0.596 814.58 1394.21 )60.72 0.42
19
Figura #7: Frontera de Pareto para primera aproximación de optimización. Luego de obtener los resultados anteriores, se realizó una gráfica del comportamiento del torque promedio en función de la longitud de la barra “a”. Esto con el fin de aclarar el sentido de los resultados y verificar si existían otros resultados válidos que el algoritmo no haya encontrado.
gráfica #2: Comportamiento del torque promedio en el rango de 5cm a 60cm para la longitud de la barra “a”. En la gráfica anterior se puede apreciar que el torque promedio cuenta con una asíntota vertical cerca de 42cm para la barra “a”. Esto se debe a que con dicha longitud de barra, el robot se invierte geométricamente, por lo cual requiere un torque infinito para lograr desplazar la plataforma. Para longitudes de barra superiores a ésta el robot se encuentra en inversión geométrica, lo cual no resulta práctico pues no está diseñado para operar de ésta manera. Por dicho motivo se ignoran los resultados de la optimización mencionada anteriormente y se procede a realizar una optimización con valores que no generen inversión geométrica y que sean factibles de instalar en el robot. El nuevo rango de optimización se determina a partir de los resultados presentados en la gráfica anterior y por las limitantes físicas con las que cuenta el robot para la instalación de las barras:
0.09𝑚 < 𝑙𝑜𝑛𝑔𝑖𝑡𝑢𝑑 𝑏𝑎𝑟𝑟𝑎 a < 0.3𝑚 Con dicho rango permisible para la longitud de la barra “a” se obtienen los siguientes resultados de la frontera de Pareto:
0.000#
1.000#
2.000#
3.000#
4.000#
5.000#
6.000#
7.000#
0# 0.1# 0.2# 0.3# 0.4# 0.5# 0.6# 0.7#
Torque
'Promed
io'[N
*m]'
Longitud'barra'¨A¨'[m]'
20
Figura #8: Frontera de Pareto para segunda aproximación de optimización. De los resultados anteriores se puede ver que 4 de los 6 resultados de la frontera de Pareto se encuentran para una longitud de barra de 12cm y los dos restantes están en el límite inferior definido como 9cm. Por ésta razón se considera el resultado de 12cm como el valor óptimo para la longitud de “a”.
Sol.%Óptima%# T_promedio%[N*m] Desv.Est%[N*m] Torque%max%[N*m] Longitud%A%[m]1 0.330 0.335 0.603 0.0902 0.247 0.500 0.598 0.1203 0.242 0.509 0.603 0.1214 0.242 0.509 0.603 0.1215 0.311 0.436 0.739 0.1186 0.330 0.335 0.603 0.090
21
4. EXPERIMENTACIÓN:
4.1 Programación de trayectorias: De MATLAB a YTERM: Para todas las trayectorias, la posición HOME se debe definir como la posición angular inicial. Al trabajar con el método de interpolación lineal (comando “LI”), YTERM recibe como parámetros los pulsos incrementales para cada punto de posición. Por otra parte los comandos de YTERM se reciben en “pulsos incrementales” por lo cual se requiere establecer la relación entre posición angular en radianes y los pulsos:
8192 𝑝𝑢𝑙𝑠𝑜𝑠2𝜋 𝑟𝑎𝑑𝑖𝑎𝑛𝑒𝑠
Además el robot cuenta con un sistema de reducción, el cual debe ser considerado cuando se establece la posición del servomotor para lograr cierta posición angular de la barra. El valor de la reducción es:
10.85: 1 Por último para convertir los pulsos en pulsos incrementales se debe tomar la diferencia entre dos líneas de pulsos:
𝑃𝑢𝑙𝑠𝑜𝑠 𝐼𝑛𝑐𝑟𝑒𝑚𝑒𝑛𝑡𝑎𝑙𝑒𝑠! = 𝑃𝑢𝑙𝑠𝑜𝑠! − 𝑃𝑢𝑙𝑠𝑜𝑠!!! Por las tres razones mencionadas anteriormente se requiere un algoritmo que transforme la matriz de ángulos (obtenida de angulosEn(p)) en una matriz de pulsos incrementales. Para esto se desarrolló el código “function [pulsos angulos]=pulsosCirculoCon(longitudA)” el cual se encuentra en el anexo “de angulos a pulsos”. Éste incluye la implementación de la posición HOME y la transformación de los valores de ángulos a pulsos incrementales, los cuales lee YTERM para realizar las trayectorias. Al obtener la matriz de pulsos incrementales que otorga el algoritmo “pulsosCirculoCon(longitudA)” ésta se debe copiar a microsoft Excel ® y realizar el siguiente procedimiento para posibilitar la lectura de YTERM:
1. Pegar los valores de pulsos incrementales en las columnas B, C, D de Excel ® y en las filas 1 hasta n.
2. En la columna A de Excel ® (y para todas las filas) se debe escribir el siguiente texto que se encuentra entre comillas “LI 0” (LI espacio cero).
3. Guardar el archivo en formato “.csv” (comma separated values). Esto permite que el valor de cada columna sea separado por una comma, lo cual coincide con el formato de lectura de YTERM para las trayectorias. De ésta manera, cada fila debe contar con los pulsos incrementales para cada servomotor (X,Y,Z,W). Por ejemplo: LI 0,245.3,322.8,415.2
22
*Modo de interpolación lineal. X se mueve 0 pulsos incrementales. “Y” 245.3 pulsos incrementales. “Z” 322.8 pulsos incrementales. Por último “W” 415.2 pulsos incrementales.
4. El archivo “.csv” creado en Excel se puede abrir como un archivo de texto “.txt” para copiar y pegar en la rutina de YTERM.
Los pulsos incrementales para el servomotor “X” siempre serán cero, pues éste servomotor no se encuentra conectado. A continuación se presenta un ejemplo con los comandos que debe tener una trayectoria de interpolación lineal en YTERM (solo se presentan las primeras líneas de la trayectoria característica mencionada para una longitud de barra de 14cm): #TRAY MO MT 1,1,1,1 CE 0,0,0,0 SH LM YZW VS 2000 VA 100000 VD 100000 LI 0,-‐886.7236754,7485.064191,6814.122207 LI 0,105.4688838,-‐466.9600619,340.9422559 LI 0,174.4310224,-‐504.494607,297.9236343 LI 0,241.3365236,-‐537.7492081,255.5811867 LI 0,305.306651,-‐565.1146115,214.2809136 … LE BGS EN *En el anexo “Código YTERM para trayectoria característica con a=0.14m” se encuentra el código completo para la trayectoria característica con una longitud de barra de 0.14m y partiendo de la posición HOME mencionada anteriormente.
4.2 Mediciones de Torque: El torque del servomotor se puede medir con facilidad porque existe una proporcionalidad con el voltaje que éste recibe. Según el manual del controlador SMC4040 y el amplificador Legend 01 la relación entre el torque y el voltaje es la siguiente:
23
0.1 𝑉 ∗ 𝑃𝑁400𝑇𝑜𝑟𝑞𝑢𝑒 𝑁𝑜𝑚𝑖𝑛𝑎𝑙 𝑆𝑒𝑟𝑣𝑜𝑚𝑜𝑡𝑜𝑟
El torque nominal del servomotor Yaskawa SGMAH que se encuentra montado en el robot Delta es 0.318 N*m. Por otra parte PN400 se refiere a un parámetro de configuración del amplificador que puede contar con valores entre 30 y 100. La configuración de fábrica tiene un valor de PN400=30. Por ésta razón la proporción entre el voltaje y el torque es:
3 𝑉0.318 𝑁𝑚 = 9.4339
𝑉𝑁 ∗𝑚
Por otra parte, el torque que se debe conocer es el que actúa sobre la barra “a” y no sobre el motor. Considerando que existe un sistema de reducción de 10.85:1 entre el motor y la barra “a”, el torque en la barra es 10.85 veces superior al del motor:
𝜏! = 10.85 ∗ 𝜏!"#"$ Considerando los dos factores mencionados anteriormente, la proporción entre el voltaje y el torque de la barra “a” es:
10.85 𝑁 ∗𝑚9.4339 𝑉 = 1.1501
𝑁 ∗𝑚𝑉 (𝑝𝑎𝑟𝑎 𝑝𝑎𝑠𝑎𝑟 𝑑𝑒 𝑣𝑜𝑙𝑡𝑖𝑜𝑠 𝑎 𝑁 ∗𝑚)
El programa YTERM cuenta con la opción de poner el factor de conversión antes de tomar mediciones con el fin de obtener una gráfica con las unidades que uno requiera. Para esto seguir las siguientes instrucciones:
1.) Ir a la pantalla gráfica de YTERM usando el comando “Next”. 2.) Presionar el botón “Scope” dentro de la interfaz gráfica. 3.) Poner el factor de conversión en forma de división en la casilla
“Engineering Units”. Por ejemplo, para pasar de voltios a N*m en la barra “a” el factor de conversión debe ser:
11.1501 = 0.86948
El procedimiento para tomar mediciones de torque en una trayectoria determinada es el siguiente:
1.) Copiar en YTERM tanto la rutina #HOME como la trayectoria programada (Ej. #TRAY).
2.) Correr la rutina #HOME desde la ventana de comandos con el siguiente comando que se encuentra entre comillas: “XQ#HOME,0”.
3.) Luego que el robot se encuentra en la posición HOME, enviar el código de la trayectoria (Ej. #TRAY) al controlador.
4.) Ir a la interfaz gráfica por medio del comando “Next”. 5.) Establecer el tiempo de muestreo, la variable a medir (en este caso:
Torque) y verificar la conversión de unidades en la casilla de “Engineering Units”.
24
6.) Presionar el comando “Start” para comenzar la trayectoria programada y la recolección de datos.
7.) Luego de terminada la trayectoria y cargados los datos, YTERM presentará la gráfica con los resultados de torque de cada uno de los 3 servomotores.
8.) En caso de requerir los datos obtenidos hacer click derecho en la gráfica y seleccionar la opción “Export Data”. La interfaz informará al usuario sobre la ubicación del archivo en el cual han sido guardados los datos (Para el vector de tiempo, tomar en consideración el tiempo de muestreo establecido).
4.3 Resultados Experimentales: Con el procedimiento descrito anteriormente se obtienen mediciones de torque en la trayectoria característica para diferentes longitudes de barras. A continuación se presenta como ejemplo la gráfica que otorga YTERM luego de realizar mediciones con la barra de 14cm:
Gráfica #3: Resultados experimentales para trayectoria característica con longitud de barra de 14cm. En la gráfica 3 se puede apreciar que al inicio de las mediciones hay una sección que no corresponde a la trayectoria, pues se trata de las mediciones de torque desde la posición HOME hasta el primer punto definido de la trayectoria. Éstos datos no se consideran en la comparación de resultados, pues en el estudio teórico no se define el inicio desde la posición HOME. Al exportar los datos a Excel ® se obtienen los 3 criterios de torque para la selección de las barras: torque promedio, desviación estándar del torque y torque máximo. Por otra parte, resulta coherente determinar éstos valores tanto para una condición global como para cada servomotor específicamente, pues los resultados globales son válidos para ser comparados con los resultados teóricos de la optimización y los específicos de cada servomotor pueden otorgar información sobre problemas de montaje o funcionamiento de cada uno de éstos.
25
Para determinar los indicadores totales se toma:
𝜏!"#$%&'# =𝜏!!
!!! ! + 𝜏!!!!! ! + 𝜏!!
!!! !
3 ∗ 𝑁
𝜎!"#$%& = 𝜎!! + 𝜎!! + 𝜎!!
𝜏!"# .= max (max 𝜏!! ,max 𝜏!! ,max 𝜏!!)
A continuación se presentan los resultados totales para diferentes longitudes de barra y su comparación con los valores teóricos:
gráfica #4: Resultados de torque promedio total. Teóricos y experimentales. De los resultados anteriores se puede ver que los resultados experimentales siguen la misma tendencia de los teóricos aunque cuentan con un leve desfase, el cual se puede deber a las condiciones de fricción que no son tomadas en consideración en el modelo de Lagrange. De la gráfica anterior es importante resaltar que no resultó posible realizar pruebas para longitudes de barra inferiores a 14cm pues al hacerlo el robot Delta interceptaba las monturas de los sensores de proximidad utilizados para implementar la posición HOME.
0.000
0.200
0.400
0.600
0.800
1.000
1.200
1.400
1.600
0 0.05 0.1 0.15 0.2 0.25 0.3
Torque Promedio [N*m
]
Longitud barra "a" [m]
Promedio-‐Totales
Teoricos
Experimentales
26
gráfica #5: Desviación estándar total para diferentes longitudes de la barra “a”. Teóricas y experimentales.
gráfica #6: Valores máximos totales para diferentes longitudes de la barra “a”. Teóricos y experimentales. De la gráfica 6 se puede ver que la diferencia entre los valores teóricos y los experimentales es significativamente mayor que para los valores promedio. Esto se puede deber a que los valores experimentales cuentan con una mayor desviación estándar, tal como se puede apreciar en la gráfica 5, por lo cual el ruido en los puntos pico genera valores máximos de mayor magnitud. A continuación se presentan los resultados, tanto teóricos como experimentales, para cada uno de los servomotores (Y, Z, W).
0.000
0.100
0.200
0.300
0.400
0.500
0.600
0.700
0.800
0 0.05 0.1 0.15 0.2 0.25 0.3
Desviación del torque [N*m
]
Longitud barra "a" [m]
Desviación-‐Totales
Teoricos
Experimentales
0.000
0.500
1.000
1.500
2.000
2.500
3.000
0 0.05 0.1 0.15 0.2 0.25 0.3
Torque Máximo [N*m
]
Longitud barra "a" [m]
Máximos-‐Totales
Teoricos
Experimentales
27
Resultados para el servomotor “Y”:
gráfica #7: Resultados de torque promedio para el servomotor “Y”.
figura #9: Resultados para el servomotor “Y”. Tanto de la gráfica 7 como de la figura 8 se puede ver que los resultados teóricos de torque promedio son de mayor magnitud a los resultados experimentales para las longitudes de barra de 0.216m y 0.276m. Dichas inconsistencias se deben a fallas en los sistemas de reducción, pues las correas se deslizan sobre los engranajes y hacen que el robot se desvíe de la trayectoria.
0.000
0.200
0.400
0.600
0.800
1.000
1.200
1.400
0 0.05 0.1 0.15 0.2 0.25 0.3
Torque Promedio [N*m
]
Longitud barra "A" [m]
Teoricos
Experimentales
Longitud)de)"a")[m] Teorico Experimental Teorica Experimental Teórico) Experimental
0.106 0.286 N.A 0.126 N.A 0.483 N.A0.126 0.257 N.A 0.307 N.A 0.647 N.A0.146 0.367 0.432 0.340 0.444 0.803 1.2750.176 0.533 0.672 0.377 0.498 1.020 1.5230.216 0.769 0.775 0.429 0.594 1.325 1.7890.246 0.976 1.132 0.476 0.661 1.594 2.2190.276 1.235 1.165 0.539 0.720 1.933 2.422
Torque)Promedio)[N*m] Desviación)Estandar)[N*m] Máximo)Absoluto)[N*m]
28
Resultados para el servomotor Z:
gráfica #8: Resultados de torque promedio para el servomotor “Z”.
figura #10: Resultados para el servomotor “Z”. De la gráfica 8 y la figura 10 se puede apreciar que los resultados experimentales para el servomotor Z son bastante consistentes con los teóricos, lo cual muestra que el sistema de reducción de éste servomotor no cuenta con problemas con sus correas.
0.000
0.200
0.400
0.600
0.800
1.000
1.200
1.400
1.600
0 0.05 0.1 0.15 0.2 0.25 0.3
Torque Promedio [N*m
]
Longitud barra "A" [m]
Teoricos
Experimentales
Longitud)de)"a")[m] Teorico Experimental Teorica Experimental Teórico) Experimental
0.106 0.347 N.A 0.237 N.A 0.677 N.A0.126 0.263 N.A 0.308 N.A 0.647 N.A0.146 0.373 0.485 0.341 0.418 0.803 1.2090.176 0.539 0.632 0.379 0.469 1.020 1.4170.216 0.776 0.854 0.431 0.557 1.325 1.7600.246 0.984 1.136 0.478 0.612 1.594 2.2120.276 1.244 1.392 0.542 0.717 1.933 2.601
Torque)Promedio)[N*m] Desviación)Estandar)[N*m] Máximo)Absoluto)[N*m]
29
Resultados para el servomotor W:
gráfica #9: Resultados de torque promedio para el servomotor “W”.
figura #11: Resultados para el servomotor “W”. Cabe resaltar que aunque existen inconsistencia entre los resultados experimentales y los teóricos (a partir del modelo de Lagrange), el modelo logra predecir la tendencia del comportamiento del torque a partir de cambios en la longitud de las barras de manera satisfactoria. Aunque no fue posible realizar una validación para la predicción del valor óptimo teórico (a=12cm) por limitaciones de espacio, se puede apreciar que los resultados experimentales siguen la tendencia de la teoría y por lo tanto se espera que el valor óptimo experimental se encuentre muy cercano al teórico.
0.000
0.200
0.400
0.600
0.800
1.000
1.200
1.400
1.600
1.800
0 0.05 0.1 0.15 0.2 0.25 0.3
Torque Promedio [N*m
]
Longitud barra "A" [m]
Teoricos
Experimentales
Longitud)de)"a")[m] Teorico Experimental Teorica Experimental Teórico) Experimental
0.106 0.356 N.A 0.251 N.A 0.677 N.A0.126 0.262 N.A 0.309 N.A 0.647 N.A0.146 0.372 0.493 0.342 0.435 0.803 1.2310.176 0.538 0.756 0.379 0.472 1.020 1.6460.216 0.775 0.946 0.431 0.517 1.325 1.7400.246 0.983 1.227 0.479 0.545 1.594 2.1590.276 1.243 1.582 0.542 0.597 1.933 2.507
Máximo)Absoluto)[N*m]Torque)Promedio)[N*m] Desviación)Estandar)[N*m]
30
5. CONCLUSIONES: A partir del modelo de Lagrange y de la validación experimental se logró hallar el valor óptimo para la longitud de la barra “a”, el cual es de 12cm para la trayectoria característica planteada. La mayor limitación del procedimiento realizado es que la optimización es válida sólo para cierta trayectoria. Sin embargo, el estudio realizado resulta bastante práctico cuando los movimientos del robot Delta cuentan con una trayectoria “típica”. Es decir, cuando el robot tenga que realizar una trayectoria muy similar de manera iterativa. Las diferencias entre los resultados teóricos y experimentales se atribuyen a las fuerzas de fricción en las uniones rotacionales y a los errores en las atribuciones de masa e inercia a las partes móviles del robot Delta. Por dicha razón se recomienda mantener lubricadas las uniones rotacionales y esféricas del robot Delta. Se recomienda realizar el estudio de optimización de la ventaja mecánica en función del ángulo de entrada (𝜃!!) pues al modificar la longitud de la barra “a” los ángulos de entrada deben cambiar para lograr la misma posición de salida.
31
6. BIBLIOGRAFÍA:
• [1] Tsai, L-‐W. (1999). Robot Analysis: The mechanics of serial and parallel manipulators. New York, NY, Estados Unidos: Jhon Wiley & sons, INC.
• [2] Villaveces, A (2010). Construcción de un robot tipo Delta. Bogotá, Colombia: Colección de Tesis Universidad de los Andes.
• [3] Romero, A (2011). Puesta a punto y desarrollo de un sistema de programación para un robot tipo Delta. Bogotá, Colombia: Colección de Tesis Universidad de los Andes.
• [4] Grass, S (2012). Formulación de trayectorias para definir los límites dinámicos de un robot tipo Delta. Bogotá, Colombia: Colección de Tesis Universidad de los Andes.
• [5] Laribi, M.A. Et al (2005). Analysis and dimensional synthesis of the
DELTA robot for a prescribed workspace. Mechanism and Machine Theory. Scoence Direct.
32
7. ANEXOS:
Función para generar puntos por trayectoria lineal: function [puntos]=puntosIntermedios(cadaCuanto) % este codigo se encarga de realizar una serie de puntos intermedios entre % los puntos de paso. esto para calcular el torque y los angulos de % entrada para cada uno de estos. %carga la serie de puntos de paso desde excel. P=load('puntos.txt'); %Pasa de cm a metros. P=P/100; %los valores de z son negativos. P(:,3)=-‐P(:,3); %se inicia el contador n. n=1; fori=1:length(P(:,1))-‐1 Po=P(i,:); vec=P(i+1,:)-‐P(i,:); unitario=vec/norm(vec); distancia=norm(vec); puntos(n,:)=Po; numeropuntos=ceil(distancia/cadaCuanto);
33
magnitud=distancia/numeropuntos; cont=1; for j=n+1:n+numeropuntos puntos(j,:)=Po+cont*magnitud*unitario; n=n+1; cont=cont+1; end end
Función para discretizar cinemática de un círculo: function [posicion velocidad aceleracionvector_t]=cinematicaCircular(R,z,numeroPuntos,numeroVueltas,TporVuelta) omega=(2*pi)/TporVuelta; delta_t=TporVuelta/(numeroPuntos+1); posicion=zeros((numeroPuntos+1)*numeroVueltas,3); velocidad=zeros((numeroPuntos+1)*numeroVueltas,3); aceleracion=zeros((numeroPuntos+1)*numeroVueltas,3); vector_t=zeros((numeroPuntos+1)*numeroVueltas,1); for i=1:(numeroPuntos+1)*numeroVueltas t=i*delta_t; %representa la posicion en x. posicion(i,1)=R*cos(omega*t); %representa la posicion en y.
34
posicion(i,2)=R*sin(omega*t); %representa la posicion en z. posicion(i,3)=z; velocidad(i,1)=-‐omega*R*sin(omega*t); velocidad(i,2)=omega*R*cos(omega*t); aceleracion(i,1)=-‐omega^2*R*cos(omega*t); aceleracion(i,2)=-‐omega^2*R*sin(omega*t); vector_t(i,1)=t; end end
Cinemática Inversa: function [angulos]=angulosEn(p,l_a) %Se establecen las dimensiones del robot delta. %La longitud "a" se toma como entrada con el fin de posibilitar la %optimizaciÛn. a=l_a; b=0.262; h=0.048908; r=0.10065; phi1=30; phi=[phi1;phi1+120;phi1+240];
35
%la matriz "angulos" tiene como filas theta_1,theta_2 y theta_3 y columnas %servomotor_1, servomotor_2 y servomotor_3. de tal forma que es [theta_11 %theta_12 theta_13;theta_21 theta_22 theta_23; theta_31 theta_32 theta_33] angulos=zeros(3,3); %la matriz C tiene la informacion de la posicion del punto C en x,y,z para %los 3 servomotores. C=zeros(3,3); fori=1:3 A=[cosd(phi(i)) sind(phi(i)) 0;-‐sind(phi(i)) cosd(phi(i)) 0;0 0 1]; B=[(h-‐r);0;0]; C(:,i)=A*p+B; end theta=zeros(3,3); fori=1:3 if (abs(C(2,i))>b) disp('No existen soluciones reales'); return
36
elseif (abs(C(2,i))<=b) theta(3,i)=acos((C(2,i))/b); k=((C(1,i))^2+(C(2,i))^2+(C(3,i))^2-‐a^2-‐b^2)/(2*a*b*sin(theta(3,i))); if (k>1) disp('En valor de k excede el lÌmite'); return else theta(2,i)=acos(k); end %la solucion de theta_1 en cada uno de los servomotores se debe %realizar de manera iterativa. para Èsto se utiliza la funcion %interna de MATLAB "fsolve" la cual recibe como parametros la %funcion y el valor inicial de adivinanza (en este caso=pi/4). options=optimset('display','off'); theta(1,i)=fsolve(@(x)a*cos(x)+b*sin(theta(3,i))*cos(x+theta(2,i))-‐C(1,i),pi/4,options); end angulos(:,i)=theta(:,i); end end
Torque a partir del análisis Jacobiano: function [Torque]=torqueEn(p,vectorFuerza,l_a) %para la matriz jacobiana del robot delta consultar libro de Tsai. %devuelve la matriz de angulos.
37
[theta]=angulosEn(p); a=l_a; Jx=zeros(3,3); %el angulo entre los brazos es de 120 grados. phi=[30;150;270]; fori=1:3 jix=cos(theta(1,i)+theta(2,i))*sin(theta(3,i))*cosd(phi(i))-‐cos(theta(3,i))*sind(phi(i)); jiy=cos(theta(1,i)+theta(2,i))*sin(theta(3,i))*sind(phi(i))+cos(theta(3,i))*cosd(phi(i)); jiz=sin(theta(1,i)+theta(2,i))*sin(theta(3,i)); Jx(i,1)=jix; Jx(i,2)=jiy; Jx(i,3)=jiz; end Jq=zeros(3,3); Jq(1,1)=a*sin(theta(2,1))*sin(theta(3,1)); Jq(2,2)=a*sin(theta(2,2))*sin(theta(3,2)); Jq(3,3)=a*sin(theta(2,3))*sin(theta(3,3)); %Jx\Jq es mas eficiente que inv(Jx)*Jq. J=Jx\Jq;
38
%Ver libro de Tsai pagina 280. Torque=(J')*vectorFuerza; end
Torque a partir del análisis dinámico (planteamiento de Lagrange): function [Tau,T_promedio,NormDesvEst,T_max]=torqueDinamicoEn(l_a,posicion,aceleracion,angulos,alpha) %Este codigo otorga matriz 3xN del torque en los 3 servomotores para los N %puntos considerados. Para mayor informacion sobre la teoria consultar TSAI %capitulo #10 pg.449-‐453 % Se definen las proporciones y caracteristicas del robot. a=l_a; %metros b=0.262; %metros h=0.048908; %metros r=0.10065; %metros phi=[30;150;270]; %grados m_p=0.3576; %masa de la plataforma en kg m_b=0.1989/2; %masa de CADA UNA de las barras dos barras de "b" en kg m_a=0.1113; %masa de la barra "a" en kg. I_m=0; %MEDIR INERCIA:momento de inercia axial del rotor montado en el brazo i. g_z=9.8; %aceleracion gravitacional en m/s^2
39
Tau=zeros(length(alpha(:,1)),3); fori=1:length(alpha(:,1)) A=zeros(3,3); B=zeros(3,1); for j=1:3 A(1,j)=2*(posicion(i,1)+h*cosd(phi(j))-‐r*cosd(phi(j))-‐a*cosd(phi(j))*cos(angulos(i,j))); A(2,j)=2*(posicion(i,2)+h*sind(phi(j))-‐r*sind(phi(j))-‐a*sind(phi(j))*cos(angulos(i,j))); A(3,j)=2*(posicion(i,3)-‐a*sin(angulos(i,j))); end B(1)=(m_p+3*m_b)*aceleracion(i,1); B(2)=(m_p+3*m_b)*aceleracion(i,2); B(3)=(m_p+3*m_b)*aceleracion(i,3)+(m_p+3*m_b)*g_z; lambda=A\B; for j=1:3 Tau(i,j)=(I_m+(1/3)*m_a*a^2+m_b*a^2)*alpha(i,j)+(0.5*m_a+m_b)*g_z*a*cos(angulos(i,j))-‐
40
2*a*lambda(j)*((posicion(i,1)*cosd(phi(j))+posicion(i,2)*sind(phi(j))+h-‐r)*sin(angulos(i,j))-‐posicion(i,3)*cos(angulos(i,j))); end end T_promedio=abs(mean2(Tau)); sigma=std(Tau)'; NormDesvEst=sqrt(sigma(1)^2+sigma(2)^2+sigma(3)^2); absT=abs(Tau); maxim=max(absT); T_max=max(maxim); end
Ejecutable para Optimización: function [vectorOpt]=ejecutable(l_a) radio_circulo=0.07; %metros altura_circulo=0.2; %metros numero_puntos=60; num_vueltas=2;
41
tiempoVuelta=5; [pos velacc t]=cinematicaCircular(radio_circulo,altura_circulo,numero_puntos,num_vueltas,tiempoVuelta); angulos=zeros(length(pos),3); longitudA=l_a; fori=1:length(pos) ang=angulosEn(pos(i,:)',longitudA); angulos(i,:)=ang(1,:); end [omega]=omegaNumericoEn(angulos,t); [alpha]=alphaNumericoEn(omega,t); [tortorquePromedioDesvEstTorquemaxTorque]=torqueDinamicoEn(longitudA,pos,acc,angulos,alpha); vectorOpt=[torquePromedio;DesvEstTorque;maxTorque]; end
42
De ángulos a pulsos: function [pulsos angulos]=pulsosCirculoCon(longitudA) radio_circulo=0.1; %metros altura_circulo=0.35; %metros numero_puntos=60; num_vueltas=2; tiempoVuelta=5; [pos velacc t]=cinematicaCircular(radio_circulo,altura_circulo,numero_puntos,num_vueltas,tiempoVuelta); angulos=zeros(length(pos)+1,3); %Se definen los angulos de la posicion %HOME=[33,33,33]*pi/180 en radianes angulos(1,:)=[0.575958,0.575958,0.575958]; fori=1:length(pos) ang=angulosEn(pos(i,:)',longitudA); angulos(i+1,:)=ang(1,:); end incrementoAngulo=zeros(length(angulos)-‐1,3); pulsos=zeros(length(incrementoAngulo),3);
43
for i=1:length(pulsos) %Se deben determinar los pulsos incrementales, pues el encoder del %servomotor los debe leer de esta forma (encoderincremetal de 13 bits). incrementoAngulo(i,:)=angulos(i+1,:)-‐angulos(i:); end %La funcion de transferencia del encoder es (4*N)/(2*pi), siendo N el %numero de puntos por rev. configurado en el parametro PN201 del %amplificador. Con las condiciones actuales N=2048, por lo tanto 4*N=8192. %10.85 es por el sistema de reduccion entre el servomotor y la barra "a". pulsos=incrementoAngulo*10.85*((8192)/(2*pi)); end
Código YTERM para trayectoria característica con a=0.14m: #TRAY MO MT 1,1,1,1 CE 0,0,0,0 SH LM YZW VS 2000 VA 100000 VD 100000 LI 0,-‐886.7236754,7485.064191,6814.122207 LI 0,105.4688838,-‐466.9600619,340.9422559 LI 0,174.4310224,-‐504.494607,297.9236343 LI 0,241.3365236,-‐537.7492081,255.5811867 LI 0,305.306651,-‐565.1146115,214.2809136
44
LI 0,365.4306801,-‐585.6588166,174.1879673 LI 0,420.7092993,-‐598.2102778,135.30112 LI 0,470.2330488,-‐601.998875,97.48938568 LI 0,513.0054201,-‐596.5812994,60.52650233 LI 0,548.1496441,-‐581.8539271,24.12159416 LI 0,574.9048647,-‐558.0328431,-‐12.05438564 LI 0,592.7021585,-‐525.6085261,-‐48.34523445 LI 0,601.227968,-‐485.2852133,-‐85.08741206 LI 0,600.4756852,-‐437.9151237,-‐122.587283 LI 0,590.7750941,-‐384.3235584,-‐161.0958207 LI 0,572.7910105,-‐325.9322973,-‐200.7794407 LI 0,547.4866014,-‐263.0306855,-‐241.6866531 LI 0,516.2116265,-‐197.003895,-‐283.7118097 LI 0,479.8722167,-‐128.6322232,-‐326.5593181 LI 0,440.1775314,-‐58.76776728,-‐369.7140109 LI 0,398.2934608,11.76842362,-‐412.4252281 LI 0,355.3364275,82.17061771,-‐453.7126729 LI 0,312.2124736,151.6299595,-‐492.4003223 LI 0,269.5931793,219.3165711,-‐527.2721495 LI 0,227.9179051,284.365444,-‐556.6649667 LI 0,187.4151913,345.8843656,-‐579.6521353 LI 0,148.1352057,402.8664612,-‐594.9684869 LI 0,109.9862043,454.4313665,-‐601.7440426 LI 0,72.77007817,499.553708,-‐599.4233594 LI 0,36.21435912,537.3327522,-‐587.7903024 LI 0,0,566.9583628,-‐566.958362 LI 0,-‐36.21435912,587.7903023,-‐537.3327522 LI 0,-‐72.77007817,599.4233595,-‐499.553708 LI 0,-‐109.9862043,601.7440414,-‐454.4313665 LI 0,-‐148.1352057,594.9684893,-‐402.8664612 LI 0,-‐187.4151913,579.6521341,-‐345.8843656 LI 0,-‐227.9179051,556.6649685,-‐284.365444 LI 0,-‐269.5931793,527.2721477,-‐219.316571 LI 0,-‐312.2124736,492.4003223,-‐151.6299595 LI 0,-‐355.3364275,453.7126729,-‐82.17061771 LI 0,-‐398.2934608,412.4252281,-‐11.76842362 LI 0,-‐440.1775314,369.7140109,58.76776728 LI 0,-‐479.8722167,326.5593181,128.6322232 LI 0,-‐516.2116265,283.7118097,197.003895 LI 0,-‐547.4866014,241.6866531,263.0306855 LI 0,-‐572.7910105,200.7794407,325.9322973 LI 0,-‐590.7750939,161.0958207,384.3235584 LI 0,-‐600.4756853,122.587283,437.9151237 LI 0,-‐601.2279681,85.08741206,485.2852134 LI 0,-‐592.7021584,48.34523445,525.6085261 LI 0,-‐574.9048647,12.05438564,558.032843 LI 0,-‐548.1496442,-‐24.12159416,581.8539271 LI 0,-‐513.0054201,-‐60.52650233,596.5812994 LI 0,-‐470.2330488,-‐97.48938568,601.9988751
45
LI 0,-‐420.7092993,-‐135.301127,598.2102774 LI 0,-‐365.4306801,-‐174.1879673,585.6588169 LI 0,-‐305.306651,-‐214.2809136,565.1146115 LI 0,-‐241.3365236,-‐255.5811867,537.7492081 LI 0,-‐174.4310224,-‐297.9236343,504.494607 LI 0,-‐105.4688838,-‐340.9422559,466.9600619 LI 0,-‐35.2904086,-‐384.0442399,426.4004425 LI 0,35.2904086,-‐426.4004425,384.0442399 LI 0,105.4688838,-‐466.9600619,340.9422559 LI 0,174.4310224,-‐504.494607,297.9236343 LI 0,241.3365236,-‐537.7492078,255.5811867 LI 0,305.306651,-‐565.1146101,214.2809136 LI 0,365.4306801,-‐585.6588196,174.1879673 LI 0,420.7092993,-‐598.2102776,135.301127 LI 0,470.2330488,-‐601.9988738,97.48938568 LI 0,513.0054201,-‐596.5812993,60.52650233 LI 0,548.1496441,-‐581.8539272,24.12159416 LI 0,574.9048647,-‐558.032843,-‐12.05438564 LI 0,592.7021586,-‐525.6085261,-‐48.34523445 LI 0,601.2279679,-‐485.2852133,-‐85.08741206 LI 0,600.4756864,-‐437.9151237,-‐122.587283 LI 0,590.7750931,-‐384.3235584,-‐161.0958207 LI 0,572.7910105,-‐325.9322973,-‐200.7794407 LI 0,547.4865978,-‐263.0306855,-‐241.6866531 LI 0,516.21163,-‐197.003895,-‐283.7118097 LI 0,479.8722167,-‐128.6322231,-‐326.559381 LI 0,440.1775314,-‐58.76776729,-‐369.7140109 LI 0,398.2934608,11.76842362,-‐412.4252281 LI 0,355.3364275,82.17061771,-‐453.7126729 LI 0,312.2124736,151.6299595,-‐492.4003223 LI 0,269.5931792,219.3165711,-‐527.2721479 LI 0,227.9179051,284.365444,-‐556.6649667 LI 0,187.4151913,345.8843656,-‐579.6521356 LI 0,148.1352057,402.8664612,-‐594.968488 LI 0,109.9862043,454.4313665,-‐601.7440427 LI 0,72.77007817,499.553708,-‐599.4233595 LI 0,36.21435912,537.3327522,-‐587.7903023 LI 0,0,566.9583627,-‐566.9583627 LI 0,-‐36.21435912,587.7903024,-‐537.3327522 LI 0,-‐72.77007817,599.4233593,-‐499.553708 LI 0,-‐109.9862043,601.7440415,-‐454.4313665 LI 0,-‐148.1352057,594.9684895,-‐402.8664612 LI 0,-‐187.4151913,579.6521339,-‐345.8843656 LI 0,-‐227.9179051,556.6649685,-‐284.365444 LI 0,-‐269.5931792,527.2721477,-‐219.3165711 LI 0,-‐312.2124736,492.4003223,-‐151.6299595 LI 0,-‐355.3364276,453.7126729,-‐82.17061771 LI 0,-‐398.2934608,412.4252281,-‐11.76842362 LI 0,-‐440.1775314,369.7140109,58.76776728
46
LI 0,-‐479.8722167,326.5593181,128.6322232 LI 0,-‐516.2116289,283.7118097,197.003895 LI 0,-‐547.4865991,241.6866531,263.0306855 LI 0,-‐572.7910088,200.7794407,325.9322978 LI 0,-‐590.7750957,161.0958207,384.3235579 LI 0,-‐600.4756852,122.587283,437.9151237 LI 0,-‐601.2279681,85.08741206,485.2852133 LI 0,-‐592.7021584,48.34523445,525.6085261 LI 0,-‐574.9048647,12.05438564,558.0328431 LI 0,-‐548.1496441,-‐24.12159416,581.8539271 LI 0,-‐513.0054201,-‐60.52650233,596.5812993 LI 0,-‐470.2330488,-‐97.48938568,601.998874 LI 0,-‐420.7092991,-‐135.301127,598.2102774 LI 0,-‐365.4306803,-‐174.1879673,585.6588195 LI 0,-‐305.306651,-‐214.2809136,565.1146119 LI 0,-‐241.3365236,-‐255.5811867,537.7492061 LI 0,-‐174.4310224,-‐297.9236343,504.494607 LI 0,-‐105.4688838,-‐340.9422559,466.9600619 LI 0,-‐35.2904086,-‐384.0442399,426.4004425 LI 0,35.29040859,-‐426.4004425,384.0442399 LI 0,105.4688839,-‐466.9600619,340.9422559 LI 0,174.4310224,-‐504.494607,297.9236343 LI 0,241.3365236,-‐537.7492076,255.5811867 LI 0,305.306651,-‐565.1146103,214.2809136 LI 0,365.4306803,-‐585.6588185,174.1879673 LI 0,420.7092991,-‐598.2102775,135.30112 LI 0,470.2330488,-‐601.9988749,97.48938568 LI 0,513.0054202,-‐596.5812995,60.52650233 LI 0,548.1496441,-‐581.8539271,24.12159416 LI 0,574.9048647,-‐558.032843,-‐12.05438563 LI 0,592.7021583,-‐525.6085261,-‐48.34523445 LI 0,601.2279682,-‐485.2852133,-‐85.08741206 LI 0,600.475684,-‐437.9151237,-‐122.587283 LI 0,590.7750954,-‐384.3235584,-‐161.0958207 LI 0,572.7910103,-‐325.9322973,-‐200.7794407 LI 0,547.4866,-‐263.0306855,-‐241.6866531 LI 0,516.2116281,-‐197.003895,-‐283.7118097 LI 0,479.8722167,-‐128.6322231,-‐326.5593181 LI 0,440.1775314,-‐58.76776729,-‐369.7140109 LI 0,398.2934608,11.76842362,-‐412.4252281 LI 0,355.3364275,82.17061771,-‐453.7126729 LI 0,312.2124736,151.6299595,-‐492.4003223 LI 0,269.5931793,219.3165711,-‐527.2721477 LI 0,227.9179051,284.365444,-‐556.6649671 LI 0,187.4151913,345.8843656,-‐579.6521367 LI 0,148.1352057,402.8664612,-‐594.9684877 LI 0,109.9862043,454.4313665,-‐601.7440417 LI 0,72.77007817,499.553708,-‐599.4233596 LI 0,36.21435912,537.3327522,-‐587.7903023
47
LI 0,0,566.9583627,-‐566.9583627 LI 0,-‐36.21435912,587.7903024,-‐537.3327522 LI 0,-‐72.77007817,599.4233594,-‐499.553708 LI 0,-‐109.9862043,601.7440419,-‐454.4313665 LI 0,-‐148.1352057,594.9684875,-‐402.8664609 LI 0,-‐187.4151913,579.6521368,-‐345.884366 LI 0,-‐227.9179051,556.6649664,-‐284.365444 LI 0,-‐269.5931793,527.2721484,-‐219.3165711 LI 0,-‐312.2124736,492.4003223,-‐151.6299595 LI 0,-‐355.3364275,453.7126729,-‐82.17061771 LI 0,-‐398.2934608,412.4252281,-‐11.76842362 LI 0,-‐440.1775314,369.7140109,58.76776728 LI 0,-‐479.8722167,326.5593181,128.6322232 LI 0,-‐516.2116286,283.7118097,197.003895 LI 0,-‐547.4865993,241.6866531,263.0306855 LI 0,-‐572.7910104,200.7794407,325.9322973 LI 0,-‐590.775094,161.0958207,384.3235584 LI 0,-‐600.4756853,122.587283,437.9151237 LI 0,-‐601.2279683,85.08741206,485.2852133 LI 0,-‐592.7021582,48.34523445,525.6085262 LI 0,-‐574.9048648,12.05438563,558.0328431 LI 0,-‐548.1496441,-‐24.12159416,581.8539271 LI 0,-‐513.0054201,-‐60.52650233,596.5812993 LI 0,-‐470.2330488,-‐97.48938569,601.998874 LI 0,-‐420.7092993,-‐135.301127,598.2102787 LI 0,-‐365.4306801,-‐174.1879673,585.658818 LI 0,-‐305.306651,-‐214.2809136,565.1146103 LI 0,-‐241.3365236,-‐255.5811867,537.7492078 LI 0,-‐174.4310224,-‐297.9236343,504.494607 LI 0,-‐105.4688838,-‐340.9422559,466.9600619 LI 0,-‐35.2904086,-‐384.0442399,426.4004425 LI 0,35.2904086,-‐426.4004425,384.0442399 LI 0,105.4688838,-‐466.9600619,340.9422559 LI 0,174.4310224,-‐504.494607,297.9236343 LI 0,241.3365236,-‐537.7492081,255.5811867 LI 0,305.306651,-‐565.11461,214.2809136 LI 0,365.4306803,-‐585.6588196,174.1879673 LI 0,420.7092991,-‐598.2102772,135.301127 LI 0,470.2330488,-‐601.998874,97.48938568 LI 0,513.0054201,-‐596.5812994,60.52650233 LI 0,548.1496441,-‐581.8539271,24.12159416 LI 0,574.9048647,-‐558.0328431,-‐12.05438563 LI 0,592.7021587,-‐525.6085261,-‐48.34523445 LI 0,601.2279681,-‐485.2852133,-‐85.08741206 LI 0,600.4756861,-‐437.9151237,-‐122.587283 LI 0,590.775093,-‐384.3235584,-‐161.0958207 LI 0,572.7910103,-‐325.9322973,-‐200.7794407 LI 0,547.4866003,-‐263.0306855,-‐241.6866531 LI 0,516.2116279,-‐197.003895,-‐283.7118097
48
LI 0,479.8722167,-‐128.6322231,-‐326.5593181 LI 0,440.1775314,-‐58.76776729,-‐369.7140109 LI 0,398.2934608,11.76842362,-‐412.4252281 LI 0,355.3364275,82.17061771,-‐453.7126729 LI 0,312.2124736,151.6299595,-‐492.4003223 LI 0,269.5931793,219.3165711,-‐527.2721477 LI 0,227.9179051,284.365444,-‐556.6649668 LI 0,187.4151913,345.8843656,-‐579.6521369 LI 0,148.1352057,402.8664612,-‐594.9684869 LI 0,109.9862043,454.4313665,-‐601.7440427 LI 0,72.77007816,499.553708,-‐599.4233594 LI 0,36.21435912,537.3327522,-‐587.7903025 LI 0,0,566.9583627,-‐566.9583626 LI 0,-‐36.21435912,587.7903023,-‐537.3327522 LI 0,-‐72.77007818,599.4233595,-‐499.553708 LI 0,-‐109.9862043,601.7440417,-‐454.4313665 LI 0,-‐148.1352057,594.968489,-‐402.8664612 LI 0,-‐187.4151913,579.6521354,-‐345.8843656 LI 0,-‐227.9179051,556.6649664,-‐284.365444 LI 0,-‐269.5931793,527.2721484,-‐219.3165711 LI 0,-‐312.2124736,492.4003223,-‐151.6299595 LI 0,-‐355.3364275,453.7126729,-‐82.17061771 LI 0,-‐398.2934608,412.4252281,-‐11.76842362 LI 0,-‐440.1775314,369.7140109,58.76776729 LI 0,-‐479.8722167,326.5593181,128.6322232 LI 0,-‐516.2116281,283.7118097,197.003895 LI 0,-‐547.4865998,241.6866531,263.0306855 LI 0,-‐572.7910103,200.7794407,325.9322978 LI 0,-‐590.7750941,161.0958207,384.3235579 LI 0,-‐600.4756853,122.587283,437.9151237 LI 0,-‐601.2279683,85.08741206,485.2852133 LI 0,-‐592.7021582,48.34523445,525.6085261 LI 0,-‐574.9048647,12.05438563,558.0328431 LI 0,-‐548.1496441,-‐24.12159416,581.8539271 LI 0,-‐513.0054201,-‐60.52650233,596.5812993 LI 0,-‐470.2330488,-‐97.48938569,601.998875 LI 0,-‐420.7092993,-‐135.301127,598.2102765 LI 0,-‐365.4306801,-‐174.1879673,585.6588194 LI 0,-‐305.306651,-‐214.2809136,565.1146103 LI 0,-‐241.3365236,-‐255.5811867,537.7492078 LI 0,-‐174.4310224,-‐297.9236343,504.494607 LI 0,-‐105.4688838,-‐340.9422559,466.9600619 LI 0,-‐35.2904086,-‐384.0442399,426.400442 LE BGS EN
