CONTROL PARA UN BRAZO ROBOT COLOCADO SOBRE LA PLATAFORMA MÓVIL ÚRSULA

Transcripción

1 CONTROL PARA UN BRAZO ROBOT COLOCADO SOBRE LA PLATAFORMA MÓVIL ÚRSULA MARCELA APARICIO GONZÁLEZ JOHANNA CAROLINA ORJUELA PARRA PONTIFICIA UNIVERSIDAD JAVERIANA FACULTAD DE INGENIERIA CARRERA INGENIERIA ELECTRÓNICA BOGOTA DC 1

2 CONTROL PARA UN BRAZO ROBOT COLOCADO SOBRE LA PLATAFORMA MÓVIL ÚRSULA MARCELA APARICIO GONZALEZ JOHANNA CAROLINA ORJUELA PARRA Trabajo de grado presentado como requsto parcal para optar al título de Ingenero Electrónco. Drector: Carlos Alberto Parra R. Ph.D. Ingenero Electrónco. PONTIFICIA UNIVERSIDAD JAVERIANA FACULTAD DE INGENIERÍA DEPARTAMENTO DE ELECTRÓNICA BOGOTÁ D.C. 25 2

3 PONTIFICIA UNIVERSIDAD JAVERIANA FACULTAD DE INGENIERÍA CARRERA DE INGENIERÍA ELECTRÓNICA RECTOR MAGNÍFICO: R.P. GERARDO REMOLINA S.J DECANO ACADÉMICO: Ing. FRANCISCO JAVIER REBOLLEDO DECANO DEL MEDIO UNIVERSITARIO: R.P. ANTONIO JOSÉ SARMIENTO S.J. DIRECTOR DE CARRERA: Ing. JUAN CARLOS GIRALDO DIRECTOR DEL PROYECTO: Ing. CARLOS ALBERTO PARRA R. Ph.D. 3

4 No éramos, n somos mprescndbles, pero s pusmos entonces y ponemos ahora el esfuerzo de una vda, para el ensanche de la fe, de la cenca y del amor a los más altos valores del espírtu. Padre Jose Gabrel Maldonado S.J. 4

5 Por t... Colomba... 5

6 AGRADECIMIENTOS Al Señor Don Carlos por acompañarnos a lo largo de este camno...por sus regaños sutles...por ser antes que nuestro drector, nuestro amgo...por sus palabras de anmo que nunca nos dejaron desfallecer...por mostrarnos el mundo de una manera dferente...por sus sonrsas snceras...por unos maravllosos abrazos que algunas veces nos hceron llorar...te vamos a extrañar. A una gran persona, que nos aguanto...que nos dedcó todo el tempo sn límtes y sn esperar nada a cambo, que sempre nos recbó con su sonrsa de nño...que nuca dudo de nosotras...y sobretodo que nunca nos abandono en los momentos más dfícles...al Señor Freddy A Camlo, por sus sabos consejos en el tempo precso...por su nconfundble sonrsa en momentos de desesperacón, por regalarnos un poquto de su conocmento A Gabrel Perlla y José I. por escucharnos...por estar sempre pendentes de nosotras y por anmarnos a segur adelante A Calche, Leo y Marlon por su pacenca... por su colaboracón... y por su amabldad durante toda nuestra carrera. Al cap José por preocuparse por nosotras... por recbrnos sempre de la mejor manera...por sus nconfundbles muecas...por el tnto con panela. 6

7 A Dos por estar junto a mí sempre sn condcones n límtes. A ms Padres por ser m guía, m apoyo, m ejemplo, pero por encma de todo por brndarme su amor constantemente. Hoy levanto las manos de trunfo, un trunfo que en gran parte es de ustedes y por ustedes, por un sacrfco que a veces parecó locura y hoy se hace realdad a pesar de todo. Es por eso que todo lo que soy es gracas a ustedes a sus ganas y fortaleza. Mles de gracas nunca serán sufcentes para ustedes. A ms hermantos Andrus y Jamtur por brndarme esos abrazos fuertes, los besos más snceros, las mejores sonrsas, y porque no, por aguantarme. A Andrus por sus ataques de rsa, por esos nconfundbles apodos, por sus juegos de aa y por consentrme. A Jamtur, m futuro Ingenero, por sus apuntes locos, por sus sabos regaños, por los mejores abrazos y por confar en m. A la persona que antes que todo sempre ha sdo m amgo, m consejero, m apoyo, m soporte: Maur. Gracas por tu valosa compañía en un poco más de la mtad de m carrera, por ponerle un tono dferente a este camno. Por tu amor fundamental en este reto que hoy tene fn pero que no es el únco, todavía sguen muchos más. A ms abueltos maternos que están en el celo, los cuales sempre me cudan y a m abuelta Marta por sus sabos consejos, su apoyo y esa experenca que solo tenen ms vejos. Por darme los mejores padres del mundo. A una amga ncondconal, Jule que a pesar del poco tempo que pudmos compartr sempre estuvo conmgo, nunca se alejo y me brndo maravllosas sonrsas en el momento ndcado con las palabras ndcadas. Y como no, a m gran compañera de trabajo, m amga, m hermana, m hombro en este lugar, m sombra y m luz: a Marce, por emprender este camno loco que nunca nos magnamos y hoy termnamos. Por una amstad espero nunca se acabe. Johanna Carolna 7

8 A Dos por darme la vda por rodearme de personas maravllosas, por darme un corazón lleno de sueños y esperanzas por todas las oportundades que me brnda. A ms Padres por su amor ncondconal, por apoyarme en los momentos más dfícles por pensar en m benestar antes que en el de ellos, por estar ahí sempre que los he necestado por ser ellos m terra frme, m guía y m ejemplo, por sus consejos que me han hecho ser quen soy...porque sempre estaremos juntos. A Sandr por su amor y por su confanza en m porque por ella quero ser mejor persona y hacer del mundo un sto en el que los sueños no tengan límtes por aguantarme todos estos años, por sus abrazos, por sus palabras por apoyarme en este camno que termna y porque se que sempre estará en los que ahora comenzan. A m nonta por su amor y confanza por ayudarme a ser quen soy por sus palabras de anmo cuando más las necesto, por sus apapachos, por sus consejos porque aunque lejos sempre estará presente en donde yo este. A Sant por ser m amgo, m nspracón, m cómplce, por todo el amor que me brnda día a día sn esperar nada a cambo, por sus besos, por acompañarme en este camno, por mostrarme cosas de mí que n yo conocía, por ayudarme a crecer y por crecer junto a m.por mostrarme un mundo mas hermoso del que nunca había conocdo, por ser m eterno sueño. A Joha por ser la mejor de las amgas, por escucharme, por comprenderme, por aguantar ms mal genos por sus palabras, por sus abrazos por su optmsmo por sus consejos por ser m compañera de batallas, por confar en mí, espero que la vda nos permta segur compartendo. Marcela 8

9 TABLA DE CONTENIDO 1 INTRODUCCIÓN MARCO TEÓRICO Brazo Robot Componentes y subsstemas de un brazo robot Fundamentos matemátcos y físcos en robótca Descrpcón de poscón y orentacón Sstemas de referenca Descrpcón de la poscón Coordenadas cartesanas Coordenadas clíndrcas Coordenadas esfércas Descrpcón de la orentacón Matrces de rotacón (respecto a uno de los ejes de referenca) Matrces y coordenadas homogéneas Matrz homogénea de transformacón nversa Transformacones báscas: Translacón y Rotacón Translacón Rotacón Composcón de transformacones Rotacones compuestas Matrces de rotacón respecto de un eje arbtraro Cnemátca Espaco artcular y espaco cartesano El problema cnemátco drecto Parámetros de Denavt-Hartenberg Asgnacón de sstemas de referenca Transformacón homogénea Problema cnemátco nverso Métodos de solucón Plataforma de Prueba Dseño y construccón Especfcacones Tarjeta de Desarrollo Especfcacones IMPLEMENTACIÓN Inco de módulos y varables UART (UNIVERSAL ASYNCHRONOUS RECEIVER TRANSMITTER) SPI (Seral Perpheral Interface) ADC (Analog to Dgtal Converter) PWM (Pulse Wdth Modulated)

10 5.1.5 Temporzador Temporzador Temporzador Drectvas Stop Home Poscón absoluta Scan Control de poscón PID PWM Puente H Potencómetro Temporzador Conversor Análogo Dgtal Referenca Funcones utlzadas Transmsón y recepcón de tramas Scan Stop Home Poscón Absoluta Evasón de obstáculos Generacón de trayectoras Lectura de poscón actual Traduccón Cnemátca drecta Cnemátca nversa Protocolo de Pruebas y Resultados Comuncacón Úrsula vs. Plataforma de demostracón Transmsón de tramas por parte de la plataforma de pruebas Recepcón de tramas ADC-Externo vs. Plataforma de pruebas Prueba Control de Poscón Sntonzacón de Controladores Error en cada posconamento Drectvas Drectva Home Drectva Stop Drectva poscón absoluta Drectva Scan Posbles Mejoras Conclusones Glosaro

11 1 Bblografía

12 LISTA DE FIGURAS FIGURA 1. TRASLACIONES Y ROTACIONES BÁSICAS FIGURA 2. EJEMPLO DE ARTICULACIONES DE 1 Y 2 GRADOS DE LIBERTAD FIGURA 3. SISTEMAS DE REFERENCIAS DE COORDENADAS FIGURA 4. DESCRIPCIÓN DE LA POSICIÓN FIGURA 5. POSICIÓN DE UN SISTEMA O RESPECTO A OTRO M FIGURA 6. COORDENADAS CARTESIANAS FIGURA 7. COORDENADAS CILINDRICAS FIGURA 8. COORDENADAS ESFÉRICAS FIGURA 9. SISTEMAS DE REFERENCIA COINCIDENTES EN EL ORIGEN FIGURA 1. TRANSLACIONES BASICAS FIGURA 11. EJEMPLO TRANSLACIÓN Y ROTACIÓN FIGURA 12. ARTICULACIONES FIGURA 13. PROBLEMA CINEMATICO DIRECTO FIGURA 14. PARAMETROS DE DENAVIT-HARTENBERG FIGURA 15. ENUMERACIÓN DE ESLABONES Y ARTICULACIONES FIGURA 16. EJES COORDENADOS FIGURA 17. TRANSFORMACIÓN HOMÓGENEA FIGURA 18. PARTES DE LA PLATAFORMA DE DEMOSTRACIÓN FIGURA 19. PLATAFORMA DE DEMOSTRACIÒN FIGURA 2. UBICACIÓN EN EL PLANO X-Y DE LA PLATAFORMA DE PRUEBA OBSERVADA DESDE ARRIBA FIGURA 21. DIAGRAMA EN BLOQUES TARJETA DE DESARROLLO FIGURA 22. DIAGRAMA DE PINES MICROCONTROLADOR FIGURA 23. DIAGRAMA DE FLUJO INICIALIZACIÓN DEL PROGRAMA FIGURA 24. DIAGRAMA DE FLUJO DIRECTIVA SCAN... 6 FIGURA 25. DIAGRAMA DE FLUJO DIRECTIVA POSICIÓN ABSOLUTA FIGURA 26. DIAGRAMA DE FLUJO DIRECTIVA HOME FIGURA 27. DIAGRAMA DE FLUJO DIRECTIVA STOP FIGURA 28. DIAGRAMA DE TIEMPOS DEL ADC EXTERNO FIGURA 29. CELDAS PARA SCAN FIGURA 3. DIAGRAMA EN BLOQUES CONTROL DE POSICIÓN FIGURA 31. RESPUESTA DE LA PLANTA COMPENSADA FIGURA 32. DIAGRAMA DE FLUJO DE RUTINA DE EVASIÓN DE OBSTÁCULOS FIGURA 33. DIAGRAMA DE FLUJO DE EVALUACIÓN DE OBSTÁCULOS A LA DERECHA FIGURA 34. DIAGRAMA DE FLUJO DE EVALUACIÓN DE OBSTÁCULOS A LA IZQUIERDA FIGURA 35. UBICACIÓN DE LOS ULTRASONIDOS EN LA PLATAFORMA DE DEMOSTRACIÓN. IMAGEN TOMADA DESDE LA PARTE INFERIOR DE LA PLATAFORMA FIGURA 36. SISTEMAS DE REFERENCIA PLATAFORMA DE PRUEBA FIGURA 37. CINEMÁTICA INVERSA PRIMER CUADRANTE FIGURA 38. ÁNGULOS 2 Y 3 CINEMATICA INVERSA PARA Z> FIGURA 39. ÁNGULOS 2 Y 3 CINEMATICA INVERSA PARA Z< FIGURA 4. PRUEBA SOBRE LA DIRECTIVA SCAN CELDA FIGURA 41. PRUEBA SOBRE LA DIRECTIVA SCAN CELDA FIGURA 42. PRUEBA SOBRE LA DIRECTIVA STOP FIGURA 43. PRUEBA SOBRE LA DIRECTIVA HOME SIN OBSTACULOS FIGURA 44. PRUEBA SOBRE LA DIRECTIVA HOME CON OBSTACULOS FIGURA 45. PRUEBA SOBRE LA DIRECTIVA POSICION ABSOLUTA

