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

Realice SLAM utilizando nubes de puntos Lidar 3-D

Este ejemplo utiliza:

Abrir script en vivo

Este ejemplo demuestra cómo implementar el algoritmo de localización y mapeo simultáneos (SLAM) en datos recopilados por sensores LiDAR 3D utilizando algoritmos de procesamiento de nubes de puntos y optimización de gráficos de pose. El objetivo de este ejemplo es estimar la trayectoria del robot y crear un mapa de ocupación tridimensional del entorno a partir de las nubes de puntos lidar tridimensionales y la trayectoria estimada.

El algoritmo SLAM demostrado estima una trayectoria utilizando un algoritmo de registro de nube de puntos basado en Transformación de distribución normal (NDT) y reduce la deriva utilizando la optimización del gráfico de pose SE3 mediante un solver de región de confianza cada vez que un robot vuelve a visitar un lugar.

Cargar datos y configurar parámetros ajustables

Cargue los datos LIDAR tridimensionales recopilados por un robot Clearpath™ Husky en un estacionamiento. Los datos LiDAR contienen un arreglo de celdas de matrices de n por 3, donde n es el número de puntos 3D en los datos LiDAR capturados y las columnas representan coordenadas xyz asociadas con cada punto capturado.

outputFolder = fullfile(tempdir,'ParkingLot');

dataURL = ['https://ssd.mathworks.com/supportfiles/lidar/data/' ...
            'NatickParkingLotLidarData.tar'];

pClouds = helperDownloadData(outputFolder,dataURL);
Downloading Lidar data (472 MB)...

Parámetros del algoritmo de registro de nubes de puntos

Especifique los parámetros para estimar la trayectoria utilizando el algoritmo de registro de nube de puntos. maxLidarRange especifica el alcance máximo del escáner láser 3D.

maxLidarRange = 20;

Los datos de la nube de puntos capturados en un entorno interior contienen puntos que se encuentran en los planos del suelo y del techo, lo que confunde los algoritmos de registro de la nube de puntos. Algunos puntos se eliminan de la nube de puntos con estos parámetros:

  • referenceVector - Normal al plano de tierra.

  • maxDistance - Distancia máxima para elementos internos al retirar los planos de suelo y techo.

  • maxAngularDistance - Desviación máxima del ángulo respecto del vector de referencia al ajustar los planos de suelo y techo.

referenceVector = [0 0 1];
maxDistance = 0.5;
maxAngularDistance = 15;

Para mejorar la eficiencia y precisión del algoritmo de registro, las nubes de puntos se muestrean de forma descendente mediante un muestreo aleatorio con una relación de muestra especificada por randomSampleRatio.

randomSampleRatio = 0.25;

gridStep especifica los tamaños de la cuadrícula de vóxeles utilizados en el algoritmo de registro NDT. Un escaneo se acepta solo después de que el robot se mueva una distancia mayor a distanceMovedThreshold.

gridStep = 2.5;
distanceMovedThreshold = 0.3;

Ajuste estos parámetros para su robot y entorno específicos.

Parámetros para el algoritmo de estimación de cierre de bucle

Especifique los parámetros para el algoritmo de estimación del cierre del bucle. Los cierres de bucle solo se buscan dentro de un radio alrededor de la ubicación actual del robot especificada por loopClosureSearchRadius.

loopClosureSearchRadius = 3;

El algoritmo de cierre de bucle se basa en submapas 2-D y coincidencia de escaneo. Se crea un submapa después de cada nScansPerSubmap (número de escaneos por submapa) escaneos aceptados. El algoritmo de cierre de bucle ignora los escaneos subMapThresh más recientes mientras busca candidatos para el bucle. 

nScansPerSubmap = 3;
subMapThresh = 50;

Se extrae de las nubes de puntos una región anular con límites z especificados por annularRegionLimits. Los puntos fuera de estos límites en el piso y el techo se eliminan después de que los algoritmos de ajuste del plano de la nube de puntos identifican la región de interés.

annularRegionLimits = [-0.75 0.75];

El error cuadrático medio (RMSE) máximo aceptable para estimar la posición relativa entre candidatos de bucle se especifica mediante rmseThreshold. Elija un valor más bajo para estimar los bordes de cierre de bucle precisos, lo que tiene un gran impacto en la optimización del gráfico de pose.

rmseThreshold = 0.26;

El umbral sobre la puntuación de coincidencia de escaneo para aceptar un cierre de bucle se especifica mediante loopClosureThreshold. La optimización del gráfico de pose se llama después de insertar bordes de cierre de bucle optimizationInterval en el gráfico de pose.

