Esta página es para la versión anterior. La página correspondiente en inglés ha sido eliminada en la versión actual.

Implemente localización y mapeo simultáneos (SLAM) con exploraciones LiDAR

En este ejemplo se muestra cómo implementar el algoritmo de localización y asignación simultánea (SLAM) en una serie recopilada de exploraciones LiDAR mediante la optimización de gráficos de pose. El objetivo de este ejemplo es crear un mapa del entorno utilizando los escaneos LiDAR y recuperar la trayectoria del robot.

Para compilar el mapa del entorno, el algoritmo SLAM procesa de forma incremental los escaneos LiDAR y crea un gráfico de pose que vincula estos escaneos. El robot reconoce un lugar previamente visitado a través de la coincidencia de escaneo y puede establecer uno o más cierres de bucle a lo largo de su camino de movimiento. El algoritmo de SLAM utiliza la información de cierre de bucle para actualizar el mapa y ajustar la trayectoria del robot estimado.

Cargar datos de escaneo láser desde archivo

Cargue un conjunto de datos muestreados por abajo que consiste en escaneos láser recogidos de un robot móvil en un entorno interior. El desplazamiento medio entre cada dos escaneos es de alrededor de 0,6 metros.

El archivo contiene la variable, que contiene todos los escaneos láser utilizados en este ejemploofflineSlamData.matscans

load('offlineSlamData.mat');

Un plano de planta y la ruta aproximada del robot se proporcionan con fines ilustrativos. Esta imagen muestra el entorno relativo que se está mapando y la trayectoria aproximada del robot.

Ejecutar algoritmo de SLAM, construir mapa optimizado y trazar la trayectoria del robot

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

maxLidarRange = 8; mapResolution = 20; slamAlg = robotics.LidarSLAM(mapResolution, maxLidarRange);

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

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

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

Agregar exploraciones de forma incremental al objeto.slamAlg Los números de escaneo se imprimen si se agregan al mapa. El objeto rechaza los escaneos si la distancia entre los escaneos es demasiado pequeña. Agrega primero los primeros 10 escaneos para probar tu 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  

Reconstruir la escena trazando los escaneos y 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 bucle y el proceso de optimización

Continúe añadiendo 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 identifique un cierre de bucle. La salida tiene un campo, que indica cuándo se produce la optimización de la gráfica de pose..optimizationInfoIsPerformed

Trace los escaneos y poses siempre que se identifique un cierre de bucle y Verifique visualmente los resultados. Esta gráfica muestra escaneos superpuestos y un gráfico de pose optimizado para el primer cierre de lazo. Se agrega un borde de cierre de bucle como un vínculo 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');

Visualiza el mapa construido y la trayectoria del robot

Trace el mapa final creado después de que todos los escaneos se agreguen al objeto.slamAlg El bucle anterior debería haber añadido todos los escaneos a pesar de sólo trazar el cierre de bucle inicial.for

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 de planta original. Puede ver que el mapa coincide con el plano de planta original después de agregar todos los escaneos y optimizar el gráfico de pose.

Crear mapa de cuadrícula de ocupación

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

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

Visualice el mapa de la cuadrícula de ocupación rellenado 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');