Diese Seite wurde mithilfe maschineller Übersetzung übersetzt. Klicken Sie hier, um die neueste Version auf Englisch zu sehen.

Planen Sie mobile Roboterpfade mit RRT

Dieses Beispiel zeigt, wie der RRT-Algorithmus (Rapsful Exploring Random Tree) zum Planen eines Pfads für ein Fahrzeug durch eine bekannte Karte verwendet wird. Mit einem benutzerdefinierten Zustandsraum werden auch spezielle Fahrzeugseinschränkungen angewendet. Sie können Ihren eigenen Planer mit benutzerdefinierten Zustandsraum- und Pfadvalidierungsobjekten für jede Navigationsanwendung optimieren.

Belegung der Ladefläche

Laden Sie eine Belegungskarte, die einen kleinen Büroraum darstellt. Definieren Sie die Start- und Zielpositionen des Roboters auf der Karte .

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

Überlegen Sie sich ein grundlegendes Kreismodell für den Roboter. Um zu verhindern, dass der Roboter Hindernissen zu nahe kommt, vergrößern Sie die Hindernisse auf der Karte leicht.

% 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); 

Zustandsraum definieren

Geben Sie den Zustandsraum des Fahrzeugs mithilfe eines stateSpaceDubins-Objekts an und legen Sie die Zustandsgrenzen fest. Dieses Objekt begrenzt die abgetasteten Zustände auf mögliche Dubin-Kurven zum Lenken eines Fahrzeugs innerhalb der Zustandsgrenzen. Ein Wenderadius von 0,4 Metern ermöglicht enge Kurven in dieser kleinen Umgebung.

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

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

Planen Sie den Weg

Um einen Pfad zu planen, prüft der RRT-Algorithmus zufällige Zustände innerhalb des Zustandsraums und versucht, einen Pfad zu verbinden. Diese Zustände und Verbindungen müssen basierend auf den Karteneinschränkungen validiert oder ausgeschlossen werden. Das Fahrzeug darf nicht mit in der Karte definierten Hindernissen kollidieren.

Erstellen Sie ein validatorOccupancyMap-Objekt mit dem angegebenen Zustandsraum. Setzen Sie die Map-Eigenschaft auf das geladene occupancyMap-Objekt. Stellen Sie einen ValdiationDistance von 0,05m ein. Diese Validierungsdistanz diskretisiert die Wegeverbindungen und prüft darauf basierend Hindernisse in der Karte.

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

Erstellen Sie den Pfadplaner und erhöhen Sie die maximale Verbindungsdistanz, um mehr Staaten zu verbinden. Legen Sie die maximale Anzahl von Iterationen für die Abtastung von Zuständen fest.

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

Passen Sie die GoalReached-Funktion an. Diese beispielhafte Hilfsfunktion prüft, ob ein möglicher Pfad das Ziel innerhalb eines festgelegten Schwellenwerts erreicht. Die Funktion gibt true zurück, wenn das Ziel erreicht wurde, und der Planer stoppt.

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

Setzen Sie den Zufallszahlengenerator zurück, um reproduzierbare Ergebnisse sicherzustellen. Planen Sie den Weg von der Start- bis zur Zielpose.

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

Pfad verkürzen

Entfernen Sie redundante Knoten entlang des Pfads mithilfe der Funktion shortenpath. Die Funktion entfernt unerwünschte Knoten und generiert einen kollisionsfreien Pfad durch die Verbindung nicht aufeinanderfolgender Knoten, die nicht zu Kollisionen führen.

shortenedPath = shortenpath(pthObj,stateValidator);

Berechnen Sie die Pfadlänge des ursprünglichen Pfades und des verkürzten Pfades

originalLength = pathLength(pthObj)

originalLength = 
33.8183

shortenedLength = pathLength(shortenedPath)

shortenedLength = 
29.0853

Sie können beobachten, dass die Verkürzung zu einer Verringerung der Pfadlänge führte.

Ursprünglichen Pfad und verkürzten Pfad plotten

Zeigen Sie die Belegungskarte an. Zeichnen Sie den Suchbaum von solnInfo. Interpolieren und überlagern Sie den endgültigen Pfad.

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

Passen Sie Dubins Fahrzeugbeschränkungen an

Um benutzerdefinierte Fahrzeugeinschränkungen festzulegen, passen Sie das Zustandsraumobjekt an. Dieses Beispiel verwendet ExampleHelperStateSpaceOneSidedDubins, das auf der Klasse stateSpaceDubins basiert. Diese Hilfsklasse begrenzt die Abbiegerichtung basierend auf einer booleschen Eigenschaft, GoLeft, nach rechts oder links. Diese Eigenschaft deaktiviert im Wesentlichen die Pfadtypen des dubinsConnection-Objekts.

Erstellen Sie das Zustandsraumobjekt mithilfe des Beispiel-Helfers. Geben Sie dieselben Zustandsgrenzen an und geben Sie den neuen Booleschen Parameter als true an (nur Linksabbiegen).

% Only making left turns
goLeft = true;

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

Pfad planen

Erstellen Sie ein neues Planerobjekt mit den benutzerdefinierten Dubins-Einschränkungen und einem auf diesen Einschränkungen basierenden Validator. Geben Sie dieselbe GoalReached-Funktion an.

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

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

Planen Sie den Weg zwischen Start und Ziel. Setzen Sie den Zufallszahlengenerator erneut zurück.

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

Pfad verkürzen

Verkürzen Sie den Pfad, indem Sie die benutzerdefinierten Bewegungsbeschränkungen beibehalten.

shortenedPath2 = shortenpath(pthObj2,stateValidator2);

Berechnen Sie die Pfadlänge des ursprünglichen Pfades und des verkürzten Pfades

originalLength2 = pathLength(pthObj2)

originalLength2 = 
46.7841

shortenedLength2 = pathLength(shortenedPath2)

shortenedLength2 = 
45.7649

Sie können beobachten, dass die Verkürzung zu einer Verringerung der Pfadlänge führte.

Pfad plotten

Um das Ziel zu erreichen, führt der Weg ausschließlich Linkskurven durch.

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