loopClosureThreshold = 150;
optimizationInterval = 2;

Inicializar variables

Configure un gráfico de pose, un mapa de ocupación y las variables necesarias.

% 3D Posegraph object for storing estimated relative poses
pGraph = poseGraph3D;
% Default serialized upper-right triangle of 6-by-6 Information Matrix
infoMat = [1,0,0,0,0,0,1,0,0,0,0,1,0,0,0,1,0,0,1,0,1];
% Number of loop closure edges added since last pose graph optimization and map refinement
numLoopClosuresSinceLastOptimization = 0; 
% True after pose graph optimization until the next scan
mapUpdated = false;
% Equals to 1 if the scan is accepted
scanAccepted = 0;

% 3D Occupancy grid object for creating and visualizing 3D map
mapResolution = 8; % cells per meter
omap = occupancyMap3D(mapResolution);

Preasigne variables para las nubes de puntos, escaneos LIDAR y submapas procesados. Cree un conjunto reducido de nubes de puntos para visualizar rápidamente el mapa.

pcProcessed = cell(1,length(pClouds));
lidarScans2d = cell(1,length(pClouds)); 
submaps = cell(1,length(pClouds)/nScansPerSubmap);

pcsToView = cell(1,length(pClouds));

Cree variables para fines de visualización.

% Set to 1 to visualize created map and posegraph during build process
viewMap = 1; 
% Set to 1 to visualize processed point clouds during build process
viewPC = 0;

Establezca una semilla aleatoria para garantizar un muestreo aleatorio consistente.

rng(0);

Inicialice las ventanas de figuras si lo desea.

% If you want to view the point clouds while processing them sequentially
if viewPC==1
    pplayer = pcplayer([-50 50],[-50 50],[-10 10],'MarkerSize',10);
end

% If you want to view the created map and posegraph during build process
if viewMap==1
    ax = newplot; % Figure axis handle
    view(20,50);
    grid on;
end

Figure contains an axes object. The axes object is empty.

Estimación y refinamiento de trayectorias mediante optimización de gráficos de poses

La trayectoria del robot es una colección de poses del robot (ubicación y orientación en el espacio tridimensional). Se estima una pose de robot en cada instancia de adquisición de escaneo lidar 3-D utilizando el algoritmo SLAM lidar 3-D. El algoritmo LIDAR SLAM 3-D tiene los siguientes pasos:

  • Filtrado de nubes de puntos

  • Reducción de resolución de nube de puntos

  • Registro de nube de puntos

  • Consulta de cierre de bucle

  • Optimización del gráfico de pose

Procese iterativamente las nubes de puntos para estimar la trayectoria.

count = 0; % Counter to track number of scans added
disp('Estimating robot trajectory...');
Estimating robot trajectory...

for i=1:length(pClouds)
    % Read point clouds in sequence
    pc = pClouds{i};

Filtrado de nubes de puntos

El filtrado de la nube de puntos se realiza para extraer la región de interés del escaneo adquirido. En este ejemplo, la región de interés es la región anular sin suelo ni techo.

Elimine los puntos no válidos fuera del rango máximo y los puntos innecesarios detrás del robot correspondiente al conductor humano.

    ind = (-maxLidarRange < pc(:,1) & pc(:,1) < maxLidarRange ...
        & -maxLidarRange  < pc(:,2) & pc(:,2) < maxLidarRange ...
        & (abs(pc(:,2))>abs(0.5*pc(:,1)) | pc(:,1)>0));
    
    pcl = pointCloud(pc(ind,:));

Eliminar puntos en el plano de tierra.

    [~, ~, outliers] = ...
        pcfitplane(pcl, maxDistance,referenceVector,maxAngularDistance);
    pcl_wogrd = select(pcl,outliers,'OutputSize','full');

Elimine puntos en el plano del techo.

    [~, ~, outliers] = ...
        pcfitplane(pcl_wogrd,0.2,referenceVector,maxAngularDistance);
    pcl_wogrd = select(pcl_wogrd,outliers,'OutputSize','full');

Seleccione puntos en la región anular.

    ind = (pcl_wogrd.Location(:,3)<annularRegionLimits(2))&(pcl_wogrd.Location(:,3)>annularRegionLimits(1));
    pcl_wogrd = select(pcl_wogrd,ind,'OutputSize','full');

Reducción de resolución de nube de puntos

