Questa pagina è stata tradotta con la traduzione automatica. Fai clic qui per vedere l’originale in inglese.

Implementare la localizzazione e la mappatura simultanee (SLAM) con scansioni Lidar

Apri live script

Questo esempio dimostra come implementare l'algoritmo SLAM (Simultaneous Localization And Mapping) su una serie di scansioni lidar raccolte utilizzando l'ottimizzazione del grafico di posa. L'obiettivo di questo esempio è quello di costruire una mappa dell'ambiente utilizzando le scansioni lidar e recuperare la traiettoria del robot.

Per costruire la mappa dell'ambiente, l'algoritmo SLAM elabora in modo incrementale le scansioni lidar e crea un grafico di posa che collega tali scansioni. Il robot riconosce un luogo precedentemente visitato tramite la corrispondenza della scansione e può stabilire una o più chiusure di loop lungo il suo percorso. L'algoritmo SLAM utilizza le informazioni sulla chiusura del loop per aggiornare la mappa e regolare la traiettoria stimata del robot.

Caricamento dei dati della scansione laser dal file

Caricare un set di dati sottocampionato costituito da scansioni laser raccolte da un robot mobile in un ambiente interno. Lo spostamento medio tra due scansioni è di circa 0,6 metri.

Il file offlineSlamData.mat contiene la variabile scans, che contiene tutte le scansioni laser utilizzate in questo esempio.

load('offlineSlamData.mat');

A scopo illustrativo vengono forniti una planimetria e un percorso approssimativo del robot. Questa immagine mostra l'ambiente relativo mappato e la traiettoria approssimativa del robot.

Esecuzione dell'algoritmo SLAM, creazione di una mappa ottimizzata e tracciamento della traiettoria del robot

Creare un oggetto lidarSLAM e impostare la risoluzione della mappa e la portata massima del lidar. In questo esempio viene utilizzato un robot Jackal™ di Clearpath Robotics™. Il robot è dotato di uno scanner laser SICK™ TiM-511 con una portata massima di 10 metri. Impostare la portata massima del lidar su un valore leggermente inferiore alla portata massima di scansione (8 m), poiché le letture laser sono meno precise in prossimità della portata massima. Impostare la risoluzione della mappa della griglia su 20 celle per metro, il che garantisce una precisione di 5 cm.

maxLidarRange = 8;
mapResolution = 20;
slamAlg = lidarSLAM(mapResolution, maxLidarRange);

I seguenti parametri di chiusura del loop sono impostati empiricamente. L'utilizzo di una soglia di chiusura del loop più elevata aiuta a scartare i falsi positivi nel processo di identificazione della chiusura del loop. Tuttavia, tieni presente che una partita con un punteggio elevato potrebbe comunque rivelarsi una partita negativa. Ad esempio, le scansioni effettuate in un ambiente con caratteristiche simili o ripetute hanno maggiori probabilità di produrre falsi positivi. Utilizzando un raggio di ricerca di chiusura del loop più elevato, l'algoritmo può cercare le chiusure del loop in un intervallo più ampio della mappa attorno alla stima della posa corrente.

slamAlg.LoopClosureThreshold = 210;  
slamAlg.LoopClosureSearchRadius = 8;

Osservazione del processo di creazione della mappa con le prime 10 scansioni

Aggiungere in modo incrementale le scansioni all'oggetto slamAlg. I numeri di scansione vengono stampati se aggiunti alla mappa. L'oggetto rifiuta le scansioni se la distanza tra le scansioni è troppo piccola. Aggiungere prima le prime 10 scansioni per testare il tuo algoritmo.

for i=1:10
    [isScanAccepted, loopClosureInfo, optimizationInfo] = addScan(slamAlg, scans{i});
    if isScanAccepted
        fprintf('Added scan %d \n', i);
    end
end
Added scan 1 
Added scan 2 
Added scan 3 
Added scan 4 
Added scan 5 
Added scan 6 
Added scan 7 
Added scan 8 
Added scan 9 
Added scan 10

Ricostruisci la scena tracciando le scansioni e le pose tracciate da slamAlg.

figure;
show(slamAlg);
title({'Map of the Environment','Pose Graph for Initial 10 Scans'});

Osservazione dell'effetto delle chiusure di loop e del processo di ottimizzazione

Continua ad aggiungere scansioni in un loop. Le chiusure dei loop dovrebbero essere rilevate automaticamente durante il movimento del robot. L'ottimizzazione del grafico delle pose viene eseguita ogni volta che viene identificata una chiusura di loop. L'output optimizationInfo ha un campo, IsPerformed, che indica quando si verifica l'ottimizzazione del grafico di posa.

Tracciare le scansioni e le pose ogni volta che viene identificata una chiusura del loop e verificare visivamente i risultati. Questo grafico mostra scansioni sovrapposte e un grafico di posa ottimizzato per la prima chiusura del loop. Un arco di chiusura del loop viene aggiunto come link rosso.

firstTimeLCDetected = false;

figure;
for i=10:length(scans)
    [isScanAccepted, loopClosureInfo, optimizationInfo] = addScan(slamAlg, scans{i});
    if ~isScanAccepted
        continue;
    end
    % visualize the first detected loop closure, if you want to see the
    % complete map building process, remove the if condition below
    if optimizationInfo.IsPerformed && ~firstTimeLCDetected
        show(slamAlg, 'Poses', 'off');
        hold on;
        show(slamAlg.PoseGraph); 
        hold off;
        firstTimeLCDetected = true;
        drawnow
    end
end
title('First loop closure');

Visualizzazione della mappa costruita e la traiettoria del robot

Tracciare la mappa finale creata dopo aver aggiunto tutte le scansioni all'oggetto slamAlg. Sebbene il precedente loop for tracciasse solo la chiusura iniziale, sono state aggiunte tutte le scansioni.

figure
show(slamAlg);
title({'Final Built Map of the Environment', 'Trajectory of the Robot'});

Ispezionare visivamente la mappa costruita rispetto alla planimetria originale

Un'immagine delle scansioni e del grafico delle pose è sovrapposta alla planimetria originale. Dopo aver aggiunto tutte le scansioni e ottimizzato il grafico delle pose, è possibile notare che la mappa corrisponde perfettamente alla planimetria originale.

Creazione di una mappa della griglia di occupazione

Le scansioni e le pose ottimizzate possono essere utilizzate per generare un occupancyMap, che rappresenta l'ambiente come una griglia di occupazione probabilistica.

[scans, optimizedPoses]  = scansAndPoses(slamAlg);
map = buildMap(scans, optimizedPoses, mapResolution, maxLidarRange);

Visualizza la mappa della griglia di occupazione popolata con le scansioni laser e il grafico di posa ottimizzato.

figure; 
show(map);
hold on
show(slamAlg.PoseGraph, 'IDs', 'off');
hold off
title('Occupancy Grid Map Built Using Lidar SLAM');

Vedi anche

| |

Argomenti