Algorithmen der Bewegungsplanung für nicht-holonome Roboter

Transkript

1 Seminar Sommersemester 2013: Automobile Systeme in der Automatisierung Prof. Dr. Dieter Zöbel, Universität Koblenz-Landau, FB Informatik Algorithmen der Bewegungsplanung für nicht-holonome Roboter Florian Kathe Eingereicht: / Fertiggestellt: Zusammenfassung Die vorliegende Arbeit vermittelt einen Einblick in den Bereich der autonomen Pfadplanung. Sie gibt einen Überblick über die Voraussetzungen und Grundlagen der Pfadplanung sowie der nicht-holonomen Systeme. Es wird eine Übersicht gegeben über allgemeine Algorithmen der Pfadplanung und deren Anpassung an nicht-holonome Pfadplanung. Dabei wird zwischen deterministischen und probabilistischen Verfahren unterschieden. Am Ende werden die im Paper vorgestellten Verfahren verglichen und gezeigt, wo die momentanen Probleme der nicht-holonomen Pfadplanung liegen und welche Verbesserungen gemacht werden können. Schlüsselwörter Pfadplanung, nicht-holonome Systeme, autonomes Fahren 1 Einleitung Pfadplanung ist für autonome Roboter jeher eine zentrale Aufgabe, da diese beschreibt, wie ein Roboter von A nach B kommt. Das Ziel ist es, von einer Startposition zu einer gegebenen Zielposition zu gelangen, ohne dabei in Berührungen mit Hindernissen zu kommen. 1.1 Architektur Damit der Roboter dies erreichen kann, ist es nötig, dass er einem bestimmtem Ablauf folgt (Abbildung 1). Der Roboter befindet sich in einer bestimmten Umgebung. Diese muss er im ersten Schritt wahrnehmen, damit er sich in dieser im zweiten Schritt lokalisieren kann. Wenn der Roboter weiß, wo er ist und wo sich das Ziel befindet, kann er im dritten Schritt einen Pfad von seiner aktuellen Position zum Ziel planen. Hat er einen Pfad gefunden, muss er nun diesem im vierten und letzten Schritt folgen. Sprich, er bewegt sich entlang des Pfades Richtung Ziel. Hat er das Ziel erreicht, kann er die Universität Koblenz-Landau, Institut für Informatik fkathe@uni-koblenz.de

2 2 F. Kathe Bewegung Pfadsuche Umgebung Lokalisierung Wahrnehmung Abbildung 1 Ablauf der Pfadplanung eines Roboters. neue Umgebung wiederum wahrnehmen und der Ablauf beginnt von Neuem. Um den eben beschriebenen Ablauf zu realisieren, braucht der Roboter bestimmte Mechanismen. Die Wahrnehmung der Umgebung erfolgt mittels Sensoren. Sensoren können physikalische oder chemische Größen messen und wandeln diese in elektrische Signale um, damit der Roboter die Größen verarbeiten kann. So misst zum Beispiel ein Laserscanner die Distanz zu seiner Umgebung, um dem Roboter zu ermöglichen, eine lokale Karte zu erstellen. Mittels dieser Karte und anderen Sensoren, wie zum Beispiel eines GPS-Empfängers, kann sich der Roboter dann lokalisieren. Hat er sich lokalisiert, kann er daraufhin einen Weg zum Zielpunkt planen. Dieses funktioniert ausschließlich mit Algorithmen, die in Abschnitt 2 erläutert werden. Ist es möglich, zum Ziel zu gelangen, bewegt sich der Roboter daraufhin mittels Aktoren in Richtung Ziel. Diese bilden das Gegenstück zu Sensoren - sie wandeln elektrische Signale des Roboters in Energieformen um, damit der Roboter seine Form, Position oder Orientierung verändern kann. Als Beispiele sind hier der Antrieb eines Roboters oder die Lenkung zu nennen. Aber auch ein Greifarm ist ein Aktor, da dieser die Umgebung des Roboters verändern kann. 1.2 Räume & Systeme Die Umgebung, in der sich der Roboter bewegt, ist formal beschrieben als Arbeitsraum. Dieser ist in den meisten Fällen der R 2 oder R 3. Abstrakt beschrieben, bewegt sich der Roboter im R 2 auf einer planaren Fläche, die auch nicht passierbare Bereiche enthält. Im R 3 werden zusätzlich zum Beispiel noch die Höheninformationen mit einbezogen. In höheren Ebenen könnte man sich die Zeit als weitere Dimension denken. Der Roboter soll z. B. zu einer bestimmten Zeit an einem bestimmten Ort sein. Konfiguration beschreibt die Position jedes Punktes im System, also, wo sich das ganze System im Arbeitsraum befindet. Ein einfacher Roboter lässt sich zum Beispiel im R 2 beschreiben mit (x, y, θ), wobei x und y die Position im zweidimensionalen Raum sind und θ die Ausrichtung ist. Der Konfigurationsraum beschreibt den Raum aller möglichen Konfigurationen des Roboters. So bekommt jede Größe eines Systems im Konfigurationsraum eine eigene Dimension.

