Questa pagina è stata tradotta con la traduzione automatica. Fai clic qui per vedere l'ultima versione in inglese.

Pianificare percorsi di robot mobili utilizzando RRT

Questo esempio mostra come utilizzare l'algoritmo RRT (Rapidly Explore Random Tree) per pianificare un percorso per un veicolo attraverso una mappa nota. Vengono inoltre applicati vincoli speciali ai veicoli con uno spazio di stato personalizzato. È possibile personalizzare il proprio pianificatore con oggetti di convalida dello spazio di stato e del percorso personalizzati per qualsiasi applicazione di navigazione.

Mappa di occupazione del carico

Carica una mappa di occupazione che rappresenti un piccolo spazio ufficio. Definisci le pose di partenza e di arrivo del robot sulla mappa .

load("office_area_gridmap.mat","occGrid")
show(occGrid)

% Set start and goal poses.
start = [-1.0,0.0,-pi];
goal = [14,-2.25,0];

% Show start and goal positions of robot.
hold on
plot(start(1),start(2),'ro')
plot(goal(1),goal(2),'mo')

% Show start and goal heading angle using a line.
r = 0.5;
plot([start(1),start(1) + r*cos(start(3))],[start(2),start(2) + r*sin(start(3))],'r-')
plot([goal(1),goal(1) + r*cos(goal(3))],[goal(2),goal(2) + r*sin(goal(3))],'m-')
hold off

Consideriamo un modello circolare di base per il robot. Per evitare che il robot si avvicini troppo agli ostacoli, gonfia leggermente gli ostacoli sulla mappa.

% Make a copy of the original map and infate it by 0.1 meters. Use this inflated map for path planning. 
% Use the original map for visualization purpose. 
inflatedMap = copy(occGrid);
inflate(inflatedMap,0.1); 

Definisci lo spazio di stato

Specificare lo spazio di stato del veicolo utilizzando un oggetto stateSpaceDubins e specificando i limiti di stato. Questo oggetto limita gli stati campionati alle curve di Dubins praticabili per sterzare un veicolo entro i limiti dello stato. Il raggio di sterzata di 0,4 metri consente curve strette in questo spazio ristretto.

bounds = [inflatedMap.XWorldLimits; inflatedMap.YWorldLimits; [-pi pi]];

ss = stateSpaceDubins(bounds);
ss.MinTurningRadius = 0.4;

Pianifica il percorso

Per pianificare un percorso, l'algoritmo RRT campiona stati casuali all'interno dello spazio degli stati e tenta di collegare un percorso. Questi stati e connessioni devono essere convalidati o esclusi in base ai vincoli della mappa. Il veicolo non deve entrare in collisione con gli ostacoli definiti sulla mappa.

Crea un oggetto validatorOccupancyMap con lo spazio di stato specificato. Impostare la proprietà Map sull'oggetto occupancyMap caricato. Impostare un ValdiationDistance di 0,05 m. Questa distanza di convalida discretizza le connessioni del percorso e, in base a ciò, controlla gli ostacoli sulla mappa.

stateValidator = validatorOccupancyMap(ss); 
stateValidator.Map = inflatedMap;
stateValidator.ValidationDistance = 0.05;

Crea il pianificatore di percorsi e aumenta la distanza massima di connessione per collegare più stati. Imposta il numero massimo di iterazioni per gli stati di campionamento.

planner = plannerRRT(ss,stateValidator);
planner.MaxConnectionDistance = 2.5;
planner.MaxIterations = 30000;

Personalizza la funzione GoalReached. Questa funzione di supporto di esempio verifica se un percorso fattibile raggiunge l'obiettivo entro una soglia impostata. La funzione restituisce true quando l'obiettivo è stato raggiunto e il pianificatore si arresta.

planner.GoalReachedFcn = @exampleHelperCheckIfGoal;

function isReached = exampleHelperCheckIfGoal(planner, goalState, newState)
    isReached = false;
    threshold = 0.1;
    if planner.StateSpace.distance(newState, goalState) < threshold
        isReached = true;
    end
end

Reimpostare il generatore di numeri casuali per garantire risultati riproducibili. Pianifica il percorso dalla posizione iniziale a quella finale.

rng default
[pthObj,solnInfo] = plan(planner,start,goal);

Accorcia il percorso

