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 demuestra cómo implementar el algoritmo de localización y mapeo simultáneos (SLAM) en datos recopilados de sensores lidar 3-D 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 n -por-3 matrices, donde n es el número de puntos 3-D en los datos lidar capturados, y las columnas representan xyz- coordenadas asociadas con cada punto capturado.

outputFolder = fullfile(tempdir,'ParkingLot');

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

pClouds = helperDownloadData(outputFolder,dataURL);

Parámetros para el algoritmo de registro de nube 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 3-D.

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 los inliers al retirar los planos de suelo y techo.

• maxAngularDistance - Desviación máxima del ángulo respecto al 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 reducen mediante muestreo aleatorio con una proporción de muestra especificada por randomSampleRatio.

randomSampleRatio = 0.25;

gridStep especifica los tamaños de cuadrícula de vóxeles utilizados en el algoritmo de registro de END. Se acepta un escaneo solo después de que el robot se mueve una distancia mayor que 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 de bucle.

nScansPerSubmap = 3;
subMapThresh = 50;

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

annularRegionLimits = [-0.75 0.75];

El error cuadrático medio (RMSE) máximo aceptable al estimar la pose 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 de Pose Graph se llama después de insertar optimizationInterval bordes de cierre de bucle 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

Estimación y refinamiento de trayectoria mediante la optimización del gráfico de pose

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 otros 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 pequeño radio 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:

• Cree submapas a partir de nScansPerSubmap escaneos consecutivos.

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

• Acepte las coincidencias si la puntuación de la coincidencia es mayor que TG. 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 restricción de cierre de bucle solo cuando el RMSE es menor que el 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 costesa, 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 occupance 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.

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 occupance 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