3 Algorithmen der Bewegungsplanung für nicht-holonome Roboter 3 Freiheitsgrade, kurz DOF (engl. Degrees Of Freedom), geben die Dimension des Konfigurationsraumes an. Dabei unterscheidet man bei den Freiheitsgraden noch zwischen aktiven und effektiven Freiheitsgraden. Die aktiven beschreiben die translatorischen und rotatorischen Bewegungen, wohingegen die effektiven Freiheitsgrade die Dimensonalität der Pose wiedergeben. Deshalb werden die aktiven Freiheitsgerade auch Steuergrößen genannt. Geht man von einem einfachen Roboter mit Differenziallenkung aus, so wären die aktiven Freiheitsgrade gleich Zwei. Der Roboter kann sich vor und zurück bewegen und dabei rotieren. Die effektiven Freiheitsgrade hingegen haben wie im obigen Beispiel drei Dimensionen, die Pose setzt sich zusammen aus zwei Positionen auf der x- und y-achse und einer Orientierung θ. Hat ein Roboter gleich viele oder mehr aktive Freiheitsgrade als effektive, spricht man von einem holonomen System. Wenn man z. B. die Bewegung des Roboters nimmt, würde das bedeuteten, der Roboter kann sich im Arbeitsraum sowohl in x-richtung, als auch in y-richtung bewegen, ohne dabei seine Orientierung θ zu verändern. Ein nicht-holonomes System hingegen kann dies nicht, da die Anzahl der effektiven Freiheitsgrade höher ist als die der aktiven. Der Roboter mit Differenzialantrieb kann damit nicht von jeder Konfiguration in eine andere übergehen. Möchte er sich z. B. nach links fortbewegen, muss er sich nach Links drehen, bevor er in diese Richtung fahren kann. 1.3 Ackermann-Lenkung Ein solches nicht-holonomes System ist zum Beispiel ein Fahrzeug mit Ackermann- Lenkung [1, S ]. Eine Konfiguration des Systems besteht aus den drei bekannten Parametern x, y und θ, zusätzlich kommt noch ϕ hinzu, das den Lenkeinschlag der vorderen Räder beschreibt. Bei einem System mit Ackermann-Lenkung geht man davon aus, dass die hintere Radachse starr ist und sich der Winkel der Räder der vorderen Achse verändern lässt (Abbildung 2). Da sich die orthogonalen Geraden beider Räder im Idealfall in einem Punkt auf Höhe der hinteren Achse im Kreismittelpunkt schneiden, kann das System soweit vereinfacht werden, dass mittig auf der Vorderachse ein virtuelles Rad gesetzt wird. Den Winkel dieses Rades nennt man Ackermann-Winkel. Abbildung 2 System mit Ackermann-Lenkung, x 1 entspricht x, x 2 = y, x 4 = θ und x 3 entspricht ϕ. L beschreibt den Abstand zwischen Vorder- und Hinterachse und r min ist die Distanz von der Mitte der Hinterachse zum Rotationscenter. Quelle: [1, S. 451]

4 4 F. Kathe Er entspricht ϕ. Die Position der Hinterachse ist (x, y) und die Ausrichtung des Systems ist θ. Als Steuergrößen v 1 erhält das System die Geschwindigkeit, mit der es sich bewegt. Die Steuergröße v 2 beschreibt die Veränderung des Lenkeinschlages. Dabei ist die Bewegung abhängig von der Orientierung und diese ist wiederum abhängig von dem Lenkeinschlag. Somit besitzt dieses System die nicht-holonome Eigenschaft. Im nächsten Kapitel wird erklärt, worauf es bei nicht-holonomer Pfadplanung zu achten ist und es werden Algorithmen der Pfadplanung vorgestellt. 2 Algorithmen der Pfadplanung Bevor in diesem Kapitel die Algorithmen vorgestellt werden, wird der Frage nachgegangen, woran man bewerten kann, dass ein Verfahren bzw. der gefundene Pfad gut ist. Mit folgenden vier Kriterien lassen sich die Algorithmen bewerten: Als erstes sollte der Algorithmus terminieren, sprich, er soll nicht unendlich lange weiterlaufen, wenn er zum Beispiel keinen Pfad findet. Als Resultat soll der Algorithmus entweder einen gefunden Pfad zurück geben oder keinen. Der Algorithmus soll vollständig sein. Der Algorithmus soll einen Pfad finden, wenn es in der realen Welt eine Pfad vom Startpunkt zum Zielpunkt gibt. Dabei sollte der Algorithmus wenn möglich, optimal sein. Der gefundene Pfad sollte der kürzeste Pfad von Start- zum Zielpunkt sein. Schließlich sollte der Algorithmus dabei auch schnell sein. Also die Berechnung des Pfades sollte in annehmbarer Zeit erfolgen. Ein Algorithmus, der mehrere Stunden zur Berechnung braucht, ist nicht geeignet, wenn für ein Auto bei voller Fahrt ein Pfad zum Ausweichen eines Objektes berechnet werden soll. Pfade für Systeme mit nicht-holonomer Eigenschaft benötigen eine komplexere Pfadplanung als Pfade für holonome Systeme (Abbildung 3). Da das nicht-holonome System mindestens einen Steuergröße weniger hat als aktive Freiheitsgerade, entstehen Abhängigkeiten, die aufgelöst werden müssen. Im Gegensatz dazu kann bei holonomer Pfadplanung einfach der Konfigurationsraum aufgespannt werden und darin ein Pfad gesucht werden. Dies ist im holonomen System möglich, da dort im Konfigurationsraum von einer Konfiguration aus alle benachbarten Konfigurationen erreicht werden können. 2.1 Stand der Forschung Im Laufe der Zeit sind mehrere Verfahren zur Pfadplanung entwickelt worden. Grob unterteilen kann man diese in probabilistische und deterministische Verfahren. Deterministisch sind Verfahren, bei denen bei gleichen Voraussetzungen immer auch das gleiche Ergebnis heraus kommt. Probabilistischen Verfahren hingegen arbeiten mit dem Zufall, deshalb kommt bei gleicher Voraussetzung nicht zwangsläufig das gleiche Ergebnis zustande. In [2] werden mehrere solcher Verfahren verglichen. Sowohl probabilistische, als auch deterministische Verfahren werden in [3] vorgestellt. In [4] sind konkret Ansätze der nicht-holonomen Pfadplanung erklärt.

