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

Estimar la pose del robot con escaneo coincidente

Este ejemplo demuestra cómo hacer coincidir dos escaneos láser utilizando el algoritmo de transformación de distribuciones normales (NDT) [1]. El objetivo de la comparación de escaneos es encontrar la pose relativa (o transformación) entre las dos posiciones del robot donde se tomaron los escaneos. Los escaneos se pueden alinear según las formas de sus características superpuestas.

Para estimar esta pose, NDT subdivide el escaneo láser en celdas 2D y a cada celda se le asigna una distribución normal correspondiente. La distribución representa la probabilidad de medir un punto en esa celda. Una vez calculada la densidad de probabilidad, un método de optimización encuentra la pose relativa entre el escaneo láser actual y el escaneo láser de referencia. Para acelerar la convergencia del método, se puede proporcionar una suposición inicial de la pose. Normalmente, se utiliza la odometría del robot para proporcionar la estimación inicial.

Si aplica la coincidencia de escaneo a una secuencia de escaneos, puede usarlo para recuperar un mapa aproximado del entorno que atraviesa el robot. La coincidencia de escaneos también juega un papel crucial en otras aplicaciones, como el seguimiento de posición y la localización y mapeo simultáneos (SLAM).

Cargar datos de escaneo láser desde un archivo

load lidarScans.mat

Los datos del escaneo láser fueron recopilados por un robot móvil en un ambiente interior. En la siguiente imagen se muestra un plano aproximado del área, junto con la trayectoria del robot a través del espacio.

Trazar dos escaneos láser

Elija dos escaneos láser para escanear coincidencias desde lidarScans. Deben compartir características comunes al estar muy juntos en la secuencia.

referenceScan = lidarScans(180);
currentScan = lidarScans(202);

Muestre los dos escaneos. Observe que hay compensaciones de traslación y rotación, pero algunas características aún coinciden.

currScanCart = currentScan.Cartesian;
refScanCart = referenceScan.Cartesian;
figure
plot(refScanCart(:,1),refScanCart(:,2),'k.');
hold on
plot(currScanCart(:,1),currScanCart(:,2),'r.');
legend('Reference laser scan','Current laser scan','Location','NorthWest');

Ejecute el algoritmo de coincidencia de escaneo y muestre el escaneo transformado

Pase estos dos escaneos a la función de coincidencia de escaneos. matchScans calcula la pose relativa del escaneo actual con respecto al escaneo de referencia.

transform = matchScans(currentScan,referenceScan)

transform = 1×3

    0.5348   -0.0065   -0.0336

Para verificar visualmente que la pose relativa se calculó correctamente, transforme el escaneo actual por la pose calculada usando transformScan. Este escaneo láser transformado se puede utilizar para visualizar el resultado.

transScan = transformScan(currentScan,transform);

Muestre el escaneo de referencia junto al escaneo láser actual transformado. Si la coincidencia del escaneo fue exitosa, los dos escaneos deberían estar bien alineados.

figure
plot(refScanCart(:,1),refScanCart(:,2),'k.');
hold on
transScanCart = transScan.Cartesian;
plot(transScanCart(:,1),transScanCart(:,2),'r.');
legend('Reference laser scan','Transformed current laser scan','Location','NorthWest');

Cree un mapa de cuadrícula de ocupación utilizando la coincidencia de escaneo iterativo

Si aplica la coincidencia de escaneo a una secuencia de escaneos, puede usarlo para recuperar un mapa aproximado del entorno. Utilice la clase occupancyMap para construir un mapa de cuadrícula de ocupación probabilística del entorno.

Cree un objeto de cuadrícula de ocupación para un área de 15 metros por 15 metros. Establezca el origen del mapa en [-7,5 -7,5].

map = occupancyMap(15,15,20);
map.GridLocationInWorld = [-7.5 -7.5]

map = 
  occupancyMap with properties:

   mapLayer Properties
        OccupiedThreshold: 0.6500
            FreeThreshold: 0.2000
    ProbabilitySaturation: [0.0010 0.9990]
                LayerName: 'probabilityLayer'
                 DataType: 'double'
             DefaultValue: 0.5000
      GridLocationInWorld: [-7.5000 -7.5000]
        GridOriginInLocal: [0 0]
       LocalOriginInWorld: [-7.5000 -7.5000]
               Resolution: 20
                 GridSize: [300 300]
             XLocalLimits: [0 15]
             YLocalLimits: [0 15]
             XWorldLimits: [-7.5000 7.5000]
             YWorldLimits: [-7.5000 7.5000]

Asigne previamente un arreglo para capturar el movimiento absoluto del robot. Inicialice la primera pose como [0 0 0]. Todas las demás poses son relativas al primer escaneo medido.

numScans = numel(lidarScans);
initialPose = [0 0 0];
poseList = zeros(numScans,3);
poseList(1,:) = initialPose;
transform = initialPose;

Cree un bucle para procesar los escaneos y mapear el área. Los escaneos láser se procesan por pares. Defina el primer escaneo como escaneo de referencia y el segundo escaneo como escaneo actual. Luego, los dos escaneos se pasan al algoritmo de coincidencia de escaneos y se calcula la pose relativa entre los dos escaneos. La función exampleHelperComposeTransform se utiliza para calcular la pose absoluta acumulada del robot. Los datos de escaneo junto con la pose absoluta del robot se pueden pasar a la función insertRay de la cuadrícula de ocupación.

% Loop through all the scans and calculate the relative poses between them
for idx = 2:numScans
    % Process the data in pairs.
    referenceScan = lidarScans(idx-1);
    currentScan = lidarScans(idx);
    
    % Run scan matching. Note that the scan angles stay the same and do 
    % not have to be recomputed. To increase accuracy, set the maximum 
    % number of iterations to 500. Use the transform from the last
    % iteration as the initial estimate.
    [transform,stats] = matchScans(currentScan,referenceScan, ...
        'MaxIterations',500,'InitialPose',transform);
    
    % The |Score| in the statistics structure is a good indication of the
    % quality of the scan match. 
    if stats.Score / currentScan.Count < 1.0
        disp(['Low scan match score for index ' num2str(idx) '. Score = ' num2str(stats.Score) '.']);
    end
    
    % Maintain the list of robot poses. 
    absolutePose = exampleHelperComposeTransform(poseList(idx-1,:),transform);
    poseList(idx,:) = absolutePose;
       
    % Integrate the current laser scan into the probabilistic occupancy
    % grid.
    insertRay(map,absolutePose,currentScan,10);

end

Visualizar mapa

Visualice el mapa de la cuadrícula de ocupación poblado con los escaneos láser.

figure
show(map);
title('Occupancy grid map built using scan matching results');

Trazar las poses absolutas del robot que fueron calculadas por el algoritmo de coincidencia de escaneo. Este muestra la ruta que tomó el robot a través del mapa del entorno.

hold on
plot(poseList(:,1),poseList(:,2),'bo','DisplayName','Estimated robot position');
legend('show','Location','NorthWest')

Referencias

[1] P. Biber, W. Strasser, "Las distribuciones normales transforman: Un nuevo enfoque para la coincidencia de escaneos láser", en Actas de la Conferencia Internacional IEEE/RSJ sobre Robots y Sistemas Inteligentes (IROS), 2003, págs. 2743-2748.