Robótica¶

Localización¶

NOTAS

Bienvenidos a la semana 15, la última de teoría del bloque de plataformas móviles. En la semana 12 aprendimos a estimar la pose con los encoders, y vimos que el error crece sin límite. En la 13 le dimos ojos al robot: con la cámara y los marcadores ArUco pudo corregir su pose en cada esquina del cuadrado.

Pero en la semana 13 hicimos algo muy brusco: cuando la cámara veía un marcador, le creíamos por completo y la pose saltaba. Hoy vamos a responder la pregunta que quedó abierta en el ejercicio: ¿hay que creerle siempre por completo al marcador, o conviene mezclar lo que dice con lo que dice la odometría?

Para eso vamos a ver las dos grandes familias de localización probabilística, la de Markov y el filtro de Kalman, y vamos a terminar con el problema más difícil: construir el mapa mientras el robot se localiza en él, que es SLAM.

🎨 Prompt sugerido: "Pixel art of a small LEGO-style robot that has ONLY TWO wheels in total, one on each side of its body like a two-wheeled balancing robot, seen from the side and small in the lower third of the image, standing on a large floor map drawn like a treasure map with a dotted path, surrounded by a soft glowing oval halo on the floor, with several faint translucent ghost copies of the same robot scattered around it, a few square black-and-white fiducial marker tiles on the floor, plain calm evening sky filling the whole upper half for a title. 16-bit retro style, purple, teal and warm grey palette, no text."

Plan de hoy: tres bloques¶

#	Bloque	Pregunta que responde
1	Localización de Markov	¿Cómo combina el robot lo que hace con lo que ve, si no sabe dónde empezó?
2	Filtro de Kalman	¿Cuánto le creo a la odometría y cuánto al marcador?
3	SLAM	¿Y si nadie le dio el mapa al robot?

Al final: los tres bloques funcionando en RHINO, en el simulador.

NOTAS

La clase tiene tres bloques. En el primero formalizamos lo que vimos con el pasillo de las tres puertas en la semana 13: la creencia del robot como una probabilidad para cada lugar, y las dos operaciones que la actualizan, mover y mirar. Es la localización de Markov, y es la más general: funciona aunque el robot no tenga ni idea de dónde empezó.

En el segundo vamos a ver el filtro de Kalman, que es el caso particular en que la creencia es una campana de Gauss. Es mucho más eficiente y es exactamente lo que necesitamos para responder la pregunta del cuadrado: cuánto le creo a cada fuente.

En el tercero quitamos el mapa: el robot tiene que dibujarlo mientras se localiza. Cerramos con las prácticas de la semana: los tres bloques funcionando en RHINO, el robot guía que vamos a conocer hoy, dentro de una reconstrucción del museo de Bonn.

Localización de Markov¶

NOTAS

Imaginen que los despiertan con los ojos vendados en algún piso del bloque 19 y les quitan la venda frente a una puerta. ¿Dónde están? Podría ser cualquier puerta del pasillo. Caminan unos pasos, ven el extintor, y ya descartan la mitad de las opciones. Caminan otro poco, ven la escalera, y ya saben exactamente dónde están.

Eso es localización de Markov: mantener todas las hipótesis a la vez, cada una con su probabilidad, y dejar que el movimiento y las observaciones vayan eliminando las que no cuadran.

🎨 Prompt sugerido: "Pixel art of a long school corridor with three identical doors along the wall, a small robot that has ONLY TWO wheels in total, one on each side of its body like a two-wheeled balancing robot, rolling along the corridor, with three ghostly translucent copies of the same robot standing in front of each door to show possible positions, one copy glowing brighter than the others. 16-bit retro style, blue, brown and cream palette, no text."

Localización de Markov¶

No description has been provided for this image

La creencia $Bel(l)$ asigna una probabilidad a cada lugar $l$ del mapa, y suman 1
Es el pasillo de la semana 13, ahora con 10 celdas de 1 m y números

NOTAS

Volvamos al pasillo de la semana 13, pero ahora discreto: 10 celdas de un metro, con puertas en las celdas 1, 3 y 7, en café. La creencia del robot es una barra por celda: la probabilidad de estar ahí. Siempre suman uno.

Al principio el robot no sabe nada: 0.1 en cada celda. Ve una puerta y la creencia se concentra en las tres puertas, cada una con 0.26. Avanza 2 metros: los tres picos se corren dos celdas y se ensanchan, porque el movimiento también es incierto. Vuelve a ver una puerta, y solo una hipótesis es consistente: la celda 3, con 0.67.

Fíjense que no es 1. El robot sigue guardando un poco de duda: tal vez el sensor se equivocó. Esa es la gran ventaja de esta representación: el robot sabe cuánto sabe. En las dos diapositivas siguientes vamos a sacar estos números a mano.

La animación va paso por paso. La línea punteada es la creencia del paso anterior, para comparar, y el recuadro dice qué regla se está aplicando. Deténganla en cada etapa y pregunten qué va a pasar antes de seguir.

