ΕΝΟΤΗΤΑ 2: Αυτόνομα Ευφυή Κινούμενα...

1

1Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτική ΙΙ (Κ. Τζαφέστας)

ΕΝΟΤΗΤΑ 2:Αυτόνομα Ευφυή Κινούμενα

Ρομποτικά Συστήματα

Ε.Μ.Π., ΣΗΜΜΥ, Ακαδημαϊκό Έτος 2009-10, 8ο ΕξάμηνοΜάθημα: Ρομποτική ΙΙ. Διδάσκων: Κ.Τζαφέστας

Σχεδιασμός Δρόμου – Πλοήγηση(path-planning, navigation)


Αυτόνομα Ευφυή Κινούμενα Ρομπότ – Περιεχόμενα Ενότητας

• Μηχανισμοί Κίνησης στο χώρο (locomotion)

• Αισθητήριες Διατάξεις (sensing)– Proprioception (internal state ) / Exteroception etc.– Σύνθεση αισθητηρίων πληροφοριών (sensor fusion)

• Αρχιτεκτονικές ελέγχου(mobile robot control architectures)

• Αυτοεντοπισμός θέσης / Χαρτογράφηση(self-localisation / map-building)

• Σχεδιασμός Δρόμου - Πλοήγηση (path planning / navigation)

perc

eptio

nac

tion

sens

ing

reas

onin

gm

otio

nco

ntro

l

2


Βασικό Σχήμα Αρχιτεκτονικής Ελέγχου

Αντίληψ

η(P

erce

ptio

n)

Μοντελοποίηση

(Mod

el)

Σχεδιασμός

Δράσης

(Pla

n)

Εκτέλεση εργασίας

(Exe

cute

)

Έλεγχος

(Mot

or C

ontr

ol)

Δράση(action)

Αίσθηση(sensing) Περιβάλλον

(environment)


Σχεδιασμός Κίνησης / Δρόμου (motion / path planning)

Motion / Path Planner

Ενδιάμεσοι στόχοι

Trajectory Generator

Ομαλή τροχιά

Controller

Motors / sensors / hardware

Σήματα ελέγχου (to joint controllers/drivers)

3


Σχεδιασμός Κίνησης: Το βασικό πρόβλημα

Σχεδιασμός δρόμου: εύρεση «βέλτιστης» ακολουθίας ενδιάμεσων θέσεων Ci

Σχεδιασμός τροχιάς: υπολογισμός «ομαλής» τροχιάς ...

Πρόβλημα: Αρχική θέση (διάταξη) C0 Τελική θέση Cf

C0

Cf


Αλγόριθμοι Σχεδιασμού Δρόμου

• Παράσταση/μοντελοποίηση περιβάλλοντος & ρομπότ

• Εγκυρότητα λύσεων (soundness)

• Πληρότητα αλγορίθμων (completeness)

• «Βέλτιστες» λύσεις (optimality)

• Πολυπλοκότητα αλγορίθμων (space/time complexity)

Αλγόρθμοι σχεδιασμού δρόμου: υποθέσεις / απαιτήσεις

4


Παράσταση Χώρου Κίνησης (1)

Mobile Robot Workspace Obstacles | Free-space

1. Spatial Decomposition

Grid-based representations (occupancy grids)

Sample environment Uniform decomposition



1. Spatial Decomposition (occupancy grids) (συνέχεια)

Quadtree (or octree) representations

5



1. Spatial Decomposition (occupancy grids) (συνέχεια)

Binary Space Partitioning trees (BSP) Exact Decomposition

4

1

2 3

1

2

3

4



2. Geometric Representations (Γεωμετρικά μοντέλα χώρου κίνησης)• 2D maps: points, lines, line segments, polynomials etc.

(π.χ. παράσταση κάτοψης χώρου κίνησης: τοίχοι, πόρτες, διάδρομοι κλπ.)

• 3D maps: points, planar surfaces, polyhedra, ellipsoids etc.

Μετρικοί χάρτες(metric maps)

6



3. Topological Representations (Τοπολογικά μοντέλα χώρου κίνησης)

Metric vs. Topological representations

Metric: exact, analytical representationsTopological: graph representations approximate (global) regions

1 2 3

4

5

1 2 3

4

5[1] [2] [3]

[4]

[1] [3]

[4]

[2]

Metric representation Topological representation


Παράσταση του κινούμενου ρομπότ (1)

Configuration Space (C-space) (Χώρος Διάταξης του Ρομπότ)

7



Configuration Space (C-space) (συνέχεια)




8







9


Μεθοδολογίες Σχεδιασμού Δρόμου

• Μέθοδοι «οδικού χάρτη» (roadmap methods)Visibility graph, Voronoi diagrams, silhouettes,probabilistic roadmaps, etc.