5 Algorithmen der Bewegungsplanung für nicht-holonome Roboter 5 (I) Holonomes System (II) Nicht-holonomes System mit Ackermann-Lenkung Abbildung 3 Geplante Pfade für holonome und nicht-holonome Systeme. Das holonome System 3(I) kann sich in alle Richtungen bewegen und davon unabhängig rotieren. So wird das direkte Fahren zum Ziel ermöglicht. Bei dem nicht-holonome System mit Ackermann-Lenkung 3(II) hingegen ist die Bewegung abhängig von der Orientierung. So muss das System Kurven fahren und erreicht das Ziel nur durch Rangieren. Quelle: [6, S. 2] 2.2 Dubins Pfade Dubins entwickelte als einer der ersten 1957 eine Idee zur Pfadplanung für Fahrzeuge mit Ackermann-Lenkung [7]. Als System nahm er das einfache Auto, welches bereits in Abschnitt 1.2 und 1.3 vorgestellt wurde. Zusätzlich nahm er an, dass sich das Auto mit einer konstanten Geschwindigkeit u s = 1 fortbewegen würde. Aus dem maximalen Lenkeinschlag ϕ max kann man den minimalen Wenderadius ρ min berechnen, den ein Auto abfahren kann. Ist nun eine Konfiguration gegeben, in der sich das Auto befindet, ergeben sich daraus drei Bewegungsmöglichkeiten, die ausgeführt werden können: Gerade aus fahren Kurve nach Links mit maximalem Einschlagwinkel ϕ max fahren Kurve nach Rechts mit maximalem Einschlagwinkel ϕ max fahren Hat man nun eine Start- und Zielkonfiguration gegeben, lässt sich - in einem Arbeitsraum ohne Hindernisse - ein Pfad finden, der eine Kombination aus den drei Bewegungsmöglichkeiten ist. Dubins hat dabei bewiesen, dass die folgenden sechs Bewegungsabläufe ausreichen, um mindestens einen Pfad von Start nach Ziel zu finden: {LRL, RLR, LSL, LSR, RSL, RSR} (1) mit L = Linkskurve, R = Rechtskurve und S = Gerade. Möchte man nun den kürzesten Pfad von Startkonfiguration zu Zielkonfiguration finden, spannt man um diese jeweils zwei Kreise mit dem Radius ρ min, die sich im Konfigurationspunkt (x, y) schneiden und orthogonal zur Orientierung θ sind. Wenn die Konfigurationen weit genug auseinander sind und die Kreise haben keine Schnittpunkte, kann man die Tangenten zwischen den zwei Kreisen der Startkonfiguration und den zwei Kreise der Zielkonfiguration ziehen. Die je zwei Kreise beschreiben die Kurven, die man von dem jeweiligen Punkt aus fahren kann, entweder eine Rechts- oder

6 6 F. Kathe Abbildung 4 Pfade von M (links) zu P (rechts) nach Dubins und Reeds und Sheep. Durchgehend sind die vier Pfade, die nach der Methode von Dubins berechnet werden. Der kürzeste Pfad dabei wäre RSR. Gestrichelt ist der kürzeste Pfad nach Reeds und Shepp gekennzeichnet ( RSR). Dieser wäre in diesem Beispiel noch kürzer als der kürzeste Dubins Pfad. Linkskurve. Dabei ist darauf zu achten, dass nur die Tangenten zwischen den Kreisen valide sind, bei denen die Fahrtrichtung gleich bleibt. Daraus resultieren vier mögliche Pfade, wie man von Start- zur Zielkonfiguration kommt. Misst man nun die Länge der einzelnen Pfade, kann man bestimmen, welcher der kürzeste Pfad ist (Abbildung 4). Schneiden sich die Kreise von Start- und Zielkonfiguration, muss man einen dritten Kreis mit ρ min erzeugen, der beide anderen schneidet. Dieser Pfad ergibt dann die Bewegungsabläufe LRL und RLR. Reeds und Shepp haben die Methode von Dubins so erweitert, dass das Dubins-Auto auch rückwärts fahren kann [8]. Dafür fügten sie eine neue Bewegung Richtungswechsel hinzu. So ist es möglich, dass zwei Tangenten zwischen zwei Kreisen valide sind, die nicht die selbe Kurvenrichtung haben, und es zu kürzeren Wegen kommen kann. Abbildung 4 zeigt, dass der Weg, der rückwärts gefahren wird, um von der Start- zur Zielkonfiguration zu kommen, deutlich kürzer ist, als der Weg, bei dem man vorwärts fährt, da man keine langen Kurven fahren muss. Enthält der Pfad von Dubins keine Geraden, lässt sich auch hier eventuell ein kürzerer Pfad durch Richtungswechsel finden. Sind zum Beispiel Start- und Zielkonfiguration nah beieinander, aber die Orientierung ist entgegengesetzt, ist es schneller zurück zu setzen und zu wenden, als einen fast kompletten Kreisbogen vorwärts zu fahren. 2.3 Potenzialfelder Potenzialfelder sind deterministische Verfahren. Dies bedeutet, sie das Ergebniss der Analyse des kompletten Konfigurationsraum. Dabei bilden die Potenzialfunktion U(q) des Potenzialfeldes eine beliebige Dimension R m des Arbeitsraums in eine reelle Zahl aus R ab. So entstehen Energie- und Kraftfelder, die sich leichter analysieren lassen und so schneller ein Pfad von der Startkonfiguration zu einer Zielkonfiguration finden [1, S ]. Nutzen lässt sich dieses Verfahren, indem man annimmt, dass

