Wegeplanung: Wegekartenverfahren

Wegeplanung:
Wegekartenverfahren
 
 
 
 
 
Idee
Sichtbarkeitsgraph
Voronoi-Diagramm
Probabilistische Wegekarten
Rapidly-Exploring Random Tree
Prof. Dr. O. Bittel, HTWG Konstanz
Autonome Roboter - Wegekartenverfahren
SS 2016
6-1
Idee
  Spanne hindernisfreien Raum
durch Graphen auf.
  Knoten liegen im hindernisfreien
Raum.
  Benachbarte Knoten (d.h. durch
Kanten verbunden) sind
kollisionsfrei und einfach zu
erreichen
  Graph sollte Arbeitsraum „gut“
aufspannen.
  Wegeplanung reduziert sich
damit auf Verbinden von Startund Endpunkt zu einem
nächstgelegenen Knoten im
Graphen und anschließender
Wegesuche im Graphen.
Prof. Dr. O. Bittel, HTWG Konstanz
Autonome Roboter - Wegekartenverfahren
SS 2016
6-2
Sichtbarkeitsgraph
  Für eine polygonale Hinderniswelt P ist der Sichtbarkeitsgraph S
definiert durch:
-  alle Hindernis-Ecken und Start- und Zielpunkt bilden die Knoten;
-  für Knoten u und v ist (u,v) eine Kante des Sichtbarkeitsgraphen, falls
uv keine Hindernisse schneidet.
Prof. Dr. O. Bittel, HTWG Konstanz
Autonome Roboter - Wegekartenverfahren
SS 2016
6-3
Berechnung des Sichtbarkeitsgraphen
Eingabe:
Algorithmus VisibiltyGraph(P, qStart, qZiel )
V = { Menge aller Ecken aus P } ∪ {qStart, qZiel };
for all v ∈ V do
W = VisibleVertices(v, P);
for all w ∈ W do
füge (v,w) zu E hinzu;
endfor
endfor
Polygonale Hinderniswelt P,
gegeben durch Menge von
Kanten.
Start- und Zielpunkt.
VisibleVertices(v, P) berechnet
alle Knoten, die
von v aus sichtbar sind.
return (V,E);
Ausgabe:
Sichtbarkeitsgraph G = (V,E)
Prof. Dr. O. Bittel, HTWG Konstanz
Autonome Roboter - Wegekartenverfahren
SS 2016
6-4
Rotierendes Sweep-Line-Verfahren (1)
  VisibleVertices(v, P) berechnet alle Knoten, die von v aus sichtbar sind.
  Algorithmus ist ein rotierendes Sweep-Line-Verfahren:
Rotiere Line l um v jeweils zur nächsten Ecke.
  Dabei werden alle Hinderniskanten, die l schneiden, in einer effizienten