13 FIGURA 46. PRUEBA SOBRE LA DIRECTIVA SCAN RECEPCION DE DATOS FIGURA 47. PRUEBA SOBRE LA DIRECTIVA STOP RECEPCION DE DATOS FIGURA 48. PRUEBA SOBRE LA DIRECTIVA HOME RECEPCION DE DATOS FIGURA 49. PRUEBA SOBRE LA DIRECTIVA POSICION ABSOLUTA RECEPCION DE DATOS FIGURA 5. INTERFAZ GRÁFICA FIGURA 51. PRUEBA DIFERENTES DISTANCIAS SOBRE EL MISMO MATERIAL FIGURA 52. PRUEBA DIFERENTES DISTANCIAS DISTINTAS FORMAS FIGURA 53. PRUEBA DIFERENTES DISTANCIAS SUPERFICIE PLANA FIGURA 54. PRUEBA DIFERENTES DISTANCIAS SUPERFICIE CONCAVA FIGURA 55. PRUEBA DIFERENTES DISTANCIAS SUPERFICIE CONVEXA FIGURA 56. PRUEBA DIFERENTES DISTANCIAS DISTINTOS MATERIALES FIGURA 57. PRUEBA DIFERENTES DISTANCIAS DISTINTOS MATERIALES FIGURA 58. PRUEBA DIFERENTES DISTANCIAS DISTINTOS MATERIALES FIGURA 59. ANGULOS MEDIDOS EN LA ARTICULACION FIGURA 6. GRÁFICA DE ERROR DE ÁNGULO SOBRE LA ARTICULACION FIGURA 61. ANGULO MEDIDOS SOBRE LA ARTICULACION FIGURA 62. GRÁFICA DE ERROR DE ÁNGULO SOBRE LA ARTICULACION FIGURA 63. ANGULOS MEDIDOS SOBRE LA ARTICULACION FIGURA 64. GRÁFICA DE ERROR DE ÁNGULO SOBRE LA ARTICULACION FIGURA 65. RESULTADOS POSICIÓN X FIGURA 66. RESULTADOS POSICIÓN Y FIGURA 67. RESULTADOS POSICIÓN Z FIGURA 68. OBSTÁCULO NO FIGURA 69. OBSTÁCULO NO

14 LISTA DE TABLAS TABLA NO.1. REPRESENTACIONES DE ÁNGULOS DE EULER TABLA NO.2. CONFIGURACIÓN DEL PUENTE H TABLA NO.3. RANGO DE VOLTAJES EN CADA UNA DE LAS ARTICULACIONES TABLA NO.4. TRAMAS DE COMUNICACIÓN TABLA NO.5. PARÁMETROS DE DENAVIT-HARTENBERG TABLA NO.6. MATRIZ HOMOGÉNEA DE LOCALIZACIÓN DEL SISTEMA DE COORDENADAS DEL EXTREMO RESPECTO A LA BASE TABLA NO.7. CONSTANTES IMPLEMENTADAS EN CADA UNA DE LAS ARTICULACIONES TABLA NO.8. POSICIONES (X,Y,Z) SEGUIDAS POR LA TRAYECTORIA RECTILÍNEA

15 LISTA DE ANEXOS Anexo 1. Tablas de error en la drectva poscón absoluta Anexo 2. Generacón de Trayectoras Anexo 3. Programas Desarrollados Anexo 4. Hojas de Especfcacones 15

16 1 INTRODUCCIÓN La deteccón y desactvacón de mnas antpersonales es uno de los temas más trascendentales y de mayor ncdenca sobre la poblacón colombana, puesto que puede herr o causar la muerte de una o varas personas sn dscrmnar a la víctma, la cual generalmente no está nvolucrada en el conflcto armado. Según el Comté Internaconal de la Cruz Roja (CICR) se calcula que mensualmente 8 personas (26 por día) perden la vda a causa de las mnas antpersonales; el Departamento de Estado de los Estados Undos ndca que cada año hay aproxmadamente 26. (entre muertos y herdos, 72 víctmas daras) personas afectadas por este problema, por otro lado la revsta IDOC Internazonale, afrma que por cada víctma que sobrevve a la explosón de una mna dos fallecen, y que en algunos países el 75% de los sobrevventes requere amputacones[1]. Al mrar la stuacón actual en Colomba, el tema es uno de los más delcados a nvel mundal, puesto que la presenca de campos mnados ha vendo crecendo en lugar de dsmnur, entre 199 y juno de 22, personas fueron víctmas de este flagelo en su mayoría nños y jóvenes, y aun quedan 7. m² de mnas sembradas en áreas de alto resgo para los cvles, ncluyendo zonas cercanas a escuelas, poblacones y camnos; por otro lado hacer una mna cuesta entre 3 y 1 dólares a los grupos al margen de la ley y desactvarla le puede costar al goberno entre 4 y 8 dólares, por lo tanto al sumar todos 16

17 estos problemas es necesaro mplementar una sere de métodos capaces de detectar y desactvar estos artefactos de manera segura[2][3]. Es ndudable que para lograr mtgar el problema y su mpacto sobre la poblacón cvl y mltar, se deben mejorar y/o tecnfcar los métodos de deteccón de mnas y la estratega de desmnado y así dsmnur o hasta elmnar por completo esta amenaza. Debdo a esta stuacón, el objetvo del presente trabajo de grado fue el desarrollo de un prototpo que ayude a la labor de deteccón de mnas antpersonales, perfecconando la estratega usada en el proyecto Úrsula por medo de un sstema de control que manpula un brazo mecánco con tres grados de lbertad y cuyas nstruccones de movmento protegen la ntegrdad de la plataforma móvl y le brndan una mayor longtud de alcance de exploracón buscando una precsón mayor en el sstema de deteccón de mnas. Las valdacones de los métodos de control y movmento se realzaron con una plataforma de demostracón. Este documento esta organzado en 8 capítulos de la sguente forma: en el capítulo 2 se explcan los conceptos báscos para la consecucón del control de la plataforma móvl, en el capítulo tres se dan a conocer las especfcacones mecáncas de la plataforma así como la confguracón de su espaco de trabajo, el capítulo cuatro muestra las especfcacones de la tarjeta de desarrollo, en el captulo 5 se presentan los módulos utlzados en el desarrollo del proyecto, la mplementacón del control de poscón y de cada una de las drectvas a 17

18 ejecutar, descrbendo además las funcones creadas para el desarrollo total del proyecto. El protocolo de pruebas y los resultados obtendos en cada una de ellas son presentados en el capítulo ses. Este análss está dvddo en tres partes: la comuncacón entre la plataforma móvl y el usuaro, el control de poscón y por últmo cada una de las drectvas. Para dejar aberto un campo de nvestgacón y/o desarrollo para que otros nvestgadores sgan aportando e nnovando partes o procesos sobre la plataforma de demostracón, se ncluyó en el presente trabajo en el séptmo capítulo las posbles mejoras las cuales se pueden realzar de acuerdo a las evaluacones y resultados obtendos, por últmo en el capítulo 8 se muestran las conclusones obtendas del desarrollo del proyecto. 18

19 2 MARCO TEÓRICO El presente marco teórco ha sdo tomado de las referencas bblográfcas nombradas a contnuacón 1 : Robots y Sstemas sensorales. Autores: Fernando Torres, Jorge Pomares, Pablo Gl, Santago T. Puente, Rafael Aracl. Edtoral Prentce Hall, 22. Robótca: Control, Deteccón, Vsón e ntelgenca. Autores: K.S.FU, R.C González, C.S.G Le. Edtoral McGraw-Hll, Exact Soluton to the nverse Knematcs of a standard 6-axs Robot Manpulator. Autores: M. Shahnpoor, M Jamshd and Young T. Km Los temas prncpales presentados en este capítulo son: matrz de transformacón homogénea, translacones báscas y compuestas sobre los ejes prncpales de un sstema de referenca, rotacones báscas y compuestas con respecto a cada uno de los prncpales ejes de un sstema de referenca, así como, el análss del problema cnemátco drecto e nverso de un brazo robot. S el lector conoce y domna los temas anterormente nombrados puede pasar drectamente al capítulo 3 en el que se tratará la mplementacón del presente proyecto. 1 Estas referencas bblográfcas se encuentran ncludas en la bblografía del documento 19

20 2.1 Brazo Robot Mecáncamente, un brazo robot está dseñado para alcanzar un lugar específco localzado dentro de su volumen de trabajo representado por una esfera de nfluenca, el cual se determna de acuerdo con los grados de lbertad que posea; de acuerdo con estos grados de lbertad se pueden generar una sere de combnacones de movmentos con el fn de posconar su efector fnal en el lugar que necesta alcanzar Componentes y subsstemas de un brazo robot La estructura mecánca básca de un brazo robot está compuesta por eslabones, que cumplen una funcón smlar a la de un hueso; acconadores, de funconaldad parecda a la de un músculo; transmsones, con certo parecdo a los tendones; y los cables de señal, en certo modo los nervos del robot. Los puntos de unón entre eslabones recben el nombre de nodos y el elemento que permte dcha unón y un movmento relatvo entre ellos, al gual que en el caso de un brazo humano, es la artculacón. La capacdad de carga depende del dmensonamento y característcas estructurales de los eslabones, sstemas de transmsón y acconadores. En general, un cuerpo rígdo puede realzar un movmento muy complejo en el espaco trdmensonal que es dfícl descrbr a pror; sn embargo, ese movmento puede ser consderado como una combnacón de movmentos de translacón y rotacón que a su vez 2

21 pueden ser consderadas como un movmento compuesto por componentes de translacón a lo largo de uno o más de los ejes de coordenadas. De la msma forma, una rotacón puede ser consderada como aquella cuyas componentes reflejan rotacones producdas en torno a los ejes coordenados como lo descrbe la fgura 1. Fgura 1. Traslacones y rotacones báscas 2 Un cuerpo lbre en el espaco puede moverse en tres dreccones ndependentes y perpendculares entre sí con la posbldad de rotar en torno a esas msmas dreccones, es por ello que se dce que posee ses grados de lbertad. Ejemplo: S una artculacón está lmtada a moverse en un plano, posee dos grados de lbertad (ver fgura 2). Fgura 2. Ejemplo de artculacones de 1 y 2 grados de lbertad 2 Todas las fguras de esté capítulo son tomadas de Robots y Sstemas sensorales. Autores: Fernando Torres, Jorge Pomares, Pablo Gl, Santago T. Puente, Rafael Aracl. Edtoral Prentce Hall,

22 2.2 Fundamentos matemátcos y físcos en robótca Descrpcón de poscón y orentacón Para poder efectuar tareas con un robot, es necesaro establecer claramente la forma de descrbr la poscón y la orentacón de un cuerpo rígdo en el espaco, es decr, localzar adecuadamente al robot en un sstema de coordenadas defndo Sstemas de referenca Es muy mportante un sstema de referenca puesto que un cuerpo rígdo mantene la relacón entre éste y el objeto; los sstemas de referenca se defnen y representan habtualmente medante ejes ortogonales que defnen a su vez la nterseccón entre tres planos ortogonales. Normalmente se utlza un sstema dextrógro, como se muestra en la fgura 3. Fgura 3. Sstemas de Referencas de Coordenadas Descrpcón de la poscón. Generalmente un robot ha de ser referencado en el espaco trdmensonal, en este caso, una poscón se establece de forma unívoca medante un vector de poscón M p con tres componentes respecto a un sstema de referenca M. 22

23 Fgura 4. Descrpcón de la poscón Al tener asocado un objeto de nterés un sstema de referenca O, el vector p la poscón del orgen de dcho sstema O con respecto al M. M O representa Fgura 5. Poscón de un sstema O respecto a otro M Coordenadas cartesanas Empleando este tpo de coordenadas, las componentes del vector M p son las proyeccones sobre cada uno de los ejes del sstema de referenca M. Una poscón en el espaco trdmensonal se representa como M p (x, y, z). 23

24 Fgura 6. Coordenadas Cartesanas Coordenadas clíndrcas Una poscón en el espaco trdmensonal se representa como M p (r,θ, z). Las componentes del vector M p en un sstema de referenca M corresponden, al módulo de la proyeccón del vector M p sobre el plano xy, el ángulo que forma dcha proyeccón con el eje x y la proyeccón del vector M p sobre el eje z, respectvamente. Fgura 7. Coordenadas Clndrcas Coordenadas esfércas La prmera y segunda componente del vector M p es la msma que en coordenadas clíndrcas. La tercera componente en este caso corresponde con el ángulo que forma el 24

