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

Mapee el área interior usando Lidar SLAM y gráfico de factores

Este ejemplo muestra cómo utilizar los objetos lidarscanmap (Lidar Toolbox) y factorGraph para implementar el algoritmo lidar de localización y mapeo simultáneos (SLAM) en una serie recopilada de escaneos lidar de un área interior. . El objetivo de este ejemplo es construir un mapa del área interior utilizando escaneos LIDAR y recuperar la trayectoria del robot.

Para construir el mapa del área, el algoritmo SLAM procesa incrementalmente los escaneos lidar y construye un mapa de escaneo lidar, y un gráfico de factores vincula estos escaneos. El robot reconoce lugares visitados previamente mediante la comparación de escaneos y los utiliza para establecer cierres de bucle a lo largo de su trayectoria en movimiento. El gráfico de factores utiliza la información de cierre del bucle para optimizar la trayectoria del robot y actualizar el mapa de escaneo lidar.

Cargar datos de escaneo láser y odometría desde un archivo

Cargue un conjunto de datos reducidos que consta de escaneos láser y datos de pose recopilados por un robot Jackal™ de Clearpath Robotics™ en un entorno interior. Los datos incluyen una variable, messages, que contiene escaneos LIDAR del tema ROS /scan y poses del tema /odom/filtered cuyas marcas de tiempo son más cercano a los de los escaneos de /scan. El conjunto de datos original incluye más de 2000 mensajes, cuyo procesamiento puede llevar mucho tiempo. Por lo tanto, para mejorar el rendimiento, se ha reducido la muestra del conjunto de datos sin procesar para incluir solo 111 mensajes, lo que sigue siendo suficiente para proporcionar un buen resultado. Después de la reducción de resolución, el desplazamiento promedio entre cada par de escaneos consecutivos es de alrededor de 0,45 metros.

Cargue el archivo IndoorLidarScanAndOdometryData.mat , que contiene la variable messages , en el espacio de trabajo.

load("IndoorLidarScanAndOdometryData.mat")

Este plano de planta y la trayectoria aproximada del robot tienen fines ilustrativos y muestran el área interior que se va a mapear y la trayectoria aproximada del robot.

Ejecute el algoritmo Lidar SLAM

Para ejecutar el algoritmo Lidar SLAM, debe construir el mapa de escaneo lidar utilizando los parámetros del sensor lidar en el robot. Luego, cree y utilice un gráfico de factores para relacionar poses relativas y optimizar el gráfico.

Construir mapa de escaneo Lidar

Cree un objeto lidarscanmap 2-D con un alcance LIDAR máximo de 8 metros y una resolución de mapa de cuadrícula de 20 celdas por metro, lo que da como resultado una precisión de 5 centímetros. El robot está equipado con un escáner láser 2-D SICK™ TiM-511 con un alcance máximo de 10 metros, pero las lecturas del láser son menos precisas cuando detecta objetos cerca del alcance máximo, por lo que limitar el alcance máximo a 8 metros da como resultado resultados más precisos. lecturas.

MaxLidarRange = 8;
MapResolution = 20;
scanMapObj = lidarscanmap(MapResolution,MaxLidarRange);

Agregue el primer escaneo al objeto lidarscanmap .

addScan(scanMapObj,messages{1}.scan);

Recopile datos de escaneo para usarlos en la visualización.

scans = cell(length(messages),1);
scans{1} = messages{1}.scan;
firstTimeLCDetected = false;

Construir gráfico de factores

A continuación, debe agregar escaneos al mapa de escaneo LIDAR. Cuando agrega exitosamente un escaneo, el objeto calcula automáticamente la pose relativa. Sin embargo, debido al ruido del sensor y a los giros bruscos, cada medición tiene una cantidad de error, llamada deriva. Este error se acumula a medida que amplías la trayectoria. Para ayudar a mitigar esta desviación, utilice un objeto factorGraph para almacenar las poses relativas.

Cree los objetos de opciones del gráfico de factores y del solver de gráficos de factores.

