11.4: SLAM basado en gráficos

Por lo general, un robot obtiene una estimación inicial de dónde está utilizando algunos sensores a bordo (odometría, flujo óptico, etc.) y utiliza esta estimación para localizar características (paredes, esquinas, patrones gráficos) en el entorno. Tan pronto como un robot vuelva a visitar la misma función dos veces, puede actualizar la estimación de su ubicación. Esto se debe a que la varianza de una estimación basada en dos mediciones independientes siempre será menor que cualquiera de las varianzas de las mediciones individuales. Como las observaciones consecutivas no son independientes, sino que están estrechamente correlacionadas, la estimación refinada puede propagarse a lo largo del camino del robot. Esto se formaliza en SLAM basado en EKF. Una comprensión más intuitiva es proporcionada por una analogía primavera-masa: cada pose posible (masa) está restringida a su pose vecina por un resorte. Cuanto mayor sea la incertidumbre de la transformación relativa entre dos poses (por ejemplo, obtenida mediante odometría), más débil es la primavera. Cada vez que un robot gana confianza en una pose relativa, el resorte se endurece en su lugar. Eventualmente, todas las poses se tirarán en su lugar. Este enfoque se conoce como SLAM basado en gráficos, véase también (?).

11.4.1. SLAM como problema de estimación de máxima verosimilitud

La formulación clásica de SLAM describe el problema como maximizar la probabilidad posterior de todos los puntos de la trayectoria del robot dada la entrada de odometría y las observaciones. Formalmente,

\[p(x_{1:T},m|z_{1:T},u_{1:T})\]

donde x _1:T son todas posiciones discretas desde el tiempo 1 hasta el tiempo T, z son las observaciones y u son las mediciones de odometría. Esta formulación hace mucho uso de la estructura temporal del problema. En la práctica, resolver el problema de SLAM requiere

1. Un modelo de actualización de movimiento, es decir, la probabilidad p (x _t |x _t−1, _ut) de estar en la ubicación x _t dada una medición de odometría u _t y estando en la ubicación x _t−1.
2. Un modelo de sensor, es decir, la probabilidad p (z _t |x _t, m _t) para hacer observación z _t dado que el robot está en la ubicación x _t y el mapa m _t.

Una posible solución a este problema la proporciona el Filtro extendido de Kalman, que mantiene una función de densidad de probabilidad para la pose del robot así como las posiciones de todas las entidades en el mapa. Ser capaz de identificar características únicas en el entorno es de suma importancia y se conoce como el problema de asociación de datos. Al igual que el SLAM basado en EKF, el SLAM basado en gráficos no resuelve este problema y fallará si se confunden las características.

En SLAM basado en gráficos, la trayectoria de un robot forma los nodos de una gráfica cuyos bordes son transformaciones (traslación y rotación) que tienen una varianza asociada a ella. Una vista alternativa es la analogía primavera-masa mencionada anteriormente. En lugar de que cada resorte mueva un nodo en su lugar, SLAM basado en gráficos tiene como objetivo encontrar aquellas ubicaciones que maximicen la probabilidad conjunta de todas las observaciones. Como tal, SLAM basado en gráficos es un problema de estimación de máxima verosimilitud.

Revisemos la distribución normal:

\[\frac{1}{\sigma \sqrt{2\pi }}e^{\frac{-(x-\mu )^{2}}{2\sigma ^{2}}}\]

Proporciona la probabilidad de que una medida tenga valor x dado que esta medida es normal distribuida con la media µ y varianza σ ². Ahora podemos asociar dicha distribución con cada transformación nodo a nodo, también conocido como restricción. Esto puede ser pares de distancia y ángulo, por ejemplo En la literatura la medición de una transformación entre el nodo i y un nodo j se denota z _ij. Su valor esperado se denota _ij. Este valor se espera por ejemplo basado en un mapa del entorno que consiste en observaciones previas.

Formular una distribución normal de medidas z _ij con media _ij y una matriz de covarianza σ _ij (que contiene todas las varianzas de los componentes de z _ij en su diagonal) ahora es sencillo. Como SLAM basado en gráficos se formula con mayor frecuencia como filtro de información, generalmente se usa la inversa de la matriz de covarianza (también conocida como matriz de información), que denotamos por Ω _ij = σ ⁻¹ _ij.

Como estamos interesados en maximizar la probabilidad conjunta de todas las mediciones πz _{ij sobre todos los pares de bordes ij} siguiendo el marco de estimación de máxima verosimilitud, es costumbre expresar el PDF usando la verosimilitud logarítmica. Al tomar el logaritmo natural en ambos lados de la expresión PDF, la función exponencial desaparece y LnπZ _{ij se convierte en σLnz ij} o σL _ij, donde l _ij es la distribución de verosimilitud logarítmica para z _ij.

