RRT-Algorithmus für Bahnplanung in der Robotik – Tutorial basierend auf AME 547 HW 5

Einführung in den RRT-Algorithmus für die Bahnplanung

In der Robotik ist die Bahnplanung eine zentrale Herausforderung: Ein Roboter muss sich von einer Startpose zu einer Zielpose bewegen, ohne mit Hindernissen zu kollidieren. Der Rapidly-exploring Random Tree (RRT)-Algorithmus ist eine weit verbreitete Methode, um solche Pfade effizient zu finden. Inspiriert von der Aufgabenstellung AME 547 HW 5, die am 4. November 2021 fällig war, zeigt dieser Tutorial, wie du einen RRT-basierten Single Tree Search implementierst – ähnlich wie in LaValle's Buch (Kapitel 5.5.3).

Aktuelle Beispiele aus der Praxis: Stell dir vor, ein autonomer Lieferroboter in einer Münchner Innenstadt muss um Baustellen und Fußgänger herum navigieren – genau hier kommt RRT zum Einsatz. Oder denk an die Mars-Rover der NASA, die mit RRT-ähnlichen Algorithmen sicher über felsiges Gelände fahren.

Grundlagen des RRT: Single Tree Search

Der RRT-Algorithmus baut einen Baum von Knoten im Konfigurationsraum auf. Start- und Zielkonfigurationen werden durch Gelenkwinkel (θ1, θ2, θ3) definiert. Hindernisse werden als Kreise approximiert, der Roboter als Liniensegment. Der Baum wächst, indem zufällige Konfigurationen generiert und mit dem nächsten Baumknoten verbunden werden – vorausgesetzt, die Verbindung kollidiert nicht mit Hindernissen.

Warum RRT? – Aktuelle Trends in der Robotik

Mit dem Aufkommen von KI-gestützten Servicerobotern (z.B. in der Gastronomie oder Logistik) ist die schnelle Bahnplanung wichtiger denn je. Auch in der Spieleentwicklung (z.B. FIFA 2026 oder GTA VI) werden ähnliche Algorithmen für die Navigation von NPCs genutzt. Der RRT ist besonders nützlich, wenn der Konfigurationsraum hochdimensional ist – wie bei Roboterarmen mit mehreren Freiheitsgraden.

Schritt 1: Problemdefinition und Annahmen

In Anlehnung an AME 547 HW 5 nehmen wir an:

Der Roboter wird durch Linien approximiert.
Hindernisse sind Kreise (maximal 4).
Start- und Zielpose sind durch Gelenkwinkel (θ1, θ2, θ3) gegeben.
Der Algorithmus soll einen kollisionsfreien Pfad finden.

Die Testdaten werden am Mittwoch, 3. November 2021 um 18 Uhr veröffentlicht. In unserem Tutorial verwenden wir fiktive Daten, um das Prinzip zu demonstrieren.

Schritt 2: Implementierung des RRT-Algorithmus

Der Algorithmus lässt sich in folgende Schritte gliedern:

Initialisierung: Erstelle einen Baum mit der Startkonfiguration als Wurzel.
Zufallspunkt generieren: Wähle eine zufällige Konfiguration im Konfigurationsraum.
Nächsten Knoten finden: Finde den Knoten im Baum, der dem Zufallspunkt am nächsten ist (z.B. euklidische Distanz im Gelenkwinkelraum).
Steuerung: Bewege vom nächsten Knoten einen Schritt in Richtung des Zufallspunkts (Schrittweite δ).
Kollisionsprüfung: Überprüfe, ob die Verbindung zwischen dem nächsten Knoten und dem neuen Knoten mit Hindernissen kollidiert. Falls ja, verwerfe den Schritt.
Einfügen: Füge den neuen Knoten in den Baum ein.
Zielfindung: Prüfe, ob der neue Knoten nahe genug an der Zielkonfiguration ist. Falls ja, verbinde mit dem Ziel und beende.
Wiederholung: Gehe zu Schritt 2, bis das Ziel erreicht oder eine maximale Anzahl von Iterationen überschritten ist.

Die Kollisionsprüfung ist entscheidend: Für jedes Hindernis (Kreis) und jede Roboterlinie (zwischen zwei Konfigurationen) wird der Abstand zwischen Liniensegment und Kreismittelpunkt berechnet. Ist dieser kleiner als der Kreisradius, liegt eine Kollision vor.

Schritt 3: Grafische Darstellung des Pfads

