Robot Motion Planning

Robot Motion Planning
Stefan Edelkamp
Überblick
- Kürzeste Wege
- Sichtbarkeitsgraphen
- Berechnung des 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 Sichtbarkeitsgraphen
2
= 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)
6
for all w ∈ W
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
v0
insert(e3 )
v
e2
sichtbar(v)
sichtbar(v 0 )
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 Sortiere die Ecken v nach winkel(p, v) (im Fall
gleicher Winkel nach Abstand zu p),
O(n log n)
(v1, . . . , vn) ← sortierte Reihenfolge
2 Sei % der horizontale Strahl beginnend bei p
3 Finde alle Kanten e die % schneiden und speichere
sie in einem Suchbaum T ,
{O(n) + O(n log n)}
4W ←∅
5 for i ← 1 to n
6
if sichtbar(vi ) {O(log n)}
7
then W ← W ∪ {vi}
8
füge die zu vi inzidenten Kanten links
von % in T ein; {O(log n)}
9
entferne die zu vi inzidenten Kanten
rechts von % aus T
{O(log n)}
10 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 false
3 if i = 1 or vi−1 6∈ pvi
4 then e ← linkeste Kante in T
5
if e existiert nicht or
vi auf oder links von e
6
then return true
7
else return false
/∗ i 6= 1 and vi−1 ∈ pvi ∗/
8 if vi−1 ist nicht sichtbar
9 then return false
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 true
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 ¨ur 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 einge- better
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 Bestimme Trapeze ∆s und ∆e, die ps und pe
enthalten
2 if ∆s oder ∆e existiert nicht
3 then melde, daß ps oder pe in Cf orb
4 else seien vs und ve Mittelpunkte von ∆s bzw.
∆e
5
berechne Weg W von vs nach ve in Groad
6
if W existiert nicht
7
then melde, daß kein Weg existiert
8
else return W ∗ = psvs ◦ W ◦ vepe
Trapezzerlegung
26
Beispiel
ps
Trapezzerlegung
pe
ps
pe
27
Korrektheit von BerechneWeg
Lemma
Algorithmus BerechneWeg gibt einen kollisionsfreien
Weg zurück gdw es einen solchen Weg gibt.
Beweis
⇒: W ∗ ist kollisionsfrei
⇐: Sei W 0 kollisionfreier Weg von ps nach pe
W 0 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
28
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
29
5
Minkowski Summen
Ziel: Berechnung des freien Konfigurations- raumes
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
30
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
31
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
32
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
33
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 e0 von P
oder R
⇒ weise e der Kante e0 zu
Minkowski Summen
34
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
3 repeat
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
7 if winkel(vi , vi+1) > winkel(wj , wj+1)
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 and j = m + 1
Minkowski Summen
35
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
36
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
37
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
38
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
39
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
40
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 ∩ e0
Endpunkt v 0 von e oder e0 liegt im Inneren von V
⇒ Weise v v 0 zu
v 0 hat zwei inzidente Kanten
⇒ höchstens zwei zugewiesene Kantenschnittpunkte
Minkowski Summen
41
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
42
Nicht-konvexe Polygone
P ⊕R
P
R
Minkowski Summen
43
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
44
6
Bewegungsplanung mit
Translationen
C-Hindernisse:
Pi ⊕ (−R(0, 0))
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
45
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
2 then return T1 ⊕ (−R(0, 0))
3 else Cf1orb = V erbotenerRaum(T1 , . . . , Tbn/2c)
4
Cf2orb = V erbotenerRaum(Tbn/2c+1 , . . . , Tn)
5
berechne Cf orb = Cf1orb ∪ Cf2orb
6
return Cf orb
Bewegungsplanung mit Translationen
46
Analyse VerbotenerRaum
Triangulierung: |Pi| = mi
t
X
mi log mi ≤
i=1
t
X
mi log n = n log n
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 (bn/2c) + T (dn/2e) + O(n log n)
= O(n log2 n)
Bewegungsplanung mit Translationen
47
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
48
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
49
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
50
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
51
Ein Algorithmus
Algorithmus
Input: ein konvexer polygonaler Roboter R und
eine Menge von Hindernisse H
Output: eine Straßenkarte Groad für C(R)
1 T−1 ← ∅; G−1 ← (∅, ∅); Groad ← ∅
2 for i ← 0 to m − 1
3
berechne Scheibe Sϕi von C(R, H)
4
berechne Trapezzerlegung Ti von Sϕi
5
berechne Straßenkarte Gi von Ti
6
verknüpfe Gi mit Gi−1 und füge Gi und die
Verknüpfungen Groad hinzu
7 verknüpfe G0 mit Gm−1 und füge die
Verknüfungen zu Groad hinzu
8 return Groad
Bewegungsplanung mit Translationen und Rotationen
52
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= ∅
3 for all (∆1 , ∆2) ∈ U
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
Bewegungsplanung mit Translationen und Rotationen
53
Berechung eines Weges
Algorithmus
Input: Startkonfiguration R(xs, ys, ϕs) und
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
4 then melde es gibt keinen Weg; 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
7 if Wroad existiert nicht
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
54
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
55