\[l_{ij}\alpha (z_{ij}-ẑ_{ij}(x_{i},x_{j}))^{T}\Omega _{ij}(z_{ij}-ẑ_{ij}(x_{i},x_{j}))\]

Nuevamente, la probabilidad logarítmica para la observación zij se deriva directamente de la definición de la distribución normal, pero usando la matriz de información en lugar de la matriz de covarianza y se monta de la función exponencial tomando el logaritmo en ambos lados.

El problema de optimización ahora se puede formular como

\[x^{*}=\arg\min_{x} \sum_{<ij>\in C}^{}e_{ij}^{T}\Omega _{ij}e_{ij} \]

con e _ij (x _i, x _j) = z _ij − _ij (x _i, x _j) el error entre la medición y el valor esperado. Tenga en cuenta que la suma realmente necesita ser minimizada ya que los términos individuales son técnicamente la verosimilitud logarítmica negativa.

11.4.2. Técnicas numéricas para SLAM basado en gráficos

Resolver el problema de MLE no es trivial, especialmente si el número de restricciones proporcionadas, es decir, observaciones que relacionan una característica con otra, es grande. Un enfoque clásico es linealizar el problema en la configuración actual y reducirlo a un problema de la forma Ax = b La intuición aquí es calcular el impacto de pequeños cambios en las posiciones de todos los nodos en todos los e _ij. Después de realizar este movimiento, la linealización y optimización pueden repetirse hasta la convergencia.

Recientemente, se han desarrollado métodos numéricos más potentes. En lugar de resolver el MLE, se puede emplear un algoritmo de descenso de gradiente estocástico. Un algoritmo de descenso de gradiente es un enfoque iterativo para encontrar lo óptimo de una función moviéndose a lo largo de su gradiente. Mientras que un algoritmo de descenso de gradiente calcularía el gradiente en un paisaje de fitness a partir de todas las restricciones disponibles, un descenso de gradiente estocástico elige solo un subconjunto (no necesariamente aleatorio). Ejemplos intuitivos son ajustar una línea a un conjunto de n puntos, pero tomar solo un subconjunto de estos puntos al calcular la siguiente mejor suposición. Como el descenso del gradiente funciona iterativamente, la esperanza es que el algoritmo tome en cuenta gran parte de las restricciones. Para resolver SLAM basado en gráficos, un algoritmo de descenso de gradiente estocástico no tomaría en cuenta todas las restricciones disponibles para el robot, sino que trabajaría iterativamente en una restricción tras otra. Aquí, las restricciones son observaciones sobre la pose mutua de los nodos i y j. Optimizar estas restricciones ahora requiere mover ambos nodos i y j para que se reduzca el error entre donde el robot piensa que deberían estar los nodos y lo que realmente ve. Como se trata de una compensación entre múltiples observaciones, quizás contradictorias, el resultado se aproximará a una estimación de Máxima Probabilidad.

Más específicamente, con e _ij el error entre una observación y lo que el robot espera ver, en base a su anterior observación y modelo de sensor, se puede distribuir el error a lo largo de toda la trayectoria entre ambas características que están involucradas en la restricción. Es decir, si la restricción involucra las características i y j, no solo se actualizará la pose de i y j sino que todos los puntos intermedios se moverán un poquito.

En SLAM basado en gráficos, los bordes codifican la traslación relativa y la rotación de un nodo a otro. De esta manera, la alteración de una relación entre dos nodos se propagará automáticamente a todos los nodos de la red. Esto se debe a que la gráfica es esencialmente una cadena de nodos cuyos bordes consisten en mediciones de odometría. Esta cadena se convierte entonces en una gráfica cada vez que las observaciones (usando cualquier sensor) introducen restricciones adicionales. Siempre que se produzca tal “cierre de bucle”, el error resultante se distribuirá por toda la trayectoria que conecta los dos nodos. Esto no siempre es necesario, por ejemplo al considerar que el robot conduce un patrón figura-8. Si se produce un cierre de bucle en una mitad de los 8, los nodos en la otra mitad de los 8 probablemente no estén involucrados.

Esto se puede abordar construyendo un árbol de expansión mínimo (MST) del gráfico de restricciones. El MST se construye haciendo una búsqueda de profundidad primero (DFS) en el gráfico de restricciones siguiendo las restricciones de odometría. En un cierre de bucle, es decir, un borde en la gráfica que impone una restricción a una pose previamente vista, el DFS retrocede a este nodo y continúa desde allí para construir el árbol de expansión. La actualización de todas las poses afectadas por esta nueva restricción aún requiere modificar todos los nodos a lo largo de la ruta entre las dos entidades involucradas, pero la inserción de restricciones adicionales se simplifica enormemente. Siempre que un robot observe nuevas relaciones entre dos nodos cualesquiera, solo los nodos en la ruta más corta entre las dos características en el MST necesitan ser actualizados.