MecanumRob

Tareas pendientes

• Calibración de la odometría.
• Migrar código de la base a C++.
• Obtener las covarianzas de los sensores.
• Filtro para la IMU y obtención de postura(comparar con el filtro Madgwick que viene por defecto con el nodo).
• Filtro de Kalman.
• Calibración de SLAM.
• Fusión de los 2 sensores Hokuyo para el SLAM.

Modelado

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 $\boldsymbol{\dot{\varphi}} = \begin{pmatrix} \dot{\varphi}_1 & \dot{\varphi}_2 & \dot{\varphi}_3 & \dot{\varphi}_4 \end{pmatrix}^T$ según la ley de la mano derecha tal que una rotación positiva 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 $\boldsymbol{\dot{u}}_R = \begin{pmatrix} \dot{x}_R & \dot{y}_R & \dot{\theta}_R \end{pmatrix}^T$. Además queremos encontrar la velocidad respecto al marco de referencia global $\boldsymbol{\dot{u}}_G = \begin{pmatrix} \dot{x}_G & \dot{y}_G & \dot{\theta} \end{pmatrix}^T$, para poder integrar y así obtener la posición. Para el robot mecanum se tiene que:

$\boldsymbol{\dot{u}}_R = \boldsymbol{J}\cdot\boldsymbol{\dot{\varphi}}$

$\begin{pmatrix} \dot{x}_R\\ \dot{y}_R\\ \dot{\theta}_R \end{pmatrix} = \boldsymbol{J} \begin{pmatrix} \dot{\varphi}_1\\ \dot{\varphi}_2\\ \dot{\varphi}_3\\ \dot{\varphi}_4 \end{pmatrix}$

con

$\boldsymbol{J} = \frac{r}{4}\begin{pmatrix} 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{pmatrix}$

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 $\boldsymbol{R}(\theta)$

$\boldsymbol{R}(\theta) = \begin{pmatrix} cos(\theta) & -sin(\theta) & 0 \\ sin(\theta) & cos(\theta) & 0 \\ 0 & 0 & 1 \end{pmatrix}$

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

$\boldsymbol{\dot{u}}_G = \boldsymbol{R}(\theta)\cdot\boldsymbol{J}\cdot\boldsymbol{\dot{\varphi}}$

$\begin{pmatrix} \dot{x}_G \\ \dot{y}_G \\ \dot{\theta} \end{pmatrix} = \frac{r}{4} \begin{pmatrix} cos(\theta) & -sin(\theta) & 0 \\ sin(\theta) & cos(\theta) & 0 \\ 0 & 0 & 1 \end{pmatrix}\begin{pmatrix} 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{pmatrix} \begin{pmatrix} \dot{\varphi}_1\\ \dot{\varphi}_2\\ \dot{\varphi}_3\\ \dot{\varphi}_4 \end{pmatrix}$