Robot SLAM y navegación autónoma (3) -Paquete de funciones SLAM

Robot SLAM y navegación autónoma (3) -Paquete de funciones SLAM

Uno, gmapping

1. Paquete de funciones Gmapping

Entrada: 1. Información de profundidad 2. Información de IMU 3. Información del odómetro
Salida: Mapa ráster

$ rosmsg show nav_msgs/OccupancyGrid 

2. El principio del valor del mapa ráster

3. instalación de gmapping

$ sudo apt-get install ros-kinetic-gmapping

4. Configure el nodo gmapping

Para obtener una descripción de los parámetros, consulte: http://wiki.ros.org/gmapping

5. Inicie la demostración de gmapping (la información de profundidad se basa en lidar)

Abra tres terminales y ejecute los siguientes comandos:

$ roslaunch mbot_gazebo mbot_laser_nav_gazebo.launch    # 启动仿真环境
$ roslaunch mbot_navigation gmapping_demo.launch  # 启动建图节点，灰色地图建成，黑色障碍物
$ roslaunch mbot_teleop mbot_teleop.launch        #启动键盘控制节点 

Después de crear el mapa, guarde el mapa

$ rosrun map_server map_saver -f cloister_gmapping  

# cloister_gmapping是文件名的意思，是自己保存文件名的意思
# 保存的路径在当前/home文件夹下,有两个文件.pgm和.yaml

Ver el archivo de mapa generado

6. Inicie gmapping (kinect)

Efecto de mapeo deficiente (no recomendado)

$ roslaunch mbot_gazebo mbot_kinect_nav_gazebo.launch
$ roslaunch mbot_navigation gmapping_demo.launch
$ roslaunch mbot_teleop mbot_teleop.launch

二 、 hector_slam

1. paquete de funciones hector_slam

Entrada: información de profundidad
Salida: mapa ráster

2. Instala hector_slam

$ sudo apt-get install ros-kinetic-hector-slam

3. Configurar el nodo hector_mapping

Para obtener una descripción del parámetro, consulte: http://wiki.ros.org/hector_slam

4. Inicie la demostración de hector_slam (la información de profundidad se basa en lidar)

El primer nodo inicia la simulación de la glorieta, que utiliza el radar simulado para dar un escaneo de datos de escaneo; luego los nodos de abajo se suscriben a estos datos y los procesan, y finalmente obtienen un mapa de cuadrícula bidimensional.

5. Fenómeno de deslizamiento

Cuando se utiliza el paquete de funciones hector_slam, el robot no debe moverse demasiado rápido, de lo contrario el cálculo no se mantendrá al día y se producirá un deslizamiento. El

algoritmo de Hector tiene un gran problema, que radica en su autoposicionamiento. Utiliza sus propios datos LIDAR. para el posicionamiento. Si los datos de escaneo de un escaneo son básicamente los mismos que los datos de escaneo del siguiente, es difícil determinar si se está moviendo. Por ejemplo, cuando doy un espacio lo suficientemente grande, hay un pasillo largo sin ningún puntos característicos, que es mucho más largo que el láser Al escanear la distancia máxima, Héctor básicamente pierde la capacidad de crear imágenes.

三 、 cartógrafo

1. Paquete de funciones de cartógrafo

2. Instalación del cartógrafo

Método uno (método nuevo):

sudo apt-get update
 
sudo apt-get install ros-<your ros version>-cartographer*  # 安装全部关于cartographer的包

Método 2: El
paquete de funciones de cartógrafo no está integrado en la fuente del software ROS, por lo que debe instalarse mediante la compilación del código fuente. Para no entrar en conflicto con otros paquetes de instalación, es mejor crear un espacio de trabajo especial para el cartógrafo. Aquí creamos un espacio de trabajo catkin_google_ws:

posibles problemas con el paquete de funciones del cartógrafo.

3. Configurar el nodo cartógrafo

4. Inicie la demostración de demostración de slam 2D

5. Inicie la demostración de demostración de 3D Slam

6. Configuración de parámetros

7. Inicie la simulación de cartógrafo

Cuatro, Karto_slam

1. Descripción general de Karto_slam

Evaluación:
Karto_SLAM se basa en la idea de optimización de grafos y utiliza como solución una alta optimización y descomposición cholesky no iterativa para el desacoplamiento de sistemas dispersos. El método de optimización de gráficos utiliza el valor promedio del gráfico para representar el mapa, y cada nodo representa un punto de posición de la trayectoria del robot y el conjunto de datos de medición del sensor, y cada nuevo nodo se agrega, se calculará y actualizará.
La versión ROS de Karto_SLAM, en la que el ajuste de pose de repuesto (SPA) está relacionado con la coincidencia de escaneo y la detección de circuito cerrado. Cuantos más puntos de referencia, mayores son los requisitos de memoria. Sin embargo, el método de optimización del mapa tiene una ventaja mayor que otros métodos para dibujar en un entorno grande, ya que solo contiene un mapa de puntos (pose de robot) y el mapa se calcula después de la pose. es obtenido.

