Path planning in mobile robotics (PDF, 1.7Mb, 48 σελίδες) - Εθνικό ...

Ε.Μ.Π., ΣΗΜΜΥ, Ακαδημαϊκό Έτος 2010-11, 8ο Εξάμηνο 

Μάθημα: Ρομποτική ΙΙ. Διδάσκων: Κ.Τζαφέστας 

ΕΝΟΤΗΤΑ 2: 

Αυτόνομα Ευφυή Κινούμενα 

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

Σχεδιασμός Δρόμου – Πλοήγηση 

(path-planning, navigation) 

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

Αυτόνομα Ευφυή Κινούμενα Ρομπότ 

– Περιεχόμενα Ενότητας 

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

• Αισθητήριες Διατάξεις (sensing) 

– Proprioception (internal state ) / Exteroception etc. 

– Σύνθεση αισθητηρίων πληροφοριών (sensor fusion) 

• Αρχιτεκτονικές ελέγχου 

(mobile robot control architectures) 

• Αυτοεντοπισμός θέσης / Χαρτογράφηση 

(self-localisation / map-building) 

• Σχεδιασμός Δρόμου - Πλοήγηση 

(path planning / navigation) 


perception 

action 

1 

motion 

control 

sensing 

reasoning 

2

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

Αντίληψη 

(Perception) 

Αίσθηση 

(sensing) 

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

(Model) 

Σχεδιασμός Δράσης 

(Plan) 

Περιβάλλον 

(environment) 

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

(Execute) 

Έλεγχος 

(Motor Control) 

Δράση 

(action) 


Σχεδιασμός Κίνησης / Δρόμου 

(motion / path planning) 

Motion / Path Planner 

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

Trajectory Generator 

Controller 

Ομαλή τροχιά 

Motors / sensors / hardware 

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


3 

4

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

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

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

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

C 0 


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

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

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

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

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

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

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


C f 

5 

6

Παράσταση Χώρου Κίνησης (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 


7 

8


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

Binary Space Partitioning trees (BSP) Exact Decomposition 

1 

2 3 

4 



2. Geometric Representations (Γεωμετρικά μοντέλα χώρου κίνησης) 

• 2D maps: points, lines, line segments, polynomials etc. 

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

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


2 

1 

3 

4 

Μετρικοί χάρτες 

(metric maps) 

9 

10


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

Metric vs. Topological representations 

Metric: exact, analytical representations 

Topological: graph representations approximate (global) regions 

1 2 3 

[1] [2] [3] 

[4] 

5 

4 

1 2 3 

4 

Metric representation Topological representation 


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

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


[1] 

5 

[2] 

[4] 

[3] 

11 

12


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





13 

14







15 

16

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

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

• Μέθοδοι «οδικού χάρτη» (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 


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

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

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} 

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

Εαν OPEN: ουρά (queue) breadth-first 

Εαν OPEN: στοίβα (stack) depth-first 

Εαν OPEN: ordered using an evaluation 

function f (n) best-first 

7. if n ∈ goal then 

8. found = true; 

9. else 

10. Let M the set of all nodes accessible from n and not in CLOSED 

11. OPEN = OPEN ∪ M ; 

12. end-if 

13. end-while 


17 

18

Αλγόριθμος 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. else 

9. for each node v ∈ Adjacency(u) and v ∉ CLOSED 

10. OPEN = OPEN ∪ v; 

11. Relax(u, v, w); 

12. end-if 

13. end-while 

Weights(G) 

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

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; 


Αλγόριθμος Dijkstra – 

Παράδειγμα Εφαρμογής (1) 

Start with upper bound 

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

(π[v]: predecessor of v, in 

shortest-path tree) 

Relax bounds at each step 


19 

20

«Μέτωπο εξερεύνησης» 

αλγορίθμου Dijkstra 

Αλγόριθμος Dijkstra – 

Παράδειγμα Εφαρμογής (2) 

Αποτέλεσμα αλγορίθμου Dijkstra σε 

πρόβλημα με «παγίδα» 


Αλγόριθμος Α* 

(Single-Pair Shortest Path) 

1. 

Procedure A*(G, s, goal) 

Initialization: d[s]=0; f[s]=d[s]+h(s); π[s]=null; 

2. 

3. 

OPEN = {s}; CLOSED = {}; 

while (OPEN ≠∅) 

f[.]: cost associated with each node 

d[.]: distance from start node s 

h(.): heuristic estimated distance to goal 

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) ; Heuristic function h[.]: 

8. if (new_f < f[v]) then 

estimated distance to target 

9. d[v] = d[u] + w(u, v); f[v] = new_f; π[v] = u; 

10. Add v to OPEN; 

11. 

12. 

end-if 

end-for 

π[v]: predecessor of v, in 

shortest-path tree 

13. Remove u from OPEN ; Add u to CLOSED; 

14. end-while 


21 

22

«Μέτωπο εξερεύνησης» 

αλγορίθμου αναζήτησης Α* 

Αλγόριθμος Α* 