7 Algorithmen der Bewegungsplanung für nicht-holonome Roboter 7 (I) Distanzen zu Hindernissen (II) Distanzen zum Ziel Abbildung 5 Zwei Potenzialfelder der gleichen Umgebung mit unterschiedlichen Potenzialfunktionen. Mit Schwarz wurden die Hindernisse markiert, das S im grünen Feld zeigt die Startkonfiguration an, das G im gelbem Feld die Zielkonfiguration. Das Potenzialfeld 5(I) zeigt in jedem Feld die Distanz zum nächsten Hindernis, Potenzialfeld 5(II) zeigt je Feld die Distanz bis zur Zielkonfiguration G. Bei beiden Potenzialfunktionen wurde die 8er-Nachbarschaft verwendet. die Zielkonfiguration den Roboter anzieht und Hindernisse den Roboter abstoßen. Die Summe U sum(q) = U goal (q) + U obstacles (q) (2) zeigt so dem Roboter den Weg zum Ziel. Dabei ist zu beachten, dass für das nachfolgend erklärte Verfahren die Holonomie-Eigenschaften des Fahrzeugs nicht beachtet werden. Um die Funktionen bzw. die Kräfte zu berechnen, nutzt man z. B. für U obstacles den Buschfeuer-Algorithmus (engl. bushfire). Für eine einfache, zweidimensionale Umgebung erhält der Algorithmus als Eingabe ein zweidimensionales Array, das als Eintrag 0 hat, wenn sich kein Hindernis an dieser Stelle befindet und 1, wenn sich dort ein Hindernis befindet. Als Ausgabe soll der Algorithmus ein weiteres, zweidimensionales Array liefern, welches an jeder Stelle im Array beschreibt, wie weit dieses Feld vom nächsten Hindernis entfernt ist. Um dies zu erreichen nimmt im ersten Schritt der Algorithmus alle Nachbarn von den Feldern, die im Eingabe-Array als Hindernis gekennzeichnet sind und weist diesen Feldern im Ausgabe-Array den Wert 1 zu, was bedeutet, dass diese Felder eine Einheit von einem Hindernis entfernt sind. Im zweiten Schritt nimmt der Algorithmus alle Nachbarn von den eben behandelten Feldern, die noch nicht gefüllt sind, und weist diesen wiederum den Wert 2 im Ausgabe-Array zu. Diese Schritte werden solange wiederholt, bis das Ausgabe-Array vollständig gefüllt ist. So hat man zum Schluss eine Karte der Umgebung und weiß für jede Position, wie weit diese von dem nächsten Hindernis entfernt ist. Als Nachbarschaft kann man hier entweder die 4er- oder 8er-Nachbarschaft benutzen. Analog zu diesem Algorithmus lässt sich ein ähnlicher nutzen, um die Distanz zur Zielkonfiguration zu berechnen. Um die Werte der beiden Potenzialfelder kombinieren zu können, muss man die Werte nach den Bedürfnissen anpassen. So ist es in diesem Beispiel sinnvoll, dass der Roboter möglichst weit um Hindernissen navigiert. Daher sind hohe Werte in der Potenzialfeldfunktion der Hindernisse zu favorisieren, eine Möglichkeit wäre die Funktion U obstacles (q) = (q max + 1) U obstacles distance (q) (3) wobei q max der maximale Wert innerhalb des Potenzialfeldes ist und U obstacles distance (q) die entsprechenden Werte aus Abbildung 5(I). Für das Po-

8 8 F. Kathe tenzialfeld U goal (q) für die Zielkonfiguration kann man einfach den Wert q nehmen. Aus der Addition der beiden Werte ergibt sich dann das kombinierte Potenzialfeld. Um nun den Pfad von einer Startkonfiguration zur Zielkonfiguration zu ermitteln, muss man den negativen Gradienten der einzelnen Felder folgen. Man kann den Gradienten annähern, indem man zunächst die Ableitung f x in x-richtung berechnet und danach die Ableitung f y in y-richtung, wobei f den Ort des Punktes im Raum beschreibt. Der Gradientenwinkel lässt sich dann wie folgt berechnen: ( f ) y Θ = arctan (4) f x Hat man den Winkel berechnet, schaut man, wo der negative Winkel hin zeigt. Dort ist das nächste Feld des Pfades. Man nimmt den negativen Winkel, da Gradienten immer zum Maximum zeigen, bei Potenzialfelder aber nach dem Minimum gesucht wird. Um den Pfad von einer Startkonfiguration zu einer Zielkonfiguration zu berechnen, setzt man als ersten Punkt des Pfades die Startkonfiguration. Dann schaut man, wohin der Gradientenwinkel des Feldes zeigt und nimmt dieses Feld als nächsten Punkt des Pfades. Dies macht man so lange, bis man an der Zielkonfiguration angekommen ist (Algorithmus 1). Algorithmus 1 Berechnung des Pfades mit Potenzialfeldern. q entspricht dem Pfad und U dem Gradientenwinkel. q(0) q start i 0 while U(q(i)) 0 do q(i + 1) q(i) + U(q(i)) i i + 1 end while Daraus ergibt sich ein Pfad, dem man nun folgen kann (Abbildung 6). Visuell kann man sich die Gradientfunktion auch so vorstellen, als ob die Werte der Potenzialfunktion pro Feld die Höhe des Feldes repräsentieren. In dieses Gebirge kann man dann eine Kugel auf die Startkonfiguration setzen und diese würde entsprechend des Pfades Zielkonfiguration hinunter rollen, bis sie an der Zielkonfiguration angekommen ist. Da diese das lokale Minimum bildet, kann sich die Kugel von dort nicht mehr weg bewegen. Ein weiteres deterministisches Verfahren sind die Roadmaps. Dabei wird der freie Bereich des Arbeitsraums z. B. in Quadtrees unterteilt. Dadurch entsteht ein Graph des freien Bereichs, mittels dem man einen Pfad von Start- zu Zielposition finden kann. In [5] wird ein solches Verfahren beschrieben. 2.4 Probabilistische Pfadplanung Im Gegensatz zu den deterministischen Verfahren analysieren die probabilistischen Verfahren aus Effizienzgründen nicht den ganzen Konfigurationsraum. Sie analysieren nur eine bestimmte Menge an zufällig ausgewählten Punkten. Diese Verfahren sind dadurch nicht vollständig, allerdings sind sie probabilistisch vollständig. Das bedeutet, je länger das Verfahren läuft, umso wahrscheinlicher ist es, dass das Verfahren eine