La reducción de resolución de la nube de puntos mejora la velocidad y la precisión del algoritmo de registro de la nube de puntos. El muestreo descendente debe ajustarse a necesidades específicas. El algoritmo de muestreo aleatorio se elige empíricamente entre las variantes de muestreo siguientes para el escenario actual.

    pcl_wogrd_sampled = pcdownsample(pcl_wogrd,'random',randomSampleRatio);
    
    if viewPC==1
        % Visualize down sampled point cloud
        view(pplayer,pcl_wogrd_sampled);
        pause(0.001)
    end

Registro de nube de puntos

El registro de la nube de puntos estima la pose relativa (rotación y traslación) entre el escaneo actual y el escaneo anterior. El primer escaneo siempre se acepta (se procesa y almacena), pero los demás escaneos solo se aceptan después de traducir más que el umbral especificado. poseGraph3D se utiliza para almacenar las poses relativas aceptadas estimadas (trayectoria). 

    if count == 0
        % First can
        tform = [];
        scanAccepted = 1;
    else
        if count == 1
            tform = pcregisterndt(pcl_wogrd_sampled,prevPc,gridStep);
        else
            tform = pcregisterndt(pcl_wogrd_sampled,prevPc,gridStep,...
                'InitialTransform',prevTform);
        end
        
        relPose = [tform2trvec(tform.T') tform2quat(tform.T')];
        
        if sqrt(norm(relPose(1:3))) > distanceMovedThreshold
            addRelativePose(pGraph,relPose);
            scanAccepted = 1;
        else
            scanAccepted = 0;
        end
    end

Consulta de cierre de bucle

La consulta de cierre de bucle determina si la ubicación actual del robot ha sido visitada previamente o no. La búsqueda se realiza haciendo coincidir el escaneo actual con los escaneos anteriores dentro de un radio pequeño alrededor de la ubicación actual del robot especificada por loopClosureSearchRadius. La búsqueda dentro del radio pequeño es suficiente debido a la baja deriva en la odometría lidar, ya que la búsqueda en todos los escaneos anteriores lleva mucho tiempo. La consulta de cierre de bucle consta de los siguientes pasos:

  • Crear submapas a partir de escaneos consecutivos nScansPerSubmap.

  • Haga coincidir el escaneo actual con los submapas dentro de loopClosureSearchRadius.

  • Acepte las coincidencias si el puntaje de la coincidencia es mayor que loopClosureThreshold. Todos los escaneos que representan el submapa aceptado se consideran candidatos probables a bucle.

  • Calcule la pose relativa entre los posibles candidatos de bucle y el escaneo actual. Una pose relativa se acepta como una restricción de cierre de bucle solo cuando el RMSE es menor que rmseThreshold. 

    if scanAccepted == 1
        count = count + 1;
        
        pcProcessed{count} = pcl_wogrd_sampled;
        
        lidarScans2d{count} = exampleHelperCreate2DScan(pcl_wogrd_sampled);
        
        % Submaps are created for faster loop closure query. 
        if rem(count,nScansPerSubmap)==0
            submaps{count/nScansPerSubmap} = exampleHelperCreateSubmap(lidarScans2d,...
                pGraph,count,nScansPerSubmap,maxLidarRange);
        end
        
        % loopSubmapIds contains matching submap ids if any otherwise empty.   
        if (floor(count/nScansPerSubmap)>subMapThresh)
            [loopSubmapIds,~] = exampleHelperEstimateLoopCandidates(pGraph,...
                count,submaps,lidarScans2d{count},nScansPerSubmap,...
                loopClosureSearchRadius,loopClosureThreshold,subMapThresh);
            
            if ~isempty(loopSubmapIds)
                rmseMin = inf;
                
                % Estimate best match to the current scan
                for k = 1:length(loopSubmapIds)
                    % For every scan within the submap
                    for j = 1:nScansPerSubmap
                        probableLoopCandidate = ...
                            loopSubmapIds(k)*nScansPerSubmap - j + 1;
                        [loopTform,~,rmse] = pcregisterndt(pcl_wogrd_sampled,...
                            pcProcessed{probableLoopCandidate},gridStep);
                        % Update best Loop Closure Candidate
                        if rmse < rmseMin
                            loopCandidate = probableLoopCandidate;
                            rmseMin = rmse;
                        end
                        if rmseMin < rmseThreshold
                            break;
                        end
                    end
                end
                
                % Check if loop candidate is valid
                if rmseMin < rmseThreshold
                    % loop closure constraint
                    relPose = [tform2trvec(loopTform.T') tform2quat(loopTform.T')];
                    
                    addRelativePose(pGraph,relPose,infoMat,...
                        loopCandidate,count);
                    numLoopClosuresSinceLastOptimization = numLoopClosuresSinceLastOptimization + 1;
                end
                     
            end
        end

