Handout

Konzepte der Robotersteuerung
Christoph Wopperer
07. Juli 2005
Inhaltsverzeichnis
1 Das Grundproblem
2
2 Formulierung mit dem Konfigurationsraum
2.1 Definition des Konfigurationsraums . . . . . .
2.2 Präzisierung des Pfadbegriffs . . . . . . . . .
2.3 Hindernisse im Konfigurationsraum . . . . . .
2.4 Umformulierung des Grundproblems . . . . .
2.5 Spezialfälle für den Konfigurationsraum . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
3 Berechnung der C-Hindernisse für einen Spezialfall
3
3
4
5
5
5
6
4 Elementare Pfadplanungsmethoden
4.1 Roadmap Methoden . . . . . . . .
4.1.1 Der Sichtbarkeitsgraph . . .
4.1.2 Das Voronoi-Diagramm . .
4.2 Zellenzerlegungen . . . . . . . . . .
4.3 Potentialfelder . . . . . . . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
7
7
7
10
13
16
5 Ergänzungen
5.1 Graphensuchalgorithmen . . . . .
5.1.1 Tiefen- und Breitensuche
5.1.2 Der A? -Algorithmus . . .
5.2 Der sweep-line Algorithmus . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
17
17
17
18
20
.
.
.
.
1
1
DAS GRUNDPROBLEM
2
Das Grundproblem
Das Grundproblem der Robotersteuerung lautet salopp wie folgt: wie kommt ein
sich bewegendes Objekt (der Roboter) von A nach B“ ohne dabei an bestimmte
”
Hindernisse anzustoßen.
Ziel
B2
B3
?
Start
A
B1
Abbildung 1: Darstellung des Grundproblems der Robotersteuerung.
Für die genauere Formulierung des Grundproblems treffen wir folgende Annahmen:
• Der Roboter ist das einzige sich bewegende Objekt in der sogenannten Arbeitsumgebung.
• Dynamische Aspekte werden nicht berücksichtigt. Es handelt sich somit um
ein zeitunabhängiges Modell.
• Die Bewegungen des Roboters sind nur durch die Hindernisse eingeschränkt.
Damit wird das Grundproblem zu einem reinen Pfadplanungsproblem und lässt sich
wie folgt formulieren
Definition 1 (Grundproblem) Sei A ein einzelnes starres Objekt (der Roboter),
welches sich in einem euklidischen Raum W bewegt (der sogenannten Arbeitsumgebung), die durch IRN dargestellt sei mit N = 2 oder N = 3.
Seien zudem B1 , . . . , Bq fixierte starre Objekte in W. Die Bi heißen Hindernisse.
Die Geometrie von A, B1 , . . . , Bq in W sei genau bekannt und ferner sei A keinen
kinematischen Nebenbedingungen unterworfen (A ist dann ein sogenanntes free”
flying object“).
Das Grundproblem der Roboter-Pfadplanung ist es zu gegebener Startposition und
Orientierung von A in W sowie einer Zielposition und -orientierung einen Weg τ
zu finden, der eine stetige Folge von Positionen und Orientierungen von A mit der
Startposition als Ursprung und der Endposition als Endpunkt angibt. Zu jeder Zeit
darf kein Kontakt von A und einem der Bi vorliegen. Existiert kein solcher Weg, so
soll FEHLER zurück gegeben werden.
2
Diese informelle Definition soll im nächsten Abschnitt zunächst präzisiert werden.
Insbesondere muss die geforderte Stetigkeit des Weges formalisiert werden.
2
FORMULIERUNG MIT DEM KONFIGURATIONSRAUM
B2
B3
B1
3
Abbildung 2: Darstellung einer möglichen Lösung für eine Instanz des Grundproblems: Ausgabe soll ein Weg in Form einer stetigen Folge von Positionen und Orientierungen des Roboters sein.
2
Formulierung mit dem Konfigurationsraum
Die Formulierung des Grundproblems mit Hilfe des Konfigurationsraums verfolgt
das Ziel, den Roboter als Punkt in einem geeigneten Raum darzustellen und die
Hindernisse in diesen Raum mit abzubilden. Dadurch wird das Problem der Pfadplanung eines dimensionierten Objekts in das Pfadplanungsproblem eines Punktes
transformiert.
2.1
Definition des Konfigurationsraums
Sei der Roboter A eine kompakte Teilmenge von W = IRN mit N = 2 oder 3 und die
Hindernisse B1 , . . . , Bq abgeschlossene Teilmengen von W. Seien ferner FA und FW
kartesische Koordinatensysteme in A bzw. W. FA ist ein sich mit A bewegendes
Koordinatensystem während FW fixiert ist. Da A starr ist, hat jeder Punkt a ∈ A
eine feste Position bezüglich FA , während die Position von a in FW von der Position
und Orientierung von FA bezüglich FW abhängt. Da die Bi sowohl starr als auch
fixiert in W sind, hat jeder Punkt aus Bi eine feste Position bezüglich W.
Eine Konfiguration eines Objekts kann nun allgemein beschrieben werden als eine Spezifikation der Position jedes der Punkte dieses Objektes bezüglich eines
festen Koordinatensystems. Dies motiviert folgende (informelle) Definition des
Konfigurationsraums:
Definition 2 (Konfigurationsraum) Eine Konfiguration q von A ist eine Spezifizierung der Position T und der Orientierung Θ von FA bezüglich FW . Die Menge
aller Konfigurationen von A heißt der Konfigurationsraum C. Die Teilmenge von W,
die durch A bei der Konfiguration q eingenommen wird, sei mit A(q) bezeichnet.
In gleicher Weise sei die Position des Punktes a aus A in der Arbeitsumgebung W
zur Konfiguration q mit a(q) bezeichnet.
2
Abbildung 3 veranschaulicht den Konfigurationsraum im Fall N = 2. Wir halten
hierbei fest, dass der Konfigurationsraum offensichtlich durch IRN ×SO(N ) beschrieben werden kann, wobei SO(N ) gerade die orthogonalen Matrizen mit Determinante
+1 sind (Drehmatrizen). Es ist bekanntlich SO(N ) eine N (N2−1) -Mannigfaltigkeit
2
FORMULIERUNG MIT DEM KONFIGURATIONSRAUM
4
und IRN natürlich eine N -Mannigfaltigkeit. Damit ist C eine N + N (N2−1) = N (N2+1) Mannigfaltigkeit und zur Darstellung des Konfigurationsraums sind somit mindestens N (N2+1) reelle Parameter nötig.
A(θ, τ )
FA
τ
θ
FW
Abbildung 3: Veranschaulichung des Konfigurationsraums im Fall N = 2. Hier
kann C beschrieben werden durch Angabe eines Vektors τ ∈ IR2 , der die Position
von FA bezüglich FW angibt und durch Angabe des Winkels θ. Als Konfiguration
0 bezeichnen wir 0 = (0, 0), d. h. FA und FW fallen exakt zusammen. Zudem
bezeichnen wir mit 0A bzw. 0W die Nullpunkte der beiden Koordinatensysteme FA
und FW . Also gilt zur Konfiguration 0 insbesondere 0A = 0W .
2.2
Präzisierung des Pfadbegriffs
In der Formulierung des Grundproblems wurde informell verlangt, dass ein Pfad
stetig sein soll. Um dies zu formalisieren, benötigen wir zunächst eine Topologie in
C. Eine Möglichkeit hierzu ist es, eine Metrik d : C × C → IR zu definieren und als
Topologie die von dieser Metrik induzierte zu verwenden. Beispielsweise kann man
hierzu folgende Metrik verwenden:
d(q, q0 ) = max ka(q) − a(q0 )k ,
a∈A
wobei kx − x0 k den euklidischen Abstand für zwei Punkte x und x0 in IRN angibt.
Diese Definition erfüllt wenigstens unsere Intuition, dass der Abstand zweier Konfigurationen q und q0 umso kleiner wird, je mehr die Teilmengen A(q) und A(q0 )
übereinstimmen. Im Rahmen des Pfadplanungsproblems bieten sich noch einige andere Metriken an, die jedoch alle dieselbe Topologie induzieren.
Damit können wir nun formell Wege im Konfigurationsraum definieren.
Definition 3 Ein Weg für A von der Konfiguration qinit zur Konfiguration qgoal
ist eine (im Sinne obiger Topologie) stetige Abbildung
τ : [0, 1] → C,
die
τ (0) = qinit und τ (1) = qgoal
erfüllt.
2
Es sei noch bemerkt, dass die Formulierung A ist ein free-flying object‘“ gerade
”
’
bedeutet, dass jeder dieser beschriebenen Wege zulässig ist (d. h. durch den Roboter
bei Nichtvorhandensein von Hindernissen ausgeführt werden kann).
2
FORMULIERUNG MIT DEM KONFIGURATIONSRAUM
2.3
5
Hindernisse im Konfigurationsraum
Wie bereits angedeutet, werden nun die Hindernisse in den Konfigurationsraum
abgebildet. Jedes Hindernis Bi , i = 1, . . . , q in der Arbeitsumgebung W wird nach
C abgebildet durch
CBi := {q ∈ C mit A(q) ∩ Bi 6= ∅}.
CBi heißt auch C-Hindernis. Die Vereinigung aller C-Hindernisse
q
[
CBi
i=1
heißt C-Hindernis-Region und die Menge
(
q
[
Cfree := C\
CBi = q ∈ C mit A(q) ∩
i=1
q
[
!
Bi
)
=∅
i=1
heißt der freie Konfigurationsraum. Eine Konfiguration aus Cfree heißt freie Konfiguration.
2.4
Umformulierung des Grundproblems
Mit Hilfe der Übertragung der Hindernisse in den Konfigurationsraum lässt sich das
Grundproblem wieder etwas anders formulieren. Dazu zunächst noch folgende
Definition 4 (Freier Weg) Ein freier Weg zwischen zwei freien Konfigurationen qinit und qgoal ist eine stetige Abbildung τ : [0, 1] → Cfree mit τ (0) = qinit und
τ (1) = qgoal . Man sagt zwei Konfigurationen gehören zu der selben Zusammenhangskomponente von Cfree , wenn sie durch einen freien Weg verbindbar sind.
2
Das Grundproblem lässt sich damit formulieren als
Definition 5 (Umformulierung des Grundproblems) Zu gegebener Start- sowie Endkonfiguration finde einen freien Weg zwischen diesen beiden Konfigurationen, falls sie zur selben Zusammenhangskomponente von Cfree gehören bzw. sonst
gib FEHLER zurück.
2
Es sei noch bemerkt, dass viele der Planungsmethoden statt freie Wege nur semifreie Wege generieren. Ein semi-freier Weg ist hierbei eine stetige Abbildung τ :
[0, 1] → cl (Cfree ), wobei cl (Cfree ) den Abschluss von Cfree bezeichnet. Dies bedeutet, dass es hierbei dem Roboter gestattet ist, die Hindernisse zu berühren. Unter
bestimmten Annahmen kann jedoch ein semi-freier Weg τ in einen freien Weg transformiert werden, der beliebig nah (im Sinne obiger Metrik) an τ liegt.
2.5
Spezialfälle für den Konfigurationsraum
Wenn der Roboter nur ein Punkt ist (Punkt-Roboter), so muss zur Spezifizierung
seiner Konfiguration keine Orientierung angegeben werden. Der Konfigurationsraum
kann deshalb durch IRN dargestellt werden und fällt in diesem Fall mit der Arbeitsumgebung W zusammen. Insbesondere sind die C-Hindernisse identisch zu den Hindernissen in der Arbeitsumgebung. In der Definition des Konfigurationsraums sieht
man dies daran, dass man jede Konfiguration q durch die Position des Roboters
τ angeben kann und damit A(q = τ ) = τ , also können wir tatsächlich C und W
miteinander identifizieren.
Einen weiteren wichtigen Spezialfall erhält man, falls A eine N -Kugel ist, oder
wenn A nur translatorische Bewegungen durchführen kann (d. h. zu jeder Konfiguration muss die Orientierung θ von FA in Bezug auf FW gleich sein). Auch in
3
BERECHNUNG DER C-HINDERNISSE FÜR EINEN SPEZIALFALL
6
diesem Fall stimmen Konfigurationsraum C und Arbeitsumgebung W überein (wieder können beide durch IRN repräsentiert werden). Für die C-Hindernisse gilt hier
jedoch:
Proposition 1 In der oben beschriebenen Situation gilt für jedes C-Hindernis CB =
{q ∈ C mit A(q) ∩ B =
6 ∅}, dass
CB = B A(0) = {x ∈ W mit ∃b ∈ B, ∃a0 ∈ A(0) so dass x = b − a0 } .
Hierbei bezeichnet den Operator der Minkowski-Differenz.
Beweis Wir zeigen zunächst die Inklusion CB ⊂ B A(0). Sei hierzu q = T ∈ CB,
d. h. es ist A(T ) ∩ B 6= ∅. Also gibt es ein b ∈ B und ein a ∈ A(T ) mit b = a. Nun
gilt aber für a0 := a − T offensichtlich a0 ∈ A(0) und zudem T = b − a0 ∈ B A(0).
Umgekehrt sei q = T ∈ B A(0), d. h. es gibt ein b ∈ B und ein a0 ∈ A(0), so dass
T = b − a0 . Dann ist aber offensichtlich a := a0 + T ∈ A(T ) und da b = a0 + T = a
gilt also a = b ∈ A(T ) ∩ B, d. h. also T ∈ CB.
FA = FW
A(0)
B
CB
Abbildung 4: Veranschaulichung obiger Proposition. Im translationalen Fall erhält
man die C-Hindernisse als Minkowski-Differenz von B und A(0). Anschaulich ergibt sich der Rand von CB als alle Positionen des Nullpunkts 0A , die man durch
Verschieben des Roboters entlang des Randes von B erhält.
3
Berechnung der C-Hindernisse im translationalen Fall für konvexe Polygone
Obige Proposition erlaubt uns die C-Hindernisse im translationalen Fall konvexer
Polygone des IR2 direkt zu berechnen. Hierzu zunächst also folgende Definition:
Definition 6 Ein konvexes polygonales Gebiet des IR2 ist der Durchschnitt
von endlich vielen abgeschlossenen Halbräumen. Ein polygonales Gebiet ist jede
Teilmenge des IR2 , die sich als Vereinigung von endlich vielen konvexen polygonalen Gebieten schreiben lässt. Ein Polygon ist zudem jedes polygonale Gebiet, das
homöomorph zur abgeschlossenen Einheitsscheibe ist.
2
Wie bereits in einem der vorigen Vorträge kurz bemerkt, gilt für zwei Polygone
A und B, dass die Minkowski Summe A ⊕ B wieder ein Polygon ist, und dass sich
zudem jede Ecke von A ⊕ B als Summe zweier Ecken von A und B schreiben lässt.
Die Umkehrung gilt jedoch nicht; um in unserem Fall genau die Ecken von B A(0)
zu berechnen, kann folgender Algorithmus verwendet werden:
Algorithmus 7 (Lozano-Pérez) Seien A und B konvexe Polygone und A könne
nur translatorische Bewegungen ausführen. Seien ferner die Ecken ai von A bekannt
und mit 1, . . . , nA mod nA im Gegenuhrzeigersinn durchnummeriert. Analog seien die Ecken bj von B mit 1, . . . , nB mod nB nummeriert. Hieraus berechne man
4
ELEMENTARE PFADPLANUNGSMETHODEN
7
zunächst die äußeren Normalen νiA , i = 1, . . . , nA von A und νjB , j = 1, . . . , nB
von B (vgl. Abbildung 5). Dann erhält man die Ecken von CB = B A(0) wie folgt:
(1.) Fixiere die Vektoren −νiA , i = 1, . . . , nA und νjB , j = 1, . . . , nB im Zentrum
des Einheitskreises S 1 . Da sowohl A und B konvexe Polygone sind, ist der
Winkel zwischen zwei im Gegenuhrzeigersinn aufeinanderfolgenden Vektoren
A
B
−νiA und −νi+1
bzw. νjB und νj+1
stets kleiner als π.
B
(2.) Angenommen es zeigt −νiA zwischen νj−1
und νjB , d. h. es lässt sich −νiA als
B
positive Linearkombination von νj−1 und νjB schreiben. Genau dann kann die
Kante EiA von A, die ai und ai+1 als Ecken besitzt, durch Translation von A
in Kontakt mit der Ecke bj von B gebracht werden. Man erhält deshalb die
Punkte
bj − ai (0) und bj − ai+1 (0)
als Ecken von CB.
A
(3.) Angenommen es zeigt νjB zwischen −νi−1
und −νiA . Genau dann kann die
Kante EjB von B, die bj und bj+1 als Ecken besitzt, durch Translation von A
in Kontakt mit der Ecke ai von A gebracht werden. Man erhält deshalb die
Punkte
bj − ai (0) und bj+1 − ai (0)
als Ecken von CB.
(4.) Für den Fall, dass zwei Kanten EiA und EjB parallel sind und der Kontakt
zwischen diesen beiden Kanten zulässig ist, d. h. es tritt (2.) in Form von −νiA
B
B
zeigt zwischen νj−1
und νjB sowie −νiA zeigt zwischen νjB und νj+1
und es tritt
B
A
A
(3.) in Form von νj zeigt zwischen −νi−1 und −νi sowie in Form von νjB zeigt
A
zwischen −νiA und −νi+1
, dann sind nicht alle wie in (2.) und (3.) berechneten
Ecken tatsächlich Ecken von CB. Dieser Fall kann aber leicht erkannt werden,
da dann −νiA = νjB gilt und bei der Berechnung in (2.) und (3.) müssen dann
nur die beiden Ecken
bj+1 − ai (0) und bj − ai+1 (0)
gestrichen werden.
Um die Ecken von CB zu berechnen reicht es also, die Normalenvektoren in S 1 beispielsweise im Gegenuhrzeigersinn einmal zu durchlaufen. Somit können alle Ecken
in O(nA + nB ) berechnet werden.
4
4.1
4.1.1
Elementare Pfadplanungsmethoden
Roadmap Methoden
Der Sichtbarkeitsgraph
Wir betrachten hier nur den Fall, dass der Konfigurationsraum gleich C = IR2 ist
und die C-Hindernis Region CB gegeben ist als polygonales Gebiet des IR2 . Mit
einer leichten Modifikation des obigen Algorithmus kann man für diesen Fall den
Rand von CB (und damit auch von Cfree ) berechnen.
Die Idee des Sichtbarkeitsgraphen ist es, einen semi-freien Weg zwischen qinit und
qgoal als polygonale Gerade zu konstruieren. Eine polygonale Gerade ist hierbei ein
stetiger aus endlich vielen Geradensegmenten zusammengesetzter Weg. Die Idee
beruht auf folgender Proposition:
4
ELEMENTARE PFADPLANUNGSMETHODEN
ν2B
ν2A
a2
ν3A
0A
a1
S1
ν2B
b2
b3
ν1A
A
a3
8
ν3B
ν1B
B
b1
b4
FA
ν4B
b3 − a1 (0)
−ν1A
ν3B
ν1B
ν4B
−ν2A
b2 − a1 (0)
−ν3A
b3 − a2 (0)
b4 − a2 (0)
CB
b2 − a3 (0)
b1 − a3 (0)
b1 − a2 (0)
Abbildung 5: Die Abbildung veranschaulicht obigen Algorithmus zur Berechnung
von CB für den Fall, dass A und B konvexe Polygone des IR2 sind sowie der Konfigurationsraum gleich IR2 ist, d. h. für den Fall eines Roboters aus dem IR2 , der nur
translationale Bewegungen ausführen darf.
Proposition 2 Es sei die C-Hindernis Region CB ein polygonales Gebiet von C =
IR2 . Dann gibt es einen semi-freien Weg zwischen zwei beliebigen Konfigurationen
qinit und qgoal genau dann, wenn es eine polygonale Gerade τ in cl(Cfree ) gibt,
deren Endpunkte gerade qinit und qgoal sind und deren Ecken auch Ecken von CB
sind.
2
Beweis Die Richtung, dass aus der Existenz von τ , die Existenz eines semi-freien
Weges folgt ist trivial. Für die Umkehrung gebe es also einen semi-freien Weg zwischen qinit und qgoal . Man zeigt dann (geometrisch):
(1.) Jeder stetige Weg (endlicher Länge) von qinit nach qgoal lässt sich durch eine
polygonale Gerade ersetzen, die höchstens genauso lang ist.
(2.) Jede polygonale Gerade mit Ecken, die nicht Knoten des Sichtbarkeitsgraphen
sind, kann durch eine polygonale Gerade ersetzt werden deren Ecken Knoten
des Sichtbarkeitsgraphen sind, die kürzere Länge hat.
(3.) Es gibt offensichtlich nur endlich viele polygonale Geraden, deren Ecken auch
Knoten des Sichtbarkeitsgraphen sind und die keine Zyklen besitzen.
(4.) Nach (3.) kann man deshalb einen Weg minimaler Länge unter diesen polygonalen Geraden finden, der nach (1.) und (2.) dann auch ein insgesamt kürzester
Weg ist.
Um also einen (kürzesten) semi-freien Weg zwischen zwei beliebigen Konfigurationen qinit und qgoal zu finden, reicht es also aus, polygonale Geraden in cl(Cfree )
mit Eckpunkten, die auch Ecken von CB sind, zu betrachten. Die Menge dieser
polygonalen Geraden heißt Sichtbarkeitsgraph, genauer:
Definition 8 Der Sichtbarkeitsgraph ist der ungerichtete Graph G, mit:
• Die Knoten von G sind qinit und qgoal sowie die Eckpunkte von CB
4
ELEMENTARE PFADPLANUNGSMETHODEN
9
• Zwei Knoten von G sind durch einen Bogen verbunden, falls entweder die
Gerade, die die beiden Punkte verbindet eine Kante von CB ist, oder falls die
Gerade ganz in Cfree liegt (bis auf möglicherweise ihre beiden Endpunkte). 2
2
2
2
3
3
3
2
3
2
2
3
3
2
3
2
3
2
2
3
$
$
6
6
6
6
7
7
7
7
6
6
6
6
7
7
7
7
.
/
6
6
6
6
6
7
7
7
7
7
!
!
!
6
6
7
7
6
6
6
6
6
6
7
7
7
7
7
7
.
.
/
/
&
&
'
!
6
6
6
6
6
6
7
7
7
7
7
7
&
'
!
6
7
&
'
!
&
.
.
'
/
/
!
$
2%
%
3
6
7
'
!
!
!
.
/
$
!
6
7
&
'
!
$
.
/
6
7
&
'
!
.
/
.
/
&
'
!
&
'
!
&
'
!
!
&
'
!
&
'
!
!
&
'
!
&
'
!
!
&
'
!
!
!
!
&
'
!
!
!
!
!
2
2%
6
7
3
3
6
7
6
7
2%
.
/
3
6
7
2%
6
7
&
'
!
3
2
3
&
'
!
3
$
2
&
'
!
3
&
'
!
2
2
3
3
!
2
3
!
2
3
!
!
2
2
3
3
2
3
%
6
6
6
6
6
6
7
7
7
7
7
7
&
!
$
$
$
!
2%
2%
3
3
$
2%
3
!
$
&
!
&
!
'
!
'
&
.
.
'
/
/
!
'
%
%
6
6
6
6
6
6
7
7
7
7
7
7
&
!
$
$
$
2%
2%
3
3
!
$
2%
3
!
$
2%
%
%
&
!
'
"
!
&
!
'
&
!
'
.
.
/
!
/
'
$
3
"
#
"
#
"
#
"
(
#
)
!
'
#
"
(
#
)
!
'
"
(
#
)
!
'
"
(
#
)
&
.
.
!
'
/
/
"
(
#
)
6
$
$
$
!
2%
2%
3
3
$
2%
!
$
2%
%
%
"
"
#
&
&
"
(
0
)
1
6
6
6
6
6
7
7
7
7
7
$
3
!
#
7
&
!
3
qgoal
$
2%
3
!
"
#
"
#
"
(
#
)
#
"
(
#
)
"
(
#
)
"
(
#
)
"
(
#
)
6
!
!
!
&
!
'
'
!
'
.
'
/
!
"
(
0
)
1
6
6
7
&
&
!
#
7
&
!
6
7
6
7
6
7
7
.
/
8
$
$
$
$
$
2%
2%
2%
%
3
3
3
9
$
%
8
8
*
8
*
8
*
8
*
8
*
8
*
,
8
*
,
8
*
,
8
*
,
9
9
+
9
+
9
+
9
+
9
+
9
+
-
9
+
-
9
+
-
9
+
-
"
"
#
qinit
8
9
8
9
2%
3
"
#
"
#
"
(
#
)
!
'
#
"
(
#
)
!
'
!
!
!
$
%
$
$
$
$
%
%
$
$
%
%
%
&
&
'
$
$
%
%
$
%
%
(
(
)
'
4
$
$
$
%
%
%
5
5
5
5
%
%
5
4
5
4
5
4
5
%
5
5
5
4
5
4
5
4
5
$
4
5
4
5
4
5
4
5
%
%
5
.
4
5
4
5
4
5
4
5
%
5
4
5
4
5
$
$
$
%
4
5
5
5
5
5
.
/
4
5
4
5
%
%
.
5
4
5
4
5
4
5
%
5
4
5
4
5
4
5
4
5
$
,
,
,
-
-
-
1
.
$
%
,
-
0
.
/
4
$
%
5
$
%
,
.
/
4
$
%
4
$
$
-
1
/
4
%
,
-
0
$
,
-
%
,
$
5
$
%
,
4
$
-
1
4
%
,
-
0
4
$
,
-
4
%
,
.
%
5
/
$
,
.
/
4
$
%
4
%
-
1
/
4
$
$
,
-
0
$
%
,
-
,
,
%
5
-
4
$
,
-
0
1
.
/
$
%
,
-
.
/
$
,
4
%
5
$
,
-
.
/
4
$
%
,
-
4
%
,
-
1
4
$
%
,
-
,
-
$
%
$
%
*
+
.
$
,
-
0
/
$
%
*
+
5
$
%
,
-
4
$
%
*
+
0
1
4
$
%
,
-
(
)
/
5
$
%
,
-
0
1
*
+
4
(
)
.
4
*
+
.
/
4
,
-
/
.
'
4
*
+
(
)
&
'
4
,
-
$
5
$
'
4
$
%
,
*
+
*
)
&
-
,
-
+
(
)
&
*
+
(
)
)
.
/
*
+
(
&
(
&
'
(
)
(
*
+
)
*
+
*
%
+
&
'
$
%
)
&
'
$
(
)
&
'
$
%
*
+
,
-
+
*
+
.
*
+
$
%
,
-
,
-
/
+
$
%
.
/
*
+
*
+
0
1
$
%
&
*
+
*
$
'
'
*
$
%
,
-
(
)
)
&
'
$
%
,
*
+
(
)
-
(
)
*
+
*
(
)
,
-
*
+
,
-
+
(
)
(
*
+
,
-
*
+
+
+
,
-
*
+
,
-
0
1
.
*
+
*
+
,
-
*
+
(
)
/
*
+
.
/
*
$
,
-
*
+
,
-
(
)
&
'
*
$
%
*
+
,
*
+
)
-
0
1
(
&
,
(
)
.
/
*
+
'
-
0
*
+
1
(
)
.
/
(
)
&
(
)
.
/
*
+
&
'
*
'
*
+
(
)
+
)
&
'
0
1
(
)
.
/
*
+
(
)
(
)
.
/
*
+
&
'
(
"
#
(
)
*
+
(
)
.
/
*
+
&
'
+
"
#
&
)
&
'
*
+
'
(
)
*
(
)
!
(
&
'
"
*
+
(
)
*
+
&
'
*
+
%
)
&
'
$
(
)
&
%
%
#
(
)
'
$
*
+
(
$
%
&
$
'
*
+
$
%
(
%
$
%
$
%
)
+
%
$
%
$
%
!
$
%
$
%
$
%
"
&
*
$
%
$
%
#
&
!
$
%
4
5
4
5
/
4
5
4
5
4
5
5
,
,
,
,
-
-
-
-
$
%
%
0
1
Abbildung 6: Beispiel für einen Sichtbarkeitsgraphen; die C-Hindernisse sind hierbei
schwarz dargestellt. Der Teilgraph von G, den man durch herausnehmen von qinit
und qgoal sowie allen von diesen Punkten ausgehenden Bögen erhält, heißt auch die
Roadmap R. Der mit G gefundene kürzeste Weg ist fett eingezeichnet.
Wir können damit also folgenden Algorithmus festhalten:
Algorithmus 9 (Sichtbarkeitsgraph) Für die Methode des Sichtbarkeitsgraphen
führe folgende Schritte aus:
(1.) Berechne den Sichtbarkeitsgraphen G.
(2.) Durchsuche G nach einem Pfad von qinit nach qgoal .
Wenn die Suche erfolgreich war, so gebe den Pfad zurück, ansonsten gebe FEHLER
zurück.
Um G zu berechnen kann man wie folgt verfahren:
• Betrachte alle Paare von Punkten (X, X 0 ), wobei X und X 0 aus {qinit , qgoal }∪
{Ecken von CB} sowie X 6= X 0 .
• Falls X und X 0 Endpunkte derselben Kante von CB sind, so sind die entsprechenden Knoten in G durch einen Bogen verbunden.
• Anderenfalls berechne die Schnittpunkte der Gerade durch X und X 0 mit CB.
Die Knoten X und X 0 in G werden genau dann durch einen Bogen verbunden,
falls keiner der Schnittpunkte auf dem offenen Geradensegment zwischen X
und X 0 liegt.
Wenn n die Gesamtanzahl der Ecken in CB bezeichnet, so gibt es gerade O(n2 ) solche
Paare (X, X 0 ). Ob X und X 0 Endpunkte einer gleichen Kante aus CB sind, lässt
sich bei geeigneter Darstellung von CB in O(1) ermitteln. Der letzte Schritt kann
für jedes Paar (X, X 0 ) schließlich in O(n) durchgeführt werden, indem man jede der
O(n) Kanten von CB betrachtet und überprüft, ob das Geradensegment zwischen
X und X 0 diese Kante schneidet. Damit hat dieser Algorithmus zur Berechnung
von G die Komplexität O(n3 ).
Durch eine Variante des sweep-line Algorithmus (vgl. zum sweep-line Algorithmus
auch Abschnitt 5.2) kann man G jedoch sogar mit Komplexität O(n2 log n) berechnen:
4
ELEMENTARE PFADPLANUNGSMETHODEN
10
Algorithmus 10 (Berechnung der Bögen des Sichtbarkeitsgraphen, Lee)
Führe folgende Schritte aus:
(1.) Für jeden Punkt X aus G, berechne die Orientierung αi ∈ [0, 2π) jeder Halbgeraden, die von X ausgeht und durch irgendeinen anderen Punkt Xi von G
geht.
(2.) Sortiere diese Orientierungen αi .
(3.) Rotiere eine Halbgerade ausgehend von X von der Orientierung 0 bis zur Orientierung 2π. Halte während der Rotation nur an jeder Orientierung αi . Bei
jedem Halt, update die Schnittkante (dies ist die erste Kante von CB, die die
Halbgerade von X nach Xi trifft). Berechne ferner den Punkt der Schnittkante,
der am nähesten bei X liegt und teste, ob das Segment, das X und Xi verbindet
CB schneidet.
Zu bemerken ist hierbei, dass – falls die Halbgerade von X nach Xi als erstes
Element aus CB gerade Xi trifft – die Schnittkante“ sich aus den zweien Kanten
”
aus CB zusammensetzt, die Xi als Endpunkt besitzen.
Die grundlegende Idee des Algorithmus ist dabei folgende: wie man sich leicht überlegt, kann die neu berechnete Schnittkante in jedem Schritt nur die vorher gemerkte
Schnittkante sein oder die Kanten, in der Xi liegt.
Da es O(n) Knoten X in G gibt benötigt man für den Schritt (1.) gerade Zeit
O(n) für jeden dieser Punkte. Für jedes X gibt es aus dem gleichen Grund gerade
O(n) solche αi und für jedes X benötigt man deshalb Aufwand O(n log n) zur
Sortierung der αi . Die Rotation der Halbgeraden aus (3.) benötigt dann noch O(n)
Schritte für jeden der Punkte X, da jeweils nur zu den αi gestoppt werden muss und
die Schnittkante sowie die Berechnung des Punktes, der am nähesten bei X liegt
nur Aufwand O(1) erfordert. Insgesamt hat der Algorithmus damit den Aufwand
O(n2 log n).
Um G nach einem Pfad von qinit nach qgoal zu durchsuchen, kann man den A? Algorithmus (vgl. Abschnitt 5.1.2) verwenden, mit dem euklidischen Abstand zu
qgoal als heuristische Funktion. Da diese lokal konsistent ist, kann die Suche in O(n2 )
durchgeführt werden, falls die Liste Open nicht sortiert ist, bzw. in O(k log n), falls
die Liste sortiert ist (mit k als Anzahl der Kanten von G). Insgesamt kann man
also bei gegebenem CB mit dem Sichtbarkeitsgraphen einen semi-freien Weg in
(höchstens) totaler Zeit O(n2 log n) berechnen.
4.1.2
Das Voronoi-Diagramm
Für das Voronoi-Diagramm benötigen wir den aus der Topologie bekannten Begriff
einer Retraktion.
Definition 11 Sei X ein topologischer Raum und Y eine Teilmenge von X . Eine
Retraktion ist eine stetige surjektive Abbildung ρ : X → Y mit ρ|Y = Id.
2
Für unsere Anwendung werden wir ausschließlich folgende Klasse von Retraktionen
betrachten:
Definition 12 Sei ρ eine Retraktion von X auf Y. Man sagt ρ erhält den Zusammenhang von X , falls für alle x ∈ X gilt: x und ρ(x) gehören zur selben Wegzusammenhangskomponente von X .
2
4
ELEMENTARE PFADPLANUNGSMETHODEN
11
Sei ρ eine Retraktion von Cfree auf eine eindimensionale Teilmenge R von Cfree ,
die den Zusammenhang von Cfree erhält. Folgende Proposition ermöglicht es, das
Pfadplanungsproblem auf ein Pfadplanungsproblem in R zu reduzieren. R heißt
dann wie beim Sichtbarkeitsgraphen wieder Roadmap.
Proposition 3 Sei ρ : Cfree → R, mit R ⊂ Cfree einer eindimensionalen Teilmenge
von Cfree , eine zusammenhangserhaltende Retraktion. Dann gibt es einen freien Weg
zwischen zwei Konfigurationen qinit und qgoal genau dann, wenn es einen Weg in
R zwischen ρ(qinit ) und ρ(qgoal ) gibt.
Beweis Sei τ : [0, 1] → Cfree ein freier Weg zwischen qinit und qgoal . Verknüpfung
von ρ mit τ gibt aufgrund der Stetigkeit von ρ einen Weg ρ ◦ τ : [0, 1] → R zwischen
ρ(qinit ) und ρ(qgoal ). Es gebe umgekehrt einen Weg s : [0, 1] → R zwischen ρ(qinit )
und ρ(qgoal ). Dann gibt es einen freien Weg zwischen qinit und qgoal , der sich aus
folgenden Teilwegen zusammensetzt:
• einem Weg zwischen qinit und ρ(qinit )
• dem Weg s zwischen ρ(qinit ) und ρ(qgoal ) sowie
• einem Weg zwischen ρ(qgoal ) und qgoal .
Der erste und der letzte Weg existieren dabei, da ρ den Zusammenhang von Cfree
erhält.
Jede auf einer Retraktion aufbauenden Pfadplanungsmethode beruht auf dieser
Konfiguration. Entscheidend ist die Wahl der Retraktion ρ. Damit die Methode
effektiv ist, werden Algorithmen benötigt um eine graphenähnliche Repräsentation
von R zu berechnen, die Punkte ρ(qinit ) und ρ(qgoal ) zu ermitteln und freie Wege
von qinit nach ρ(qinit ) sowie zwischen ρ(qgoal ) und qgoal zu konstruieren.
Eine wichtige Pfadplanungsmethode für C = IR2 (also im Falle eines ausschließlich
translatierenden Roboters im IR2 ) und für den Fall, dass Cfree das Innere eines
beschränkten polygonalen Gebiets ist, erhält man durch Retraktion von Cfree auf
das Voronoi-Diagramm. Dieses Diagramm hat den Vorteil, dass es den Abstand
zwischen dem Roboter und den Hindernissen zu maximieren versucht. Außerdem
können wir für diesen Spezialfall obigen Algorithmus zur Berechnung des Randes
von CB und damit auch von Cfree verwenden.
Definition 13 Sei β = ∂Cfree 1 . Für jedes q ∈ Cfree sei
clearance(q) := min kq − pk ,
p∈β
wobei kq − pk der Euklidische Abstand zwischen q und p ist. Sei zudem
near(q) := {p ∈ β mit kq − pk = clearance(q)} .
Das Voronoi-Diagramm von Cfree ist die Menge
Vor(Cfree ) = {q ∈ Cfree mit card(near(q)) > 1} ,
wobei card(E) die Kardinalität einer Menge E bezeichnet.
2
Man kann zeigen, dass Vor(Cfree ) sich aus einer endlichen Anzahl von geraden
und parabolischen Kurvensegmenten zusammensetzt, die wir als Bögen bezeichnen.
Einen geraden Bogen in Vor(Cfree ) erhält man als die Menge von Konfigurationen,
1 Hier
ist ∂Cfree als ∂ (cl(Cfree )) zu verstehen.
4
ELEMENTARE PFADPLANUNGSMETHODEN
12
die am nähesten zum gleichen Paar von Kanten oder zum gleichen Paar von Ecken
sind. Einen parabolischen Bogen erhält man als Menge von Konfigurationen, die
am nähesten zu einem Paar bestehend aus einer Ecke und einer Kante sind. Für
jeden Punkt q ∈ Cfree \Vor(Cfree ) gilt card(near(q)) = 1.
Sei n die Anzahl der Ecken von ∂Cfree . Dann kann man das Voronoi-Diagramm
beispielsweise in O(n4 ) berechnen, indem man die O(n2 ) Paare (Kante, Kante),
(Ecke, Kante) und (Ecke, Ecke) betrachtet und die Durchschnitte der zugehörigen
äquidistanten Kurven berechnet. Es gibt jedoch auch Algorithmen, die das VoronoiDiagramm in O(n log n) berechnen.
Nun muss noch die Retraktion ρ : Cfree → R definiert werden. Sei hierzu q ∈
Cfree \Vor(Cfree ). Dann ist card(near(q)) = 1 und es gibt folglich einen eindeutigen
Punkt p in ∂Cfree , der näher an q liegt, als jeder andere Punkt aus ∂Cfree , also mit
kq − pk = clearance(q). Sei L die Halbgerade die von p ausgehend durch q geht.
Es sei ρ(q) gerade der erste Schnittpunkt von L mit Vor(Cfree ). Für q ∈ Vor(Cfree )
setzt man dagegen entsprechend ρ(q) := q.
ρ(qinit )
(
(
)
)
ρ(qgoal )
,
,
,
,
,
,
,
,
,
-
-
-
-
-
-
-
-
qinit
,
,
,
,
,
,
,
-
-
-
-
-
-
,
,
-
-
&
&
'
,
'
,
,
,
,
,
,
,
,
-
-
-
-
-
-
-
-
,
,
,
,
,
,
,
-
-
-
-
-
-
qgoal
,
-
,
-
,
,
,
,
,
,
,
,
-
-
-
-
-
-
-
,
,
-
,
,
,
,
,
,
,
,
-
-
-
-
-
-
-
-
,
,
,
,
,
,
,
,
,
-
-
-
-
-
-
-
-
,
,
,
,
,
,
,
,
-
-
-
-
-
-
-
,
,
-
,
-
,
,
,
,
,
,
-
-
-
-
-
-
,
-
+
*+
*
.
.
.
.
.
.
.
.
/
/
/
/
/
/
/
.
.
.
.
.
.
.
.
/
/
/
/
/
/
/
.
.
.
.
.
.
.
/
/
/
/
/
/
.
.
/
.
.
/
/
CB
.
/
.
.
.
.
/
/
/
.
.
.
.
/
/
/
/
.
.
.
.
/
/
/
/
!
.
.
.
.
.
.
.
/
/
/
/
/
/
.
.
/
.
.
.
.
.
.
.
/
/
/
/
/
/
/
Cf ree
"#
$%
Abbildung 7: Veranschaulichung des Voronoi-Diagramms. Ein damit gefundener
Weg von qinit nach qgoal ist fett eingezeichnet.
Proposition 4 Die dadurch definierte Abbildung ρ : Cfree → R ist stetig.
2
Also ist ρ eine Retraktion von Cfree auf Vor(Cfree ). Nach Konstruktion erhält ρ zudem
den Zusammenhang von Cfree .
Wir halten also zusammenfassend folgenden Algorithmus fest:
Algorithmus 14 (Ó’Dúnlaing und Yap, 1982) Führe folgende Schritte aus:
(1.) Berechne das Voronoi-Diagramm Vor(Cfree ).
(2.) Berechne die Punkte ρ(qinit ) und ρ(qgoal ) und identifiziere zu welchen Bögen
von Vor(Cfree ) diese beiden Punkte gehören.
4
ELEMENTARE PFADPLANUNGSMETHODEN
13
(3.) Durchsuche Vor(Cfree ) nach einer Folge von Bögen A1 , . . . , Ap , so dass ρ(qinit ) ∈
A1 , ρ(qgoal ) ∈ Ap und für alle i = 1, . . . , p − 1 gilt, dass Ai und Ai+1 einen
gemeinsamen Endpunkt besitzen.
Falls die Suche erfolgreich war, gibt ρ(qinit ) und ρ(qgoal ) sowie A1 , . . . , Ap zurück,
ansonsten gebe FEHLER zurück.
Man kann zeigen, dass der Algorithmus insgesamt die Komplexität O(n log n) besitzt.
4.2
Zellenzerlegungen
Die Grundidee ist hier, Cfree in einfache Gebiete (sog. Zellen) zu zerlegen, so dass
ein Pfad innerhalb einer Zelle leicht generiert werden kann. Für die Zellen wird
dann ein ungerichteter Graph, der die Nachbarschaftsrelationen zwischen den Zellen
angibt, generiert und durchsucht. Dieser Graph wird auch als connectivity graph“
”
bezeichnet. Die Knoten sind gerade die Zellen und zwischen zwei Knoten ist genau
dann ein Bogen, wenn die entsprechenden Zellen benachbart sind. Das Durchsuchen
des Graphen liefert eine Folge von Zellen, die auch als Kanal bezeichnet wird.
Hieraus kann dann ein freier Weg konstruiert werden.
Grundlegend unterscheidet man bei Zellenzerlegungen exakte und approximative
Methoden:
• Bei exakten Zellenzerlegungen liefert die Vereinigung der Zellen genau Cfree .
• Bei approximativen Zellenzerlegungen werden Zellen einer festen Form (z. B.
Trapeze oder Dreiecke) erzeugt, deren Vereinigung strikt in Cfree enthalten ist.
Die Abbildungen 8 und 9 geben ein Beispiel einer exakten Zellenzerlegung für den
Fall, dass der Konfigurationsraum C gleich dem IR2 ist und die C-Hindernis Region
CB ein polygonales Gebiet des IR2 ist. Der freie Konfigurationsraum ist zudem
durch ein Polygon beschränkt und kann dann in Form von Trapezen und Dreiecken
zerlegt werden. Dazu werden von den Ecken der C-Hindernisse ausgehend vertikale
Linien gebildet. Als Ergebnis erhält man die Zellen, bei denen genau dann zwei
benachbart sind, falls sie eine gemeinsame Kante positiver Länge besitzen. Aus
einem Pfad im connectivity graph“ von der Zelle die qinit enthält zu der Zelle die
”
qgoal enthält, erhält man dann einen freien Weg von qinit nach qgoal , indem man
die Mittelpunkte zweier nachfolgender Zellen des Pfades miteinander verbindet.
Die Abbildung 10 gibt ein Beispiel einer approximativen Zellenzerlegung wieder für
den Fall, dass der Konfigurationsraum C gleich dem IR2 ist und die C-Hindernis
Region CB ein polygonales Gebiet des IR2 ist. Der freie Konfigurationsraum wird
hier durch ein Rechteck R beschränkt. Das Rechteck R wird nun sukzessiv in kleinere Rechtecke zerlegt. Jede Zerlegung liefert vier weitere gleich große Rechtecke.
Irgendwann wird die Iteration abgebrochen und nur die Rechtecke, die vollständig
im freien Konfigurationsraum Cfree liegen, werden verwendet um den connectivity
”
graph“ zu bilden. Wenn der Graph erfolgreich durchsucht wurde, kann analog zu
den exakten Zellenzerlegungen ein freier Weg gebildet werden. Ansonsten war entweder die Auflösung zu gering oder es gibt tatsächlich keinen freien Weg. Bei dieser
Methode wird daher üblicherweise mit einem groben Rechtecksgitter angefangen,
und die Auflösung sukzessiv verfeinert, bis entweder ein freier Weg gefunden wurde,
oder die Auflösung einen vorher festgelegten Wert überschritten hat.
Grundsätzlich ist es so, dass exakte Zellenzerlegungen immer einen freien Weg liefern, falls ein solcher existiert. Bei approximativen Zellenzerlegungen ist jedoch nur
gewährleistet, dass bei beliebiger Verfeinerung der Auflösung irgendwann ein freier
Weg gefunden wird, falls ein solcher existiert.
4
ELEMENTARE PFADPLANUNGSMETHODEN
14
qinit
Cf ree
qgoal
(a)
3
12
2
qinit
4
11
20
5
7
9
8
10
13
qgoal
19
14
15
6
1
18
17
16
(b)
11
3
8
4
9
10
12
19
14
20
1
2
5
7
13
18
15
6
16
17
(c)
Abbildung 8: Beispiel für eine exakte Zellenzerlegung für den Fall, dass die CHindernis Region ein polygonales Gebiet des IR2 ist und der freie Konfigurationsraum durch ein C-Hindernis beschränkt ist. Der freie Konfigurationsraum kann hier
in Zellen zerlegt werden, mit denen ein connectivity graph“ gebildet und nach
”
einem Pfad von Zellen durchsucht werden kann.
4
ELEMENTARE PFADPLANUNGSMETHODEN
3
12
2
15
qinit
4
11
20
5
7
9
8
10
13
14
19
qgoal
15
6
18
1
17
16
(d)
3
12
'
'
'
'
'
(
(
(
(
(
'
(
2
'
(
'
(
qinit
'
'
'
'
(
(
(
(
'
'
'
'
(
(
(
(
4
'
'
'
(
(
(
%
&
11
%
&
%
%
&
&
%
%
%
%
&
&
&
&
%
%
%
&
&
&
20
%
&
%
%
%
%
%
%
&
&
&
&
&
&
'
'
'
'
'
(
(
(
(
(
%
%
%
%
%
%
&
&
&
&
&
&
'
(
5
9
'
'
'
(
(
(
'
'
'
'
'
(
(
(
(
(
8
'
(
'
(
'
'
'
'
(
(
(
(
7
'
(
!
"
%
%
%
%
%
%
&
&
&
&
&
&
%
%
%
%
%
&
&
&
&
&
"
%
%
%
%
%
%
&
&
&
&
&
&
10
19
%
&
!
%
&
14
%
&
%
%
%
%
&
&
&
&
'
'
'
'
'
(
(
(
(
(
13
%
&
'
'
'
'
'
(
(
(
(
(
#
#
#
#
$
$
$
$
%
%
%
%
&
&
&
&
#
#
$
%
$
%
&
qgoal
%
&
%
%
%
%
&
&
&
&
&
#
#
#
#
#
#
$
$
$
$
$
$
%
%
%
&
&
%
%
&
&
&
%
&
#
#
#
$
$
$
#
$
%
%
&
&
#
#
#
#
$
$
$
$
#
#
#
$
$
$
15
#
$
%
&
#
$
1
%
%
&
&
%
&
18
#
$
#
#
#
$
$
$
16
#
$
6
17
(e)
Abbildung 9: Ein Pfad des connectivity graph“ bestimmt einen Kanal in Cfree .
”
Hieraus erhält man einen freien Weg, indem man beispielsweise qinit und qgoal
über die Mittelpunkte der Schnittkanten jeder zwei nachfolgenden Zellen verbindet.
qgoal
R
qinit
qgoal
(a)
qinit
(b)
Abbildung 10: Beispiel für eine approximative Zellenzerlegung. Es ist Cfree durch
ein Rechteck R beschränkt, welches durch die Zerlegungen sukzessiv in vier gleich
große Rechtecke weiter aufgeteilt wird.
4
ELEMENTARE PFADPLANUNGSMETHODEN
4.3
16
Potentialfelder
Es soll hier nur kurz die Idee dieser Methode formuliert werden. Die Grundidee ist,
dass man den Roboter als Teilchen im Konfigurationsraum betrachtet, der Kräften
eines künstlich erzeugten Potentialfeldes ausgesetzt ist. Hierbei herrscht bei der Zielkonfiguration ein anziehendes“ Potential, welches den Roboter in Richtung Ziel zu
”
bewegen versucht. Die C-Hindernisse erzeugen hingegen ein abstoßendes“ Potenti”
al, welches den Roboter von diesen wegdrückt. Der negative Gradient des gesamten
Potentials kann als künstliche Kraft aufgefasst werden, die auf den Roboter wirkt.
Die nachfolgende Abbildung veranschaulicht die einzelnen Potentialfelder und das
durch Überlagerung entstehende Gesamtpotential. Das anziehende Potential hat
hierbei ein Minimum bei der Zielkonfiguration. Das abstoßende Potential ist nur
innerhalb einer gewissen Entfernung von den C-Hindernissen nicht Null und strebt
gegen unendlich, wenn der Abstand zu den C-Hindernissen gegen Null geht (ist also
in der Zeichnung abgeschnitten worden). Indem man beispielsweise dem negativen
Gradienten des Potentials (also der künstlich ausgeübten Kraft) folgt, kann man
versuchen, zur Zielkonfiguration zu gelangen. Allerdings garantiert diese Methode
nur das Auffinden eines lokalen Minimums des Potentials, so dass eventuell kein
Weg von qinit nach qgoal produziert wird, obwohl ein solcher existiert. Um dies zu
verhindern kann man versuchen, Potentialfelder zu konstruieren, die keine Minima
außer das globale bei qgoal besitzen.
Abbildung 11: Potentialfeldmethode: das durch Addition der Felder aus (b) und (c)
entstehende Feld (d) kann zum Aufsuchen eines freien Weges verwendet werden.
5
ERGÄNZUNGEN
5
5.1
5.1.1
17
Ergänzungen
Graphensuchalgorithmen
Tiefen- und Breitensuche
Sei G = (X, A) ein endlicher gerichteter Graph mit Knoten X und Kanten A. Sei
n die Anzahl der Knoten und r die der Kanten, wobei im worst-case r ∈ O(n2 ) ist.
Wir nehmen hier immer an, dass r ≥ n.
Suche in die Tiefe und Suche in die Breite sind zwei einfache Algorithmen zum
Durchsuchen von Graphen und zur Aufstellung eines spannenden Baumes T . Bei
der Suche in die Tiefe wird ausgehend von einem Startknoten dieser als besucht
markiert und die Suche bei einer unmarkierten Nachbarecke iterativ fortgeführt.
Außerdem wird T um diesen Knoten N und die Kante, die diesen Knoten mit dem
zuvor besuchten Knoten verbindet, erweitert. Der Algorithmus lautet also wie folgt:
dfs(vertex N)
{
visit(N);
for each neighbor N’ of N
if N’ is unvisited
{
dfs(N’);
add edge NN’ to tree T
}
}
Wenn r die Anzahl der Kanten bezeichnet, so besitzt die Tiefensuche Komplexität
O(n + r) bzw. unter obiger Annahme O(r).
Bei der Suche in die Breite werden dagegen zu einem aktuell besuchten Knoten
iterativ alle noch unbesichtigten Nachbarknoten besucht. Der Baum T wird wie
bei der Tiefensuche konstruiert. Die Breitensuche kann beispielsweise durch eine
Schlange implementiert werden und der Algorithmus lautet dann wie folgt:
unmark all vertices
choose some starting vertex N
mark N
queue OPEN = N
tree T = N
while OPEN nonempty
{
choose vertex N from OPEN
visit N
for each unmarked neighbor N’ of N
{
mark N’
add N’ to OPEN
add edge NN’ to T
}
}
Wieder beträgt die Komplexität O(n + r) bzw. unter obiger Annahme O(r).
5
ERGÄNZUNGEN
5.1.2
18
Der A? -Algorithmus
Nun nehmen wir an, die Kanten von G seien mit Kosten aus IR+ versehen. Ziel
ist es, einen bezüglich dieser Kosten kürzesten Weg zwischen einem gegebenem
Startknoten Ninit und einem Zielknoten Ngoal zu finden. Der A? - Algorithmus kann
als Verallgemeinerung des Algorithmus von Dijkstra gesehen werden, der ebenfalls
einen kürzesten Weg sucht.
Der A? -Algorithmus durchsucht G iterativ, indem er Pfaden ausgehend von Ninit
folgt. Für jeden besuchten Knoten N haben die vorhergehenden Iterationen bereits einen oder mehrere Pfade von Ninit nach N produziert, doch der Algorithmus
speichert nur den Pfad mit minimalen Kosten (unter diesen bisherigen Pfaden). Zu
jedem Zeitpunkt erzeugen diese Pfade einen spannenden Baum T mit den Knoten
aus G, die bisher besucht wurden. T wird hierbei wie üblich repräsentiert, indem
zu jedem besuchten Knoten N (außer Ninit ) ein Zeiger zum Vaterknoten verweist.
Das wesentliche Element des A? -Algorithmus ist die Tatsache, dass zu jedem Knoten
N aus dem aktuellen Baum T eine Funktion f (N ) berechnet wird. Diese Funktion
ist eine Schätzung für die Kosten des Pfades, der
• Ninit und Ngoal verbindet und durch den Knoten N geht und
• minimale Kosten unter all diesen Pfaden besitzt.
Die Funktion f (N ) wird dabei wie folgt berechnet:
f (N ) = g(N ) + h(N ),
wobei
• g(N ) die Kosten des Pfades zwischen Ninit und N im aktuellen Baum T sind,
sowie
• h(N ) eine heuristische Schätzung für die Kosten h? (N ) eines minimalen
Pfades von N nach Ninit in G sind.
Unten ist der A? -Algorithmus angegeben. Die Eingabe ist gerade G, Ninit , Ngoal , h
und k, wobei k : X ×X → IR+ die Kosten der Kanten von G angibt. Zu Beginn sind
alle Knoten als unbesucht markiert. Der Algorithmus benötigt eine Liste genannt
Open, die Knoten von G enthält, sortiert nach den Funktionswerten f . Die Liste
kann folgende Operationen ausführen:
• First(Open): Entferne den Knoten von Open, mit dem geringsten Wert von
f und gib ihn aus,
• Insert(N, Open): Füge den Knoten N zu Open hinzu,
• Delete(N, Open): Entferne den Knoten N aus Open,
• MEMBER(N, Open): Gibt true aus, falls N in Open ist und sonst false
• EMPTY(Open): Gibt true aus, falls Open leer ist und sonst false
Bei jeder Iteration untersucht der Algorithmus die Knoten, die benachbart sind zu
dem Knoten, der durch First(Open) ausgegeben wird. Zu Beginn seien sowohl der
Baum T , als auch die Liste Open beide leer.
5
ERGÄNZUNGEN
19
procedure A? (G, Ninit , Ngoal , k, h)
begin
install Ninit into T ;
Insert(Ninit , Open); mark Ninit visited ;
while ¬EMPTY(Open) do
begin
N ← First(Open);
if N = Ngoal then exit while-loop;
for every node N 0 adjacent to N in G do
if N 0 is not visited then
begin
add N 0 to T with a pointer toward N;
Insert(N 0 , Open); mark N’ visited ;
end;
else if g(N 0 ) > g(N ) + k(N, N 0 ) then
begin
modify T by redirecting the pointer of N 0 toward N ;
if MEMBER(N 0 , Open) then Delete(N 0 , Open);
Insert(N 0 , Open);
end;
end;
if ¬EMPTY(Open) then return the constructed path by tracing the pointers in T
from Ngoal back to Ninit ;
else return failure;
end;
Zu bemerken ist, dass falls die Untersuchung von N auf einen bereits besuchten
Knoten N 0 stößt, der Fall eintreten kann, dass der neue Weg N 0 zu erreichen vielleicht geringere Kosten produziert, als jeder der vorher generierten Pfade von Ninit
nach N 0 . Dann updatet der Algorithmus den Baum T , indem er den Zeiger ausgehend von N 0 (zu dessen Vaterknoten) auf N setzt. Wenn N 0 in Open ist, muss
dessen Position in der Liste gemäß des neuen Wertes von f (N 0 ) angepasst werden.
Außerdem kann es sein, dass der neue Pfad zwischen Ninit und N 0 auch kürzere
Wege liefert, um Knoten zu erreichen, die im aufspannenden Baum mit minimalen
Kosten Vorgänger von N 0 sind, es aber im aktuellen Baum T nicht sind. Diese Wege
werden jedoch nicht sofort untersucht, sondern stattdessen wird N 0 – falls es nicht
schon in Open war – in die Liste mit aufgenommen und später untersucht. (Also
enthält der Baum T zu jedem Zeitpunkt nicht notwendigerweise stets die kürzesten
unter den bereits betrachteten Wegen.)
Eine Variante des Algorithmus besteht darin, die Liste Open nicht zu sortieren und
dafür in jedem Schritt nach dem Knoten N mit dem kleinsten Funktionswert f zu
in Open zu suchen.
Eine heuristische Funktion h heißt zulässig, falls
∀N ∈ G :
0 ≤ h(N ) ≤ h? (N ).
Wenn h zulässig ist, so liefert der A? -Algorithmus stets einen kürzesten Weg, falls
ein solcher existiert und gibt sonst FEHLER zurück.
Eine triviale zulässige heuristische Funktion ist h1 (N ) = 0 ∀N . In diesem Fall erhält
man gerade den Dijkstra-Algorithmus. Eine andere zulässige heuristische Funktion
ist h2 (N ) = d(N, Ngoal ) für den Fall, dass die Knoten von G Punkte des IR2 sind
und k(N1 , N2 ) = d(N1 , N2 ) gerade der euklidische Abstand von N1 und N2 ist
(falls diese in G verbunden sind). Man kann zeigen, dass in diesem Fall h2 in einem
gewissen Sinn besser ist als h1 , genauer hierzu:
5
ERGÄNZUNGEN
20
Definition 15 Wenn h1 und h2 zwei zulässige heuristische Funktionen sind, so
heißt h2 informationsreicher als h1 , falls
h2 (N ) ≥ h1 (N )
für alle N aus G gilt.
2
Man kann zeigen, dass wenn h2 informationsreicher als h1 ist, jeder Knoten aus G,
der durch A? mit h2 untersucht wurde, auch durch A? mit h1 untersucht wird.
Definition 16 Eine heuristische Funktion h heißt lokal konsistent, falls
0 ≤ h(N ) ≤ h(N 0 ) + k(N, N 0 )
für alle Paare von benachbarten Knoten N und N 0 gilt.
2
Proposition 5 Wenn die Liste Open nicht sortiert ist, so ist die Komplexität von
A? bei Verwendung einer lokal konsistenten heuristischen Funktion gerade O(n2 ).
Wenn Open dagegen sortiert wird, so ist die Komplexität gerade O(r log n).
2
5.2
Der sweep-line Algorithmus
Der sweep-line Algorithmus tritt im Rahmen der Pfadplanung in verschiedenen
Modifikationen häufiger auf. In seiner Grundformulierung behandelt er folgendes
Problem:
Gegeben sind n Geradensegmente, jede beschrieben durch die Koordinaten (x, y) der beiden Endpunkte. Berechne alle Schnittpunkte dieser
Segmente und ordne diese aufsteigend nach der x-Koordinate.
Ein naiver Ansatz besteht darin, alle Paare O(n2 ) von Geradensegmente zu betrachten, zu prüfen, ob diese sich schneiden und wenn ja, den Schnittpunkt zu berechnen.
Sei c die Anzahl der Schnittpunkte ist. Im worst case ist c ∈ O(n2 ), aber im Allgemeinen wird c viel kleiner sein. Bei dem naiven Ansatz werden die Schnittpunkte
gerade in O(n2 ) berechnet und die Sortierung benötigt dann noch Zeit O(c log c),
also insgesamt ist die Komplexität O(n2 + c log c).
Ein sweep-line Algorithmus löst das Problem dagegen in O(n log n + c log n) und
eignet sich damit, falls die Anzahl der Schnittpunkte klein ist. Die Idee besteht darin,
eine vertikale Gerade L von links nach rechts entlang der x-Achse zu verschieben. Zu
jedem x-Abschnitt x0 schneidet die Gerade L eine Teilmenge der gegebenen Gerade,
welche auch als bei x0 aktive Segmente bezeichnet werden. Der Status S von L
ist definiert als die Menge der aktiven Segmente, sortiert nach den y-Koordinaten
des Schnittpunktes mit L. In Abbildung 12 ist zur Abszisse x0 der Status S gerade
S = (D B A). Wenn zwei Segmente X1 und X2 aus S benachbart sind, und X1 vor
X2 kommt, so heißt X1 unterhalb X2 und X2 oberhalb X1 .
Während die Gerade L entlang der x-Achse verschoben wird, so ändert sich S nur
bei endlich vielen sogenannten kritischen Abszissen xc , die von einem der folgenden
Typen sind:
(1.) xc ist Abszisse des linken Endpunkts eines Geradensegments X. X wird dann
in S aufgenommen.
(2.) xc ist Abszisse des rechten Endpunkts eines Geradensegments X. X wird dann
aus S entfernt.
5
ERGÄNZUNGEN
21
(3.) xc ist Abszisse eines Schnittpunkts zweier Geradensegmente X1 und X2 . X1
und X2 vertauschen dann ihre Position in S.
Im folgenden nehmen wir an, dass kein Gerademsegment vertikal verläuft und dass
keine zwei kritischen Abszissen gleich sind. Durch einfache Anpassungen des Algorithmus können diese Fälle ebenfalls behandelt werden.
y
L
A
B
C
D
E
x0
x
Abbildung 12: Die Abbildung zeigt 5 Liniensegmente (A, B, C, D, E) in der Ebene.
Beim sweep-line Algorithmus wird die Gerade L entlang der x-Achse verschoben.
Zum Zeitpunkt x0 ist der Zustand S gerade (D B A).
Der sweep-line Algorithmus verwendet folgende Datenstrukturen:
• Eine ist der aktuelle Status S von L. Diese Struktur unterstützt folgende
Operationen:
– Insert(X, S): Füge X in S ein
– Delete(X, S): Entferne X aus S
– Above(X, S): Gebe falls vorhanden, das Segment oberhalb von X in S
zurück
– Below(X, S): Gebe falls vorhanden, das Segment unterhalb von X in S
zurück
• Die andere Datenstruktur E ist eine Schlange, die zukünftige Aktionen verwaltet. Sie unterstützt folgende Operationen:
– First(E): Entferne das Element mit der kleinsten Abszisse aus E und
gib es aus
– Insert(x, E): Füge die Abszisse x in E ein
– MEMBER(x, E): Überprüfe, ob Abszisse x in E vorkommt
Zu Beginn sind sowohl S als auch E beide leer. Es werden die 2n Abszissen der Endpunkte der Geradensegmente in E aufgenommen. Dann verfährt der Algorithmus
iterativ wie folgt. Bei jeder Iteration wird die kleinste Abszissen aus E entfernt (d. h.
die nächste Aktion) und S und E werden gemäß des Typs der Aktion upgedatet. Das
Update von S kann das Hinzufügen oder Entfernen eines Geradensegment, oder die
Vertauschung der Ordnung zweier benachbarter Geradensegmente darstellen. Für
jedes Paar von Geradensegmenten, die durch ein solches Update benachbart werden, wird überprüft, ob sich diese schneiden. Wenn dies der Fall ist, und dadurch
ein neuer Schnittpunkt entsteht, wird dieser in E eingefügt. Jedes Mal, wenn ein
5
ERGÄNZUNGEN
22
Schnittpunkt aus E entfernt wird, so wird dieser in eine weitere Schlange L gegeben,
welche zum Schluss die Schnittpunkte sortiert nach Abszissen enthält.
procedure Linesweep;
begin
place the abscissae of the 2n endpoints of the line segments into E;
S ← ∅; L ← ∅; Q ← ∅;
[Q is a queue used internally by the procedure]
while E 6= ∅ do
begin
x ← First(E);
p ← endpoint or intersection point of which x is the abscissa;
if p is a left endpoint then
begin
X ← segment of which p is endpoint;
Insert(X, S);
X1 ← Above(X, S);
X2 ← Below(X, S);
if X1 intersects X then Insert((X1 , X), Q);
if X2 intersects X then Insert((X, X2 ), Q);
end;
else
if p is a right endpoint then
begin
X ← segment of which p is endpoint;
X1 ← Above(X, S);
X2 ← Below(X, S);
if X1 intersects X2 then Insert((X1 , X2 ), Q);
Delete(X, S);
end;
else [p is an intersection point]
begin
L ← Insert(x, L);
(X1 , X2 ) ← pair of segments intersecting at p;
[with X1 = Above(X2 ) on the left of p]
X3 ← Above(X1 , S);
X4 ← Below(X2 , S);
if X3 intersects X2 then Insert((X3 , X2 ), Q);
if X1 intersects X4 then Insert((X1 , X4 ), Q);
interchange X1 and X2 in S;
end;
while Q =
6 ∅ do
begin
(X, X 0 ) ← First(Q);
x ← abscissa of the intersection point of X and X 0 ;
if ¬MEMBER(x, E) then Insert(x, E);
end;
end;
end;
Zusammenfassend kann man festhalten, dass die Hauptidee des sweep-line Algorithmus folgende ist: verschiebe L entlang der x-Achse und berechne jeweils, ob sich in
S benachbarte Geradensegmente schneiden. Wie man sich leicht überlegt, muss der
nächste Schnittpunkt zweier Geraden dann genau derjenige dieser Schnittpunkte sein, der die geringste Abszisse besitzt. Bei einem solchen Schnittpunkt ändert
sich in S gerade die Position zweier Geraden und somit kann man iterativ fortfahren, indem man jeweils nur an den linken Endpunkten, den bislang berechneten
5
ERGÄNZUNGEN
23
Schnittpunkten und den rechten Endpunkten stoppt. Wie bereits oben erwähnt besitzt der sweep-line Algorithmus die Komplexität O(n log n + c log n), wobei c die
Gesamtanzahl der Schnittpunkte aller Geraden bezeichnet.
Literatur:
Jean-Claude Latombe
Robot Motion Planning
1991, Kluwer Academic Publishers
Insbesondere Kapitel 1-4 (bis Seite 176),
sowie Anhang S. 602-614 zu den Ergänzungen aus Abschnitt 5.