Cómo fusionar datos lineales y angulares de sensores

Mi equipo y yo estamos configurando un robot para exteriores que tiene codificadores, una IMU de grado comercial y un sensor GPS . El robot tiene una unidad de tanque básica, por lo que los codificadores suministran las garrapatas de las ruedas izquierda y derecha. La IMU proporciona aceleraciones de balanceo, cabeceo, guiñada y lineal en x, y y z. Más tarde podríamos agregar otras IMU, lo que daría redundancia, pero también podría proporcionar tasas angulares de balanceo, cabeceo y guiñada. El GPS publica las coordenadas globales x, y y z.

Conocer la posición y el rumbo xy del robot será útil para que el robot localice y mapee su entorno para navegar. La velocidad del robot también podría ser útil para tomar decisiones de movimiento suaves. Es un robot terrestre, por lo que no nos importa demasiado el eje z. El robot también tiene un sensor lidar y una cámara, por lo que rodar y lanzar será útil para transformar los datos lidar y de la cámara para una mejor orientación.

Estoy tratando de descubrir cómo fusionar todos estos números de una manera que aproveche de manera óptima la precisión de todos los sensores. En este momento estamos usando un filtro de Kalman para generar una estimación de [x, x-vel, x-accel, y, y-vel, y-accel]con la matriz de transición simple:

[[1, dt, .5*dt*dt, 0,  0,        0],
 [0,  1,       dt, 0,  0,        0],
 [0,  0,        1, 0,  0,        0],
 [0,  0,        0, 1, dt, .5*dt*dt],
 [0,  0,        0, 0,  1,       dt],
 [0,  0,        0, 0,  0,        1]]

El filtro estima el estado exclusivamente en función de las aceleraciones proporcionadas por la IMU. (La IMU no es de la mejor calidad; dentro de unos 30 segundos mostrará al robot (en reposo) a una buena distancia de 20 metros de su ubicación inicial.) Quiero saber cómo usar el balanceo, cabeceo y guiñada desde IMU, y posiblemente velocidades de balanceo, inclinación y guiñada, datos del codificador de las ruedas y datos de GPS para mejorar la estimación del estado.

Usando un poco de matemática, podemos usar los dos codificadores para generar información de rumbo, x, y en el robot, así como velocidades lineales y angulares. Los codificadores son muy precisos, pero pueden ser susceptibles al deslizamiento en un campo al aire libre.

Me parece que hay dos conjuntos de datos separados aquí, que son difíciles de fusionar:

1. Estimaciones de x, x-vel, x-accel, y, y-vel, y-accel
2. Estimaciones de balanceo, cabeceo, guiñada y tasas de balanceo, cabeceo y guiñada

A pesar de que hay un cruce entre estos dos conjuntos, tengo problemas para razonar sobre cómo juntarlos. Por ejemplo, si el robot va a una velocidad constante, la dirección del robot, determinada por sus x-vel e y-vel, será la misma que su guiñada. Sin embargo, si el robot está en reposo, la guiñada no puede determinarse con precisión por las velocidades x e y. Además, los datos proporcionados por los codificadores, traducidos a velocidad angular, podrían ser una actualización de la velocidad de guiñada ... pero ¿cómo podría una actualización de la velocidad de guiñada terminar proporcionando mejores estimaciones posicionales?

¿Tiene sentido poner los 12 números en el mismo filtro, o normalmente se mantienen separados? ¿Existe ya una forma bien desarrollada de tratar este tipo de problema?

Dos cosas.

1. Si planea hacer un mapeo, necesita un algoritmo completo de localización y mapeo simultáneo (SLAM). Ver: Localización y mapeo simultáneo (SLAM): Parte I Los algoritmos esenciales . En SLAM, estimar el estado del robot es solo la mitad del problema. Cómo hacerlo es una pregunta más grande de la que se puede responder aquí.