• Γράφοι συννεκτικότητας (connectivity graphs) Exact or Approximate cell decomposition

• Μέθοδος Δυναμικών πεδίων (potential fields)Local collision avoidance / Global navigation functions

Σχεδιασμός δρόμου κινούμενων ρομπότ (robot path planning)


Σχεδιασμός Δρόμου σε Διακριτό Χώρο

1. Procedure GraphSearch(s, goal)2. OPEN = {s}; CLOSED = {};3. found=false;4. while (OPEN ≠ ∅) and (not found) 5. Select a node n from OPEN;6. OPEN = OPEN – {n}; CLOSED = CLOSED ∪ {n}7. if n ∈ goal then 8. found = true;9. else 10. Let M the set of all nodes accessible from n and not in CLOSED11. OPEN = OPEN ∪ M ;12. end-if13. end-while

Γενική Ρουτίνα Αναζήτησης σε Γράφο (graph search)

Εαν OPEN: ουρά (queue) breadth-firstΕαν OPEN: στοίβα (stack) depth-first

Εαν OPEN: ordered using an evaluationfunction f (n) best-first

«Επισκέψιμοι κόμβοι»

10


Αλγόριθμος Dijkstra (Single-Source Shortest Path)

Procedure Dijkstra(G, s)1. Initialize_Single_Source(G, s)2. OPEN = {s}; CLOSED = {};3. found=false;4. while (OPEN ≠ ∅) and (not found) 5. Select a node: u Extract_Min(OPEN);6. OPEN = OPEN – {u}; CLOSED = CLOSED ∪ {u}7. if u ∈ goal then found = true;8. else9. for each node v ∈ Adjacency(u) and v ∉ CLOSED10. OPEN = OPEN ∪ v;11. Relax(u, v, w);12. end-if13. end-while

Initialize_Single_Source(G, s)1. For each node v ∈ V(G)2. d[v] = ∞;3. π[v] = NULL;4. d[s] = 0;

Relax(u, v, w)1. if d[u] + w(u, v) < d[v] 2. d[v] = d[u] + w(u, v);3. π[v] = u;

Weights(G)

Start with upper bound

Relax bounds at each step

(π[v]: predecessor of v, in shortest-path tree)

(d[u]: shortest-path distance to u)

(Υπόθεση: w≥0)


Αλγόριθμος Dijkstra –Παράδειγμα Εφαρμογής (1)

11


Αλγόριθμος Dijkstra –Παράδειγμα Εφαρμογής (2)

«Μέτωπο εξερεύνησης»αλγορίθμου Dijkstra

Αποτέλεσμα αλγορίθμου Dijkstra σε πρόβλημα με «παγίδα»


Αλγόριθμος Α*(Single-Pair Shortest Path)

Procedure A*(G, s, goal)1. Initialization: d[s]=0; f[s]=d[s]+h(s); π[s]=null;2. OPEN = {s}; CLOSED = {};3. while (OPEN ≠ ∅) 4. Select a node: u Get_Best_Node(OPEN); /* min f[u] */5. if (u == goal) then return true;6. for each node v ∈ Adjacency(u)7. new_f = d[u] + w(u, v) + h(v) ;8. if (new_f < f[v]) then9. d[v] = d[u] + w(u, v); f[v] = new_f; π[v] = u;10. Add v to OPEN;11. end-if12. end-for13. Remove u from OPEN ; Add u to CLOSED;14. end-while

f[.]: cost associated with each node d[.]: distance from start node sh(.): heuristic estimated distance to goal

Heuristic function h[.]: estimated distance to target

π[v]: predecessor of v, in shortest-path tree

12


Αλγόριθμος Α*Παράδειγμα Εφαρμογής

«Μέτωπο εξερεύνησης»αλγορίθμου αναζήτησης Α*

Αποτέλεσμα αλγορίθμου Α* σε πρόβλημα με «παγίδα»


Μεθοδολογίες «Οδικού Χάρτη» (roadmap path-planning methods) (1)

1. Γράφος «ορατότητας» (visibility graph planning): G = (V,E), V: κορυφές εμποδίων (vertices), Ε: ακμές που συνδέουν «ορατές» μεταξύ τους κορυφές (edges)

starts

goal

13


Μεθοδολογίες «Οδικού Χάρτη» (roadmap path-planning methods) (2)

2. Διάγραμμα Voronoi (Voronoi diagrams): γεωμετρικός τόπος σημείων που απέχουν εξίσου (equidistant) από τα πλησιέστερα δύο εμπόδια του χώρου

V1V2

V3

V4

V5V6

V7

E1

E2

E3

E4

E5

E6E7

Vor(Cfree): maximizes “clearance”