Rimuovere i nodi ridondanti lungo il percorso utilizzando la funzione shortenpath. La funzione rimuove i nodi indesiderati e genera un percorso privo di collisioni collegando i nodi non sequenziali che non generano collisioni.

shortenedPath = shortenpath(pthObj,stateValidator);

Calcola la lunghezza del percorso originale e del percorso abbreviato

originalLength = pathLength(pthObj)

originalLength = 
33.8183

shortenedLength = pathLength(shortenedPath)

shortenedLength = 
29.0853

Si può osservare che l'accorciamento ha portato ad una riduzione della lunghezza del percorso.

Traccia il percorso originale e il percorso abbreviato

Mostra la mappa dell'occupazione. Rappresentare graficamente l'albero di ricerca da solnInfo . Interpolare e sovrapporre il percorso finale.

show(occGrid)
hold on

% Plot entire search tree.
plot(solnInfo.TreeData(:,1),solnInfo.TreeData(:,2),plannerLineSpec.tree{:})

% Interpolate and plot path.
interpolate(pthObj,300)
plot(pthObj.States(:,1),pthObj.States(:,2),plannerLineSpec.path{:})

% Interpolate and plot path.
interpolate(shortenedPath,300);
plot(shortenedPath.States(:,1),shortenedPath.States(:,2),'g-','LineWidth',3)

% Show start and goal in grid map.
plot(start(1),start(2),'ro')
plot(goal(1),goal(2),'mo')
legend('search tree','original path','shortened path')
hold off

Personalizza i vincoli del veicolo Dubins

Per specificare vincoli personalizzati per il veicolo, personalizzare l'oggetto spazio di stato. In questo esempio viene utilizzata ExampleHelperStateSpaceOneSidedDubins, basata sulla classe stateSpaceDubins. Questa classe helper limita la direzione di svolta a destra o a sinistra in base a una proprietà booleana, GoLeft . Questa proprietà sostanzialmente disabilita i tipi di percorso dell'oggetto dubinsConnection.

Creare l'oggetto dello spazio di stato utilizzando l'helper di esempio. Specificare gli stessi limiti di stato e assegnare il nuovo parametro booleano come true (solo svolte a sinistra).

% Only making left turns
goLeft = true;

% Create the state space
ssCustom = ExampleHelperStateSpaceOneSidedDubins(bounds,goLeft);
ssCustom.MinTurningRadius = 0.4;

Pianifica il percorso

Creare un nuovo oggetto planner con i vincoli Dubins personalizzati e un validatore basato su tali vincoli. Specificare la stessa funzione GoalReached.

stateValidator2 = validatorOccupancyMap(ssCustom); 
stateValidator2.Map = inflatedMap;
stateValidator2.ValidationDistance = 0.05;

planner = plannerRRT(ssCustom,stateValidator2);
planner.MaxConnectionDistance = 2.5;
planner.MaxIterations = 30000;
planner.GoalReachedFcn = @exampleHelperCheckIfGoal;

Pianifica il percorso tra la partenza e l'arrivo. Reimpostare nuovamente il generatore di numeri casuali.

rng default
[pthObj2,solnInfo] = plan(planner,start,goal);

Accorcia il percorso

Riduci il percorso mantenendo i vincoli di movimento personalizzati.

shortenedPath2 = shortenpath(pthObj2,stateValidator2);

Calcola la lunghezza del percorso originale e del percorso abbreviato

originalLength2 = pathLength(pthObj2)

originalLength2 = 
46.7841

shortenedLength2 = pathLength(shortenedPath2)

shortenedLength2 = 
45.7649

Si può osservare che l'accorciamento ha portato ad una riduzione della lunghezza del percorso.

Percorso della trama

Per raggiungere la meta, il percorso effettua solo svolte a sinistra.

figure
show(occGrid)
hold on
% Interpolate and plot path.
interpolate(pthObj2,300)
h1 = plot(pthObj2.States(:,1),pthObj2.States(:,2),plannerLineSpec.path{:});

% Interpolate and plot path.
interpolate(shortenedPath2,300)
h2 = plot(shortenedPath2.States(:,1),shortenedPath2.States(:,2),'g-','LineWidth',3);

% Show start and goal in grid map.
plot(start(1),start(2),'ro')
plot(goal(1),goal(2),'mo')
legend([h1 h2],'original path','shortened path')
hold off