25 vector M p con el eje z del sstema de referenca M. Una poscón en el espaco trdmensonal se representa como M p (r,θ,φ) (ver fgura 8). Fgura 8. Coordenadas Esfércas Descrpcón de la orentacón Para localzar totalmente un cuerpo en el espaco es necesaro conocer, además de su poscón, su orentacón con respecto a un sstema de referenca, es decr, mentras que la poscón de un cuerpo rígdo respecto a un sstema de referenca M vene dada por la poscón del orgen del sstema de referenca O asocado a este, la orentacón del cuerpo con respecto a un sstema de referenca M vendrá dada por la orentacón relatva de los ejes del sstema de referenca O asocado a este con respecto al sstema M (ver fgura 9). Fgura 9. Sstemas de referenca concdentes en el orgen 25

26 Matrces de rotacón (respecto a uno de los ejes de referenca) Una matrz de rotacón 3 se puede defnr como una matrz de transformacón que opera sobre un vector de poscón en un espaco trdmensonal y transforma sus coordenadas expresadas en un sstema de coordenadas rotado OX o Y o Z o (sstema lgado al cuerpo) a un sstema de coordenadas de referenca MX m Y m Z m P XmYmZm = R*P XoYoZo (2.1) Recordando la defncón de las componentes de un vector se tene: P XoYoZo = P * Xo Xo + P Yo *j Yo + P Zo *k Zo (2.2) Donde P Xm, P Ym y P Zm representan las componentes de P a lo largo de los ejes MZ m, MY m y MZ m respectvamente, de allí se obtene: P Xm Xm * Xo Xm *j Yo Xm * k Zo P Xo P Ym = j Ym * Xo j Ym * j Yo j Ym * k Zo P Yo (2.3) P Zm k Zm * Xo k Zm * j Yo k Zm * k Zo P Zo Análogamente, se pueden obtener las coordenadas P XoYoZo con las coordenadas P XmYmZm P XoYoZo = Q*P XmYmZm (2.4) 3 Se entende como una matrz de 3x3 26

27 P Xo Xo * Xm Xo *j Ym Xo * k Zm P Xm P Yo = j Yo * Xm j Yo * j Ym j Yo * k Zm P Ym (2.5) P Zo k Zo * Xm k Zo * j Ym k Zo * k Zm P Zm Como los productos escalares son conmutatvos Q = R -1 = R T (2.6) Matrces y coordenadas homogéneas En el área de robótca es mportante dsponer de un mecansmo que permta localzar un objeto en el espaco trdmensonal, es decr, en poscón y orentacón conjuntamente, esto se logra medante el uso de las coordenadas homogéneas las cuales permten tener en una sola matrz la poscón y orentacón de un objeto respecto a un sstema de referenca. La matrz de transformacón homogénea es una matrz de 4 x 4 que transforma un vector de poscón expresado en coordenadas homogéneas desde un sstema de coordenadas hasta otro sstema de coordenadas. Una matrz de transformacón homogénea está compuesta por 4 submatrces: T = R 3 x 3 p 3 x 1 Matrz de rotacón Vector de Poscón (traslacón) f 1 x 3 1 x 1 = Transformacón de Escalado perspectva (2.7) La submatrz 3 x 3 superor zquerda representa la matrz de rotacón; la submatrz superor derecha 3 x 1 representa el vector de poscón del orgen del sstema de coordenadas rotado 27

28 con respecto al sstema de referenca que tene como efecto el trasladar un sstema de coordenadas OX o Y o Z o con ejes paralelos al sstema de coordenadas de referenca MX m Y m Z m pero cuyo orgen está en dx, dy, dz; Los vectores de transformacón de perspectva y escalzacón no tenen sentdo en robótca, puesto se trabaja drectamente con poscones y orentacones reales; al gual que se trabaja con un escalzacón real 1 a Matrz homogénea de transformacón nversa La matrz de transformacón permte localzar un sstema O respecto a otro M. En ocasones nteresa conocer la relacón nversa, es decr, conocer la localzacón de M respecto a O, lo que corresponderá a la nversa de la matrz de transformacón homogénea. T = T rotacón rotacón T * traslacón 1 (2.8) 2.3 Transformacones báscas: Translacón y Rotacón Translacón Se pueden consderar tres translacones báscas sobre cada uno de los ejes prncpales de un sstema de referenca, a partr de estas es posble construr una translacón compuesta, que esta representada por el vector p(x, y, z) cuyas componentes corresponden a los valores asocados a cada una de las traslacones báscas (ver fgura 1). 28

29 Fgura 1. Translacones bascas Debdo a que solo se esta tomando el caso en el cual se realza una translacón, la submatrz de rotacón de la matrz de transformacón homogénea será la dentdad y la de translacón son las magntudes de las translacones efectuadas sobre cada uno de los ejes prncpales Rotacón 1 x 1 y Trans ( p) = Trans( x, y, z) = (2.9) 1 z 1 Se pueden obtener tres rotacones báscas consderando tres gros con respecto a cada uno de los ejes prncpales de un sstema de referenca; la localzacón fnal depende del orden en que se haya efectuado cada una de las rotacones báscas. Al realzar cualquer rotacón la matrz de transformacón homogénea tene como vector de traslacón cero, ya que se analza en este momento solo la rotacón. Al aplcar la ecuacón 2.3 y realzando los productos escalares se obtene: Rotacón sobre el eje x: Se refere a la rotacón con un ángulo α sobre el eje x: 29