straight-lines: {configurations} closestto a pair of edges, or vertices, i.e.(edge,edge) or (vertex,vertex)

parabolic arc: {configurations} closestto a pair (edge, vertex)

(διχοτόμος) (μεσοκάθετος)


Μεθοδολογίες «Οδικού Χάρτη» (3):Probabilistic Roadmaps

3. «Πιθανοτικοί» οδικοί χάρτες δειγματοληψία χώρουΑρχική θέση Στόχος Δειγματοληψία χώρου κίνησης

Κατασκευή Γράφου

Α* αναζήτηση

Έλεγχος συμβατότητας

Βέλτιστη Διαδρομή

14


Σχεδιασμός Δρόμου με «Γράφους Συννε-κτικότητας» (connectivity graphs) (1)

1. Exact cell decomposition: Διάσπαση του χώρου Cfree σε ένα υποσύνολο από μη-επικαλυπτόμενες υποπεριοχές (cells), και δημιουργία του «γράφου συννεκτικότητας» (connectivity graph)

1

2

34

56

9

7

8

10

11

12

13

starts

goal


Σχεδιασμός Δρόμου με «Γράφους Συννε-κτικότητας» (connectivity graphs) (2)

1. Exact cell decomposition (συνέχεια)

starts

goal

1

2

34

56

9

7

8

10

11

12

13

15


Μεθοδολογίες «Δυναμικών Πεδίων» (potential fields methods) (1)

• «Τεχνητά δυναμικά πεδία» (artificial potential fields)Εμπόδια: “repulsive” potential field Urep(q)

Στόχος: “attractive” potential field Uatt(q)

• Ένα τεχνητό δυναμικό πεδίο πλοήγησης μπορεί:να αντανακλά την τοπική πληροφορία ύπαρξης εμποδίων

στο χώρο κίνησης «τοπική αποφυγή εμποδίων»(Πρόβλημα: ύπαρξη τοπικών ελαχίστων – local minima)να αντανακλά τη συνολική εικόνα του ελεύθερου χώρου

διατάξεων Cfree, μέσω μιας ιδανικής συνάρτησης πλοήγησης(navigation function) με ένα μόνο ολικό ελάχιστο στο στόχο.



• Γενική δομή μεθοδολογίας δυναμικών πεδίων:

F(q) = –∇U(q), και U(q) = Uatt(q) + Urep(q)

2att goal

1( ) ( )2 attK ρ= ⋅ ⋅U q q

και ( )att att goal goal goal( ) ( ) ( ) ( )att attK Kρ ρ= −∇ = − ⋅ ∇ = − ⋅ −UF q q q q q q

Στόχος: “attractive” potential field Uatt(q)

goal( )ρ = −q q qόπου

16



k

2

0obst 0

0

1 1 1 , ( )( ) 2 ( )

0 , ( )

rep kk

k

K ρ ρρ ρ

ρ ρ

⎧ ⎛ ⎞⋅ ⋅ − ≤⎪ ⎜ ⎟= ⎨ ⎝ ⎠

⎪ >⎩

Uq

q qq

óταν

óταν

Εμπόδια: “repulsive” potential fieldkrep obst

1

( ) ( )n

k=

=∑U Uq q

όπου

kobst 020

1 1 1( ) ( ), ( )( ) ( )rep k k

k kK ρ ρ ρ

ρ ρ ρ⎛ ⎞

⇒ = ⋅ − ⋅ ⋅∇ ≤⎜ ⎟⎝ ⎠

F q q qq q

óταν

και

obst( ) min

kkρ

′∈′= −

qq q q

k kobst obst( ) ( )= −∇UF q q

Frep

q

Urep

q



Urep(q)Uatt(q)

U(q) = Uatt+Urep

(a) (b) (c)

(d)

17


Ρομποτικός Σχεδιασμός Δρόμου οδηγούμενος από «Δυναμικά Πεδία» (1)

“Potential-guided” path planning: local collision avoidanceΤο ρομπότ μοντελοποιείται ως μια σημειακή μάζα στο χώρο κατάστασης, κινούμενη υπό την επίδραση του πεδίου δυνάμεων:

F(q) = –∇U(q) on-line path generation

a. Depth-First, without backtracking (σε κάθε βήμα i: qi qi+1)

a(qi) = F(qi)/||F(qi)|| και

( increment δi: no collision in the segment qi qi+1 )

( ) ( )1 ai i i i i i i ii

Uδ δ+∂

= + ⋅ = + ⋅∂

q q q q qq

(i) Steepest-descent of the potential function(ii) Problem of local-minima

goallocal


Ρομποτικός Σχεδιασμός Δρόμου οδηγούμενος από «Δυναμικά Πεδία» (2)“Reactive” collision avoidance based on artificial potential fieldsΠαράδειγμα: Τοπική («ανακλαστική») αποφυγή εμποδίων

