Cartographer es un sistema complejo. Para depurarlo, primero debe tener una buena comprensión de su principio de funcionamiento interno. Este artículo intenta ofrecer una visión general intuitiva de cada subsistema de cartógrafo y sus parámetros de configuración. Si está muy interesado en el cartógrafo, le recomendamos que lea este artículo. Aunque solo introduce 2D-SLAM, introduce estrictamente cada concepto que presentaremos aquí. Por supuesto, estos conceptos también son aplicables a 3D-SLAM.
W. Hess, D. Kohler, H. Rapp y D. Andor, Cierre de bucle en tiempo real en 2D LIDAR SLAM , en Robótica y Automatización (ICRA), Conferencia Internacional IEEE 2016 sobre. IEEE, 2016. pp. 1271–1278.
Resumen
El cartógrafo puede verse como dos subsistemas independientes y relacionados. El primero se llama SLAM local (a veces lo llamamos generador de trayectoria local o front-end ). Su trabajo es construir subgrafías continuas . Cada subgrafo es localmente consistente y libre de errores, pero también reconoce el problema de que los errores locales de SLAM se acumulan gradualmente con el tiempo. La mayoría de los archivos de configuración locales de SLAM se pueden encontrar aquí: 2D: /cartographer/configuration_files/trajectory_builder_2d.lua 3D: /cartographer/configuration_files/trajectory_builder_3d.lua Otro subsistema se llama SLAM global (a veces también lo llamamos backend ) . Se ejecuta en un subproceso en segundo plano, y su trabajo principal es encontrar las restricciones de la detección de bucle cerrado (escaneando el marco del láser para que coincida con el subgráfico creado anteriormente). También puede usar los datos de otros sensores para fusionarse con el láser y obtener un resultado globalmente consistente (piense en el punto de referencia de la cámara que mencionamos anteriormente). En 3D, también puede corregir la dirección de la gravedad. La mayor parte de la totalidad de la Junta SLAM archivo de configuración se puede encontrar aquí: 3D: /cartographer/configuration_files/pose_graph.lua
En pocas palabras, SLAM local es construir cada buen subgrafo , y SLAM global es vincular cada subgrafo correctamente.
De entrada
Debido al error de medición y al ruido del punto láser que está demasiado cerca y demasiado lejos, los datos del punto láser de entrada pueden estar limitados por la distancia mínima y la distancia máxima. Los parámetros específicos son los siguientes:
TRAJECTORY_BUILDER_nD.min_range
TRAJECTORY_BUILDER_nD.max_range
Nota:
1. Para puntos láser mayores que el rango máximo, TRAJECTORY_BUILDER_2D.missing_data_ray_length se usará para reemplazar su distancia. Este enfoque es para evitar el problema de pérdida de datos a corta distancia debido a que la detección de puntos está demasiado lejos.
2. Las unidades de distancia en cartógrafo son metros.
Debido al problema de la distorsión del movimiento del láser, cada punto del láser en el cartógrafo tiene una marca de tiempo y luego corrige el problema de la distorsión del movimiento del láser de acuerdo con los datos de alta frecuencia, como imu, por lo que se recomienda recolectar la frecuencia del láser lo más alta posible.
El método de medición láser 2D da como resultado puntos láser densos a distancias cortas y puntos dispersos en la distancia. Para resolver este problema, utilizamos disminución de muestreo de la red de vóxel . Este método se entiende bien. Consulte varias muestras de PCL Método, los parámetros específicos son los siguientes
TRAJECTORY_BUILDER_nD.voxel_filter_size
Además, existe otro método adaptativo de filtrado de voxel ( muestreo ) en el cartógrafo , cuyo propósito es calcular adaptativamente el intervalo de muestreo apropiado para obtener el número especificado de nubes de puntos. Los parámetros específicos son los siguientes:
TRAJECTORY_BUILDER_nD.*adaptive_voxel_filter.max_length //采样最大间隔
TRAJECTORY_BUILDER_nD.*adaptive_voxel_filter.min_num_points //采样后最小点云数量
IMU es una fuente de datos muy efectiva, ya que proporciona una dirección de gravedad precisa, proporciona el valor inicial de cada marco láser, lo que reduce en gran medida la cantidad de cálculo de coincidencia (especialmente 3D) (creo que esto se compara en 3D-SLAM Obviamente[Con la resolución de problemas] Creo que la dirección de la gravedad no se usa en 2D, pero la aceleración de imu se usa para corregir la distorsión del láser), así que no importa 2D o 3D, si tiene datos de imu, debe usarlos.
[¿Por qué? ]Me preguntará si no tengo datos de imu, ¿puede 2D-SLAM no tener forma de construir una imagen? La respuesta es sí, pero hay que pagar otro precio para abrir.
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
El papel de esta oración es usar la pose de pose de láser de correlación de CSM como el valor inicial de la coincidencia de optimización de ceres antes de usar ceres para la coincidencia de optimización. En este caso, puede reemplazar el efecto de imu para calcular el valor inicial de pose, pero esto aumentará La cantidad de cálculo en el extremo frontal, así que úselo según corresponda según su propia situación. Los parámetros específicos son los siguientes:
TRAJECTORY_BUILDER_2D.use_imu_data
TRAJECTORY_BUILDER_nD.imu_gravity_time_constant
SLAM local
SLAM local se lleva a cabo por cada uno de los sub-trama actual de láser figura escaneo emparejado construido, pero antes del partido por estimador pose ( extrapolador POSE administró cada trama coincida con la posición inicial y la orientación del láser), pero estimador pose Además del uso de datos láser, también se pueden usar otros datos (como imu, odom, etc.) El inferidor de pose implementa una actualización de pose que combina múltiples datos.
Hay 2 formas de escanear coincidencias:
- Coincidencia CeresScanMatcher: utilice la biblioteca de optimización ceres para construir ecuaciones de optimización para la coincidencia. Esta coincidencia es adecuada para proporcionar buenos valores iniciales de poses. Puede lograr una coincidencia de subpíxeles, que es rápida. La desventaja es que si los valores iniciales no son ideales, la coincidencia puede no ser Ideal
- Coincidencia RealTimeCorrelativeScanMatcher (coincidencia CSM): cuando no tiene otros datos o no confía en otros datos, utiliza una estrategia de coincidencia similar a la detección de bucle invertido, y utiliza el resultado coincidente como el valor inicial de la coincidencia CeresScanMatcher. Esta coincidencia es más costosa Alto, abrumará el papel de otros sensores, pero es más robusto en un entorno rico en funciones, y aumenta el cálculo de las desventajas.
La coincidencia de CeresScanMatcher puede dar peso a los datos de entrada. Este peso es la confianza de los datos medidos y puede considerarse como una covarianza estática. Cuanto mayor sea el peso, más se dedicará el cartógrafo a esta regla de parámetro. Estos pesos incluyen:
TRAJECTORY_BUILDER_3D.ceres_scan_matcher.occupied_space_weight
TRAJECTORY_BUILDER_3D.ceres_scan_matcher.occupied_space_weight_0
TRAJECTORY_BUILDER_3D.ceres_scan_matcher.occupied_space_weight_1
TRAJECTORY_BUILDER_nD.ceres_scan_matcher.translation_weight
TRAJECTORY_BUILDER_nD.ceres_scan_matcher.rotation_weight
La coincidencia CeresScanMatcher utiliza ceres de Google para construir un problema de mínimos cuadrados no lineal. Este problema de coincidencia de plantillas se construye para resolver los parámetros de la matriz de rotación entre 2 cuadros de datos láser. Ceres utiliza el descenso de gradiente y establece el número de iteraciones para optimizar este problema. Ceres puede configurar la velocidad de convergencia de acuerdo con sus necesidades.
TRAJECTORY_BUILDER_nD.ceres_scan_matcher.ceres_solver_options.use_nonmonotonic_steps //？？？
TRAJECTORY_BUILDER_nD.ceres_scan_matcher.ceres_solver_options.max_num_iterations
TRAJECTORY_BUILDER_nD.ceres_scan_matcher.ceres_solver_options.num_threads
La coincidencia RealTimeCorrelativeScanMatcher se puede configurar de acuerdo con su confianza en el sensor, tiene una ventana de búsqueda (que incluye una distancia de búsqueda máxima y un ángulo de búsqueda máximo) y 2 pesos, respectivamente, establecen el costo de la traducción y la rotación, es decir, la distancia desde la inicial Cuanto más lejos esté la postura, mayor será el costo y la credibilidad disminuirá gradualmente. Los parámetros específicos son los siguientes:
TRAJECTORY_BUILDER_nD.use_online_correlative_scan_matching
TRAJECTORY_BUILDER_nD.real_time_correlative_scan_matcher.linear_search_window
TRAJECTORY_BUILDER_nD.real_time_correlative_scan_matcher.angular_search_window
TRAJECTORY_BUILDER_nD.real_time_correlative_scan_matcher.translation_delta_cost_weight
TRAJECTORY_BUILDER_nD.real_time_correlative_scan_matcher.rotation_delta_cost_weight
Para evitar insertar demasiados datos redundantes en una imagen secundaria , los parámetros de configuración aquí solo insertan la imagen secundaria de este cuadro de datos láser cuando los dos cuadros de datos láser están separados por un cierto ángulo, distancia y tiempo.
TRAJECTORY_BUILDER_nD.motion_filter.max_time_seconds
TRAJECTORY_BUILDER_nD.motion_filter.max_distance_meters
TRAJECTORY_BUILDER_nD.motion_filter.max_angle_radians
El número de cuadros láser en una imagen secundaria se configura manualmente. Cuando una imagen secundaria acumula un número fijo de datos láser, se genera una imagen secundaria . El número de láseres en una imagen secundaria no puede ser demasiado grande, demasiado grande puede causar deriva de pose, ni demasiado pequeño, demasiado pequeño no es propicio para la detección de bucle de fondo. El valor específico debe determinarse mediante depuración manual, los parámetros son los siguientes
TRAJECTORY_BUILDER_nD.submaps.num_range_data
El mapa tiene 2 formatos de almacenamiento: 1 es un mapa de probabilidad, 1 es un TSDF, los parámetros son los siguientes:
TRAJECTORY_BUILDER_2D.submaps.grid_options_2d.grid_type
La cuadrícula de probabilidad divide el espacio en cuadrículas bidimensionales o tridimensionales. Cada cuadrícula ahorra la probabilidad de ser bloqueada. La probabilidad se actualiza de acuerdo con el peso de aciertos y fallas. Los parámetros específicos son los siguientes:
TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.probability_grid_range_data_inserter.hit_probability
TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.probability_grid_range_data_inserter.miss_probability
TRAJECTORY_BUILDER_3D.submaps.range_data_inserter.hit_probability
TRAJECTORY_BUILDER_3D.submaps.range_data_inserter.miss_probability
Los mapas de probabilidad 2D y 3D se pueden ver en rviz
SLAM global
SLAM local genera sub-gráficos uno por uno, mientras que SLAM global es responsable de reajustar la postura de cada sub-gráfico en segundo plano a través de la optimización (SPA) para formar un mapa global completo y coherente. Esta optimización es responsable de cambiar la trayectoria actualmente construida para alinear adecuadamente los subgramas basados en la detección de bucle cerrado.
La optimización global se realiza de acuerdo con una frecuencia regular determinada. Cuando se detecta que se inserta un cierto número de nodos de trayectoria en el mapa, se realiza una optimización. La configuración de los parámetros es la siguiente:
POSE_GRAPH.optimize_every_n_nodes
Nota: Cuando POSE_GRAPH.optimize_every_n_nodes es igual a 0, es equivalente a desactivar la optimización global para que pueda centrarse en el estado de SLAM local, que a menudo es el primer paso para depurar el cartógrafo.
Global SLAM también se puede llamar optimización de gráficos , estableciendo restricciones entre los nodos de ruta y subgrafos , y luego optimizando estas restricciones . Los nodos y subgrafos de ruta son poses espaciales, y las restricciones son matrices de rotación y traslación de poses espaciales. La guía automática elimina el trabajo de buscar a Jacoby. Nota: Estas restricciones se pueden mostrar en rviz y abrir
POSE_GRAPH.constraint_builder.log_matches
Luego puede ver el informe de registro en forma de histograma restringido.
Hay dos tipos de restricciones: restricciones no globales y restricciones globales
- Las restricciones no globales son restricciones que están relativamente cerca de sus propios nodos de ruta. Generalmente, se establecen entre el subgrafo anterior y el subgrafo actual. El propósito es mantener la continuidad de la ruta local.
- Las restricciones globales son restricciones que se establecen cuando se detecta un bucle. Generalmente se establecen entre el subgrafo recién generado y el nodo de ruta anterior. El propósito es mantener la continuidad de la ruta global.
Los parámetros son los siguientes:
POSE_GRAPH.constraint_builder.max_constraint_distance
POSE_GRAPH.fast_correlative_scan_matcher.linear_search_window
POSE_GRAPH.fast_correlative_scan_matcher_3d.linear_xy_search_window
POSE_GRAPH.fast_correlative_scan_matcher_3d.linear_z_search_window
POSE_GRAPH.fast_correlative_scan_matcher*.angular_search_window
Nota: De hecho, las restricciones globales se pueden realizar no solo en una sola ruta, sino también en múltiples robots y múltiples rutas. De hecho, esta función no es difícil de entender. Por ejemplo, usted ha construido una parte del mapa. De repente, el robot se ha quedado sin energía en este momento. ¿No ha sido creado el mapa antes en vano? Por supuesto, este no es el caso. Puede guardar el mapa creado anteriormente y cargar el mapa anterior la próxima vez. Este es el mismo principio que la ruta múltiple de varios robots, pero no lo he probado, tengo tiempo para probarlo.
Para limitar el número de restricciones, el cartógrafo restringe algunos nodos de ruta para establecer restricciones y controles mediante un factor de muestreo restringido . Sin embargo, un factor que es demasiado pequeño puede causar restricciones faltantes y detección de bucle invertido inválido. Un factor que es demasiado grande reducirá la eficiencia del SLAM global y No se puede realizar loopback en tiempo real. Los parámetros son los siguientes:
POSE_GRAPH.constraint_builder.sampling_ratio
pruebas de bucle necesitan utilizar un juego de láser, y este algoritmo se llama FastCorrelativeScanMatcher, fue diseñado para conseguir en tiempo real bucle de detección tanto como sea posible, se utiliza un " rama y obligado " ( Branch y ligado ) método para lograr, de hecho, similar a la comparación de plantillas La estrategia de usar la pirámide de imágenes es buscar primero a baja resolución, y luego buscar gradualmente píxeles a alta resolución.El método recursivo se implementa en código 2d, solo 20 líneas de código. Quizás este algoritmo sea más claro en 3D. Los parámetros específicos son los siguientes:
POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher.branch_and_bound_depth
POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher_3d.branch_and_bound_depth
POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher_3d.full_resolution_depth
Una vez que FastCorrelativeScanMatcher encuentra una coincidencia lo suficientemente buena (mayor que el valor de coincidencia mínimo), utilizará Ceres Scan Matcher para optimizar aún más la pose (resultado de coincidencia).
POSE_GRAPH.constraint_builder.min_score
POSE_GRAPH.constraint_builder.ceres_scan_matcher_3d
POSE_GRAPH.constraint_builder.ceres_scan_matcher
Cuando el cartógrafo ejecuta la optimización de back-end, Ceres ajusta la postura del subgrafo a través de una variedad de residuos, y cada residuo recibe diferentes pesos (también llamados funciones de costo ) para ajustar el nivel de confianza de las diferentes fuentes de datos. No es la sombra de la matriz de covarianza. Estos pesos incluyen: pesos de restricción global (detección de bucle cerrado = cierre de bucle), restricciones no globales (restricción local = igualador), aceleración de IMU y pesas de medición de velocidad angular, pesos de cuadro fijo (como sistemas GPS), estos parámetros son los siguientes:
POSE_GRAPH.constraint_builder.loop_closure_translation_weight
POSE_GRAPH.constraint_builder.loop_closure_rotation_weight
POSE_GRAPH.matcher_translation_weight
POSE_GRAPH.matcher_rotation_weight
POSE_GRAPH.optimization_problem.*_weight
POSE_GRAPH.optimization_problem.ceres_solver_options
El resultado de la optimización se puede ajustar por el número máximo de iteraciones.
POSE_GRAPH.max_num_final_iterations
IMU es parte del residuo. Por defecto, el problema de optimización proporciona cierta flexibilidad para la pose de IMU, es decir, los parámetros externos entre IMU y tracking_frame pueden usarse como variables de optimización para resolver, y los resultados de la optimización global de ceres se usan para parámetros externos Corregido y salida al registro. Si el resultado de la optimización no es ideal o si cree en sus propios parámetros externos, puede establecer esta variable como una constante con los siguientes parámetros:
(Todavía no se ha probado ...)
POSE_GRAPH.optimization_problem.log_solver_summary
POSE_GRAPH.optimization_problem.use_online_imu_extrinsics_in_3d
En el procesamiento residual, la influencia de los valores atípicos (datos incorrectos) está controlada por la función de pérdida de Huber, que es una función del núcleo que se ajusta mediante un factor de Huber. Los datos incorrectos) tienen un mayor impacto en general.
POSE_GRAPH.optimization_problem.huber_scale
Una vez que se completa la trayectoria, el cartógrafo realizará una nueva optimización global, y generalmente realizará más iteraciones que la optimización global anterior. Esto se hace para mejorar el resultado final del cartógrafo, que generalmente no necesita realizarse en tiempo real, por lo que una gran cantidad de iteraciones suele ser la opción correcta.
POSE_GRAPH.max_num_final_iterations
De hecho, todavía hay muchos detalles que no se explican, que se agregarán.
