MecanumRob

Tareas pendientes

• Sistema de gestión de batería
• Documentación: arquitectura del software (diagramas y ejemplos de código, información de parámetros relevantes y launchfiles).
• Actualizar página del CERLab con el Mecanum: Hardware, Software y videos de demostración.
• Prueba para determinar drift del giroscopio.
• Licencias para abrir el código y Copyright y sin garantía ("as is").
• Calibración de la odometría: sistema de adquisición de postura global basado en visión por computador.
• Obtener las covarianzas de los sensores: buscar los de la IMU en la página del fabricante.
• Calibración de SLAM.
• Fusión de los 2 sensores Hokuyo para el SLAM.
• Filtro de Kalman: implementación y filtrar mensajes por frecuencia más o menos constante.
• Deadman switch para ejecutar cualquier algoritmo si se presiona un botón.
• Buscar otros algoritmos SLAM.
• Propagar el software a los otros robots.
• Separar los puntos de anclaje para el transporte del robot.

Terminadas

• Calibración de la IMU en la página del fabricante.
  • Calibración del acelerómetros
  • Calibración del giroscopio
  • Filtro para IMU
• Botón para cambiar la velocidad máxima (análogo).
• Calibración de evasión de obstáculos y graficación/visualización en tiempo real.
• Buscar algoritmos de evasión ya implementados en ROS y hacer una comparativa.
  • Se encontraron los algoritmos: Elastic Band, Timed Elastic Band, Dynamic Window Approach y Active Scene Recognition. Todos los algoritmos están integrados al ROS Navigation Stack, para hacer una comparativa efectiva probablemente sea necesario adecuar el algoritmo VFH+ al estándar (migrar a C++ y separar funcionalidades de detección y evasión) y adecuar los nodos que ya se desarrollaron al estándar.

Setup y propagación del sistema

Instalación de ROS y compilación de paquetes

Asegúrese de que el Sistema Operativo es Ubuntu 16.04

Siga las instrucciones de instalación de ROS para este sistema operativo, para Ubuntu 16.04 deberá instalar ROS Kinetic. Se recomienda instalar la versión básica (ros-kinetic-ros-base). Asegúrese de configurar el catkin workspace.

Luego debe clonar los repositorios del Mecanum en el espacio de trabajo de catkin

 $ cd ~/catkin_ws/src
 $ git clone http://cerlab.ucr.ac.cr/git/danidim13/mecanumrob_ros.git
 $ git clone http://cerlab.ucr.ac.cr/git/danidim13/vfhp_ros.git

Estos repositorios incluyen dos paquetes que realmente no son necesarios para el funcionamiento del robot: mecanum_nav2d y vfhp_rviz_plugin. Podemos indicarle a catkin que no compile estos paquetes creando un archivo CATKIN_IGNORE en la raíz del repositorio.

 $ touch ~/catkin_ws/src/mecanumrob_ros/mecanumrob_nav2d/CATKIN_IGNORE
 $ touch ~/catkin_ws/src/vfhp_ros/vfhp_rviz_plugin/CATKIN_IGNORE

El paso final antes de compilar es instalar las dependencias de los paquetes, para hacer esto de manera automatizada usamos la herramienta rosdep

 $ rosdep install --from-paths ~/catkin_ws/src/mecanumrob_ros/mecanumrob_common -y
 $ rosdep install --from-paths ~/catkin_ws/src/mecanumrob_ros/mecanumrob_roboclaw -y
 $ rosdep install --from-paths ~/catkin_ws/src/mecanumrob_ros/mecanumrob_model -y
 $ rosdep install --from-paths ~/catkin_ws/src/vfhp_ros/vfhp_local_planner -y

Con esto estamos listos para compilar los paquetes

 $ cd ~/catkin_ws
 $ catkin_make
 $ source devel/setup.bash

Detección automática de hardware

Para la detección del hardware debemos generar la regla udev que se incluye en el paquete mecanumrob_common

 $ roscd mecanumrob_common
 $ ./scripts/install_rules

Este script copia el archivo plantilla mecanumrob_common/etc/89-mecanum.rules.template a /etc/udev/rules.d/. Además, para que la regla se ejecute correctamente agrega el path de las librerías de ROS a la configuración del linker. Para eso crea un archivo /etc/ld.so.conf.d/ros.conf con la ubicación de las librerías de ROS. Finalmente refresca el SO con la configuración nueva.

Con esto el robot debería asignar automáticamente nombres significativos a los dispositivos conectados. Los sensores se encuentran en /dev/sensors y los actuadores en /dev/actuators

Además de lo anterior debemos agregar el usuario al grupo dialout, esto le da permisos de escritura sobre los dispositivos seriales

 $ sudo usermod -a -G dialout <usuario>