9 Algorithmen der Bewegungsplanung für nicht-holonome Roboter 9 Abbildung 6 Das kombinierte Potenzialfeld der beiden Potenzialfelder aus Abbildung 5(I) und 5(II) nach Formel 3. Orange eingezeichnet sind die einzelnen Gradientenwinkel pro Feld des Pfades von Startkonfiguration zu Zielkonfiguration. Lösung findet Probabilistic Roadmap Method Eines der Standardverfahren der probabilistischen Pfadplanung ist die Probabilistic Roadmap Method, kurz PRM, [6]. Dieses Verfahren streut zunächst eine bestimmte Anzahl von Konfigurationen im Konfigurationsraum. Diese werden mittels eines lokalen Planers verbunden. Es werden die Nachbarschaften ermittelt und so entsteht ein Netz von Konfigurationen. Dieses Netz entspricht einem Graphen, jede Verbindung zwischen zwei Konfigurationen erhält ein Gewicht. Hat man eine Start- und Zielkonfiguration gegeben, kann man diese zwei Konfigurationen auch mit dem Graphen verbinden. Nun ist es möglich, zum Beispiel mittels Dijkstra-Algorithmus eine Abfolge von Konfigurationen zu finden, die einen Pfad von der Startkonfiguration zur Zielkonfiguration bilden. Der Graph R setzt sich zusammen aus einer Menge an Konfigurationen N und einer Menge an Verbindungen E. Eine Verbindung e besteht aus zwei Konfigurationen {a, b} N und einem Gewicht. Der Algorithmus erzeugt, solange noch nicht die maximale Anzahl an Konfiguration gestreut wurden, eine neue, zufällige Konfiguration. Ist dies valide, sprich, liegt die Konfiguration im freien Bereich des Konfigurationsraumes, wird sie der Menge der Konfigurationen N in R hinzugefügt. Danach werden alle Nachbarkonfigurationen von der Konfiguration ermittelt und überprüft, ob die Verbindung zwischen der Konfiguration und der Nachbarkonfiguration auch im freien Konfigurationsraum liegt. Ist dem so, wird auch diese Verbindung E in R hinzugefügt (Algorithmus 2). Abbildung 7 zeigt einen solchen Graph, überlagert mit dem Konfigurationsraum. Bei einer zufälligen, gleich verteilten Streuung der Konfigurationen ist zu sehen, dass in Bereichen, die frei sind, sprich in deren Nähe sich keine Hindernisse befinden, viele Konfigurationen gestreut werden. In Bereichen, die viele Hindernisse in der Nähe haben, wie zum Beispiel der Engpass unten rechts in der Abbildung werden hingegen nur wenige Konfigurationen gestreut. So kann es passieren, dass kein Pfad durch solche Engpässe gefunden wird und ein viel längerer Pfad gewählt werden muss. Aller-

10 10 F. Kathe Algorithmus 2 PRM -Algorithmus mit N max Anzahl der gestreuten Konfigurationen, N random eine zufällig gestreute Konfiguration, F der freie Raum im Konfigurationsraum und n neighbors alle Konfigurationen in der Nachbarschaft von Konfiguration n. n count 0 while n count N max do n N random if n F then N = N n for all m n neighbors do if {n, m} F then E = E {n, m} end if end for end if end while dings kann dieses Porblem mit Anpassung von Parametern oder durch eine veränderte Streuung der Konfigurationen entgegengewirkt werden. In Abschnitt 2.5 wird eine solche Streuung vorgestellt Customizing-PRM Da die resultierenden Pfade des PRM -Algorithmus sich nicht direkt für Fahrzeuge eignen, bedarf es Anpassungen, damit der Algorithmus bessere Pfade für nicht-holonome Systeme generiert. Song und Amato erweiterten dafür den PRM-Algorithmus um eine weitere Ebene zum Customizing-PRM -Algorithmus, kurz C-PRM, [9]. Dieser nutzt den Graphen R des PRM -Algorithmus und formt daraus einen Pfad mit glatten Übergängen zwischen den Knoten. Dafür nutzt der Algorithmus die quadratischen Bézier-Kurve, kurz B-Splines. Ein B-Spline wird durch drei Punkte definiert, einem Startpunkt P 0, einem Hilfspunkt P 1 und einem Endpunkt P 2. Er geht durch Start- und Endpunkt, wobei er am Startpunkt die Ausrichtung von Start- zu Hilfspunkt und am Endpunkt die Ausrich- Abbildung 7 PRM -Graph mit 200 zufällig gestreuten Konfigurationen (dunkelgrün) und die Nachbarschaftsfunktion (hellgrün) der euklidische Distanz von maximal 100. Die Karte hat dabei eine Auflösung von 800 zu 600. Rot markiert sind die Start- und Zielkonfiguration, sowie der kürzeste Pfad, der die beiden miteinander verbindet.

