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