Diseño, Implementación Y Construcción De Extremidades Para Micr MATLAB... · 2008-07-29 · •...

Facultad de Informática

Luis Díaz Villanueva 2002TFC

Diseño, Implementación Y Construcción de Extremidades para Micro-robots Caminantes

(II)

Trabajo de fin de carrera



Puntos tratados :

• Conceptos fundamentales• Estructura del sistema• Arquitectura y organización de la familia 8051• Software de la Tarjeta controladora de extremidades (TCE)• Software del Sistema de control• Comentarios Cinemática y Dinámica



Introducción y Descripción general



Conceptos fundamentales:

• Robótica y Micro-robótica• La robótica es un campo interdisciplinar: Mecánica, electrónica, Informática, etc...• Robot = Sensores + Actuadores + Red de interconeción + Inteligencia + Medio• Un Robot es un ente autónomo• Problemas asociados a la robótica móvil:

• Problema de navegación• Problema de percepción(medio muy heterogéneo)• Problema de actuación• Problema de autonomía• Problema de comunicación



Modelos de control Clásico:• Control en lazo abierto: Válvula.• Control en lazo cerrado: Termostato.• Control multi-nivel: Vehículo inteligente.

comp Decision ActuadoresObjetivo

Sensores



Control Multi-nivel típico:

Perc

epcio

n

Mod

elo

Plan

ifica

cion

Ejec

uncio

n

Cont

rol

sentir actuar

Entorno

• Percepción: Lectura de sensores y procesado de la señal.• Modelo: Adpatación del medio a un modelo conceptual.• Planificación: Razonamiento y decisión.• Ejecución: Coordinación y secuenciación de acciones.• Control: Supervisión y generación de señal.

Este modelo de control puede ser usado de forma recursiva endistintos niveles de control de mayor o menor grado de abstracción



Ejemplo de control:

• Nivel 0: Objetivo Coger objeto.• Nivel 1: Plan Desplazarse, Bajar, coger, subir.• Nivel 1: Objetivo Desplazarse.• Nivel 2: Plan Motor a la izquierda.• Nivel 3: Ejecución y control.• Nivel 1: Objetivo Bajar.• Nivel 2: Plan Motor bajar.• Nivel 3: Ejecución y control.• Nivel 1: Objetivo Agarrar.• Nivel 2: Plan Cerrar mano, detectar.• Nivel 3: Ejecución y control.

....................



Características del sistema desarrollado:

• Modelo de control multinivel.• Centrado en el en el aspecto de control físico de la extremidades.• Definición de un modelo de referencia en donde se encuadran los distintos subsistemas.• Aplicación del modelo de referencia en una estructura física y lógica flexible y ampliable.• Diseño modular enfocado al diseño de interfaces.• Simplicidad y enfoque didáctico.• Uso de estándares y herramientas abiertas y de dominio público.



Modelo de Referencia:

• Niveles superiores: Navegación, razonamiento. (No desarrollado)• Interfaz A: Conjunto de primitivas para la coordinación.• SC: Control y planificación de movimientos.• Interfaz B: Primitivas de movimiento.• SCT: Control “real” del sistema físico.• Interfaz C: Red de conexionado con las partes físicas.• SF: Actuadores y sensores.

Sistema de coordinacion(SC)

Sistema de control(SCT)

Interfaz B

Sistema Fisico(SF)Niveles superiores

Interfaz A Interfaz C

1 N1

N



Estructura del sistema:

Sistema de coordinacion(SC)

Sistema Fisico(SF)

Interfaz C

Niveles superiores

Interfaz A

Interfaz B1 N Sistema de control

(SCT)

1

N

Niveles superiores

Bus RS485 o interfaz RS232

TCE 1

TCE n

Extremidades n

Extremidades 1

Interfaz P

Interfaz P

PC



Sistema físico e interfaz C: Extremidades e Interfaz P

• Conjunto de motores, pulsadores , cableado y material para la construcción de las extremidades.• Circuitos de comunicación y alimentación.• Interfaz físico: Cableado directo de los motores y sensores.• Interfaz Analógico: Modulación en ancho de pulso

Consta de:

Requiere de:

• Estudio cinemático y dinámico de las extremidades• Estudio y diseño físico de la extremidad: Diseño mecánico, eléctrico, motores, pulsadores.• Construcción de piezas y adaptación de materiales.• Montaje e integracón de componentes



SCT e Interfaz B: TCE y bus de interconexión

