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.

Implemente localización y mapeo simultáneos en línea (SLAM) con escaneos Lidar

Este ejemplo utiliza:

Abrir script en vivo

Este ejemplo demuestra cómo implementar el algoritmo de localización y mapeo simultáneos (SLAM) en escaneos LIDAR obtenidos de un entorno simulado mediante la optimización del gráfico de pose. Este ejemplo requiere Simulink® 3D Animation™ y Navigation Toolbox™.

El objetivo de este ejemplo es construir un mapa del entorno utilizando escaneos LIDAR y recuperar la trayectoria del robot, con el simulador de robot en el bucle.

Los conceptos básicos del algoritmo SLAM se pueden encontrar en el ejemplo Implementar localización y mapeo simultáneos (SLAM) con escaneos Lidar .

Cargar trayectoria del robot desde archivo

La trayectoria del robot son puntos de referencia que se le dan al robot para que se mueva en el entorno simulado. Para este ejemplo, se le proporciona la trayectoria del robot.

load slamRobotTrajectory.mat

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

Cargar y ver el mundo virtual

Este ejemplo utiliza una escena virtual con dos vehículos y cuatro paredes como obstáculos y un robot equipado con un escáner lidar que se muestra en Simulink 3D Animation Viewer. Puede navegar en una escena virtual usando la barra de menú, la barra de herramientas, el panel de navegación, el mouse y el teclado.

Cree y abra el objeto vrworld .

w = vrworld('slamSimulatedWorld.x3d');
open(w)

Crea una figura que muestre la escena virtual.

vrf = vrfigure(w)

vrf = 

	vrfigure object: 1-by-1

	Differential Wheeled Robot with LIDAR Sensor

Inicializar la posición y rotación del robot en el mundo virtual

La escena virtual se representa como la estructura jerárquica de un archivo VRML utilizado por Simulink 3D Animation. La posición y orientación de los objetos secundarios es relativa al objeto principal. El robot vrnode se utiliza para manipular la posición y orientación del robot en la escena virtual.

Para acceder a un nodo VRML, se debe crear un objeto vrnode apropiado. El nodo se identifica por su nombre y el mundo al que pertenece.

Cree un identificador vrnode para el robot en un entorno virtual. 

robotVRNode = vrnode(w,'Robot');

Establezca la posición inicial del robot desde el primer punto de la trayectoria y establezca la rotación inicial en 0 rad alrededor del eje y. 

robotVRNode.children.translation = [trajectory(1,1) 0 trajectory(1,2)];
robotVRNode.children.rotation = [0 1 0 0];

Cree un identificador para el sensor lidar en el robot creando vrnode.

lidarVRNode = vrnode(w,'LIDAR_Sensor');

El lidar simulado utiliza un total de 240 líneas láser y el ángulo entre estas líneas es de 1,5 grados. 

angles  = 180:-1.5:-178.5;
angles = deg2rad(angles)';

Esperando actualizar e inicializar la escena virtual

pause(1)

Crear objeto Lidar Slam

Cree un objeto lidarSLAM y establezca la resolución del mapa y el rango lidar máximo. Este ejemplo utiliza un entorno virtual simulado. El robot de este vrworld tiene un sensor lidar con un alcance de 0 a 10 metros. Establezca el alcance máximo del lidar (8 m) más pequeño que el alcance máximo de escaneo, ya que las lecturas del láser son menos precisas cerca del alcance 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. Estos dos parámetros se utilizan en todo el ejemplo.

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

Los parámetros de cierre del 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. 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 = 200;
slamAlg.LoopClosureSearchRadius = 3;
controlRate = rateControl(10);

Observe el efecto del proceso de optimización y cierre del bucle

Cree un bucle para navegar el robot a través de la escena virtual. La posición del robot se actualiza en el bucle desde los puntos de la trayectoria. Los escaneos se obtienen del robot mientras éste navega por el entorno.

Los cierres de bucle se detectan automáticamente a medida que el robot se mueve. La optimización del gráfico de pose se realiza cada vez que se detecta un cierre de bucle. Esto se puede verificar usando el valor de salida optimizationInfo.IsPerformed de addScan.

Se muestra una instantánea para demostrar los escaneos y las poses cuando se identifica el primer cierre del bucle y verificar los resultados visualmente. Este gráfico muestra escaneos superpuestos y un gráfico de pose optimizado para el primer cierre del bucle.

El mapa final construido se presentará después de que se recopilen y procesen todos los escaneos.

La gráfica se actualiza continuamente a medida que el robot navega por la escena virtual.

firstLoopClosure = false;
scans = cell(length(trajectory),1);

figure
for i=1:length(trajectory)
    % Use translation property to move the robot. 
    robotVRNode.children.translation = [trajectory(i,1) 0 trajectory(i,2)];
    vrdrawnow;
    
    % Read the range readings obtained from lidar sensor of the robot.
    range = lidarVRNode.pickedRange;
    
    % The simulated lidar readings will give -1 values if the objects are
    % out of range. Make all these value to the greater than
    % maxLidarRange.
    range(range==-1) = maxLidarRange+2;

    % Create a lidarScan object from the ranges and angles. 
    scans{i} = lidarScan(range,angles);
    
    [isScanAccepted,loopClosureInfo,optimizationInfo] = addScan(slamAlg,scans{i});
    if isScanAccepted
        % Visualize how scans plot and poses are updated as robot navigates
        % through virtual scene
        show(slamAlg);
        
        % Visualize the first detected loop closure
        % firstLoopClosure flag is used to capture the first loop closure event
        if optimizationInfo.IsPerformed && ~firstLoopClosure
            firstLoopClosure = true;
            show(slamAlg,'Poses','off');
            hold on;
            show(slamAlg.PoseGraph);
            hold off;
            title('First loop closure');
            snapnow
        end
    end

    waitfor(controlRate);
end

Trace el mapa final construido después de agregar todos los escaneos al objeto slamAlg .

show(slamAlg,'Poses','off');
hold on
show(slamAlg.PoseGraph); 
hold off
title({'Final Built Map of the Environment','Trajectory of the Robot'});

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');

Cierra la escena virtual.

close(vrf);
close(w);
delete(w);