Upload
dangcong
View
227
Download
0
Embed Size (px)
Citation preview
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
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
requisito parcial para optar al título de
Ingeniero Electrónico.
Director:
Carlos Alberto Parra R. Ph.D.
Ingeniero Electrónico.
PONTIFICIA UNIVERSIDAD JAVERIANA
FACULTAD DE INGENIERÍA
DEPARTAMENTO DE ELECTRÓNICA
BOGOTÁ D.C.
2005
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.
4
“No éramos, ni somos imprescindibles, pero si pusimos entonces y ponemos ahora el esfuerzo de una vida,
para el ensanche de la fe, de la ciencia y del amor a los más altos valores del espíritu”.
Padre Jose Gabriel Maldonado S.J.
5
Por ti... Colombia...
6
AGRADECIMIENTOS
Al Señor Don Carlos por acompañarnos a lo largo de este camino ...por sus regaños sutiles ...por ser antes que nuestro director, nuestro amigo...por sus palabras de animo que nunca
nos dejaron desfallecer...por mostrarnos el mundo de una manera diferente...por sus sonrisas sinceras...por unos maravillosos abrazos que algunas veces nos hicieron llorar...Te
vamos a extrañar.
A una gran persona, que nos aguanto...que nos dedicó todo el tiempo sin límites y sin esperar nada a cambio, que siempre nos recibió con su sonrisa de niño...que nuca dudo de nosotras...y sobretodo que nunca nos abandono en los momentos más difíciles .....Al Señor
Freddy
A Camilo, por sus sabios consejos en el tiempo preciso...por su inconfundible sonrisa en momentos de desesperación, por regalarnos un poquito de su conocimiento
A Gabriel Perilla y José I. por escucharnos...por estar siempre pendientes de nosotras y por animarnos a seguir adelante
A Caliche, Leo y Marlon por su paciencia... por su colaboración... y por su amabilidad durante toda nuestra carrera.
Al capi José por preocuparse por nosotras... por recibirnos siempre de la mejor manera...por sus inconfundibles muecas...por el tinto con panela.
7
A Dios por estar junto a mí siempre sin condiciones ni límites.
A mis Padres por ser mi guía, mi apoyo, mi ejemplo, pero por encima de todo por brindarme su amor constantemente. Hoy levanto las manos de triunfo, un triunfo que en gran parte es de ustedes
y por ustedes, por un sacrificio que a veces pareció locura y hoy se hace realidad a pesar de todo. Es por eso que todo lo que soy es gracias a ustedes a sus ganas y fortaleza. Miles de gracias nunca
serán suficientes para ustedes.
A mis hermanitos Andrius y Jaimitur por brindarme esos abrazos fuertes, los besos más sinceros, las mejores sonrisas, y porque no, por aguantarme. A Andrius por sus ataques de risa, por esos
inconfundibles apodos, por sus juegos de aia y por consentirme. A Jaimitur, mi futuro Ingeniero, por sus apuntes locos, por sus sabios regaños, por los mejores
abrazos y por confiar en mi.
A la persona que antes que todo siempre ha sido mi amigo, mi consejero, mi apoyo, mi soporte: Mauri. Gracias por tu valiosa compañía en un poco más de la mitad de mi carrera, por ponerle un
tono diferente a este camino. Por tu amor fundamental en este reto que hoy tiene fin pero que no es el único, todavía siguen muchos más….
A mis abuelitos maternos que están en el cielo, los cuales siempre me cuidan y a mi abuelita Marta por sus sabios consejos, su apoyo y esa experiencia que solo tienen mis viejos. Por darme los
mejores padres del mundo.
A una amiga incondicional, Julie que a pesar del poco tiempo que pudimos compartir siempre estuvo conmigo, nunca se alejo y me brindo maravillosas sonrisas en el momento indicado con las
palabras indicadas.
Y como no, a mi gran compañera de trabajo, mi amiga, mi hermana, mi hombro en este lugar, mi sombra y mi luz: a Marce, por emprender este camino loco que nunca nos imaginamos y hoy
terminamos. Por una amistad espero nunca se acabe.
Johanna Carolina
8
A Dios por darme la vida… por rodearme de personas maravillosas, por darme un corazón lleno de sueños y esperanzas…por todas las oportunidades que me brinda.
A mis Padres… por su amor incondicional, por apoyarme en los momentos más difíciles… por pensar en mi bienestar antes que en el de ellos, por estar ahí siempre que los he
necesitado…por ser ellos mi tierra firme, mi guía y mi ejemplo, por sus consejos que me han hecho ser quien soy ....porque siempre estaremos juntos.
A Sandri… por su amor y por su confianza en mi…porque por ella quiero ser mejor persona y hacer del mundo un sitio en el que los sueños no tengan límites…por aguantarme
todos estos años, por sus abrazos, por sus palabras por apoyarme en este camino que termina y porque se que siempre estará en los que ahora comienzan.
A mi nonita por su amor y confianza por ayudarme a ser quien soy…por sus palabras de animo cuando más las necesito, por sus apapachos, por sus consejos…porque aunque lejos
siempre estará presente en donde yo este.
A Santi por ser mi amigo, mi inspiración, mi cómplice, por todo el amor que me brinda día a día sin esperar nada a cambio, por sus besos, por acompañarme en este camino, por mostrarme cosas de mí que ni yo conocía, por ayudarme a crecer y por crecer junto a
mi….por mostrarme un mundo mas hermoso del que nunca había conocido, por ser mi eterno sueño.
A Joha… por ser la mejor de las amigas, por escucharme, por comprenderme, por aguantar mis mal genios…por sus palabras, por sus abrazos…por su optimismo…por sus
consejos…por ser mi compañera de batallas, por confiar en mí, espero que la vida nos permita seguir compartiendo.
Marcela
9
TABLA DE CONTENIDO 1 INTRODUCCIÓN ....................................................................................................... 16
2 MARCO TEÓRICO..................................................................................................... 19
2.1 Brazo Robot.................................................................................................................... 20 2.1.1 Componentes y subsistemas de un brazo robot ........................................................................ 20
2.2 Fundamentos matemáticos y físicos en robótica ......................................................... 22 2.2.1 Descripción de posición y orientación...................................................................................... 22 2.2.2 Sistemas de referencia ............................................................................................................. 22 2.2.3 Descripción de la posición........................................................................................................ 22
2.2.3.1 Coordenadas cartesianas ................................................................................................. 23 2.2.3.2 Coordenadas cilíndricas .................................................................................................. 24 2.2.3.3 Coordenadas esféricas..................................................................................................... 24
2.2.4 Descripción de la orientación ................................................................................................... 25 2.2.4.1 Matrices de rotación (respecto a uno de los ejes de referencia) ...................................... 26
2.2.5 Matrices y coordenadas homogéneas ....................................................................................... 27 2.2.6 Matriz homogénea de transformación inversa.......................................................................... 28
2.3 Transformaciones básicas: Translación y Rotación ................................................... 28 2.3.1 Translación ............................................................................................................................... 28 2.3.2 Rotación.................................................................................................................................... 29 2.3.3 Composición de transformaciones............................................................................................ 30
2.3.3.1 Rotaciones compuestas ................................................................................................... 32 2.3.3.2 Matrices de rotación respecto de un eje arbitrario........................................................... 34
2.4 Cinemática...................................................................................................................... 35 2.4.1 Espacio articular y espacio cartesiano ...................................................................................... 36 2.4.2 El problema cinemático directo ................................................................................................ 37
2.4.2.1 Parámetros de Denavit-Hartenberg ................................................................................. 39 2.4.2.2 Asignación de sistemas de referencia.............................................................................. 41
2.4.3 Transformación homogénea ..................................................................................................... 44 2.4.4 Problema cinemático inverso.................................................................................................... 46
2.4.4.1 Métodos de solución ....................................................................................................... 48
3 Plataforma de Prueba.................................................................................................. 49
3.1 Diseño y construcción ................................................................................................... 50
3.2 Especificaciones.............................................................................................................. 52
4 Tarjeta de Desarrollo................................................................................................... 55
4.1 Especificaciones.............................................................................................................. 55
5 IMPLEMENTACIÓN ................................................................................................. 57
5.1 Inicio de módulos y variables........................................................................................ 63 5.1.1 UART (UNIVERSAL ASYNCHRONOUS RECEIVER TRANSMITTER) .......................... 64 5.1.2 SPI (Serial Peripheral Interface) ............................................................................................... 65 5.1.3 ADC (Analog to Digital Converter) ........................................................................................ 67 5.1.4 PWM (Pulse Width Modulated) ............................................................................................... 68
10
5.1.5 Temporizador ........................................................................................................................... 68 5.1.5.1 Temporizador 2 ............................................................................................................... 68 5.1.5.2 Temporizador 3 ............................................................................................................... 69
5.2 Directivas ........................................................................................................................ 69 5.2.1 Stop........................................................................................................................................... 70 5.2.2 Home ........................................................................................................................................ 71 5.2.3 Posición absoluta ...................................................................................................................... 71 5.2.4 Scan .......................................................................................................................................... 72
5.3 Control de posición ........................................................................................................ 73 5.3.1 PID............................................................................................................................................ 74 5.3.2 PWM ........................................................................................................................................ 77 5.3.3 Puente H ................................................................................................................................... 77 5.3.4 Potenciómetro........................................................................................................................... 77 5.3.5 Temporizador ........................................................................................................................... 78 5.3.6 Conversor Análogo Digital....................................................................................................... 78 5.3.7 Referencia................................................................................................................................. 78
5.4 Funciones utilizadas....................................................................................................... 79 5.4.1 Transmisión y recepción de tramas .......................................................................................... 79
5.4.1.1 Scan�00......................................................................................................................... 79 5.4.1.2 Stop�01 ......................................................................................................................... 80 5.4.1.3 Home�10....................................................................................................................... 81 5.4.1.4 Posición Absoluta�11 ................................................................................................... 81
5.4.2 Evasión de obstáculos.............................................................................................................. 83 5.4.3 Generación de trayectorias ....................................................................................................... 90 5.4.4 Lectura de posición actual ........................................................................................................ 90 5.4.5 Traducción................................................................................................................................ 90 5.4.6 Cinemática directa .................................................................................................................... 91 5.4.7 Cinemática inversa ................................................................................................................... 94
6 Protocolo de Pruebas y Resultados ............................................................................. 99
6.1 Comunicación................................................................................................................. 99 6.1.1 “Úrsula” vs. Plataforma de demostración................................................................................. 99
6.1.1.1 Transmisión de tramas por parte de la plataforma de pruebas ........................................ 99 6.1.1.2 Recepción de tramas ..................................................................................................... 103
6.1.2 ADC-Externo vs. Plataforma de pruebas................................................................................ 106
6.2 Prueba Control de Posición ........................................................................................ 111 6.2.1 Sintonización de Controladores .............................................................................................. 112 6.2.2 Error en cada posicionamiento ............................................................................................... 113
6.3 Directivas ...................................................................................................................... 119 6.3.1 Directiva Home ...................................................................................................................... 119 6.3.2 Directiva Stop ......................................................................................................................... 119 6.3.3 Directiva posición absoluta..................................................................................................... 120 6.3.4 Directiva Scan ........................................................................................................................ 123
7 Posibles Mejoras ........................................................................................................ 126
8 Conclusiones .............................................................................................................. 127
9 Glosario...................................................................................................................... 129
11
10 Bibliografía ................................................................................................................ 131
12
LISTA DE FIGURAS
FIGURA 1. TRASLACIONES Y ROTACIONES BÁSICAS ................................................................................... 21 FIGURA 2. EJEMPLO DE ARTICULACIONES DE 1 Y 2 GRADOS DE LIBERTAD ................................................. 21 FIGURA 3. SISTEMAS DE REFERENCIAS DE COORDENADAS ........................................................................ 22 FIGURA 4. DESCRIPCIÓN DE LA POSICIÓN ................................................................................................... 23 FIGURA 5. POSICIÓN DE UN SISTEMA O RESPECTO A OTRO M..................................................................... 23 FIGURA 6. COORDENADAS CARTESIANAS .................................................................................................. 24 FIGURA 7. COORDENADAS CILINDRICAS .................................................................................................... 24 FIGURA 8. COORDENADAS ESFÉRICAS ....................................................................................................... 25 FIGURA 9. SISTEMAS DE REFERENCIA COINCIDENTES EN EL ORIGEN .......................................................... 25 FIGURA 10. TRANSLACIONES BASICAS ......................................................................................................... 29 FIGURA 11. EJEMPLO TRANSLACIÓN Y ROTACIÓN ....................................................................................... 32 FIGURA 12. ARTICULACIONES ...................................................................................................................... 36 FIGURA 13. PROBLEMA CINEMATICO DIRECTO ............................................................................................ 38 FIGURA 14. PARAMETROS DE DENAVIT-HARTENBERG ................................................................................ 39 FIGURA 15. ENUMERACIÓN DE ESLABONES Y ARTICULACIONES .................................................................. 42 FIGURA 16. EJES COORDENADOS ................................................................................................................. 42 FIGURA 17. TRANSFORMACIÓN HOMÓGENEA .............................................................................................. 45 FIGURA 18. PARTES DE LA PLATAFORMA DE DEMOSTRACIÓN ...................................................................... 51 FIGURA 19. PLATAFORMA DE DEMOSTRACIÒN ............................................................................................. 52 FIGURA 20. UBICACIÓN EN EL PLANO X-Y DE LA PLATAFORMA DE PRUEBA OBSERVADA DESDE ARRIBA .... 53 FIGURA 21. DIAGRAMA EN BLOQUES TARJETA DE DESARROLLO .................................................................. 55 FIGURA 22. DIAGRAMA DE PINES MICROCONTROLADOR .............................................................................. 56 FIGURA 23. DIAGRAMA DE FLUJO INICIALIZACIÓN DEL PROGRAMA ............................................................. 59 FIGURA 24. DIAGRAMA DE FLUJO DIRECTIVA SCAN ..................................................................................... 60 FIGURA 25. DIAGRAMA DE FLUJO DIRECTIVA POSICIÓN ABSOLUTA ............................................................. 61 FIGURA 26. DIAGRAMA DE FLUJO DIRECTIVA HOME .................................................................................... 62 FIGURA 27. DIAGRAMA DE FLUJO DIRECTIVA STOP...................................................................................... 63 FIGURA 28. DIAGRAMA DE TIEMPOS DEL ADC EXTERNO............................................................................. 66 FIGURA 29. CELDAS PARA SCAN .................................................................................................................. 72 FIGURA 30. DIAGRAMA EN BLOQUES CONTROL DE POSICIÓN ....................................................................... 74 FIGURA 31. RESPUESTA DE LA PLANTA COMPENSADA.................................................................................. 75 FIGURA 32. DIAGRAMA DE FLUJO DE RUTINA DE EVASIÓN DE OBSTÁCULOS ................................................ 85 FIGURA 33. DIAGRAMA DE FLUJO DE EVALUACIÓN DE OBSTÁCULOS A LA DERECHA ................................... 86 FIGURA 34. DIAGRAMA DE FLUJO DE EVALUACIÓN DE OBSTÁCULOS A LA IZQUIERDA ................................. 87 FIGURA 35. UBICACIÓN DE LOS ULTRASONIDOS EN LA PLATAFORMA DE DEMOSTRACIÓN. IMAGEN TOMADA
DESDE LA PARTE INFERIOR DE LA PLATAFORMA ........................................................................ 88 FIGURA 36. SISTEMAS DE REFERENCIA PLATAFORMA DE PRUEBA................................................................ 91 FIGURA 37. CINEMÁTICA INVERSA PRIMER CUADRANTE .............................................................................. 95 FIGURA 38. ÁNGULOS 2 Y 3 CINEMATICA INVERSA PARA Z>16 ................................................................... 96 FIGURA 39. ÁNGULOS 2 Y 3 CINEMATICA INVERSA PARA Z<16 .................................................................... 97 FIGURA 40. PRUEBA SOBRE LA DIRECTIVA SCAN “CELDA 1” ..................................................................... 100 FIGURA 41. PRUEBA SOBRE LA DIRECTIVA SCAN “CELDA 3” ..................................................................... 101 FIGURA 42. PRUEBA SOBRE LA DIRECTIVA STOP. ....................................................................................... 101 FIGURA 43. PRUEBA SOBRE LA DIRECTIVA HOME”SIN OBSTACULOS” . ...................................................... 102 FIGURA 44. PRUEBA SOBRE LA DIRECTIVA HOME “CON OBSTACULOS”. .................................................... 102 FIGURA 45. PRUEBA SOBRE LA DIRECTIVA POSICION ABSOLUTA. ............................................................... 102
13
FIGURA 46. PRUEBA SOBRE LA DIRECTIVA SCAN “RECEPCION DE DATOS”. ............................................... 103 FIGURA 47. PRUEBA SOBRE LA DIRECTIVA STOP “RECEPCION DE DATOS”. ................................................ 104 FIGURA 48. PRUEBA SOBRE LA DIRECTIVA HOME “RECEPCION DE DATOS”. .............................................. 104 FIGURA 49. PRUEBA SOBRE LA DIRECTIVA POSICION ABSOLUTA “RECEPCION DE DATOS”. ........................ 105 FIGURA 50. INTERFAZ GRÁFICA.................................................................................................................. 105 FIGURA 51. PRUEBA “DIFERENTES DISTANCIAS SOBRE EL MISMO MATERIAL”. ......................................... 107 FIGURA 52. PRUEBA “DIFERENTES DISTANCIAS – DISTINTAS FORMAS ”. ................................................... 107 FIGURA 53. PRUEBA “DIFERENTES DISTANCIAS – SUPERFICIE PLANA ”. .................................................... 108 FIGURA 54. PRUEBA “DIFERENTES DISTANCIAS – SUPERFICIE CONCAVA ”. ............................................... 108 FIGURA 55. PRUEBA “DIFERENTES DISTANCIAS – SUPERFICIE CONVEXA ”. ............................................... 109 FIGURA 56. PRUEBA “DIFERENTES DISTANCIAS – DISTINTOS MATERIALES ”. ............................................ 109 FIGURA 57. PRUEBA “DIFERENTES DISTANCIAS – DISTINTOS MATERIALES ”. ............................................ 110 FIGURA 58. PRUEBA “DIFERENTES DISTANCIAS – DISTINTOS MATERIALES ”. ............................................ 111 FIGURA 59. ANGULOS MEDIDOS EN LA ARTICULACION 1............................................................................ 114 FIGURA 60. GRÁFICA DE ERROR DE ÁNGULO SOBRE LA ARTICULACION 1. ................................................. 114 FIGURA 61. ANGULO MEDIDOS SOBRE LA ARTICULACION 2....................................................................... 115 FIGURA 62. GRÁFICA DE ERROR DE ÁNGULO SOBRE LA ARTICULACION 2. ................................................. 116 FIGURA 63. ANGULOS MEDIDOS SOBRE LA ARTICULACION 3..................................................................... 117 FIGURA 64. GRÁFICA DE ERROR DE ÁNGULO SOBRE LA ARTICULACION 3. ................................................. 117 FIGURA 65. RESULTADOS POSICIÓN X ........................................................................................................ 120 FIGURA 66. RESULTADOS POSICIÓN Y ........................................................................................................ 121 FIGURA 67. RESULTADOS POSICIÓN Z......................................................................................................... 121 FIGURA 68. OBSTÁCULO NO 1.................................................................................................................... 124 FIGURA 69. OBSTÁCULO NO 2.................................................................................................................... 124
14
LISTA DE TABLAS
TABLA NO.1. REPRESENTACIONES DE ÁNGULOS DE EULER........................................................................ 33 TABLA NO.2. CONFIGURACIÓN DEL PUENTE H .......................................................................................... 77 TABLA NO.3. RANGO DE VOLTAJES EN CADA UNA DE LAS ARTICULACIONES ............................................. 78 TABLA NO.4. TRAMAS DE COMUNICACIÓN................................................................................................. 82 TABLA NO.5. PARÁMETROS DE DENAVIT-HARTENBERG............................................................................ 92 TABLA NO.6. MATRIZ HOMOGÉNEA DE LOCALIZACIÓN DEL SISTEMA DE COORDENADAS DEL EXTREMO
RESPECTO A LA BASE ............................................................................................................ 93 TABLA NO.7. CONSTANTES IMPLEMENTADAS EN CADA UNA DE LAS ARTICULACIONES ........................... 112 TABLA NO.8. POSICIONES (X,Y,Z) SEGUIDAS POR LA TRAYECTORIA RECTILÍNEA ..................................... 123
15
LISTA DE ANEXOS
Anexo 1. Tablas de error en la directiva posición absoluta Anexo 2. Generación de Trayectorias Anexo 3. Programas Desarrollados Anexo 4. Hojas de Especificaciones
16
1 INTRODUCCIÓN
La detección y desactivación de minas antipersonales es uno de los temas más
trascendentales y de mayor incidencia sobre la población colombiana, puesto que puede
herir o causar la muerte de una o varias personas sin discriminar a la víctima, la cual
generalmente no está involucrada en el conflicto armado. Según el Comité Internacional de
la Cruz Roja (CICR) se calcula que mensualmente 800 personas (26 por día) pierden la
vida a causa de las minas antipersonales; el Departamento de Estado de los Estados Unidos
indica que cada año hay aproximadamente 26.000 (entre muertos y heridos, 72 víctimas
diarias) personas afectadas por este problema, por otro lado la revista IDOC Internazionale,
afirma que por cada víctima que sobrevive a la explosión de una mina dos fallecen, y que
en algunos países el 75% de los sobrevivientes requiere amputaciones[1].
Al mirar la situación actual en Colombia, el tema es uno de los más delicados a nivel
mundial, puesto que la presencia de campos minados ha venido creciendo en lugar de
disminuir, entre 1990 y junio de 2002, 1.634 personas fueron víctimas de este flagelo en su
mayoría niños y jóvenes, y aun quedan 70.000 m² de minas sembradas en áreas de alto
riesgo para los civiles, incluyendo zonas cercanas a escuelas, poblaciones y caminos; por
otro lado hacer una mina cuesta entre 3 y 10 dólares a los grupos al margen de la ley y
desactivarla le puede costar al gobierno entre 400 y 800 dólares, por lo tanto al sumar todos
17
estos problemas es necesario implementar una serie de métodos capaces de detectar y
desactivar estos artefactos de manera segura[2][3].
Es indudable que para lograr mitigar el problema y su impacto sobre la población civil y
militar, se deben mejorar y/o tecnificar los métodos de detección de minas y la estrategia de
desminado y así disminuir o hasta eliminar por completo esta amenaza.
Debido a esta situación, el objetivo del presente trabajo de grado fue el desarrollo de un
prototipo que ayude a la labor de detección de minas antipersonales, perfeccionando la
estrategia usada en el proyecto “Úrsula” por medio de un sistema de control que manipula
un brazo mecánico con tres grados de libertad y cuyas instrucciones de movimiento
protegen la integridad de la plataforma móvil y le brindan una mayor longitud de alcance
de exploración buscando una precisión mayor en el sistema de detección de minas. Las
validaciones de los métodos de control y movimiento se realizaron con una plataforma de
demostración.
Este documento esta organizado en 8 capítulos de la siguiente forma: en el capítulo 2 se
explican los conceptos básicos para la consecución del control de la plataforma móvil, en el
capítulo tres se dan a conocer las especificaciones mecánicas de la plataforma así como la
configuración de su espacio de trabajo, el capítulo cuatro muestra las especificaciones de la
tarjeta de desarrollo, en el capitulo 5 se presentan los módulos utilizados en el desarrollo
del proyecto, la implementación del control de posición y de cada una de las directivas a
18
ejecutar, describiendo además las funciones creadas para el desarrollo total del proyecto. El
protocolo de pruebas y los resultados obtenidos en cada una de ellas son presentados en el
capítulo seis. Este análisis está dividido en tres partes: la comunicación entre la plataforma
móvil y el usuario, el control de posición y por último cada una de las directivas.
Para dejar abierto un campo de investigación y/o desarrollo para que otros investigadores
sigan aportando e innovando partes o procesos sobre la plataforma de demostración, se
incluyó en el presente trabajo en el séptimo capítulo las posibles mejoras las cuales se
pueden realizar de acuerdo a las evaluaciones y resultados obtenidos, por último en el
capítulo 8 se muestran las conclusiones obtenidas del desarrollo del proyecto.
19
2 MARCO TEÓRICO El presente marco teórico ha sido tomado de las referencias bibliográficas nombradas a
continuación1:
� Robots y Sistemas sensoriales. Autores: Fernando Torres, Jorge Pomares, Pablo Gil,
Santiago T. Puente, Rafael Aracil. Editorial Prentice Hall, 2002.
� Robótica: Control, Detección, Visión e inteligencia. Autores: K.S.FU, R.C
González, C.S.G Le. Editorial McGraw-Hill, 1.988.
� Exact Solution to the inverse Kinematics of a standard 6-axis Robot Manipulator.
Autores: M. Shahinpoor, M Jamshidi and Young T. Kim
Los temas principales presentados en este capítulo son: matriz de transformación
homogénea, translaciones básicas y compuestas sobre los ejes principales de un sistema de
referencia, rotaciones básicas y compuestas con respecto a cada uno de los principales ejes
de un sistema de referencia, así como, el análisis del problema cinemático directo e inverso
de un brazo robot. Si el lector conoce y domina los temas anteriormente nombrados puede
pasar directamente al capítulo 3 en el que se tratará la implementación del presente
proyecto.
1 Estas referencias bibliográficas se encuentran incluidas en la bibliografía del documento
20
2.1 Brazo Robot
Mecánicamente, un brazo robot está diseñado para alcanzar un lugar específico localizado
dentro de su volumen de trabajo representado por una esfera de influencia, el cual se
determina de acuerdo con los grados de libertad que posea; de acuerdo con estos grados de
libertad se pueden generar una serie de combinaciones de movimientos con el fin de
posicionar su efector final en el lugar que necesita alcanzar.
2.1.1 Componentes y subsistemas de un brazo robot
La estructura mecánica básica de un brazo robot está compuesta por eslabones, que
cumplen una función similar a la de un hueso; accionadores, de funcionalidad parecida a la
de un músculo; transmisiones, con cierto parecido a los tendones; y los cables de señal, en
cierto modo los nervios del robot. Los puntos de unión entre eslabones reciben el nombre
de nodos y el elemento que permite dicha unión y un movimiento relativo entre ellos, al
igual que en el caso de un brazo humano, es la articulación.
La capacidad de carga depende del dimensionamiento y características estructurales de los
eslabones, sistemas de transmisión y accionadores.
En general, un cuerpo rígido puede realizar un movimiento muy complejo en el espacio
tridimensional que es difícil describir a priori; sin embargo, ese movimiento puede ser
considerado como una combinación de movimientos de translación y rotación que a su vez
21
pueden ser consideradas como un movimiento compuesto por componentes de translación a
lo largo de uno o más de los ejes de coordenadas. De la misma forma, una rotación puede
ser considerada como aquella cuyas componentes reflejan rotaciones producidas en torno a
los ejes coordenados como lo describe la figura 1.
Figura 1. Traslaciones y rotaciones básicas2
Un cuerpo libre en el espacio puede moverse en tres direcciones independientes y
perpendiculares entre sí con la posibilidad de rotar en torno a esas mismas direcciones, es
por ello que se dice que posee seis grados de libertad. Ejemplo: Si una articulación está
limitada a moverse en un plano, posee dos grados de libertad (ver figura 2).
Figura 2. Ejemplo de articulaciones de 1 y 2 grados de libertad
2 Todas las figuras de esté capítulo son tomadas de Robots y Sistemas sensoriales. Autores: Fernando Torres, Jorge Pomares, Pablo Gil, Santiago T. Puente, Rafael Aracil. Editorial Prentice Hall, 2002.
22
2.2 Fundamentos matemáticos y físicos en robótica 2.2.1 Descripción de posición y orientación
Para poder efectuar tareas con un robot, es necesario establecer claramente la forma de
describir la posición y la orientación de un cuerpo rígido en el espacio, es decir, localizar
adecuadamente al robot en un sistema de coordenadas definido.
2.2.2 Sistemas de referencia
Es muy importante un sistema de referencia puesto que un cuerpo rígido mantiene la
relación entre éste y el objeto; los sistemas de referencia se definen y representan
habitualmente mediante ejes ortogonales que definen a su vez la intersección entre tres
planos ortogonales. Normalmente se utiliza un sistema dextrógiro, como se muestra en la
figura 3.
Figura 3. Sistemas de Referencias de Coordenadas
2.2.3 Descripción de la posición.
Generalmente un robot ha de ser referenciado en el espacio tridimensional, en este caso,
una posición se establece de forma unívoca mediante un vector de posición Mp con tres
componentes respecto a un sistema de referencia M.
23
Figura 4. Descripción de la posición
Al tener asociado un objeto de interés un sistema de referencia O, el vectorMOp representa
la posición del origen de dicho sistema O con respecto al M.
Figura 5. Posición de un sistema O respecto a otro M 2.2.3.1 Coordenadas cartesianas
Empleando este tipo de coordenadas, las componentes del vector Mp son las proyecciones
sobre cada uno de los ejes del sistema de referencia M. Una posición en el espacio
tridimensional se representa como Mp (x, y, z).
24
Figura 6. Coordenadas Cartesianas 2.2.3.2 Coordenadas cilíndricas
Una posición en el espacio tridimensional se representa como Mp (r,θ, z). Las componentes
del vector Mp en un sistema de referencia M corresponden, al módulo de la proyección del
vector Mp sobre el plano xy, el ángulo que forma dicha proyección con el eje x y la
proyección del vector Mp sobre el eje z, respectivamente.
Figura 7. Coordenadas Cilindricas
2.2.3.3 Coordenadas esféricas
La primera y segunda componente del vector Mp es la misma que en coordenadas
cilíndricas. La tercera componente en este caso corresponde con el ángulo que forma el
25
vector Mp con el eje z del sistema de referencia M. Una posición en el espacio
tridimensional se representa como Mp (r,θ,φ) (ver figura 8).
Figura 8. Coordenadas Esféricas 2.2.4 Descripción de la orientación
Para localizar totalmente un cuerpo en el espacio es necesario conocer, además de su
posición, su orientación con respecto a un sistema de referencia, es decir, mientras que la
posición de un cuerpo rígido respecto a un sistema de referencia M viene dada por la
posición del origen del sistema de referencia O asociado a este, la orientación del cuerpo
con respecto a un sistema de referencia M vendrá dada por la orientación relativa de los ejes
del sistema de referencia O asociado a este con respecto al sistema M (ver figura 9).
Figura 9. Sistemas de referencia coincidentes en el origen
26
2.2.4.1 Matrices de rotación (respecto a uno de los ejes de referencia)
Una matriz de rotación3 se puede definir como una matriz de transformación que opera
sobre un vector de posición en un espacio tridimensional y transforma sus coordenadas
expresadas en un sistema de coordenadas rotado OXoYoZo (sistema ligado al cuerpo) a un
sistema de coordenadas de referencia MXmYmZm
PXmYmZm = R*PXoYoZo (2.1)
Recordando la definición de las componentes de un vector se tiene:
PXoYoZo = PXo*iXo + PYo*jYo + PZo*kZo (2.2)
Donde PXm, PYm y PZm representan las componentes de P a lo largo de los ejes MZm, MYm y
MZm respectivamente, de allí se obtiene:
(2.3)
Análogamente, se pueden obtener las coordenadas PXoYoZo con las coordenadas PXmYmZm
PXoYoZo = Q*PXmYmZm (2.4)
3 Se entiende como una matriz de 3x3
PXm iXm * iXo iXm *jYo iXm* kZo PXo
PYm = jYm * iXo jYm * jYo jYm * kZo PYo
PZm kZm * iXo kZm * jYo kZm * kZo PZo
27
(2.5)
Como los productos escalares son conmutativos
Q = R-1 = RT (2.6)
2.2.5 Matrices y coordenadas homogéneas
En el área de robótica es importante disponer de un mecanismo que permita localizar un
objeto en el espacio tridimensional, es decir, en posición y orientación conjuntamente, esto
se logra mediante el uso de las coordenadas homogéneas las cuales permiten tener en una
sola matriz la posición y orientación de un objeto respecto a un sistema de referencia.
La matriz de transformación homogénea es una matriz de 4 x 4 que transforma un vector de
posición expresado en coordenadas homogéneas desde un sistema de coordenadas hasta
otro sistema de coordenadas. Una matriz de transformación homogénea está compuesta por
4 submatrices:
(2.7)
La submatriz 3 x 3 superior izquierda representa la matriz de rotación; la submatriz superior
derecha 3 x 1 representa el vector de posición del origen del sistema de coordenadas rotado
PXo iXo* iXm iXo*jYm iXo * kZm PXm
PYo = jYo * iXm jYo * jYm jYo * kZm PYm
PZo kZo* iXm kZo* jYm kZo* kZm PZm
R 3 x 3 p 3 x 1 Matriz de rotación Vector de Posición (traslación)
T = f 1 x 3 1 x 1 = Transformación de perspectiva
Escalado
28
con respecto al sistema de referencia que tiene como efecto el trasladar un sistema de
coordenadas OXoYoZo con ejes paralelos al sistema de coordenadas de referencia
MXmYmZm pero cuyo origen está en dx, dy, dz;
Los vectores de transformación de perspectiva y escalización no tienen sentido en robótica,
puesto se trabaja directamente con posiciones y orientaciones reales; al igual que se trabaja
con un escalización real 1 a 1.
2.2.6 Matriz homogénea de transformación inversa
La matriz de transformación permite localizar un sistema O respecto a otro M. En ocasiones
interesa conocer la relación inversa, es decir, conocer la localización de M respecto a O, lo
que corresponderá a la inversa de la matriz de transformación homogénea.
(2.8)
2.3 Transformaciones básicas: Translación y Rotación
2.3.1 Translación
Se pueden considerar tres translaciones básicas sobre cada uno de los ejes principales de un
sistema de referencia, a partir de estas es posible construir una translación compuesta, que
esta representada por el vector p(x, y, z) cuyas componentes corresponden a los valores
asociados a cada una de las traslaciones básicas (ver figura 10).
T = Trotación traslaciónrotaciónT *− 0 1
29
Figura 10. Translaciones basicas Debido a que solo se esta tomando el caso en el cual se realiza una translación, la submatriz
de rotación de la matriz de transformación homogénea será la identidad y la de translación
son las magnitudes de las translaciones efectuadas sobre cada uno de los ejes principales.
����
�
�
����
�
�
==
1000100010001
),,()(z
y
x
zyxTranspTrans (2.9)
2.3.2 Rotación
Se pueden obtener tres rotaciones básicas considerando tres giros con respecto a cada uno
de los ejes principales de un sistema de referencia; la localización final depende del orden
en que se haya efectuado cada una de las rotaciones básicas. Al realizar cualquier rotación
la matriz de transformación homogénea tiene como vector de traslación cero, ya que se
analiza en este momento solo la rotación. Al aplicar la ecuación 2.3 y realizando los
productos escalares se obtiene:
• Rotación sobre el eje x: Se refiere a la rotación con un ángulo α sobre el eje x:
30
����
�
�
����
�
�
−+
=
10000)cos()90cos(00)90cos()cos(00001
),(αα
αααxRot (2.10)
• Rotación sobre el eje y: Se refiere a la rotación con un ángulo β sobre el eje y:
����
�
�
����
�
�
+
−
=
10000)cos(0)90cos(00100)90cos(0)cos(
),(αβ
ββ
βyRot (2.11)
• Rotación sobre el eje z: Se refiere a la rotación con un ángulo γ sobre el eje z.
����
�
�
����
�
�
−+
=
1000010000)cos()90cos(00)90cos()cos(
),(γγ
γγ
γzRot (2.12)
2.3.3 Composición de transformaciones
Una transformación compleja se descompone en una serie de transformaciones básicas de
traslación en el caso de un cambio del objeto respecto a la referencia, y/o de rotación, si lo
que se produce es un giro del objeto respecto al sistema de referencia.
La composición de transformaciones, al estar representadas por matrices, supone que el
orden en que se aplica cada una de las transformaciones básicas que la componen es
relevante, ya que el producto de matrices no es conmutativo. Además, es necesario
31
identificar con respecto a qué sistema se realiza cada transformación, debido a que ayuda a
determinar el orden de las operaciones a realizar.
Cuando se realiza una transformación de una localización (posición y giro) a otra se tienen
dos posibilidades para referenciarla. La primera se determina respecto al sistema resultante
de la transformación anterior, que se le denomina móvil, la segunda con respecto al que fue
referencia para la última transformación, que se le conoce como fijo.
La primera vez que se aplica una transformación no existe móvil, o se puede considerar
como coincidente con el fijo; teniendo en cuenta esto se determinan las siguientes reglas:
• Si la transformación se realiza con respecto al sistema fijo se premultiplica sobre las
transformaciones ya efectuadas.
• Si la transformación se efectúa sobre el sistema móvil, es decir, con respecto a la
última localización del sistema transformado se postmultiplica respecto a las
aplicadas previamente.
Tomando como ejemplo,
finalfinal paTraszRotp *)0,,0(*)90,( −= (2.13)
Se puede representar primero con una translación con respecto al sistema fijo y
posteriormente una rotación también con respecto al sistema fijo (ver figura 11a).
32
Figura 11. Ejemplo Translación y Rotación Sin embargo, considerando las reglas anteriores, también se puede interpretar como una
primera transformación de rotación respecto al sistema fijo seguida de una translación
respecto al sistema móvil (ver figura 11b.), siendo el resultado el mismo para ambos casos.
2.3.3.1 Rotaciones compuestas
Las matrices de rotación básicas se pueden multiplicar entre si para representar una
secuencia de rotación finita respecto del eje principal del sistema de coordenadas
MXmYmZm4
.
Cabe anotar que en robótica interesa representar la orientación de un sistema respecto a otro
a través de cualquier eje de rotación (no solamente con respecto al eje principal). Un giro
general se puede descomponer en una combinación de tres rotaciones básicas realizadas en
un determinado orden, teniendo en cuenta las reglas generales de composición de
transformaciones es posible obtener más de una agrupación de rotaciones básicas para un
mismo giro; entre estas existen veinticuatro combinaciones definidas: doce de ellas se
obtienen mediante combinación de tres rotaciones simples, realizadas sobre los ejes
4 Como las multiplicaciones de matrices no conmutan, es importante el orden o secuencia de realización de las rotaciones
33
principales del sistema fijo, las otras doce conocidas como ángulos de Euler, se definen
mediante combinación de tres giros sobre ejes principales del sistema móvil.
2.3.3.1.1 Matriz de rotación con representación de ángulos de Euler
Los ángulos de Euler describen la orientación de un cuerpo rígido con respecto a un sistema
de referencia fijo. Hay muchos tipos diferentes de representaciones de ángulos de Euler.
Sistema I Ángulos Eulerianos
Sistema II Ángulos De Euler
Sistema III Elevación Desviación y Giro
� respecto del eje MZm
� respecto del eje MZm
� respecto del eje MXm
� respecto del eje OXo
� respecto del eje OYo
� respecto del eje MYm
Secuencia De Rotaciones
� respecto del eje OZo
� respecto del eje OZo
� respecto del eje MZm
Tabla No.1. Representaciones de ángulos de Euler
En la tabla 1 la primera representación de los ángulos Eulerianos corresponde a:
1. Una rotación de ángulo � respecto del eje MZm.
2. Una rotación de ángulo � respecto del eje OXo.
3. Finalmente, una rotación de ángulo � respecto del eje OZo.
La matriz de rotación de ángulos se puede especificar también en términos de las rotaciones
respecto de los ejes principales del sistema de coordenadas de referencia de la siguiente
forma: una rotación del ángulo � respecto del eje MZm seguida por una rotación del ángulo
34
� respecto del eje MXm y finalmente una rotación de � respecto del eje OZo. Por lo tanto la
matriz de rotación resultante es:
(2.14)
Igualmente se pueden interpretar las demás representaciones.
2.3.3.2 Matrices de rotación respecto de un eje arbitrario
Para hallar la matriz de rotación alrededor de un eje r se hacen algunas rotaciones respecto
de los ejes principales del sistema MXmYmZm para alinear el eje r con el eje MZm luego se
hace la rotación respecto de r con ángulo n y se gira el eje principal del sistema MXmYmZm
para volver al eje r otra vez a su posición original.
Una vez realizado el procedimiento anterior se llega a la siguiente matriz de transformación
rXm
2V(�) + C(�) rXm * rYm * V(�) – rZm * S(�) rXm * rZm * V(�) + rYm * S(�) Rr,�
= rXm * rYm * V(�) + rZm * S(�) rYm2 * V(�) + C(�) rYm * rZm * V(�) – rXm * S(�)
rXm * rZm * V(�) – rYm * S(�) rYm * rZm * V(�) + rXm * S(�)
rXm2 * V(�) + C(�)
(2.15)
En la anterior matriz se tiene:
V (�) = 1 – Cos (�)
C (�) = Cos (�)
S (�) = Sen (�) (2.16)
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(�)
35
2.4 Cinemática
La cinemática del brazo del robot trata del estudio analítico de la geometría del movimiento
de un brazo robot con respecto a un sistema de coordenadas de referencia fijo sin
considerar las fuerzas o momentos que originan el movimiento, interesándose por la
descripción analítica del desplazamiento espacial del robot como una función del tiempo,
en particular de las relaciones entre la posición de las variables de articulación y la
orientación del efector final del brazo robot.
Hay dos problemas fundamentales en la cinemática del robot:
• Problema cinemático directo: estudia cuál es la orientación y la posición del efector
final con respecto a un sistema de coordenadas de referencia dado el vector de
ángulos de las articulaciones q(t)=(q1(t),q2(t),…,qn(t))T y los parámetros
geométricos del brazo.5
• Problema cinemático inverso: estudia si el brazo puede alcanzar la posición y
orientación de la mano que se desea, y si puede, cuántas configuraciones satisfacen
la misma condición dada una posición y orientación deseada del efector final del
brazo y los parámetros geométricos de los elementos con respecto a un sistema de
coordenadas de referencia.
5 donde n es el número de grados de libertad
36
2.4.1 Espacio articular y espacio cartesiano
Para un robot como el de la figura 12, en el que las articulaciones han sido numeradas
desde 1 hasta n, el valor del parámetro de la articulación i, notado como iq , es llamado
variable articular y el conjunto de variables:
),,,( 321 qnqqqq �= (2.17)
Se denota como vector de variables articulares.
Figura 12. Articulaciones
En el caso más general, en el cual el extremo del robot puede tomar una posición y
orientación cualquiera en el espacio tridimensional, el vector de coordenadas para el
extremo del robot tendrá seis parámetros, tres de posición y tres de orientación:
),,,,,( γβαzyxp = (2.18)
La relación que liga los parámetros en el espacio articular y el cartesiano generalmente es
de tipo no lineal, y para la cinemática directa viene dada por:
)(qFp = (2.19)
la cual presenta una solución única a este problema. Por otro lado para la cinemática
inversa la relación está dada por la expresión:
37
)(1 pFq −= (2.20)
Desde un punto de vista analítico, no está garantizada la solución a la cinemática inversa y
si existe, puede que no sea única. Por lo tanto, algunas de las posibles soluciones no son
una respuesta adecuada, ya que las articulaciones estarían en posiciones físicamente
imposibles.
2.4.2 El problema cinemático directo
Como los elementos de un brazo pueden girar y/o trasladarse con respecto a un sistema de
coordenadas de referencia, es necesario establecer un sistema de coordenadas ligado al
cuerpo a lo largo del eje de la articulación para cada elemento; el problema cinemático
directo se reduce a encontrar una matriz de transformación que relaciona el sistema de
coordenadas ligado al cuerpo con el sistema de coordenadas de referencia.
Para resolver el problema de cinemática directa, en el caso más general se puede asociar un
sistema de referencia a cada uno de los eslabones, incluidos la base y el extremo del robot.
Si se tiene en cuenta que siempre existirá una transformación homogénea, compuesta por
traslaciones y rotaciones básicas, estos permitirán pasar de un sistema de referencia
asociado al eslabón i al del eslabón i+1, y dicha transformación quedará en función de los
parámetros de la articulación i:
)(1 iii qFT =+ (2.21)
38
El problema se reduce a iterar el proceso de búsqueda de las n+1 transformaciones
necesarias para pasar desde el sistema asociado a la base del robot hasta el extremo del
robot, pasando por los sistemas asociados a todos y cada uno de los eslabones, generándose
con todas ellas una transformación homogénea total que expresa la posición y orientación
del extremo del robot con respecto a la base en función de los parámetros de las
articulaciones.
),,,(*** 3211
32
21
1 nextremon
nnbase
extremobase qqqqFTTTTTT �� == − (2.22)
En la figura 13 se esquematiza el proceso para un robot genérico de n grados de libertad.
Figura 13. Problema Cinematico Directo
Existe un método sistemático para resolver el problema de cinemática directa a través de
una transformación homogénea, el cual se divide en tres fases:
• Definición de los parámetros de Denavit-Hartenberg
• Asignación de sistemas de referencia
• Transformación homogénea.
39
2.4.2.1 Parámetros de Denavit-Hartenberg
Los robots están compuestos por una serie de articulaciones ya sean del tipo prismático6 o
rotacional7, las cuales están conectadas entre si por medio de eslabones, de acuerdo a esto
se pueden establecer cuatro parámetros, dos relativos al tamaño y forma del eslabón y otros
dos relacionados con la posición relativa entre los eslabones consecutivos8.
Al hallar dichos parámetros es necesario tener en cuenta que el eje de una articulación se
define según el eje con respecto al que se produce el movimiento, es decir, en una
articulación rotacional se define respecto al giro y en una articulación prismática respecto a
la dirección de desplazamiento (ver figura 14).
Figura 14. Parametros de Denavit-Hartenberg
6 Permiten una translación de un eslabón con respecto a otro 7 Permiten un giro en torno a un eje de un eslabón con respecto a otro 8 Parámetros relativos a la articulación que los enlaza
40
Los dos parámetros relativos al tamaño y forma del eslabón son:
• ia , distancia entre los ejes i e i+1 de las articulaciones a lo largo de la normal común.
Este parámetro define en cierto modo el tamaño del eslabón, y por esto se le conoce
como “longitud del eslabón”.
• iα , ángulo que existiría entre los ejes i e i+1 de las articulaciones si estos se cortaran
en los puntos de corte de la línea normal común. Este parámetro mide la forma del
eslabón a través del ángulo que sobre el mismo se encuentra girado, por lo que se le
conoce como ángulo de torsión del eslabón.
Los parámetros que relacionan la posición relativa de un eslabón con respecto a su
predecesor son:
• id , distancia entre las intersecciones de las normales comunes al eje de la articulación
i, medida a lo largo de ese mismo eje; este parámetro expresa la distancia entre los
dos eslabones, marcada por el tamaño y forma de la articulación, por esto se le
conoce como longitud articular.
• iθ , ángulo que existiría entre las líneas normales comunes al eje de la articulación i si
se cortarán en el mismo punto del eje de la articulación. Expresa, entonces, el
ángulo que forman los dos eslabones, teniendo en cuenta la forma de la articulación,
por lo que se denomina longitud articular.
41
Los parámetros relativos a la forma y tamaño del eslabón, por ser este un cuerpo rígido, no
sufren ninguna variación, sin embargo, de los dos parámetros que relacionan la posición
relativa entre los eslabones, al estar unidos por una articulación uno de ellos varia, dicho
parámetro depende del tipo de articulación que se esté manejando y es llamada variable
articular iq . De esta forma para una articulación del tipo rotacional, al ángulo iθ , es el que
produce la variación y la distancia relativa id permanece constante, mientras que para una
articulación prismática, el parámetro variable es la distancia relativa id , y permanece fijo el
ángulo iθ .
2.4.2.2 Asignación de sistemas de referencia
El problema cinemático directo se puede resolver encontrando una transformación
homogénea, función de los parámetros de las articulaciones, que exprese la posición y
orientación del extremo del robot con respecto a un sistema de referencia situado en la base
de este.
El método de solución se basa en la definición de sistemas de referencia asociados a cada
uno de los eslabones, logrando así realizar la transformación entre dos eslabones
consecutivos sólo mediante dos giros y dos translaciones, es importante aclarar que se
pueden realizar diferentes asignaciones de sistema de referencia a un mismo eslabón,
logrando con algunos de ellos que determinados parámetros de los eslabones resulten nulos;
42
uno de ellos es localizar el sistema de referencia del eslabón en el eje de la articulación que
la enlaza con el siguiente eslabón.
Una vez numerados los eslabones y articulaciones (ver 15), el sistema de referencia
asociado al eslabón i estará situado en un punto que pueda considerarse como fin del
eslabón, a lo largo de la articulación i+1 que lo une con el eslabón posterior(ver figura 16).
Figura 15. Enumeración de eslabones y articulaciones
Figura 16. Ejes Coordenados
El eje iz del sistema de referencia del eslabón i se alinea con el eje de la articulación i+1. El
eje ix del sistema de referencia se alinea con la normal común entre las articulaciones i e
43
i+1, apuntando de i a i+1, al fijar este eje queda fijo el origen del sistema. El eje iy se
establece para que el sistema de referencia sea dextrógiro.
Pueden darse dos situaciones en la que la línea normal común (eje ix ) no sea única:
• Cuando los ejes son paralelos, en este caso el origen del sistema de referencia i
queda indefinido, por convenio se toma el origen en la articulación i+1.
• Los ejes se cortan entre sí, el origen del sistema i se localiza en el punto de corte, en
cuyo caso ix está según la dirección perpendicular al plano que forman iz y 1−iz ,
tomándose el sentido en este caso de forma arbitraria.
De la misma forma descrita anteriormente, se determina el resto de sistemas de referencia
para los eslabones 1 a n-1, es decir, todos los eslabones excluyendo la base del robot y el
eslabón del extremo.
El sistema de referencia asociado a la base del robot, el eslabón 0, es el único que
permanece fijo en la cadena cinemática, por lo que es considerado como el sistema de
referencia de todos los demás, el eje 0z esta alineado con la articulación 1; sin embargo, al
no existir la articulación 0, se toma 0y de tal forma que el sistema resulte dextrógiro.
44
El sistema de referencia asociado al último eslabón se localiza en el extremo del robot; en
este caso no existe articulación n+1, por lo que su eje nz se toma coincidente con el eje z
del sistema asociado al eslabón n-1.
2.4.3 Transformación homogénea
De acuerdo a lo visto anteriormente, se observa que para pasar del sistema i-1 al i es
necesario aplicar dos giros y dos translaciones en el orden adecuado, los cuales
corresponden con los parámetros de Denavit-Hartenberg para la articulación i (ver figura
17), los que toman los siguientes valores:
• iθ como el ángulo entre 1−ix y ix medido en torno a 1−iz .
• id como la distancia medida a lo largo de 1−iz entre el origen del sistema i-1 y la
intersección de los ejes ix y 1−iz .
• iα como ángulo entre 1−iz y iz medido en torno a ix
• ia como la distancia medida a lo largo de ix entre la intersección de los ejes ix y 1−iz
y el origen del sistema i.9
9 Las operaciones entre iθ y id pueden ser conmutadas entre si al igual que las operaciones entre iα y ia .
45
Figura 17. Transformación Homógenea
Con base en estas cuatro transformaciones, se obtiene la matriz de transformación
homogénea para pasar del sistema i-1 al sistema i:
),(*),(*),(*),( 111
iiiiiiiiii xRotaxTrasdzTraszRotT αθ −−− =
����
�
�
����
�
�
−
����
�
�
����
�
�
����
�
�
����
�
�
����
�
�
����
�
� −
=
10000)cos()(00)()cos(00001
*
100001000010
001
*
1000100
00100001
*
1000010000)cos()(00)()cos(
ii
ii
i
i
ii
ii
sen
sen
a
d
sen
sen
ααααθθ
θθ
== −− ),(*),(*),(*),( 11 iiiiiiii axTrasxRotzRotdzTras αθ
����
�
�
����
�
�
−−
=
1000)cos()(0
)()cos()()cos()cos()()cos()()()()cos()cos(
iii
iiiiiii
iiiiiii
dsen
senasensen
asensensen
ααθθαθαθθθαθαθ
(2.23)
46
Determinando cada una de las matrices de transformación entre los distintos sistemas de
coordenadas del robot, se puede llegar a la matriz de transformación entre el sistema de
coordenadas de la base del robot y el del extremo del mismo10.
2.4.4 Problema cinemático inverso
El problema de cinemática inversa consiste en determinar qué valores tienen que tomar las
variables articulares para que el extremo del robot se encuentre en una posición y
orientación dadas.
Teniendo en cuenta la matriz de transformación homogénea total y las submatrices de
rotación y de posición se puede observar que:
�����
�
�
�����
�
�
=
1000333231
232221
131211
0oextremo
oextremo
extremoo
extremo zrrr
yrrr
xrrr
T (2.24)
����
�
�
����
�
�
=333231
232221
131211
rrr
rrr
rrr
Rotextremoo (2.25)
),,( oextremo
oextremo
oextremo
oextremoextremo
o zyxpTras = (2.26)
10 Ecuación 2.22
47
Cada uno de los elementos no nulos de la matriz total son función de las n variables
articulares, resultando doce ecuaciones, nueve correspondientes a los elementos de la
submatriz de rotación y las otras tres a los del vector de translación.
Sin embargo, para expresar una rotación se necesitan tres grados de libertad, por lo que solo
tres de las nueve ecuaciones son linealmente independientes, el conjunto de ecuaciones se
expresa como:
( )( )( )( )( )
( )noextremo
noextremo
noextremo
nzoextremo
nyoextremo
nxoextremo
qqqf
qqqf
qqqf
qqqfz
qqqfy
qqqfx
,,,
,,,
,,,
,,,
,,,
,,,
21
21
21
21
21
21
�
�
�
�
�
�
γ
β
α
γ
βα
=
=
=
=
=
=
(2.27)
Debido a la no linealidad del sistema de ecuaciones pueden presentarse dos circunstancias:
• No existe solución: este caso se presenta, cuando aún teniendo desde un punto de
vista analítico una solución al conjunto de ecuaciones 2.27, todas las soluciones
encontradas se encuentren fuera del espacio de trabajo del robot o simplemente no
satisfacen los rangos de movimiento de las articulaciones reales.
• Existencia de múltiples soluciones: La solución óptima depende del problema que
se trata, no siempre es factible la solución que minimice el recorrido de las
articulaciones.
48
2.4.4.1 Métodos de solución
Para resolver el problema cinemático inverso se puede optar por dos vías: solución
numérica o solución cerrada. La primera posibilidad se desecha generalmente, ya que
resulta más lenta y costosa computacionalmente que una solución cerrada.
La solución cerrada hace referencia a la búsqueda de una solución basada en expresiones
analíticas o en soluciones polinómicas, que no hacen uso de cálculos iterativos. Existen dos
métodos:
• Solución algebraica: consiste en seleccionar seis ecuaciones de las doce de todo el
conjunto dado, de forma que se establezca un sistema de seis ecuaciones con seis
incógnitas que sea fácil de resolver.
• Solución geométrica: consiste en descomponer la cadena cinemática del robot en
varios planos geométricos, resolviendo por trigonometría el problema asociado a
cada plano.
49
3 Plataforma de Prueba
Debido a las limitaciones que tiene el brazo móvil de la plataforma “Úrsula” al poseer solo
dos grados, se ve la necesidad de aumentarlos, logrando así ampliar las capacidades de
movimiento y la precisión alcanzada. En este contexto, se recuerdan los objetivos
propuestos en este proyecto, los cuales pretenden optimizar y expandir la labor del brazo
utilizado en la plataforma móvil “Úrsula” para la detección de minas antipersonales:
1. Ejecutar un movimiento controlado de exploración para garantizar el barrido de la
región mediante el seguimiento de una trayectoria predeterminada haciendo uso de
un control de posición.
2. Calcular la cinemática inversa que le proporcione al brazo la capacidad de ir a
posiciones específicas alcanzables en el espacio.
3. Desarrollar los algoritmos de control del brazo desde un sistema microcontrolado.
4. Generar una rutina de evasión de obstáculos para el brazo móvil mediante el
seguimiento del respectivo contorno, utilizando sensores de ultrasonido.
No siendo objetivo del trabajo de grado, pero constituyéndose en una herramienta
fundamental para mostrar las posibilidades de los desarrollos realizados y por razones
ajenas a la realización del presente proyecto, se hizo necesario desarrollar una plataforma
artesanal de pruebas, con la que se pudiera validar las capacidades de los algoritmos de
control desarrollados de una manera demostrativa y con ello poder presentar y evaluar los
50
objetivos del proyecto. El desarrollo y especificaciones de dicha plataforma se muestran a
continuación.
3.1 Diseño y construcción Inicialmente el diseño y la construcción de la plataforma mecánica fue guiado por personas
con conocimiento en el desarrollo de sistemas mecánicos, este proceso se realizó en varias
etapas: primero se realizaron los cortes correspondientes a los 3 primeros eslabones
utilizando aluminio debido a que es un material durable, liviano y económico, el cuarto
eslabón se fabricó en balso con el objetivo de minimizar el peso generado sobre la
plataforma y disminuir al máximo el esfuerzo realizado por los motores, luego se procedió
a la fabricación de 6 piñones en nylon, dos para cada articulación, con un diámetro de
3.8cm y 20 dientes; posteriormente se inició con la integración de los piñones, los
eslabones, pasadores y bujes, los últimos dos están hechos en bronce y permiten asegurar
los ejes de los motores que generan el movimiento y de los potenciómetros que sensan un
voltaje proporcional al ángulo en el que se encuentra la articulación.
Esta estructura fue ubicada en una caja hueca fabricada en acero con medidas 24.5cm de
ancho, 24.5cm de largo y 17.5cm de alto, la cual soporta el peso de la plataforma de
pruebas y adicionalmente almacena las tarjetas utilizadas para controlar la plataforma
demostrativa y manejar los ultrasonidos (ver figura 18).
51
Figura 18a. Eslabones y piñones
Figura 18b. Articulaciones
Figura 18. Partes de la plataforma de demostración Por otro lado el tercer eslabón soporta una caja rectangular fabricada en cartón paja de
medidas 27cm de ancho, 5cm de largo y 5cm alto, la cual soporta las 3 parejas de
ultrasonidos, con el fin de lograr que cada pareja de ultrasonidos funcione de una manera
independiente y no genere interferencia sobre los demás dispositivos.
Piñones
Eslabón 0
Eslabón 2
Eslabón 1
Eslabón 4
Articulación 2 Articulación 3
Articulación 1
52
3.2 Especificaciones
Figura 19a. Vista superior Figura 19b.Vista lateral
Figura 19. Plataforma de demostraciòn
• La plataforma de pruebas posee tres articulaciones, encargándose cada una de ellas
de uno de los grados de libertad (ver figura 19).
• La plataforma de pruebas consta de 4 eslabones, el primer eslabón corresponde a la
base sobre la cual gira la plataforma; el segundo, tercero y cuarto eslabón se
encargan de unir las articulaciones entre si, dichos eslabones tienen longitudes de
16, 23 y 25cm respectivamente.
• Para generar el movimiento en cada una de las articulaciones se utilizan 3
motores11, marca Pittman GM8223 con reductor de 60.5 a 1.
• El movimiento de las articulaciones se sensa por medio del voltaje sobre tres
potenciómetros sin fin, cada uno de los cuales está acoplado mecánicamente al eje
de cada motor.
• El espacio de trabajo de la plataforma corresponde a un movimiento de 180º grados
en el plano x-y, un movimiento de 85 grados en el plano y-z utilizando la
11 Un motor para cada articulación.
53
articulación número 2 y un movimiento de 180 grados en el plano y-z utilizando la
articulación número 3.
• El primer grado de libertad corresponde al movimiento en el plano X-Y, dicho
movimiento corresponderá a un ángulo positivo si se está moviendo a su derecha y a
un ángulo negativo si se está moviendo a su izquierda como se puede observar en la
figura 20 en la que se muestra la ubicación de la plataforma para un observador
ubicado en la parte superior :
Figura 20. Ubicación en el plano X-Y de la plataforma de prueba observada desde arriba
El segundo y tercer grado de libertad corresponden a movimientos en el plano Y-Z,
dicho movimiento corresponde a un ángulo positivo si se está moviendo hacia arriba
y a un ángulo negativo si se está moviendo hacia abajo, encontrando el eje Z
positivo hacia arriba del plano, y el eje Z negativo hacia abajo del plano.
• Para la detección de obstáculos la plataforma demostrativa cuenta con 3 parejas de
ultrasonidos ubicados como se puede observar en la figura 18 (derecha, izquierda y
abajo).
Frente
Y (Negativas) Y (Positivas)
X (Negativas)
X (Positivas)
Q1 Negativos Q1 positivos
54
• En la estrategia de cableado se tuvo en cuenta una distancia mínima de 5cm entre
los cables de los ultrasonidos transmisores y de los receptores para evitar
interferencia, además se permitió que los cables tuvieran libertad de movimiento en
cada una de las articulaciones.
• La fuente utilizada para alimentar el dspic30F2010 es de 24 voltios. En un futuro
esta fuente de alimentación será tomada del sistema de alimentación del robot móvil
“Úrsula”.
55
4 Tarjeta de Desarrollo
4.1 Especificaciones La tarjeta de desarrollo esta conformada por un microcontrolador dspic30F2010 el cual se
comunica con el ADC externo MCP3208 para el manejo de los ultrasonidos, además cuenta
con tres puente H L6203 para el manejo de los motores y con un manejador de línea para
la comunicación serial con el computador12(ver figura 21).
Figura 21. Diagrama en bloques tarjeta de desarrollo El microcontrolador utilizado tiene las siguientes especificaciones13:
� Modificación de la arquitectura Harvard � 12 Kbytes de memoria Flash � 512 bytes de memoria RAM � 1 Kbyte de memoria EEPROM no volátil � Registros de trabajo de 16 x 16 � 7 niveles de prioridad de las interrupciones internas � 2 acumuladores de 40 bits de ancho con saturación lógica opcional � 6 canales de salida para PWM
� Modos de salida complementarios e independientes � Conversor análogo digital de 10 bits
12 El esquemático de la tarjeta de desarrollo se muestra en el anexo de especificaciones 13 Para mas detalles de las especificaciones del microcontrolador ver anexo dspic30F2010Datasheet
Ucontrolador
Manejador de Línea (SN65C3221)
Puente H (L6203)
ADC (MCP3208)
Computador
Filtro Pasabajos
Motor
Ultrasonidos Potenciometrosa
56
� 6 canales de entrada En la figura 22 se muestra el diagrama de pines del microcontrolador utilizado:
Figura 22. Diagrama de pines microcontrolador
57
5 IMPLEMENTACIÓN
La plataforma de prueba utilizada para la implementación del presente proyecto posee tres
grados de libertad y su control se desarrolló en un microcontrolador dspic30F2010, dicho
control no tiene memoria, es decir, no tiene conocimiento de los puntos por los que ha
pasado haciendo necesario que las limitaciones creadas por esto se manejen por medio de
las directivas disponibles. Esta plataforma de prueba está diseñada para servir como soporte
para la detección de minas y de la plataforma móvil “Úrsula”. Por esto su funcionamiento
debe ser eficiente para poder garantizar la seguridad e integridad tanto de “Úrsula” como
de la estructura fija. Con este fin, la plataforma de prueba está conformada por 4 bloques
independientes que trabajan conjuntamente (control de posición, comunicaciones, análisis
de directivas y actualización de ultrasonidos).
El primer y fundamental bloque es “el control de posición”, siendo este responsable de la
ubicación de la plataforma demostrativa en todo momento, siempre está activo y lo único
que los demás bloques pueden modificarle es la referencia, asegurando así, que en todo
momento, la plataforma tenga una posición controlada.
El segundo bloque es el encargado de las comunicaciones de la plataforma de prueba con el
procesador central “Úrsula” a través de la UART14, dicho módulo tiene como función
14 Universal Asinchronus Receiver Transmiter
58
principal recibir las órdenes enviadas por la plataforma móvil y transmitir la respuesta a la
directiva enviada.
El tercer bloque “Análisis de directivas” es el encargado de iniciar la ejecución de una
orden recibida. Las cuatro posibles directivas a ejecutar son:
• Home: Ordena a la plataforma ir a una posición predeterminada como Home. Cabe
anotar que antes de la ejecución de las demás directivas es necesario que la
plataforma demostrativa se encuentre en posición Home con el fin de conservar
siempre la misma referencia.
• Stop: Mediante esta directiva se detiene cualquier movimiento que este en
ejecución, permitiendo que la plataforma realice una parada de emergencia si ésta
fuera necesario.
• Scan: De acuerdo al modelo propuesto en el proyecto de investigación del
Ingeniero Javier Coronado la estrategia de barrido del terreno se realiza por
carriles los cuales están divididos en cinco celdas15. Así la exploración que realiza
la plataforma de demostración se adecua a está propuesta, ejecutando un
movimiento en línea recta con el número de celdas especificado, con el fin de
realizar la búsqueda de minas evadiendo los obstáculos que se le presenten.
• Posicionamiento absoluto: Por medio de esta directiva, la plataforma móvil puede
pedir a la plataforma demostrativa que se posicione en un punto específico en el
15 “Integración de Capacidades para la Navegación de un Robot Móvil”. Ingeniero Javier Coronado.
59
espacio de trabajo, es decir en los puntos donde los límites mecánicos de la
plataforma demostrativa le permitan ir. Esta directiva revisa si hay obstáculos pero
no los evade.
Por último, el cuarto bloque tiene como fin actualizar las variables utilizadas en el
programa de lectura de los ultrasonidos, que sirven para determinar si hay algún obstáculo
en el sentido del movimiento o no, evitando que la plataforma de pruebas se golpee contra
algún objeto existente mientras se encuentra ejecutando alguna de las directivas de
movimiento.
El funcionamiento de la plataforma demostrativa, se representa a continuación mediante un
diagrama de flujo en el cual se describen los 4 bloques explicados anteriormente (ver figura
23 a 27).
Figura 23. Diagrama de flujo inicialización del programa
60
Figura 24. Diagrama de flujo directiva Scan
61
Figura 25. Diagrama de flujo directiva posición absoluta
62
Figura 26. Diagrama de flujo directiva Home
63
������������� ���
�������������������������������
�� ������������������������������
����� �������������� ���
�������������������
������ ������� �
��������� !"
#�$�
%���� �&�'
Figura 27. Diagrama de flujo directiva Stop
5.1 Inicio de módulos y variables. El programa llena y habilita una sola vez los registros necesarios en todo el proceso de
control, al igual que las interrupciones y su respectivo nivel de prioridad, a su vez se
configuran los módulos UART16 , SPI17, ADC18, PWM19, temporizador, el
16 Universal Asynchronous Receiver Transmitter 17 Serial Peripheral Interface 18 Analog to Digital Converter 19 Pulse Width Modulated
64
microcontrolador dspic30F2010 para trabajar con multiplicación fraccional y control de
saturación. Este proceso lo realiza un subprograma llamado Inicializaciones20.
En las subsecciones siguientes se explicarán detalle cada uno de estos componentes.
5.1.1 UART (UNIVERSAL ASYNCHRONOUS RECEIVER TRANSMITTER)
El módulo se habilita con el fin de proporcionar un medio de comunicación entre la
plataforma de demostración y “Úrsula”, el cual se encuentra configurado para trabajar con
8 bits de transmisión y uno de parada, con una tasa de transmisión de 9600 bps y con
interrupciones de transmisión y de recepción.
En el caso de la interrupción de transmisión, el módulo está configurado para que
interrumpa cuando transmite un carácter (8 bits), ya que la mayoría de tramas que el
módulo necesita enviar constan de un solo carácter, en la atención a subrutina de la
transmisión se borra la bandera de interrupción de la transmisión y se habilita al módulo
para una nueva transmisión.
La interrupción de la recepción está configurada para habilitarse cuando se recibe un
carácter, en esta atención a interrupción se lee el dato recibido y se limpia la bandera de
atención a interrupción. Gracias a la información brindada por esta interrupción es posible
20 Para detalles de esta función ver anexo 2 “programas”
65
analizar y ejecutar la directiva recibida correctamente mediante la función Rx_Tramas21;
dicha función se encarga de actualizar el valor de las variables necesarias para la directiva
que se desea ejecutar.
El nivel de interrupción que fue asignado a la transmisión y recepción de la UART fue
respectivamente 4 y 522 debido a que es muy importante recibir y analizar lo que transmita
“Úrsula” y además avisarle a tiempo en caso de peligro.
5.1.2 SPI (Serial Peripheral Interface)
Este módulo es utilizado para comunicar serialmente el dspic30F2010 con el ADC externo
MCP3208, el cual tiene como entradas las señales análogas provenientes de los sensores de
proximidad.
El MCP3208 tiene una resolución de 12 bits y ocho canales de entrada; los canales
utilizados son los cuatro últimos ya que estos tienen un amplificador que le da ganancia a
la señal de entrada y además la aísla haciendo que los sensores vean alta impedancia en
todo momento y no sean afectados. Por otro lado el ADC puede trabajar en modo único, es
decir, referido a tierra o diferencial, actualmente está configurado como único.
21 Para detalles de esta función ver Anexo 2 “programas” 22 De 7 interrupciones 7 es el máximo nivel de interrupción
66
La interfaz serial del microcontrolador consta de un bus de entrada de datos, una salida
serial de datos y el reloj, el cual es manejado por el maestro que en este caso es
dspic30F2010 el y el esclavo es el MCP3208, en cuanto a la transmisión y recepción de
datos entre estos se realiza en modo 16, es decir, se utiliza un registro de 16 bits para la
comunicación.
Para iniciar la comunicación, el ADC debe recibir un borde de bajada en la señal CS,
(utilizando lógica negada), la cual es enviada por medio de un puerto digital del
microcontrolador antes de comenzar una comunicación en cualquiera de los canales;
además, el ADC necesita en su bus de datos de entrada una trama de control que indica, en
los cinco bits más significativos, el inicio de conversión, el modo de trabajo (modo único o
diferencial) y el canal que se desea convertir; después de un ciclo y medio de reloj del
ADC, este empieza a retornar al microcontrolador el valor convertido desde el bit más
significativo al menos significativo.
Figura 28. Diagrama de tiempos del ADC externo
67
Para alinear el valor convertido por el ADC se envían dos tramas: en la primera se envían
los tres primeros bits de control del ADC, esta trama no varia ya que siempre se utilizan los
4 últimos canales. El valor recibido por el microcontrolador después de esta trama no es
utilizado, ya que no contiene ningún bit del valor de conversión; la segunda trama enviada
al ADC indica el canal que se desea convertir. El valor recibido por el microcontrolador
después del envió de esta trama es el valor convertido por el ADC.
5.1.3 ADC (Analog to Digital Converter)
El ADC que posee el microcontrolador tiene una resolución de 10 bits y es utilizado para
convertir el voltaje de los potenciómetros que sensan la posición de cada una de las
articulaciones que posee la plataforma de demostración. El ADC está configurado para
trabajar con un formato de datos fraccional con signo, esto con el fin de evitar la saturación
de los registros debido a las multiplicaciones y sumas que se utilizan para implementar el
control de posición.
El temporizador 3 da inicio a la conversión del ADC, determinando el tiempo de muestreo
utilizado, para obtener las constantes del control de posición.
En la atención a interrupción del ADC se llaman los controles de posición de cada uno de
los motores; es por esto que la prioridad de la interrupción del ADC debe ser la más alta ya
que nunca se debe detener el control de posición.
68
5.1.4 PWM (Pulse Width Modulated)
El PWM es el encargado de manejar el movimiento de los tres motores mediante el cambio
en su ciclo útil, el cual es manejado por el control de posición, para este fin consta de 6
pines que se agrupan por parejas (complementarias), salida alta y baja, correspondiendo a
cada motor una pareja.
El PWM está configurado para trabajar con una base de tiempo de 20 Khz y en modo
continuo “up-down” lo que significa que va a empezar a contar hacia arriba continuamente
hasta un valor predeterminado y cuando alcance ese valor empieza a contar hacia abajo
generando con base en esto, el ciclo útil a la salida. Se escogió el modo continuo “up-
down” para evitar que las conmutaciones de diferentes referencias se den al mismo tiempo.
5.1.5 Temporizador
5.1.5.1 Temporizador 2
El temporizador dos está configurado para interrumpir cada segundo con el fin de que en la
atención a interrupción se analicen los sensores de proximidad mediante la función
Ultrasonidos23, permitiendo conocer si existe o no obstáculo alrededor y en que dirección se
encuentra dicho obstáculo. De esta manera, se actualizan las banderas necesarias para
realizar las directivas de manera adecuada y segura para “Úrsula” y la plataforma de
demostración. Adicionalmente se escogió un tiempo de 1 segundo, ya que es un tiempo
prudente al movimiento que realiza la plataforma de demostración.
23 Ver anexo 2 “Programas”
69
5.1.5.2 Temporizador 3
Como se mencionó anteriormente el temporizador 3 es el encargado de dar inicio a la
conversión del ADC, es decir, es el encargado de manejar el tiempo de muestreo del
control, función que lo convierte en parte esencial, ya que mediante la actualización de la
posición de los motores, el control decide cuánto error posee la salida y qué tipo de acción
se debe tomar para disminuir el error; el temporizador 3 está configurado para interrumpir
cada dos milisegundos, tiempo con el que se realizó el cálculo de las constantes del
control.
5.2 Directivas Antes de analizar a fondo cada una de las directivas, es importante entender el
funcionamiento global de todo el programa, existen 4 procesos independientes,
simultáneos y complementarios que se explicarán a continuación:
El primero de ellos es la atención a interrupción de la UART, una vez inicializados todos
los módulos a utilizar se pueden recibir directivas por medio de ella; en esta atención se
reciben y se analizan los datos para determinar qué instrucción se desea realizar y de
acuerdo a esto organizar la información necesaria para ejecutarla. Si después de dar inicio a
los módulos no se recibe ninguna directiva la plataforma de pruebas ejecutará siempre la
directiva Home.
70
El segundo es el programa principal el cual siempre se encuentra ejecutando el análisis de
la directiva, es decir, responde y ejecuta las acciones correspondientes a cada una de las
directivas dependiendo de la información recibida y organizada por la atención a
interrupción de la recepción de la UART.
El tercero es el control de posición que se ejecuta constantemente dependiendo de la
posición final que se debe alcanzar, dicha posición final es modificada en el análisis de
directivas que se ejecuta en el programa principal.
El cuarto proceso es el encargado de actualizar qué obstáculos se encuentran alrededor de
la plataforma de demostración, el cual es implementado mediante el temporizador 2, que
cada segundo en la atención a interrupción actualiza las banderas necesarias para obtener
un conocimiento sobre el ambiente donde se esta moviendo la plataforma.
5.2.1 Stop
Como su nombre lo indica la finalidad de esta directiva es detener todos los movimientos
que se encuentre realizando la plataforma.
Primero se analiza la posición actual de la plataforma, mediante la lectura de las
conversiones realizadas por el ADC, luego de conocer la posición actual se le asigna como
posición final a cada uno de los motores la posición en la que se encuentra actualmente,
deteniendo inmediatamente el movimiento que se encontraba realizando.
71
A continuación, se obtiene el valor de la posición actual, esto con el fin de poder realizar la
correspondiente cinemática directa para que a partir de estos ángulos se pueda obtener la
posición en el espacio en el que se encuentra la plataforma y transmitirla a “Úrsula”.
5.2.2 Home
La directiva Home modifica en orden consecutivo las posiciones finales de cada uno de los
motores llevando a la plataforma de pruebas a una posición predeterminada llamada Home,
es decir, primero revisa si hay obstáculos a la derecha e izquierda, si no encuentra
obstáculos modifica la posición final del motor 1, imponiéndole como posición final la que
ya está predeterminada como posición Home. A continuación espera a que la plataforma se
situé en la posición pedida, si se encuentra un obstáculo que impida la continuidad del
movimiento se detiene y envía la información correspondiente a “Úrsula”; posteriormente
realiza el mismo procedimiento para cada uno de los motores revisando desde luego los
obstáculos que interfieren en cada movimiento, es decir, para la articulación dos y tres se
revisa que no se encuentre obstáculo abajo.
5.2.3 Posición absoluta
Esta directiva tiene como finalidad ubicar a la plataforma de pruebas en una posición
determinada en el espacio que es escogida por el usuario y que está limitada
mecánicamente por la plataforma. En esta directiva lo primero que se necesita hacer es
cinemática inversa de la posición en el espacio que se desea alcanzar para obtener los
ángulos finales a los que necesita llegar cada motor, estos ángulos son obtenidos en
72
radianes y por lo tanto para poder utilizarlos es necesario pasarlos a fraccional debido a que
el control trabaja enteramente con aritmética fraccional.
A continuación se empiezan a modificar las posiciones finales de los motores uno a uno,
revisando antes de ejecutar el movimiento los obstáculos en dirección al movimiento. Si un
obstáculo no le permite llegar a la posición final en alguno de los tres motores, la
plataforma se detiene utilizando la directiva Stop y se envía la trama correspondiente.
5.2.4 Scan
De acuerdo al modelo propuesto en el proyecto de investigación del Ingeniero Javier
Coronado la estrategia de barrido del terreno se realiza por carriles los cuáles están
divididos en cinco celdas24. De esta manera, la directiva Scan consiste en el seguimiento de
una trayectoria recta que está dividida en cinco celdas como se muestra a continuación:
Efector final
Figura 29. Celdas para Scan
El recorrido se realizara partiendo de la celda 1 y luego ubicándose en las celdas como se
indica a continuación: 2, 3, 2, 1, 4, 5, 4, 1. Como se puede observar este tipo de recorrido
permite situarse 2 veces sobre cada posición brindando mayor seguridad al proceso de
Scan, aspecto relevante cuando se está analizando un terreno minado, este recorrido no se
24 “Integración de Capacidades para la Navegación de un Robot Móvil”. Ingeniero Javier Coronado.
5 4 1 2 3 1
73
genera en tiempo real sino que es almacenado previamente al funcionamiento de la
plataforma.
Esta directiva se implementó modificando las posiciones finales de los motores para lograr
que se ubiquen en las celdas correspondientes, por lo tanto no es necesario la utilización de
la cinemática inversa ya que se almacenan en memoria directamente los ángulos de cada
motor correspondientes a cada una de las 5 celdas.
Al igual que en las demás directivas, antes de ejecutar cada movimiento analiza la
presencia de obstáculo antes de moverse, la diferencia es que si encuentra obstáculo en ésta
directiva intenta evitarlo mediante una rutina de evasión de obstáculos.
5.3 Control de posición
Con el fin de ejecutar un movimiento controlado en la exploración de minas antipersonales,
se implementó una malla de control que se encarga de manejar la posición de la plataforma
demostrativa generando movimientos en una sola dimensión.
Para este control se utiliza el microcontrolador en modo fraccional, es decir, que trabaja con
números entre -1 y 1 que son representados con 16 bits, 1 bit de signo (0�número positivo,
1� número negativo) y 15 de magnitud, obteniendo números entre -32768 y 32767 que
mediante reglas de tres se interpretan como números entre -1 y 1
74
1 �32767 -1 �-32768
La malla de control se puede analizar como la suma de los bloques presentados en la figura
30.
Figura 30. Diagrama en bloques control de posición
5.3.1 PID
La escogencia de las constantes del control PID se realizaron con la ayuda de Matlab,
utilizando la función c2d se discretizó la planta con un periodo de muestreo de dos
milisegundos y “ZOH25”. Luego se utilizó la herramienta rltool por medio de la cual se
puede observar el lugar de las raíces de la planta, su respuesta paso y además se puede
incluir un compensador para mejorar la respuesta paso a conveniencia del diseñador.
La respuesta obtenida finalmente de la planta incluyendo el compensador se presenta en la
figura 31, en esta se pueden observar puntos relevantes como el sobrepico y el tiempo de
establecimiento.
25 Método de discretización.
75
Figura 31. Respuesta de la planta compensada El compensador obtenido fue:
941.1765 (z-0.966) (z-0.9)
-------------------------- z (z-1)
Comparando con la función de transferencia de un compensador PID digital:
PID = Z2(Kp + Ki + Kd) + Z(- Kp – 2 * Kd) + Kd
(Z - 1)Z Se obtienen las constantes: proporcional (Kp), integral (Ki) y derivativa (Kd)
Kp = 818 Ki = 3 Kd = 69
Debido a que se desea trabajar con aritmética fraccional se le debe hacer un tratamiento
matemático adicional a las constantes obtenidas ya que su valor está por encima de 1 y
como es sabido en aritmética fraccional se trabaja con números entre -1 y 1 que están
representados con 15 bits de magnitud y uno de signo es decir que 1 equivale a 32767 y -1
equivale a -32768.
76
El número al que más se aproxima la acción proporcional en potencias de dos es a 1024 es
decir 210, al que más se aproxima la acción integral es a 4 es decir 22 y el número al que
más se aproxima la acción derivativa es a 128 es decir 27, por lo tanto las constantes
obtenidas se deben dividir por dichos valores respectivamente, realizando dicho
procedimiento se obtienen las siguientes constantes:
Kp = 0.798828 Ki = 0.75 Kd = 0.5390
Finalmente estas constantes se pasan a su representación fraccional utilizando la siguiente
regla de 3:
1 � 32768
Constante � X
Obteniendo:
Kp = 26176 Ki = 24576 Kd = 17661
Después de hacer las operaciones con las constantes es necesario realizar un corrimiento de
10 hacia la izquierda al resultado de la acción proporcional, 2 hacia la izquierda al resultado
de la acción integral y un corrimiento de 7 hacia la izquierda del resultado de la acción
derivativa. Estos corrimientos hacia la izquierda representan la multiplicación de las
constantes por 210, 22 y 27 respectivamente, cancelando la división que se había realizado
anteriormente logrando así no afectar los valores de las constantes encontradas mediante
Matlab. A continuación las constantes fueron ajustadas en cada articulación mediante la
realización de pequeños cambios en los valores de las constantes hasta obtener la respuesta
adecuada en el movimiento de la plataforma de demostración.
77
5.3.2 PWM
Se habilita el PWM para ser manejado de acuerdo a la salida del control PID
5.3.3 Puente H
El puente H utilizado es el L6203 y está configurado para manejar el motor de acuerdo
con el ciclo útil que arroja el PWM (ver tabla 2):
Ciclo Útil del PWM Movimiento del Motor
0% Máxima velocidad hacia el uno de los extremos
50% Quieto
100% Máxima velocidad hacia el extremo contrario
Tabla No.2. Configuración del Puente H
5.3.4 Potenciómetro
El potenciómetro está acoplado al eje del motor reflejando de esta forma su posición por
medio de un voltaje. De sus tres terminales, un extremo se encuentra conectado a una
fuente de 5V, el otro extremo se encuentra conectado a tierra y la terminal del centro se
encuentra conectada a la entrada del conversor Análogo Digital que está referida a tierra.
El potenciómetro tiene una resistencia total de 10K entre los dos extremos y debido a las
limitaciones mecánicas los voltajes obtenidos se encuentran en el rango mostrado en la
tabla 3 dependiendo de la articulación que se este analizando. Este rango es totalmente
lineal en cada una de las articulaciones.
78
Articulación Máx. Angulo Voltaje Máx. Angulo Mín. Angulo Voltaje Mín. Angulo
Uno 90° 2.05 -90° 4.76
Dos 90° 4.62 0° 3.38
Tres 78.75° 2.47 -90° 4.89
Tabla No.3. Rango de voltajes en cada una de las articulaciones
5.3.5 Temporizador
Se configuró el temporizador 3 como un temporizador de 16 bits con el fin de darle inicio
al conversor análogo-digital cada 20ms y así tener el mismo periodo de muestreo en el
conversor análogo digital, que el que se utilizó en la escogencia de las constantes del
control.
5.3.6 Conversor Análogo Digital
Se configuró el conversor análogo digital de 10 bits de tal forma que la señal de comienzo
de conversión la reciba del temporizador y además interrumpa cada vez que acabe de
convertir. De esta forma, en la rutina de atención a interrupción, se realiza la acción de
control con un periodo de muestreo de 20ms.
5.3.7 Referencia
El valor de referencia que se desea que el control alcance debe estar acoplado con el valor
que genera el ADC para que de esta forma, se encuentre estabilidad en el punto deseado, así
que se necesita poner una referencia acorde a lo que interpreta el ADC.
79
El ADC puede recibir voltajes entre 0 y 5V y debido a que está configurado en modo
fraccional el voltaje cero lo interpreta como su máximo valor negativo es decir -32768 (-1)
y el 5 como su máximo valor positivo 32767 (1), sin embargo el ADC utiliza 10 bits y los
últimos 6 los fija en cero y debido a esto el mínimo y máximo valor que obtiene en los 10
bits es -511 y 511 respectivamente, luego al tener en cuenta y agregarle a este valor los
últimos 6 bits en cero obtenemos los números reales que interpreta el ADC para cada
voltaje.
5.4 Funciones utilizadas Para la implementación de las cuatro directivas (Home, Stop, posición absoluta y Scan), se
utilizaron las siguientes funciones26:
5.4.1 Transmisión y recepción de tramas
5.4.1.1 Scan�00
Para que la plataforma de prueba realice la directiva Scan debe recibir una trama de 8 bits
(1 byte), la cual está conformada de la siguiente forma: los dos bits más significativos
indican el número de la directiva, es decir 00, y los tres menos significativos indican la
cantidad de veces que se va a realizar esta directiva, en cuanto a los bits restantes no son
relevantes.
26 El número de las directivas, debe interpretarse en notación binaria.
80
Por su parte la plataforma de prueba al recibir la directiva Scan envía a “Úrsula” dos tramas
de 8 bits; la primera trama contiene en los dos bits más significativos el número de la
directiva, en los bits 4, 5 y 6 se coloca el número de celda en que se encuentra, en el bit 3
se determina si la plataforma de prueba se encuentra evadiendo un obstáculo o no y los
últimos tres bits están vacíos. La segunda trama indica cuál sensor de proximidad encontró
un obstáculo representado en los cuatro bits más significativos, si no hay algún obstáculo
los cuatro bits son cero, además si por limitaciones mecánicas no se puede continuar con la
directiva los 4 bits tomarán el valor de 1.
En la directiva Scan, la plataforma genera una trayectoria predeterminada en línea recta,
esta trayectoria parte de la posición Home hacia uno de los extremos y de allí se dirige
hacia el otro extremo, cubriendo dos veces el terreno con una misma trayectoria.
5.4.1.2 Stop�01
Esta directiva permite que la plataforma de pruebas se detenga y le envié a la plataforma
“Úrsula” la posición en la cual terminó. Para que la plataforma de demostración ejecute
esta directiva es necesario que se le envíe una trama con sus dos bits más significativos en
01, sin importar el contenido de los demás bits.
Por su parte, la plataforma de demostración transmite cuatro bytes donde el primer byte
tiene los 2 bits más significativos en 01 y en los seis bits restantes agrupados por parejas
tiene los bits más significativos de las posiciones (x, y, z) respectivamente; la segunda trama
81
indica los ocho bits menos significativos de la posición x, la tercera trama indica los bits
menos significativos de la posición y, y por último la cuarta trama indica los bits menos
significativos de la posición z, estas posiciones se obtienen mediante el análisis de
cinemática directa.
5.4.1.3 Home�10
Para ejecutar esta directiva, la plataforma debe recibir de “Úrsula” una trama de un byte
con sus bits más significativos en 10. Por otro lado, la plataforma de demostración envía
una trama de 8 bits con los dos primeros en 10 y en el quinto bit indica si pudo llegar o no a
la posición Home.
5.4.1.4 Posición Absoluta�11
Esta directiva permite que la plataforma de demostración se ubique en una posición
determinada en su espacio de trabajo; para dicho propósito “Úrsula” debe enviarle una
trama de 4 bytes donde en el primer byte los bits 6 y 7 indican el número de la directiva, es
decir, 11 y los otros 6 bits agrupados por parejas indican los bits más significativos de las
posiciones (x,y,z) respectivamente; la segunda trama indica los ocho bits menos
significativos de la posición x, la tercera trama indica los 8 bits menos significativos de la
posición y y la cuarta trama indica los 8 bits menos significativos de la posición z, al
terminar la ejecución de esta directiva la plataforma de demostración envía a “Úrsula” 1
byte en el cual los bits más significativos son 11 y los siguientes cuatro bits indican cuál
sensor de proximidad detectó algún obstáculo; en el caso de no encontrar algún obstáculo,
82
los cuatro bits son cero, además si por limitaciones mecánicas no se puede cumplir con la
directiva se pondrán todos en 1.
Para ubicar correctamente a la plataforma de demostración se utiliza el cálculo de la
cinemática inversa para hallar los ángulos necesarios en cada articulación y alcanzar la
posición deseada.
Como resumen de la transmisión y recepción de tramas de la plataforma de demostración
ver tabla 4.
No. Directiva Nombre directiva Recepción Transmisión
00 Scan
0 0
#Directiva Vacío # de Scan
0 0
#Directiva # Celda Evita Vació Obstáculo.
Ultra analizado Vacío
01 Stop
Directiva Vacío
0 1
0 1 #Directiva Pos. x Pos. y Pos. z
Posición en x
Posición en y
Posición en z
10 Home
1 0 #Directiva Vacío
1 0
#Directiva Pos. Alcanzada Vacío
11 Posición Absoluta
1 1
# Directiva Pos. x Pos. y Pos. z
Posición en x
Posición en y
Posición en z
1 1
#Directiva Ultrasonido que senso
Tabla No.4. Tramas de comunicación
83
5.4.2 Evasión de obstáculos
Para la detección de obstáculos se cuenta con tres parejas de ultrasonidos ubicados de la
siguiente forma: dos parejas de sensores se encuentran a la derecha e izquierda del efector
final y la última pareja abajo para determinar la proximidad con el suelo.
El manejo de estos sensores se hace por medio un módulo desarrollado por el Ingeniero
Camilo Otálora, el cual está implementado en un microcontrolador pic16F84 en el que se
incluye la estrategia de control para determinar la proximidad de los obstáculos; este
control maneja la emisión y recepción de la onda de ultrasonidos, procesa la señal recibida
y tiene como salida digital una señal en forma de ancho de pulso (PWM) y como salida
analógica el filtrado de la señal digital por medio de un pasabajos.
La emisión de la señal de ultrasonido se hace de forma diferencial por medio de una
implementación algorítmica realizada en el microcontrolador, la señal recibida es
amplificada para mayor facilidad de manejo de la onda reflejada y filtrada para
contrarrestar los efectos inductivos de la línea de alimentación de 60 Hz; la señal filtrada es
pasada a través de un detector de picos para luego comparar ese nivel pico con un nivel de
comparación fijo que determina la distancia máxima de detección del módulo. El resultado
de la comparación es enviado directamente al controlador, de tal manera que cuando hay
señal de alarma (1 lógico), la señal de salida PWM del microcontrolador varía de acuerdo a
la distancia a la que haya sido detectado el obstáculo.
84
Cabe anotar que la amplificación necesaria para cada módulo dependerá directamente de
las características de radiación y recepción de los ultrasonidos, en cuánto más selectivos
resulten los patrones de radiación de los ultrasonidos, mayor podrá ser la ganancia de
amplificación de la señal recibida.
La salida digital en forma de ancho de pulso (PWM) es proporcional a la distancia del
obstáculo encontrado, aumentándose el ancho de pulso a medida que el obstáculo se
encuentra más distante del ultrasonido receptor. De igual manera, cuanto más lejano se
encuentre el obstáculo del receptor, mayor será el nivel promedio encontrado en la salida
análoga del módulo 27.
Para terminar, es necesario aclarar que la plataforma de demostración utiliza la señal
análoga, con la cual se implementa la rutina para la evasión de obstáculos. La forma en que
fue implementada dicha rutina se muestra en las figuras 32 a 34.
27 Tomado de: “Sistema Móvil para la Detección y Localización de Minas Antipersonales”. Camilo Campos, Javier Coronado y Javier Rizo. 2002.
85
%(����� ������
&����� ���)��'*��
�� ���!
*��
�� ���+
�,-
�� ���.
%��&����� ����
�������/�'
%$0� �������
����)�������'*��
1�
1�� 2)�������" #�
������������
� ���
���������
������������������
�� �����������
��0���+��3����)
��,���4�!
%$0� �������
����)�������'
������������
� ���
���������
������������������
�� �����������
��&�����!5��3�
/��������)�������
6&�������
2)�������
1�
#�
%$0� �������
�������/�'*��
������������
� ���
���������
������������������
�� �����������
��0���+��3����)
��,���4�!
%$0� �������
�������/�'
������������
� ���
���������
������������������
�� �����������
��&�����!5��3�
/�����������/�
6&�������
����/�
1�
#�
1�
Figura 32. Diagrama de flujo de rutina de evasión de obstáculos
86
• Evaluación Derecha
%$0� ����'
������������
� ����
��������
������������������
�� �����������
��0���+��3����)
��,���4�!
$0� ����7�����/�82)�������8
�0�9�������/��2)���������
����/�8�0�9�������/�
������������
� ���
�������������������
��������� ������
�������&�����5��3�
:�����������/�
$0� ����7��0�9����0�9�8
2)�������
$0� ����7�1��;��������9���
2)�������
%��,�-'
%��������&�)�
����0�9�'
��
������������
� ���
��
�������������������
��������� �����������
��&�����!5��3�:����
�������/�
������������
� ���
��������
������������������
�� �����������
0�9���+��3�6��)�
��,���8 !
������������
� ���
��������
������������������
�� �����������
0�9���+��3�6��)�
��,���8 !
���<� ����
*��
��
Figura 33. Diagrama de flujo de evaluación de obstáculos a la derecha
87
• Evaluación Izquierda
%$0� ����'
������������
� ����
��������
������������������
�� �����������
��0���+��3����)
��,���4�!
$0� ����7�����/�82)�������8
�0�9�������/��2)���������
2)�������8�0�9���2)�������
������������
� ���
�������������������
��������� ������
�������&�����5��3�
:��������)�������
$0� ����7��0�9����0�9�8
����/�
$0� ����7�1��;��������9���
����/�
%��,�-'
%��������&�)�
����0�9�'
��
������������
� ���
��
�������������������
��������� �����������
��&�����!5��3�:����
����)�������
������������
� ���
��������
������������������
�� �����������
0�9���+��3�6��)�
��,���8 !
������������
� ���
��������
������������������
�� �����������
0�9���+��3�6��)�
��,���8 !
���<� ����
*��
��
Figura 34. Diagrama de flujo de evaluación de obstáculos a la izquierda
88
Inicialmente la rutina, se basa en el análisis de la pareja de sensores28 derecha o izquierda
dependiendo de hacia dónde se esté desplazando la plataforma de demostración, en el caso
de detectar un obstáculo, se realizan los cálculos cinemáticos correspondientes con el fin de
desplazar el efector cinco centímetros hacia arriba; luego de ubicarse continua analizando
la misma pareja de ultrasonidos y si aún detecta el obstáculo continua desplazándose en la
misma dirección, en el momento que los ultrasonidos no detecten ningún obstáculo en la
dirección del movimiento, este se desplaza 15cm hacia la derecha o izquierda dependiendo
de hacia a donde se estuviera moviendo inicialmente29.
Figura 35. Ubicación de los ultrasonidos en la plataforma de demostración. Imagen tomada desde la parte inferior de la plataforma
Como se puede observar en la figura 35, las parejas de ultrasonidos ubicadas en los
extremos se encuentran a una distancia de 15cm de la pareja de ultrasonidos ubicada
28 Tenga en cuenta que las parejas de ultrasonidos están ajustadas para tener un rango de alcance de 15cm aproximadamente para un objeto paralelo a la pareja de ultrasonidos. 29 Se realiza un movimiento de 15cm debido a la forma geométrica y a la distancia a la que están ubicados los ultrasonidos (ver figura 32).
89
inferiormente y por lo tanto es necesario un movimiento de 15cm en la dirección del
movimiento antes de encontrar el obstáculo30.
El siguiente movimiento realizado por la plataforma de demostración dependerá del análisis
de la pareja de ultrasonidos que se encuentra en la dirección del movimiento y de la pareja
inferior, para este análisis se utiliza una variable que lleva el conteo del número de veces
que el efector se desplaza hacia arriba, la cual es llamada n, con esta variable se realiza el
análisis de la siguiente forma:
• Obstáculo Derecha-Izquierda-Abajo ó Derecha-Izquierda ó Sentido del
movimiento-Abajo ó Sentido del movimiento: El efector se desplaza 5cm hacia
arriba, incrementa n y vuelve a analizar.
• Obstáculo Abajo ó Abajo-Dirección contraria al movimiento: Se mueve 5cm en la
dirección del movimiento.
• No hay obstáculo-Dirección contraria al movimiento: Se analiza la variable n, si no
está en cero y es la primera vez que va a bajar se desplaza 15cm en la dirección del
movimiento para liberar al efector del obstáculo, baja 2cm y disminuye el valor de
n; si no es la primera vez solo baja 2cm y disminuye el valor de n, por otro lado si n
30 Se asegura que durante el movimiento de 15cm no se va a encontrar ningún obstáculo debido al alcance de 15cm de los ultrasonidos
90
se encuentra en 0 sale de la rutina de evasión de obstáculos llamando a la función
trayectoria31.
5.4.3 Generación de trayectorias
El módulo de generación de trayectorias modifica la variable de entrada del control de
posición, es decir, la referencia haciendo que el efector final siga 5 puntos que forman una
línea recta los cuales corresponden a su vez al centro de cada una de las celdas utilizadas
para realizar el Scan.
5.4.4 Lectura de posición actual
Se realiza con base en los valores convertidos por el ADC, el cual tiene como entrada el
voltaje sobre los potenciómetros acoplados a los ejes de los motores y por lo tanto tienen un
voltaje proporcional a la posición en la que se encuentran las articulaciones.
5.4.5 Traducción
Se utilizan dos tipos de traducciones la primera transforma radianes en fraccionales y la
segunda transforma fraccionales en radianes, dichas traducciones son necesarias ya que la
cinemática directa e inversa trabaja con ángulos en radianes y posiciones en centímetros,
mientras que el control fue implementado con base en aritmética fraccional.
31 La función trayectoria se explicará más adelante
91
5.4.6 Cinemática directa
Para la implementación de la cinemática directa correspondiente a la plataforma de prueba
se enumeraron los eslabones de la cadena cinemática, al igual que las articulaciones (ver
figura 18).
Después se localizaron los ejes de las coordenadas teniendo en cuenta que el sistema de
referencia asociado al eslabón i estuviera situado en un punto que se pudiera considerar en
cierto modo como fin del eslabón. El eje zi del sistema de referencia del eslabón i, se
alineó con el eje de la articulación i+1. El eje xi de dicho sistema de referencia se alineó
con la normal común entre las articulaciones i e i +1, apuntando de i a i +1, y por último el
eje yi se estableció para que el sistema de referencia resultara dextrógiro.
Teniendo en cuenta lo anterior se obtuvo la asignación de sistemas de referencia mostrada
en la figura 36.
X3
Z 3Y3
Figura 36. Sistemas de Referencia plataforma de prueba
92
Con base en los sistemas de referencia anteriormente escogidos se creó la tabla 5 de los
parámetros de Denavith y Hartenberg, los cuáles tienen en cuenta los parámetros de
posición relativa del eslabón (θi, di), los parámetros de tamaño y forma del eslabón (αi, ai) y
la geometría de la plataforma de prueba.
Articulación θi di αi ai
1 -Q1 L1 0 90
2 Q2+90 0 L2 0
3 Q3 0 L3 0
Tabla No.5. Parámetros de Denavit-Hartenberg
Donde L1, L2 y L3 corresponden a la longitud de los eslabones 2, 3 y 4 respectivamente, es
decir 16, 23 y 25cm y donde Q1, Q2 y Q3 corresponden al ángulo girado por las
articulaciones 1, 2 y 3 respectivamente. Mediante los parámetros de Denavit- Hartenberg
y por medio de transformaciones homogéneas que relacionan el sistema de referencia i con
el sistema de referencia i+1 se obtiene la siguiente matriz homogénea que especifica la
localización del sistema de coordenadas del extremo del robot con respecto al sistema de
coordenadas de la base:
93
Tabla No.6. Matriz homogénea de localización del sistema de coordenadas del extremo respecto a la base
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) 0 Cos(Q2)*(cos(Q3)*L3+L2)+L1-L3*sen(Q2)*sen(Q3)
0 0 0 1
94
La anterior matriz especifica la posición y orientación del extremo de la plataforma de
prueba siendo la posición (x,y,z) representadas respectivamente por las posiciones 1-4, 2-4
y 3-4 de la matriz.
5.4.7 Cinemática inversa
Mediante la cinemática inversa y a partir de la posición (x,y,z) que se desee alcanzar en el
espacio es posible obtener los ángulos necesarios en cada articulación para alcanzar dicha
posición; existen varios métodos para resolver el problema cinemático inverso, el presente
trabajo de grado fue desarrollado con una solución geométrica, la cual consiste en
descomponer la cadena cinemática de la plataforma de pruebas en varios planos
geométricos, resolviendo por trigonometría el problema asociado a cada plano. Se escogió
la solución geométrica conservando los ejes coordenados utilizados para la cinemática
directa debido a su menor costo de recursos computacionales, además, por medio de esta
solución se obtiene una respuesta mucho más exacta y eficiente que con la solución
algebraica.
El problema se dividió en dos partes, la primera parte corresponde a la ubicación del efector
final en el plano x-y, es decir, en encontrar el ángulo correspondiente a la articulación 132,
esto se realizó dividiendo el espacio de la articulación en dos cuadrantes y dando una
solución para cada cuadrante como se muestra en la figura 37.
32 llamado Q1 a lo largo del presente documento
95
Figura 37. Cinemática inversa primer cuadrante Si el cuadrante es el primero, es decir que la posición en el espacio deseada es negativa en
X y positiva en Y se utiliza ���
�
� −=extremo
extremo
xy
Q 0
0
arctan1 , por el contrario si el cuadrante es el
segundo es decir que la posición es negativa tanto en X como en Y, se utiliza
���
�
�=
extremo
extremo
xy
Q 0
0
arctan1 , además si la posición deseada no corresponde a ninguno de los
casos anteriores se transmite una trama de posición inalcanzable.
La segunda parte del problema consiste en hallar los ángulos correspondientes a las
articulaciones 2 y 3 es decir Q2 y Q3, igualmente este problema se resolvió por medio de
trigonometría y dividiendo el espacio en 2 planos coordenados uno para las z mayores que
16cm y otro para las z menores que 16cm, la primera parte se realizó tomando como base la
figura 38.
Y(+)
Y(+) Y(-)
X(-)
Primer Cuadrante Segundo Cuadrante
Q1 Q1 m2 m1
l2 l1
96
-X
Z
( -X , Z )extremo extremo0 0
Figura 38. Ángulos 2 y 3 Cinematica Inversa para z>16 Donde
cbQ −=2
���
�
�
−−=
10
0
arctanlz
xb
extremo
extremo
( )
( )���
�
�
∗+∗
=3cos
3sinarctan
32
3
QllQl
c
Para obtener Q3
( ) ( ) ( )( ) ( )223
232
2
10202 3sin3cos QlQlllzxd extremoextremo ∗+∗+=−+−=
( ) ( ) ( ) ( ) ( )22332
223
22
2
1020 3sin3cos23cos QlQllQlllzx extremoextremo ∗+∗∗∗+∗+=−+−
97
( ) ( )32
23
22
2
1020
2arccos
3ll
lllzxQ
extremoextremo
∗∗−−−+−
=
La segunda parte se realizó tomando en cuenta la figura 39.
-X
Z
( -X , Z )extremo extremo0 0
Figura 39. Ángulos 2 y 3 cinematica inversa para z<16
Donde:
( ) ( )( ) ( )( )232
23
201
20 3cos3sin QllQlzlxd extremoextremo ∗++∗=−+=
( )��
�
�
�
∗∗−−−+
=32
22
23
201
20
2arccos3
llllzlx
Qextremoextremo
Para obtener Q2
θ−−= CQ 1802
20202extremoextremo zxr +=
98
( )2201
202extremoextremo zlxd −+=
( )θcos2 12
122 ∗∗∗−+= dlldr
( ) ���
�
�
�
−+∗∗
∗−∗=
201
201
01
21
2
*22arccos
extremoextremo
extremo
zlxl
zllθ
( )( ) ��
�
�
�
�
−+
∗=
201
20
3 3sinarcsin
extremoextremo zlx
QlC
99
6 Protocolo de Pruebas y Resultados
6.1 Comunicación
6.1.1 “Úrsula” vs. Plataforma de demostración
Por medio del programa Com Port Toolkit se determinó el adecuado funcionamiento de la
comunicación entre “Úrsula” y la plataforma de demostración.
Las pruebas que se describen a continuación se realizaron enviando tramas
correspondientes a cada una de las directivas33 desde “Úrsula” hacia la plataforma de
demostración, determinando de esta forma que se realizara cada una de las sentencias
enviadas; en respuesta la plataforma de demostración envía una determinada cantidad de
tramas dependiendo de la directiva en que se encuentre y el estado de los sensores de
proximidad que le anuncie al usuario el estado actual del entorno de la plataforma de
demostración.
6.1.1.1 Transmisión de tramas por parte de la plataforma de pruebas
En esta primera etapa de pruebas la trama enviada por “Úrsula” era indiferente para la
plataforma de demostración, ya que lo único que se desea analizar es el correcto envió de
cada una de las tramas de las directivas por parte de la plataforma, es decir, solo necesita
atender la atención de recepción de la UART para enviar la trama correspondiente a la
directiva que se estaba analizando en el momento. Los valores de las variables utilizadas en
100
la trama fueron fijados en la plataforma para analizar únicamente el correcto envió y
análisis de la trama.
6.1.1.1.1 Directiva Scan:
• Inicialmente se le asignó a la plataforma el número de celda 1 sin colocarle
obstáculos alrededor y encontrándose en una posición alcanzable. La Parte derecha
figura 40 muestra la variación de la segunda trama de la directiva Scan, es decir, la
variación de lectura de los ultrasonidos.
Sin obstáculo Obstáculo a la Derecha Obstáculo en la Izquierda Obstáculo Derecha e Izquierda
Figura 40. Prueba sobre la directiva Scan “Celda 1”
33 Capítulo 5.2
101
• En la siguiente prueba se le asignó a la plataforma el número de celda 3, se
encuentra en una posición inalcanzable y evitando un obstáculo. La parte
derecha de la figura 41 muestra la variación en la primera y segunda trama de la
directiva Scan con respecto a la anterior.
Figura 41. Prueba sobre la directiva Scan “Celda 3”
6.1.1.1.2 Directiva Stop:
A continuación se muestra la trama correspondiente a la directiva Stop con las siguientes
posiciones: X = -86cm, Y = -16cm, Z = -170cm respectivamente (ver figura 42):
Figura 42. Prueba sobre la directiva Stop.
6.1.1.1.3 Directiva Home
La figura 43 muestra la trama de la directiva Home cuando esta ha sido alcanzada.
102
Figura 43. Prueba sobre la directiva Home”Sin obstaculos” .
La otra posibilidad que se da en la trama de esta directiva es cuando la posición no fue
alcanzada, es decir, se encontró obstáculo.
Figura 44. Prueba sobre la directiva Home “Con obstaculos”. 6.1.1.1.4 Posición Absoluta
A continuación se muestra la trama de la directiva posición absoluta cuando su posición es
inalcanzable debido a obstáculos, en este caso tiene obstáculo a la derecha y la izquierda.
Figura 45. Prueba sobre la directiva posicion absoluta.
103
6.1.1.2 Recepción de tramas
Esta prueba se diseño para comprobar que la plataforma esté recibiendo y analizando
correctamente los datos enviados por “Úrsula”, por lo tanto se envía la trama
correspondiente de la directiva que se desea probar y la plataforma transmite cada uno de
los parámetros que le fueron enviados alineados a la derecha.
6.1.1.2.1 Directiva Scan
En la figura 46 se muestra el llamado a la directiva Scan, en el primer llamado el número de
Scan solicitado es 7 y en el segundo es 1, por lo tanto el la plataforma de demostración
transmite el número de la directiva y el número de Scan solicitado:
Figura 46. Prueba sobre la directiva Scan “Recepcion de datos”. 6.1.1.2.2 Directiva Stop
En esta prueba se le envía a la plataforma la directiva Stop.
104
Figura 47. Prueba sobre la directiva Stop “Recepcion de datos”. 6.1.1.2.3 Directiva Home:
En esta prueba se envía la trama correspondiente a la directiva Home y se transmite el
número de la directiva solicitada alineada a la derecha:
Figura 48. Prueba sobre la directiva Home “Recepcion de datos”.
6.1.1.2.4 Directiva posición absoluta
En esta prueba se enviaron las cuatro tramas correspondientes34 a la directiva posición
absoluta, en respuesta la plataforma transmite el número de directiva y a continuación los 8
bits más significativos de cada una de las posiciones enviadas35:
34 Ver sección 5.1 35 Las posiciones enviadas son de 10 bits.
105
Figura 49. Prueba sobre la directiva posicion absoluta “Recepcion de datos”.
Adicionalmente, se implemento una interfaz gráfica (ver figura 50) en Visual Basic, la cual
cumple todas las funciones de comunicación de Úrsula, es decir, por medio de la
información que ingresa el usuario es capaz de enviar las tramas correspondientes a cada
directiva y además de interpretar las tramas recibidas de la plataforma de demostración ,
esta comunicación se realiza utilizando el puerto serial
Figura 50. Interfaz gráfica
106
6.1.2 ADC-Externo vs. Plataforma de pruebas
Con el fin de determinar la precisión de conversión del ADC externo MCP3208, es decir, la
precisión para identificar si encontró o no un obstáculo, se tomaron los datos convertidos
por el ADC para analizarlos. Cada una de las pruebas se realizaron colocando los sensores
de ultrasonido perpendiculares al material, garantizando de esta forma la mayor cantidad de
onda reflejada.
Para hacer un análisis más completo se hicieron algunas variaciones en la prueba:
• Diferentes distancias sobre un mismo material: El material escogido para esta prueba
fue el aluminio; esta prueba se realizó variando la distancia del material (4.5, 16 y
30cm) con respecto a los sensores de proximidad para ver variaciones apreciables
tomando los datos que convertía el ADC. De esta forma se observó que a mayor
distancia, en este caso 30cm, el voltaje convertido no converge alrededor de un valor;
sin embargo, para la aplicación de la plataforma la distancia crítica para detectar
obstáculo es 15cm donde los valores como muestra la figura 51, no son tan variables.
107
Diferentes Distancias sobre el mismo Material
0,00000
0,50000
1,00000
1,50000
2,00000
2,50000
3,00000
3,50000
4,00000
0 5 10 15 20 25 30 35
Distancias
Val
or c
onve
rtid
o po
r el
AD
C
Figura 51. Prueba “Diferentes distancias sobre el mismo material”.
• Diferentes superficies a diferentes distancias: Las pruebas del voltaje convertido por
el ADC se hicieron sobre superficies planas, cóncavas y convexas a diferentes
distancias 4.5cm, 16cm y 30cm. Como se aprecia en la figura 52, el comportamiento
a distancias menores de 15cm ante cualquier material es muy parecido, es por esto
que se utilizan los ultrasonidos a una distancia máxima de 15cm.
Diferentes Superficies
00,5
11,5
22,5
33,5
44,5
5
0 5 10 15 20 25 30 35
Distancias
Vo
ltaje
s C
onv
ertid
os
por
el A
DC
Plano
Concavo
Convexo
Figura 52. Prueba “Diferentes distancias – Distintas formas ”.
Para interpretar los datos se utilizó una aproximación exponencial para relacionar la
distancia con el voltaje convertido por el ADC en cada una de las superficies. Este
108
tipo de aproximación se eligió porque con ellas en series de Taylor se abarca una
aproximación de polinomio grado n (ver figura 53 a 55).
Superficie Plana
y = 1,0513e0,0413x
0
0,5
1
1,5
2
2,5
3
3,5
4
0 5 10 15 20 25 30 35
Distancia
Vo
ltaje
Con
vert
ido
por
el A
DC
Figura 53. Prueba “Diferentes distancias – Superficie plana ”.
Superficie Concava
y = 0,921e0,0506x
00,5
11,5
22,5
33,5
44,5
5
0 5 10 15 20 25 30 35
Distancia
Val
or C
On
vert
ido
por
el A
DC
Figura 54. Prueba “Diferentes distancias – Superficie concava ”.
109
Superficie Convexa
y = 0,9563e0,0485x
00,5
11,5
22,5
33,5
44,5
5
0 5 10 15 20 25 30 35
Distancia
Val
or C
onve
rtid
o p
or e
l AD
C
Figura 55. Prueba “Diferentes distancias – Superficie convexa ”.
• Diferentes materiales a diferentes distancias: Las pruebas se realizaron con diferentes
materiales tales como: icopor, madera, plástico y aluminio, con cada uno de ellos se
tomaron veinte muestras a una distancia predeterminada y se capturo el valor
convertido por el ADC externo por medio del programa Com Port Toolkit, una vez
realizado este procedimiento con cada uno de los materiales utilizados se cambió la
distancia (4.5, 15 y 30cm) a la cual se encontraban cada uno de los sensores de
proximidad.
Distacia de 4,5 cm
1,00000
1,05000
1,10000
1,15000
1,20000
1,25000
1,30000
1,35000
1 3 5 7 9 11 13 15 17 19
Número de Muestras
Vol
taje
s co
nver
tidos
po
r el
AD
C
Aluminio
Icopor
Plástico
Madera
Figura 56. Prueba “Diferentes distancias – Distintos materiales ”.
110
Con una distancia de 4.5cm entre los sensores de ultrasonido y el material escogido se
observa en la figura 56, que el rango del voltaje convertido por el ADC no es muy distante
entre los diversos materiales, el cual se encuentran en un rango máximo de 0.1859 V
debido a tener un pico máximo de voltaje de 1.3067 V en el aluminio y el mínimo de
1.1208 V en la madera. Por otra parte el promedio del voltaje convertido por el ADC en
cada uno de los materiales es: 1.2813 V para el aluminio, 1.2385 V para el icopor, 1.1821 V
para el plástico y 1.1563 V para la madera.
Así mismo a una distancia de 16cm el promedio de voltaje en cada uno de los materiales es:
1.9946 V en el aluminio, 1.9457 V en el icopor, 1.8940V en el plástico y por último en la
madera tenemos 1.8973 V; en cuanto al pico máximo esta en el material de aluminio con un
voltaje de 2.06 V y el mínimo 1.80V en el plástico, es decir, a una distancia de 16cm el
cambio de voltaje es 0.26 V (ver figura 57).
Distancia 16cm
1,75000
1,80000
1,85000
1,90000
1,95000
2,00000
2,05000
2,10000
1 3 5 7 9 11 13 15 17 19
Número de Muestras
Vol
taje
s co
nver
tidos
po
r el
AD
C
Aluminio
Icopor
Plástico
Madera
Figura 57. Prueba “Diferentes distancias – Distintos materiales ”.
111
Distancia 30cm
3,30000
3,50000
3,70000
3,90000
4,10000
4,30000
4,50000
1 3 5 7 9 11 13 15 17 19 21
Número de Muestras
Vol
taje
s co
nver
tido
s po
r el
AD
C
Aluminio
Icopor
Plástico
Madera
Figura 58. Prueba “Diferentes distancias – Distintos materiales ”.
Por último en la figura 58, a una distancia de 30cm entre los sensores de ultrasonido y el
material, el icopor se aleja del rango que se venia dando a distancias menores, por lo que se
concluye que la señal recibida por el ADC es más amplia, lo que conlleva a que los
sensores de proximidad no estén detectando el material con facilidad; sin embargo, el resto
de materiales permanecen en un rango de 3.75 V y 3.44 V, siendo estos mas fáciles de
percibir.
6.2 Prueba Control de Posición
Se realizaron pruebas del control de posición para cada uno de los motores con el fin de
comprobar su correcto funcionamiento, es decir, el seguimiento de diferentes posiciones
mediante diversas referencias. Además, se ajustaron las constantes de los controles para
lograr un movimiento suave en cada uno de los tres grados de libertad.
112
6.2.1 Sintonización de Controladores
Para encontrar las mejores constantes se hizo necesario analizar el movimiento de cada una
de las articulaciones, de esta manera se fueron ajustando las constantes y con esto se
obtuvieron unos valores diferentes para cada una de las articulaciones. Además, debido a la
influencia de la gravedad en las articulaciones de elevación36 se hizo necesario implementar
unas constates diferentes en el momento que se genera un movimiento hacia arriba y otras
cuando el movimiento es de bajada.
Las constantes halladas teóricamente sirvieron como indicadores en el momento de
sintonizar la planta. El control que se desarrolló fue un PI y las constantes implementadas
se muestran en la tabla 7.
Articulación Movimiento Kp Ki
Uno Indiferente 0.5000 0.0046
Dos Subida 6.1037 0.0055
Dos Bajada 0.0030 0.0024
Tres Subida 0.1526 0.0024
Tres Bajada 1 0.0024
Tabla No.7. Constantes implementadas en cada una de las articulaciones
36 Articulaciones dos y tres
113
Las constantes de los controles de posición garantizan un movimiento suave para que
mediante la actualización de los ultrasonidos se detecten los obstáculos a tiempo y de esta
forma tomar los correctivos necesarios.
La diferencia obtenida entre las constantes encontradas teóricamente y las constantes
implementadas en el proyecto, se debe al reductor tan grande (60.5:1) que poseen los
motores utilizados, haciendo que las constantes del control de posición sean difíciles de
ajustar. Además, se debe a factores como la gravedad y la fricción que no se tuvieron en
cuenta en el diseño teórico de la planta.
6.2.2 Error en cada posicionamiento
Cada uno de los motores tiene asociado su control de posición el cual fue probado
independientemente, es decir, dejando los otros dos motores en posiciones fijas y variando
uno solo a la vez. Las pruebas fueron realizadas con una posición inicial predeterminada en
la articulación a evaluar, de esta forma se trasladó la plataforma móvil cambiado la
referencia correspondiente hacia diferentes posiciones abarcando todo su espacio de trabajo
en cada una de las articulaciones. Para abarcar su espacio de trabajo éste se dividió en
segmentos de 11.25 grados, siendo suficiente esta separación para la función de desminado
que realiza la plataforma de pruebas.
Para la articulación 1, que genera el movimiento de derecha a izquierda, el ángulo de
referencia es cero grados y las otras dos articulaciones se encuentran fijas en 45 grados, se
114
realizaron 10 repeticiones del movimiento hacia el ángulo a evaluar comenzado desde 0
grados. La figura 59 muestra la línea de tendencia de los datos obtenidos en la prueba.
Angulos Medidos en Articulación 1
y = 1,0082x + 0,1643
-100,000
-50,000
0,000
50,000
100,000
150,000
-100 -50 0 50 100
Valor Teórico
Val
or M
edid
o
Figura 59. Angulos medidos en la articulacion 1. La ecuación teórica que describe el ángulo deseado contra el ángulo medido debería ser una
línea recta con pendiente de uno, y = x, sin embargo con los datos obtenidos tenemos una
línea de tendencia 1643.0*0082.1 += xy . De esta manera, para poder determinar una
gráfica de error se realiza la resta entre la ecuación teórica y la ecuación dada por la prueba,
la pendiente de esta recta es negativa debido a que el ángulo medido normalmente es más
grande que el ángulo teórico medido.
Gráfica de Error de Angulo sobre la Articulación 1
y = -0,0082x - 0,1643
-1
-0,8
-0,6
-0,4
-0,2
0
0,2
0,4
0,6
0,8
-100 -50 0 50 100
Angulos
Err
or e
n A
ngul
o(G
rad
os)
Figura 60. Gráfica de error de ángulo sobre la articulacion 1.
115
Como resultado en la figura 60, se observa que el error a medida que el ángulo se aleja de
la posición de referencia (ángulo1 = 0 grados) se hace más grande debido a que el control
de posición tiende a acercarse rápidamente a la referencia lejana a él tendiendo a
desfasarse, además se observa que el error en los extremos de esta articulación se hace más
grande que en el resto del espacio, debido a las limitaciones mecánicas de la plataforma de
demostración.
Por el contrario, el error en cercanía a la posición de referencia es pequeño ya que en este
caso el cambio de ángulo y por consiguiente de referencia es más pequeño y el control
tiende suavemente a ubicarse en la posición adecuada.
Ahora, el análisis para las articulaciones restantes, dos y tres, es el mismo. El espacio de
trabajo de la articulación dos está entre cero y noventa grados como se muestra en la figura
61. Para esta articulación el ángulo de referencia es noventa grados y las articulaciones uno
y tres se encuentran en cero grados y cuarenta y cinco grados respectivamente.
Angulos Medidos en Articulación 2
y = 1,001x + 0,7495
0102030405060708090
100
0 20 40 60 80 100
Angulos Teóricos
Ang
ulo
s M
edid
os
Figura 61. Angulo medidos sobre la articulacion 2. La figura 62 muestra el error de ángulo sobre la articulación dos.
116
Gráfica de Error de Angulo en Articulación 2
y = -0,001x - 0,7495
-0,85-0,84-0,83-0,82-0,81-0,8
-0,79-0,78-0,77-0,76-0,75-0,74
0 20 40 60 80 100
Angulo
Err
or e
n A
ngul
o(G
rado
s)
Figura 62. Gráfica de error de ángulo sobre la articulacion 2.
La articulación dos es la que presenta mayor error en sus posiciones debido a que debe
soportar la articulación tres y además la fuerza de gravedad ejerce una influencia mayor
sobre ella al generar el movimiento de elevación de la plataforma de demostración. De
igual forma, se mantiene un mayor error al alejarse el ángulo de la referencia escogida para
la prueba.
Por último, para la articulación tres se escogió como referencia en ángulo cero grados y se
posicionaron las otras dos articulaciones en cero grados para la articulación uno y para las
articulación dos se posicionó en noventa grados. La figura 63 indica el resultado de la
prueba con la articulación tres y posteriormente se indica el error en ángulo sobre está
articulación.
117
Angulos Medidos en Articulación 3
y = 1,0028x + 0,1557
-120-100-80-60-40-20
020406080
100
-100 -50 0 50 100
Angulos Teóricos
An
gulo
s M
edid
os
Figura 63. Angulos medidos sobre la articulacion 3.
Gráfica de Error de Angulo sobre Articulación 3
y = -0,0028x - 0,1557
-0,5
-0,4
-0,3
-0,2
-0,1
0
0,1
0,2
-100 -50 0 50 100
Angulo
Err
or e
n A
ngul
o(G
rado
s)
Figura 64. Gráfica de error de ángulo sobre la articulacion 3.
La tercera articulación, aunque genera un movimiento de elevación al igual que la
articulación 2, por no tener más articulaciones acopladas le es más fácil moverse y la
gravedad no le afecta de manera notoria, es decir, no tiene un error tan grande en
comparación con la articulación dos. De manera similar, el error en los ángulos extremos
es más grande debido en parte a las limitaciones mecánicas de la plataforma de prueba.
118
El control obtenido en cada una de las articulaciones tiene un mejor comportamiento
cuando se hacen movimientos angulares pequeños ya que realiza un movimiento más suave
y continuo alcanzando con mayor exactitud la posición pedida.
Cuando se tienen las tres articulaciones en movimiento es apreciable que las articulaciones
dos y tres no son independientes, están acopladas, de manera tal que mientras se posiciona
la articulación dos este movimiento le afecta al posicionamiento del motor tres. Debido a
esto, fue necesario ejecutar primero el control de la articulación tres para que el
movimiento de la articulación dos que es el más crítico por soportar una mayor cantidad de
peso, no se vea afectado por el posicionamiento de la articulación 3.
Cuando se tiene las tres articulaciones se aprecia que el error en los extremos de estas se
hace más grande que en el resto del espacio, esto debido a las limitaciones mecánicas de la
plataforma de pruebas. Además, como es notorio el error era menor cuando se tenia la
posición de referencia y se enviaba a la misma posición. Por otra parte, se hace necesario
que el cambio de posición de la plataforma demostrativa sea pequeño ya que a cambios
bruscos la gravedad tiene una influencia notoria en el momento de ejecutar el control de
posición, esto se hace más notorio en las articulaciones dos y tres.
Como resultados conjuntos de las tres articulaciones se aprecia que:
• El error en los extremos se hace más grande debido a las limitaciones mecánicas de
la plataforma de prueba.
119
• El error cuando se tenía la posición de referencia y se enviaba a la misma posición
era menor.
• Es necesario que el cambio de posición de la plataforma de demostración sea
pequeño ya que a cambios bruscos la gravedad tiene una influencia notoria en la
ejecución del control de posición, haciéndose esto más notorio en las articulaciones
dos y tres.
6.3 Directivas
6.3.1 Directiva Home
Al realizar repeticiones de la ejecución de la directiva Home desde diferentes sitios, se
obtuvieron resultados similares a los obtenidos cuando se estaba evaluando el control de
posición de cada una de las articulaciones independientes, siempre y cuando se realizaran
los controles en el siguiente orden: control de la articulación 1, control de la articulación 3
y por último control de la articulación 2, comprobando así que la articulaciones 2 y 3 no
son independientes y que la 2 es la más afectada por el movimiento de la plataforma en
conjunto.
Por otro lado, se comprobó que en el caso de encontrar un obstáculo en la trayectoria hacia
la posición Home la plataforma se detiene y cambia su ejecución por la directiva Stop
enviando a “Úrsula” la trama correspondiente a dicha directiva.
6.3.2 Directiva Stop
El correcto funcionamiento de esta directiva se comprobó invocando las directivas posición
absoluta y Scan seguidas por la directiva Stop. De esta forma se verificó que al poco tiempo
120
de haber invocado la directiva Stop los movimientos de la plataforma de demostración se
detienen retornando a “Úrsula” la trama correspondiente a la posición en la que se
encontraba en el momento de parar.
6.3.3 Directiva posición absoluta
Debido a la geometría de la plataforma de demostración se hizo necesario dividir el espacio
de trabajo de esta en cuatro regiones y resolver la cinemática inversa para cada una de ellas,
obteniendo de esta forma resultados favorables en la implementación de la directiva
posicionamiento absoluto.
El correcto funcionamiento de esta directiva se determinó haciendo que la plataforma se
moviera a diferentes posiciones en su espacio de trabajo mediante la cinemática inversa, de
dicha prueba se obtuvieron las gráficas 61,62 y 63 en las que se puede observar la relación
existente entre el valor esperado y el valor medido37:
Resultados "Posición X"
-50
-40
-30
-20
-10
0
10
-50 -40 -30 -20 -10 0
Posición Esperada
Pos
ició
n M
edid
a
Figura 65. Resultados posición x
37 Idealmente la pendiente obtenida en las gráficas debería ser uno, por lo tanto mediante la resta de las pendientes obtenidas con 1 se obtiene el error en la posición
121
La regresión obtenida de la anterior gráfica es y= 1.021*x + 0, por lo tanto el error
corresponde a 0.021, es decir, un error del 2.1% en el promedio obtenido en 5 repeticiones
hechas a 20 puntos (x,y,z).
Resultados "Posición Y"
-60
-40
-20
0
20
40
60
-60 -40 -20 0 20 40 60
Posición Esperada
Pos
ició
n M
edid
a
Figura 66. Resultados posición y La regresión obtenida de la anterior gráfica es y= 1.01*x + 0, por lo tanto el error
corresponde a 0.01, es decir, un error del 1% en el promedio obtenido en 5 repeticiones
hechas a 20 puntos (x,y,z).
Resultados "Posición Z"
-20,00-10,00
0,0010,0020,0030,0040,0050,00
-20 -10 0 10 20 30 40 50
Posición Esperada
Posi
ció
n M
edid
a
Figura 67. Resultados posición z
122
La regresión obtenida de la anterior gráfica es y= 1.042*x + 0, por lo tanto el error
corresponde a 0.01, es decir, un error del 4.6% en el promedio obtenido en 5 repeticiones
hechas a 20 puntos (x,y,z).
De lo anterior se puede concluir que la directiva posicionamiento absoluto tiene un error
aceptable para el tipo de aplicación en el que se va a utilizar. Cabe anotar que el error más
grande se obtuvo en la ubicación en el eje z, situación que es bastante congruente con los
resultados obtenidos en el control de posición en los cuales se pudo apreciar como el
máximo error lo tiene la articulación 2.
El máximo error obtenido en cada una de las articulaciones es un error de 4cm38 y para una
determinada posición se mantiene un error casi constante. De esta prueba se obtuvieron
resultados similares a los obtenidos cuando se estaba evaluando cada articulación por
separado, el error para la plataforma queda circunscrito dentro de un cubo que tiene de lado
el error de una articulación.
Por otra parte, se comprobó que el funcionamiento de esta directiva en el caso de encontrar
obstáculos fuera asignar la directiva Stop deteniendo así el movimiento de la plataforma de
pruebas.
38 Ver anexo 1 para mayor información. Tablas de error en la directiva posición absoluta
123
6.3.4 Directiva Scan
La trayectoria en línea recta tiene una longitud de 40cm esto debido a las limitaciones
mecánicas de la plataforma de demostración. La trayectoria seguida por el efector final se
divide en cinco celdas separadas entre ellas cada 10cm. La plataforma de demostración se
ubica en el centro de cada una de las celdas las cuáles se encuentran ubicados en las
coordenadas (x,y,z) mostrados en la tabla 8.
Posición en x(cm) Posición en y(cm) Posición en z(cm)
-39 -20 16
-39 -10 16
-39 0 16
-39 10 16
-39 20 16
Tabla No.8. Posiciones (x,y,z) seguidas por la trayectoria rectilínea
Inicialmente se realizó un Scan sin obstáculos permitiendo el libre seguimiento de la
trayectoria (línea recta) por parte de la plataforma de demostración, se realizaron varias
repeticiones, solicitando números de Scan diferentes entre 1 y 7. El error al posicionarse en
cada una de las celdas es similar al obtenido en la directiva posición absoluta.
Como segunda prueba se le colocaron obstáculos de diferentes tipos, que representaran
objetos texturados (ver figura 68 y 69), como los que se encuentran en un ambiente natural.
124
Esto con el objetivo de comprobar el correcto funcionamiento de la rutina de evasión de
obstáculos.
Figura 68. Obstáculo No 1
Figura 69. Obstáculo No 2
Como resultado de esta prueba se observa que la plataforma de demostración realiza los
movimientos adecuados dependiendo de la ubicación del obstáculo, siendo muy importante
para el correcto funcionamiento de la rutina de evasión que los obstáculos queden
perpendiculares a los ultrasonidos permitiendo una mejor recepción de la onda emitida.
La rutina de evasión de obstáculos utiliza la cinemática inversa y analiza los sensores de
proximidad en tiempo real lo que conlleva a una retardo en el tiempo de procesamiento
debido a los cálculos que debe realizar. Además, debe avanzar en pasos muy cortos para
125
prevenir así la colisión con otro obstáculo que no halla sido analizado aún y que aparezca a
causa del movimiento.�
Debido a la distancia mínima que deben tener las parejas de ultrasonidos con los obstáculos
para realizar una detección adecuada y además, la información limitada que se utiliza para
realizar la evasión se hace necesario limitar la clase de objetos a evadir por la rutina de
evasión de obstáculos.
126
7 Posibles Mejoras
• La parte mecánica desempeña una labor muy importante en la labor de desminado,
por dicha razón es necesario el uso de una estructura mecánica más robusta y
apropiada que permita mayor confiabilidad y precisión.
• La rutina de evasión de obstáculos se encuentra limitada debido a dos razones
fundamentales: la primera es que por falta de memoria del microcontrolador no se
pudo contemplar un mayor número de posibles estrategias de evasión de obstáculos
y la segunda es que debido a limitaciones mecánicas39 dicha rutina se implementó
con base en la información obtenida de 3 parejas de ultrasonidos ubicados a la
derecha, izquierda y abajo del efector final, dejando por tanto sin analizar la parte de
arriba y de enfrente de la plataforma de demostración.
• Para trabajos futuros sobre el sistema de desarrollo es necesario utilizar un
microcontrolador con mayor capacidad de memoria y puertos a utilizar, ya que el
porcentaje de memoria utilizada al finalizar el presente proyecto fue un 100% y los
únicos puertos libres que quedaron fueron 3 entradas al conversor análogo digital
interno, viéndose con esto limitado el alcance del proyecto.
39 Debido al peso.
127
8 Conclusiones
1. Desarrollar una plataforma artesanal conlleva a tener limitaciones mecánicas que se
ven reflejados en el control implementado en la plataforma.
2. El programa implementado cumple con los objetivos propuestos, ya que los errores
acumulados de la estructura mecánica, el control de posición y los cálculos de la
cinemática directa e inversa son moderados y permiten un funcionamiento adecuado
de la plataforma de demostración en la aplicación de exploración de terrenos
minados.
3. La trayectoria implementada para la búsqueda de minas antipersonales explora el
terreno siempre dos veces, lo cual garantiza la seguridad de la plataforma móvil
“Úrsula” y la plataforma demostrativa, aspecto relevante en este tipo de
aplicaciones.
4. La plataforma de demostración tiene la capacidad de ubicarse donde el experto que
la opere en el marco de desminado humanitario lo indique.
128
5. Para una operación autónoma se incorporó en la plataforma de pruebas una rutina
de evasión de obstáculos que le permite explorar superficies de objetos que estén
sobre el nivel del suelo.
6. Todo el desarrollo del proyecto de grado fue implementado en un sistema de
desarrollo proporcionado por la Universidad Javeriana, el cual cuenta con diferentes
módulos relevantes en este tipo de proyecto. Sin embargo, al trabajar con un sistema
ya estipulado es necesario adaptar y optimizar cada una de las partes del proyecto al
sistema de desarrollo, limitando los alcances del proyecto.
7. Este módulo ha sido desarrollado y concebido para ser ensamblado en el robot
móvil “Úrsula”. Por ello, se tomaron en cuenta las premisas de diseño seguidas
dentro del anterior proyecto.
129
9 Glosario
• ADC: Analog to Digital Converter
• Cable blindado: Cable con un cierto recubrimiento especial, el cual aísla la señal
transportada de ruido y posibles interferencias.
• Campo minado: Campo con más de 10 minas antipersonales.
• Coordenadas Homogéneas: mecanismo que permite localizar un objeto en el
espacio tridimensional, es decir, en posición y orientación conjuntamente.
• Dextrógiro: Representación que permite utilizar la regla de la mano derecha para
establecer el sentido de los ejes y de las rotaciones positivas, resultando una forma
fácil de visualizar el sistema de coordenadas, en especial cuando se realizan
translaciones y rotaciones
• Grados de libertad: Número de componentes de movimiento que se requieren para
generar un movimiento
• Plataforma de demostración: Se refiere a la estructura mecánica utilizada y al que
se utiliza para controlarla, los algoritmos desarrollados en el presente trabajo de
grado se encuentran programados en dicho microcontrolador40.
• PWM: Pulse Width Modulated
• SPI: Serial Peripheral Interface
• Trama: Conjunto de bits.
40 Microcontrolador dspic30F2010
130
• UART: Universal Asynchronus Receiver Transmitter
• Ultrasonidos: Sensores de proximidad, que utilizan el principio de reflexión para
averiguar a que distancia se encuentra algo.
• “Úrsula”: Plataforma móvil utilizada en el proyecto “Sistema para la detección de
minas antipersonales”41 con la cual debe trabajar conjuntamente la plataforma de
demostración desarrollada, vale la pena aclarar que cuando se nombra a “Úrsula” a
lo largo del presente trabajo de grado se esta haciendo referencia en realidad a una
interfaz que cumpla con las labores de “Úrsula”.
• Volumen de trabajo: Espacio en el que el brazo se puede mover sin limitaciones
mecánicas.
41 “Sistema Móvil para la Detección y Localización de Minas Antipersonales”. Camilo Campos, Javier Coronado y Javier Rizo. Universidad Javeriana.2002.
131
10 Bibliografía [1] Centro de investigación para la paz Enemigos invisibles, campos de la muerte: las
minas antipersonales. Lucia Alonso Ollacarizqueta. Sacado de
http://www.fuhem.es/portal/areas/paz/observatorio/informes/minas.htm
[2] Conflicto armado Colombiano #622. Secretaria General Andi 8 de octubre de 2002.
Sacado de http://www.andi.com.co/dependencias/sriagral/Conflicto2002/Conflicto622.htm
[3] Derechos humanos y derecho humanitario. Ministerio de defensa nacional, Republica
de Colombia. Sacado de:
http://www.mindefensa.gov.co/derechos_humanos/documentos/20030103ddhh_prioridad_
politica_democratica.html
[4] Robots y Sistemas sensoriales. Autores: Fernando Torres, Jorge Pomares, Pablo Gil,
Santiago T. Puente, Rafael Aracil. Editorial Prentice Hall, 2002.
[5] Robótica: Control, Detección, Visión e inteligencia. Autores: K.S.FU, R.C González,
C.S.G Le. Editorial McGraw-Hill, 1.988.
[6] Exact Solution to the inverse Kinematics of a standard 6-axis Robot Manipulator.
Autores: M. Shahinpoor, M Jamshidi and Young T. Kim
132
[7] “Integración de Capacidades para la Navegación de un Robot Móvil”. Ingeniero Javier
Coronado. Trabajo de Maestría. Universidad Javeriana. 2004.
[8] “Sistema Móvil para la Detección y Localización de Minas Antipersonales”. Camilo
Campos, Javier Coronado y Javier Rizo. Universidad Javeriana.2002.