Nota importante: este artículo está compilado a partir de materiales en línea y solo registra el proceso de aprendizaje, intrusión y eliminación de puntos de conocimiento relevantes de los blogueros.
1. Materiales de referencia
Análisis del marco VIO convencional y análisis parcial VINS
Explicación detallada de la fusión multisensor
Columna CSDN: SLAM
2. Introducción a VIO
1. Vision e IMU complementan las ventajas de cada uno
Este capítulo presenta las características de la visión y la IMU respectivamente, y explica de manera integral las ventajas complementarias de la visión y la IMU analizando las desventajas y defectos de la visión pura y la IMU pura en la estimación de la actitud.
1.1 Comparación de visión y características de IMU
| Visión | IMU |
|---|---|
| VIO usando imágenes | unidad de medida Inercial |
| Utilice la coincidencia de puntos de características de la imagen y píxeles (método directo) para estimar la pose | IMU de seis grados de libertad, el giroscopio mide la velocidad angular, el acelerómetro mide la aceleración |
| Los datos se registran en forma de imágenes, con una frecuencia de 15 Hz a 60 Hz. La postura del portador no puede cambiar demasiado rápido y se ve afectada por el entorno externo. | Alta frecuencia >=100 HZ, para hacer frente a cambios rápidos en la postura del portador |
| Sin deriva, medición directa de rotación y traslación. | Respuesta rápida, no afectada por la calidad de la imagen, estimación generalmente precisa de la velocidad angular, se puede estimar la escala absoluta |
| Afectada por la oclusión de la imagen y los objetos en movimiento; la visión monocular tiene problemas de incertidumbre de escala; el movimiento rotacional puro monocular no puede realizar la estimación de pose y las escenas dinámicas tienen una coincidencia incorrecta; cuando se mueve rápidamente, las imágenes se vuelven borrosas y se pierden fácilmente; en áreas con poca textura no puede funcionar |
Las predicciones puras de IMU tendrán deriva, la deriva del ángulo diverge linealmente con el tiempo y la deriva de posición diverge con la potencia cuadrática del tiempo; la IMU tiene sesgo cero; la postura de la integración de la IMU de baja precisión diverge y la IMU de alta precisión es costosa |
| Adecuado para escenas largas y de movimiento lento. | Adecuado para escenas de corto plazo y de rápido movimiento. |
1.2 Comparación de ventajas y desventajas de visión e IMU
| plan | Visión | IMU |
|---|---|---|
| Ventaja | Mida directamente la rotación y la traslación sin deriva |
Respuesta rápida , no afectada por la calidad de la imagen, la velocidad angular generalmente es precisa, se puede estimar la escala absoluta |
| desventaja | Interferido por la oclusión de la imagen y los objetos en movimiento La visión monocular no puede medir la escala No se puede estimar la rotación pura monocular Se pierde fácilmente cuando se mueve rápido |
No hay sesgo, la postura de la integración de IMU de baja precisión es divergente y la IMU de alta precisión es costosa. |
1.3 Desventaja Visual
Cuando el movimiento es demasiado rápido, la cámara (obturador enrollable) experimentará desenfoque de movimiento o el área de superposición entre dos fotogramas es demasiado pequeña para permitir la coincidencia de características, por lo que SLAM visual puro no es adecuado para escenas de movimiento rápido. Cuando la imagen cambia, esencialmente no tenemos forma de saber si la cámara se ha movido o si las condiciones externas han cambiado, por lo que SLAM visual puro tiene dificultades para manejar obstáculos dinámicos. Estos son los puntos débiles de la cámara.
1.4 Desventajas de la IMU
Aunque la IMU puede medir la velocidad angular y la aceleración, estas cantidades tienen una deriva obvia, lo que hace que los datos de pose obtenidos al integrar dos veces sean muy poco confiables. Para dar una analogía exagerada, colocamos la IMU sobre la mesa para que se quede quieta, y la postura obtenida al integrar sus grados se desplazará miles de kilómetros.
1.5 Las ventajas complementarias de la visión y la IMU
La IMU puede detectar su propia información de movimiento, lo que alivia hasta cierto punto el impacto de objetos dinámicos. Además, para movimientos rápidos en un corto período de tiempo, la IMU puede proporcionar algunas buenas estimaciones. En comparación con la IMU, los datos de la cámara básicamente no se desvían. Si la cámara está fija en un lugar, entonces, en una escena estática, la estimación de pose del SLAM visual también es fija. Por lo tanto, los datos de la cámara pueden estimar y corregir eficazmente la deriva de la IMU, de modo que la estimación de la pose después de la cámara lenta siga siendo válida. En resumen, la información visual se utiliza para compensar el sesgo cero de la IMU y reducir la divergencia y los errores acumulativos causados por el sesgo cero de la IMU; la IMU se utiliza para proporcionar posicionamiento para la visión durante el movimiento rápido. En pocas palabras, IMU proporciona una mejor solución para cámara rápida y la cámara puede resolver el problema de deriva de IMU en cámara lenta. En este sentido, la IMU y la cámara se complementan.
1.6 Resumen
La visión y la fusión IMU son muy complementarias. Primero, al alinear la secuencia de pose estimada por la IMU con la secuencia de pose estimada por la cámara, se puede estimar la escala real de la trayectoria de la cámara y la IMU puede predecir bien la pose del cuadro de la imagen y los puntos característicos del anterior. momento en el siguiente cuadro de la imagen, mejorando la velocidad de coincidencia del algoritmo de seguimiento de características y la robustez del algoritmo contra la rotación rápida. Finalmente, el vector de gravedad proporcionado por el acelerómetro en la IMU puede convertir la posición estimada en el sistema de coordenadas mundial necesario para la navegación real. Al mismo tiempo, la gran demanda de dispositivos MEMS y cámaras de terminales móviles como smartphones ha reducido el precio y coste de ambos sensores. En términos de implementación de hardware, los dispositivos MEMS se pueden integrar directamente en la placa de circuito de la cámara. En resumen, el algoritmo VINS que integra información visual + IMU puede mejorar en gran medida el rendimiento del algoritmo SLAM monocular y es una solución de navegación de alto rendimiento y bajo costo.
2. Razones para introducir VIO
En general, los esquemas de visión y posicionamiento IMU tienen ciertas propiedades complementarias:
- IMU es adecuado para calcular movimientos rápidos a corto plazo;
- La visión es adecuada para calcular movimientos largos y lentos.
Después de la integración de la visión y la IMU, compensarán sus respectivas desventajas. La información de posicionamiento visual se puede utilizar para reducir la divergencia y los errores acumulativos de la IMU debido al sesgo cero; la IMU puede proporcionar visión con posicionamiento durante el movimiento rápido, reduciendo fallas de posicionamiento debido a influencias externas y, al mismo tiempo, resuelve de manera efectiva el problema de la incertidumbre en la escala de la cámara monocular.
3. Concepto de VIO
La odometría visual-inercial (VIO), conocida como navegación visual -inercial, también conocida como sistema visual-inercial (VINS, sistema visual-inercial), es un algoritmo que integra datos visuales e IMU para implementar SLAM.
4. VSLAM y VIO
VSLAM se refiere a una solución SLAM basada en visión sin información IMU; VIO se refiere a navegación visual inercial, una solución SLAM basada en la fusión de datos visión + IMU.
5. Ventajas de la visión + integración IMU
Entonces, ¿por qué deberíamos integrar visión e IMU? Los puntos principales que resumí son los siguientes:
- La integración de visión e IMU puede aprovechar la mayor frecuencia de muestreo de la IMU para aumentar la frecuencia de salida del sistema.
- La fusión de visión e IMU puede mejorar la robustez de la visión, como los resultados erróneos del SLAM visual debido a ciertos movimientos o escenas.
- La integración de la visión y la IMU puede eliminar eficazmente la deriva integral de la IMU.
La fusión de visión e IMU puede corregir el sesgo de la IMU. - La fusión de monocular e IMU puede resolver eficazmente el problema de la inobservabilidad a escala monocular.
6. Visión convencional + solución de fusión IMU
6.1 Solución monocular+IMU
ROVIO (monocular+IMU)
RKSLAM (monocular+IMU)
ORB_SLAM-IMU (monocular+IMU)
VINS (monocular+IMU, binocular+IMU)
6.2 Solución multivisor + IMU
OKVIS (monocular+IMU, binocular+IMU)
6.3 Esquema de fusión basado en filtros
Acoplamiento flojo: SSF (IMU fusionada y sensor de visión única) y MSF (IMU fusionada y sensores de posición múltiples) de ETH Zurich;
Acoplamiento estrecho: los algoritmos más clásicos son MSCKF, ROVIO;
6.4 Solución Fusion basada en optimización
Acoplamiento flexible: no hay mucho trabajo basado en la optimización del acoplamiento flexible. Gabe Sibley mencionó este método en un artículo "Métodos densos y semidensos asistidos por inercia para una odometría visual directa robusta" en IROS2016. En pocas palabras, la transformación de pose calculada por VO se agrega al marco de optimización de IMU.
Acoplamiento estrecho: OKVIS (multivisor + IMU), VINS-Mono, VINS-Fusion, VI-ORB;
7. Calibración cámara-IMU
El objetivo de la calibración de la cámara-IMU es obtener la relación espacial y el retraso de los datos entre los dos sistemas de coordenadas del sensor, lo cual es un requisito previo para que el sistema VIO funcione. La calibración de la cámara-IMU puede verse como el proceso inverso de la estimación del estado: el estado de movimiento preciso en cada momento se obtiene a través de la placa de calibración y se calculan los parámetros del modelo (rotación, desplazamiento entre sistemas de coordenadas, retardo de tiempo, polarización cero de la IMU). , mientras que la estimación del movimiento consiste en estimar el estado de movimiento en cada momento en función de los parámetros del modelo entre los dos sistemas de coordenadas del sensor conocidos.
Actualmente existe una biblioteca de calibración lista para usar: Calibración IMU de cámara . Además, la calibración de la cámara-IMU requiere conocer los parámetros internos de la cámara y la IMU de antemano. En SLAM, a menudo es necesario calibrar los parámetros internos de la cámara de las cámaras nuevas. Recomendamos la propia herramienta de calibración de cámara de ROS: Resumen, que puede lograr calibración en línea.
8. Clasificación de las VIO
Según la diferencia entre el marco de fusión, se divide en acoplamiento estrecho y acoplamiento flojo. En el acoplamiento flojo, los sistemas de estimación de movimiento visual y de estimación de movimiento de navegación inercial son dos módulos independientes, y los resultados de salida de cada módulo están fusionados, mientras que en acoplamiento estrecho, dos Los datos sin procesar de cada sensor estiman conjuntamente un conjunto de variables, y el ruido del sensor también se afecta entre sí. El algoritmo de acoplamiento estrecho es relativamente complejo, pero hacer un uso completo de los datos del sensor puede lograr mejores resultados, que es el enfoque de la investigación actual.
8.1 Acoplamiento suelto
El acoplamiento flojo trata múltiples sensores (tomando el sensor visual y la IMU como ejemplo) como dos módulos independientes. Ambos módulos pueden calcular la información de pose y luego fusionarla. De comprensión simple, la imagen estima la pose a través de los puntos característicos y luego se combina con imu para optimizar la pose. La fusión generalmente se realiza mediante filtrado EKF Kalman.
Comparación de filtrado y optimización bajo acoplamiento flexible. Como puede verse en la figura, las ventajas de la optimización no son muy obvias.
8.2 紧覦合
8.2.1 El concepto de acoplamiento estrecho
El acoplamiento estrecho se refiere a procesar los datos intermedios obtenidos por la visión y la IMU a través de un filtro de optimización . El acoplamiento estrecho requiere agregar características de la imagen al vector de características para construir conjuntamente la ecuación de movimiento y la ecuación de observación, y luego realizar una estimación del estado para finalmente obtener la información de pose. proceso. El acoplamiento estrecho utiliza datos sin procesar de dos sensores para estimar conjuntamente un conjunto de variables, y el ruido de los sensores se influye entre sí. En base a esto, la dimensión final del vector de estado del sistema será muy alta y la cantidad de cálculo también será grande. Sin embargo, hacer un uso completo de los datos de los sensores puede lograr mejores resultados, que es el foco de la investigación actual.
El diagrama de flujo general estrechamente acoplado se puede descomponer en cinco partes: preprocesamiento de datos, inicialización, optimización no lineal local, detección de cierre de bucle y optimización global. Las principales funciones de cada módulo son:
Módulo de preprocesamiento de datos de imágenes e IMU : para las imágenes, se extraen los puntos característicos y se utiliza la pirámide KLT para el seguimiento del flujo óptico a fin de prepararse para la inicialización visual posterior para resolver la pose de la cámara. Para la IMU, preintegre los datos de la IMU para obtener la pose, la velocidad y el ángulo de rotación en el momento actual. Al mismo tiempo, calcule el incremento de preintegración entre fotogramas adyacentes que se utilizarán en la optimización de back-end. y la matriz de covarianza previa a la integración y la matriz jacobiana.
Módulo de inicialización : durante la inicialización, la visión primero se inicializa para calcular la pose relativa de la cámara; luego se alinea con la preintegración de IMU para resolver los parámetros de inicialización.
Módulo de optimización no lineal local : correspondiente a la optimización no lineal de navegación inercial visual de la ventana deslizante en el diagrama de flujo, es decir, las restricciones visuales y las restricciones IMU se colocan en una función objetivo grande para la optimización. La optimización local aquí es optimizar solo las variables en la ventana del cuadro actual y los n cuadros anteriores, y la optimización no lineal local genera una pose más precisa.
Módulo de detección de bucle invertido : La detección de bucle invertido sirve para guardar los fotogramas clave de la imagen detectados previamente. Cuando regresa al mismo lugar por el que pasó originalmente, puede juzgar si ha estado aquí a través de la relación coincidente de puntos característicos. Los fotogramas clave mencionados anteriormente se filtran para anotarlos, pero se evitan fotogramas de cámara redundantes.Los criterios de selección para fotogramas clave son: el desplazamiento entre el fotograma actual y el fotograma anterior excede un cierto umbral, o el número de puntos característicos coincidentes es menor que un cierto umbral, que se registra como un fotograma clave.。
Módulo de optimización global : la optimización global utiliza restricciones de cámara y restricciones de IMU, además de las restricciones de cierre de bucle, para realizar una optimización no lineal cuando se produce la detección de cierre de bucle. La optimización global se realiza sobre la base de la optimización local y genera poses más precisas.
8.2.2 Ejemplos de código
//i时刻相机坐标系下的map point的逆深度
double inv_dep_i = parameters[3][0];
//i时刻相机坐标系下的map point坐标
Eigen::Vector3d pts_camera_i = pts_i / inv_dep_i;
//i时刻IMU坐标系下的map point坐标
Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic;
//世界坐标系下的map point坐标
Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi;
//在j时刻imu坐标系下的map point坐标
Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj);
//在j时刻相机坐标系下的map point坐标
Eigen::Vector3d pts_camera_j = qic.inverse() * (pts_imu_j - tic);
Eigen::Map<Eigen::Vector2d> residual(residuals);
#ifdef UNIT_SPHERE_ERROR
residual = tangent_base * (pts_camera_j.normalized() - pts_j.normalized());
#else
double dep_j = pts_camera_j.z();
residual = (pts_camera_j / dep_j).head<2>() - pts_j.head<2>();
#endif
Comparación de filtrado y optimización bajo acoplamiento estrecho. Como puede verse en la figura, las ventajas de la optimización son obvias.
for (uint32_t j = 0; j < gnss_meas_buf[0].size(); ++j)
{
const uint32_t sys = satsys(gnss_meas_buf[0][j]->sat, NULL);
const uint32_t sys_idx = gnss_comm::sys2idx.at(sys);
const double obs_local_ts = time2sec(gnss_meas_buf[0][j]->time) - diff_t_gnss_local;
const double lower_ts = Headers[0].stamp.toSec();
const double upper_ts = Headers[1].stamp.toSec();
const double ts_ratio = (upper_ts-obs_local_ts) / (upper_ts-lower_ts);
GnssPsrDoppFactor *gnss_factor = new GnssPsrDoppFactor(gnss_meas_buf[0][j],
gnss_ephem_buf[0][j], latest_gnss_iono_params, ts_ratio);
ResidualBlockInfo *psr_dopp_residual_block_info = new ResidualBlockInfo(gnss_factor, NULL,
vector<double *>{para_Pose[0], para_SpeedBias[0], para_Pose[1],
para_SpeedBias[1],para_rcv_dt+sys_idx, para_rcv_ddt,
para_yaw_enu_local, para_anc_ecef},
vector<int>{0, 1, 4, 5});
marginalization_info->addResidualBlockInfo(psr_dopp_residual_block_info);
}
const double gnss_dt = Headers[1].stamp.toSec() - Headers[0].stamp.toSec();
for (size_t k = 0; k < 4; ++k)
{
DtDdtFactor *dt_ddt_factor = new DtDdtFactor(gnss_dt);
ResidualBlockInfo *dt_ddt_residual_block_info = new ResidualBlockInfo(dt_ddt_factor, NULL,
vector<double *>{para_rcv_dt+k, para_rcv_dt+4+k, para_rcv_ddt, para_rcv_ddt+1},
vector<int>{0, 2});
marginalization_info->addResidualBlockInfo(dt_ddt_residual_block_info);
}
// margin rcv_ddt smooth factor
DdtSmoothFactor *ddt_smooth_factor = new DdtSmoothFactor(GNSS_DDT_WEIGHT);
ResidualBlockInfo *ddt_smooth_residual_block_info = new ResidualBlockInfo(ddt_smooth_factor, NULL,
vector<double *>{para_rcv_ddt, para_rcv_ddt+1}, vector<int>{0});
marginalization_info->addResidualBlockInfo(ddt_smooth_residual_block_info);
3. Experiencia relevante
VINS-MONO
Columna CSDN: VINS-MONO
Repositorio oficial de Github: VINS-Mono
Código fuente de github de VINS-MONO
Interpretación detallada de VINS-mono
VSLAM: comprensión del documento VINS-MONO: análisis general de ideas
Universidad de Ciencia y Tecnología de Hong Kong Introducción a VINS-MONO (1): introducción al marco y análisis del código fuente
[SLAM] Análisis de VINS-MONO - Sugerencias de mejora de vins-mono
para la navegación cartográfica - Gu Yueju
Resumen del experimento VINS-MONO
Introducción a VINS-MONO
VINS-Mono es un conjunto de algoritmos de posicionamiento de fusión visual-inercial de código abierto del equipo de Shen Shaojie en HKUST. Se implementa mediante un método de acoplamiento estrecho. La escala se restaura mediante monocular + IMU y el efecto es muy bueno.
VINOS DE FUSIÓN
Explicación completa y detallada del código front-end y back-end de VINS-FUSION