fg = factorGraph;
opts = factorGraphSolverOptions;

Para optimizar la corrección de la deriva, el gráfico de factores requiere algo más que poses relativas. El gráfico de factores también debe tener algunas poses conocidas exactas para fijar las poses relativas en el espacio y reconocer cuándo el robot regresa a una ubicación visitada anteriormente. Un cierre de bucle es cuando el algoritmo detecta que el robot ha regresado a una ubicación anterior. Al detectar el cierre del bucle, puede agregar una restricción adicional entre la última pose y una pose anterior. La función de objeto detectLoopClosure (Lidar Toolbox) de lidarscanmap busca un escaneo anterior que coincida con el escaneo más reciente del objeto lidarscanmap para verificar si el actual pose crea un cierre de bucle en el mapa. Tenga en cuenta que, si la frecuencia de muestreo del sensor lidar es lo suficientemente alta, la pose actual puede estar lo suficientemente cerca en el tiempo de una pose reciente como para que la función detectLoopClosure detecte falsamente la pose actual como un cierre de bucle. . Para evitar esto, excluya varios de los análisis más recientes de la detección de cierre de bucle.

Establezca el número de escaneos recientes para excluir de la detección de cierre de bucle en 30.

numExcludeViews = 30;

Para aumentar la precisión del cierre de bucle detectado, debe ajustar los valores del umbral de coincidencia de cierre de bucle y del radio de búsqueda de cierre de bucle. El umbral de coincidencia de cierre de bucle es la puntuación de correlación mínima que debe tener un escaneo para considerarse una coincidencia para el cierre de bucle, y el radio de búsqueda de cierre de bucle es la distancia máxima que detectLoopClosure puede buscar desde el escaneo actual. para encontrar un cierre de bucle. Este ejemplo establece estos valores empíricamente, pero el uso de un umbral de coincidencia de cierre de bucle más alto generalmente ayuda a rechazar falsos positivos en el proceso de identificación de cierre de bucle.

Establezca el umbral de coincidencia de cierre de bucle en 185 y el radio de búsqueda de cierre de bucle en 8 metros.

lcMatchThreshold = 185;
lcSearchRadius = 8;

Para cada uno de los escaneos:

1. Utilice la función addScan (Lidar Toolbox) para agregar los escaneos al objeto lidarscanmap . La función addScan rechaza los escaneos si están demasiado cerca para escaneos consecutivos para garantizar que los datos LIDAR sean precisos.

2. Si el mapa de escaneo lidar acepta el nuevo escaneo, cree un factor de pose relativo SE(2) para conectar la pose actual y la pose anterior. El factor de pose relativa es la pose relativa 2-D entre la pose actual y la pose anterior. Luego, agregue el factor al gráfico de factores para crear un nuevo nodo para la pose actual y conéctelo a la pose anterior en el gráfico de factores.

3. Actualice el estado del nodo de pose actual con una suposición de pose a partir de los datos de odometría en ese paso de tiempo.

4. Si el escaneo no es uno de los escaneos recientes iniciales excluidos, use la función detectLoopClosure para verificar si el escaneo actual coincide con el cierre de bucle. Si el escaneo coincide, entonces detectLoopClosure devuelve la pose y el ID de pose del escaneo coincidente.

5. Si hay un cierre de bucle, cree otro factor de pose relativo entre la pose actual y la pose que coincide con el cierre de bucle. Luego agrega ese factor a la gráfica de factores.

6. Corrija el primer nodo en el gráfico de factores y luego optimice el gráfico.

7. Obtenga todos los ID de pose en el gráfico de factores y sus estados posteriores a la optimización.

8. Para el primer cierre de bucle, utilice la función auxiliar exampleHelperPlotLidarScanMap para visualizar el primer cierre de bucle detectado como una línea verde antes y después de la optimización.

9. Utilice la función de objeto updateScanPoses (Lidar Toolbox) para actualizar incrementalmente los escaneos en el mapa de escaneo lidar con los estados optimizados para obtener una mejor estimación de pose para poses relativas y cierres de bucle.