Σύγκλιση σε στόχο με παράλληλη τοπική αποφυγή εμποδίωνΠρόβλημα: πιθανή ύπαρξη τοπικών ελαχίστων

Σύγκλιση στο στόχο Αποφυγή εμποδίου Τοπικό ελάχιστο

στόχος

18


Ρομποτικός Σχεδιασμός Δρόμου οδηγούμενος από «Δυναμικά Πεδία» (3)b. Best-First με backtracking: grid-based (GC: grid of configurations qi)

Αναδρομική κατασκευή ενός δέντρου T, με κόμβους στο GC και το qstart ως «ρίζα».

Σε κάθε βήμα, ο αλγόριθμος «επισκέπτεται» όλους τους p-γείτονες (p-connectivity, συνήθως p=4 ή p=8) του φύλλου του Τ με τη μικρότερη τιμή δυναμικού (best-first), και προσθέτει ως παιδιά τους κόμβους του GC που δεν ανήκουν ήδη στο Τ, και που το δυναμικό τους δεν ξεπερνά κάποιο ανώτατο κατώφλι ( εμπόδιο).Για κάθε κόμβο του Τ διατηρούμε και ένα δείκτη προς τον «πατέρα», με στόχο τηνανακατασκευή της βέλτιστης διαδρομής, όταν φτάσουμε στο qgoal.

Best-First με backtracking: Διακριτή προσέγγιση του steepest-decent (depth-first), αλλά όταν φτάσουμε σε κάποιο τοπικό ελάχιστο ο αλγόριθμος «οπισθοδρομεί»(«πλημμυρίζοντας το πηγάδι» του τοπικού ελαχίστου) μέχρις ότου «ξεφύγει», σεαναζήτηση της βέλτιστης διαδρομής προς το στόχο.


Ρομποτικός Σχεδιασμός Δρόμου οδηγούμενος από «Δυναμικά Πεδία» (4)

b. Αλγόριθμος Best-First-Planning (συνέχεια)1. Procedure BFP;2. Install qinit in T; INSERT(qinit in OPEN); Mark qinit as visited;3. success=false;4. while (OPEN ≠ ∅) and (not success) 5. (Select a node q from OPEN) ⇒ q = BEST(OPEN);6. For every neighbor q’of q in GC do7. if U( q’) < Max and q’not visited, then8. Install q’ in T; INSERT(q’, OPEN); Mark q’as visited;9. if q’= qgoal then success = true;10. end-if 11. end-while12. if (success) then return the constructed path by tracing back the pointers

in T from qgoal to qinit

19


Ρομποτικός Σχεδιασμός Δρόμου με «Συνάρτηση Πλοήγησης» (1)

Κατασκευή Γενικής Συνάρτησης Πλοήγησης (global navigation function) με βάση αλγορίθμους “wavefront expansion”

α. Κατασκευή «χάρτη απόστασης» (distance map) από το στόχοGlobal Distance Map: Global vs. Local Path Planning


Ρομποτικός Σχεδιασμός Δρόμου με «Συνάρτηση Πλοήγησης» (2)

Κατασκευή Συνάρτησης Πλοήγησης (global navigation function) με βάση αλγορίθμους “wavefront expansion” (συνέχεια)

555555555555555

444444444444444

433333333333334

432222222222234

432111111111234

432100000001234

432111111111234

432222222222234

433333333333334

444444444444444

555555555555555

666666666666666

777777777777777

888888888888888

β. Κατασκευή «χάρτη απόστασης» (distance map) από εμπόδια

Δημιουργία “skeletons” as roadmaps

20


Ρομποτικός Σχεδιασμός Δρόμου με «Συνάρτηση Πλοήγησης» – Εφαρμογή

«Human-Demonstration» of Desired Mobile Robot Trajectory

Automatic Waypoint Computation / EditingPre-Viewing / Playback

Animation–1 Video Clip

Active Assistance Functions:collision avoidance / motion guides

(Tele-planning)


Βιβλιογραφία

• J-C. Latombe, Robot Motion Planning, Kluwer, 1991.• G. Dudek, M.Jenkin, Computational Principles of Mobile

Robotics, Cambridge University Press, 2000.• H. Choset, K. M. Lynch, S. Hutchinson, G. Kantor, W. Burgard,

L. E. Kavraki and S. Thrun, Principles of Robot Motion –Theory, Algorithms and Implementations, The MIT Press, Cambridge, Massachusetts, 2005

ΕΝΟΤΗΤΑ 2: Αυτόνομα Ευφυή Κινούμενα...

Documents

Transcript of ΕΝΟΤΗΤΑ 2: Αυτόνομα Ευφυή Κινούμενα...