• Placa controladora de extremidades.• Software monitor.• Software de control.• Circuitos de interfaz física Rs232/485• Protocolo de comunicaciones.

Consta de:

Requiere de:

• Diseño electrónico y mecánico de la placa y cableado.• Diseño de protocolo de comunicación.• Diseño e implementación del software de control.• Montaje e integración de las partes.



SC: Software de coordinación

• Sistema Pc compatible con Windows 9x/Nt/2000 o Linux + X11• Software de coordinación.

• Implementación de protocolo de interfaz B.• Interfaz de usuario.• Representación 3D de modelo de extremidades.

Consta de:

Requiere de:

• Diseño e Implementación del software de coordinación.• Adaptación a bus 485(opcional).• Integración de sistema.



Ejemplo de funcionamiento:

TCE

TCE

Linea a seguir

Sensor

NS

SC

Pata 3 Pata 1

Patata 4 Pata 2

1. El nivel superior detecta la línea justo delante. Para seguir su objetivo establece,mediante su sistema de reglas, que el robot debe avanzar hacia adelante. Después reevaluará la situación tomando otras decisiones.

2. El nivel superior notifica al SC la orden de avanzar AV.

3. El SC recibe la orden de avanzar AV. Para ello pone en ejecución el siguiente plan de coordinación: {UP(1), FW(1), AP(1), UP(4), FW(4), AP(4), UP(2), FW(2), AP(2), UP(3), FW(3), AP(3), RE(0)}

4. El SC secuencia cada una de las acciones y las transmite a la TCE correspondiente de cada pata.

5. Cada una de las TCE ejecuta sus comandos recibidos moviendo los motores correspondientes.

1. El SC espera la operación de cada una de las TCE corrigiendo los movimientos en caso de error.

2. El NS recupera el estado del SC obteniendo una confirmación de "<movimiento realizado">.



Descripción de la familia 8051/52



Familia del 8051/52:

• Micro seleccionado: SAB80515/35• Características fundamentales:

• Aritmética entera de 8 bit y bit a bit.• 32+24 bits de E/S agrupados en puertos de 8 bits• 3 temporizadores/contadores.• UART full-duplex.• 256x8 bits de memoria interna.• 6+6 Lineas de interrupción.• Puerto con conversores A/D.• Watchdog.• Soporte para ahorro de energía.• Ejecución de instrucciones mejorada.• 8Kbytes de ROM interna (sólo 80535)



Organización interna:



Tipos de memoria:

• Arquitectura tipo Harvard de 3 niveles independientes.• Bloque CODE: Solo lectura, almacenamiento, código.(64K)• Bloque XDATA: Datos de programa, externa.• Bloque DATA/IDATA: Memoria RAM en el propio chip, acceso en un ciclo.

• DATA/IDATA: Datos para el usuario.• SFR: Registros de control de periféricos.



Periféricos integrados:

• Gestor de prioridad de interrupciones.• Temporizadores/Contadores:

• Modos de operación: Contador de pulsos, temporizador, modo auto-recarga, etc...• Termporizador 2: Funciones especiales, captura, comparación.

• UART:• Modo sincrono.• Modo asincrono, paridad, protocolo serie.

• Puertos de entrada salida con muestro el doble a al frecuencia de ciclo de procesador.• Conversores A/D.



Bus externo:



Partes utilizadas:

80515/35

RAM y ROM

Servomotores

Sensores

P1

P4

P2

P0

P3.6=WR

P3.7=RD

RSTPulsador

InterfazRS232

Txd

Rxd

• P0 y P2 para el bus externo de datos y control.• P1 para el control de los servomotores.• P4 lectura de sensores de apoyo.• UART con velocidad fija a 9600 baudios.• Temporizador 1 para generación de baudios.• Temporizador 0 y 2 para control pwm.• Entrada de reset para reinicio.



Software de la Tarjeta controladora de extremidades



RAM

ROM

paulmon.hex

*.as

*.c;*.h

*.rel

*.sym

TCEpaulmon.asm

core.ihx

core.hex

packihx

Compilacion del software de la TCE

Compilacion del monitor

Terminal,SC

Entorno de ejecucion

8051+UART

AS31

aslink

Asxx51

sdcc

*.asm

Plataforma de desarrollo:

• Tres Dominios: Compilación del monitor, Compilación del del software de la TCE, Entorno de ejecución.• Uso de herramientas Freeware.• Secuenciación de comandos mediante Makefiles



Programa monitor: PAULMON

• Almacenado en memoria ROM (EEPROM).• Se ejecuta en la propia placa. (Diferente a emuladores)• Funcionalidades ofrecidas:

• Telecarga de código ejecutable.• Acceso a la UART: Escritura, lectura, detección de velocidad de la linea.• Funciones de conversión ASCII<-> HEX.• Funciones de utitlidad y depuración: volcado de memoria, desensamblado, gestión de interrupciones.

• Necesita el ensablador AS31, siendo este incompatible con otros para la misma plataforma.• Compatible con el estandar Intel Hexadecimal.(.HEX)



Compilador SDCC:

• Soporte casi completo de ANSI C.• Tipos de datos: char, int, long , float.• Control de interrupciones.• Interfaz con módulos en ensamblador.• Simulación de pila para funciones reentrantes.• Soporte de la librería clib: malloc, printf, etc..• Librería de comunicaciones serie.• Modificadores de almacenamiento para varibles: data, idata, etc...• Acceso a periféricos mediante Macros.• Genera código ensamblador con múltiples optimizaciones.

A pesar de estas ventajas se detectaron errores en el código generado por parte del compilador.



Ensamblador Asx8051, Montador AsLink y Packihx:

• El Asx8051es un ensamblador cruzado para distintas plataformas y mircro-controladores.• Soporte de intercambio de simbolos entre módulos.• Definición de funciones y macro.• Montaje relativo de todos lo módulos y traducción al espacio de direcciones del micro-controlador.• Compatibilidad con el emulador uSim51.• El programa Packihx compacta el código y las cabeceras del formato de salida generando Intel Hexadecimal “puro”.• Integrados con el compilador SDCC para las distintas plataformas objetivo.



Organización del software:

Gestion de la UART Funciones E/S

Lector de comandos

Procesador de comandos

Gestion PWMGestion de sensores Cinematica inversa

Funciones coma fija

usa

main.c

pcmd.c

paulmon.as

debug.c

sensor.c

pwm.c inversa.c

acos.c atan.c

asig.asfixpoint.c

misc.c

Usa

• Organizado en módulos independientes.• Abstracción de los subsistemas fundamentales.• Integración con PAULMON.• Diseño abierto y reutilizable.• Posibilidad de ampliación de funcionalidades.




• Gestión de la UART: Configurar y transmitir por el puerto de de comunicaciones.• Funciones de E/S: Funciones avanzadas de E/S.• Lector de comando: Procesador léxico de comandos.• Procesador de comandos: Sistema ejecutivo de control.• Gestión de sensores: Capa de abstracción de los sensores.• Gestión PWM: Abstracción de control PWM, control de interrupciones y de salidas digitales.• Cinemática inversa: Módulo de cálculo de cinemática inversa.• Librería de coma fija: Para realizar cálculos decimales sin recurrir al tipo float.



Control PWM:

tc

tg ton

Senal

Posicion servo

90 grados 0 grados 90 grados

• La posición absoluta del servomotor es relativa al ancho del del pulso (periodo ton).• Se trata de una señal periódica de ciclo tc.• Se necesita un tiempo de guarda tg para estabilizar la electrónica de control del servomotor.• Es necesario una calibracicón de cada motor para determiar sus parámetro de funcionamiento.



Implementación del control PWM:

Salida 0

Salida 1

Salida 2

Salida 3

ints2 0 0

Programa timer 0

00

• Se programa una interrupción cíclica para indicar el comienzo del ciclo PWM. (Timer 2)• Se dispone de una lista ordenada indicando las diferencias de tiempo de bajada de cada una de las salidas y el estado de las salidas (agrupamiento).• Se programa el timer 0 de forma secuencial cada uno de los elementos de la lista produciendo el flanco de bajada.



Características del control PWM:

• El control PWM se ejecuta continuamente mediante la activación cíclica de las interrupciones.• Cuando se quiere actualizar el valor las salidas se dispone de un conjunto de funciones que abstraen el tratamiento del subsistema.• La actualización de las señales se puede realizar únicamente después de la ejecución completa de un ciclo.• Se utilizan una bit “cerrojo”(accesible mediante instrucción atómica) para la sincronización de las estructuras de datos del subsistema sin que se interfiera en la ejecución del ciclo.• El numero de salidas puede ser variable y configurable, si bien el tiempo actulización y la calidad de la señal pueden verse afectados.



Lector de comandos:

• Realiza 3 funciones básicas: Lectura, análisis léxico y sintáctico y manejo de direcciones.• La lectura se realiza mediante un espera activa y utilizando las funciones de E/S.• Se extraen los telegramas mediante un autónoma finito que que interpreta tanto el nivel sintáctico como la posición de los campos significativos.• Los telegramas que no pertenecen al leguaje reconocido se desechan.• Los telegramas (una vez interpretados) son se envían al procesador de comandos siempre y cuando la dirección de destino concida o bien sea un telegrama tipo broadcast.



Gramática del protocolo:MENSAJE:= SEP ID CMD SEP | SEP ID RESPUESTA SEP

CMD:= NOP

CMD:= RESET

CMD:= CONFIG INT16 INT16 INT16 FIX FIX FIX

CMD:= STATUS

CMD:= MOVA INT8 INT8 INT8

CMD:= MOVP FIX FIX FIX

CMD:= LEAN

CMD:= UP

CMD:= FWD

CMD:= REW

CMD:= CENT

RESPUESTA:= RSTATUS INT8 INT8 INT8 AP

NOP:= 00

RESET:= 01

CONFIG:= 02

STATUS:= 03

MOVA:= 04

MOVP:= 05

LEAN:= 09

UP:= 0A

FWD:= 0B

REW:= 0C

CENT:= 0D

RSTATUS:= 0E

ID:= 00..FF

SEP := *

INT8:= 00..FF

INT16:= 0000..FFFF

FIX:= 00000000..FFFFFFFF

Esta grámtica puede ser facilemente apliada paraincorporar nuevos comandos o respuestas



Procesador de comandos:

• Ejecuta cada uno de los comandos suministrados por el lector de comandos.• Soporta diferentes tipos de comandos:

• Inicialización.• Configuración.• Reset.• Movimiento angular y cartesiano(cinematica inversa)• Notificación de estado.• Movimientos prediseñados.

• Ejecución instantánea de comandos.• Posibilidad de incluir ejecutar secuencias de comandos en serie.• Posibilidad de incluir progrmación remota de comandos nuevos.



Librería matemática en coma fija:

• Dato de 32 bits: 12 bits parte entera, 20 parte decimal• Codificación en complemento a 2.• Basado en aritmética entera: aumento de velocidad.• Soporte de funciones matemáticas:

• Aritmeticas: +, - , * , /• Ajuste de precisión.• Comparaciones.• Raiz cuadrada: Método iterativo de Newton.• Trigonometricas: acrsen y arccos mediante tablas ‘look-up’ pregeneradas e interpolación lineal.• Converisiones de tipos: int y char.

El algoritmo de cálculo de la cinematica inversa se apoya en estas funcionalidades.



Software del sistema de coordinación



Características:

• Implementado en C++ sobre un PC Compatible.• Soporta sistemas operativos Win32 y Linux.• Entorno gráfico.• Modelo en 3D de las extremidades.• Diseñado como campo de pruebas para la TCE.• Implementa la comunicación mediante el puerto serie del PC.• Soporte de los comandos básicos de la TCE: Configuración , movimiento, visualización de estado, etc ...• Implementa un modelo coordinación simple a modo de ejemplo.• Posibilidad de ampliación y migración a un sistema empotrado basado en un microprocesador/microcontrolador.



Interfaz de usuario:



Software de apoyo:

• Software de desarrollo enteramente de dominio público.• Compilador GCC si bien se puede compilar con MS-Visual C++.• MESA OpenGL/MS OpenGL:

• Modelado en 3D.• Dibujo en 3D.• Soporte GLU/GLUT para funciones avanzadas.

• Librería gráfica FLTK:• Soporte de ventanas, botones, cuadros de texto, barras, etc.• Soporte par dibujo en 2D.• Compatibilidad con OpenGL.

• Librería de comunicaciones para el puerto serie:• Abstracción del medio de comunicación.• Compatibilidad con sistemas Unix y Win32.




Package

Package

Package

Package

Package

Comunicacionesserie

Acceso a ficheros

Control

Inversa

GUI/Graficos

• Software orientado a Objetos.• Modelo en dos Capas separables: Control – Vista.• Diseño abierto, portable y reutilizable.• Facilidad y flexibilidad de integración.



Diagramas de clases:

Class

Class

Class

Class

Class Class

Class

Class

Fl_Window

uipata

cpata

puertoserie

vpata glpata

funciones de inversa.cy fixpoint.c

cpata usa ademas

coord

muestra

1

11 2muesta

1

1

usa

11

1

usa

