En primer lugar, se investigaron los componentes que forman la plataforma robótica móvil PIERO, los cuales se mencionan a continuación:
- Ruedas, motores y encoders: las ruedas usadas son de goma, de 65mm de diámetro, con casquillos de 4mm (eje). Los motores tienen una velocidad nominal de 170rpm, trabajarán a 12V, contando con encoders incorporados. Estos encoders convertirán el movimiento de las ruedas en una señal eléctrica, que luego se podrá leer a través de una conexión con Arduino y Simulink.
- Sensores de distancia: se han usado sensores de distancia de ultrasonidos, para poder detectar los obstáculos que estén cercanos al PIERO.
- Led: se ha utilizado un LED RGB con tres pines, como método de señalización.
- Baterías: se han usado una serie de baterías de litio, de 3.7V, para alimentar el sistema del PIERO.
- Voltímetro: se ha utilizado para controlar la descarga de la batería.
- Sensor de voltaje: va a permitir captar el voltaje en el que se está trabajando (12V), ya que la placa de Arduino solo es capaz de captar hasta 5V, por defecto.
- Driver de potencia L298N: cuenta con un puente H, que permite el control de la velocidad y el sentido de giro de las ruedas que están con los motores. También ofrece unos pines dedicados a una señal de "Enable", que permite habilitar o deshabilitar los motores. Tiene protecciones contra corriente, temperatura y corrientes inducidas. Es capaz de trabajar desde los 3V, hasta los 35V.
- Arduino MEGA 2560: microcontrolador que permitirá programar el robot móvil, está basado en la ATmega2560. Cuenta con 54 pines de I/O (entrada/salida), de los cuales 15 pueden funcionar como PWM. Además, tiene 16 pines de entrada analógica y 4 pines de comunicación serie (UART). En cuanto a memoria fija y variable, posee una memoria flash de 256kB y una memoria SRAM de 8kB. Por otra parte, posee un puerto USB que se puede conectar directamente al ordenador, y que trabaja a 5V.
Una vez que el robot ya está montado y los componentes conectados, se crean varios modelos que permiten simular las entradas y salidas del sistema.
El esquema realizado en Simulink es el siguiente:
Como se puede ver en la imagen, se han usado dos bloques de la librería de Arduino, junto con un bloque de ganancia. Estos son bloques de entrada analógica, que reciben los datos de voltaje del sónar de la izquierda (pin A5) y de la derecha (pin A4). Luego, se ha conectado un bloque de ganancia para poder convertir ese voltaje a centímetros.
A continuación, se explica el subsistema de los motores.
Se han usado bloques de salida de PWM, junto con bloques de salida digital para las señales de "enable" de cada motor (derecho e izquierdo). La entrada a esos bloques tiene que estar limitada entre 0 y 255 para cuando las ruedas van hacia delante, y de -255 a 0 cuando van hacia atrás. Es por esto por lo que se han colocado los bloques de "Saturation". Como el PWM solo maneja valores positivos, se ha colocado un bloque que calcula el valor absoluto de la señal en los casos en los que se quiera ir hacia atrás.
En este apartado se tienen dos modelos: el primer modelo sirve para elegir el color del led según la entrada que se reciba, y el segundo modelo sirve como un sistema de señalización para detectar obstáculos cercanos.
El modelo que enciende el led de un color u otro, según los bits de la señal de entrada, es el siguiente.
Por otra parte, el sistema de señalización es el siguiente.
Con los bloques explicados anteriormente, se crea un primer modelo de navegación reactiva para el robot PIERO.
Se utiliza el bloque de los sensores de ultrasonidos de entrada, que entra al bloque de señalización de los leds, para luego pasar al bloque que enciende el led RGB.
Hay varias opciones disponibles para la lectura de los codificadores. Una de ellas es leer directamente con un bloque de "encoder" de la librería de Arduino en Simulink, describiendo el número de pin que corresponde a cada encoder. Esta opción es la que se ha usado para este proyecto. El modelo de Simulink creado para esto se puede ver a continuación. Del bloque del encoder salen los "ticks" leídos por el codificador.
Se ha creado otro modelo donde se añade un bloque de derivada discreta. Esto permite que se pueda convertir los flancos del encoder en velocidad en metros por segundo.
Identificación y simulación de los sistemas motor, comunicaciones serie y generador de señales en Simulink
Para la simulación de los sistemas motor, se han usado dos tipos de señales creadas con el bloque "Signal Builder". Las dos señales se pueden observar en las imágenes a continuación.
La primera de las señales ha sido una señal rampa.
La segunda ha sido un tren de pulsos.
Para simular los sistemas se ha creado este modelo en Simulink.
Este modelo contiene el bloque de "SignalBuilder" comentado anteriormente, junto con un subsistema llamado "PieroHW", que se corresponde con el modelo de Hardware del PIERO, estudiado en los puntos anteriores. También se han añadido unos bloques que guardan los datos de velocidad y PWM procedentes del PIERO. Estos datos son introducidos en la aplicación "System Identification", tanto los de la rueda derecha como la izquierda. Una vez hecho, se ve qué funciones de transferencia funcionan mejor, y se seleccionan y reúnen los coeficientes para las funciones de transferencia discretas de cada rueda.
Reunidos estos datos, creamos un modelo que permita elegir si se está en modelo de Piero Hardware, o en modelo de Piero con las funciones de transferencia. Este modelo es el indicado a continuación:
La otra opción sería programando el PIERO con una función de programación de bajo nivel en Simulink. Esta opción utiliza la herramienta S-Function Builder, almacenando un programa en C que permite hacer la lectura de los encoders. Esto se ha probado con la función proporcionada en los vídeos de clase, pero al final se ha optado por la solución de la lectura de los encoders.
Ahora, se integra todo en un sistema de bucle abierto, que contiene las partes de Hardware y Software del PIERO.
El modelo del bucle abierto se puede ver a continuación, con el controlador en bucle abierto y el modelo de piero hardware ya mencionado anteriormente:
A continuación se ve el controlador para el bucle abierto. Para dicho controlador se han utlizado dos tablas de consulta (Look-Up Tables), una para cada rueda del robot móvil.
En este modelo, se cambian las Look-Up Tables por las funciones de transferencia correspondientes a cada rueda. Además, se introduce una realimentación para reducir el error que se obtenía en el modelo de bucle abierto.
Teniendo en cuenta los modelos construidos anteriormente, se construyen los modelos cinemático directo (MCD) e inverso (MCI) del PIERO, junto con la odometría. Con el modelo cinemático inverso se obtienen las velocidades de ambas ruedas a partir de las velocidades lineal y angular, mientras que el modelo cinemático directo hace la operación inversa. Luego se tiene el modelo de Odometría, que junto con los bloques anteriores permite estimar la posición y orientación del robot móvil.
Modelo de cinemática completo: la entrada son las consignas de velocidad lineal y angular. Luego, se aplica el modelo cinemático inverso para obtener velocidades articulares y que el PIERO pueda leerlas. Después, se aplica el modelo cinemático directo para volver a obtener las velocidades cartesianas y obtener una representación en el plano XY del movimiento gracias a la odometría.
Modelo cinemático inverso (MCI): convierte velocidades cartesianas en velocidades articulares.
Modelo cinemático directo (MCD): convierte velocidades articulares en velocidades cartesianas.
Sistema de Odometría: obtiene una representación en un plano XY del movimiento del robot móvil, por lo que permite observar la trayectoria del robot. Este bloque se basa en las siguientes ecuaciones: Vx = vcos(v) ; Vy = vsin(v) ; w = w
Para el control de trayectorias, se tienen varios métodos, los cuales se comentan a continuación.
Teniendo en cuenta que la trayectoria es la siguiente:
- 4 metros en línea recta
- Giro antihorario 90º
- 12 metros en línea recta
- Giro horario 90º
- 0.8 metros en línea recta
- Giro antihorario 90º
- 2 metros en linea recta
Se puede modelar la trayectoria del PIERO utilizando los bloques comentados en el apartado de cinemática, y utilizando como consigna de velocidades un bloque de "Signal Builder" que contenga las variaciones de velocidad lineal y angular en cada momento de la trayectoria. Este modelo no ha sido el final, ya que es muy poco óptimo porque se le tiene que indicar al robot la velocidad a cada instante, en vez de indicarle los puntos de la trayectoria por los que tiene que pasar (que es mucho más intuitivo).
El modelo de trayectoria con el bloque de "Signal Builder" es el siguiente:
La segunda opción planteada es hacer un diagrama de estados, en el que cada estado refleje un momento de la trayectoria comentada anteriormente. Para hacer el diagrama, se ha utilizado la librería "StateFlow", con el bloque "Chart" para hacer dichos diagramas. A continuación se puede ver una imagen del diagrama de estados utilizado.
En el diagrama de estados se puede observar como cada estado se corresponde con cada uno de los puntos de la trayectoria. En cada estado, dependiendo de si se trata de un avance hacia delante, o un giro, se modifican los valores de velocidad lineal y angular dependiendo de lo que se requiera (avanzar hacia delante, o girar para la derecha/izquierda). A continuación, se inserta la imagen del modelo de control de trayectoria, incluyendo la correción de la orientación utilizando un controlador PD.
En esta parte, se sigue la trayectoria indicada anteriormente, pero se indica en forma de "waypoints". Es decir, se definen los puntos por los que el robot PIERO debe pasar, y se toman como datos de entrada. A partir de ahí, con la cinemática del robot implementada anteriormente y un sistema de persecución pura (que puede ser implementado con un bloque de la librería "Robotics System Toolbox", o mediante una "MFunction") se consigue el seguimiento de la trayectoria.
En esta opción, se implementa el seguimiento de la trayectoria mediante el bloque "PurePursuit" de Simulink. En la imagen a continuación se puede ver el modelo:
El bloque de persecución pura de la librería crea consignas de velocidad linear y angular a partir de una serie de puntos ("waypoints"). De esta manera, actualiza en cada momento los comandos de velocidad que manda al robot.
En este apartado, se hace lo mismo que en el apartado anterior, pero se utiliza un código de persecución pura redactado. Esto permite adquirir los datos de velocidad lineal, angular y orientación. De esta manera, se puede corregir la orientación con un controlador PD, y relacionarla con la velocidad angular.
El código para la función se observa a continuación:
En primer lugar, se definen los valores iniciales de las variables del programa. A continuación, se comprueba si en el instante actual se está en la trayectoria (no se ha alcanzado el número máximo de "waypoints"). Si es así, se calcula la distancia entre el punto actual y el siguiente. Si esta distancia es menor que "L" y siguen quedando puntos, se vuelve a calcular esta distancia. Cuando se llega al waypoint, se calcula el ángulo entre la posición actual y el siguiente "waypoint" de la trayectoria. En caso de que no queden "waypoints", se resetea la velocidad lineal a cero. Finalmente, se comprueba si el índice "i" es menor al número de "waypoints" de la trayectoria; en caso de ser así, el índice "il" de la salida será reduce en uno, ya que hemos recorrido un punto de más. En caso contrario, significa que todavía no hemos pasado un "waypoint", por lo que se mantiene el valor del índice "i".
En este apartado final se modela el seguimiento de la trayectoria incorporando evitación de obstáculos mediante diagrama de estados, e incorporando limitación de aceleración. El diagrama de estados se encarga de detectar los obstáculos (utilizando la información de los sensores de ultrasonidos del PIERO, modelados anteriormente), de manera que se permuta entre navegación reactiva y navegación predeterminada (persecución pura) de la trayectoria dependiendo de si se detectan obstáculos en el camino.
El diagrama de estados para la evitación de obstáculos se puede ver a continuación:
Cuando se detecta algún obstáculo, se actualiza el valor de la variable de control "C", que sirve para permutar entre navegación reactiva (cuando se detectan obstáculos en el camino), y navegación normal (persecución pura, explicada anteriormente, para esta opción se ha utilizado el bloque de "PurePursuit").
El modelo se puede ver a continuación:
