Ε.Μ.Π., ΣΗΜΜΥ, Ακαδημαϊκό Έτος 009-0, ο Εξάμηνο Μάθημα: Ρομποτική ΙΙ. Διδάσκων: Κ.Τζαφέστας ΕΝΟΤΗΤΑ : Αυτόνομα Ευφυή Κινούμενα Ρομποτικά Συστήματα Σχεδιασμός Δρόμου Πλοήγηση (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 motion control sensing reasoning
Βασικό Σχήμα Αρχιτεκτονικής Ελέγχου Αντίληψη (Perception) Μοντελοποίηση (Model) Σχεδιασμός Δράσης (Plan) Εκτέλεση εργασίας (Execute) Έλεγχος (Motor Control) Αίσθηση (sensing) Περιβάλλον (environment) Δράση (action) Σχεδιασμός Κίνησης / Δρόμου (motion / path planning) Motion / Path Planner Ενδιάμεσοι στόχοι Trajectory Generator Ομαλή τροχιά Controller Σήματα ελέγχου (to joint controllers/drivers) Motors / sensors / hardware
Σχεδιασμός Κίνησης: Το βασικό πρόβλημα Πρόβλημα: Αρχική θέση (διάταξη) C 0 Τελική θέση C f Σχεδιασμός δρόμου: εύρεση «βέλτιστης» ακολουθίας ενδιάμεσων θέσεων C i Σχεδιασμός τροχιάς: υπολογισμός «ομαλής» τροχιάς... C f C 0 Αλγόριθμοι Σχεδιασμού Δρόμου Αλγόρθμοι σχεδιασμού δρόμου: υποθέσεις / απαιτήσεις Παράσταση/μοντελοποίηση περιβάλλοντος & ρομπότ Εγκυρότητα λύσεων (soundness) Πληρότητα αλγορίθμων (completeness) «Βέλτιστες» λύσεις (optimality) Πολυπλοκότητα αλγορίθμων (space/time complexity)
Παράσταση Χώρου Κίνησης () Mobile Robot Workspace Obstacles Free-space. Spatial Decomposition Grid-based representations (occupancy grids) Sample environment Uniform decomposition Παράσταση Χώρου Κίνησης (). Spatial Decomposition (occupancy grids) (συνέχεια) Quadtree (or octree) representations
Παράσταση Χώρου Κίνησης (). Spatial Decomposition (occupancy grids) (συνέχεια) Binary Space Partitioning trees (BSP) Exact Decomposition 9 Παράσταση Χώρου Κίνησης (). Geometric Representations (Γεωμετρικά μοντέλα χώρου κίνησης) D maps: points, lines, line segments, polynomials etc. (π.χ. παράσταση κάτοψης χώρου κίνησης: τοίχοι, πόρτες, διάδρομοι κλπ.) D maps: points, planar surfaces, polyhedra, ellipsoids etc. Μετρικοί χάρτες (metric maps) 0
Παράσταση Χώρου Κίνησης (). Topological Representations (Τοπολογικά μοντέλα χώρου κίνησης) Metric vs. Topological representations Metric: exact, analytical representations Topological: graph representations approximate (global) regions [] [] [] [] Metric representation [] [] [] [] Topological representation Παράσταση του κινούμενου ρομπότ () Configuration Space (C-space) (Χώρος Διάταξης του Ρομπότ)
Παράσταση του κινούμενου ρομπότ () Configuration Space (C-space) (συνέχεια) Παράσταση του κινούμενου ρομπότ () Configuration Space (C-space) (συνέχεια)
Παράσταση του κινούμενου ρομπότ () Configuration Space (C-space) (συνέχεια) Παράσταση του κινούμενου ρομπότ () Configuration Space (C-space) (συνέχεια)
Μεθοδολογίες Σχεδιασμού Δρόμου Σχεδιασμός δρόμου κινούμενων ρομπότ (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). Procedure GraphSearch(s, goal). OPEN = {s}; CLOSED = {};. found=false;. while (OPEN ) and (not found). Select a node n from OPEN;. OPEN = OPEN {n}; CLOSED = CLOSED {n} «Επισκέψιμοι κόμβοι» Εαν OPEN: ουρά (queue) breadth-first Εαν OPEN: στοίβα (stack) depth-first Εαν OPEN: ordered using an evaluation function f (n) best-first. if n goal then. found = true; 9. else 0. Let M the set of all nodes accessible from n and not in CLOSED. OPEN = OPEN M ;. end-if. end-while
Αλγόριθμος Dijkstra (Single-Source Shortest Path) Procedure Dijkstra(G, s). Initialize_Single_Source(G, s). OPEN = {s}; CLOSED = {};. found=false;. while (OPEN ) and (not found). Select a node: u Extract_Min(OPEN);. OPEN = OPEN {u}; CLOSED = CLOSED {u}. if u goal then found = true;. else 9. for each node v Adjacency(u) and v CLOSED 0. OPEN = OPEN v;. Relax(u, v, w);. end-if. end-while Weights(G) (Υπόθεση: w 0) Initialize_Single_Source(G, s). For each node v V(G). d[v] = ;. π[v] = NULL;. d[s] = 0; Relax(u, v, w). if d[u] + w(u, v) < d[v]. d[v] = d[u] + w(u, v);. π[v] = u; Start with upper bound (d[u]: shortest-path distance to u) (π[v]: predecessor of v, in shortest-path tree) Relax bounds at each step 9 Αλγόριθμος Dijkstra Παράδειγμα Εφαρμογής () 0
Αλγόριθμος Dijkstra Παράδειγμα Εφαρμογής () «Μέτωπο εξερεύνησης» αλγορίθμου Dijkstra Αποτέλεσμα αλγορίθμου Dijkstra σε πρόβλημα με «παγίδα» Αλγόριθμος Α* (Single-Pair Shortest Path) Procedure A*(G, s, goal). Initialization: d[s]=0; f[s]=d[s]+h(s); π[s]=null;. OPEN = {s}; CLOSED = {};. while (OPEN ). Select a node: u Get_Best_Node(OPEN); /* min f[u] */. if (u == goal) then return true;. for each node v Adjacency(u). new_f = d[u] + w(u, v) + h(v) ;. if (new_f < f[v]) then 9. d[v] = d[u] + w(u, v); f[v] = new_f; π[v] = u; 0. Add v to OPEN;. end-if. end-for. Remove u from OPEN ; Add u to CLOSED;. end-while f[.]: cost associated with each node d[.]: distance from start node s h(.): heuristic estimated distance to goal Heuristic function h[.]: estimated distance to target π[v]: predecessor of v, in shortest-path tree
Αλγόριθμος Α* Παράδειγμα Εφαρμογής «Μέτωπο εξερεύνησης» αλγορίθμου αναζήτησης Α* Αποτέλεσμα αλγορίθμου Α* σε πρόβλημα με «παγίδα» Μεθοδολογίες «Οδικού Χάρτη» (roadmap path-planning methods) (). Γράφος «ορατότητας» (visibility graph planning): G = (V,E), V: κορυφές εμποδίων (vertices), Ε: ακμές που συνδέουν «ορατές» μεταξύ τους κορυφές (edges) start s goal
Μεθοδολογίες «Οδικού Χάρτη» (roadmap path-planning methods) (). Διάγραμμα Voronoi (Voronoi diagrams): γεωμετρικός τόπος σημείων που απέχουν εξίσου (equidistant) από τα πλησιέστερα δύο εμπόδια του χώρου V E V E E E V E V V E E V V 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) Μεθοδολογίες «Οδικού Χάρτη» (): Probabilistic Roadmaps. «Πιθανοτικοί» οδικοί χάρτες δειγματοληψία χώρου Αρχική θέση Στόχος Δειγματοληψία χώρου κίνησης Α* αναζήτηση Έλεγχος συμβατότητας Κατασκευή Γράφου Βέλτιστη Διαδρομή
Σχεδιασμός Δρόμου με «Γράφους Συννεκτικότητας» (connectivity graphs) (). Exact cell decomposition: Διάσπαση του χώρου C free σε ένα υποσύνολο από μη-επικαλυπτόμενες υποπεριοχές (cells), και δημιουργία του «γράφου συννεκτικότητας» (connectivity graph) start s 9 goal 0 Σχεδιασμός Δρόμου με «Γράφους Συννεκτικότητας» (connectivity graphs) (). Exact cell decomposition (συνέχεια) start s 9 goal 0
Μεθοδολογίες «Δυναμικών Πεδίων» (potential fields methods) () «Τεχνητά δυναμικά πεδία» (artificial potential fields) Εμπόδια: repulsive potential field U rep (q) Στόχος: attractive potential field U att (q) Ένα τεχνητό δυναμικό πεδίο πλοήγησης μπορεί: να αντανακλά την τοπική πληροφορία ύπαρξης εμποδίων στο χώρο κίνησης «τοπική αποφυγή εμποδίων» (Πρόβλημα: ύπαρξη τοπικών ελαχίστων local minima) να αντανακλά τη συνολική εικόνα του ελεύθερου χώρου διατάξεων C free, μέσω μιας ιδανικής συνάρτησης πλοήγησης (navigation function) με ένα μόνο ολικό ελάχιστο στο στόχο. 9 Μεθοδολογίες «Δυναμικών Πεδίων» (potential fields methods) () Γενική δομή μεθοδολογίας δυναμικών πεδίων: F(q) = U(q), και U(q) = U att (q) + U rep (q) και Στόχος: attractive potential field U att (q) Uatt ( q) = Katt ρgoal ( q) όπου ρ ( q) = q qgoal F ( q) = U ( q) = K ρ ( q) ρ ( q) = K q q ( ) att att att goal goal att goal 0
Μεθοδολογίες «Δυναμικών Πεδίων» (potential fields methods) () Εμπόδια: repulsive potential field U n rep = obst k k= ( q) U ( q) όπου και U ρ k K, óταν ρk ( q) ρ0 = 0, óταν ρk ( q) > ρ0 rep obst ( q) k ρk ( q) ρ0 ( q) = min q q obstk q obstk F ( q) = U ( q) obstk F obst ( q ) = K ( ), ( ) k rep ρ k ρk ρ0 ρk( ) ρ 0 ρk( ) q óταν q q q F rep U rep q q Μεθοδολογίες «Δυναμικών Πεδίων» (potential fields methods) () (a) (b) (c) U att (q) U rep (q) (d) U(q) = U att +U rep
Ρομποτικός Σχεδιασμός Δρόμου οδηγούμενος από «Δυναμικά Πεδία» () Potential-guided path planning: local collision avoidance Το ρομπότ μοντελοποιείται ως μια σημειακή μάζα στο χώρο κατάστασης, κινούμενη υπό την επίδραση του πεδίου δυνάμεων: F(q) = U(q) on-line path generation a. Depth-First, without backtracking (σε κάθε βήμα i: q i q i+ ) a(q i ) = F(q i )/ F(q i ) και U qi+ = qi + δi ai qi = qi + δi qi i ( increment δ i : no collision in q the segment q i q i+ ) (i) Steepest-descent of the potential function (ii) Problem of local-minima ( ) ( ) local goal Ρομποτικός Σχεδιασμός Δρόμου οδηγούμενος από «Δυναμικά Πεδία» () Reactive collision avoidance based on artificial potential fields Παράδειγμα: Τοπική («ανακλαστική») αποφυγή εμποδίων Σύγκλιση σε στόχο με παράλληλη τοπική αποφυγή εμποδίων Πρόβλημα: πιθανή ύπαρξη τοπικών ελαχίστων Σύγκλιση στο στόχο Αποφυγή εμποδίου Τοπικό ελάχιστο στόχος
Ρομποτικός Σχεδιασμός Δρόμου οδηγούμενος από «Δυναμικά Πεδία» () b. Best-First με backtracking: grid-based (GC: grid of configurations q i ) Αναδρομική κατασκευή ενός δέντρου T, με κόμβους στο GC και το q start ως «ρίζα». Σε κάθε βήμα, ο αλγόριθμος «επισκέπτεται» όλους τους p-γείτονες (p-connectivity, συνήθως p= ή p=) του φύλλου του Τ με τη μικρότερη τιμή δυναμικού (best-first), και προσθέτει ως παιδιά τους κόμβους του GC που δεν ανήκουν ήδη στο Τ, και που το δυναμικό τους δεν ξεπερνά κάποιο ανώτατο κατώφλι ( εμπόδιο). Για κάθε κόμβο του Τ διατηρούμε και ένα δείκτη προς τον «πατέρα», με στόχο την ανακατασκευή της βέλτιστης διαδρομής, όταν φτάσουμε στο q goal. Best-First με backtracking: Διακριτή προσέγγιση του steepest-decent (depth-first), αλλά όταν φτάσουμε σε κάποιο τοπικό ελάχιστο ο αλγόριθμος «οπισθοδρομεί» («πλημμυρίζοντας το πηγάδι» του τοπικού ελαχίστου) μέχρις ότου «ξεφύγει», σε αναζήτηση της βέλτιστης διαδρομής προς το στόχο. Ρομποτικός Σχεδιασμός Δρόμου οδηγούμενος από «Δυναμικά Πεδία» () b. Αλγόριθμος Best-First-Planning (συνέχεια). Procedure BFP;. Install q init in T; INSERT(q init in OPEN); Mark q init as visited;. success=false;. while (OPEN ) and (not success). (Select a node q from OPEN) q = BEST(OPEN);. For every neighbor q of q in GC do. if U( q ) < Max and q not visited, then. Install q in T; INSERT(q, OPEN); Mark q as visited; 9. if q = q goal then success = true; 0. end-if. end-while. if (success) then return the constructed path by tracing back the pointers in T from q goal to q init
Ρομποτικός Σχεδιασμός Δρόμου με «Συνάρτηση Πλοήγησης» () Κατασκευή Γενικής Συνάρτησης Πλοήγησης (global navigation function) με βάση αλγορίθμους wavefront expansion α. Κατασκευή «χάρτη απόστασης» (distance map) από το στόχο Global Distance Map: Global vs. Local Path Planning Ρομποτικός Σχεδιασμός Δρόμου με «Συνάρτηση Πλοήγησης» () Κατασκευή Συνάρτησης Πλοήγησης (global navigation function) με βάση αλγορίθμους wavefront expansion (συνέχεια) 0 0 0 0 0 0 0 β. Κατασκευή «χάρτη απόστασης» (distance map) από εμπόδια Δημιουργία skeletons as roadmaps
Ρομποτικός Σχεδιασμός Δρόμου με «Συνάρτηση Πλοήγησης» Εφαρμογή «Human-Demonstration» of Desired Mobile Robot Trajectory (Tele-planning) Active Assistance Functions: collision avoidance / motion guides Pre-Viewing / Playback Automatic Waypoint Computation / Editing Animation Video Clip 9 Βιβλιογραφία J-C. Latombe, Robot Motion Planning, Kluwer, 99. G. Dudek, M.Jenkin, Computational Principles of Mobile Robotics, Cambridge University Press, 000. 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, 00 0