Robobo: Sensores de Movimiento

Contenidos

• Introducción
• readOrientationSensor()
• readAccelerationSensor()
• Ejercicios

Introducción

Una unidad de medición inercial o IMU (del inglés inertial measurement unit), es un dispositivo electrónico que mide e informa acerca de la velocidad, orientación y fuerzas gravitacionales de un aparato, usando una combinación de acelerómetros y giróscopos. Las unidades de medición inercial son normalmente usadas para maniobrar aviones, pero son muy populares también en vehículos autónomos, como drones, submarinos o robots móviles.

Su principal uso en estos últimos, como Robobo, es conocer la posición y orientación del robot mientras se mueve, ya que es fundamental conocer en todo momento su localización exacta para poder navegar de forma segura y eficiente.

El Robobo real incluye una IMU gracias al smartphone, pero no todos los modelos lo incorporan, por lo que estos métodos podrían no estar disponibles para algunos usuarios. En RoboboSim, la IMU está simulada, por lo que se pueden utilizar sin problema.

Los métodos disponibles son dos: uno para conocer la orientación del robot, y otro para conocer su aceleración.

readOrientationSensor()

Función: Lee la orientación en la que se encuentra el robot.

Devuelve:

• La orientación (Orientation). Ver Clase Orientation.

Clase Orientation

Representa la orientación del smartphone en base a estos ejes de referencia:

Atributos:

• yaw (float): Rotación en grados alrededor del eje Z. Los valores están en el rango de -180 a 180. El ángulo de giro yaw se consigue girando la base con los motores de las ruedas y el motor PAN.
• pitch (float): Rotación en grados alrededor del eje Y. Los valores están en el rango de -180 a 180. El ángulo de giro pitch se consigue usando el motor TILT o moviendo el robot por una superficie inclinada frontalmente.
• roll (float): Rotación en grados alrededor del eje X. Los valores están en el rango de -180 a 180. El ángulo de giro roll no lo puede conseguir Robobo directamente, pero cambiará si la superficie en la que se mueve tiene una inclinación lateral.

Para probar este método se cargará un mundo cualquiera en RoboboSim, ya que lo que va a hacer Robobo es girar sobre su eje sin avanzar. El siguiente programa realizará cuatro lecturas para ver cómo cambian los 3 parámetros del sensor de orientación:

• La primera lectura mostrará los valores antes de realizar cualquier movimiento
• La segunda lectura se hará después de girar a la derecha
• La tercera lectura se hará después de girar a la izquierda
• La cuarta y última lectura se hará al mover el TILT a 90º

from robobopy.Robobo import Robobo
rob = Robobo("localhost")
rob.connect()

#Se añade un retardo para que se haga la lectura bien, porque de lo contrario, dará todo ceros.
rob.wait(1)
print(f"Primera lectura: {rob.readOrientationSensor()} grados")
#Gira hacia la derecha
rob.wait(1)
rob.moveWheelsByTime(-2,2,3)
print(f"Segunda lectura: {rob.readOrientationSensor()} grados")
rob.wait(1)
#Gira hacia la izquierda
rob.moveWheelsByTime(3,-3,3)
print(f"Tercera lectura: {rob.readOrientationSensor()} grados")
rob.wait(1)
#Mueve el TILT a 90º
rob.moveTiltTo(90,15)
print(f"Cuarta lectura: {rob.readOrientationSensor()} grados")
rob.wait(1)

Una vez realizadas las pruebas, y tras haber visto las lecturas, se pueden llegar a esas conclusiones:

• Yaw siempre vale 0 cuando el robot se conecta. Si Robobo gira a la izquierda, el rango de valores va de 0 a 180. Y si gira hacia la derecha, el rango es de 0 a -180. Si por ejemplo, sigue girando hacia la derecha y se excede del rango, pasará a recibir los valores positivos del semicírculo izquierdo. Esta distribución también se aplica a pitch y roll.

• Pitch vale 15 cuando el robot se conecta, porque el TILT está a una inclinación de 75º por defecto, lo que quiere decir que el ángulo 0 es cuando el smartphone está de pie y recto. Cuanto más inclinado esté hacia atrás está el TILT o mayor en la inclinación del suelo, mayor será el valor de este eje. Y cuanto más esté inclinado hacia abajo, menor será el valor.