Suchstruktur (z.B. balanzierter Suchbaum) gespeichert.
l
v
l
Line l, die um v rotiert
Prof. Dr. O. Bittel, HTWG Konstanz
v
Line l und alle Hinderniskanten,
die l schneiden, in einem Suchbaum.
Autonome Roboter - Wegekartenverfahren
SS 2016
6-5
Rotierendes Sweep-Line-Verfahren (2)
Algorithmus VisibleVertices(v, P)
Eingabe:
Polygonale Hinderniswelt P und Knoten v.
(1)
sortiere alle Ecken aus P nach ihren Polarkoordinaten
bzgl. Knoten v (zuerst nach Winkel und dann nach Abstand);
sortierte Folge sei w1, w2, ..., wn;
(2)
Sei l die Linie von v in Richtung x-Achse (0 Grad);
Speichere alle Hinderniskanten, die l schneiden, in einem Suchbaum T;
(Reihenfolge ergibt sich durch Entfernung zwischen v und Schnittpunkt
der Hinderniskanten mit l )
(3)
W = { };
(4)
for i = 1 to n do
(5)
(6)
(7)
(8)
(9)
(10)
(11)
(12)
(13)
drehe Linie l, so dass sie durch wi geht;
if wi ist von v aus sichtbar then
W = W ∪ {(v,wi)};
if wi ist der Beginn einer oder zweier Hinderniskanten e1,2 ∉T then
füge e1,2 zu T dazu;
if wi ist das Ende einer oder zwei Hinderniskanten e1,2 ∈T then
lösche e1,2 in T;
Ausgabe:
endfor
Menge W alle Knoten, die
return W;
von v aus sichtbar sind.
Prof. Dr. O. Bittel, HTWG Konstanz
Linienbezogene Sichtbarkeitsprüfung (später)
Autonome Roboter - Wegekartenverfahren
SS 2016
6-6
Beispiel zu Sweep-Line-Verfahren
e7
e3
e8
e4
e6
e2
e1
e5
Sweep-Line l
durch Ecke
Suchbaum T
mit Kanten
Ecke
sichtbar
Initialisierung:
Schritt (2)
{e4,e2,e8,e6}
v3
{e4,e3,e8,e6}
v7
{e4,e3,e8,e7}
v4
{e8, e7}
ja
v8
{}
ja
v1
{e1,e4}
ja
v5
{e4,e1,e8,e5}
v2
{e4,e2,e8,e5}
v6
{e4,e2,e8,e6}
Ecken vi sind nach dem Winkel zur
x-Achse sortiert.
l rotiert gegen den Uhrzeigersinn.
Prof. Dr. O. Bittel, HTWG Konstanz
Autonome Roboter - Wegekartenverfahren
SS 2016
6-7
Linienbezogene Sichtbarkeitsprüfung (1)
wi ist nicht sichtbar, falls
(1)  auf der Sweep-Line l liegt links von wi keine Ecke (d.h. wi-1 liegt nicht
auf l), aber es liegt eine Hinderniskante e∈T links von wi.
l
wi
v
Prof. Dr. O. Bittel, HTWG Konstanz
Autonome Roboter - Wegekartenverfahren
SS 2016
6-8
Linienbezogene Sichtbarkeitsprüfung (2)
wi ist nicht sichtbar, falls
(2)  wi-1 liegt auch auf der Sweep-Line l und ist nicht sichtbar oder
(3)  wi-1 liegt auch auf der Sweep-Line l und ist sichtbar, aber zwischen wi-1
und wi liegt eine Hinderniskante.
v
v
l
Prof. Dr. O. Bittel, HTWG Konstanz
Autonome Roboter - Wegekartenverfahren
l
SS 2016
6-9
Linienbezogene Sichtbarkeitsprüfung (3)
wi ist nicht sichtbar, falls
(4)  wi-1 liegt auch auf der Sweep-Line l und wi und wi-1 gehören zum selben
Hindernis (Polygonzug) und die Kante (wi-1,wi) schneidet das Hindernis.
v
l
Prof. Dr. O. Bittel, HTWG Konstanz
Autonome Roboter - Wegekartenverfahren
SS 2016
6-10
Linienbezogene Sichtbarkeitsprüfung (4)
wi ist sichtbar in allen anderen Fällen:
v
v
l
l
v
l
Prof. Dr. O. Bittel, HTWG Konstanz
Autonome Roboter - Wegekartenverfahren
SS 2016
6-11
Laufzeit des Sweep-Line-Verfahrens
Algorithmus VisibleVertices(v, P):
  Zeile (1): Sortieren aller n Ecken in O(n log n)
  Zeile (2): Einfügen in Suchbaum T kostet O(log n); also insgesamt O(n log n)
  for-Schleife (4): n-mal Zugriff auf Suchbaum T; also T(n log n)
Insgesamt: T(n) = O(n log n).
Algorithmus VisibiltyGraph(P, qStart, qZiel)
Insgesamt: T(n) = O(n2 log n).
Prof. Dr. O. Bittel, HTWG Konstanz
Autonome Roboter - Wegekartenverfahren
SS 2016
6-12
Reduzierter Sichtbarkeitsgraph (1)
Reduzierung unnötiger Kanten im Sichtbarkeitsgraph
Alle Kanten, die nicht auf einer „2-Punkt-Tangente“ liegen werden entfernt
Rot: alle 2-PunktTangenten
Blau: Beispiel für
Nicht-Tangente
Prof. Dr. O. Bittel, HTWG Konstanz
Autonome Roboter - Wegekartenverfahren
SS 2016
6-13
Reduzierter Sichtbarkeitsgraph (2)
Nicht-reduzierter und
reduzierter Sichtbarkeitsgraph
Prof. Dr. O. Bittel, HTWG Konstanz
Autonome Roboter - Wegekartenverfahren
SS 2016
6-14
Wegeplanung:
Wegekartenverfahren
 
 
 
 
 