Παράδειγμα Εφαρμογής 

Αποτέλεσμα αλγορίθμου Α* σε 

πρόβλημα με «παγίδα» 


Μεθοδολογίες «Οδικού Χάρτη» 

(roadmap path-planning methods) (1) 

1. Γράφος «ορατότητας» (visibility graph planning): 

G = (V,E), V: κορυφές εμποδίων (vertices), Ε: ακμές που 

συνδέουν «ορατές» μεταξύ τους κορυφές (edges) 

start 

s 

goal 


23 

24

Μεθοδολογίες «Οδικού Χάρτη» 

(roadmap path-planning methods) (2) 

2. Διάγραμμα Voronoi (Voronoi diagrams): 

γεωμετρικός τόπος σημείων που απέχουν εξίσου (equidistant) 

από τα πλησιέστερα δύο εμπόδια του χώρου 

V 1 

V 5 

E 5 

E 4 

E7 

E 1 

V 4 

V 7 

V 2 

E 3 

E 6 

E 2 

V 6 

V 3 

Vor(C free ): maximizes “clearance” 

straight-lines: {configurations} closest 

to a pair of edges, or vertices, i.e. 

(edge,edge) or (vertex,vertex) 

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

parabolic arc: {configurations} closest 

to a pair (edge, vertex) 


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

Πιθανοτικές Μέθοδοι 

Πιθανοτικοί Οδικοί Χάρτες 

(Probabilistic Roadmaps – PRM) 

Πιθανοτικές Μέθοδοι 

Μέθοδοι «δειγματοληψίας» 

Rapidly exploring Random Trees 

(RRT) 


25 

26

Πιθανοτικοί «Οδικοί Χάρτες» (1) 

(Probabilistic Roadmaps – PRM) 

3. «Πιθανοτικοί» οδικοί χάρτες δειγματοληψία χώρου 

Α* αναζήτηση 

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

Αρχική θέση Στόχος Δειγματοληψία χώρου κίνησης 

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

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



(Probabilistic Roadmaps) 

Δειγματοληψία Χώρου 

(χώρος διατάξεων - configuration space) 


27 

28



Δειγματοληψία Χώρου (Sampling_strategy) 

Δημιουργία τυχαίων σημείων στο χώρο ρομποτικών διατάξεων 




Έλεγχος Συμβατότητας ( free configurations) 

Διατήρηση τυχαίων σημείων στον «ελεύθερο» χώρο διατάξεων 


29 

30



Διασύνδεση «γειτονικών» κόμβων 

(neighbour_strategy + verify_path) 




Αλγόριθμος κατασκευής πιθανοτικού γράφου PRM 

{V, E} 


31 

32



Γράφος στον «ελεύθερο» χώρο διατάξεων 




Αναζήτηση βέλτιστης διαδρομής στο γράφο 


33 

34

Σχεδιασμός Δρόμου με «Γράφους Συννεκτικότητας» 

(connectivity graphs) (1) 

1. Exact cell decomposition: Διάσπαση του χώρου C free σε ένα 

υποσύνολο από μη-επικαλυπτόμενες υποπεριοχές (cells), και 

δημιουργία του «γράφου συννεκτικότητας» (connectivity graph) 

start 

s 

1 

2 

10 

8 

9 

7 

3 


6 

5 

13 

4 

12 

11 

goal 

Σχεδιασμός Δρόμου με «Γράφους Συννεκτικότητας» 

(connectivity graphs) (2) 

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

start 

s 

1 

2 

10 

8 

9 

7 

3 

goal 


6 

5 

13 

4 

12 

11 

35 

36

Μεθοδολογίες «Δυναμικών Πεδίων» 

(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) = U att (q) + U rep (q) 

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

1 

2 

Uatt ( q) = ⋅Katt ⋅ρgoal( 

q) 

2 

όπου 

ρ ( q) = q−qgoal F ( q) =−∇ U ( q) =−K ⋅ρ ( q) ∇ ρ ( q) =−K ⋅ q−q και att att att goal goal att ( goal) 


37 

38



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

όπου 

και 

ρ 

U 

rep 

n 

=∑ 

k= 1 

obstk 

U ( q) U ( q) 

2 

⎧1 ⎛ 1 1 ⎞ 

⎪ ⋅K ⋅⎜ − ⎟ , óταν ρ ( q) 

≤ρ0 

= ⎨ ⎝ ⎠ 

⎪ 

⎩ 0 , óταν ρk ( q) 

> ρ0 

rep k 

obst ( q) k 2 ρk( q) 

ρ0 

q = q−q′ k ( ) min 

q′∈ 

obstk 

F ( q) =−∇U ( q) 

obstk obstk 

⎛ 1 1 ⎞ 1 

⇒ Fobst ( q) = K ( ), ( ) 

k 

rep ⋅⎜ − ρ 2 

k ρkρ0 ρk( ) ρ 

⎟⋅ 

⋅∇ q óταν q ≤ 

⎝ q 0 ⎠ ρk( 

q) 

F rep 


q 