Actualización de percepción: Bayes¶

$$P(l \mid i) = \frac{P(i \mid l)\, P(l)}{P(i)}$$

$P(l)$: creencia antes de mirar
$P(i \mid l)$: modelo del sensor: probabilidad de leer $i$ estando en $l$
$P(i)$: suma de todos los productos: normaliza
$P(l \mid i)$: creencia después de leer $i$

Celdas	Antes	P(veo puerta)	Producto	Después
con puerta (3)	0.1	0.9	0.09	0.09 / 0.34 = 0.26
sin puerta (7)	0.1	0.1	0.01	0.01 / 0.34 = 0.03
suma	1.0		0.34	1.0

NOTAS

La primera operación es mirar. La regla de Bayes dice que la probabilidad de estar en el lugar l después de leer i es proporcional a dos cosas: qué tan probable era estar ahí antes, P de l, y qué tan probable es leer eso desde ahí, P de i dado l. Ese segundo término es el modelo del sensor junto con el mapa: en una celda con puerta, el sensor dice "puerta" el 90 % de las veces; en una celda sin puerta, se equivoca y dice "puerta" el 10 %.

Hagámoslo en el tablero. Antes, todas valen 0.1. Multiplicamos: las tres celdas con puerta quedan en 0.09 y las siete sin puerta en 0.01. Eso suma 3 por 0.09 más 7 por 0.01, 0.34, y no uno. El denominador P de i es justamente eso: la suma, que sirve para normalizar. Dividimos y quedan 0.26 y 0.03.

Noten que las celdas sin puerta no quedan en cero. Un sensor que se equivoca el 10 % de las veces no puede descartar nada por completo. Este es el robot Dervish, que ganó un concurso en 1994 localizándose con sonares en un mapa de pasillos y puertas, con exactamente este tipo de tabla.

Actualización de acción: suma total¶