Idee
Sichtbarkeitsgraph
Voronoi-Diagramm
Probabilistische Wegekarten
Rapidly-Exploring Random Tree
Prof. Dr. O. Bittel, HTWG Konstanz
Autonome Roboter - Wegekartenverfahren
SS 2016
6-15
Voronoi-Diagramm
Ein Voronoi-Diagramm VD(P) für eine Menge von Hindernispunkte P teilt die
Ebene in (Voronoi-)Regionen auf, die jeweils alle Punkte enthalten, die zum
gleichen Hindernispunkt am nächsten gelegen sind.
Hindernispunkt
VD mit 6 Regionen.
Prof. Dr. O. Bittel, HTWG Konstanz
VD mit 2 Regionen.
Autonome Roboter - Wegekartenverfahren
SS 2016
6-16
Definition der Voronoi-Region
  Menge von Hindernispunkte P = {p1, p2, ..., pn}
  Voronoi-Region zu einem Hindernispunkt p∈P :
VR(p) = {x∈R2 | dp(x) ≤ dq(x) für alle q∈P mit q ≠ p }
wobei dp(x) der Euklidische Abstand vom Punkt x zu Punkt p ist.
p
VR(p)
Prof. Dr. O. Bittel, HTWG Konstanz
Autonome Roboter - Wegekartenverfahren
SS 2016
6-17
Charakterisierung einer Voronoi-Region als Polygon (1)
  H(p1|p2) = Halbebene von p1 mit p2.
Besteht aus allen Punkten, die näher bei p1 als bei p2 liegen.
  H(p2|p1)
analog definiert.
Prof. Dr. O. Bittel, HTWG Konstanz
Autonome Roboter - Wegekartenverfahren
SS 2016
6-18
Charakterisierung einer Voronoi-Region als Polygon (2)
Für p ∈ P ist die Voronoi-Region VR(p) der Durchschnitt
aller Halbebenen von p mit den anderen Punkten aus P:
VR(p1) ist schaffriert.
Prof. Dr. O. Bittel, HTWG Konstanz
Autonome Roboter - Wegekartenverfahren
SS 2016
6-19
Voronoi-Kanten und -Ecken
•  Grenzlinien zwischen Voronoi-Regionen sind Kanten und werden auch
Voronoi-Kanten genannt. Die Enden der Voronoi-Kanten werden auch
Vornoi-Ecken genannt.
•  Kanten können Strecken, Strahlen oder Geraden sein
•  Oft wird unter einem VD auch die Menge alle Voronoi-Kanten verstanden.
•  Die Voronoi-Ecken sind von 3 Hindernispunkte gleichweit entfernt;
die Punkte einer Voronoi-Kante sind von 2 Hindernispunkten gleichweit
entfernt.
Voronoi-Kante;
alle Punkte der Kante sind von
p und q gleichweit entfernt.
p
r
Prof. Dr. O. Bittel, HTWG Konstanz
q
Voronoi-Ecke ist von
p, q und r gleichweit entfernt.
Autonome Roboter - Wegekartenverfahren
SS 2016
6-20
Sweep-Line-Verfahren
  Bewege horizontale Sweep-Line l von oben nach unten.
  Dabei wird das VD für die oberhalb von l liegende Halbebene bis zur
sogenannten „Beach-Line“ konstruiert.
  Die Beach-Line ist die Menge aller Punkte, die von l und dem nächsten
Punkt aus P gleichweit entfernt sind.
Voronoi-Kanten oberhalb der
Beach-Line sind bekannt.
Beach-Line
Voronoi-Kanten unterhalb der
Beach-Line sind noch
unbekannt.
Prof. Dr. O. Bittel, HTWG Konstanz
Autonome Roboter - Wegekartenverfahren
SS 2016
6-21
Beach-Line
  Die Beach-Line ist die Menge aller Punkte, die von l und dem nächsten
Punkt aus P gleichweit entfernt sind.
  Die Beach-Line besteht aus einer Folge von Parabelstücken. Für ein
Hindernispunkt ist die Beach-Line eine Parabel.
p
d
l
d
Beach
Line
Die Menge alle Punkte, die von
l und p gleicht weit entfernt
sind, bilden eine Parabel.
Prof. Dr. O. Bittel, HTWG Konstanz
Autonome Roboter - Wegekartenverfahren
SS 2016
6-22
Punkt- und Kreis-Ereignisse
Beim Bewegen der horizontalen Sweep-Line l
gibt es zwei Ereignisse:
  Punkt-Ereignis:
⇒ es wird ein neues Parabelstück zur Beach-Line dazugefügt.
  Kreis-Ereignis:
⇒ es wird ein Parabelstück von der Beach-Line entfernt.
Prof. Dr. O. Bittel, HTWG Konstanz
Autonome Roboter - Wegekartenverfahren
SS 2016
6-23
Punkt-Ereignis
Sweep-Line l trifft auf ein Punkt p∈P:
  es wird ein neues Parabelstück zur Beach-Line dazugefügt.
Eine neue Parabel mit
Breite = 0 wird dazugefügt.
Parabel wird breiter.
  Beachte: die Verbindungsstellen
der Parabelstücke bewegen sich
auf den Voronoi-Kanten:
Prof. Dr. O. Bittel, HTWG Konstanz
Autonome Roboter - Wegekartenverfahren
SS 2016
6-24
Kreis-Ereignis (1)
Sweep-Line l trifft auf den unteren Punkt eines Kreises,
der auf 3 Punkte aus P liegt:
Sweep-Line l trifft
unteren Punkt eines
Kreises, der auf den 3
Punkten pi, pj und pk
liegt.
  Damit ist der Mittelpunkt q des Kreises eine Voronoi-Ecke.
Prof. Dr. O. Bittel, HTWG Konstanz
Autonome Roboter - Wegekartenverfahren
SS 2016
6-25
Kreis-Ereignis (2)
  Es wird ein Parabelstück von der Beach-Line entfernt (Bildmitte):
Prof. Dr. O. Bittel, HTWG Konstanz
Autonome Roboter - Wegekartenverfahren
SS 2016
6-26
Steuerung der Sweep-Line-Bewegung
  Es wird eine Ereignis-Schlange Q mit allen Punkten aus P initialisiert
(nach y-Koordinaten sortiert).
 
Damit sind die Punkt-Ereignisse generiert.
  Sobald ein neues Parabelstück eingefügt oder gelöscht wird, wird geprüft,
ob ein neues Kreis-Ereignis möglich wird. Falls ja, wird es in Q eingefügt.
 
Prüfe dazu, ob sich die zwei Verbindungspunkte 3 aufeinanderfolgender
Parabelstücke aufeinander zubewegen.
 
Beachte: ein in Q eingefügtes Kreis-Ereignis muss evtl. später ignoriert
werden.
Die 3 gezeigten Punkte werden
(eventuell) ein Kreis-Ereignis
auslösen, da sich die zwei rot
dargestellten Verbindungspunkte
aufeinander zubewegen
Prof. Dr. O. Bittel, HTWG Konstanz
Autonome Roboter - Wegekartenverfahren
SS 2016
6-27
Beach-Line als balanzierter Baum
Beach-Line mit Folge von
Parabelstücke
Parabelstücke der Beach-Line in
einem balanziertem Suchbaum
Laufzeit des Sweep-Line-Verfahrens
für n Hindernispunkte und Beach-Line als balanzierter Suchbaum:
T(n) = O(n log n)
Prof. Dr. O. Bittel, HTWG Konstanz
Autonome Roboter - Wegekartenverfahren
SS 2016
6-28
Verallgemeinertes Voronoi-Diagramm
  Statt Punkte besteht die Umgebung nun aus beliebigen Hindernissen COi.
  Voronoi-Region zu einem Hindernis COi:
VR(i) = {x∈Cfree | di(x) ≤ dj(x) für alle Hindernisse COj mit i ≠ j }
wobei di(x) der kürzeste Abstand vom Punkt x zum Hindernis COi ist.
  Ein Verallgemeinertes VoronoiDiagramm GVD ist eine Zerlegung von Cfree in VoronoiRegionen.
  Oft versteht man unter GVD auch
die Grenzlinien zwischen den
Voronoi-Regionen.
COj
Prof. Dr. O. Bittel, HTWG Konstanz
Autonome Roboter - Wegekartenverfahren
COi
SS 2016
6-29
GVD für polygonale Hindernisse
Freier Raum lässt sich in 2 Arten von Regionen zerlegen:
  die nächsten Hindernisteile sind Kanten (z.B. Region A und C)