30 3 + = 1 ) cos( ) cos(9 ) cos(9 ) cos( 1 ), ( α α α α α x Rot (2.1) Rotacón sobre el eje y: Se refere a la rotacón con un ángulo β sobre el eje y: + = 1 ) cos( ) cos(9 1 ) cos(9 ) cos( ), ( α β β β β y Rot (2.11) Rotacón sobre el eje z: Se refere a la rotacón con un ángulo γ sobre el eje z. + = 1 1 ) cos( ) cos(9 ) cos(9 ) cos( ), ( γ γ γ γ γ z Rot (2.12) Composcón de transformacones Una transformacón compleja se descompone en una sere de transformacones báscas de traslacón en el caso de un cambo del objeto respecto a la referenca, y/o de rotacón, s lo que se produce es un gro del objeto respecto al sstema de referenca. La composcón de transformacones, al estar representadas por matrces, supone que el orden en que se aplca cada una de las transformacones báscas que la componen es relevante, ya que el producto de matrces no es conmutatvo. Además, es necesaro

31 dentfcar con respecto a qué sstema se realza cada transformacón, debdo a que ayuda a determnar el orden de las operacones a realzar. Cuando se realza una transformacón de una localzacón (poscón y gro) a otra se tenen dos posbldades para referencarla. La prmera se determna respecto al sstema resultante de la transformacón anteror, que se le denomna móvl, la segunda con respecto al que fue referenca para la últma transformacón, que se le conoce como fjo. La prmera vez que se aplca una transformacón no exste móvl, o se puede consderar como concdente con el fjo; tenendo en cuenta esto se determnan las sguentes reglas: S la transformacón se realza con respecto al sstema fjo se premultplca sobre las transformacones ya efectuadas. S la transformacón se efectúa sobre el sstema móvl, es decr, con respecto a la últma localzacón del sstema transformado se postmultplca respecto a las aplcadas prevamente. Tomando como ejemplo, p = Rot( z, 9) * Tras(, a,) * (2.13) fnal p fnal Se puede representar prmero con una translacón con respecto al sstema fjo y posterormente una rotacón tambén con respecto al sstema fjo (ver fgura 11a). 31

32 Fgura 11. Ejemplo Translacón y Rotacón Sn embargo, consderando las reglas anterores, tambén se puede nterpretar como una prmera transformacón de rotacón respecto al sstema fjo seguda de una translacón respecto al sstema móvl (ver fgura 11b.), sendo el resultado el msmo para ambos casos Rotacones compuestas Las matrces de rotacón báscas se pueden multplcar entre s para representar una secuenca de rotacón fnta respecto del eje prncpal del sstema de coordenadas 4 MX m Y m Z m. Cabe anotar que en robótca nteresa representar la orentacón de un sstema respecto a otro a través de cualquer eje de rotacón (no solamente con respecto al eje prncpal). Un gro general se puede descomponer en una combnacón de tres rotacones báscas realzadas en un determnado orden, tenendo en cuenta las reglas generales de composcón de transformacones es posble obtener más de una agrupacón de rotacones báscas para un msmo gro; entre estas exsten ventcuatro combnacones defndas: doce de ellas se obtenen medante combnacón de tres rotacones smples, realzadas sobre los ejes 4 Como las multplcacones de matrces no conmutan, es mportante el orden o secuenca de realzacón de las rotacones 32

33 prncpales del sstema fjo, las otras doce conocdas como ángulos de Euler, se defnen medante combnacón de tres gros sobre ejes prncpales del sstema móvl Matrz de rotacón con representacón de ángulos de Euler Los ángulos de Euler descrben la orentacón de un cuerpo rígdo con respecto a un sstema de referenca fjo. Hay muchos tpos dferentes de representacones de ángulos de Euler. Secuenca De Rotacones Sstema I Ángulos Euleranos respecto del eje MZ m respecto del eje OX o respecto del eje OZ o Sstema II Ángulos De Euler respecto del eje MZ m respecto del eje OY o respecto del eje OZ o Tabla No.1. Representacones de ángulos de Euler Sstema III Elevacón Desvacón y Gro respecto del eje MX m respecto del eje MY m respecto del eje MZ m En la tabla 1 la prmera representacón de los ángulos Euleranos corresponde a: 1. Una rotacón de ángulo respecto del eje MZ m. 2. Una rotacón de ángulo respecto del eje OX o. 3. Fnalmente, una rotacón de ángulo respecto del eje OZ o. La matrz de rotacón de ángulos se puede especfcar tambén en térmnos de las rotacones respecto de los ejes prncpales del sstema de coordenadas de referenca de la sguente forma: una rotacón del ángulo respecto del eje MZ m seguda por una rotacón del ángulo 33

34 respecto del eje MX m y fnalmente una rotacón de respecto del eje OZ o. Por lo tanto la matrz de rotacón resultante es: C()*C() S()*C()*S() -C()*S() S()*C()*C() S()*S() S()*C() C()*C()*S() -S()*S() + C()*C()*C() -C()*S() S()*S( ) S()*C() C() (2.14) Igualmente se pueden nterpretar las demás representacones Matrces de rotacón respecto de un eje arbtraro Para hallar la matrz de rotacón alrededor de un eje r se hacen algunas rotacones respecto de los ejes prncpales del sstema MX m Y m Z m para alnear el eje r con el eje MZ m luego se hace la rotacón respecto de r con ángulo n y se gra el eje prncpal del sstema MX m Y m Z m para volver al eje r otra vez a su poscón orgnal. Una vez realzado el procedmento anteror se llega a la sguente matrz de transformacón r 2 Xm V() + C() r Xm * r Ym * V() r Zm * S() r Xm * r Zm * V() + r Ym * S() R r, = r Xm * r Ym * V() + r Zm * S() r 2 Ym * V() + C() r Ym * r Zm * V() r Xm * S() r Xm * r Zm * V() r Ym * S() r Ym * r Zm * V() + r Xm * r 2 Xm * V() + C() S() (2.15) En la anteror matrz se tene: V () = 1 Cos () C () = Cos () S () = Sen () (2.16) 34

35 2.4 Cnemátca La cnemátca del brazo del robot trata del estudo analítco de la geometría del movmento de un brazo robot con respecto a un sstema de coordenadas de referenca fjo sn consderar las fuerzas o momentos que orgnan el movmento, nteresándose por la descrpcón analítca del desplazamento espacal del robot como una funcón del tempo, en partcular de las relacones entre la poscón de las varables de artculacón y la orentacón del efector fnal del brazo robot. Hay dos problemas fundamentales en la cnemátca del robot: Problema cnemátco drecto: estuda cuál es la orentacón y la poscón del efector fnal con respecto a un sstema de coordenadas de referenca dado el vector de ángulos de las artculacones q(t)=(q1(t),q2(t),,qn(t)) T y los parámetros geométrcos del brazo. 5 Problema cnemátco nverso: estuda s el brazo puede alcanzar la poscón y orentacón de la mano que se desea, y s puede, cuántas confguracones satsfacen la msma condcón dada una poscón y orentacón deseada del efector fnal del brazo y los parámetros geométrcos de los elementos con respecto a un sstema de coordenadas de referenca. 5 donde n es el número de grados de lbertad 35

36 2.4.1 Espaco artcular y espaco cartesano Para un robot como el de la fgura 12, en el que las artculacones han sdo numeradas desde 1 hasta n, el valor del parámetro de la artculacón, notado como q, es llamado varable artcular y el conjunto de varables: q = ( q1, q2, q3, qn) (2.17) Se denota como vector de varables artculares. Fgura 12. Artculacones En el caso más general, en el cual el extremo del robot puede tomar una poscón y orentacón cualquera en el espaco trdmensonal, el vector de coordenadas para el extremo del robot tendrá ses parámetros, tres de poscón y tres de orentacón: p = ( x, y, z, α, β, γ ) (2.18) La relacón que lga los parámetros en el espaco artcular y el cartesano generalmente es de tpo no lneal, y para la cnemátca drecta vene dada por: p = F(q) (2.19) la cual presenta una solucón únca a este problema. Por otro lado para la cnemátca nversa la relacón está dada por la expresón: 36

37 q = F 1 ( p) (2.2) Desde un punto de vsta analítco, no está garantzada la solucón a la cnemátca nversa y s exste, puede que no sea únca. Por lo tanto, algunas de las posbles solucones no son una respuesta adecuada, ya que las artculacones estarían en poscones físcamente mposbles El problema cnemátco drecto Como los elementos de un brazo pueden grar y/o trasladarse con respecto a un sstema de coordenadas de referenca, es necesaro establecer un sstema de coordenadas lgado al cuerpo a lo largo del eje de la artculacón para cada elemento; el problema cnemátco drecto se reduce a encontrar una matrz de transformacón que relacona el sstema de coordenadas lgado al cuerpo con el sstema de coordenadas de referenca. Para resolver el problema de cnemátca drecta, en el caso más general se puede asocar un sstema de referenca a cada uno de los eslabones, ncludos la base y el extremo del robot. S se tene en cuenta que sempre exstrá una transformacón homogénea, compuesta por traslacones y rotacones báscas, estos permtrán pasar de un sstema de referenca asocado al eslabón al del eslabón +1, y dcha transformacón quedará en funcón de los parámetros de la artculacón : T + 1 = F( q ) (2.21) 37

38 El problema se reduce a terar el proceso de búsqueda de las n+1 transformacones necesaras para pasar desde el sstema asocado a la base del robot hasta el extremo del robot, pasando por los sstemas asocados a todos y cada uno de los eslabones, generándose con todas ellas una transformacón homogénea total que expresa la poscón y orentacón del extremo del robot con respecto a la base en funcón de los parámetros de las artculacones. base T extremo = base 1 2 n 1 n T1 * T2* T3 Tn* Textremo = F( q1, q2, q3, qn ) (2.22) En la fgura 13 se esquematza el proceso para un robot genérco de n grados de lbertad. Fgura 13. Problema Cnematco Drecto Exste un método sstemátco para resolver el problema de cnemátca drecta a través de una transformacón homogénea, el cual se dvde en tres fases: Defncón de los parámetros de Denavt-Hartenberg Asgnacón de sstemas de referenca Transformacón homogénea. 38

39 Parámetros de Denavt-Hartenberg Los robots están compuestos por una sere de artculacones ya sean del tpo prsmátco 6 o rotaconal 7, las cuales están conectadas entre s por medo de eslabones, de acuerdo a esto se pueden establecer cuatro parámetros, dos relatvos al tamaño y forma del eslabón y otros dos relaconados con la poscón relatva entre los eslabones consecutvos 8. Al hallar dchos parámetros es necesaro tener en cuenta que el eje de una artculacón se defne según el eje con respecto al que se produce el movmento, es decr, en una artculacón rotaconal se defne respecto al gro y en una artculacón prsmátca respecto a la dreccón de desplazamento (ver fgura 14). Fgura 14. Parametros de Denavt-Hartenberg 6 Permten una translacón de un eslabón con respecto a otro 7 Permten un gro en torno a un eje de un eslabón con respecto a otro 8 Parámetros relatvos a la artculacón que los enlaza 39

40 Los dos parámetros relatvos al tamaño y forma del eslabón son: a, dstanca entre los ejes e +1 de las artculacones a lo largo de la normal común. Este parámetro defne en certo modo el tamaño del eslabón, y por esto se le conoce como longtud del eslabón. α, ángulo que exstría entre los ejes e +1 de las artculacones s estos se cortaran en los puntos de corte de la línea normal común. Este parámetro mde la forma del eslabón a través del ángulo que sobre el msmo se encuentra grado, por lo que se le conoce como ángulo de torsón del eslabón. Los parámetros que relaconan la poscón relatva de un eslabón con respecto a su predecesor son: d, dstanca entre las nterseccones de las normales comunes al eje de la artculacón, medda a lo largo de ese msmo eje; este parámetro expresa la dstanca entre los dos eslabones, marcada por el tamaño y forma de la artculacón, por esto se le conoce como longtud artcular. θ, ángulo que exstría entre las líneas normales comunes al eje de la artculacón s se cortarán en el msmo punto del eje de la artculacón. Expresa, entonces, el ángulo que forman los dos eslabones, tenendo en cuenta la forma de la artculacón, por lo que se denomna longtud artcular. 4

41 Los parámetros relatvos a la forma y tamaño del eslabón, por ser este un cuerpo rígdo, no sufren nnguna varacón, sn embargo, de los dos parámetros que relaconan la poscón relatva entre los eslabones, al estar undos por una artculacón uno de ellos vara, dcho parámetro depende del tpo de artculacón que se esté manejando y es llamada varable q artcular. De esta forma para una artculacón del tpo rotaconal, al ángulo θ, es el que produce la varacón y la dstanca relatva d permanece constante, mentras que para una d artculacón prsmátca, el parámetro varable es la dstanca relatva, y permanece fjo el θ ángulo Asgnacón de sstemas de referenca El problema cnemátco drecto se puede resolver encontrando una transformacón homogénea, funcón de los parámetros de las artculacones, que exprese la poscón y orentacón del extremo del robot con respecto a un sstema de referenca stuado en la base de este. El método de solucón se basa en la defncón de sstemas de referenca asocados a cada uno de los eslabones, logrando así realzar la transformacón entre dos eslabones consecutvos sólo medante dos gros y dos translacones, es mportante aclarar que se pueden realzar dferentes asgnacones de sstema de referenca a un msmo eslabón, logrando con algunos de ellos que determnados parámetros de los eslabones resulten nulos; 41

42 uno de ellos es localzar el sstema de referenca del eslabón en el eje de la artculacón que la enlaza con el sguente eslabón. Una vez numerados los eslabones y artculacones (ver 15), el sstema de referenca asocado al eslabón estará stuado en un punto que pueda consderarse como fn del eslabón, a lo largo de la artculacón +1 que lo une con el eslabón posteror(ver fgura 16). Fgura 15. Enumeracón de eslabones y artculacones Fgura 16. Ejes Coordenados El eje z del sstema de referenca del eslabón se alnea con el eje de la artculacón +1. El eje x del sstema de referenca se alnea con la normal común entre las artculacones e 42

43 +1, apuntando de a +1, al fjar este eje queda fjo el orgen del sstema. El eje y se establece para que el sstema de referenca sea dextrógro. Pueden darse dos stuacones en la que la línea normal común (eje x ) no sea únca: Cuando los ejes son paralelos, en este caso el orgen del sstema de referenca queda ndefndo, por conveno se toma el orgen en la artculacón +1. Los ejes se cortan entre sí, el orgen del sstema se localza en el punto de corte, en cuyo caso x está según la dreccón perpendcular al plano que forman z y z 1, tomándose el sentdo en este caso de forma arbtrara. De la msma forma descrta anterormente, se determna el resto de sstemas de referenca para los eslabones 1 a n-1, es decr, todos los eslabones excluyendo la base del robot y el eslabón del extremo. El sstema de referenca asocado a la base del robot, el eslabón, es el únco que permanece fjo en la cadena cnemátca, por lo que es consderado como el sstema de referenca de todos los demás, el eje z esta alneado con la artculacón 1; sn embargo, al no exstr la artculacón, se toma y de tal forma que el sstema resulte dextrógro. 43

44 El sstema de referenca asocado al últmo eslabón se localza en el extremo del robot; en este caso no exste artculacón n+1, por lo que su eje z n se toma concdente con el eje z del sstema asocado al eslabón n Transformacón homogénea De acuerdo a lo vsto anterormente, se observa que para pasar del sstema -1 al es necesaro aplcar dos gros y dos translacones en el orden adecuado, los cuales corresponden con los parámetros de Denavt-Hartenberg para la artculacón (ver fgura 17), los que toman los sguentes valores: θ como el ángulo entre x 1 y x meddo en torno a z 1. d como la dstanca medda a lo largo de z 1 entre el orgen del sstema -1 y la nterseccón de los ejes x y z 1. α como ángulo entre z 1 y z meddo en torno a x a como la dstanca medda a lo largo de x entre la nterseccón de los ejes x y z 1 y el orgen del sstema. 9 9 Las operacones entre θ y d pueden ser conmutadas entre s al gual que las operacones entre α y a. 44

45 45 Fgura 17. Transformacón Homógenea Con base en estas cuatro transformacones, se obtene la matrz de transformacón homogénea para pasar del sstema -1 al sstema : ), ( ) *, ( ) *, ( ) *, ( x Rot a x Tras d z Tras z Rot T α θ = = 1 ) cos( ) ( ) ( ) cos( 1 * * * 1 1 ) cos( ) ( ) ( ) cos( sen sen a d sen sen α α α α θ θ θ θ = = ), ( ) *, ( ) *, ( ) *, ( 1 1 a x Tras x Rot z Rot d z Tras α θ = 1 ) cos( ) ( ) ( ) ) cos( ( ) ) cos( cos( ) ( ) cos( ) ( ) ( ) ( ) cos( ) cos( d sen a sen sen sen a sen sen sen α α θ θ α θ α θ θ θ α θ α θ (2.23)

46 Determnando cada una de las matrces de transformacón entre los dstntos sstemas de coordenadas del robot, se puede llegar a la matrz de transformacón entre el sstema de coordenadas de la base del robot y el del extremo del msmo Problema cnemátco nverso El problema de cnemátca nversa consste en determnar qué valores tenen que tomar las varables artculares para que el extremo del robot se encuentre en una poscón y orentacón dadas. Tenendo en cuenta la matrz de transformacón homogénea total y las submatrces de rotacón y de poscón se puede observar que: o r 11 r12 r13 x extremo o = r21 r22 r23 yextremo T extremo (2.24) o r 31 r32 r33 zextremo 1 o Rot extremo r r = r r r r r r r (2.25) o o o o Tras = p ( x, y, z ) (2.26) o extremo extremo extremo extremo extremo 1 Ecuacón

47 Cada uno de los elementos no nulos de la matrz total son funcón de las n varables artculares, resultando doce ecuacones, nueve correspondentes a los elementos de la submatrz de rotacón y las otras tres a los del vector de translacón. Sn embargo, para expresar una rotacón se necestan tres grados de lbertad, por lo que solo tres de las nueve ecuacones son lnealmente ndependentes, el conjunto de ecuacones se expresa como: x y z α β γ o extremo o extremo o extremo o extremo o extremo o extremo = = = = = = f f f f f f x z ( q1, q2,, qn ) ( q1, q2,, qn ) ( q1, q2,, qn ) ( q1, q2,, qn ) ( q1, q2,, qn ) ( q, q,, q ) y α γ β 1 2 n (2.27) Debdo a la no lnealdad del sstema de ecuacones pueden presentarse dos crcunstancas: No exste solucón: este caso se presenta, cuando aún tenendo desde un punto de vsta analítco una solucón al conjunto de ecuacones 2.27, todas las solucones encontradas se encuentren fuera del espaco de trabajo del robot o smplemente no satsfacen los rangos de movmento de las artculacones reales. Exstenca de múltples solucones: La solucón óptma depende del problema que se trata, no sempre es factble la solucón que mnmce el recorrdo de las artculacones. 47

48 Métodos de solucón Para resolver el problema cnemátco nverso se puede optar por dos vías: solucón numérca o solucón cerrada. La prmera posbldad se desecha generalmente, ya que resulta más lenta y costosa computaconalmente que una solucón cerrada. La solucón cerrada hace referenca a la búsqueda de una solucón basada en expresones analítcas o en solucones polnómcas, que no hacen uso de cálculos teratvos. Exsten dos métodos: Solucón algebraca: consste en selecconar ses ecuacones de las doce de todo el conjunto dado, de forma que se establezca un sstema de ses ecuacones con ses ncógntas que sea fácl de resolver. Solucón geométrca: consste en descomponer la cadena cnemátca del robot en varos planos geométrcos, resolvendo por trgonometría el problema asocado a cada plano. 48

49 3 Plataforma de Prueba Debdo a las lmtacones que tene el brazo móvl de la plataforma Úrsula al poseer solo dos grados, se ve la necesdad de aumentarlos, logrando así amplar las capacdades de movmento y la precsón alcanzada. En este contexto, se recuerdan los objetvos propuestos en este proyecto, los cuales pretenden optmzar y expandr la labor del brazo utlzado en la plataforma móvl Úrsula para la deteccón de mnas antpersonales: 1. Ejecutar un movmento controlado de exploracón para garantzar el barrdo de la regón medante el segumento de una trayectora predetermnada hacendo uso de un control de poscón. 2. Calcular la cnemátca nversa que le proporcone al brazo la capacdad de r a poscones específcas alcanzables en el espaco. 3. Desarrollar los algortmos de control del brazo desde un sstema mcrocontrolado. 4. Generar una rutna de evasón de obstáculos para el brazo móvl medante el segumento del respectvo contorno, utlzando sensores de ultrasondo. No sendo objetvo del trabajo de grado, pero consttuyéndose en una herramenta fundamental para mostrar las posbldades de los desarrollos realzados y por razones ajenas a la realzacón del presente proyecto, se hzo necesaro desarrollar una plataforma artesanal de pruebas, con la que se pudera valdar las capacdades de los algortmos de control desarrollados de una manera demostratva y con ello poder presentar y evaluar los 49

50 objetvos del proyecto. El desarrollo y especfcacones de dcha plataforma se muestran a contnuacón. 3.1 Dseño y construccón Incalmente el dseño y la construccón de la plataforma mecánca fue guado por personas con conocmento en el desarrollo de sstemas mecáncos, este proceso se realzó en varas etapas: prmero se realzaron los cortes correspondentes a los 3 prmeros eslabones utlzando alumno debdo a que es un materal durable, lvano y económco, el cuarto eslabón se fabrcó en balso con el objetvo de mnmzar el peso generado sobre la plataforma y dsmnur al máxmo el esfuerzo realzado por los motores, luego se procedó a la fabrcacón de 6 pñones en nylon, dos para cada artculacón, con un dámetro de 3.8cm y 2 dentes; posterormente se ncó con la ntegracón de los pñones, los eslabones, pasadores y bujes, los últmos dos están hechos en bronce y permten asegurar los ejes de los motores que generan el movmento y de los potencómetros que sensan un voltaje proporconal al ángulo en el que se encuentra la artculacón. Esta estructura fue ubcada en una caja hueca fabrcada en acero con meddas 24.5cm de ancho, 24.5cm de largo y 17.5cm de alto, la cual soporta el peso de la plataforma de pruebas y adconalmente almacena las tarjetas utlzadas para controlar la plataforma demostratva y manejar los ultrasondos (ver fgura 18). 5

51 Pñones Eslabón 2 Eslabón 4 Eslabón 1 Eslabón Fgura 18a. Eslabones y pñones Artculacón 2 Artculacón 3 Artculacón 1 Fgura 18b. Artculacones Fgura 18. Partes de la plataforma de demostracón Por otro lado el tercer eslabón soporta una caja rectangular fabrcada en cartón paja de meddas 27cm de ancho, 5cm de largo y 5cm alto, la cual soporta las 3 parejas de ultrasondos, con el fn de lograr que cada pareja de ultrasondos funcone de una manera ndependente y no genere nterferenca sobre los demás dspostvos. 51

52 3.2 Especfcacones Fgura 19a. Vsta superor Fgura 19. Fgura 19b.Vsta lateral Plataforma de demostracòn La plataforma de pruebas posee tres artculacones, encargándose cada una de ellas de uno de los grados de lbertad (ver fgura 19). La plataforma de pruebas consta de 4 eslabones, el prmer eslabón corresponde a la base sobre la cual gra la plataforma; el segundo, tercero y cuarto eslabón se encargan de unr las artculacones entre s, dchos eslabones tenen longtudes de 16, 23 y 25cm respectvamente. Para generar el movmento en cada una de las artculacones se utlzan 3 motores11, marca Pttman GM8223 con reductor de 6.5 a 1. El movmento de las artculacones se sensa por medo del voltaje sobre tres potencómetros sn fn, cada uno de los cuales está acoplado mecáncamente al eje de cada motor. El espaco de trabajo de la plataforma corresponde a un movmento de 18º grados en el plano x-y, un movmento de 85 grados en el plano y-z utlzando la 11 Un motor para cada artculacón. 52

53 artculacón número 2 y un movmento de 18 grados en el plano y-z utlzando la artculacón número 3. El prmer grado de lbertad corresponde al movmento en el plano X-Y, dcho movmento corresponderá a un ángulo postvo s se está movendo a su derecha y a un ángulo negatvo s se está movendo a su zquerda como se puede observar en la fgura 2 en la que se muestra la ubcacón de la plataforma para un observador ubcado en la parte superor : X (Postvas) Y (Postvas) Y (Negatvas) Frente Q1 postvos Q1 Negatvos Fgura 2. X (Negatvas) Ubcacón en el plano X-Y de la plataforma de prueba observada desde arrba El segundo y tercer grado de lbertad corresponden a movmentos en el plano Y-Z, dcho movmento corresponde a un ángulo postvo s se está movendo haca arrba y a un ángulo negatvo s se está movendo haca abajo, encontrando el eje Z postvo haca arrba del plano, y el eje Z negatvo haca abajo del plano. Para la deteccón de obstáculos la plataforma demostratva cuenta con 3 parejas de ultrasondos ubcados como se puede observar en la fgura 18 (derecha, zquerda y abajo). 53

54 En la estratega de cableado se tuvo en cuenta una dstanca mínma de 5cm entre los cables de los ultrasondos transmsores y de los receptores para evtar nterferenca, además se permtó que los cables tuveran lbertad de movmento en cada una de las artculacones. La fuente utlzada para almentar el dspc3f21 es de 24 voltos. En un futuro esta fuente de almentacón será tomada del sstema de almentacón del robot móvl Úrsula. 54

55 4 Tarjeta de Desarrollo 4.1 Especfcacones La tarjeta de desarrollo esta conformada por un mcrocontrolador dspc3f21 el cual se comunca con el ADC externo MCP328 para el manejo de los ultrasondos, además cuenta con tres puente H L623 para el manejo de los motores y con un manejador de línea para la comuncacón seral con el computador 12 (ver fgura 21). Manejador de Línea (SN65C3221) Computador Ucontrolador Puente H (L623) Motor Potencometros a ADC (MCP328) Fltro Pasabajos Ultrasondos Fgura 21. Dagrama en bloques tarjeta de desarrollo El mcrocontrolador utlzado tene las sguentes especfcacones 13 : Modfcacón de la arqutectura Harvard 12 Kbytes de memora Flash 512 bytes de memora RAM 1 Kbyte de memora EEPROM no volátl Regstros de trabajo de 16 x 16 7 nveles de prordad de las nterrupcones nternas 2 acumuladores de 4 bts de ancho con saturacón lógca opconal 6 canales de salda para PWM Modos de salda complementaros e ndependentes Conversor análogo dgtal de 1 bts 12 El esquemátco de la tarjeta de desarrollo se muestra en el anexo de especfcacones 13 Para mas detalles de las especfcacones del mcrocontrolador ver anexo dspc3f21datasheet 55

56 6 canales de entrada En la fgura 22 se muestra el dagrama de pnes del mcrocontrolador utlzado: Fgura 22. Dagrama de pnes mcrocontrolador 56

57 5 IMPLEMENTACIÓN La plataforma de prueba utlzada para la mplementacón del presente proyecto posee tres grados de lbertad y su control se desarrolló en un mcrocontrolador dspc3f21, dcho control no tene memora, es decr, no tene conocmento de los puntos por los que ha pasado hacendo necesaro que las lmtacones creadas por esto se manejen por medo de las drectvas dsponbles. Esta plataforma de prueba está dseñada para servr como soporte para la deteccón de mnas y de la plataforma móvl Úrsula. Por esto su funconamento debe ser efcente para poder garantzar la segurdad e ntegrdad tanto de Úrsula como de la estructura fja. Con este fn, la plataforma de prueba está conformada por 4 bloques ndependentes que trabajan conjuntamente (control de poscón, comuncacones, análss de drectvas y actualzacón de ultrasondos). El prmer y fundamental bloque es el control de poscón, sendo este responsable de la ubcacón de la plataforma demostratva en todo momento, sempre está actvo y lo únco que los demás bloques pueden modfcarle es la referenca, asegurando así, que en todo momento, la plataforma tenga una poscón controlada. El segundo bloque es el encargado de las comuncacones de la plataforma de prueba con el procesador central Úrsula a través de la UART 14, dcho módulo tene como funcón 14 Unversal Asnchronus Recever Transmter 57

58 prncpal recbr las órdenes envadas por la plataforma móvl y transmtr la respuesta a la drectva envada. El tercer bloque Análss de drectvas es el encargado de ncar la ejecucón de una orden recbda. Las cuatro posbles drectvas a ejecutar son: Home: Ordena a la plataforma r a una poscón predetermnada como Home. Cabe anotar que antes de la ejecucón de las demás drectvas es necesaro que la plataforma demostratva se encuentre en poscón Home con el fn de conservar sempre la msma referenca. Stop: Medante esta drectva se detene cualquer movmento que este en ejecucón, permtendo que la plataforma realce una parada de emergenca s ésta fuera necesaro. Scan: De acuerdo al modelo propuesto en el proyecto de nvestgacón del Ingenero Javer Coronado la estratega de barrdo del terreno se realza por carrles los cuales están dvddos en cnco celdas 15. Así la exploracón que realza la plataforma de demostracón se adecua a está propuesta, ejecutando un movmento en línea recta con el número de celdas especfcado, con el fn de realzar la búsqueda de mnas evadendo los obstáculos que se le presenten. Posconamento absoluto: Por medo de esta drectva, la plataforma móvl puede pedr a la plataforma demostratva que se poscone en un punto específco en el 15 Integracón de Capacdades para la Navegacón de un Robot Móvl. Ingenero Javer Coronado. 58

59 espaco de trabajo, es decr en los puntos donde los límtes mecáncos de la plataforma demostratva le permtan r. Esta drectva revsa s hay obstáculos pero no los evade. Por últmo, el cuarto bloque tene como fn actualzar las varables utlzadas en el programa de lectura de los ultrasondos, que srven para determnar s hay algún obstáculo en el sentdo del movmento o no, evtando que la plataforma de pruebas se golpee contra algún objeto exstente mentras se encuentra ejecutando alguna de las drectvas de movmento. El funconamento de la plataforma demostratva, se representa a contnuacón medante un dagrama de flujo en el cual se descrben los 4 bloques explcados anterormente (ver fgura 23 a 27). Fgura 23. Dagrama de flujo ncalzacón del programa 59

60 Fgura 24. Dagrama de flujo drectva Scan 6

61 Fgura 25. Dagrama de flujo drectva poscón absoluta 61

62 Fgura 26. Dagrama de flujo drectva Home 62

63 #$!" % &' Fgura 27. Dagrama de flujo drectva Stop 5.1 Inco de módulos y varables. El programa llena y hablta una sola vez los regstros necesaros en todo el proceso de control, al gual que las nterrupcones y su respectvo nvel de prordad, a su vez se confguran los módulos UART 16, SPI 17, ADC 18, PWM 19, temporzador, el 16 Unversal Asynchronous Recever Transmtter 17 Seral Perpheral Interface 18 Analog to Dgtal Converter 19 Pulse Wdth Modulated 63

64 mcrocontrolador dspc3f21 para trabajar con multplcacón fracconal y control de saturacón. Este proceso lo realza un subprograma llamado Incalzacones 2. En las subseccones sguentes se explcarán detalle cada uno de estos componentes UART (UNIVERSAL ASYNCHRONOUS RECEIVER TRANSMITTER) El módulo se hablta con el fn de proporconar un medo de comuncacón entre la plataforma de demostracón y Úrsula, el cual se encuentra confgurado para trabajar con 8 bts de transmsón y uno de parada, con una tasa de transmsón de 96 bps y con nterrupcones de transmsón y de recepcón. En el caso de la nterrupcón de transmsón, el módulo está confgurado para que nterrumpa cuando transmte un carácter (8 bts), ya que la mayoría de tramas que el módulo necesta envar constan de un solo carácter, en la atencón a subrutna de la transmsón se borra la bandera de nterrupcón de la transmsón y se hablta al módulo para una nueva transmsón. La nterrupcón de la recepcón está confgurada para habltarse cuando se recbe un carácter, en esta atencón a nterrupcón se lee el dato recbdo y se lmpa la bandera de atencón a nterrupcón. Gracas a la nformacón brndada por esta nterrupcón es posble 2 Para detalles de esta funcón ver anexo 2 programas 64

65 analzar y ejecutar la drectva recbda correctamente medante la funcón Rx_Tramas 21 ; dcha funcón se encarga de actualzar el valor de las varables necesaras para la drectva que se desea ejecutar. El nvel de nterrupcón que fue asgnado a la transmsón y recepcón de la UART fue respectvamente 4 y 5 22 debdo a que es muy mportante recbr y analzar lo que transmta Úrsula y además avsarle a tempo en caso de pelgro SPI (Seral Perpheral Interface) Este módulo es utlzado para comuncar seralmente el dspc3f21 con el ADC externo MCP328, el cual tene como entradas las señales análogas provenentes de los sensores de proxmdad. El MCP328 tene una resolucón de 12 bts y ocho canales de entrada; los canales utlzados son los cuatro últmos ya que estos tenen un amplfcador que le da gananca a la señal de entrada y además la aísla hacendo que los sensores vean alta mpedanca en todo momento y no sean afectados. Por otro lado el ADC puede trabajar en modo únco, es decr, referdo a terra o dferencal, actualmente está confgurado como únco. 21 Para detalles de esta funcón ver Anexo 2 programas 22 De 7 nterrupcones 7 es el máxmo nvel de nterrupcón 65

66 La nterfaz seral del mcrocontrolador consta de un bus de entrada de datos, una salda seral de datos y el reloj, el cual es manejado por el maestro que en este caso es dspc3f21 el y el esclavo es el MCP328, en cuanto a la transmsón y recepcón de datos entre estos se realza en modo 16, es decr, se utlza un regstro de 16 bts para la comuncacón. Para ncar la comuncacón, el ADC debe recbr un borde de bajada en la señal CS, (utlzando lógca negada), la cual es envada por medo de un puerto dgtal del mcrocontrolador antes de comenzar una comuncacón en cualquera de los canales; además, el ADC necesta en su bus de datos de entrada una trama de control que ndca, en los cnco bts más sgnfcatvos, el nco de conversón, el modo de trabajo (modo únco o dferencal) y el canal que se desea convertr; después de un cclo y medo de reloj del ADC, este empeza a retornar al mcrocontrolador el valor convertdo desde el bt más sgnfcatvo al menos sgnfcatvo. Fgura 28. Dagrama de tempos del ADC externo 66

67 Para alnear el valor convertdo por el ADC se envían dos tramas: en la prmera se envían los tres prmeros bts de control del ADC, esta trama no vara ya que sempre se utlzan los 4 últmos canales. El valor recbdo por el mcrocontrolador después de esta trama no es utlzado, ya que no contene nngún bt del valor de conversón; la segunda trama envada al ADC ndca el canal que se desea convertr. El valor recbdo por el mcrocontrolador después del envó de esta trama es el valor convertdo por el ADC ADC (Analog to Dgtal Converter) El ADC que posee el mcrocontrolador tene una resolucón de 1 bts y es utlzado para convertr el voltaje de los potencómetros que sensan la poscón de cada una de las artculacones que posee la plataforma de demostracón. El ADC está confgurado para trabajar con un formato de datos fracconal con sgno, esto con el fn de evtar la saturacón de los regstros debdo a las multplcacones y sumas que se utlzan para mplementar el control de poscón. El temporzador 3 da nco a la conversón del ADC, determnando el tempo de muestreo utlzado, para obtener las constantes del control de poscón. En la atencón a nterrupcón del ADC se llaman los controles de poscón de cada uno de los motores; es por esto que la prordad de la nterrupcón del ADC debe ser la más alta ya que nunca se debe detener el control de poscón. 67

68 5.1.4 PWM (Pulse Wdth Modulated) El PWM es el encargado de manejar el movmento de los tres motores medante el cambo en su cclo útl, el cual es manejado por el control de poscón, para este fn consta de 6 pnes que se agrupan por parejas (complementaras), salda alta y baja, correspondendo a cada motor una pareja. El PWM está confgurado para trabajar con una base de tempo de 2 Khz y en modo contnuo up-down lo que sgnfca que va a empezar a contar haca arrba contnuamente hasta un valor predetermnado y cuando alcance ese valor empeza a contar haca abajo generando con base en esto, el cclo útl a la salda. Se escogó el modo contnuo updown para evtar que las conmutacones de dferentes referencas se den al msmo tempo Temporzador Temporzador 2 El temporzador dos está confgurado para nterrumpr cada segundo con el fn de que en la atencón a nterrupcón se analcen los sensores de proxmdad medante la funcón Ultrasondos 23, permtendo conocer s exste o no obstáculo alrededor y en que dreccón se encuentra dcho obstáculo. De esta manera, se actualzan las banderas necesaras para realzar las drectvas de manera adecuada y segura para Úrsula y la plataforma de demostracón. Adconalmente se escogó un tempo de 1 segundo, ya que es un tempo prudente al movmento que realza la plataforma de demostracón. 23 Ver anexo 2 Programas 68

69 Temporzador 3 Como se menconó anterormente el temporzador 3 es el encargado de dar nco a la conversón del ADC, es decr, es el encargado de manejar el tempo de muestreo del control, funcón que lo converte en parte esencal, ya que medante la actualzacón de la poscón de los motores, el control decde cuánto error posee la salda y qué tpo de accón se debe tomar para dsmnur el error; el temporzador 3 está confgurado para nterrumpr cada dos mlsegundos, tempo con el que se realzó el cálculo de las constantes del control. 5.2 Drectvas Antes de analzar a fondo cada una de las drectvas, es mportante entender el funconamento global de todo el programa, exsten 4 procesos ndependentes, smultáneos y complementaros que se explcarán a contnuacón: El prmero de ellos es la atencón a nterrupcón de la UART, una vez ncalzados todos los módulos a utlzar se pueden recbr drectvas por medo de ella; en esta atencón se recben y se analzan los datos para determnar qué nstruccón se desea realzar y de acuerdo a esto organzar la nformacón necesara para ejecutarla. S después de dar nco a los módulos no se recbe nnguna drectva la plataforma de pruebas ejecutará sempre la drectva Home. 69

70 El segundo es el programa prncpal el cual sempre se encuentra ejecutando el análss de la drectva, es decr, responde y ejecuta las accones correspondentes a cada una de las drectvas dependendo de la nformacón recbda y organzada por la atencón a nterrupcón de la recepcón de la UART. El tercero es el control de poscón que se ejecuta constantemente dependendo de la poscón fnal que se debe alcanzar, dcha poscón fnal es modfcada en el análss de drectvas que se ejecuta en el programa prncpal. El cuarto proceso es el encargado de actualzar qué obstáculos se encuentran alrededor de la plataforma de demostracón, el cual es mplementado medante el temporzador 2, que cada segundo en la atencón a nterrupcón actualza las banderas necesaras para obtener un conocmento sobre el ambente donde se esta movendo la plataforma Stop Como su nombre lo ndca la fnaldad de esta drectva es detener todos los movmentos que se encuentre realzando la plataforma. Prmero se analza la poscón actual de la plataforma, medante la lectura de las conversones realzadas por el ADC, luego de conocer la poscón actual se le asgna como poscón fnal a cada uno de los motores la poscón en la que se encuentra actualmente, detenendo nmedatamente el movmento que se encontraba realzando. 7

71 A contnuacón, se obtene el valor de la poscón actual, esto con el fn de poder realzar la correspondente cnemátca drecta para que a partr de estos ángulos se pueda obtener la poscón en el espaco en el que se encuentra la plataforma y transmtrla a Úrsula Home La drectva Home modfca en orden consecutvo las poscones fnales de cada uno de los motores llevando a la plataforma de pruebas a una poscón predetermnada llamada Home, es decr, prmero revsa s hay obstáculos a la derecha e zquerda, s no encuentra obstáculos modfca la poscón fnal del motor 1, mponéndole como poscón fnal la que ya está predetermnada como poscón Home. A contnuacón espera a que la plataforma se stué en la poscón pedda, s se encuentra un obstáculo que mpda la contnudad del movmento se detene y envía la nformacón correspondente a Úrsula ; posterormente realza el msmo procedmento para cada uno de los motores revsando desde luego los obstáculos que nterferen en cada movmento, es decr, para la artculacón dos y tres se revsa que no se encuentre obstáculo abajo Poscón absoluta Esta drectva tene como fnaldad ubcar a la plataforma de pruebas en una poscón determnada en el espaco que es escogda por el usuaro y que está lmtada mecáncamente por la plataforma. En esta drectva lo prmero que se necesta hacer es cnemátca nversa de la poscón en el espaco que se desea alcanzar para obtener los ángulos fnales a los que necesta llegar cada motor, estos ángulos son obtendos en 71

72 radanes y por lo tanto para poder utlzarlos es necesaro pasarlos a fracconal debdo a que el control trabaja enteramente con artmétca fracconal. A contnuacón se empezan a modfcar las poscones fnales de los motores uno a uno, revsando antes de ejecutar el movmento los obstáculos en dreccón al movmento. S un obstáculo no le permte llegar a la poscón fnal en alguno de los tres motores, la plataforma se detene utlzando la drectva Stop y se envía la trama correspondente Scan De acuerdo al modelo propuesto en el proyecto de nvestgacón del Ingenero Javer Coronado la estratega de barrdo del terreno se realza por carrles los cuáles están dvddos en cnco celdas 24. De esta manera, la drectva Scan consste en el segumento de una trayectora recta que está dvdda en cnco celdas como se muestra a contnuacón: Efector fnal Fgura 29. Celdas para Scan El recorrdo se realzara partendo de la celda 1 y luego ubcándose en las celdas como se ndca a contnuacón: 2, 3, 2, 1, 4, 5, 4, 1. Como se puede observar este tpo de recorrdo permte stuarse 2 veces sobre cada poscón brndando mayor segurdad al proceso de Scan, aspecto relevante cuando se está analzando un terreno mnado, este recorrdo no se 24 Integracón de Capacdades para la Navegacón de un Robot Móvl. Ingenero Javer Coronado. 72

73 genera en tempo real sno que es almacenado prevamente al funconamento de la plataforma. Esta drectva se mplementó modfcando las poscones fnales de los motores para lograr que se ubquen en las celdas correspondentes, por lo tanto no es necesaro la utlzacón de la cnemátca nversa ya que se almacenan en memora drectamente los ángulos de cada motor correspondentes a cada una de las 5 celdas. Al gual que en las demás drectvas, antes de ejecutar cada movmento analza la presenca de obstáculo antes de moverse, la dferenca es que s encuentra obstáculo en ésta drectva ntenta evtarlo medante una rutna de evasón de obstáculos. 5.3 Control de poscón Con el fn de ejecutar un movmento controlado en la exploracón de mnas antpersonales, se mplementó una malla de control que se encarga de manejar la poscón de la plataforma demostratva generando movmentos en una sola dmensón. Para este control se utlza el mcrocontrolador en modo fracconal, es decr, que trabaja con números entre -1 y 1 que son representados con 16 bts, 1 bt de sgno (número postvo, 1 número negatvo) y 15 de magntud, obtenendo números entre y que medante reglas de tres se nterpretan como números entre -1 y 1 73

74 La malla de control se puede analzar como la suma de los bloques presentados en la fgura 3. Fgura 3. Dagrama en bloques control de poscón PID La escogenca de las constantes del control PID se realzaron con la ayuda de Matlab, utlzando la funcón c2d se dscretzó la planta con un perodo de muestreo de dos mlsegundos y ZOH 25. Luego se utlzó la herramenta rltool por medo de la cual se puede observar el lugar de las raíces de la planta, su respuesta paso y además se puede nclur un compensador para mejorar la respuesta paso a convenenca del dseñador. La respuesta obtenda fnalmente de la planta ncluyendo el compensador se presenta en la fgura 31, en esta se pueden observar puntos relevantes como el sobrepco y el tempo de establecmento. 25 Método de dscretzacón. 74

75 El compensador obtendo fue: Fgura 31. Respuesta de la planta compensada (z-.966) (z-.9) z (z-1) Comparando con la funcón de transferenca de un compensador PID dgtal: PID = Z 2 (K p + K + K d ) + Z(- K p 2 * K d ) + K d (Z - 1)Z Se obtenen las constantes: proporconal (K p ), ntegral (K ) y dervatva (K d ) K p = 818 K = 3 K d = 69 Debdo a que se desea trabajar con artmétca fracconal se le debe hacer un tratamento matemátco adconal a las constantes obtendas ya que su valor está por encma de 1 y como es sabdo en artmétca fracconal se trabaja con números entre -1 y 1 que están representados con 15 bts de magntud y uno de sgno es decr que 1 equvale a y -1 equvale a

76 El número al que más se aproxma la accón proporconal en potencas de dos es a 124 es decr 2 1, al que más se aproxma la accón ntegral es a 4 es decr 2 2 y el número al que más se aproxma la accón dervatva es a 128 es decr 2 7, por lo tanto las constantes obtendas se deben dvdr por dchos valores respectvamente, realzando dcho procedmento se obtenen las sguentes constantes: K p = K =.75 K d =.539 Fnalmente estas constantes se pasan a su representacón fracconal utlzando la sguente regla de 3: Constante X Obtenendo: K p = K = K d = Después de hacer las operacones con las constantes es necesaro realzar un corrmento de 1 haca la zquerda al resultado de la accón proporconal, 2 haca la zquerda al resultado de la accón ntegral y un corrmento de 7 haca la zquerda del resultado de la accón dervatva. Estos corrmentos haca la zquerda representan la multplcacón de las constantes por 2 1, 2 2 y 2 7 respectvamente, cancelando la dvsón que se había realzado anterormente logrando así no afectar los valores de las constantes encontradas medante Matlab. A contnuacón las constantes fueron ajustadas en cada artculacón medante la realzacón de pequeños cambos en los valores de las constantes hasta obtener la respuesta adecuada en el movmento de la plataforma de demostracón. 76

77 5.3.2 PWM Se hablta el PWM para ser manejado de acuerdo a la salda del control PID Puente H El puente H utlzado es el L623 y está confgurado para manejar el motor de acuerdo con el cclo útl que arroja el PWM (ver tabla 2): Cclo Útl del PWM Movmento del Motor % Máxma velocdad haca el uno de los extremos 5% Queto 1% Máxma velocdad haca el extremo contraro Tabla No.2. Confguracón del Puente H Potencómetro El potencómetro está acoplado al eje del motor reflejando de esta forma su poscón por medo de un voltaje. De sus tres termnales, un extremo se encuentra conectado a una fuente de 5V, el otro extremo se encuentra conectado a terra y la termnal del centro se encuentra conectada a la entrada del conversor Análogo Dgtal que está referda a terra. El potencómetro tene una resstenca total de 1K entre los dos extremos y debdo a las lmtacones mecáncas los voltajes obtendos se encuentran en el rango mostrado en la tabla 3 dependendo de la artculacón que se este analzando. Este rango es totalmente lneal en cada una de las artculacones. 77

78 Artculacón Máx. Angulo Voltaje Máx. Angulo Mín. Angulo Voltaje Mín. Angulo Uno Dos Tres Tabla No.3. Rango de voltajes en cada una de las artculacones Temporzador Se confguró el temporzador 3 como un temporzador de 16 bts con el fn de darle nco al conversor análogo-dgtal cada 2ms y así tener el msmo perodo de muestreo en el conversor análogo dgtal, que el que se utlzó en la escogenca de las constantes del control Conversor Análogo Dgtal Se confguró el conversor análogo dgtal de 1 bts de tal forma que la señal de comenzo de conversón la recba del temporzador y además nterrumpa cada vez que acabe de convertr. De esta forma, en la rutna de atencón a nterrupcón, se realza la accón de control con un perodo de muestreo de 2ms Referenca El valor de referenca que se desea que el control alcance debe estar acoplado con el valor que genera el ADC para que de esta forma, se encuentre establdad en el punto deseado, así que se necesta poner una referenca acorde a lo que nterpreta el ADC. 78

79 El ADC puede recbr voltajes entre y 5V y debdo a que está confgurado en modo fracconal el voltaje cero lo nterpreta como su máxmo valor negatvo es decr (-1) y el 5 como su máxmo valor postvo (1), sn embargo el ADC utlza 1 bts y los últmos 6 los fja en cero y debdo a esto el mínmo y máxmo valor que obtene en los 1 bts es -511 y 511 respectvamente, luego al tener en cuenta y agregarle a este valor los últmos 6 bts en cero obtenemos los números reales que nterpreta el ADC para cada voltaje. 5.4 Funcones utlzadas Para la mplementacón de las cuatro drectvas (Home, Stop, poscón absoluta y Scan), se utlzaron las sguentes funcones 26 : Transmsón y recepcón de tramas Scan Para que la plataforma de prueba realce la drectva Scan debe recbr una trama de 8 bts (1 byte), la cual está conformada de la sguente forma: los dos bts más sgnfcatvos ndcan el número de la drectva, es decr, y los tres menos sgnfcatvos ndcan la cantdad de veces que se va a realzar esta drectva, en cuanto a los bts restantes no son relevantes. 26 El número de las drectvas, debe nterpretarse en notacón bnara. 79

80 Por su parte la plataforma de prueba al recbr la drectva Scan envía a Úrsula dos tramas de 8 bts; la prmera trama contene en los dos bts más sgnfcatvos el número de la drectva, en los bts 4, 5 y 6 se coloca el número de celda en que se encuentra, en el bt 3 se determna s la plataforma de prueba se encuentra evadendo un obstáculo o no y los últmos tres bts están vacíos. La segunda trama ndca cuál sensor de proxmdad encontró un obstáculo representado en los cuatro bts más sgnfcatvos, s no hay algún obstáculo los cuatro bts son cero, además s por lmtacones mecáncas no se puede contnuar con la drectva los 4 bts tomarán el valor de 1. En la drectva Scan, la plataforma genera una trayectora predetermnada en línea recta, esta trayectora parte de la poscón Home haca uno de los extremos y de allí se drge haca el otro extremo, cubrendo dos veces el terreno con una msma trayectora Stop1 Esta drectva permte que la plataforma de pruebas se detenga y le envé a la plataforma Úrsula la poscón en la cual termnó. Para que la plataforma de demostracón ejecute esta drectva es necesaro que se le envíe una trama con sus dos bts más sgnfcatvos en 1, sn mportar el contendo de los demás bts. Por su parte, la plataforma de demostracón transmte cuatro bytes donde el prmer byte tene los 2 bts más sgnfcatvos en 1 y en los ses bts restantes agrupados por parejas tene los bts más sgnfcatvos de las poscones (x, y, z) respectvamente; la segunda trama 8

81 ndca los ocho bts menos sgnfcatvos de la poscón x, la tercera trama ndca los bts menos sgnfcatvos de la poscón y, y por últmo la cuarta trama ndca los bts menos sgnfcatvos de la poscón z, estas poscones se obtenen medante el análss de cnemátca drecta Home1 Para ejecutar esta drectva, la plataforma debe recbr de Úrsula una trama de un byte con sus bts más sgnfcatvos en 1. Por otro lado, la plataforma de demostracón envía una trama de 8 bts con los dos prmeros en 1 y en el qunto bt ndca s pudo llegar o no a la poscón Home Poscón Absoluta11 Esta drectva permte que la plataforma de demostracón se ubque en una poscón determnada en su espaco de trabajo; para dcho propósto Úrsula debe envarle una trama de 4 bytes donde en el prmer byte los bts 6 y 7 ndcan el número de la drectva, es decr, 11 y los otros 6 bts agrupados por parejas ndcan los bts más sgnfcatvos de las poscones (x,y,z) respectvamente; la segunda trama ndca los ocho bts menos sgnfcatvos de la poscón x, la tercera trama ndca los 8 bts menos sgnfcatvos de la poscón y y la cuarta trama ndca los 8 bts menos sgnfcatvos de la poscón z, al termnar la ejecucón de esta drectva la plataforma de demostracón envía a Úrsula 1 byte en el cual los bts más sgnfcatvos son 11 y los sguentes cuatro bts ndcan cuál sensor de proxmdad detectó algún obstáculo; en el caso de no encontrar algún obstáculo, 81

82 los cuatro bts son cero, además s por lmtacones mecáncas no se puede cumplr con la drectva se pondrán todos en 1. Para ubcar correctamente a la plataforma de demostracón se utlza el cálculo de la cnemátca nversa para hallar los ángulos necesaros en cada artculacón y alcanzar la poscón deseada. Como resumen de la transmsón y recepcón de tramas de la plataforma de demostracón ver tabla 4. No. Drectva Nombre drectva Recepcón Transmsón Scan #Drectva Vacío # de Scan #Drectva # Celda Evta Vacó Obstáculo. Ultra analzado Vacío Stop Drectva Vacío #Drectva Pos. x Pos. y Pos. z Poscón en x Poscón en y Poscón en z 1 Home 1 1 #Drectva Vacío #Drectva Pos. Alcanzada Vacío Poscón Absoluta # Drectva Pos. x Pos. y Pos. z Poscón en x #Drectva Ultrasondo que senso Poscón en y Tabla No.4. Poscón en z Tramas de comuncacón 82

83 5.4.2 Evasón de obstáculos Para la deteccón de obstáculos se cuenta con tres parejas de ultrasondos ubcados de la sguente forma: dos parejas de sensores se encuentran a la derecha e zquerda del efector fnal y la últma pareja abajo para determnar la proxmdad con el suelo. El manejo de estos sensores se hace por medo un módulo desarrollado por el Ingenero Camlo Otálora, el cual está mplementado en un mcrocontrolador pc16f84 en el que se ncluye la estratega de control para determnar la proxmdad de los obstáculos; este control maneja la emsón y recepcón de la onda de ultrasondos, procesa la señal recbda y tene como salda dgtal una señal en forma de ancho de pulso (PWM) y como salda analógca el fltrado de la señal dgtal por medo de un pasabajos. La emsón de la señal de ultrasondo se hace de forma dferencal por medo de una mplementacón algorítmca realzada en el mcrocontrolador, la señal recbda es amplfcada para mayor facldad de manejo de la onda reflejada y fltrada para contrarrestar los efectos nductvos de la línea de almentacón de 6 Hz; la señal fltrada es pasada a través de un detector de pcos para luego comparar ese nvel pco con un nvel de comparacón fjo que determna la dstanca máxma de deteccón del módulo. El resultado de la comparacón es envado drectamente al controlador, de tal manera que cuando hay señal de alarma (1 lógco), la señal de salda PWM del mcrocontrolador varía de acuerdo a la dstanca a la que haya sdo detectado el obstáculo. 83

84 Cabe anotar que la amplfcacón necesara para cada módulo dependerá drectamente de las característcas de radacón y recepcón de los ultrasondos, en cuánto más selectvos resulten los patrones de radacón de los ultrasondos, mayor podrá ser la gananca de amplfcacón de la señal recbda. La salda dgtal en forma de ancho de pulso (PWM) es proporconal a la dstanca del obstáculo encontrado, aumentándose el ancho de pulso a medda que el obstáculo se encuentra más dstante del ultrasondo receptor. De gual manera, cuanto más lejano se encuentre el obstáculo del receptor, mayor será el nvel promedo encontrado en la salda análoga del módulo 27. Para termnar, es necesaro aclarar que la plataforma de demostracón utlza la señal análoga, con la cual se mplementa la rutna para la evasón de obstáculos. La forma en que fue mplementada dcha rutna se muestra en las fguras 32 a Tomado de: Sstema Móvl para la Deteccón y Localzacón de Mnas Antpersonales. Camlo Campos, Javer Coronado y Javer Rzo

85 *! %( &)' + *.,- 1 2)" # %& /' * 1 %$ )' %$ /' 1 * + 3) + 3),4!,4! %$ )' # # %$ /' 1 1 &!5 3 / ) &!5 3 / / 6& 2) 6& / Fgura 32. Dagrama de flujo de rutna de evasón de obstáculos 85