11 Algorithmen der Bewegungsplanung für nicht-holonome Roboter 11 1 P1 0,75 P0 P2 0,5 P1 0,25 P0 P2 0 0,1 0,2 0,3 0,4 0,5 0,6 0,7 0,8 0,9 1 (I) Bézier-Kurve (II) Gewichtungsfunktion Abbildung 8 Bézier-Kurve 2ten Grades mit der entsprechenden Gewichtungsfunktion. In 8(I) sind blau markiert die drei Punkte: P 0 ist Startpunkt, P 2 Endpunkt. Grau markiert sind die Verbindungen zwischen den Punkten und Rot markiert ist der entsprechende B-Spline. 8(II) stellt die Gewichtungsfunktion dar,die den Einfluss der drei Punkte auf den B-Spline anzeigt. Dabei steht die x-achse für die Zeit [0, 1] und die y-achse symbolisiert den Einfluss auf den Spline mit [0, 1], wobei 1 voller Einfluss bedeutet. tung von Hilfs- zu Endpunkt hat. Durch die Gewichtungsfunktion P (t) = (1 t) 2 P t (1 t) P 1 + t 2 P 2 (5) entsteht der glatte Übergang von Start- zu Zielpunkt (Abbildung 8). Die glatten Kreisübergänge eignen sich besser für nicht-holonome Systeme. Um nun einen Pfad des PRM -Algorithmus in einen Pfad mit glatten Übergängen zu transformieren, werden zuerst die Mittelpunkte aller Verbindungen in E zu neuen Konfigurationen in N custom in einem zweiten Graphen R custom. Die Nachbarschaft in diesem Graphen ist definiert durch den ersten Graphen. Zwei Konfigurationen aus N custom sind miteinander verbunden, wenn die entsprechenden Nachbarschaften in E, aus denen die Konfigurationen in N custom hervor gehen, mit einem gleichen Knoten aus N verbunden sind. Dann gibt es eine Nachbarschaft e in E custom. Der Verbindungsknoten aus N dient auch als Hilfspunkt für die Spline-Bildung. Nun kann man daraus das Netz erstellen und mittels Graphensuche den kürzesten Pfad zwischen Start- und Zielkonfiguration finden. 2.5 Geschwindigkeitsplanung Ziel der Geschwindigkeitsplanung ist es, für ein System nicht nur den Pfad von einer Startkonfiguration zu einer Zielkonfiguration zu planen, sondern auch eine Geschwindigkeit, mit der sich das System während des Pfades bewegt. Dadurch wird ermöglicht, dass sich das System schneller bewegen kann. Gibt es keine Geschwindigkeitsplanung, muss das System mit der Minimalgeschwindigkeit fahren, da es z. B. Kurven nicht schnell fahren sollte. Besitzt das System hingegen eine Geschwindigkeitsplanung, kann es auf Geraden schnell fahren und vor Kurven bremsen, um die Kurven mit einer langsamereren Geschwindigkeit zu fahren. In [10] wird eine solche Geschwindigkeitsplanung im Zusammenspiel mit der Pfadplanung für ein nicht-holonomes Personen-Transportfahrzeug vorgestellt. Die Geschwindigkeitsplanung nutzt dabei den Pfad einer probabilistischen Pfadplanung, ist selbst aber deterministisch, es wird also immer das gleiche Geschwindigkeitsprofil für einen Pfad berechnet.