→ GVD-Grenzlinie ist eine Gerade
  die nächsten Hindernisteile sind Kanten und Ecken sind (z.B: Region B)
→ GVD-Grenzlinie ist eine Parabel
GVD-Grenzlinie
eine Gerade
GVD-Grenzlinie
eine Parabel
Prof. Dr. O. Bittel, HTWG Konstanz
Autonome Roboter - Wegekartenverfahren
SS 2016
6-30
Approximative Berechnung der Voronoi-Diagramme (1)
  Erstelle zur Umgebung ein Belegtheitsgitter.
  Ermittle alle Randzellen. Randzellen sind Zellen, die zu einem Hindernisrand gehören.
  Starte nun das Dijkstra-Verfahren, wobei Open List mit allen Randzellen initialisiert
wird. Der d-Wert der Randzellen wird auf 0 gesetzt.
  Das Dijkstra-Verfahren berechnet nun für alle Zellen den Abstand zu den
nächstgelegenen Hindernissen.
Belegtheitsgitter
Gitter mit Entfernungswerten zu
den nächstgelegenen Hindernissen
Bilder aus [Corke 2011]
Prof. Dr. O. Bittel, HTWG Konstanz
Autonome Roboter - Wegekartenverfahren
SS 2016
6-31
Approximative Berechnung der Voronoi-Diagramme (2)
  Das Voronoi-Diagramm besteht aus allen Zellen,
deren d-Wert ein lokales Maximum bilden.
  Voronoi-Diagramm kann zu Graph abstrahiert werden. Nehme Voronoi-Ecken als
Knoten und Verbindungen als Kanten. Führe evtl. Zwischenknoten ein.
Gitter mit Entfernungswerten zu
den nächstgelegenen Hindernissen
Bilder aus [Corke 2011]
Prof. Dr. O. Bittel, HTWG Konstanz
Voronoi-Diagramm. Voronoi-Ecken sind
blau dargestellt. Bei Überführung in ein
Graph können noch Zwischenknoten
eingebaut werden.
Autonome Roboter - Wegekartenverfahren
SS 2016
6-32
Wegeplanung:
Wegekartenverfahren
 
 
 
 
 
Idee
Sichtbarkeitsgraph
Voronoi-Diagramm
Probabilistische Wegekarten
Rapidly-Exploring Random Tree
Prof. Dr. O. Bittel, HTWG Konstanz
Autonome Roboter - Wegekartenverfahren
SS 2016
6-33
Probabilistische Wegekarten
  Beim PRM-Verfahren (Probabilistic Roadmap Method) werden zufällig Punkte auf den
Freiflächen generiert.
  Punkte werden mit allen sichtbaren Nachbarknoten, die innerhalb einer bestimmten
Entfernung liegen, mit einer Kante verbunden.
  In den PRM-Karten lassen sich mit den üblichen Graphenalgorithmen Wege finden.
Prof. Dr. O. Bittel, HTWG Konstanz
Autonome Roboter - Wegekartenverfahren
SS 2016
6-34
Bemerkungen
  Voronoi-Diagramme haben den Nachteil, dass große Freiflächen nur durch wenige Ecken
und Verbindungen aufgespannt werden. Die Planung kann daher sehr ineffiziente Wege
generieren.
  Das PRM-Verfahren führt bei großen Freiflächen zu einer wesentlich besseren Abdeckung.
  Jedoch werden möglicherweise kleine Freiflächen nicht abgedeckt oder ein nicht
zusammenhängender Graph wird generiert. Das Verfahren kann dadurch unvollständig
werden oder suboptimale Wege generieren.
Suboptimaler Weg aufgrund zu weniger
Knoten in der Engstelle ganz links.
Prof. Dr. O. Bittel, HTWG Konstanz
Nicht zusammenhängender Graph.
Autonome Roboter - Wegekartenverfahren
SS 2016
6-35
Wegeplanung:
Wegekartenverfahren
 
 
 
 
 
Idee
Sichtbarkeitsgraph
Voronoi-Diagramm
Probabilistische Wegekarten
Rapidly-Exploring Random Tree
Prof. Dr. O. Bittel, HTWG Konstanz
Autonome Roboter - Wegekartenverfahren
SS 2016
6-36
Probleme bei 2-dimensionalen Planungen
  Hat ein mobiler Roboter kinematische Einschränkungen,
