CONTROL PARA UN BRAZO ROBOT COLOCADO SOBRE LA … · 2008-08-06 · CONTROL PARA UN BRAZO ROBOT...

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.


FACULTAD DE INGENIERÍA

DEPARTAMENTO DE ELECTRÓNICA

BOGOTÁ D.C.

2005

3


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 MXm

� respecto del eje OXo

� respecto del eje OYo

� respecto del eje MYm

Secuencia De Rotaciones

� respecto del eje OZo

� respecto del eje OZo


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


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


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.


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)


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


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)


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


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


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.

CONTROL PARA UN BRAZO ROBOT COLOCADO SOBRE LA … · 2008-08-06 · CONTROL PARA UN BRAZO ROBOT...

Documents

Transcript of CONTROL PARA UN BRAZO ROBOT COLOCADO SOBRE LA … · 2008-08-06 · CONTROL PARA UN BRAZO ROBOT...