2. Introducción específica de Karto_slam

El marco general del programa

Como se puede ver en la figura anterior, el proceso es bastante simple: el mecanismo tradicional de operación suave en tiempo real de Slam, cada vez que se ingresa un marco de datos, se procesa y luego se devuelve.
1)
Palabras clave de coincidencia de exploración :
Región de interés: el
método de coincidencia es scanTomap, y la región de interés es un submapa rectangular, que también puede entenderse como un modelo de referencia.
Área de búsqueda:
un área rectangular centrada en la posición estimada por el odómetro para indicar el rango posible de la posición final. Al hacer coincidir, atraviesa el área de búsqueda para obtener la posición con el valor de respuesta más alto.
Tabla de búsqueda:
Para la información de datos obtenida por el láser, proyecte con una determinada resolución angular y valor de compensación angular para obtener una tabla de búsqueda para la coincidencia.
Running-scans:
mantenimiento en tiempo real del enlace de datos láser local. El primero y los dos últimos fotogramas están dentro de una cierta distancia y cumplen una determinada escala de datos. De lo contrario, es necesario eliminar el fotograma de datos final. Mantenga el enlace de datos local actual.
Generar submapa (porque el método de coincidencia es ScanToMap).

Generar tabla de búsqueda:
el significado de la tabla de búsqueda es que, en comparación con la coincidencia violenta, no recalcula cada información de datos láser cada vez. La información de datos láser en el mismo ángulo y en diferentes posiciones solo necesita para ser indexado una vez.

El ángulo de actitud estimado actual se puede obtener mediante la predicción del odómetro, el ángulo de actitud real debe estar cerca;
la información del láser representada en el sistema de coordenadas del robot original (sistema de coordenadas local) se asigna a diferentes ángulos con una determinada resolución y valor de compensación, Obtener una tabla de búsqueda que contiene n ángulos.
Entre ellos:

tabla de búsqueda -> submapa (coincidencia)
El estado del robot móvil en sí incluye (x, y, θ). El problema del ángulo se resuelve de manera efectiva con la tabla de búsqueda, y el problema de posición aún debe resolverse en este momento. El área de búsqueda discretizada se usa para el posicionamiento y la resolución múltiple se utiliza para aumentar la velocidad de búsqueda.

El cálculo del valor de respuesta se describe de la siguiente manera: envía la tabla de búsqueda al submapa en un cierto nivel. En este momento, el submapa se ha generado mediante escaneos en ejecución y se usa Gaussian para difuminar. Suponiendo que hay un total de n puntos aciertos por la tabla de búsqueda, aciertos Cada punto en la puntuación es diferente (el efecto del desenfoque gaussiano), la puntuación se acumula y se divide por la puntuación más alta que se puede lograr.

Tabla de búsqueda -> submapa (coincidencia aproximada -> coincidencia fina)
Para mejorar la eficiencia de la búsqueda, se adopta un método de resolución múltiple, es decir, el área candidata se busca con una resolución más baja durante la coincidencia aproximada, y luego el área candidata se divide para obtener una solución precisa.

Cálculo del valor medio del nodo
En cada proceso de coincidencia (coincidencia aproximada y coincidencia fina), seleccione varios estados de pose con valores de respuesta óptimos y tome el valor promedio como resultado de coincidencia.
El valor de respuesta óptimo es uno, y se establece un 10 ^ -6 como tolerancia. Una vez que el valor de respuesta de otros estados de pose y el valor de respuesta óptimo están dentro de la tolerancia, es igual; el nodo final es su valor promedio.
Iteración:
coincidencia aproximada (media obtenida) -> (como valor inicial) coincidencia precisa (para dar un promedio final)
calcular la covarianza del nodo - posición de la covarianza (coincidencia aproximada)

calcular los nodos de covarianza - el ángulo de covarianza (coincidencia

aproximada ) por encima de la Asociación Existe una situación en la que la varianza es contraria a la teoría y el componente principal calculado cada vez debe multiplicarse por un valor de respuesta.
2) Añadir vértices y aristas
Añadir vértices: la pose del fotograma clave
Añadir aristas: el fotograma actual está conectado con el fotograma anterior, escaneos en ejecución y cadenas cercanas.