for i = 2:length(messages)
    % Try to add scan to lidar scan map
    isScanAccepted = addScan(scanMapObj,messages{i}.scan);
    scans{i} = messages{i}.scan;

    % If map accepts scan
    if isScanAccepted
        % Set IDs for previous node and current node
        prevPoseID = scanMapObj.NumScans-1;
        currPoseID = scanMapObj.NumScans;

        % Get relative 2-D measurement between current and previous pose
        relPoseMeasure = scanMapObj.ConnectionAttributes.RelativePose(end,:);

        % Create 2-D relative pose factor and add it to factor graph
        poseFactor = factorTwoPoseSE2([prevPoseID currPoseID],Measurement=relPoseMeasure);
        addFactor(fg,poseFactor);

        % Update current pose node with initial pose guess
        nodeState(fg,currPoseID,messages{i}.Pose);

        % If scan is not one of initial excluded scans
        if i > numExcludeViews

            % Detect any new loop closure in lidar scan map
            [lcPose,lcPoseID] = detectLoopClosure(scanMapObj, ...
                                                  MatchThreshold=lcMatchThreshold, ...
                                                  SearchRadius=lcSearchRadius, ...
                                                  NumExcludeViews=numExcludeViews);
            % If there is a loop closure
            if ~isempty(lcPose)
                % Create relative pose factor between current pose
                % and previous pose that is a loop closure match
                lcFactor = factorTwoPoseSE2([lcPoseID currPoseID],Measurement=lcPose);
                addFactor(fg,lcFactor);

                % Fix the initial node, and optimize the factor graph
                fixNode(fg,1); 
                optimize(fg,opts);

                % Get all pose node IDs in the factor graph, and use those IDs to extract
                % the optimized poses of the corresponding pose nodes
                poseIDs = nodeIDs(fg,NodeType="POSE_SE2");
                optimizedPoseStates = nodeState(fg,poseIDs);

                % Visualize the first loop closure before and after factor graph optimization
                if ~firstTimeLCDetected
                    exampleHelperPlotLidarScanMap(lcPoseID,scanMapObj,optimizedPoseStates);
                    firstTimeLCDetected = true;
                end

                % Update the lidar scan map with optimized pose states
                updateScanPoses(scanMapObj,optimizedPoseStates);
            end
        end
    end
end

En el gráfico del mapa de escaneo antes de la optimización, observe que la deriva ha provocado dos escaneos desplazados de la misma pared cerca de la parte inferior del mapa. Después de optimizar el gráfico de factores, los dos escaneos de la pared están más cerca porque los cierres del bucle han reconciliado las paredes detectadas.

Visualizar Mapa construido y trayectoria del robot

Traza el mapa final construido después de haber agregado todos los escaneos al objeto lidarscanmap con poses optimizadas para gráficos de factores.

figure
show(scanMapObj);
title("Final Built Map of Environment with Robot Trajectory")

Esta imagen muestra los escaneos y la trayectoria del robot superpuestos en el plano original.

Tenga en cuenta que el mapa de escaneo coincide estrechamente con el plano de planta original una vez que haya agregado todos los escaneos y haya optimizado el gráfico de factores. Actualmente, este flujo de trabajo utiliza los objetos factorTwoPoseSE2 para la optimización del gráfico de factores siempre que detectLoopClosure detecta un cierre de bucle para reducir la deriva, pero el gráfico de factores admite el uso de otros objetos de factores para refina las poses si tienes IMU u otros datos de sensores.

Construir mapa de cuadrícula de ocupación

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

map = buildMap(scans,scanMapObj.ScanAttributes.AbsolutePose,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(scanMapObj,ShowMap=false);
hold off
title("Occupancy Grid Map Built Using Lidar SLAM")

Consulte también

factorGraph | lidarscanmap (Lidar Toolbox)

Temas relacionados

• SLAM de punto de referencia usando marcadores AprilTag
• Realice SLAM utilizando nubes de puntos Lidar 3-D