Main Content

Esta página se ha traducido mediante traducción automática. Haga clic aquí para ver la última versión en inglés.

Implementar localización y mapeo simultáneos (SLAM) con escaneos Lidar

Abrir script en vivo

Este ejemplo demuestra cómo implementar el algoritmo de localización y mapeo simultáneos (SLAM) en una serie recopilada de escaneos LIDAR mediante la optimización del gráfico de pose. El objetivo de este ejemplo es construir un mapa del entorno utilizando escaneos LIDAR y recuperar la trayectoria del robot.

Para construir el mapa del entorno, el algoritmo SLAM procesa incrementalmente los escaneos LIDAR y construye un gráfico de pose que vincula estos escaneos. El robot reconoce un lugar visitado previamente mediante la comparación de escaneos y puede establecer uno o más cierres de bucle a lo largo de su trayectoria en movimiento. El algoritmo SLAM utiliza la información de cierre del bucle para actualizar el mapa y ajustar la trayectoria estimada del robot.

Cargar datos de escaneo láser desde un archivo

Cargue un conjunto de datos muestreados que consta de escaneos láser recopilados de un robot móvil en un ambiente interior. El desplazamiento medio entre cada dos escaneos ronda los 0,6 metros.

El archivo offlineSlamData.mat contiene la variable scans , que contiene todos los escaneos láser utilizados en este ejemplo.

load('offlineSlamData.mat');

Se proporciona un plano de planta y una trayectoria aproximada del robot con fines ilustrativos. Esta imagen muestra el entorno relativo que se está mapeando y la trayectoria aproximada del robot.

Ejecute el algoritmo SLAM, construya un mapa optimizado y trace la trayectoria del robot

Cree un objeto lidarSLAM y establezca la resolución del mapa y el rango lidar máximo. Este ejemplo utiliza un robot Jackal™ de Clearpath Robotics™. El robot está equipado con un escáner láser SICK™ TiM-511 con un alcance máximo de 10 metros. Configure el rango máximo del lidar ligeramente más pequeño que el rango máximo de escaneo (8 m), ya que las lecturas del láser son menos precisas cerca del rango máximo. Establezca la resolución del mapa de cuadrícula en 20 celdas por metro, lo que da una precisión de 5 cm.

maxLidarRange = 8;
mapResolution = 20;
slamAlg = lidarSLAM(mapResolution, maxLidarRange);

Los siguientes parámetros de cierre de bucle se establecen empíricamente. El uso de un umbral de cierre de bucle más alto ayuda a rechazar falsos positivos en el proceso de identificación de cierre de bucle. Sin embargo, tenga en cuenta que una coincidencia con una puntuación alta puede seguir siendo una mala coincidencia. Por ejemplo, los escaneos realizados en un entorno que tiene características similares o repetidas tienen más probabilidades de producir falsos positivos. El uso de un radio de búsqueda de cierre de bucle más alto permite que el algoritmo busque un rango más amplio del mapa alrededor de la estimación de pose actual para cierres de bucle.

slamAlg.LoopClosureThreshold = 210;  
slamAlg.LoopClosureSearchRadius = 8;

Observe el proceso de creación de mapas con 10 escaneos iniciales

Agregue exploraciones incrementalmente al objeto slamAlg . Los números de escaneo se imprimen si se agregan al mapa. El objeto rechaza los escaneos si la distancia entre escaneos es demasiado pequeña. Agregue primero los primeros 10 escaneos para probar su algoritmo.

for i=1:10
    [isScanAccepted, loopClosureInfo, optimizationInfo] = addScan(slamAlg, scans{i});
    if isScanAccepted
        fprintf('Added scan %d \n', i);
    end
end
Added scan 1 
Added scan 2 
Added scan 3 
Added scan 4 
Added scan 5 
Added scan 6 
Added scan 7 
Added scan 8 
Added scan 9 
Added scan 10

Reconstruya la escena trazando los escaneos y las poses rastreadas por el slamAlg.

figure;
show(slamAlg);
title({'Map of the Environment','Pose Graph for Initial 10 Scans'});

Observe el efecto de los cierres de bucles y el proceso de optimización

Continúe agregando escaneos en un bucle. Los cierres de bucle deben detectarse automáticamente a medida que el robot se mueve. La optimización del gráfico de pose se realiza siempre que se identifica un cierre de bucle. La salida optimizationInfo tiene un campo, IsPerformed, que indica cuándo se produce la optimización del gráfico de pose.

Trace los escaneos y las poses cada vez que se identifique un cierre de bucle y verifique los resultados visualmente. Este gráfico muestra escaneos superpuestos y un gráfico de pose optimizado para el primer cierre del bucle. Se agrega un borde de cierre de bucle como un enlace rojo.

firstTimeLCDetected = false;

figure;
for i=10:length(scans)
    [isScanAccepted, loopClosureInfo, optimizationInfo] = addScan(slamAlg, scans{i});
    if ~isScanAccepted
        continue;
    end
    % visualize the first detected loop closure, if you want to see the
    % complete map building process, remove the if condition below
    if optimizationInfo.IsPerformed && ~firstTimeLCDetected
        show(slamAlg, 'Poses', 'off');
        hold on;
        show(slamAlg.PoseGraph); 
        hold off;
        firstTimeLCDetected = true;
        drawnow
    end
end
title('First loop closure');

Visualice el mapa construido y la trayectoria del robot

Trace el mapa final construido después de agregar todos los escaneos al objeto slamAlg . Aunque el bucle for anterior solo trazó el cierre inicial, se agregaron todos los escaneos.

figure
show(slamAlg);
title({'Final Built Map of the Environment', 'Trajectory of the Robot'});

Inspeccione visualmente el mapa construido en comparación con el plano de planta original

Una imagen de los escaneos y el gráfico de pose se superpone en el plano original. Puede ver que el mapa coincide bien con el plano original después de agregar todos los escaneos y optimizar el gráfico de pose.

Construir mapa de cuadrícula de ocupación

Los escaneos y poses optimizados se pueden utilizar para generar un occupancyMap, que representa el entorno como una cuadrícula de ocupación probabilística.

[scans, optimizedPoses]  = scansAndPoses(slamAlg);
map = buildMap(scans, optimizedPoses, mapResolution, maxLidarRange);

Visualice el mapa de la cuadrícula de ocupación con los escaneos láser y el gráfico de pose optimizado.

figure; 
show(map);
hold on
show(slamAlg.PoseGraph, 'IDs', 'off');
hold off
title('Occupancy Grid Map Built Using Lidar SLAM');