Hay tres pasos principales para agregar un borde:
(1) Enlace al escaneo previo
(2) Enlace a escaneos en
ejecución Cadena RunningScan: un cierto número de enlaces de datos láser dentro de una cierta distancia de la corriente.
(3) Enlace a otras cadenas cercanas
NearChain: atraviesa todos los nodos dentro de una cierta distancia desde el gráfico de una manera amplia desde el nodo actual, y busca cadenas dentro de un cierto rango desde el sensorManager de acuerdo con la identificación actual. NearLinkScans.
Los dos primeros pasos son fáciles de entender, principalmente el tercero, con el nodo actual como centro de búsqueda, realice una búsqueda de amplitud en un rango determinado (por ejemplo, 4 m), obtenga todos los nodos asociados y luego use los nodos para generar un enlace de datos (el enlace de datos necesita Contiene alrededor de 5 fotogramas clave consecutivos con ID) (excluyendo el nodo actual; de lo contrario, el enlace de datos se descarta), el nodo actual se usa para hacer coincidir, si el valor de respuesta alcanza un cierto umbral, un borde Un extremo de este borde es el nodo actual. Un extremo es el nodo cuyo centroide está más cerca del centroide del nodo actual en la cadena de datos.

3) Detección
de bucle La operación de detección de bucle es similar a agregar bordes adyacentes, y los pasos son más engorrosos:
(1) Según el nodo actual, busque todos los nodos adyacentes a él (dentro de una cierta distancia) del gráfico.
(2) Adopte el método de búsqueda en amplitud y agregue adyacente (siguiente) y conectado (adyacenteVertices) a nearLinkedScans.
(3) Tome de adelante hacia atrás del sensorManager, seleccione los escaneos candidatos que estén dentro de una cierta distancia de la corriente y no en los escaneos nearLinkedScans de acuerdo con el número de serie de identificación, y regrese cuando el número alcance un cierto tamaño.
(4) loopScanMatcher realiza la coincidencia de scanTomap. Cuando la respuesta de coincidencia y la covarianza cumplen ciertos requisitos, se considera que se detecta el bucle cerrado. Consigue la pose correcta ajustada.
(5) Agregar enlace al bucle: ajuste el borde (bucle cerrado global).
(6) Disparador correctoPose: El
primer paso de la optimización del spa , primero eliminar los nodos que están adyacentes al nodo actual en el tiempo, generar un enlace de datos para aquellos nodos que están dentro del rango de búsqueda y no son adyacentes en el tiempo, y luego proceder Coincidir, si el valor de respuesta es mayor que el umbral, agregue loopback y realice una optimización global.

4) Solución

optimizada Todo el algoritmo de optimización utiliza el algoritmo de optimización LM. En el texto original, el autor tiene una gran cantidad de información sobre cómo obtener la matriz Hessian y optimizar la solución de manera eficiente. El sistema LM es el siguiente:

La escasez de la matriz de Hesse:

3. resumen de karto_slam

1) Desde la perspectiva del artículo solo, el artículo no presenta el marco completo, la mayor parte del contenido trata sobre métodos de optimización, la construcción de la matriz de Hesse, etc. Hay muy poca mención sobre la coincidencia de escaneo, vértices y bordes, detección de bucles, etc.
2) El código fuente contiene dos partes, la función principal y la función de biblioteca, con un marco claro y una gran legibilidad; a excepción de eigen, no se requieren bibliotecas dependientes adicionales.
3) No hay tantas introducciones sobre karto_slam como hector y gmapping, es posible que no se adopte el mecanismo principal de actualización del mapa (mapa de probabilidad). Solo el nodo de pose y los datos del láser se guardan como gráfico de pose.

Cinco, ORB_SLAM

1. Paquete de funciones ORB_SLAM

基于特征点的实时单目SLAM系统
实时解算摄像机的移动轨迹
构建三维点云地图
不仅适用于手持设备获取的一组连续图像，也可以应用于汽车行驶过程中获取的连续图像

Raul Mur-Artal，J.M.M.Montiel和Juan D.Tardos于2015年发表在IEEE Transactions on Robotics上

2. Instalación de ORB_SLAM2

1), herramienta de instalación y código fuente de descarga:

sudo apt-get install libboost-all-dev libblas-dev liblapack-dev
git clone https://github.com/raulmur/ORB_SLAM2.git ORB_SLAM2       # 克隆到home下

2), instale eigen3.2.10

去官网下载：http://eigen.tuxfamily.org/index.php?title=Main_Page

解压源码包，并进入目录：

mkdir build

cd build

cmake ..

make

sudo make install

3), compila g2o

cd ~/ORB_SLAM2/Thirdparty/g2o/

mkdir build

cd build

cmake .. -DCMAKE_BUILD_TYPE=Release