12 12 F. Kathe Gegeben sind Start- und Zielpunkt des Fahrzeugs, der maximale Lenkeinschlag, den das Fahrzeug ausführen kann und das Geschwindigkeitsprofil des Fahrzeugs, sprich die maximale Beschleunigung und die Höchstgeschwindigkeit. Gesucht ist ein Pfad vom Start- zum Zielpunkt, wobei der Pfad keine Hindernisse berühren darf. Dabei sollte der Pfad die nicht-holonomen Eigenschaften berücksichtigen, damit das Fahrzeug den Pfad abfahren kann. Zusätzlich sollte sowohl der Pfad als auch die Geschwindigkeitsänderungen für den Menschen angepasst sein, starke Beschleunigungen oder abrupte Richtungsänderungen sollten vermieden werden. Außerdem sollte dabei das Vehikel schnellstmöglich vom Startpunkt zum Zielpunkt kommen. Für die Pfadplanung wird ein angepasster PRM -Algorithmus verwendet. Anstatt die Konfigurationen rein zufällig zu streuen, wird der Konfiguratuionsraum in Bereiche unterteilt. In freien Bereichen, sprich, wo nicht viele Hindernisse vorhanden sind, und in kritische Bereiche, in denen sich viele Hindernisse befinden bzw. in denen nur ein schmaler, freier Bereich vorhanden ist. In diesen Bereichen werden mehr Konfigurationen gestreut, als in den freien Bereichen. Zusätzlich werden dort, wo bereits viele Konfigurationen gestreut wurden, nur noch wenige weitere Konfigurationen gestreut. Damit erreicht man, dass Konfigurationen über den gesamten freien Konfigurationsraum gestreut werden und jeder Bereich erreichbar ist. Zusätzlich wird als Distanzfunktion zur Nachbarschaftberechnung nicht die einfache, euklidische Distanz genutzt, sondern die in Abschnitt 2.2 beschriebenen Dubins-Pfade. Gibt es einen Dubins-Pfad zwischen zwei Konfigurationen, der sich komplett im freien Raum befindet, so sind die zwei Konfigurationen miteinander benachbart. Für die Gewichtung der Kanten bei der Pfadberechnung dient die Fourier-Transformation der Bogenlänge der Dubins-Pfade als Gewicht. Damit erreicht man, dass geringe Lenkbwegungen bevorzugt werden und man insgesamt eine sanftere Fahrt hat. Die Geschwindigkeitsplanung baut auf der Pfadplanung auf. Es wird der berechnete Pfad in verschiedene Abschnitte unterteilt, denen jeweils eine Geschwindigkeit zugeordnet wird. Ist die Bogenlänge eines Abschnittes des Pfades unter einem zuvor definiertem kritischen Wert, so wird dieser Abschnitt als Gerade angesehen und man kann dort die Geschwindigkeit V max fahren. Dies bedeutet, solange V max nicht erreicht wurde, beschleunigt das Fahrzeug weiter. Ist die Bogenlänge des Abschnitts über dem kritischen Wert, so wird die passende Geschwindigkeit berechnet, mit der das Fahrzeug maximal den Abschnitt abfahren kann. Ist das Fahrzeug schneller als diese Geschwindigkeit, so wird es, bevor es diesen Abschnitt erreicht, schon auf die Geschwindigkeit herunter gebremst. So lässt sich einfach ein Geschwindigkeitsprofil für einen Pfad berechnen, dem das Fahrzeug folgen kann. In Abbildung 9 wurde das Verfahren mit realen Fahrern verglichen. Dabei ist zu sehen, dass das Verfahren eine sanfterere Fahrweise bietet, allerdings noch zu zaghaft ist, also bei Stellen abbremst, bei denen der Mensch normal weiter fahren würde. 3 Fazit 3.1 Resultate Im Vergleich der in Abschnitt 2 vorgestellten Verfahren zur Pfadplanung lässt sich sagen, dass alle ihre Vor- und Nachteile haben. Das Verfahren von Dubins und die Erweiterung dazu von Reeds und Shepp (Abschnitt 2.2) ist vollständig, sprich, wenn

13 Algorithmen der Bewegungsplanung für nicht-holonome Roboter 13 (I) Gefahrener Weg (II) Geschwindigkeitsdiagramm Abbildung 9 Die zwei Graphen zeigen einen mit dem in Abschnitt 2.5 gezeigtem Verfahren geplanten Pfad (Abbildung 9(I)) und die dazugehörige Geschwindigkeitsplanung (Abbildung 9(II)). Verglichen wurden das Verfahren (PSP) mit realen Personen, die die selbe Strecke abgefahren sind. AD steht dabei für Advanced Driver, MD für Medium Driver und SD für Saftey Driver. Quelle: [10, S. 262] es mindestens einen Pfad von Startkonfiguration zu Zielkonfiguration im Arbeitsraum gibt, dann findet dieser Algorithmus auch einen Pfad. Zusätzlich ist das Verfahren optimal für das Dubins Auto, der gefundene Pfad ist also der kürzeste von Start- zu Zielkonfiguration mit den Möglichkeiten, die das Verfahren von Dubins bietet. Nachteil dieses Verfahren ist z. B., dass das Dubins Auto immer mit maximalem Lenkeinschlag fährt und es keine Übergänge von gerader Strecke in Kurven gibt. So muss das Dubins Auto vor jedem Übergang stehen bleiben und dann voll die Lenkung einschlagen, bevor es weiterfahren kann. Potenzialfelder (Abschnitt 2.3), die nur die Distanz zum Ziel berechnen, sind ebenfalls vollständig und optimal. Dabei zu beachten ist, dass es immer auf die Auflösung ankommt. Ist die Rasterung des Konfigurationraumes zu grob gewählt, kann es sein, dass mögliche Verbindungen zwischen freien Flächen verloren gehen und dadurch kein Pfad mehr zwischen Start- und Zielkonfiguration gefunden werden kann. Da in den meisten Fällen nur das Potenzialfeld mit der Distanzberechnung zum Ziel nicht ausreicht, muss zusätzlich noch die Distanzberechnung zu den Hindernissen mit einberechnet werden. Dabei kann es passieren, wenn z. B. die Gewichtung für die Vermeidung von Hindernissen zu groß ist, lokale Minima entstehen, aus denen es keinen Ausweg mehr gibt. Lokale Minima sind Punkte im Potenzialfeld, die, genau wie das Zielfeld, die kleinsten Wert in ihrer Nachbarschaft besitzen. Dadurch ist das System auf diesen Feldern gefangen,da es auf kein anderes Feld in der Nachbarschaft mehr zeigen kann. Somit ist das Potenzialfeld mit Zielfunktion und Hindernisfunktion nicht mehr vollständig. Der im Bereich der probabilistischen Verfahren (Abschnitt 2.4) vorgestellte Algorithmus PRM ist probabilistisch abgeschlossen, aber nicht optimal. Dies ist damit zu begründen, dass durch das zufällige Streuen von Konfigurationen nicht davon ausgegangen werden kann, dass immer der optimale Pfad zwischen Start- und Zielkonfiguration gefunden wird. Die eben beschriebene Vergleiche der Verfahren sind auch in Tabelle 1 zu finden. Zusammenfassend kann man sagen, dass sich im Vergleich von deterministischen und probabilistischen Verfahren die probabilistischen in Bezug auf die nicht-holonome Pfad-

14 14 F. Kathe Verfahren Vollständigkeit Optimal Reeds & Shepp vollständig optimal (für Dubins Auto) Potenzialfelder Distanz zum Ziel vollständig (abhängig von Auflösung) optimal (abhängig von Auflösung) & zu Hindernissen nicht vollständig (lokale Minima) optimal (abhängig von Auflösung) PRM probabilistisch vollständig nicht optimal Tabelle 1 Vergleich der im Paper vorgestellten Verfahren. planung durchsetzen. Sie berechnen den Pfad deutlich schneller und lassen sich leichter modifizieren. 3.2 Ausblick Als Vorteil der hier vorgestellten Algorithmen lässt sich in erster Linie hervorheben, dass sie sich aus mehreren Bestandteilen zusammensetzen. So haben die Potenzialfelder eine Funktion zur Berechnung der Distanz zum Ziel und eine für die Hindernisse, der PRM -Algorithmus hat u. a. einen Teil zum Streuen der Konfigurationen und einen für die Berechnung der Nachbarschaften. Dadurch wird es möglich, die Verfahren nach den eigenen Bedürfnissen anzupassen und machen sie flexibel. Allerdings waren diese Verfahren zuerst für die Pfadplanung für holonome Systeme gedacht und sie müssen für die nicht-holonomen angepasst werden. Dadurch ist gegeben, dass die Verfahren für nicht-holonome Systeme nicht zwangsläufig den optimalsten Pfad finden. Direkte nicht-holonome Pfadplanung ist aber eine sehr komplexes Verfahren, bei dem es schnell zu vielen Dimensionen kommen kann. Deswegen wird sich heute mit der holonomen Pfadplanung geholfen, sie ermöglicht die Pfadplanung in deutlich weniger Dimensionen. Der PRM -Algorithmus eigent sich besser als die Potenzialfelder für Pfadplanung, da er nicht das Problem der lokalen Minima hat, aber dafür gibt es hier Probleme beim Streuen von Konfigurationen im engen, freien Raum. Es wird ein besser Ansatz für das Streuen benötigt, als das zufällige Streuen. Ein Beispiel dafür ist die Methode in Abschnitt 2.5. Für das Problem der lokalen Minima haben Conner et al. in [11] und [12] eine Lösung vorgestellt. Sie vermeiden lokale Minima, indem sie den freien Raum in konvexe Teilbereiche aufteilen, da konvexe Flächen keine lokalen Minima besitzen. Diese werden als Graph repräsentiert. Nun kann dort einen Pfad von Fläche zu Fläche gesucht werden, der Start und Ziel miteinander verbindet. Danach erst wird die Potenzialfunktionen auf den einzelnen Flächen ausgeführt. Die in Abschnitt 2.5 vorgestellte Geschwindigkeitsplanung hat den Nachteil, dass sie nur auf dem berechneten Pfad aufbaut. Ein Zusammenspiel von Pfad- und Geschwindigkeitsplanung wäre wünschenswert, dann könnte man z. B. überlegen, ob bei der Planung des Pfades mehr darauf geachtet werden soll, ob das Fahrzeug den kürzesten Pfad fahren soll oder ob er den Pfad wählen soll, der zeitlich am schnellsten zu fahren ist. Alles in allem ist zu sagen, dass es im Bereich der Pfadplanung für nicht-holonome Fahrzeuge schon gute Fortschritte gibt, der Spielraum für Optimierungen ist aber auch noch groß.

15 Algorithmen der Bewegungsplanung für nicht-holonome Roboter 15 Literatur 1. Howie Choset, Kevin M. Lynch, Seth Hutchinson, et al.: Principles of Robot Motion; 2005; in MIT Press; ISBN: Abraham Sánchez López, René Zapata, Maria A. Osorio Lama: Sampling-Based Motion Planning: A Survey ; 2008; in Computación y Sistemas Vol. 12 No. 1, 2008 ; C. Goerzen, Z. Kong, B. Mettler: A Survey of Motion Planning Algorithms from the Perspective of Autonomous UAV Guidance; 2009; in Journal Intell Robot System (2010), 57 ; Jean-Paul Laumond, et al.: Robot Motion Planning and Control; 1998; in Springer; ISBN: Behnke, S.: Local multiresolution path planning; 2004; in: Proceedings of 7th RoboCup International Symposium; Petr Svestka and Mark H. Overmars: Motion Planning for Carlike Robots Using a Probabilistic Learning Approach; 1997; in The International Journal of Robotics Research Vol. 16, No. 2 ; L.E. Dubins: On curves of minimal length with a constraint on average curvature, and with prescribed initial and terminal positions and tangents; 1957; in American Journal of Mathematics, 79(3), 1957, J. A.Reeds, L. A. Shepp: Optimal Paths for a Car that goes both Forwards and backwards; 1990; in PACIFIC JOURNAL OF MATHEMATICS Vol. 145, No. 2, 1990 ; Guang Song and Nancy M. Amato:Randomized motion planning for car-like robots with c-prm; 2001; in emphproc. IEEE/RSJ Int. Conf. Intelligent Robots and Systems (IROS 01); Jorge Villagra, Vicente Milanés, Joshué Pérez, Jorge Godoy: Smooth path and speed planning for anautomated public transport vehicle; 2011; in Robotics and Autonomous Systems 60 ; David C. Conner, Alfred Rizzi, and Howie Choset: Composition of local potential functions for global robot control and navigation; 2003; in IEEE/RSJ International Conference on Intelligent Robots and Systems; David C. Conner, Howie Choset, and Alfred Rizzi:Integrated Planning and Control for Convex-bodied Nonholonomic Systems Using Local Feedback; 2006; in Proceedings of Robotics: Science and Systems II ; 57-64