Questa pagina è stata tradotta con la traduzione automatica. Fai clic qui per vedere l'ultima versione 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 utilizzando l'ottimizzazione del grafico di posa. L'obiettivo di questo esempio è 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 costruisce un grafico di posa che collega tali scansioni. Il robot riconosce un luogo precedentemente visitato tramite la scansione corrispondente 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.

Carica i 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.

Esegui l'algoritmo SLAM, costruisci una mappa ottimizzata e traccia la traiettoria del robot

Crea un oggetto lidarSLAM e imposta 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 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 a 20 celle per metro, il che fornisce una precisione di 5 cm.

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

I seguenti parametri di chiusura del ciclo 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 raccolte in un ambiente che presenta 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;

Osserva il processo di creazione della mappa con le prime 10 scansioni

Aggiungere gradualmente 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. Aggiungi 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'});

Osservare l'effetto delle chiusure di loop e del processo di ottimizzazione

Continua ad aggiungere scansioni in un ciclo. 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 ciclo. L'output optimizationInfo ha un campo, IsPerformed, che indica quando si verifica l'ottimizzazione del grafico di posa.

Tracciare grafici delle scansioni e delle 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 bordo di chiusura ad anello è aggiunto come collegamento 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');

Visualizza la mappa costruita e la traiettoria del robot

Dopo aver aggiunto tutte le scansioni all'oggetto slamAlg, tracciare il grafico della mappa finale. Sebbene il precedente ciclo 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 bene alla planimetria originale.

Costruisci la mappa della griglia di occupazione

Le scansioni e le pose ottimizzate possono essere utilizzate per generare uno 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 compilata 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