U rep 



(a) (b) (c) 

U att(q) 

(d) 

U rep(q) 


q 

39 

U(q) = U att+U rep 

40

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

οδηγούμενος από «Δυναμικά Πεδία» (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 ) 

qi+ 1 = qi + δi⋅ ai 

qi ∂U 

= qi + δi⋅ 

∂qi 

qi 

(i) Steepest-descent of the potential function 

(ii) Problem of local-minima 

( ) ( ) 




“Reactive” collision avoidance based on artificial potential fields 

Παράδειγμα: Τοπική («ανακλαστική») αποφυγή εμποδίων 

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

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

local 

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

στόχος 


goal 

41 

42



b. (Greedy) Best-First με backtracking (GC: grid of configurations q i ) 

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

Σε κάθε βήμα, ο αλγόριθμος «επισκέπτεται» όλους τους p-γείτονες (p-connectivity, 

συνήθως p=4 ή p=8) του φύλλου του Τ με τη μικρότερη τιμή δυναμικού (best-first), 

και προσθέτει ως παιδιά τους κόμβους του GC που δεν ανήκουν ήδη στο Τ, και που 

το δυναμικό τους δεν ξεπερνά κάποιο ανώτατο κατώφλι ( εμπόδιο). 

Για κάθε κόμβο του Τ διατηρούμε και ένα δείκτη προς τον «πατέρα», με στόχο την 

ανακατασκευή της βέλτιστης διαδρομής, όταν φτάσουμε στο qgoal . 

Best-First με backtracking (επανιχνηλάτηση): Διακριτή προσέγγιση του steepestdecent 

(πλέον απότομης κατάβασης ή μέγιστης κλίσης), αλλά όταν φτάσουμε σε 

κάποιο τοπικό ελάχιστο ο αλγόριθμος «οπισθοδρομεί»(«πλημμυρίζοντας το 

πηγάδι» του τοπικού ελαχίστου) μέχρις ότου «ξεφύγει», συνεχίζοντας την 

αναζήτηση της βέλτιστης διαδρομής προς το στόχο. 




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 do 

7. if U( q’) < Max and q’not visited, then 

8. Install q’ in T; INSERT(q’, OPEN); Mark q’as visited; 

9. if q’= qgoal then success = true; 

10. end-if 

11. end-while 

12. if (success) then return the constructed path by tracing back the pointers 

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

43 

44

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

«Συνάρτηση Πλοήγησης» (1) 

Κατασκευή Γενικής Συνάρτησης Πλοήγησης (global navigation 

function) με βάση αλγορίθμους “wavefront expansion” 

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

Global Distance Map: Global vs. Local Path Planning 



«Συνάρτηση Πλοήγησης» (2) 

Κατασκευή Συνάρτησης Πλοήγησης (global navigation function) 

με βάση αλγορίθμους “wavefront expansion” (συνέχεια) 

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

8 

7 

6 

5 

4 

4 

4 

4 

4 

4 

4 

4 

4 

5 

8 

7 

6 

5 

4 

3 

3 

3 

3 

3 

3 

3 

4 

5 

8 

7 

6 

5 

4 

3 

2 

2 

2 

2 

2 

3 

4 

5 

8 

7 

6 

5 

4 

3 

2 

1 

1 

1 

2 

3 

4 

5 

8 

7 

6 

5 

4 

3 

2 

1 

0 

1 

2 

3 

4 

5 

8 

7 

6 

5 

4 

3 

2 

1 

0 

1 

2 

3 

4 

5 

8 

7 

6 

5 

4 

3 

2 

1 

0 

1 

2 

3 

4 

5 

8 

7 

6 

5 

4 

3 

2 

1 

0 

1 

2 

3 

4 

5 

8 

7 

6 

5 

4 

3 

2 

1 

0 

1 

2 

3 

4 

5 

8 

7 

6 

5 

4 

3 

2 

1 

0 

1 

2 

3 

4 

5 

8 

7 

6 

5 

4 

3 

2 

1 

0 

1 

2 

3 

4 

5 

8 

7 

6 

5 

4 

3 

2 

1 

1 

1 

2 

3 

4 

5 

8 

7 

6 

5 

4 

3 

2 

2 

2 

2 

2 

3 

4 

5 

8 

7 

6 

5 

4 

3 

3 

3 

3 

3 

3 

3 

4 

5 

8 

7 

6 

5 

4 

4 

4 

4 

4 

4 

4 

4 

4 

5 

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


45 

46


«Συνάρτηση Πλοήγησης» – Εφαρμογή 

«Human-Demonstration» of Desired Mobile Robot Trajectory 

(Tele-planning) 

Active Assistance Functions: 

collision avoidance / motion guides 

Animation–1 

Video Clip 

Pre-Viewing / Playback 

Automatic Waypoint 

Computation / Editing 


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

• 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 


47 

48

Path planning in mobile robotics (PDF, 1.7Mb, 48 σελίδες) - Εθνικό ...

Create successful ePaper yourself

Delete template?

Save as template?