Localización de Robots Móviles

Diapositivas

PDF

En la semana 13 el robot corrigió su pose con los marcadores ArUco, pero le creía por completo al marcador y la pose saltaba. Esta semana formalizamos la localización probabilística: cómo combinar lo que el robot hace con lo que ve, y cuánto creerle a cada fuente.

Temas

Localización de Markov: la creencia como una probabilidad por lugar, actualización de percepción (Bayes), actualización de acción (suma total), filtro de partículas
Filtro de Kalman: fusionar dos medidas, ganancia de Kalman, predicción y corrección en 1D y en 2D, compuerta de validación
SLAM: estimar el robot y el mapa a la vez, cierre del lazo

Localización de Markov

El robot guarda una creencia $Bel(l)$ : una probabilidad para cada lugar $l$ del mapa, que suman 1. En cada paso la actualiza dos veces:

Acción: se mueve la creencia según lo que dicen los encoders $o_t$ , sumando todas las formas de llegar a cada lugar:
$P(l_t \mid o_t) = \sum_{l'} P(l_t \mid l', o_t)\, P(l')$
donde $l'$ es el lugar donde estaba y $P(l_t \mid l', o_t)$ es el modelo de movimiento. Moverse difumina la creencia.
Percepción: se multiplica por el modelo del sensor y se normaliza (regla de Bayes):
$P(l \mid i) = \frac{P(i \mid l)\, P(l)}{P(i)}$
donde $P(i \mid l)$ es la probabilidad de leer $i$ estando en $l$ , y $P(i)$ es la suma de todos los productos. Mirar afila la creencia.

El ejemplo del pasillo con tres puertas de las diapositivas está resuelto paso a paso en markov-puertas.xlsx: cada actualización con su ecuación y sus cálculos intermedios, celda por celda. Cambie el modelo del sensor, el de movimiento o las lecturas y vea cómo cambia la creencia.

Markov funciona aunque el robot no sepa dónde empezó, pero en una grilla de $x, y, \theta$ hay millones de celdas que actualizar. El filtro de partículas (localización de Monte Carlo) representa la creencia con unos cientos de hipótesis que se pesan con el sensor y se remuestrean.

Un caso real es RHINO, el robot guía del Deutsches Museum Bonn en 1997: usaba Markov en una grilla de 15 cm y 2°, y un filtro de entropía que solo acepta las lecturas que dejan al robot más seguro de su posición, para descartar las que tapan las personas.

Filtro de Kalman

Si la creencia es una campana de Gauss, basta guardar su media $\hat{x}$ y su varianza $P$ . Kalman alterna dos pasos:

Predecir con los encoders: $\hat{x}^- = \hat{x} + \Delta s$ y $P^- = P + Q$ , donde $\Delta s$ es el avance medido y $Q$ la varianza que agrega. La incertidumbre crece.
Corregir con una medida $z$ de varianza $R$ , por ejemplo la pose que da un marcador:
$K = \frac{P^-}{P^- + R} \qquad \hat{x} = \hat{x}^- + K\,(z - \hat{x}^-) \qquad P = (1 - K)\, P^-$
La diferencia $z - \hat{x}^-$ es la innovación y $K$ , entre 0 y 1, dice cuánto se le cree a la medida. Con $K = 1$ la pose salta a lo que dice el marcador, como en la semana 13; con $K = 0$ el marcador se ignora.

$Q$ y $R$ no se inventan: $R$ se mide con el robot quieto frente al marcador y $Q$ repitiendo recorridos.

En 2D la varianza es una matriz de covarianza $\Sigma$ de $(x, y, \theta)$ y se dibuja como una elipse. La odometría la propaga con los Jacobianos $F_p$ y $F_{rl}$ ; cuando el sensor no mide directamente la pose, se linealiza en cada paso: es el filtro de Kalman extendido (EKF).

Antes de corregir, conviene descartar las medidas imposibles con la distancia de Mahalanobis: si la innovación es mayor que unas 3 desviaciones, $d^2 = (z - \hat{x}^-)^2 / (P^- + R) > 9$ , probablemente el detector se equivocó.

SLAM

En SLAM (Simultaneous Localization and Mapping) el robot construye el mapa mientras se localiza en él. En EKF-SLAM el estado incluye la pose del robot y la posición de cada marca:

\mathbf{x} = (x,\ y,\ \theta,\ m_{1x},\ m_{1y},\ \dots,\ m_{Nx},\ m_{Ny})

Cada marca nueva hereda la incertidumbre del robot, y sus errores quedan correlacionados. Cuando el robot vuelve a ver una marca conocida, cierra el lazo: la corrección se propaga a todo el mapa. Los desafíos actuales son reconocer que se volvió a un lugar, los entornos que cambian y la escala.

Prácticas

Las prácticas de esta semana son demostrativas: se ejecutan en el simulador de RHINO, con el museo reconstruido a partir de la planta del artículo, para ver funcionando los tres bloques de la clase.

Markov: RHINO se ubica sin saber dónde arrancó, con una grilla de 15 cm y con partículas, y se recupera de un secuestro
Kalman: EKF con la odometría y las columnas del museo; qué pasa al apagar las marcas
SLAM: RHINO arma el mapa de columnas mientras se localiza, y cierra el lazo

Referencias

R. Siegwart, I. R. Nourbakhsh y D. Scaramuzza, Introduction to Autonomous Mobile Robots, MIT Press, cap. 5: Mobile Robot Localization.
D. Fox, W. Burgard, S. Thrun y A. B. Cremers, Position Estimation for Mobile Robots in Dynamic Environments, AAAI 1998.
W. Burgard, A. B. Cremers, D. Fox, D. Hähnel, G. Lakemeyer, D. Schulz, W. Steiner y S. Thrun, The Interactive Museum Tour-Guide Robot, AAAI 1998. Su Fig. 4 es la planta del museo del simulador.