$$P(l_t \mid o_t) = \sum_{l'} P(l_t \mid l', o_t)\; P(l')$$

$l'$: celda donde estaba · $l_t$: celda donde estoy ahora · $o_t$: la odometría, lo que dicen los encoders
$P(l')$: creencia anterior · $P(l_t \mid l', o_t)$: modelo de movimiento (la tabla)

Orden: avanzar 2 celdas	Probabilidad
quedó en $l' + 1$ (patinó)	0.1
quedó en $l' + 2$	0.8
quedó en $l' + 3$ (se pasó)	0.1

Cada pico se corre y se ensancha: moverse siempre agrega incertidumbre

NOTAS

La segunda operación es moverse. La probabilidad de estar ahora en una celda se arma sumando todas las formas de haber llegado ahí: desde cada celda anterior l prima, la probabilidad de haber estado ahí por la probabilidad de que el movimiento me haya traído de allá. Es el teorema de probabilidad total, y en el libro aparece como una integral; con celdas es una suma.

El modelo de movimiento sale del robot: si le pido avanzar dos celdas, el 80 % de las veces avanza dos, y el resto se queda corto o se pasa una. Eso no es más que el patinaje de la semana 12 escrito como probabilidades.

Fíjense en el efecto: la percepción afila la creencia, el movimiento la difumina. Un robot que solo se mueve, sin mirar, termina con una creencia cada vez más plana. Es la misma nube que crecía en los cuadrados de la semana 13, ahora en lenguaje de probabilidades.

El ciclo de Markov¶

La suposición de Markov: la creencia nueva solo depende de la anterior y de la última acción y lectura

En Python¶

puerta = np.array([0, 1, 0, 1, 0, 0, 0, 1, 0, 0])
bel = np.ones(10) / 10                        # no sé dónde estoy

def percibir(bel, veo_puerta):
    p = np.where(puerta == veo_puerta, 0.9, 0.1)   # P(lectura | celda)
    bel = p * bel                                  # Bayes
    return bel / bel.sum()                         # normalizar

def mover(bel, celdas):
    nueva = np.zeros_like(bel)
    for paso, q in [(celdas - 1, 0.1), (celdas, 0.8), (celdas + 1, 0.1)]:
        nueva[paso:] += q * bel[:len(bel) - paso]  # suma total
    return nueva / nueva.sum()

bel = percibir(bel, 1); bel = mover(bel, 2); bel = percibir(bel, 1)

¿Y en un piso de verdad?¶

Grilla de 2 × 2 m	Celdas a actualizar en cada paso
1 cm y 1°	200 × 200 × 360 = 14.4 millones
5 cm y 5°	40 × 40 × 72 = 115 200

La resolución de la creencia no puede ser mejor que la de la grilla
Con celdas gruesas el cálculo es manejable, pero se pierde precisión

NOTAS

En un pasillo de 10 celdas, Markov es trivial. Pero un robot real tiene pose en dos dimensiones y una orientación. Si tomamos el piso del simulador, 2 por 2 metros, con celdas de 1 cm y 1 grado, son más de 14 millones de celdas, y en cada paso hay que multiplicar todas por el modelo del sensor y sumarlas con el modelo de movimiento.

Si la grilla es gruesa, el cálculo es manejable, pero la precisión queda limitada por el tamaño de la celda: el robot nunca va a saber su pose mejor que una celda.

¿Entonces para qué sirve? La gran ventaja de Markov es que funciona aunque el robot no tenga ni idea de dónde está: se arranca con una creencia uniforme y el robot se encuentra solo. Eso se llama localización global, y una creencia de una sola hipótesis no la puede hacer. Veamos un robot que la usó en un lugar muy difícil: un museo lleno de gente.

RHINO: guía de museo¶

Deutsches Museum Bonn, mayo de 1997
operación	6 días · 47 horas
recorrido	18.6 km · hasta 80 cm/s
visitantes	más de 2000 en el museo
tours pedidos	2400 · 99.75 % cumplidos
sensores	láser, sonar, infrarrojo y táctil

Fotos: proyecto RHINO, Universidad de Bonn · Datos: Burgard et al., The Interactive Museum Tour-Guide Robot, AAAI 1998

NOTAS

RHINO fue un robot guía de museo, de las universidades de Bonn y Carnegie Mellon. En mayo de 1997 trabajó seis días en el Deutsches Museum de Bonn: recibía a los visitantes, les ofrecía un recorrido, los llevaba de una exhibición a otra y les explicaba cada una. En la foto, desde el balcón, el cilindro negro con rojo en el centro del grupo es RHINO.

Miren los números de la tabla, que son los del artículo de los autores: 47 horas de operación, 18.6 km recorridos, más de 2000 visitantes y 2400 recorridos pedidos, de los que cumplió el 99.75 %. Además, por una página web, personas de todo el mundo podían ver el museo a través de él y mandarlo a una exhibición.

Lo interesante para nosotros es el ambiente. Un museo es lo opuesto a nuestro laboratorio: está lleno de gente que rodea al robot, que le tapa los sensores y que se mueve todo el tiempo. Y aun así el robot tenía que saber dónde estaba con precisión de centímetros, porque muchas exhibiciones eran frágiles. En la siguiente diapositiva vemos cómo lo logró.

¿Cómo se localizaba RHINO?¶

Markov en una grilla de $(x, y, \theta)$ de 15 cm y 2°: Bayes al medir, suma total al moverse
Arrancaba con creencia uniforme: localización global
Filtro de entropía: descarta las lecturas tapadas por personas
Obstáculos invisibles (vitrinas, barras, consolas): con la pose más probable y el mapa genera lecturas virtuales
Error menor que 15 cm en 118 puntos de referencia

Fox, Burgard, Thrun y Cremers, Position Estimation for Mobile Robots in Dynamic Environments, AAAI 1998

NOTAS

Lo primero que hay que notar es que RHINO usaba exactamente las dos ecuaciones de hoy: Bayes con un normalizador al medir y el teorema de probabilidad total al moverse, con la pose en x, y, θ. La grilla tenía celdas de 15 cm y 2 grados: comparen con la tabla de la diapositiva anterior. Cuando el robot no sabía dónde estaba, la creencia empezaba uniforme y se iba concentrando, como en nuestro pasillo.

El problema que no tiene nuestro pasillo son las personas. Si alguien se para frente al láser, la lectura dice "hay una pared a 30 cm" donde el mapa no tiene nada, y Bayes castigaría las hipótesis correctas. El artículo de Fox y sus colegas, que está citado abajo, propone un filtro de entropía: la entropía mide la incertidumbre de la creencia, y cada lectura se acepta solo si la hace bajar, es decir, si deja al robot más seguro de su posición. Las lecturas tapadas por personas se descartan. Para no quedarse ciego si se equivocaba, siempre dejaba entrar además unas pocas lecturas al azar. Con datos grabados en el museo en las horas de más gente, el método estimó bien la posición en más del 98 % de los casos, donde Markov sin filtro se perdía.

La foto muestra el otro problema: la consola que sobresale de la exhibición queda por encima de los sensores, y las vitrinas de vidrio y las barras metálicas tampoco se ven. La solución fue usar la localización: con la pose más probable y el mapa, el robot calculaba dónde estaban esos obstáculos y los agregaba como lecturas virtuales al evitador de colisiones. Por eso la precisión importaba tanto: el error medido fue siempre menor que 15 cm, y los autores reportan una sola vez en los seis días en que superó su margen de seguridad de 30 cm.

RHINO en video¶

"Rhino the tour guide robot" · Wolfram Burgard

Filtro de partículas¶

En lugar de todas las celdas, unas cientos de hipótesis que se pesan y se remuestrean

NOTAS

La salida al costo de la grilla es no representar todas las celdas, sino una muestra. Cada punto azul es una partícula: una hipótesis de dónde está el robot. Al principio se riegan al azar por todo el pasillo.

Cuando el robot ve una puerta, a cada partícula se le da un peso con el mismo modelo del sensor de antes: 0.9 si está frente a una puerta, 0.1 si no. Luego se sortean 300 partículas nuevas en proporción a esos pesos: las que estaban frente a puertas se multiplican y las demás tienden a desaparecer. Eso es remuestrear. Cuando el robot avanza, cada partícula avanza 2 metros con un poco de ruido distinto, y al volver a ver una puerta la mayoría queda en la celda 3.

Esto es localización de Monte Carlo, y es lo que usan muchos robots de servicio y el paquete de navegación de ROS. Con unos cientos o miles de partículas se hace el trabajo de millones de celdas, y el número de partículas se puede bajar cuando el robot está seguro de dónde está.

En la animación, cuando el robot ve una puerta, el tamaño de cada punto es su peso; el recuadro de abajo cuenta qué fracción de las partículas está en la celda 3: empieza en un 11 % y termina cerca de la mitad.

Filtro de Kalman¶

NOTAS

Piensen en dos amigos que les indican cuánto falta para llegar a un sitio. Uno es muy confiable y el otro se equivoca a menudo. Si dicen cosas distintas, nadie haría el promedio simple: le harían más caso al confiable, pero sin ignorar del todo al otro.

El filtro de Kalman es esa intuición convertida en una fórmula: un promedio pesado, donde el peso de cada fuente depende de qué tan confiable es. Lo usó el computador de navegación del Apolo para llegar a la Luna, y hoy está en el GPS de su celular.

🎨 Prompt sugerido: "Pixel art of a small robot that has ONLY TWO wheels in total, one on each side of its body like a two-wheeled balancing robot, balancing on a seesaw scale, with a glowing gear-shaped wheel icon on one side of the scale and a square black-and-white fiducial marker tile on the other side, two overlapping bell-shaped curves of light above them merging into one narrower bright curve. 16-bit retro style, green, red and dark blue palette, no text."

Filtro de Kalman¶

Si la creencia es una campana de Gauss	basta guardar
posición	la media $\hat{x}$
incertidumbre	la varianza $P = \sigma^2$ (en 2D, la covarianza $\Sigma$)

Markov con muchas celdas → Kalman con dos números por variable
Supone: modelos lineales, ruido gaussiano y de media cero, una sola hipótesis

NOTAS

El filtro de Kalman es la localización de Markov cuando la creencia es una campana de Gauss. Y una campana se describe con solo dos números: dónde está su centro, la media, y qué tan ancha es, la varianza. En lugar de millones de celdas, el robot guarda su pose y una matriz de 3 por 3. Por eso Kalman es tan eficiente.

El precio son las suposiciones. Los modelos deben ser lineales, o al menos casi lineales, y el ruido gaussiano. Y la más importante: hay una sola hipótesis. Kalman no puede representar el pasillo de las tres puertas; si el robot no sabe más o menos dónde está, Kalman no sirve. Tiene que arrancar desde una pose conocida, como en nuestro cuadrado, donde el robot parte sobre el marcador 1.

Para nosotros eso no es un problema, porque los marcadores ArUco no tienen aliasing: cada uno tiene su ID. La pregunta deja de ser "¿en cuál de las puertas estoy?" y pasa a ser "¿cuánto le creo a lo que dice el marcador?".

Fusionar dos medidas¶

$$\hat{x} = \frac{\sigma_2^2\, x_1 + \sigma_1^2\, x_2}{\sigma_1^2 + \sigma_2^2} \qquad \sigma^2 = \frac{\sigma_1^2\, \sigma_2^2}{\sigma_1^2 + \sigma_2^2}$$

$x_1,\ \sigma_1^2$: odometría y su varianza · $x_2,\ \sigma_2^2$: marcador y su varianza · $\hat{x},\ \sigma^2$: fusión y su varianza

NOTAS

Empecemos con lo más simple: dos medidas de la misma posición. La odometría dice que el robot está en 52 cm, con una desviación de 3 cm, porque ya lleva un rato acumulando error. El marcador dice 49.5 cm, con una desviación de 1 cm. ¿Dónde está el robot?

La respuesta óptima, en el sentido de mínimos cuadrados, es un promedio pesado donde cada medida pesa en proporción inversa a su varianza. El marcador tiene varianza 1 y la odometría 9, así que el marcador pesa nueve veces más: 49.75 cm. Hagan la cuenta: 1 por 52 más 9 por 49.5, dividido entre 10.

Y lo más bonito es la varianza del resultado: 9 por 1 sobre 10, 0.9, o sea una desviación de 0.95 cm. Es menor que la de las dos medidas. Combinar dos fuentes siempre da algo mejor que la mejor de ellas, aunque la otra sea mala. Por eso no conviene botar la odometría cuando aparece el marcador.

La animación cambia la desviación del marcador de 1 a 6 cm y vuelve. Antes de que llegue al extremo, pregúnteles hacia dónde se va a mover la curva verde. Con un marcador ruidoso, el peso K baja y la fusión se acerca a la odometría; y en ningún momento la verde es más ancha que la más angosta de las otras dos.

La ganancia de Kalman¶

$$K = \frac{P^-}{P^- + R}$$

$$\hat{x} = \hat{x}^- + K\,(z - \hat{x}^-) \qquad P = (1 - K)\, P^-$$

$\hat{x}^-,\ P^-$: predicción y su varianza
$z,\ R$: medida del marcador y su varianza
$z - \hat{x}^-$: innovación
$K$: ganancia, entre 0 y 1
$\hat{x},\ P$: estimación corregida y su varianza

Ejemplo: P⁻ = 9 cm², R = 1 cm²	K = 9 / 10 = 0.9
innovación	49.5 − 52 = −2.5 cm
corrección: K × innovación	0.9 × (−2.5) = −2.25 cm → 49.75 cm
K → 1 (marcador perfecto)	le creo al marcador: lo de la semana 13
K → 0 (marcador malo)	lo ignoro: solo odometría

NOTAS

La misma fórmula se reescribe en la forma que usa el filtro de Kalman. Se parte de la predicción, x gorro menos, con su varianza P menos. Llega una medida z con varianza R. La diferencia entre lo que vi y lo que esperaba ver se llama innovación: es la información nueva. Y la predicción se corrige sumando una fracción K de la innovación.

K es la ganancia de Kalman, y es un número entre cero y uno que dice cuánto le creo a la medida. Con nuestros números da 0.9: corrijo el 90 % de la diferencia, y llego a los mismos 49.75 cm de antes.

Miren los dos extremos. Si el marcador fuera perfecto, R igual a cero, K vale uno y la pose salta a lo que dice el marcador: eso es exactamente lo que hicimos en el ejercicio de la semana 13. Si el marcador fuera muy malo, K tiende a cero y nos quedamos con la odometría. Kalman decide en cada esquina el punto intermedio, según cuánta incertidumbre lleve acumulada el robot.

Predecir y corregir¶

Predecir: $\hat{x}^- = \hat{x} + \Delta s$, $\;P^- = P + Q$ · la incertidumbre crece
$\Delta s$: avance medido con los encoders · $Q$: varianza que agrega ese avance
Corregir con la marca y la ganancia $K$: $P$ cae · el error queda acotado

NOTAS

Ahora pongamos el robot a andar en línea recta 2 metros. En cada paso, la predicción suma a la posición lo que dicen los encoders, y suma a la varianza un poco, Q, proporcional a lo recorrido. Es el modelo de error de la odometría. Cada medio metro, la cámara ve una marca y se corrige.

En rojo, el error de la odometría sola: los encoders de este ejemplo miden un 3 % de más, como un radio de rueda mal medido, y el error crece hasta unos 5 cm. En verde, el error de Kalman: cada vez que ve una marca vuelve a casi cero. La franja verde es lo que Kalman cree que es su error, más o menos dos sigmas: es el diente de sierra de la semana 13, pero ahora calculado por el propio filtro.

Fíjense también en dos detalles. Al ver la marca, el error verde no cae a cero sino a una fracción: K no es uno, porque la marca también tiene error. Y entre marcas el error verde siempre sube, nunca baja: el 3 % es sistemático y el filtro supone ruido de media cero. Kalman no reemplaza la calibración: un robot bien calibrado le cumple mejor las suposiciones al filtro.

Los números sobre cada línea punteada son la K de esa corrección: alrededor de 0.7, no 1. El filtro le cree bastante a la marca, pero no del todo.

En Python¶

x, P = 0.0, 0.0              # estimación (m) y varianza (m²)
Q_POR_METRO = 0.0004         # varianza que agrega la odometría por metro
R = 0.01 ** 2                # la marca: σ = 1 cm

def predecir(x, P, ds):      # ds: avance medido con los encoders
    return x + ds, P + Q_POR_METRO * abs(ds)

def corregir(x, P, z):       # z: posición medida con la marca
    K = P / (P + R)
    return x + K * (z - x), (1 - K) * P

$Q$ y $R$ se miden: $R$ con el robot quieto frente a la marca, $Q$ con las vueltas de la semana 12

Kalman en el robot: cinco pasos¶

Con ArUco, el matching es fácil: cada marcador tiene un ID único

NOTAS

En un robot real, el ciclo de Kalman se suele explicar en cinco pasos. Uno: predecir la pose con la odometría, y con ella la elipse de incertidumbre, que crece. Dos: observar, es decir, sacar del sensor las características útiles; en nuestro caso, el ID del ArUco y su pose relativa al robot, lo que hicieron en la práctica 4 de la semana 13.

Tres: con la pose predicha y el mapa, calcular dónde debería verse la marca. Cuatro: emparejar lo visto con lo esperado y calcular la innovación. Cinco: con la ganancia de Kalman, corregir la pose y encoger la elipse.

El paso cuatro es el más difícil en general. El robot Pygmalion de la EPFL extraía líneas de las paredes con un láser, y todas las paredes se parecen: había que decidir cuál línea vista correspondía a cuál línea del mapa. Con ArUco nos ahorramos ese problema, porque el marcador trae su nombre escrito.

En 2D: la elipse de incertidumbre¶

$$\Sigma^- = F_p\, \Sigma\, F_p^T + F_{rl}\, Q\, F_{rl}^T \qquad Q = \begin{bmatrix} k\,|\Delta s_r| & 0 \\ 0 & k\,|\Delta s_l| \end{bmatrix}$$

$\Sigma,\ \Sigma^-$: covarianza de $(x, y, \theta)$ antes y después del paso · $Q$: varianza nueva de cada rueda
$F_p$: transforma la incertidumbre que ya había · $F_{rl}$: hace entrar el error de cada rueda
$\Delta s_r,\ \Delta s_l$: avance de la rueda derecha e izquierda · $k$: constante de error: se mide

NOTAS

En el plano, la varianza se vuelve una matriz de covarianza de 3 por 3, para x, y y θ, y se dibuja como una elipse: aquí, de dos sigmas. La predicción la propaga con dos Jacobianos: F p dice cómo se transforma la incertidumbre que ya había, y F rl cómo entra la incertidumbre nueva de cada rueda. Q supone que cada rueda agrega una varianza proporcional a lo que recorrió, con una constante k que se mide.

A la izquierda, dos vueltas al cuadrado de 50 cm con solo odometría. Las elipses crecen, y no de forma pareja: se alargan en la dirección perpendicular al movimiento. Es la lección de la semana 12: un error pequeño en el ángulo se vuelve un error lateral grande después de avanzar. Los giros en el sitio también agregan incertidumbre, y bastante, aunque el robot no se desplace.

A la derecha, el mismo recorrido con una marca en cada esquina, de 1 cm y 2 grados de error. En cada esquina la elipse se encoge, y nunca llega a crecer mucho. Es el diente de sierra en dos dimensiones. La constante k de esta figura es de ejemplo: la de su robot la tienen que medir.

En la animación, el recuadro de cada panel muestra la incertidumbre actual: sin marcas termina en unos 6.6 cm y 9° después de dos vueltas; con marcas queda por debajo de 1 cm y 2°.

Corregir en 2D¶

Si el marcador entrega la pose completa $z = (x, y, \theta)$:

$$K = \Sigma^- \left(\Sigma^- + R\right)^{-1} \qquad \hat{x} = \hat{x}^- + K\,(z - \hat{x}^-) \qquad \Sigma = (I - K)\, \Sigma^-$$

$\hat{x}^-,\ \Sigma^-$: pose predicha y su covarianza · $R$: covarianza de la medida del marcador
$K$: ganancia, ahora una matriz de 3 × 3 · $I$: matriz identidad · $\hat{x},\ \Sigma$: pose corregida
Las mismas fórmulas de 1D: la división se vuelve una inversa de matriz
La innovación del ángulo se envuelve entre −180° y 180°
Si el sensor mide otra cosa (distancia y ángulo a la marca), se linealiza: Kalman extendido (EKF)

NOTAS

En la semana 13 calculamos la pose del robot a partir de un marcador: posición y orientación. Eso es una medida de la pose completa, y en ese caso la corrección de Kalman en 2D es idéntica a la de 1D: solo hay que cambiar la división por una inversa de matriz. R es ahora una matriz de 3 por 3 con las varianzas de la pose que da el marcador.

Hay un detalle que se les va a olvidar y les va a costar una tarde: la innovación del ángulo. Si el filtro cree que el robot está en 179° y el marcador dice −179°, la diferencia no es 358°, es −2°. Hay que envolverla, igual que hicimos con atan2 en el control de la semana 13.

Cuando el sensor no mide la pose sino otra cosa, por ejemplo la distancia y el ángulo a una marca, la relación entre lo medido y la pose no es lineal. Entonces se linealiza con un Jacobiano en cada paso: eso es el filtro de Kalman extendido, el EKF, y es el que usa casi todo robot móvil.

¿Cuándo no creerle al marcador?¶

$$d^2 = \frac{(z - \hat{x}^-)^2}{P^- + R} \qquad \text{si } d^2 > 9 \;(3\sigma) \;\Rightarrow\; \text{descartar la medida}$$

$d^2$: distancia de Mahalanobis al cuadrado: la innovación medida en varianzas
$z - \hat{x}^-$: innovación · $P^- + R$: la varianza que el filtro espera para ella
$9 = 3^2$: una innovación de más de 3 desviaciones casi nunca es ruido

Lo que dice el marcador	Innovación	d²	Decisión
49.5 cm	−2.5 cm	0.6	se usa
40 cm	−12 cm	14.4	se descarta

NOTAS

Kalman tiene un punto débil: le cree a todo lo que se le da, con el peso que diga R. Si el detector se equivoca, porque la imagen salió borrosa, porque leyó mal el ID, o porque alguien movió el marcador, la corrección arrastra al robot a una pose falsa.

La defensa es una compuerta de validación. Antes de corregir, se compara la innovación con lo que el propio filtro considera posible: la suma de las dos varianzas, P menos más R. Esa razón es la distancia de Mahalanobis al cuadrado. En nuestro ejemplo, P menos más R es 10 cm cuadrados. Una innovación de 2.5 cm da 0.6: normal. Una de 12 cm da 14.4, más de tres sigmas: es muy improbable que sea ruido, así que se descarta.

En 2D, la cuenta es la misma con matrices, y el umbral se toma de la tabla chi cuadrado para tres variables, cerca de 11.3 para el 99 %. Es una línea de código que separa un robot que funciona en la demo de uno que funciona todos los días.

SLAM¶

NOTAS

Hasta ahora alguien le dio el mapa al robot: las posiciones de los marcadores, medidas con regla. ¿Qué pasa si llegamos a un lugar nuevo? Es como los exploradores que llegaban a una costa desconocida: iban dibujando el mapa mientras navegaban, y la posición del barco la sacaban de ese mismo mapa que estaban dibujando.

Eso es SLAM: localización y mapeo simultáneos. Es el problema central de la robótica móvil autónoma, y es lo que hace la aspiradora robot la primera vez que la prenden en la casa.

🎨 Prompt sugerido: "Pixel art of a small robot that has ONLY TWO wheels in total, one on each side of its body like a two-wheeled balancing robot, exploring a dark unknown room, with a glowing hand-drawn map appearing on the floor only where the robot has already been, fog covering the rest of the room, a few square fiducial marker tiles being revealed by the light. 16-bit retro style, dark blue, gold and white palette, no text."

SLAM¶

Para localizarme...	Para hacer el mapa...
necesito el mapa	necesito saber dónde estoy

$$\mathbf{x} = \big(\underbrace{x,\; y,\; \theta}_{\text{robot}},\; \underbrace{m_{1x},\; m_{1y},\; \dots,\; m_{Nx},\; m_{Ny}}_{\text{marcas}}\big)$$

$x, y, \theta$: pose del robot · $m_{ix}, m_{iy}$: posición de la marca $i$ · $N$: número de marcas
Con 8 marcadores: estado de 19 números y covarianza de 19 × 19 = 361
El robot y las marcas se estiman juntos, en el mismo filtro

NOTAS

El problema parece circular: para localizarme necesito el mapa, y para dibujar el mapa necesito saber dónde estoy. La salida del EKF-SLAM es no separar los dos problemas: se mete todo en un solo vector de estado, la pose del robot y la posición de cada marca, y se estima todo con el mismo filtro de Kalman.

Con los ocho marcadores del piso del simulador, el estado tiene 3 más 16, 19 números, y la covarianza es una matriz de 19 por 19. Cuando el robot ve un marcador por primera vez, el estado crece en dos números; cuando lo vuelve a ver, se corrige como siempre, pero la corrección actualiza tanto al robot como a la marca.

La parte importante de esa matriz no es la diagonal sino lo de afuera: las correlaciones. Dicen, por ejemplo, que si el robot está en realidad 2 cm más a la derecha de lo que cree, la marca que vio hace un momento probablemente también. En la siguiente diapositiva vemos qué consecuencia tiene eso.

Cerrar el lazo¶

Cada marca nueva hereda la incertidumbre del robot · al volver a la marca 0, todas bajan

NOTAS

Una simulación de EKF-SLAM: el robot, en rojo, da una vuelta por un círculo y va viendo ocho marcas, las cruces. Parte de una pose conocida, así que la primera marca queda muy bien ubicada. Las elipses son de dos sigmas, ampliadas tres veces para que se vean.

Durante la vuelta, miren el recuadro de arriba a la izquierda. Cada marca nueva queda con una elipse más grande que la anterior: el robot la ubica a partir de su propia pose, que ya trae error acumulado, y la marca hereda ese error. Es la odometría filtrándose al mapa.

Al final de la vuelta, el robot vuelve a ver la marca 0, la primera. Como esa marca es muy confiable, la pose del robot se corrige, y gracias a las correlaciones esa corrección se propaga hacia atrás: las elipses de todas las marcas se encogen a la vez, aunque el robot no las esté viendo. Esto es cerrar el lazo, y es el momento más importante de cualquier SLAM.

En el recuadro se ve con números: justo antes de cerrar el lazo, el robot lleva unos 6 cm de desviación y la peor marca más de 7; al ver la marca 0, el robot baja a menos de 2 cm y la peor marca a menos de 5.

SLAM hoy¶

Desafío	Por qué es difícil
Reconocer que volví	dos lugares parecidos: aliasing otra vez
Entornos dinámicos	personas, sillas y cajas que cambian de lugar
Escala	miles de marcas: la covarianza crece con el cuadrado

Aspiradoras robot: láser o cámara · celulares (realidad aumentada): cámara + IMU
Hoy se combinan submapas locales precisos con un grafo topológico global

NOTAS

SLAM funciona muy bien en un laboratorio con ocho marcadores únicos. Afuera aparecen tres problemas. El primero es reconocer que volví: con marcadores es trivial, pero con paredes y esquinas aparece otra vez el aliasing de la semana 13. Si el robot cree que volvió a un lugar y no es así, el cierre de lazo deforma todo el mapa.

El segundo es que el mundo cambia: personas que caminan, sillas que se mueven. El mapa tiene que olvidar lo que ya no está y no guardar lo que solo pasaba. El tercero es la escala: la covarianza de EKF-SLAM crece con el cuadrado del número de marcas, así que con miles de marcas hay que usar otras técnicas, como los grafos de poses o los filtros de partículas de FastSLAM.

Y aun así, SLAM ya está en la casa de muchos: las aspiradoras que hacen el mapa del apartamento, y su propio celular, que cuando usa realidad aumentada combina la cámara con el acelerómetro y el giroscopio para saber dónde está en la sala. Es fusión de sensores: el mismo filtro, con más fuentes.

Prácticas de la semana¶

#	Guía	Qué se ve
1	Markov	RHINO se ubica sin saber dónde arrancó: grilla de 15 cm, partículas y secuestro
2	Kalman	EKF con la odometría y las columnas; qué pasa al apagar las marcas
3	SLAM	RHINO arma el mapa de columnas y cierra el lazo

Simulador: RHINO en el museo (Webots), con la planta de la Fig. 4 de Burgard et al. 1998 · Guías: Semana 15 en el sitio

NOTAS

Las prácticas de esta semana son demostrativas: no hay que programar, sino ver funcionando lo que vimos hoy y cambiar parámetros para entender qué hace cada parte. Todas son en Webots, con RHINO dentro del Deutsches Museum Bonn, reconstruido a partir de la planta del artículo: las celdas grises de la figura son la grilla de 15 cm de RHINO, y con esa escala se midió todo el museo.

En la primera, RHINO arranca sin saber dónde está y se ubica con Markov en una grilla, como el original, y después con partículas; también lo secuestramos. En la segunda sabe dónde arranca y sigue su pose con un filtro de Kalman extendido, usando las columnas y los postes como marcas; si le apagamos las marcas mucho tiempo, se pierde. En la tercera no le damos el mapa: hace SLAM con las columnas y vemos qué pasa cuando cierra el lazo.

¿Qué nos llevamos hoy?¶

Un compañero programa la localización de Markov para el piso de 2 × 2 m del simulador con celdas de 1 cm y 1°, y cada paso tarda varios segundos. ¿Qué le proponen, sabiendo que el robot siempre arranca sobre el marcador 1?
Con $R$ muy pequeño, su filtro hace exactamente lo mismo que el programa de la semana 13; con $R$ muy grande, lo mismo que el de la semana 12. ¿Por qué pasa eso, y cómo escogerían $R$ con su robot?
Ponen los 8 marcadores sin medir dónde quedaron y el robot hace SLAM. Al volver al primer marcador, el programa corrige la posición de todos los marcadores, no solo la del robot. ¿Es un error del programa?

NOTAS

Para cerrar, en parejas, unos dos minutos por pregunta y sin mirar las diapositivas anteriores.

Pregunta 1 (Markov). Queremos que lleguen a que son 14.4 millones de celdas por paso, y que esa generalidad no hace falta: Markov se justifica cuando el robot no sabe dónde empezó o puede confundirse entre lugares parecidos. Aquí el robot arranca en una pose conocida y los ArUco tienen ID único, así que no hay aliasing: basta un filtro de Kalman, o si insisten en Markov, celdas más grandes o un filtro de partículas con unos cientos de hipótesis. Si no llegan, retomen "¿Y en un piso de verdad?" y "Filtro de Kalman".

Pregunta 2 (Kalman). La ganancia es K = P menos sobre P menos más R. Con R casi cero, K vale uno y la pose salta a lo que dice el marcador, como en la semana 13; con R enorme, K vale casi cero y el marcador se ignora, como si solo hubiera encoders. R no se escoge a ojo: se mide dejando el robot quieto frente al marcador, leyendo muchas veces la pose y calculando la varianza. Si dudan, vuelvan a "La ganancia de Kalman" y "En Python".

Pregunta 3 (SLAM). No es un error: es el cierre del lazo. Cada marcador se ubicó a partir de la pose del robot, así que sus errores están correlacionados con el error del robot en ese momento. Cuando el primer marcador, que es el más confiable, corrige al robot, la covarianza propaga esa corrección a todos los marcadores. Retomen "Cerrar el lazo". Con esto cerramos y pasamos al laboratorio.