Robot Motion Planning

Robot Motion
Planning
Stefan Edelkamp
Überblick
- Kürzeste Wege
- Sichtbarkeitsgraphen
- Kürzeste Wege für punktförmige Roboter
- Kürzeste Wege für polygonale Roboter
- Trapezzerlegung
- Wegeberechnung
- Minkowski-Summen
- Bewegungsplanung mit Translation und
Rotation
1
Kürzeste Wege für Punktroboter
Gegeben:
• Menge H von disjunkten polygonalen
(offenen) Hindernissen mit insgesamt n
Kanten
• Startposition ps, Zielposition pe, beide
in Cf ree
Gesucht:
Der kürzeste Weg von ps nach pe in Cf ree
ps
pe
ps
pe
2
Eigenschaften kürzester Wege
Gummiband-Eigenschaft:
pe
ps
Definition
Sei W ein polygonaler Weg von ps nach pe.
Dann heißen die Ecken von W , die ungleich ps
oder pe sind, innere Ecken von W .
ps
pe
3
Eigenschaften kürzester Wege
Lemma
Jeder kürzeste Weg W von ps nach pe
zwischen einer Menge H von disjunkten
polygonalen Hindernissen ist ein polygonaler
Weg, dessen innere Ecken Ecken von H sind.
Beweis :
Polygonalität:
Angenommen, W ist nicht polygonal
⇒ Es gibt ein p ∈ W im Inneren von Cf ree ,
so daß kein Liniensegment in W p
enthält.
⇒ Es gibt einen Kreis in Cf ree, der p
enthält
W
4
Eigenschaften kürzester Wege
Beweis : (Fortsetzung)
Ecke v von W :
Angenommen, v ist keine Ecke von H
⇒ v ist nicht im Inneren von Cf ree
⇒ v ist im inneren einer Kante
⇒ Es gibt einen Halbkreis in Cf ree , der v
enthält
W
5
Eigenschaften kürzester Wege
Ein kürzester Weg von ps nach pe zwischen
einer Menge H von disjunkten polygonalen
Hindernissen besteht aus:
1. Liniensegmenten von ps bzw. pe zu Ecken
von H und
2. Liniensegmenten von einer Ecke von H zu
einer anderen.
Definition
Zwei Punkte v1 und v2 heißen gegenseitig
sichtbar, falls das Liniensegment v1v2 nicht
das Innere eines Hindernisses schneidet.
6
1
Sichtbarkeitsgraphen
Sichtbarkeitsgraph Gvis(H) von H:
Knoten Vvis (H) von Gvis(H): Ecken von H
Kanten Evis(H) von Gvis(H):
(v1, v2) ∈ Evis(H), falls v1 und v2
gegenseitig sichtbar
sind.
Sichtbarkeitsgraphen
7
Sichtbarkeitsgraphen und kürzeste
Wege
Ein kürzester Weg von ps nach pe zwischen
einer Menge H von disjunkten polygonalen
Hindernissen besteht aus Kanten des
Sichtbarkeitsgraphen Gvis(H∗), wobei
H∗ = H ∪ {ps, pe}.
pe
ps
Sichtbarkeitsgraphen
8
Berechnung kürzester Wege
Algorithmus kürzesterWeg
Input:
Eine Menge H von disjunkten
polygonalen Hindernissen und zwei
Punkte ps und pe in Cf ree
Output: Der kürzeste kollisionsfreie Weg
von ps nach pe
1
Gvis ← Sichtbarkeitsgraph(H ∪ {ps, pe })
2
Gewichte jede Kante (u, v) in Gvis mit
der euklidischen Distanz von u nach
v
3
Berechne einen kürzesten Weg W von
ps nach pe in Gvis mit dem
Algorithmus von Dijkstra
4
Gebe W zurück
Analyse
|Evis| ≤
n + 2
2
Sichtbarkeitsgraphen
= O(n2), Dijkstra O(n log n)
9
2
Berechnung des
Sichtbarkeitsgraphen
Naives Verfahren:
Algorithmus Sichtbarkeitsgraph1
Input:
Eine Menge H von disjunkten
polygonalen Hindernissen
Output: Der Sichtbarkeitsgraph Gvis(H) von
H
1
Gvis ← (Vvis , Evis) mit
2
Vvis ← die Menge der Eckpunkte
von H
3
Evis ← ∅
4
for all (u, v) ∈ Vvis × Vvis
;;
O(n2)
5
if uv schneidet kein Hindernis
;;
O(n)
6
then füge (u, v) zu Evis hinzu
7
Gebe Gvis = (Vvis , Evis) zurück
⇒ O(n3)
Berechnung des Sichtbarkeitsgraphen
10
Berechnung des
Sichtbarkeitsgraphen
Besserer Algorithmus:
Algorithmus Sichtbarkeitsgraph2
Input:
Eine Menge H von disjunkten
polygonalen Hindernissen
Output: Der Sichtbarkeitsgraph Gvis(H) von
H
1
Gvis ← (Vvis , Evis) mit
2
Vvis ← die Menge der Eckpunkte
von H
3
Evis ← ∅
4
for all v ∈ Vvis
{ O(n) }
5
W ← SichtbareEcken(v, H)
for all w ∈ W
6
7
füge die Kante (v, w) zu Evis
hinzu
8
Gebe Gvis = (Vvis , Evis) zurück
Berechnung des Sichtbarkeitsgraphen
11
Sichtbarkeitspolygon
Definition
Die Menge der Punkte, die von einem Punkte
p der Ebene aus sichtbar sind, heißt das
Sichtbarkeitspolygon von p.
Hier: von p aus sichtbare Ecken
Berechnung des Sichtbarkeitsgraphen
12
Berechnung der sichtbaren Ecken
Idee:
• Sortiere die Ecken zyklisch um p
• Betrachte die Ecken in dieser
Reihenfolge mit Hilfe eines
Scanstrahls ̺
• Verwalte die Kanten, die von ̺
geschnitten werden, in einer
Suchstruktur
e1
e8
p
e7
e6
e4
e3
e2
̺
e5
T
e7
e3
e2
e4
e6
e8
Berechnung des Sichtbarkeitsgraphen
13
Rotationsweep Events
e2
sichtbar(v)
delete(e1 )
insert(e2 )
v
p
e1
e3
e1
p
v e2
p
v′
insert(e3 )
v
e2
sichtbar(v)
sichtbar(v ′ )
insert(e1 )
e1
Berechnung des Sichtbarkeitsgraphen
sichtbar(v)
delete(e1 )
delete(e2 )
14
Sichtbare Ecken
Algorithmus SichtbareEcken
Input:
Menge H von Polygonen und Punkt
p nicht im Innern eines Polygons
Output: Die von p aus sichtbaren Ecken in H
1
2
3
4
5
6
7
8
9
10
(v1, . . . , vn) ← Sortiere Ecken v nach
winkel(p, v) ;; O(n log n)
̺ ← horizontale Strahl beginnend bei p
Finde alle Kanten e die ̺ schneiden
und speichere sie in einem Suchbaum
T , ;; O(n) + O(n log n)
W ←∅
for i ← 1 to n
if sichtbar(vi )
; ; O(log n)
then W ← W ∪ {vi}
füge die zu vi inzidenten
Kanten links von ̺ in T
;; O(log n)
ein;
entferne die zu vi inzidenten
Kanten rechts von ̺ aus
T
;; O(log n)
return W
⇒ O(n log n)
Berechnung des Sichtbarkeitsgraphen
15
Sichtbarkeit von Ecken
Sichtbarkeit von vi ist abhängig von:
• Suchstruktur T
• Ecke vi−1
Verschiedene Fälle: vi−1 ist sichtbar
p
vi−1
p
vi−1
vi
p
vi−1
vi
p
vi−1
vi
vi
Berechnung des Sichtbarkeitsgraphen
16
Sichtbarkeit von Ecken
Algorithmus sichtbar
1
if pvi schneidet das Polygon von vi
lokal bei vi
2
then return
6
7
if i = 1 or vi−1 6∈ pvi
then e ← linkeste Kante in T
if e existiert nicht or
vi auf oder links von e
then return true
else return false
8
9
/∗ i 6= 1 and vi−1 ∈ pvi ∗/
if vi−1 ist nicht sichtbar
then return false
3
4
5
10
11
12
13
/∗ vi−1 ist sichtbar ∗/
Suche Kante e in T , die vi−1vi
schneidet
if e existiert
then return false
else return
Berechnung des Sichtbarkeitsgraphen
17
Analyse von SichtbareEcken
Sortieren: O(n log n)
Aufbau von T : O(n2 log n)
sichtbar(vi ): n × O(log n)
Update von T : n × O(log n)
⇒ O(n log n + Evis)
Satz
Der Sichtbarkeitsgraph einer Menge H von
disjunkten polygonalen Hindernissen mit n
Kanten kann in Zeit O(n2 log n) berechnet
werden.
Berechnung des Sichtbarkeitsgraphen
18
3
Wege für polygonale Roboter
Translatorisches Bewegungsplanungproblem
für polygonalen Roboter R zwischen
Hindernissen {P1, . . . , Pt} entspricht
Bewegungsplanungsproblem für Punktroboter
zwischen Hindernissen {−R ⊕ P1, . . . , −R ⊕ Pt}.
Wege für polygonale Roboter
19
4
Trapezzerlegung
Annahmen:
• punktförmiger Roboter R
• polygonale, disjunkte Hindernisse
H = {P1, . . . , Pt}
mit insgesamt n Eckpunkten
• Rechteck B enthält die Hindernisse
Cf ree = Cf ree(R, H) = B \
t
[
Pi
i=1
P6
R
P1
P3
P4
P2
Trapezzerlegung
B
P5
Cf ree
20
Der freie Raum
Trapez-Zerlegung:
for all v Eckpunkt in H
zeichne ein vertikales Liniensegment
in v nach oben und eines nach
unten
die Liniensegmente enden beim
ersten Hindernis oder bei B
Algorithmus BerechneFreienRaum(H)
Input:
Eine Menge H von disjunkten,
polygonalen Hindernissen
Output: Die vertikale Dekomposition von
Cf ree(R,H) für einen Punktroboter
R
1
Sei E die Menge der Kanten in H
2
Berechne die Trapez-Zerlegung T (E)
von E
3
Entferne die Trapeze im inneren der
Hindernisse von H
4
Gebe die entstehende Unterteilung
T (Cf ree) zurück
Trapezzerlegung
21
Der freie Raum
Lemma
Die Trapez-Zerlegung des freien
Konfigurationsraumes kann in O(n log n)
erwarteter Zeit berechnet werden.
Trapezzerlegung
22
Berechnung eines Weges
Gegeben: ps der Startpunkt und pe der
Zielpunkt
Gesucht: Ein kollisionsfreier Weg von ps
nach pe
1. Fall: ps und pe gehören zum gleichen
Trapez
pe
ps
2. Fall: ps und pe gehören zu verschiedenen
Trapezen ⇒ ??
Trapezzerlegung
23
Die Straßenkarte (Road map)
Straßenkarte Groad: planarer, in Cf ree eingebetter Graph
Knoten von Groad: Trapez- und vertikale
Kanten-Mittelpunkte
Kanten von Groad:
for all Trapeze T
füge Kanten von dem Mittelpunkt
von T zu den Mittelpunkten der T
be- grenzenden vertikalen Kanten
ein
Trapezzerlegung
24
Die Straßenkarte (Road map)
ps
pe
ps
pe
Zeit zur Konstruktion der Straßenkarte: O(n)
Trapezzerlegung
25
Algorithmus zur Wegeberechnung
Algorithmus BerechneWeg
Input:
vertikale Dekomposition T (Cf ree ),
Straßenkarte Groad, Start- und
Zielpunkt ps und pe
Output: Ein Weg von ps nach pe , falls einer
existiert; ansonsten eine Meldung,
daß kein Weg existiert
1
2
3
4
5
6
7
8
Bestimme Trapeze ∆s und ∆e, die ps
und pe enthalten
if ∆s oder ∆e existiert nicht
then melde, daß ps oder pe in Cf orb
else seien vs und ve Mittelpunkte
von ∆s bzw. ∆e
berechne Weg W von vs nach ve
in Groad
if W existiert nicht
then melde, daß kein Weg
existiert
else return
W ∗ = psvs ◦ W ◦ vepe
Trapezzerlegung
26
Korrektheit von BerechneWeg
Lemma
Algorithmus BerechneWeg gibt einen
kollisionsfreien Weg zurück gdw es einen
solchen Weg gibt.
Beweis
⇒: W ∗ ist kollisionsfrei
⇐: Sei W ′ kollisionfreier Weg von ps nach pe
W ′ schneidet eine Folge F von benachbarten
Trapezen:
F = (∆1, ∆2, . . . , ∆k )
mit
∆1 = ∆s
und
∆5
pe
∆1
ps
∆4
∆2
Trapezzerlegung
∆k = ∆e
∆3
27
Korrektheit von BerechneWeg
Beweis (Fortsetzung)
Sei vi Mittelpunkt von ∆i
∆i und ∆i+1 sind benachbart
⇒ Es gibt einen Weg von vi nach vi+1 in
Groad
⇒ Es gibt einen Weg von v1 nach vk in Groad
⇒ Der Algorithmus findet einen Weg von vs
nach ve
⇒ Der Algorithmus findet einen Weg von ps
nach pe
∆5
pe
∆1
ps
∆4
∆2
∆3
Zeitkomplexität von BerechneWeg: O(n log n)
(erwartet)
Trapezzerlegung
28
5
Minkowski Summen
Ziel: Berechnung des freien Konfigurationsraumes für polygonale Roboter
Definition
Konfigurationshindernis oder C-Hindernis C(P)
zu Hindernis P:
C(P) = {(x, y) | R(x, y) ∩ P 6= ∅}
Minkowski Summen
29
Minkowski Summe
Definition
Seien S1, S2 ⊆ IR2. Die Minkowski-Summe von
S1 und S2 ist definiert als
S1 ⊕ S2 = {p + q | p ∈ S1, q ∈ S2}
wobei mit p = (p1, p2) und q = (q1, q2)
p + q = (p1 + q1, p2 + q2)
Ferner sei
−p = (−p1, −p2)
und
−S = {−p | p ∈ S}
P ⊕R
P
R
Minkowski Summen
30
C-Hindernisse
Satz
C(P) = P ⊕ (−R(0, 0))
Beweis
“⊆”: Sei q ∈ C(P) und p ∈ P ∩ R(q)
⇒ p − q ∈ R(0, 0)
⇒ −p + q ∈ {(0, 0)} ⊕ (−R(0, 0))
⇒
q ∈ {p} ⊕ (−R(0, 0)) ⊆ P ⊕ (−R(0, 0))
“⊇”: Sei q ∈ P ⊕ (−R(0, 0))
⇒ q = p − r, mit p ∈ P, r ∈ R(0, 0)
⇒ p=q+r
⇒ p ∈ P ∩ R(q)
Minkowski Summen
31
Extreme Punkte
Beobachtung
Sei
Q = P ⊕ R,
dann ist ein in Richtung d~ extremer Punkt von
Q die Summe von in Richtung d~ extremen
Punkten von P und R.
P ⊕R
d~
P
R
Minkowski Summen
32
Komplexität von P ⊕ R
Satz
Seien P und R konvex mit n bzw. mit m
Kanten, dann ist
P ⊕R
ein konvexes Polygon mit höchstens n + m
Kanten.
Beweis Konvexität von P ⊕ R folgt aus der
Definition
Sei e eine Kante von P ⊕ R
⇒ e ist extrem in Richtung ihrer äußerer
Normalen ~
ue
⇒ e wird von in Richtung ~
ue extremen
Punkten von P und R erzeugt
⇒ es gibt eine in Richtung ~
ue extreme
Kante e′ von P oder R
⇒ weise e der Kante e′ zu
Minkowski Summen
33
Berechung der Minkowski-Summe
Definition
winkel(p, q)
p
q
Algorithmus Minkowski-Summe
Input:
Konvexe Polygone P = (v1, . . . , vn)
und R = (w1, . . . , wm) mit v1 und
w1 y-minimale Ecken
Output: Die Minkowski-Summe P ⊕ R
1
i ← 1; j ← 1
2
vn+1 ← v1; wm+1 ← w1
repeat
3
4
Füge vi + wj als Ecke zu P ⊕ R hinzu
5
if winkel(vi , vi+1) < winkel(wj , wj+1)
6
then i ← i + 1
if winkel(vi , vi+1) > winkel(wj , wj+1)
7
8
then j ← j + 1
9
if winkel(vi , vi+1) = winkel(wj , wj+1)
10
then i ← i + 1, j ← j + 1
11 until i = n + 1 ∧ j = m + 1;
Minkowski Summen
34
Beispiel
v3
w3
w4
R
v2
P
w2
v1
w1
Satz
Die Minkowski-Summe zweier konvexer
Polygone mit n bzw. m Kanten kann in Zeit
O(n + m) berechnet werden.
Minkowski Summen
35
Pseudo-Scheiben
Definition
Zwei planare Objekte O1 und O2 heißen
Pseudo-Scheiben, falls O1 \ O2 und O2 \ O1
zusammenhängend ist.
Beobachtung
Die Ränder zweier Pseudo-Scheiben haben
höchstens zwei echte Kreuzungen.
Minkowski Summen
36
Extreme Punkte
Definition
Seien P1 und P2 zwei Polygone mit
disjunktem Innerem in der Ebene. P1 heißt
extremer in Richtung d~ als P2, falls P1 Punkte
besitzt, die weiter in Richtung d~ liegen als P2.
d~1
P2
d~2
P1
d~3
Minkowski Summen
37
Extreme Punkte
Beobachtung
Seien P1 und P2 zwei Polygone in der Ebene
mit disjunktem Inneren und d~1 und d~2 zwei
Richtungen, so daß P1 extremer als P2 in
Richtung d~1 und d~2 ist.
Dann ist P1 entweder in allen Richtungen von
d~1 nach d~2 oder in allen Richtungen von d~2
nach d~1 extremer als P2.
d~2
P2 ist extremer
P2
d~1
P1
d~2
d~3
d~1
d~3
P1 ist extremer
Minkowski Summen
38
Minkowski-Summen als
Pseudo-Scheiben
Satz
Seien P1 und P2 zwei konvexe Polygone mit
disjunktem Innerem und R ein weiteres
konvexes Polygon. Dann sind C1 = P1 ⊕ R und
C2 = P2 ⊕ R Pseudo-Scheiben.
Beweis
Zu zeigen: C1 \ C2 und C2 \ C1 sind
zusammenhängend
Annahme: C1 \ C2 besteht aus mindestens
zwei Zus.hangskomponenten Z1
und Z2
C1 und C2 sind konvex
⇒ es gibt d~1 ∈ Z1 und d~2 ∈ Z2 mit C1 ist
extremer als C2 in Richtung d~1 und d~2
⇒ P1 ist extremer als P2 in Richtung d~1 und
d~2
P1 und P2 haben disjunkte Innere
⇒ P1 ist extremer als P2 für alle Richtungen
von d~1 bis d~2 (oder d~2 bis d~1)
⇒ C1 ist extemer als C2 für alle Richtungen
von d~1 bis d~2
Widerspruch!
Minkowski Summen
39
Vereinigung von Pseudo-Scheiben
Satz
Sei S eine Menge von polygonalen
Pseudo-Scheiben mit insgesamt n Kanten.
Dann ist die Komplexität ihrer Vereinigung V
höchstens 2n.
Beweis V enthält zwei Arten von Ecken
1. Ecken der Polygone in S und
2. Kantenschnittpunkte von Polygonen in S
zu 2.: Betrachte Kantenschnittpunkt
v = e ∩ e′
Endpunkt v ′ von e oder e′ liegt im
Inneren von V
⇒ Weise v v ′ zu
v ′ hat zwei inzidente Kanten
⇒ höchstens zwei zugewiesene Kantenschnittpunkte
Minkowski Summen
40
Nicht-konvexe Polygone
S1 ⊕ (S2 ∪ S3) = {s1 + s | s1 ∈ S1, s ∈ S2 ∪ S3}
= {s1 + s2 | s1 ∈ S1, s2 ∈ S2}
∪{s1 + s3 | s1 ∈ S1, s3 ∈ S3}
= (S1 ⊕ S2) ∪ (S1 ⊕ S3)
P nicht konvex:
1. trianguliere P ⇒ Dreiecke T1, . . . , Tn−2
P
2. berechne
P ⊕R=
n−2
[
Ti ⊕ R
i=1
Komplexität: O(nm)
Minkowski Summen
41
Nicht-konvexe Polygone
P ⊕R
P
R
Minkowski Summen
42
Nicht-konvexe Polygone
P und R nicht konvex:
1. trianguliere P ⇒ Dreiecke T1, . . . , Tn−2
2. trianguliere auch R ⇒ Dreiecke
U1, . . . , Um−2
3. berechne
P ⊕R=
n−2
[
[ m−2
Ti ⊕ U j
i=1 j=1
Komplexität: O(n2m2)
P
P ⊕R
R
Minkowski Summen
43
6
Bewegungsplanung mit
Translationen
C-Hindernisse: P ⊕ (−R(0, 0))
i
Satz
Sei R ein Roboter mit konstant vielen Kanten
und H eine Menge von Hindernissen mit n
Kanten. Dann ist die Komplexität des freien
Konfigurationsraumes für Translationen O(n).
Seien T1, . . . , Tn die Dreiecke einer
Triangulation der Hindernisse
Wir berechnen
Cf orb =
n
[
i=1
Ci =
n
[
Ti ⊕ (−R(0, 0))
i=1
Bewegungsplanung mit Translationen
44
Berechung von Cf orb
Algorithmus VerbotenerRaum
Input:
Eine Menge H = {T1, . . . , Tn} von
Dreiecken und ein Roboter R
Sn
S
C
=
Output: Cf orb = n
i=1 Ti ⊕(−R(0, 0))
i=1 i
1
if n = 1
then returnT1 ⊕ (−R(0, 0))
2
3
else Cf1orb = V erbotenerRaum(T1 , . . . , T⌊n/2⌋)
4
Cf2orb = V erbotenerRaum(T⌊n/2⌋+1, . . . , Tn)
5
berechne Cf orb = Cf1orb ∪ Cf2orb
return Cf orb
6
Bewegungsplanung mit Translationen
45
Analyse VerbotenerRaum
Triangulierung: |Pi| = mi
t
X
mi log mi ≤
t
X
mi log n = n log n
i=1
i=1
Berechnung der C-Hindernisse:
n
X
O(1) = O(n)
i=1
Berechnung von Cf1orb ∪ Cf2orb :
|Cf1orb| = n1, |Cf2orb| = n2, |Cf orb| = k
O((n1 + n2 + k) log(n1 + n2)) = O(n log n)
Rekursion:
T (n) = T (⌊n/2⌋) + T (⌈n/2⌉) + O(n log n)
= O(n log2 n)
Bewegungsplanung mit Translationen
46
Zusammenfassung
Satz
Sei R ein konvexer Roboter mit konstant
vielen Kanten, der translatorische Bewegungen
zwischen einer Menge H von disjunkten
Hindernissen mit insgesamt n Kanten ausführt.
Man kann H in O(n log2 n) erwarteter Zeit
vorverarbeiten, so daß für jede Start- und
Zielposition ein kollisionsfreier Weg für R,
sofern er existiert, in O(n) Zeit berechnet
werden kann.
Bewegungsplanung mit Translationen
47
7
Bewegungsplanung mit
Translationen und Rotationen
• konvexer, polygonaler Roboter R
• polygonale, disjunkte Hindernisse
H = {P1, . . . , Pt}
• drei Freiheitsgrade: zwei translatorische
und einen für die Rotation
Konfigurationsraum:
(x, y, ϕ) ∈ C(R) = IR2 × [0, 2π) 7→ R(x, y, ϕ)
Konfigurationshindernisse:
C(Pi) = {(x, y, ϕ) | R(x, y, ϕ) ∩ Pi 6= ∅}
Bewegungsplanung mit Translationen und Rotationen
48
Konfigurationshindernisse
Betrachte Schnitte mit den Ebenen h : ϕ = ϕ0
P
R
0
π/4
π/2
3π/4
π
Minkowski-Summe
Pi + (−R(0, 0, ϕ))
ändert sich stetig in ϕ.
Bewegungsplanung mit Translationen und Rotationen
49
Ein Algorithmus
Definition
Der Schnitt des Konfigurationsraums von R
mit einer Ebene h : ϕ = ϕ0 heißt Scheibe Sϕ0
des Konfigurationsraums.
Idee:
• Zerteile Konfigurationsraum in Scheiben
• Zerlege die Bewegung des Roboters in
Translationen innerhalb einer Scheibe
und
• Rotationen zwischen Scheiben
m = Anzahl der Scheiben
Scheiben werden für Winkel ϕi berechnet:
ϕi = i ×
2π
m
Bewegungsplanung mit Translationen und Rotationen
50
Ein Algorithmus
Algorithmus
ein konvexer polygonaler Roboter R
Input:
und eine Menge von Hindernisse H
Output: eine Straßenkarte Groad für C(R)
1
2
3
4
5
6
7
8
T−1 ← ∅; G−1 ← (∅, ∅); Groad ← ∅
for i ← 0 to m − 1
berechne Scheibe Sϕi von C(R, H)
berechne Trapezzerlegung Ti von
Sϕi
berechne Straßenkarte Gi von Ti
verknüpfe Gi mit Gi−1 und füge Gi
und die Verknüpfungen Groad hinzu
verknüpfe G0 mit Gm−1 und füge die
Verknüfungen zu Groad hinzu
return Groad
Bewegungsplanung mit Translationen und Rotationen
51
Verknüpfung zweier Straßenkarten
Algorithmus Verknüpfung von Gi mit Gi−1
1
Berechne die Überlagerung von Ti−1
und Ti
2
Sei U die Menge der Trapezpaare
(∆1, ∆2) ∈ Ti−1 × Ti mit
∆1 ∩ ∆2 6= ∅
for all (∆1, ∆2) ∈ U
3
4
Sei (x, y) der Mittelpunkt von
∆1 ∩ ∆2
5
Füge v1 = (x, y, ϕi−1) als Knoten zu
Gi−1 hinzu und verbinde v1 mit
dem Mittelpunkt von ∆1
6
Füge v2 = (x, y, ϕi) als Knoten zu Gi
hinzu und verbinde v2 mit dem
Mittelpunkt von ∆2
7
Verbinde v1 und v2 durch eine
Kante
111111
000000
000000
111111
Bewegungsplanung mit Translationen und Rotationen
52
Berechung eines Weges
Algorithmus
Startkonfiguration R(xs, ys, ϕs ) und
Input:
Zielkonfiguartion R(xz , yz , ϕz ),
Straßenkarte Groad
Output: Ein Weg von (xs, ys, ϕs) nach
(xz , yz , ϕz ) oder eine Meldung, daß
kein Weg existiert
1
is ← round(m ∗ ϕs); iz ← round(m ∗ ϕz )
2
Berechne Trapeze ∆s ∈ Tis und
∆z ∈ Tiz , die (xs, ys, ϕis ) und
(xz , yz , ϕiz ) enthalten
3
if ∆s oder ∆z existiert nicht
then melde es gibt keinen Weg;
4
return
5
Sei vs der Mittelpunkt von ∆s und vz
der Mittelpunkt von ∆z
6
Berechne einen Weg Wroad in Groad von
vs nach vz
if Wroad existiert nicht
7
8
then melde es gibt keinen Weg;
return
9
Gebe Weg von (xs, ys, ϕs) über
(xs, ys, ϕis ), vs, Wroad, vz , (xz , yz , ϕiz )
nach (xz , yz , ϕz ) zurück
Bewegungsplanung mit Translationen und Rotationen
53
Probleme
Algorithmus ist nicht korrekt
Beispiel:
(xs, ys, ϕs) ∈ Cf ree(R, H), aber
(xs, ys, ϕis ) 6∈ Cf ree(R, H)
Schlimmer:
Wege sind nicht kollisionsfrei
• Translationen innerhalb einer Scheibe
sind kollisionsfrei
• Rotationen sind zwischen freien
Konfigurationen
Lösung:
1. Erhöhung von m
2. Verwende vergrößerten (rotierten)
Roboter
Bewegungsplanung mit Translationen und Rotationen
54