Damit ein Fahrzeug autonom fahren kann, sind einige Kernfunktionalitäten mit entsprechender Algorithmik notwendig: Die Bestimmung der aktuellen Position und Ausrichtung, das Berechnen eines Weges vom Standort zu einem Ziel, das Verfolgen des Wegs zum Ziel, der Umgang mit unerwarteten Hindernissen und natürlich die entsprechende Ansteuerung und Regelung der Motoren und der Lenkung. Es ist klar, dass die Ermittlung der eigenen Position von primärer Bedeutung ist (Bild 1).
Oft reicht ein einzelner Positionssensor nicht aus
In vielen Anwendungen reicht ein einzelner Positionssensor wie ein GPS-Empfänger im Freien oder UWB in Räumen, alleine nicht aus, um eine befriedigende, genügend dynamische und präzise Positionsschätzung zu erhalten. Stattdessen werden weitere Grössen gemessen und benutzt, wie Fahrzeuggeschwindigkeit und -beschleunigung, Himmelsrichtung und deren Änderung usw. Mittels Sensorfusion, zum Beispiel durch ein Kalman-Filter, entsteht daraus eine gemeinsame Schätzung.
Zudem wird vorausgesetzt, dass ein autonomes Fahrzeug in der Lage ist, Hindernisse in der Umgebung zu erfassen, um darauf entsprechend zu reagieren, zum Beispiel indem es stoppt oder ihnen ausweicht. Bei solchen Anti-Kollisionssystemen will man sich im Normalfall auch nicht auf einen einzelnen Sensor verlassen. Ein Ultraschallsystem ist nicht geeignet für lange Distanzen, Radar kann sehr kleine Hindernisse, wie einen Maschendraht übersehen, ein Laserdistanzmesssystem schwächelt bei Regen und ein optisches System mit Kameras kann bei Nebel oder direkter Sonneneinstrahlung keine guten Resultate erzielen. Die Schwächen und Stärken der einzelnen Sensoren können durch geeignete Algorithmik zu einem robusten und zuverlässigen Gesamtsystem vereint werden.
Elementarer Path Planner
Eine weitere wichtige Grundfunktion eines autonomen Fahrzeugs ist das Anfahren eines Ziels. Für die meisten industriellen Anwendungen genügt es nicht, sich in Richtung des Zielpunktes zu bewegen und unterwegs Hindernissen auszuweichen. Statt dieser eher zufälligen Strategie ist es oft erwünscht, von der eigenen Position zum Ziel, unter Berücksichtigung von bekannten Hindernissen, den oder einen optimalen Weg zu finden. Der sogenannte Path Planner ist dafür verantwortlich.
Er berechnet von einem Startpunkt aus einen optimalen Weg zum Ziel. Was optimal ist, definiert ein Kriterium, zum Beispiel indem der Weg minimale Steigungen haben soll, minimalen Treibstoffverbrauch verursacht, keine zu engen Kurven enthält, oder etwas Ähnliches. Oft jedoch ist einfach der kürzeste Weg gesucht. Bekannte Hindernisse werden in einer Karte markiert und dem Algorithmus zur Verfügung gestellt. Zur Illustration dient eine Lagerhalle, in der sich ein autonomer Roboter bewegt. Der soll nun auf dem kürzesten Weg zu einer gewissen Stelle bei Regal C fahren. Man muss dabei beachten, dass die Regale Hindernisse darstellen (Bild 2).
Diskretisierte Karte als Problemlöser
Eine Strategie, um dieses Problem zu lösen, geht von einer diskretisierten Karte aus (Bild 3). Dabei wird ein Gitter mit Zellen über die Karte gelegt. Dann wird eine Bewertungsfunktion definiert. Mit Hilfe dieser Funktion wird anschliessend für jede Zelle iterativ eine Art Kostenwert errechnet. Jede Kostenzahl wird direkt in die jeweils betreffende Zelle notiert. Will man den kürzesten Weg vom Startpunkt zum Ziel finden, so muss man als Kosten eines Feldes sinnvollerweise gerade die Anzahl Felder betrachten, die überquert werden müssen, um vom Startpunkt zum Ziel zu gelangen.
Die Distanz zwischen zwei horizontal oder vertikal benachbarten Feldern zählen wir als eine Einheit. Bei diagonal benachbarten Feldern zählen wir die Distanz als 1,4 Einheiten, was eine einfache Annäherung an die Euklidische Distanz ist – die Quadratwurzel aus zwei. Um für alle Zellen bis zum Ziel die Kostenfunktion zu berechnen, geht man so vor:
- es werden alle befahrbaren Zellen auf den Kostenwert unendlich initialisiert
- alle befahrbaren Zellen werden als noch nicht besucht markiert (weiss)
- die Startzelle erhält den Kostenwert 0
Iterativ werden nun die Kosten der Knotenpunkte nach folgendem Schema reduziert:
- Wähle unter den noch nicht besuchten Zellen jene mit dem tiefsten Kostenwert aus und markiere sie temporär (gelb). Sollte es mehrere Kandidaten mit demselben tiefsten Kostenwert geben, so wähle einen davon aus
- Berechne die Kostenfunktion für alle unmittelbaren Nachbarzellen, die noch nicht besucht wurden (temporär grün)
- Vergleiche für jeden Nachbarn den betreffenden neuen Kostenwert mit dem schon bestehenden und übernimm den tieferen Wert von beiden
- Markiere die aktuelle Zelle als besucht (blau)
- Wiederhole die Punkte 4 bis 7 solange, bis die Zielzelle als besucht markiert wurde
Nun kann man den kürzesten Weg auslesen:
- Wähle als Ausgangspunkt die Zelle am Ziel
- Wähle unter den unmittelbaren Nachbarzellen, die als besucht markiert sind, jene Zelle als Nachfolger im Pfad aus, die den kleinsten Kostenwert hat. Markiere sie (roter Rand). Sollte es mehrere Kandidaten mit demselben tiefsten Kostenwert geben, so wähle einen davon aus
- Wiederhole Punkt 10 für jede Nachfolgerzelle so lange, bis der Startpunkt erreicht ist
Algorithmus ist Teil der Graphen-Theorie
Der eben beschriebene Algorithmus ist übrigens nicht neu. Er wurde von Edsger Dijkstra 1959 publiziert, als von autonomen Fahrzeugen noch kaum die Rede war, und ist heute Teil der modernen Graphen-Theorie. In den Bildern 4 bis 12 wird das Verfahren auf der Beispielkarte angewendet. Bild 4 zeigt die initialisierte Karte. Der Startpunkt ist als ausgewählt markiert (gelb). Die Kostenwerte seiner Nachbarn (grün) werden in der ersten Iteration reduziert. In Bild 5 sind die neuen Kostenwerte eingetragen. Damit ist die erste Iteration abgeschlossen, die Startzelle wird als besucht markiert (blau). Jetzt wird die nächste Zelle ausgewählt, nämlich eine, die noch nicht besucht ist und davon jene, die aktuell den kleinsten Kostenwert hat. Bild 6 zeigt, dass das Feld links des Startpunkts gewählt wurde. Es hätte noch drei weitere Kandidaten mit demselben Kostenwert gegeben. Die Wahl ist zufällig. Wiederum bezeichnen die grünen Felder jene, die in der zweiten Iteration aufdatiert wurden. Bild 7 zeigt den Zustand nach der dritten Iteration. Bild 8 und 9 zeigen, wie die blaue Fläche der schon besuchten Felder sich langsam in alle Richtungen ausbreitet. In Bild 10 wird die Zielzelle erreicht. in Bild 11 wird das Ziel als besucht markiert. Damit endet der Algorithmus. In Bild 12 ist der gesuchte Weg markiert. Er folgt rückwärts vom Ziel zum Start, entlang des grössten Gradienten.
Verbesserungen für Echtzeitsysteme
Obwohl das wunderbar funktioniert, hat Dijkstras Algorithmus ein Problem: Je nach Auflösung und Grösse der Karte kann das Verfahren eine extrem hohe Anzahl von Iterationen erfordern und ist folglich für Echtzeitsysteme schlecht geeignet. Aus diesem Grund gibt es einige Varianten davon.
Das sogenannte A*-Verfahren verbessert in erster Linie die Effizienz. Zu diesem Zweck wird die Kostenfunktion (Entfernung eines Feldes vom Startpunkt) erweitert durch eine heuristische Schätzung der Distanz des Feldes zum Ziel. Die beiden Entfernungen werden addiert. Das führt dazu, dass der Kostenwert von Zellen, die vom Ziel wegführen anwächst und folglich nicht sofort weiter verfolgt wird. Erst wenn die scheinbar direkten Wege nicht zum Ziel führen, greift der Algorithmus auf diese Zellen zurück.
Der D*-Algorithmus ist für autonome Systeme besonders nützlich. Er beginnt nicht beim Startpunkt und sucht den kürzesten Weg zum Ziel, sondern umgekehrt: Er sucht den kürzesten Weg vom Ziel zum Startpunkt. Dabei verwendet er im Wesentlichen das A*-Verfahren. Verändert sich nun der Startpunkt, weil sich das Fahrzeug bewegt und tritt plötzlich ein zuvor noch nicht bekanntes Hindernis auf, so muss nicht der gesamte Weg neu berechnet werden. Stattdessen wird die Kostenkarte nur partiell in der Umgebung des Hindernisses aufdatiert. Vom Hindernis zum Ziel bleiben die Kostenwerte erhalten. Dies erlaubt ein schnelles Umplanen und Reagieren auf unvorhergesehene Veränderungen der Karte.
Fazit
Path Planning ist bei autonomen Systemen nicht mehr wegzudenken und bildet einen essentiellen Bestandteil intelligenter Fahrzeuge. Gerade im Vergleich zu Systemen ohne diese Fähigkeit, welche oft den Anschein erwecken, als irren sie ziellos umher, wird schnell klar, wie wichtig ein solches mathematisches Verfahren in der Praxis ist. Der Rechenaufwand ist indessen nicht zu vernachlässigen. Ausserdem muss der Path Planner mit unerwarteten Hindernissen oder Änderungen zurechtkommen. Verschiedene Varianten des klassischen Dijkstra-Algorithmus tragen diesen Bedürfnissen Rechnung.
Infoservice
Stettbacher Signal Processing AG
Neugutstrasse 54, 8600 Dübendorf
Tel. 043 299 57 23