Fl_Gl_Window

usa

1

Class

Class

Class

Class Class

Class

recursocomm tcsection

puertoserie tbuffer

tficheroini parser

1 1

1 n

1

1 1usa

usa

manipula

accede

1

General

Librería de comunicaciones



Coordinación de extremidades:

• Motivación: coordinar los distintos motores para realizar movimientos para caminar.• Se dispone de un sistema de guiones muy simplificado.

UP(1) // Subir pata 1FWD(1) // Mover “hacia atrás”LEA(1) // Apoyar DEL(1500) // retardoUP(2) // Levantar Pata 2FWD(2) // AvanzarLEA(2) // ApoyarDEL(1500) // RetardoCET(0) // centrar ambas patasEND // fin

Script de ejmplo



Comentarios sobre cinemática ydinámica



El problema cinemático:

Posicion actuadores

q1,q2,q3,...

Posicion extremo final

x,yz

directo

inversoproblema

problema

x= f x 1 ,⋯, n

y= f y 1 ,⋯, n

z= f z 1 ,⋯, n k= f −1 x , y , z ∀ k=1,⋯, n

• Problema Directo: Actuadores Extremo final• Una sola solución posible.• Solución: Método geométrico, Denavit-Hartenberg.

• Problema Inverso: Extremo final Actuadores• Múltiples soluciones.• Solución: Método gemétrio + reglas, Métodos interativos.



Algoritmo para cálculo del problema inverso:

θ2

θ1

θο l3

l1

l2

m=(x,y,z)

Y

ZX

plano giradom

θο

ZX

Y

mx

my

l1

m' m''

l3

l2

Y

Y

Situación inicial:• m es el punto objetivo.• se conoce las longitudes de cada segmento

Paso 1: Plano girado• La extremidad está contenida en un plano.

0=arctg [zx ]m'=x 2y2

y Paso 2: Translación• Se traslada el centro de referencia.

m ''=m'−l10 ∣m ''∣=m ''x

2m ''y2



Algoritmo para cálculo del problema inverso (II):

m''

ρ

α

β

θ1

θ2

l2

l1Y

X

Paso 3: Aplicación del teorema del coseno• Se calculan los ángulos del triangulo.• Se adapatan los caáculos a una posición determinada de las dos posibles.

=arctg m ''ym ''x

=ar cos ∣m ''∣2−l 22−l3

2

−2 l 2 l3

=ar cos l 32−∣m ''∣2−l 2

2

−2 l 2∣m ''∣ 1=

2= −

Diversas constantes y factores pueden ser precalculadospara obtener mayor velocidad de computo



Dinámica:

• El estudio de la dinámica se ocupa de el estudio de las duezas pares, inercias, rozamientos, etc ...• Es un problema muy complejo de resolver y además hay que calcular el problema de forma global(teniendo en cuenta todo el robot y sus cargas).• Los modelos de cálculo están basados en ecuaciones diferenciales extremadamente complejas que han de ser calculadas mediante algoritmos numéricos.• El cálculo en tiempo real es inviable para micro-robots.• Se suelen aplicar modelos simplificados del robot y estimaciones que permiten resolver el problema.• La estructura sensitiva es mas compleja ya que necesitan información extra.

La mayoría de los robots no incomparan modelo dinámico.



Equilibrio:

• Diferencia entre equilibrio dinámico y estático.• Pretende resolver el problema dinámico mediante modelos no matemáticos.• Garantizar el equilibrio del robot en cada movimiento mediante predicción en tiempo real.• Técnicas biométricas:

• Observación de la naturaleza.• Simulación de técnicas de características naturales.

• Métodos típicos usados:• Utilización de reglas.• Redes de neuronas.• Control fuzzy.• Utilización de heurísticas.



Regla simple de equilibrio:

M

MM

Si la proyección del centro de masas M en el plano que contiene al polígono de apoyo está contenida dentro del polígono de apoyo (cae dentro del

polígono), se garantiza el equilibrio estático del robot.

Equilibrio Desequilibrio

• Regla heurística para equilibrio estático.• Basado en la estimación del poligono de apoyo y el centro de masas.• Cálculo sencillo.• Limitaciones:

• Movilidad reducida.• Velocidad lenta.



FIN

Diseño, Implementación Y Construcción De Extremidades Para Micr MATLAB... · 2008-07-29 · •...

Documents

Transcript of Diseño, Implementación Y Construcción De Extremidades Para Micr MATLAB... · 2008-07-29 · •...