Conexión al WiFi sin login

Para permitir a la NUC conectarse al wifi automáticamente puede configurarlo desde la interfaz gráfica del Network Manager.

• Desde la barra del sistema en esquina superior derecha haga click en el ícono de red, luego en Edit Connections...
• Seleccione la red y haga click en Edit.
• En la pestaña General marque las casillas Automatically connect to this network when it is available y All users may connect to this network

• 
• 
• 

Servicio

Para hacer que ROS corra automáticamente desde el arranque se usó el paquete robot_upstart. Lo que hace este paquete es crear un servicio del sistema operativo a partir de uno o mas .launch files.

$ sudo apt-get install ros-kinetic-robot-upstart

Corremos el script de instalación de la siguiente manera

$ rosrun robot_upstart install --job mecanumrob --interface wlp3s0 mecanumrob_common/launch/compa.manual.launch

• La opción --job mecanumrob indica el nombre del servicio.
• La opción --interface wlp3s0 indica al servicio esperar que levante la interfaz wlp3s0 (wifi) antes de correr el servicio
• Finalmente indicamos el launch file que queremos correr como un servicio del Sistema Operativo.

Al correr el comando se pedirán los credenciales de sudo para poder crear los archivos. Luego se indica que debe correr los siguientes comandos para finalizar la instalación

sudo systemctl daemon-reload && sudo systemctl start mecanumrob

Como un último detalle vamos a modificar el script de lanzamiento que crea robot_upstart para que en caso de no lograr connectarse al wifi igual corra ROS. El archivo que vamos a modificar es /usr/sbin/mecanumrob-start y la sección que vamos a modificar es la siguiente:

export ROS_IP=`rosrun robot_upstart getifip wlp3s0` 
if [ "$ROS_IP" = "" ]; then 
  log err "mecanumrob: No IP address on wlp3s0, cannot roslaunch." 
  exit 1 
fi 

Como se puede ver este código intenta obtener la IP de la interfaz de red del wifi, y si no lo logra finaliza con un código de error.

En vez de eso queremos que lo vuelva a intentar un par de veces para darle tiempo al sistema operativo de encontrar la red, y si definitavemente no logramos conectarnos que igualmente levante ROS localmente.

MAX_TRIES=5
RETRY_SLEEP_SEC=2
for i in $(seq 1 $MAX_TRIES);
do
  export ROS_IP=`rosrun robot_upstart getifip wlp3s0`

  if [ "$ROS_IP" = "" ]; then
    sleep $RETRY_SLEEP_SEC
    log warn "mecanumrob: No IP address on wlp3s0, will retry after ${RETRY_SLEEP_SEC} s."
  else
    log info "mecanumrob: IP address found on wlp3s0."
    break
  fi
done

if [ "$ROS_IP" = "" ]; then
  log err "mecanumrob: No IP address on wlp3s0, defaulting to HOSTNAME roslaunch."
  unset ROS_IP
  export ROS_HOSTNAME=$(hostname)
fi

Con esto en caso de no encontar la red wifi el robot esperará un segundo y lo volverá a intentar, hasta un máximo de 5 veces.

Si por alguna razón fuera necesario reiniciar el servicio (si por ejemplo algún nodo no responde) se puede usar el siguiente comando desde la terminal:

$ sudo service mecanumrob restart

Acceso Remoto

Para iniciar ROS en el mecanum desde una laptop debe conectarse por ssh, tiene que estar conectado al wifi del CERLab.

 $ ssh -X mecanumrob02@mecanumrob02.local

Una vez conectado se recomienda abrir una terminal gráfica (remota)

 $ gnome-terminal

Hay un servicio que corre ROS con algunos de los nodos en el startup, debe detenerlo

 $ sudo service mecanumrob stop

Finalmente se debe iniciar ROS y correr los launch files

 $ roscore

Desde otra pestaña remota (ctrl + shift + t) lanzar el launch file básico del robot, este launch file inicia los nodos que requiere la base móvil: motores sensores, modelo y odometría.

 $ roslaunch mecanumrob_common mecanum.launch
 

Para controlar el robot se puede usar el control de PlayStation. Desde otra pestaña remota:

 $ roslaunch mecanumrob_common dualshock4.launch

Para conectar rviz desde una laptop al ROS del mecanum es necesario definir ciertas variables de entorno, en particular ROS_MASTER_URI que le indica a ROS en qué computadora está corriendo el master, y ya sea ROS_IP o ROS_HOSTNAME que permite a los nodos identificar en qué máquina están corriendo.

Software

ROS

El MecanumRob usa los siguientes paquetes de ROS:

• Paquetes del repositorio del MecanumRob.
  • mecanumrob_common: contiene las definiciones de mensajes y servicios usados en los demás nodos, además de los .launch files, las reglas udev y el nodo para el control de play.
  • mecanumrob_roboclaw: el controlador de las motores, envía los comandos a los motores y publica la velocidad de las ruedas.
  • mecanumrob_model: el modelo cinemático del robot, tanto directo como inverso. Además el nodo que publica las transformaciones (próximamente el filtro de Kalman) y las pruebas para la odometría.
• vfhp_local_planner: el módulo de evasión de obstáculos.
• urg_node: controla un sensor Hokuyo.
• phidgets_imu: controla la IMU.
• gmapping: SLAM basado en las lecturas de los sensores láser.
• joy: maneja el control de play.

Arquitectura

Loren ipsum...

Loren impsum...

Hardware

Sensores y actuadores

• PhidgetsSpacial Precision 3/3/3 High Resolution [1]
• Roboclaw 2x30A Motor Controller (x2) [2]
• URG-04LX-UG01 Scanning Laser Rangefinder (x2) [3]

Calibración

IMU

Consultar la hoja del fabricante.

Para calibrar los sensores de la IMU tenemos que entender que hay dos fuentes importantes de error que debemos corregir. La primera es el ruido blanco o error de alta frecuencia (White Noise). Este se corrige promediando las mediciones a través del tiempo. El segundo tipo de error es el camino aleatorio o ruido de baja frecuencia (Random Walk o Drift). Este tipo de ruido tiende a hacer que el valor medio de las muestras se aleje del valor real con el paso del tiempo. La forma en que se corrige es que aproxima su efecto mediante un experimento y se resta a cada muestra. Para aproximar el Random Walk lo que se debe hacer es tomar mediciones durante un largo período (un día entero) sin mover el sensor, y ver cuanto divergen las mediciones de los valores iniciales. Sabiendo esta diferencia y el total de tiempo del experimento, se puede calcular un aproximado del error por unidad de tiempo. Para mayor información consultar este artículo sobre Desviación de Allan.

Para la calibración del acelerómetro, la principal fuente de error es el ruido blanco, por lo que la técnica más efectiva es promediar los valores sobre un intervalo de tiempo. En el caso del Phidgets, si se selecciona una tasa de datos menor a la máxima, el dispositivo automáticamente promedia tantas muestras como pueda en esa ventana de tiempo. Para más información consultar este artículo del fabricante

En el caso del giroscopio, la caminata aleatoria rápidamente se vuelve una fuente importante de error. Hay dos formas de contrarrestar esto, usando la función de cero del girosocopio, o restando un estimado del error a cada muestra. Eso sí, para poner en cero el giroscopio el robot debe estar inmóvil, por lo que se recomienda una combinación de ambas técnicas, constantemente restar el error y cada cierto tiempo prudencial usar la función de cero.

Para la calibración del magnetómetro es necesario seguir las instrucciones de instalación de la librería phidgets para Ubuntu 16.04 y luego descargar y compilar el programa de calibración dado por el fabricante. En el comprimido se incluye un README con instrucciones. Para más información ver [4] y [5].

Modelado

Marcos de referencia

Primero es necesario definir el sistema de coordenadas del robot (local) y el estático (global).

Se define el marco de referencia local tal que el eje $X_R$ corresponde al frente del robot y el eje $Y_R$ corresponde al lado izquierdo. Además, se define la velocidad angular de las ruedas $\dot{\mathit{\phi }}={\left(\begin{array}{cccc}{\dot{\phi }}_1& {\dot{\phi }}_2& {\dot{\phi }}_3& {\dot{\phi }}_4\end{array}\right)}^T$, tal que una rotación positiva de todas las ruedas hace que el robot avance hacia adelante en el eje $X_R$. Más formalmente, una velocidad angular positiva usando la ley de la mano derecha se proyecta en dirección $Y_R$ positivo.

Luego, se define el marco de referencia global con ejes $X_G$, $Y_G$. Para encontrar la posición del robot en este marco de referencia global al partir del movimiento de las ruedas se usa el modelo cinemático directo. Por otro lado, para determinar los comandos que se le deben enviar a los motores para alcanzar una cierta velocidad lineal o angular se usa el modelo cinemático inverso.

Modelo Cinemático Directo

El modelo cinemático directo nos permite obtener la velocidad del robot a partir de la velocidad de las ruedas. Definimos la velocidad respecto al marco de referencia local ${\dot{\mathit{u}}}_R={\left(\begin{array}{ccc}{\dot{x}}_R& {\dot{y}}_R& {\dot{\theta }}_R\end{array}\right)}^T$. Además queremos encontrar la velocidad respecto al marco de referencia global ${\dot{\mathit{u}}}_G={\left(\begin{array}{ccc}{\dot{x}}_G& {\dot{y}}_G& \dot{\theta }\end{array}\right)}^T$, para poder integrar y así obtener la posición. Para el robot mecanum se tiene que:

${\dot{\mathit{u}}}_R=\mathit{J}\cdot \dot{\mathit{\phi }}$

$\left(\begin{array}{c}{\dot{x}}_R\\ {\dot{y}}_R\\ {\dot{\theta }}_R\end{array}\right)=\mathit{J}\left(\begin{array}{c}{\dot{\phi }}_1\\ {\dot{\phi }}_2\\ {\dot{\phi }}_3\\ {\dot{\phi }}_4\end{array}\right)$

con

$\mathit{J}=\frac{r}{4}\left(\begin{array}{cccc}1& 1& 1& 1\\ 1& -1& 1& -1\\ \frac{1}{l_a+l_b}& -\frac{1}{l_a+l_b}& -\frac{1}{l_a+l_b}& \frac{1}{l_a+l_b}\end{array}\right)$

Con esto obtenemos la velocidad lineal y angular del robot respecto al marco de referencia local. Luego, para obtener velocidades globales se debe multiplicar por la matriz de rotación $\mathit{R}(\theta )$

$\mathit{R}(\theta )=\left(\begin{array}{ccc}cos(\theta )& -sin(\theta )& 0\\ sin(\theta )& cos(\theta )& 0\\ 0& 0& 1\end{array}\right)$

Así, el modelo completo del movimiento del robot es:

${\dot{\mathit{u}}}_G=\mathit{R}(\theta )\cdot \mathit{J}\cdot \dot{\mathit{\phi }}$

$\left(\begin{array}{c}{\dot{x}}_G\\ {\dot{y}}_G\\ \dot{\theta }\end{array}\right)=\frac{r}{4}\left(\begin{array}{ccc}cos(\theta )& -sin(\theta )& 0\\ sin(\theta )& cos(\theta )& 0\\ 0& 0& 1\end{array}\right)\left(\begin{array}{cccc}1& 1& 1& 1\\ 1& -1& 1& -1\\ \frac{1}{l_a+l_b}& -\frac{1}{l_a+l_b}& -\frac{1}{l_a+l_b}& \frac{1}{l_a+l_b}\end{array}\right)\left(\begin{array}{c}{\dot{\phi }}_1\\ {\dot{\phi }}_2\\ {\dot{\phi }}_3\\ {\dot{\phi }}_4\end{array}\right)$

Modelo Cinemático Inverso

El modelo cinemático inverso se usa para determinar las velocidades de las ruedas necesarias para obtener un movimiento determinado. Es decir a partir de un comando de velocidad en el marco de referencia local (muévase hacia adelante a 3 m/s) determina la velocidad a la que debe rotar cada rueda para obtener el movimiento local deseado.

$\dot{\mathit{\phi }}=\mathit{H}\cdot {\dot{\mathit{u}}}_R$

$\left(\begin{array}{c}{\dot{\phi }}_1\\ {\dot{\phi }}_2\\ {\dot{\phi }}_3\\ {\dot{\phi }}_4\end{array}\right)=\frac{1}{r}\left(\begin{array}{ccc}1& 1& l_a+l_b\\ 1& -1& -(l_a+l_b)\\ 1& 1& -(l_a+l_b)\\ 1& -1& l_a+l_b\\ \end{array}\right)\left(\begin{array}{c}{\dot{x}}_R\\ {\dot{y}}_R\\ \dot{\theta }\end{array}\right)$

Referencias

1. C. Röhrig, D. Heß, C. Kirsch and F. Künemund, "Localization of an omnidirectional transport robot using IEEE 802.15.4a ranging and laser range finder", 2010 IEEE/RSJ International Conference on Intelligent Robots and Systems, Taipei, 2010, pp. 3798-3803.
2. D. Hess, F. Kuenemund and C. Roehrig, "Simultaneous Calibration of Odometry and external Sensors of Omnidirectional Automated Guided Vehicles (AGVs)", Proceedings of ISR 2016: 47st International Symposium on Robotics, Munich, Germany, 2016, pp. 1-8.
3. C. Röhrig, D. Heß and F. Künemund, "Motion controller design for a mecanum wheeled mobile manipulator", 2017 IEEE Conference on Control Technology and Applications (CCTA), Mauna Lani, HI, 2017, pp. 444-449. doi: 10.1109/CCTA.2017.8062502
4. L. Marín, "Modular Open Hardware Omnidirectional Platform for Mobile Robot Research*", 2018 IEEE 2nd Colombian Conference on Robotics and Automation (CCRA), Barranquilla, 2018, pp. 1-6. doi: 10.1109/CCRA.2018.8588120