• Roll no cambió nada en esta prueba porque esta orientación depende de lateral del robot. Pero si la parte izquierda está en una posición más elevada que la derecha, el valor será mayor que 0, y si la derecha está más elevada, este valor será menor que 0. Esto se pueda dar en un suelo irregular o con baches, y es de utilidad para evitar que el robot pueda volcar.

Ahora que ya compredemos cómo funciona este sensor, probamos el siguiente programa, que hace lo siguiente:

• En un mundo cualquiera, se le pide al eje cardinal: norte, sur, este u oeste. Norte representa los 0 grados, Sur los 180, Este los 270, y Oeste los 90.
• Robobo girará sobre el eje z, y en ese momento se usará la lectura del yaw porque está asociado a los giros de ese eje. Girará hacia la derecha o hacia la izquierda dependiendo del tiempo que le tome hacer el giro. Por ejemplo, si el ángulo actual es 5, y el ángulo a desear es el de 180, girará hacia la izquierda porque le tomaría menos que girando hacia la derecha.
• Para que sea más fácil hacer las condiciones que se encargan de decidir si se gira hacia la derecha o hacia la izquierda, se ha añadido una condición que hace que los valores negativos del lado derecho sumen 360, de forma que la lectura sea de 0 a 360 en el sentido antihorario y no de 0 a 180 en el semicirculo izquierdo para pasar de -180 a 0 cuando se va al semicirculo derecho en el sentido antihorario.

NOTA: La conversión de (180,0) y (0,-180 ) grados a ser de (0 a 360) se puede aplicar también en pitch y roll en caso de que se desee realizar condiciones más sencillas.

from robobopy.Robobo import Robobo
rob = Robobo("localhost")
rob.connect()

#Se le pide al usuario el polo
polo=input("¿En qué polo quieres que mire Robobo? ¿Norte (N), Sur (S), Este (E) u Oeste(O)?: ")

#Dependiendo del polo, se asignara un angulo objetivo
match polo:
    case "N":
        ang=0
    case "O":
        ang=90
    case "E":
        ang=-90
    case "S":
        ang=180


#Primera lectura para determinar si tiene que girar a la izquierda o a la derecha dependiendo del trayecto más corto
lectura=rob.readOrientationSensor().yaw

#Bucle 
if lectura<ang or lectura>(ang+360):
    rob.moveWheels(4,-4)
elif lectura>ang or lectura<(ang+360):
    rob.moveWheels(-4,4)

while (lectura>ang+6 or lectura<ang-6):
    lectura=rob.readOrientationSensor().yaw
    print(lectura)
    rob.wait(0.1)

rob.stopMotors()
rob.disconnect()

readAccelerationSensor()

Función: Lee la aceleración a la que está sometida el robot.

Devuelve:

• La aceleración (Acceleration). Ver Clase Acceleration.

Clase Acceleration

Representa la aceleración a la que está sometido el robot en cada eje.

Atributos:

• x (float): aceleración en el eje X en m/s2.
• y (float): aceleración en el eje Y en m/s2.
• z (float): aceleración en el eje Z axis (m/s2).

Estos atributos hacen referencia al aumento o disminución de la velocidad de forma progresiva en cada eje.

El acelerómetro de un smartphone suele compartir el mismo eje XYZ que el del giroscopio que se emplea para medir la orientación. Actualmente en el simulador, los ejes son diferentes.

Los ejes del acelerómetro, cuando el smarthphone de Robobo está recto, son estos:

• x: Representa el movimiento horizontal (de lado a lado). Por ejemplo, si giras el móvil de izquierda a derecha o lo mueves lateralmente, el acelerómetro detecta cambios en este eje.
• z: Representa el movimiento vertical (de arriba abajo). Detecta, por ejemplo, cuando levantas o bajas el móvil.
• y: Representa el movimiento en profundidad (hacia adelante o atrás). Detecta cuando el móvil se mueve hacia adelante, hacia atrás o perpendicular al suelo.

En robótica móvil, el acelerómetro se suele usar para comprobar si el robot sufrió alguna caída o golpe, o si está atascado (los motores se mueven pero no hay aceleración). No es un sensor simple, por lo que dejamos como opcional probar el siguiente ejemplo de su uso:

IMPORTANTE: El siguiente ejemplo requiere tener la extensión matplotlib instalada. Matplotlib es una extensión de Python que permite realizar gráficas, y el ejemplo que váis a ejecutar va a crear tres gráficas distintas para cada eje del acelerómetro.

