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 la localización y mapeo simultáneos en línea (SLAM) con escaneos LiDAR

En este ejemplo se muestra cómo implementar el algoritmo de localización y asignación simultánea (SLAM) en escaneos LiDAR obtenidos del entorno simulado mediante la optimización de gráficos de pose.

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

Los fundamentos del algoritmo de SLAM se pueden encontrar en el ejemplo.Implemente localización y mapeo simultáneos (SLAM) con exploraciones LiDAR Este ejemplo requiere Simulink®™ de animación 3D y Robotics System Toolbox™.

Cargue la trayectoria del robot desde archivo

La trayectoria del robot es un waypoint dado al robot para moverse en el entorno simulado. Para este ejemplo, la trayectoria del robot se proporciona para usted.

load slamRobotTrajectory.mat 

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

Cargue y vea el mundo virtual

Este ejemplo utiliza una escena virtual con dos vehículos y cuatro muros como obstáculos y un robot equipado con un escáner LiDAR que se muestra en el visor de animación 3D de Simulink. Puede navegar en una escena virtual mediante la barra de menús, la barra de herramientas, el panel de navegación, el mouse y el teclado. Las características clave del visor se ilustran en el VR example.

Cree y abra el objeto.vrworld

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

Cree 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 animación 3D. La posición y la orientación de los objetos secundarios es relativa al objeto primario. El robot se utiliza para manipular la posición y la orientación del robot en la escena virtual.vrnode

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

Crear manija para el robot en el entorno virtual.vrnode

robotVRNode = vrnode(w,'Robot'); 

Establece la posición inicial del robot desde el primer punto de la trayectoria y establece la rotación inicial en 0 Rad sobre el eje y.

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

Cree la manija para el sensor LiDAR en el robot creando el vrnode.

lidarVRNode = vrnode(w,'LIDAR_Sensor'); 

El LiDAR simulado está utilizando el total de líneas láser 240 y el ángulo entre estas líneas es de 1,5 grados.

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

A la espera de actualizar e inicializar la escena virtual

pause(1) 

Crear objeto LiDAR Slam

Cree un objeto y defina la resolución del mapa y el rango LiDAR máximo.robotics.LidarSLAM En este ejemplo se utiliza un entorno virtual simulado. El robot en esto tiene un sensor LiDAR con rango de 0 a 10 metros.vrworld Establezca el rango LiDAR máximo (8m) más pequeño que el rango de escaneo máximo, 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. Estos dos parámetros se utilizan en todo el ejemplo.

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

Los 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 de bucle. Tenga en cuenta que un partido de puntuación alta puede seguir siendo un mal partido. Por ejemplo, los escaneos recopilados 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 = robotics.Rate(10); 

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

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

Los cierres de bucle se detectan automáticamente a medida que el robot se mueve. La optimización de la gráfica de pose se realiza siempre que se detecte un cierre de bucle. Esto se puede comprobar utilizando el valor de salida from.optimizationInfo.IsPerformedaddScan

Se muestra una instantánea para demostrar los escaneos y poses cuando se identifica el primer cierre de bucle y comprobar visualmente los resultados. Esta gráfica muestra escaneos superpuestos y un gráfico de pose optimizado para el primer cierre de lazo.

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

La trama se actualiza continuamente a medida que el robot navega a través de 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 creado después de que todos los escaneos se agreguen 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'}); 

Crear mapa de cuadrícula de ocupación

Los escaneos y poses optimizados se pueden utilizar para generar un que represente 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'); 

Cierre la escena virtual.

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