Nach erfolgreicher Berechnung soll der Pfad grafisch dargestellt werden. Typischerweise werden Hindernisse als rote Kreise, der Roboter als schwarze Linie und der gefundene Pfad als grüne Linie gezeichnet. Die Startpose wird mit einem grünen Punkt, die Zielpose mit einem roten Punkt markiert.

In Python kann dies mit matplotlib umgesetzt werden. Ein Code-Snippet für die Darstellung:

import matplotlib.pyplot as plt
import numpy as np

# Beispiel: Hindernisse als Kreise
obstacles = [{'center': (0.5, 0.5), 'radius': 0.2}]
# Roboterpfad als Liste von Punkten
path = [(0,0), (0.1,0.1), ...]

fig, ax = plt.subplots()
for obs in obstacles:
    circle = plt.Circle(obs['center'], obs['radius'], color='r', alpha=0.5)
    ax.add_patch(circle)
path = np.array(path)
ax.plot(path[:,0], path[:,1], 'g-', linewidth=2)
ax.scatter(path[0,0], path[0,1], color='green', s=100, label='Start')
ax.scatter(path[-1,0], path[-1,1], color='red', s=100, label='Ziel')
ax.set_xlim(0,1)
ax.set_ylim(0,1)
plt.legend()
plt.show()

Schritt 4: Code zur Pfadberechnung

Der vollständige Code sollte die RRT-Logik, Kollisionsprüfung und Pfadextraktion enthalten. Ein minimalistisches Beispiel in Python:

import random
import math

class RRT:
    def __init__(self, start, goal, obstacles, step_size=0.1, max_iter=1000):
        self.start = start
        self.goal = goal
        self.obstacles = obstacles
        self.step_size = step_size
        self.max_iter = max_iter
        self.tree = [start]
        self.parent = {start: None}
    
    def plan(self):
        for _ in range(self.max_iter):
            rand = self.random_config()
            nearest = self.nearest(rand)
            new = self.steer(nearest, rand)
            if not self.collision(nearest, new):
                self.tree.append(new)
                self.parent[new] = nearest
                if self.distance(new, self.goal) < self.step_size:
                    self.tree.append(self.goal)
                    self.parent[self.goal] = new
                    return self.extract_path()
        return None
    
    def random_config(self):
        return (random.uniform(0, 2*math.pi), random.uniform(0, 2*math.pi), random.uniform(0, 2*math.pi))
    
    def nearest(self, config):
        return min(self.tree, key=lambda n: self.distance(n, config))
    
    def steer(self, from_node, to_node):
        # Lineare Interpolation im Gelenkwinkelraum
        dist = self.distance(from_node, to_node)
        if dist < self.step_size:
            return to_node
        ratio = self.step_size / dist
        return tuple(f + ratio * (t - f) for f, t in zip(from_node, to_node))
    
    def distance(self, a, b):
        return math.sqrt(sum((a[i]-b[i])**2 for i in range(3)))
    
    def collision(self, a, b):
        # Vereinfachte Kollisionsprüfung (hier nur Abstand zu Kreisobstakeln)
        # In der Realität: Roboterlinie vs. Kreis
        return False  # Platzhalter
    
    def extract_path(self):
        path = []
        node = self.goal
        while node is not None:
            path.append(node)
            node = self.parent[node]
        path.reverse()
        return path

Dieser Code ist bewusst einfach gehalten. Für die vollständige Aufgabe musst du die Kollisionsprüfung implementieren (Linie-Kreis-Intersektion) und die grafische Ausgabe ergänzen.

Häufige Fehler und Tipps

Schrittweite zu groß: Der Pfad kann Hindernisse überspringen. Wähle δ klein genug (z.B. 0.1 rad).
Kollisionsprüfung zu grob: Teste mehrere Punkte entlang der Linie.
Zu wenige Iterationen: Erhöhe die max_iter bei schwierigen Umgebungen.

Zusammenfassung und Ausblick

Der RRT-Algorithmus ist eine leistungsfähige Methode zur Bahnplanung in der Robotik. Mit diesem Tutorial hast du die Grundlagen für die Implementierung eines Single Tree Search gelernt, wie in AME 547 HW 5 gefordert. Die Technik findet nicht nur in der Robotik Anwendung, sondern auch in Computerspielen, autonomen Fahrzeugen und sogar in der Animation von virtuellen Charakteren.

Hinweis: Dieses Tutorial dient als Lernhilfe. Die vollständige Lösung der Hausaufgabe erfordert eigene Implementierungsarbeit. Nutze die Konzepte, um deinen eigenen Code zu schreiben und zu testen.