Estoy tratando de implementar la planificación del 'espacio de creencias' para un robot que tiene una cámara como sensor principal. Al igual que SLAM, el robot tiene un mapa de puntos 3D, y se localiza realizando una coincidencia 2D-3D con el entorno en cada paso. A los efectos de esta pregunta, supongo que el mapa no cambia.

Como parte de la planificación del espacio de creencias, quiero planificar rutas para el robot que lo lleven de principio a objetivo, pero de una manera que su precisión de localización siempre se maximice. Por lo tanto, tendría que muestrear posibles estados del robot, sin moverme realmente allí, y las observaciones que el robot haría si fuera en esos estados, que juntos (corríjalos si estoy equivocado) forman la 'creencia' del robot , codificando posteriormente su incertidumbre de localización en esos puntos. Y luego mi planificador intentaría conectar los nodos que me dan la menor incertidumbre (covarianza).

Como mi incertidumbre de localización para este robot basado en cámara depende completamente de cosas como cuántos puntos de características son visibles desde una ubicación determinada, el ángulo de rumbo del robot, etc.: necesito una estimación de cuán 'mala' es mi localización en una muestra determinada sería, para determinar si debería descartarlo. Para llegar allí, ¿cómo defino el modelo de medición para esto, sería el modelo de medición de la cámara o sería algo relacionado con la posición del robot? ¿Cómo 'adivino' mis mediciones de antemano y cómo calculo la covarianza del robot a través de esas mediciones adivinadas?

EDITAR: La referencia principal para mí es la idea de explorar rápidamente Random Belief Trees , que es una extensión del método Belief Road Maps . Otro documento relevante utiliza RRBT para la planificación restringida. En este documento, los estados se muestrean de manera similar a los RRT convencionales, representados como vértices como un gráfico, pero cuando se van a conectar los vértices, el algoritmo propaga la creencia del vértice actual al nuevo, (función PROPAGATE en la sección V de 1 ) , y aquí es donde estoy atascado: no entiendo completamente cómo puedo propagar la creencia a lo largo de un borde sin atravesarla y obtener nuevas medidas, por lo tanto, nuevas covarianzas de la localización. El documento RRBT dice "las predicciones de covarianza y las ecuaciones de expectativas de costos se implementan en la función PROPAGAR": pero si solo se usa la predicción, ¿cómo sabe, por ejemplo, si hay suficientes características en la posición futura que podrían mejorar / degradar la precisión de localización?

Utilice la localización solo de rodamientos para modelar la información de la cámara y simular mediciones con cero ruido (por ejemplo, sin innovación).

Por una variedad de razones, esta es en realidad una forma teóricamente sólida de estimar la informatividad de una ruta.

Hay muchas métricas informativas de "medición libre", como la Matriz de información de Fisher . Todo lo que necesita son las posiciones del robot y las posiciones de los puntos de referencia en el mapa para determinar cuánta información sobre la posición del robot se obtendría midiendo las ubicaciones de los puntos de referencia. (O viceversa, la innovación de las mediciones se aplica tanto al objetivo como al robot (¿es SLAM correcto?), Por lo que la misma métrica funciona para ambos).

Comenzaría con un sensor de rodamiento, ya que es un modelo bueno y bien aceptado de un sensor de visión. Calcule el "ruido" en las mediciones del rodamiento suponiendo unos pocos píxeles de error al ubicar las funciones en el mundo. Deje que el estado del sistema sea la posición del robot más su incertidumbre, y luego muestre las rutas (como sugiere). Desde cada posición en la ruta muestreada, volvería a calcular la incertidumbre predicha utilizando el FIM. Esto no es difícil de hacer, simplemente suponga que no hay error en las mediciones (es decir, no habrá "innovación" sobre la creencia del robot, pero aún experimentará una caída en la incertidumbre representada por la reducción de la covarianza en la estimación de posición del robot. no actualice las ubicaciones o incertidumbres de los puntos de referencia, solo para simplificar el problema.

Este es un enfoque bastante bien entendido por lo que recuerdo en mi última revisión de esta literatura, pero no confíe en mi palabra (¡revíselo usted mismo!). Al menos esto debería formar un enfoque de línea de base que sea fácil de simular. Usemos el poder de la literatura. Puede leer detenidamente esta tesis para la configuración y las ecuaciones.

Resumiendo

1. Forme el vector de estado que es la posición (y la orientación si le importa) del robot, y la incertidumbre en esos elementos de posición .$x$$\Sigma$
2. Use un RRT, un planificador gráfico discreto (dijkstra / A *) o una búsqueda de cuadrícula (A * probablemente), pero en cada "paso" de la trayectoria muestreada, recalcule usando las ecuaciones de actualización EKF estándar.$\Sigma_i$
3. Deje que el "costo" de la trayectoria sea una combinación convexa de progreso hacia la meta y la inversa de la covarianza (por ejemplo, la matriz de información )

Algunas sutilezas

Use el vector de estado más pequeño que tenga sentido. Si puede suponer que el robot puede apuntar la cámara independientemente del movimiento, o tiene varias cámaras, entonces ignore la orientación y solo siga la posición. Procederé solo en posiciones 2D.

Tendrá que derivar el sistema linealizado, pero puede tomarlo prestado de la tesis anterior. asegúrese de no molestarse con las mediciones simuladas (por ejemplo, si solo realiza actualizaciones de EKF con "mediciones simuladas", entonces suponga que las mediciones son verdaderas y sin ruido.

Críticamente, esto simplifica el problema de planificación ya que su estado no cambia, excepto por la incertidumbre y el movimiento planificado. Verifique aquí las ecuaciones para EKF , y observe que las únicas dos ecuaciones que son relevantes para calcular la nueva covarianza son y ... (expandí la ganancia de Kalman en la última ecuación).

Si aplicamos la identidad de la matriz de Woodbury

simplificamos que segundo uno a .

Ya hemos terminado Tienes una buena medida informativa sobre los pasos de la trayectoria como,$n$

donde es el ruido de medición (p. ej., el error de medición de rumbo a cada punto de referencia), para cada paso de tiempo en su trayectoria. Dado que estamos buscando información (inversa de covarianza), queremos maximizar la traza, el determinante u otra cosa de$R$$\sum_{i=1}^n H_i^TR^{-1}H_i$

Entonces, ¿qué es ? Bueno, es el jacobiano de la función de medición con respecto a la ubicación del robot. Es decir, es de tamaño , ya que tiene puntos de referencia ( funciones de medición) y elementos de estado del robot: x, y. tiene un tamaño , donde lo establecería en para algún valor simulado de , (la incertidumbre real no importa, es la misma constante.$H$$H$$nx2$$n$$n$$2$$R$$n\times n$$\sigma I_{n\times n}$$\sigma$

¿Cuál es la ecuación de medición? Sus

para el objetivo y la posición del robot .$t$$r$

Desenrollar la recursividad. Procedería de la siguiente manera:

1. Escriba un algoritmo de búsqueda de ruta que encuentre su camino sin considerar la incertidumbre.
2. Derive , o búsquelo en cualquier papel de localización de rodamientos.$H$
3. Para determinar la "bondad" de una ruta para usar en el algoritmo de búsqueda, agregue para cada pose a lo largo de la ruta.$trace(H^TRH)$
4. Observe que el resultado coincide con la FIM de la trayectoria (ejercicio que le queda al lector), y usted ha determinado correcta y teóricamente la trayectoria más informativa.