Abrid el mapa TEST PLAZE con la opción de "mundos aleatorios" desactivada y ejecutad el código de abajo:

• El programa dura 160 ciclos de 100 ms cada uno. En cada ciclo, se guardarán los valores leídos por la función del acelerómetro en 3 listas, una por cada eje de coordenadas.

NOTA: Los valores se convertirán de m/s^2 a mm/s^2 debido a que los valores leídos son demasiado bajos para usar la medida en metros.

• Cuando hayan pasado 20 ciclos, Robobo se moverá de forma constante a una velocidad de 50 durante 3 segundos. En este instante se deberá notar el primer pico en las gráficas.
• Cuando el programa esté en el ciclo número 100, Robobo se moverá de forma constante a su velocidad máxima (100) durante 3 segundos. En este momento se deberá notar el segundo pico en las gráficas.
• Cuando hayan los 160 ciclos, se crearán 3 gráficas mostrando las medidas de cada eje.

from robobopy.Robobo import Robobo
import matplotlib
import matplotlib.pyplot as plt
import numpy as np

rob = Robobo("localhost")
rob.connect()

#Declaración de variables y listas
timeout=0
X=[]
Y=[]
Z=[]


#Bucle que termina tras realizar 160 iteraciones
while timeout<160:

    #Dependiendo del valor del timeout, se activarán los comandos de las ruedas
    match timeout:

        case 20:
            rob.moveWheelsByTime(50,50,3)

        case 100:
            rob.moveWheelsByTime(100,100,3)

    rob.wait(0.1)
    #print(f"Lectura: {rob.readAccelerationSensor()} ")

    X.append((rob.readAccelerationSensor().x)*1000)
    Y.append((rob.readAccelerationSensor().y)*1000)
    Z.append((rob.readAccelerationSensor().z)*1000)
    timeout+=1

#Se paran los motores
rob.stopMotors()

#Se realizan las gráficas
plt.figure()

plt.ylabel('x (mm/s^2)', fontsize=15, color='blue')
plt.xlabel('cuentas', fontsize=15, color='blue')
plt.plot(range(1,161),X)
plt.figure()

plt.ylabel('y (mm/s^2)', fontsize=15, color='red')
plt.xlabel('cuentas', fontsize=15, color='red')
plt.plot(range(1,161),Y,color='red' )
plt.figure()

plt.ylabel('z (mm/s^2)', fontsize=15, color='green')
plt.xlabel('cuentas', fontsize=15, color='green')
plt.plot(range(1,161),Z,color='green' )
rob.disconnect()

Una vez se haya ejecutado el programa y realizado las gráficas, se van a explicar los resultados:

• La gráfica x suele dar esos valores debido a que a veces, Robobo se puede desviar ligeramente a la derecha o a la izquierda.
• La gráfica y es la que da los valores más altos debido a que Robobo se mueve hacia delante.
• La gráfica z es la que da los valores más bajos debido a que está avanzando en línea recta sin pasar por ninguna rampa o camino con un desnivel de altura.

Con esta prueba se quiere demostrar lo complejo que es utilizar el acelerómetro para ejercicios con Robobo, y que por las dimensiones del mismo, no todos los ejes darán los mismos valores aunque se hagan pruebas extremas.

Ejercicios

EJERCICIO 1

Carga el mundo FOUR WALLS, y mediante el método moveWheels(), intenta recrear el siguiente recorrido. Para hacer los giros debéis de utilizar el método readOrientationSensor().

Trata de utilizar el menor número de movimientos posible.

#INSERTAR CÓDIGO AQUÍ

EJERCICIO 2

Carga el mundo SLOPE, y haz que Robobo vaya hacia delante constantemente a una velocidad de 15. Cuando esté subiendo y note una inclinación de mínimo 4º, los leds Front-C, Front-LL y Front-RR deberán estar encendidos y de color azul, y Robobo debe de poner una cara de sorprendido. Si la inclinación es inferior a -2º, los leds Front-L y Front-R deberán estar encendidos y de color rojo, y Robobo debe de poner una cara de enfadado. Si no se cumple ninguna de las dos condiciones, los leds deberán estar apagados y Robobo debe de poner una cara normal.

El programa deberá estar en funcionamiento durante 45 segundos aproximadamente, luego se apagarán los motores y finalizará el programa.

#INSERTAR CÓDIGO AQUÍ