dann kann oft ein in der x-y-Ebene geplanter Weg nicht
direkt vom Roboter abgefahren werden.
  Beispiel: Auto-Roboter, der seitwärts einparken soll.
  Eine einfache 2-dimensionale Planung (z.B. A* in einem Belegtheitsgitter)
würde einen direkten Weg seitwärts in die Parklücke finden (blauer Pfeil).
Dieser Weg könnte zwar von einem Roboter mit omnidirektionalem Antrieb
aber nicht von einem Auto-Roboter abgefahren werden.
Prof. Dr. O. Bittel, HTWG Konstanz
Autonome Roboter - Wegekartenverfahren
SS 2016
6-37
Planung in höher-dimensionalen Räumen
  Planung der kompletten Pose (x,y,θ).
Kinematische Einschränkung des Roboters wird berücksichtigt.
  Planung in einem Graphen mit Posen als Knoten.
Zwei Posen sind durch eine Kanten verbunden, falls die eine Pose aus der anderen
Pose durch einen einfachen Steuerbefehl erreichbar ist.
  Das folgende Bild zeigt für die Start-Pose s=(0,0,0°) eines Auto-Roboters verschiedene
mögliche Folge-Posen v, die durch einfache Steuerbefehle erreichbar sind.
v
s
  Verschiedene Pfade für ein
Auto-Roboter, der in der Pose
s = (0,0,0°) startet.
  Pfade für eine Periode von
T = 2 sec und verschiedenen
Lenkwinkeln α ∈ [−1, 1].
  v = -1 m/sec (rot) und
v = +1 m/sec (blau).
  Bild aus [Corke 2011].
Prof. Dr. O. Bittel, HTWG Konstanz
Autonome Roboter - Wegekartenverfahren
SS 2016
6-38
Planung in höher-dimensionalen Räumen (2)
  Problem: Anzahl der Knoten.
Würde auf eine Folge-Pose v wiederum die verschiedenen einfachen Steuerbefehle
angewendet, würde sich ein kombinatorisch explodierender Graph ergeben.
  Beim RRT-Verfahren wird dieses Problem umgangen, indem vergleichsweise wenige
Posen zufällig generiert werden.
s
Prof. Dr. O. Bittel, HTWG Konstanz
v
...
Autonome Roboter - Wegekartenverfahren
SS 2016
6-39
Rapidly-Exploring Random Tree (RRT-Verfahren)
Algorithmus RRT(x_Start, x_Target)
initialisiere Baum RT mit Start-Pose x_Start;
for i = t to K do
(1)  erzeuge zufällige Pose x_rand;
(2)  bestimme in RT diejenige Pose x_near,
die am nächsten an x_rand liegt;
(3)  bestimme einfachen Steuerbefehl u,
der x_near möglichst nahe an x_rand führt;
(4)  x_new ergibt sich durch Anwendung
von Steuerbefehl u auf x_near;
Steuerbefehl u würde beim
Auto-Roboter aus v und
Lenkwinkel α bestehen.
Der Steuerbefehl wird über
eine feste Zeitperiode T
durchgeführt.
(5)  erweitere RT um x_new;
(6)  breche ab, falls sich von x_new die
Ziele-Pose x_Target ungefähr erreichen lässt;
Prof. Dr. O. Bittel, HTWG Konstanz
Autonome Roboter - Wegekartenverfahren
SS 2016
6-40
Beispiel für RRT-Verfahren (1)
  Grüne Punkte:
im RRT-Verfahren
erzeugte Posen (x,y,θ)
  Start-Pose = (0, 0, 0°)
  Ziel-Pose = (0, 2, 0°)
  K = 500 Knoten
  v = ± 1 m/sec.
  T = 1 sec
  Lenkwinkel α ∈ [−1.2, 1.2].
  aus [Corke 2011].
Prof. Dr. O. Bittel, HTWG Konstanz
Autonome Roboter - Wegekartenverfahren
SS 2016
6-41
Beispiel für RRT-Verfahren (2)
  Aus dem Baum gewonnenen Weg für Start-Pose = (0, 0, 0°) und
Ziel-Pose = (0, 2, 0°).
  Beachte: Fahrzeug fährt rückwärts.
  Ziel-Pose wird aufgrund des probabilistischen Ansatzes nur ungefähr erreicht.
Prof. Dr. O. Bittel, HTWG Konstanz
Autonome Roboter - Wegekartenverfahren
SS 2016
6-42