86 Evaluacón Derecha $ 7 /82)8 9 /2) /89 / $ ) %$ ' $ 71;9 2) %,-' + 3) %&) 9' < &5 3 : /,4! * &!5 3: / 9+ 36),8! 9+ 36),8! Fgura 33. Dagrama de flujo de evaluacón de obstáculos a la derecha 86

87 Evaluacón Izquerda $ 7 /82)8 9 /2) 2)892) $ 7998 / %$ ' $ 71;9 / %,-' + 3) %&) 9' < &5 3 : ),4! * &!5 3: ) 9+ 36),8! 9+ 36),8! Fgura 34. Dagrama de flujo de evaluacón de obstáculos a la zquerda 87

88 Incalmente la rutna, se basa en el análss de la pareja de sensores 28 derecha o zquerda dependendo de haca dónde se esté desplazando la plataforma de demostracón, en el caso de detectar un obstáculo, se realzan los cálculos cnemátcos correspondentes con el fn de desplazar el efector cnco centímetros haca arrba; luego de ubcarse contnua analzando la msma pareja de ultrasondos y s aún detecta el obstáculo contnua desplazándose en la msma dreccón, en el momento que los ultrasondos no detecten nngún obstáculo en la dreccón del movmento, este se desplaza 15cm haca la derecha o zquerda dependendo de haca a donde se estuvera movendo ncalmente 29. Fgura 35. Ubcacón de los ultrasondos en la plataforma de demostracón. Imagen tomada desde la parte nferor de la plataforma Como se puede observar en la fgura 35, las parejas de ultrasondos ubcadas en los extremos se encuentran a una dstanca de 15cm de la pareja de ultrasondos ubcada 28 Tenga en cuenta que las parejas de ultrasondos están ajustadas para tener un rango de alcance de 15cm aproxmadamente para un objeto paralelo a la pareja de ultrasondos. 29 Se realza un movmento de 15cm debdo a la forma geométrca y a la dstanca a la que están ubcados los ultrasondos (ver fgura 32). 88