Optimización del gráfico de pose

La optimización del gráfico de pose se ejecuta después de que se acepta una cantidad suficiente de bordes de bucle para reducir la deriva en la estimación de la trayectoria. Después de cada optimización de cierre de bucle, el radio de búsqueda de cierre de bucle se reduce debido al hecho de que la incertidumbre en la estimación de la pose se reduce después de la optimización. 

        if (numLoopClosuresSinceLastOptimization == optimizationInterval)||...
                ((numLoopClosuresSinceLastOptimization>0)&&(i==length(pClouds)))
            if loopClosureSearchRadius ~=1
                disp('Doing Pose Graph Optimization to reduce drift.');
            end
            % pose graph optimization
            pGraph = optimizePoseGraph(pGraph);
            loopClosureSearchRadius = 1;
            if viewMap == 1
                position = pGraph.nodes;
                % Rebuild map after pose graph optimization
                omap = occupancyMap3D(mapResolution);
                for n = 1:(pGraph.NumNodes-1)
                    insertPointCloud(omap,position(n,:),pcsToView{n}.removeInvalidPoints,maxLidarRange);
                end
                mapUpdated = true;
                ax = newplot;
                grid on;
            end
            numLoopClosuresSinceLastOptimization = 0;
            % Reduce the frequency of optimization after optimizing
            % the trajectory
            optimizationInterval = optimizationInterval*7;
        end

Visualice el mapa y pose el gráfico durante el proceso de construcción. Esta visualización es costosa, así que habilítela solo cuando sea necesario configurando viewMap en 1. Si la visualización está habilitada, el gráfico se actualiza cada 15 escaneos agregados.

        pcToView = pcdownsample(pcl_wogrd_sampled, 'random', 0.5);
        pcsToView{count} = pcToView;
        
        if viewMap==1
            % Insert point cloud to the occupancy map in the right position
            position = pGraph.nodes(count);
            insertPointCloud(omap,position,pcToView.removeInvalidPoints,maxLidarRange);
            
            if (rem(count-1,15)==0)||mapUpdated
                exampleHelperVisualizeMapAndPoseGraph(omap, pGraph, ax);
            end
            mapUpdated = false;
        else
            % Give feedback to know that example is running
            if (rem(count-1,15)==0)
                fprintf('.');
            end
        end

Actualice la estimación de pose relativa anterior y la nube de puntos.

        prevPc = pcl_wogrd_sampled;
        prevTform = tform;
    end
end
Doing Pose Graph Optimization to reduce drift.

Figure contains an axes object. The axes object contains 4 objects of type patch, line. One or more of the lines displays its values using only markers

Cree y visualice un mapa de ocupación en 3D

Las nubes de puntos se insertan en occupancyMap3D utilizando las poses globales estimadas. Después de recorrer todos los nodos, se muestra el mapa completo y la trayectoria estimada del vehículo.

if (viewMap ~= 1)||(numLoopClosuresSinceLastOptimization>0)
    nodesPositions = nodes(pGraph);
    % Create 3D Occupancy grid
    omapToView = occupancyMap3D(mapResolution);
    
    for i = 1:(size(nodesPositions,1)-1)
        pc = pcsToView{i};
        position = nodesPositions(i,:);
        
        % Insert point cloud to the occupancy map in the right position
        insertPointCloud(omapToView,position,pc.removeInvalidPoints,maxLidarRange);
    end
    
    figure;
    axisFinal = newplot;
    exampleHelperVisualizeMapAndPoseGraph(omapToView, pGraph, axisFinal);
end

Funciones de utilidad

function pClouds = helperDownloadData(outputFolder,lidarURL)
% Download the data set from the given URL to the output folder.
    lidarDataTarFile = fullfile(outputFolder,'NatickParkingLotLidarData.tar');    
    if ~exist(lidarDataTarFile,'file')
        mkdir(outputFolder);        
        disp('Downloading Lidar data (472 MB)...');
        websave(lidarDataTarFile,lidarURL);
        untar(lidarDataTarFile,outputFolder);
    end    
    % Extract the file.
    if (~exist(fullfile(outputFolder,'NatickParkingLotLidarData'),'dir'))
        untar(lidarDataTarFile,outputFolder);
    end

    pClouds = load(fullfile(outputFolder,'NatickParkingLotLidarData', 'NatickParkingLotLidarData.mat')).pClouds(1:5:end);
end

Consulte también

| (Computer Vision Toolbox) | (Computer Vision Toolbox)

Temas