2. En cuanto a la localización (estimación del estado del robot), este no es un trabajo para un filtro de Kalman. La transición de a no es una función lineal debido a aceleraciones angulares y velocidades. Por lo tanto, debe considerar estimadores no lineales para esta tarea. Sí, hay formas estándar de hacer esto. Sí, están disponibles en la literatura. Sí, generalmente todas las entradas se colocan en el mismo filtro. La posición, la velocidad, la orientación y la velocidad angular del robot se utilizan como salidas. Y sí, presentaré una breve introducción a sus temas comunes aquí. Las principales conclusiones sonx ( t + 1 $x(t)=[x,y,\dot{x},\dot{y},\theta,\dot{\theta}]$$x(t+1)$

   1. incluya el sesgo de Gyro e IMU en su estado o sus estimaciones divergirán
   2. Un filtro de Kalman extendido (EKF) se usa comúnmente para este problema
   3. Las implementaciones pueden derivarse desde cero y, por lo general, no necesitan ser "buscadas".
   4. Existen implementaciones para la mayoría de los problemas de localización y SLAM, así que no haga más trabajo del que tiene que hacer. Ver: sistema operativo del robot ROS

Ahora, para explicar el EKF en el contexto de su sistema. Tenemos un IMU + Gyro, GPS y odometría. El robot en cuestión es un accionamiento diferencial como se mencionó. La tarea de filtrado es tomar la estimación de pose actual del robot , las entradas de control y las mediciones de cada sensor, , y producir la estimación en el próximo paso de tiempo . Llamaremos a las mediciones de IMU , GPS es y odometría, .utzt x t+1ItGtO${\hat{x}_t}$$u_t$$z_t$$\hat{x}_{t+1}$$I_t$$G_t$$O_t$

Supongo que estamos interesados ​​en estimar la pose del robot como . El problema con IMU y Gyros es la deriva. Hay un sesgo no estacionario en las aceleraciones que debe tener en cuenta en el EKF. Esto se hace (generalmente) poniendo el sesgo en el estado estimado. Esto le permite estimar directamente el sesgo en cada paso de tiempo. , para un vector de sesgos . x t = x , y , ˙ x , ˙ y , θ , ˙ θ , b$x_t={x,y,\dot{x},\dot{y},\theta,\dot{\theta}}$$x_t={x,y,\dot{x},\dot{y},\theta,\dot{\theta},b}$$b$

Estoy asumiendo:

1. $O_t$ = dos mediciones de distancia que representan la distancia que han recorrido las huellas en un pequeño incremento de tiempo
2. α , β , θ ¨ x , ¨ y , ¨ $I_t$ = tres medidas de orientación y tres medidas de aceleración .${\alpha, \beta, \theta}$${\ddot{x},\ddot{y},\ddot{z}}$
3. G x t , G y $G_t$ = la posición del robot en el marco global , .$^Gx_t,^Gy_t$

Normalmente, el resultado de las entradas de control (velocidades deseadas para cada banda de rodadura) es difícil de asignar a las salidas (el cambio en la postura del robot). En lugar de , es común (vea Thrun , Odometry Question ) usar la odometría como el "resultado" del control. Esta suposición funciona bien cuando no se encuentra en una superficie casi sin fricción. La IMU y el GPS pueden ayudar a corregir el deslizamiento, como veremos.$u$

Entonces, la primera tarea es predecir el siguiente estado desde el estado actual: . En el caso de un robot de accionamiento diferencial, esta predicción se puede obtener directamente de la literatura (ver Sobre la cinemática de los robots móviles con ruedas o el tratamiento más conciso en cualquier libro de texto de robótica moderno), o derivar desde cero como se muestra aquí: Pregunta de odometría .$\hat{x}_{t+1} = f(\hat{x}_t,u_t)$

Entonces, ahora podemos predecir . Este es el paso de propagación o predicción. Usted puede operar un robot simplemente propagando. Si los valores son completamente precisos, nunca tendrá una estimación que no sea exactamente igual a su estado real. Esto nunca sucede en la práctica.Ot$\hat{x}_{t+1} = f(\hat{x}_t, O_t)$$O_t$$\hat{x}$

Esto solo da un valor predicho de la estimación anterior, y no nos dice cómo la precisión de la estimación se degrada con el tiempo. Entonces, para propagar la incertidumbre, debe usar las ecuaciones EKF (que propagan la incertidumbre en forma cerrada bajo supuestos de ruido gaussianos), un filtro de partículas (que usa un enfoque basado en muestreo) *, el UKF (que usa un punto aproximación de la incertidumbre), o una de muchas otras variantes.

En el caso del EKF, procedemos de la siguiente manera. Deje ser la matriz de covarianza del estado del robot. Linealizamos la función usando la expansión de la serie Taylor para obtener un sistema lineal. Un sistema lineal se puede resolver fácilmente con el filtro Kalman. Suponga que la covarianza de la estimación en el tiempo es , y la covarianza supuesta del ruido en la odometría se da como la matriz (generalmente una matriz diagonal , como ) . En el caso de la función , obtenemos el jacobiano y f t P t U t 2 × 2 .1 × I 2 × 2 f F x = ∂ $P_t$$f$$t$$P_t$$U_t$$2\times2$$.1\times I_{2\times 2}$$f$ Fu=∂$F_x=\frac{\partial f}{\partial x}$$F_u=\frac{\partial f}{\partial u}$, luego propague la incertidumbre como,

Ahora podemos propagar la estimación y la incertidumbre. Tenga en cuenta que la incertidumbre aumentará monotónicamente con el tiempo. Esto se espera. Para solucionar esto, lo que generalmente se hace es usar y para actualizar el estado predicho. Esto se llama el paso de medición del proceso de filtrado, ya que los sensores proporcionan una medición indirecta del estado del robot.G $I_t$$G_t$

Primero, use cada sensor para estimar alguna parte del estado del robot como alguna función y para GPS, IMU. Forme el residuo o innovación que es la diferencia de los valores predichos y medidos. Luego, calcule la precisión para cada estimación del sensor en forma de una matriz de covarianza para todos los sensores ( , en este caso). Finalmente, encuentre los jacobianos de y actualice la estimación del estado de la siguiente manera:h i ( ) R R g R i$h_g()$$h_i()$$R$$R_g$$R_i$$h$

Para cada sensor con estado estimado ( Siguiendo la entrada de wikipedia )z $s$$z_s$

S s = H s * P t + 1 * H T s + R s K = P t + 1 * H T s S - 1 s x t + 1 = x t + 1 - K *

En el caso del GPS, la medida probablemente sea solo una transformación de la latitud y la longitud al marco local del robot, por lo que la jacobiana será casi Identidad. es reportado directamente por la unidad GPS en la mayoría de los casos.$z_g=h_g()$$H_g$$R_g$

En el caso de IMU + Gyro, la función es una integración de aceleraciones y un término de sesgo aditivo. Una forma de manejar la IMU es integrar numéricamente las aceleraciones para encontrar una estimación de posición y velocidad en el momento deseado. Si su IMU tiene un pequeño término de ruido aditivo para cada estimación de aceleración, entonces debe integrar este ruido para encontrar la precisión de la estimación de posición. Entonces la covarianza es la integración de todos los pequeños términos de ruido aditivo, . Incorporar la actualización para el sesgo es más difícil y está fuera de mi experiencia. Sin embargo, dado que está interesado en el movimiento plano, probablemente pueda simplificar el problema. Tendrás que buscar en la literatura para esto.$z_i=h_i()$$p_i$$R_i$$p_i$

Algunas referencias extravagantes:

1. Mejora de la precisión de la odometría inercial visual basada en EKF

2. Estimadores EKF consistentes basados ​​en la observabilidad para la localización cooperativa de múltiples robots

3. Adaptable EKF de dos etapas para el sistema INS-GPS acoplado libremente con sesgo de falla desconocido

Este campo es lo suficientemente maduro como para que google (académico) probablemente pueda encontrar una implementación que funcione. Si va a hacer mucho trabajo en esta área, le recomiendo que tome un libro de texto sólido. Tal vez algo como Probablistic Robotics por S. Thrun de la fama de Google Car. (Lo he encontrado una referencia útil para esas implementaciones nocturnas).

* Hay varios estimadores basados ​​en PF disponibles en el Sistema Operativo de Robot (ROS). Sin embargo, estos han sido optimizados para uso en interiores. Los filtros de partículas tratan con los archivos PDF multimodales que pueden resultar de la localización basada en mapas (estoy cerca de esta puerta o de esa puerta). Creo que la mayoría de las implementaciones al aire libre (especialmente aquellas que pueden usar GPS, al menos de manera intermitente) usan el Filtro Kalman Extendido (EKF). He utilizado con éxito el filtro Kalman extendido para un rover de tierra al aire libre con accionamiento diferencial.

Puede simplificar enormemente el problema en los casos más comunes:

• Muchos IMus de "grado comercial" (por ejemplo, Xsens) tienen acelerómetros muy ruidosos. Ni siquiera se moleste en fusionarlos para obtener velocidad, la odometría ya es un orden de magnitudes mejor. Los únicos datos utilizables que proporcionará la IMU son el pitch and roll y, en cierta medida, el encabezado (ver el siguiente punto)
• partir de las IMU no es tan confiable. Utiliza magetómetros y mostrará grandes desviaciones (hasta 25 grados en más de 2 m en nuestro caso) cerca de masas ferromagnéticas, como la que puedes encontrar en las paredes de los edificios. Lo que hicimos para resolver esto es usar el encabezado IMU, pero estimar un sesgo de encabezado.
• Si está al aire libre, no olvide que viajar 10m en una pendiente de 10 grados no conduce al mismo cambio en X e Y que viajar 10m en un terreno plano. Esto generalmente se explica estimando Z, pero supongo que se puede estimar de manera diferente.
• El GPS también es una perra mentirosa, por lo general en entornos de alta trayectoria múltiple. Además, los GPS de bajo grado (e incluso en algunas condiciones de alto grado) tienden a informar desviaciones estándar muy erróneas. Utilizamos algunas pruebas simples de chi-cuadrado para verificar si una medida de GPS particular debe integrarse (es decir, verificar que coincida con la estimación actual del filtro hasta cierto punto), lo que nos dio resultados decentes.

La solución "típica" para nosotros es usar odometría + IMU para obtener una estimación del movimiento del ego y luego usar el GPS para corregir X, Y, Z y el sesgo de rumbo.

Aquí hay una implementación de EKF que utilizamos ampliamente. Si necesita estimar la orientación de la IMU (es decir, si aún no tiene un filtro incorporado), también puede usar uno de estos dos filtros: UKF y EKF .