89 nferormente y por lo tanto es necesaro un movmento de 15cm en la dreccón del movmento antes de encontrar el obstáculo 3. El sguente movmento realzado por la plataforma de demostracón dependerá del análss de la pareja de ultrasondos que se encuentra en la dreccón del movmento y de la pareja nferor, para este análss se utlza una varable que lleva el conteo del número de veces que el efector se desplaza haca arrba, la cual es llamada n, con esta varable se realza el análss de la sguente forma: Obstáculo Derecha-Izquerda-Abajo ó Derecha-Izquerda ó Sentdo del movmento-abajo ó Sentdo del movmento: El efector se desplaza 5cm haca arrba, ncrementa n y vuelve a analzar. Obstáculo Abajo ó Abajo-Dreccón contrara al movmento: Se mueve 5cm en la dreccón del movmento. No hay obstáculo-dreccón contrara al movmento: Se analza la varable n, s no está en cero y es la prmera vez que va a bajar se desplaza 15cm en la dreccón del movmento para lberar al efector del obstáculo, baja 2cm y dsmnuye el valor de n; s no es la prmera vez solo baja 2cm y dsmnuye el valor de n, por otro lado s n 3 Se asegura que durante el movmento de 15cm no se va a encontrar nngún obstáculo debdo al alcance de 15cm de los ultrasondos 89