make

4), compila DBoW2

cd ~/ORB_SLAM2/Thirdparty/DBoW2/

mkdir build && cd build 

cmake .. -DCMAKE_BUILD_TYPE=Release

make

5), compila Pangolin

sudo apt-get install libglew-dev 

git clone https://github.com/stevenlovegrove/Pangolin.git

cd Pangolin

mkdir build && cd build 

cmake ..

cmake --build .

6), compila ORM_SLAM

cd ~/ORB_SLAM2

mkdir build && cd build 

cmake .. -DCMAKE_BUILD_TYPE=Release

make

7), compile el paquete de funciones (configure el entorno ROS)

# 首先修改.bashrc文件
$ cd ~
$ gedit .bashrc

# 打开.bashrc文件在最后一行加入
source ~/ORB_SLAM2/Examples/ROS/ORB_SLAM2/build/devel/setup.bash

$ source ~/.bashrc

Nota: El séptimo paso, el problema de compilar el paquete de funciones:

solución:

修改 ~/ORB_SLAM2/Examples/ROS/ORB_SLAM2/CMakeLists.txt,添加 -lboost_system

3. Inicie el ejemplo de SLAM monocular (basado en el paquete de datos)

1) Primero descargue el paquete de datos TUM: esto es principalmente para interiores, monocular y RGB-D

https://vision.in.tum.de/data/datasets/rgbd-dataset/download#freiburg1_desk

在ORB-SLAM2下新建文件夹Data，把测试的数据解压在这里。
TUM数据集分为相机fr1，fr2，fr3，对应TUM1-3.yaml；
一般第一次测试用fr1/xyz这个数据集，这个就是x，y，z方向来回动，用来检测一下系统出没出什么问题。
其他的数据看名字就知道，比如desk就是在桌子附近来回转，room就是在房间里面扫来扫去。
值得注意的是，运行其他数据集的时候，单目不一定能追踪成功，在台式机上能成功的在虚拟机上也不一定能成功，这就需要我们进行一些调整，比如调整初始化需求数量等，这个关系到对SLAM系统的理解。

2) Abra tres terminales y ejecute los siguientes tres comandos:

$ roscore

# 进入ORB_SLAM2目录下启动Mono功能节点。
# 看到开启的等待数据的可视化建图界面
$ rosrun ORB_SLAM2 Mono Vocabulary/ORBvoc.txt Examples/ROS/ORB_SLAM2/Asus.yaml

# 进入数据包所在目录下，运行命令
# /camera/image_raw表示播放的数据要发布的话题名称，此时可以在界面中看到建图的效果
$ rosbag play rgbd_dataset_freiburg1_desk.bag /camera/rgb/image_color:=/camera/image_raw

PATH_TO_VOCABULARY：算法参数文件，在ORB_SLAM2/Vocabulary中，将其中的压缩包解压即可；

PATH_TO_SETTINGS_FILE：相机参数设置文件，需要对camera进行标定产生，也可以使用ORB_SLAM2/Examples/ROS/ORB_SLAM2中已有的设置文件Asus.yaml。

4. Inicie el ejemplo de AR (basado en el paquete de datos)

$ roscore

# 进入ORB_SLAM2目录下启动Mono功能节点。
# 看到开启的等待数据的可视化建图界面
$ rosrun ORB_SLAM2 MonoAR Vocabulary/ORBvoc.txt Examples/ROS/ORB_SLAM2/Asus.yaml

# 进入数据包所在目录下，运行命令
# /camera/image_raw表示播放的数据要发布的话题名称，此时可以在界面中看到建图的效果
$ rosbag play rgbd_dataset_freiburg1_desk.bag /camera/rgb/image_color:=/camera/image_raw

Haga clic en la interfaz Insertar cubo, vea el cuadrado insertado, muestre el efecto AR

5. Inicie el ejemplo de ORB_SLAM (cámara real)

$ roslaunch mbot_navigation usb_cam_remap.launch

# 进入ORB_SLAM2目录下启动Mono功能节点。
# 看到开启的等待数据的可视化建图界面
$ rosrun ORB_SLAM2 Mono Vocabulary/ORBvoc.txt Examples/ROS/ORB_SLAM2/Asus.yaml

6. Inicie el ejemplo de RA (cámara real)

Es necesario mover la cámara para completar la construcción del mapa.

$ roslaunch mbot_vision usb_cam_remap.launch

# 进入ORB_SLAM2目录下启动Mono功能节点。
# 看到开启的等待数据的可视化建图界面
$ rosrun ORB_SLAM2 MonoAR Vocabulary/ORBvoc.txt Examples/ROS/ORB_SLAM2/Asus.yaml