90 se encuentra en sale de la rutna de evasón de obstáculos llamando a la funcón trayectora Generacón de trayectoras El módulo de generacón de trayectoras modfca la varable de entrada del control de poscón, es decr, la referenca hacendo que el efector fnal sga 5 puntos que forman una línea recta los cuales corresponden a su vez al centro de cada una de las celdas utlzadas para realzar el Scan Lectura de poscón actual Se realza con base en los valores convertdos por el ADC, el cual tene como entrada el voltaje sobre los potencómetros acoplados a los ejes de los motores y por lo tanto tenen un voltaje proporconal a la poscón en la que se encuentran las artculacones Traduccón Se utlzan dos tpos de traduccones la prmera transforma radanes en fracconales y la segunda transforma fracconales en radanes, dchas traduccones son necesaras ya que la cnemátca drecta e nversa trabaja con ángulos en radanes y poscones en centímetros, mentras que el control fue mplementado con base en artmétca fracconal. 31 La funcón trayectora se explcará más adelante 9

91 5.4.6 Cnemátca drecta Para la mplementacón de la cnemátca drecta correspondente a la plataforma de prueba se enumeraron los eslabones de la cadena cnemátca, al gual que las artculacones (ver fgura 18). Después se localzaron los ejes de las coordenadas tenendo en cuenta que el sstema de referenca asocado al eslabón estuvera stuado en un punto que se pudera consderar en certo modo como fn del eslabón. El eje z del sstema de referenca del eslabón, se alneó con el eje de la artculacón +1. El eje x de dcho sstema de referenca se alneó con la normal común entre las artculacones e +1, apuntando de a +1, y por últmo el eje y se establecó para que el sstema de referenca resultara dextrógro. Tenendo en cuenta lo anteror se obtuvo la asgnacón de sstemas de referenca mostrada en la fgura 36. X 3 Z 3 Y 3 Fgura 36. Sstemas de Referenca plataforma de prueba 91

92 Con base en los sstemas de referenca anterormente escogdos se creó la tabla 5 de los parámetros de Denavth y Hartenberg, los cuáles tenen en cuenta los parámetros de poscón relatva del eslabón (θ, d ), los parámetros de tamaño y forma del eslabón (α, a ) y la geometría de la plataforma de prueba. Artculacón θ d α a 1 -Q1 L1 9 2 Q2+9 L2 3 Q3 L3 Tabla No.5. Parámetros de Denavt-Hartenberg Donde L1, L2 y L3 corresponden a la longtud de los eslabones 2, 3 y 4 respectvamente, es decr 16, 23 y 25cm y donde Q1, Q2 y Q3 corresponden al ángulo grado por las artculacones 1, 2 y 3 respectvamente. Medante los parámetros de Denavt- Hartenberg y por medo de transformacones homogéneas que relaconan el sstema de referenca con el sstema de referenca +1 se obtene la sguente matrz homogénea que especfca la localzacón del sstema de coordenadas del extremo del robot con respecto al sstema de coordenadas de la base: 92

93 cos(q1)*(-cos(q2)*sen(q3)-cos(q3)*sen(q2)) cos(q1)*(sen(q2)*sen(q3)-cos(q2)*cos(q3)) -sen(q1) cos(q1)*(-cos(q2)*l3*sen(q3)-cos(q3)*l3*sen(q2)-l2*sen(q2)) cos(q2)*sen(q1)*sen(q3)+cos(q3)*sen(q1)*sen(q2) -sen(q1)*sen(q2)*sen(q3) + cos(q2)*cos(q3)*sen(q1) -cos(q1) cos(q2)*l3*sen(q1)*sen(q3)+cos(q3)*l3*sen(q1)*sen(q2)+l2*sen(q1)*sen(q2) cos(q2)*cos(q3)-sen(q2)*sen(q3) -cos(q2)*sen(q3)-cos(q3)*sen(q2) Cos(Q2)*(cos(Q3)*L3+L2)+L1-L3*sen(Q2)*sen(Q3) 1 Tabla No.6. Matrz homogénea de localzacón del sstema de coordenadas del extremo respecto a la base 93

94 La anteror matrz especfca la poscón y orentacón del extremo de la plataforma de prueba sendo la poscón (x,y,z) representadas respectvamente por las poscones 1-4, 2-4 y 3-4 de la matrz Cnemátca nversa Medante la cnemátca nversa y a partr de la poscón (x,y,z) que se desee alcanzar en el espaco es posble obtener los ángulos necesaros en cada artculacón para alcanzar dcha poscón; exsten varos métodos para resolver el problema cnemátco nverso, el presente trabajo de grado fue desarrollado con una solucón geométrca, la cual consste en descomponer la cadena cnemátca de la plataforma de pruebas en varos planos geométrcos, resolvendo por trgonometría el problema asocado a cada plano. Se escogó la solucón geométrca conservando los ejes coordenados utlzados para la cnemátca drecta debdo a su menor costo de recursos computaconales, además, por medo de esta solucón se obtene una respuesta mucho más exacta y efcente que con la solucón algebraca. El problema se dvdó en dos partes, la prmera parte corresponde a la ubcacón del efector fnal en el plano x-y, es decr, en encontrar el ángulo correspondente a la artculacón 1 32, esto se realzó dvdendo el espaco de la artculacón en dos cuadrantes y dando una solucón para cada cuadrante como se muestra en la fgura llamado Q1 a lo largo del presente documento 94

95 X(-) Segundo Cuadrante Prmer Cuadrante m 1 Q 1 Q 1 m 2 Y(-) Y(+) l 1 l 2 Y(+) Fgura 37. Cnemátca nversa prmer cuadrante S el cuadrante es el prmero, es decr que la poscón en el espaco deseada es negatva en X y postva en Y se utlza y extremo 1 = arctan, por el contraro s el cuadrante es el x extremo Q segundo es decr que la poscón es negatva tanto en X como en Y, se utlza y extremo 1 = arctan, además s la poscón deseada no corresponde a nnguno de los x extremo Q casos anterores se transmte una trama de poscón nalcanzable. La segunda parte del problema consste en hallar los ángulos correspondentes a las artculacones 2 y 3 es decr Q2 y Q3, gualmente este problema se resolvó por medo de trgonometría y dvdendo el espaco en 2 planos coordenados uno para las z mayores que 16cm y otro para las z menores que 16cm, la prmera parte se realzó tomando como base la fgura

96 Z ( -X extremo, Z extremo ) -X Fgura 38. Ángulos 2 y 3 Cnematca Inversa para z>16 Donde Q2 = b c x b = arctan z c = arctan l 2 l 3 + l extremo extremo sn 3 l 1 ( Q3) ( Q3) cos Para obtener Q3 d 2 = 2 2 ( x extremo ) + ( z extremo l ) = l + l cos( Q3) 1 2 ( ) + l 2 sn( Q3) ( x extremo ) + ( z extremo l ) = l + l cos( Q3) + 2 l l cos( Q3) + l 2 sn( Q3)

97 arccos Q3 = 2 ( x extremo ) + ( z extremo l ) 2 l 2 l l 2 2 l 2 3 La segunda parte se realzó tomando en cuenta la fgura 39. Z ( -X extremo, Z extremo ) -X Fgura 39. Ángulos 2 y 3 cnematca nversa para z<16 Donde: d = x 2 extremo + 2 ( l z extremo ) = l sn( Q3) 1 2 ( ) + ( l + l cos( Q3) ) x Q3 = arccos 2 extremo + ( l z extremo ) 1 2 l 2 l 3 2 l 2 3 l 2 2 Para obtener Q2 Q2 = 18 C θ 2 2 r = x extremo + z 2 extremo 97

98 98 ( ) extremo extremo z l x d + = ( ) θ cos = d l l d r ( ) + = * 2 2 arccos extremo extremo extremo z l x l z l l θ ( ) ( ) + = sn arcsn extremo extremo z l x Q l C

99 6 Protocolo de Pruebas y Resultados 6.1 Comuncacón Úrsula vs. Plataforma de demostracón Por medo del programa Com Port Toolkt se determnó el adecuado funconamento de la comuncacón entre Úrsula y la plataforma de demostracón. Las pruebas que se descrben a contnuacón se realzaron envando tramas correspondentes a cada una de las drectvas 33 desde Úrsula haca la plataforma de demostracón, determnando de esta forma que se realzara cada una de las sentencas envadas; en respuesta la plataforma de demostracón envía una determnada cantdad de tramas dependendo de la drectva en que se encuentre y el estado de los sensores de proxmdad que le anunce al usuaro el estado actual del entorno de la plataforma de demostracón Transmsón de tramas por parte de la plataforma de pruebas En esta prmera etapa de pruebas la trama envada por Úrsula era ndferente para la plataforma de demostracón, ya que lo únco que se desea analzar es el correcto envó de cada una de las tramas de las drectvas por parte de la plataforma, es decr, solo necesta atender la atencón de recepcón de la UART para envar la trama correspondente a la drectva que se estaba analzando en el momento. Los valores de las varables utlzadas en 99

100 la trama fueron fjados en la plataforma para analzar úncamente el correcto envó y análss de la trama Drectva Scan: Incalmente se le asgnó a la plataforma el número de celda 1 sn colocarle obstáculos alrededor y encontrándose en una poscón alcanzable. La Parte derecha fgura 4 muestra la varacón de la segunda trama de la drectva Scan, es decr, la varacón de lectura de los ultrasondos. Sn obstáculo Obstáculo a la Derecha Obstáculo en la Izquerda Obstáculo Derecha e Izquerda Fgura 4. Prueba sobre la drectva Scan Celda 1 33 Capítulo 5.2 1

101 En la sguente prueba se le asgnó a la plataforma el número de celda 3, se encuentra en una poscón nalcanzable y evtando un obstáculo. La parte derecha de la fgura 41 muestra la varacón en la prmera y segunda trama de la drectva Scan con respecto a la anteror. Fgura 41. Prueba sobre la drectva Scan Celda Drectva Stop: A contnuacón se muestra la trama correspondente a la drectva Stop con las sguentes poscones: X = -86cm, Y = -16cm, Z = -17cm respectvamente (ver fgura 42): Fgura 42. Prueba sobre la drectva Stop Drectva Home La fgura 43 muestra la trama de la drectva Home cuando esta ha sdo alcanzada. 11

102 Fgura 43. Prueba sobre la drectva Home Sn obstaculos. La otra posbldad que se da en la trama de esta drectva es cuando la poscón no fue alcanzada, es decr, se encontró obstáculo. Fgura 44. Prueba sobre la drectva Home Con obstaculos Poscón Absoluta A contnuacón se muestra la trama de la drectva poscón absoluta cuando su poscón es nalcanzable debdo a obstáculos, en este caso tene obstáculo a la derecha y la zquerda. Fgura 45. Prueba sobre la drectva poscon absoluta. 12

103 Recepcón de tramas Esta prueba se dseño para comprobar que la plataforma esté recbendo y analzando correctamente los datos envados por Úrsula, por lo tanto se envía la trama correspondente de la drectva que se desea probar y la plataforma transmte cada uno de los parámetros que le fueron envados alneados a la derecha Drectva Scan En la fgura 46 se muestra el llamado a la drectva Scan, en el prmer llamado el número de Scan solctado es 7 y en el segundo es 1, por lo tanto el la plataforma de demostracón transmte el número de la drectva y el número de Scan solctado: Fgura 46. Prueba sobre la drectva Scan Recepcon de datos Drectva Stop En esta prueba se le envía a la plataforma la drectva Stop. 13

104 Fgura 47. Prueba sobre la drectva Stop Recepcon de datos Drectva Home: En esta prueba se envía la trama correspondente a la drectva Home y se transmte el número de la drectva solctada alneada a la derecha: Fgura 48. Prueba sobre la drectva Home Recepcon de datos Drectva poscón absoluta En esta prueba se envaron las cuatro tramas correspondentes 34 a la drectva poscón absoluta, en respuesta la plataforma transmte el número de drectva y a contnuacón los 8 bts más sgnfcatvos de cada una de las poscones envadas 35 : 34 Ver seccón Las poscones envadas son de 1 bts. 14

105 Fgura 49. Prueba sobre la drectva poscon absoluta Recepcon de datos. Adconalmente, se mplemento una nterfaz gráfca (ver fgura 5) en Vsual Basc, la cual cumple todas las funcones de comuncacón de Úrsula, es decr, por medo de la nformacón que ngresa el usuaro es capaz de envar las tramas correspondentes a cada drectva y además de nterpretar las tramas recbdas de la plataforma de demostracón, esta comuncacón se realza utlzando el puerto seral Fgura 5. Interfaz gráfca 15