In I t n r t o r d o u d c u ti t on o n to t o Rob o o b t o i t c M ot o i t on o n Planning 1

Transcript

1 Introduction to Robotic Motion Planning 1

2 Robot Planning & Control Architecture Trajectory Generator Trajectory (i.e. geometry +time) d d ( t) ( ) Control: Trajectory / Interaction u Robot, & h t, f µ Robot Environment Path (i.e. geometry ) ( ) [ 0,1] τ i s s i ( ) s [ 0,1] h s Path Planner Task ( ) ( ) τ i 0, τ i 1 i= 1, K, N Task Planner User Reuirements & Specifications 2

3 Το Πρόβληµα του Προγραµµατισµού της Κίνησης Robots accomplish tasks by moving in the workspace How can we accomplish automated motion planning? How to move given an initial and a final pose / posture (i.e. position + orientation) and the robot workspace) 3

4 Το Πρόβληµα του Προγραµµατισµού της Κίνησης Motion Planning is uite more involved than obstacle avoidance. Computational complexity is a central issue in motion planning. 4

5 Motion Planning Issues This simple assembly is defined by the 3 composing parts and a seuence of 2 spatial relations. Every relation can be defined as a set of elementary relationships between geometric features e.g. FaceΑof obj. 1 must fit faceβof obj. 2", Shaft C of obj. 3 fits hole D of obj. 1" Then it can be translated to a uantitative formulation e.g. using a homogeneous transformation 5

6 The Basic Motion Planning Problem Α : solid object (robot) moving in the subspace W (work- space) of the euclidean spacer N, where N = 2 or 3. Α is a «free object», i.e. there is no motion constraint. Α Α W R 2 6

7 The Basic Motion Planning Problem B 1, B 2,, B m solid objects distributed over W. B i are called obstacles. The geometry ofα, B 1, B 2,, B m and the positions of B i in W are accurately known. Α B 2 Α B 3 B 1 B 2 B 1 W R 2 7

8 The Basic Motion Planning The problem is: Initial position and orientation Problem Considering an initial position & orientation and a final position & orientation of Α in W, produce a path τ defining a continuous seuence of positions and orientations of Α avoiding collisions with B, i, starting from the initial position & orientation and arriving at the final position & orientation. Report failure of determining such a path. Α B 2 Α B 3 B 1 B 2 Final position and orientation B 1 W R 2 8

9 The configuration space F A, F W : Cartesian systems attached at A and W. The posture of A is the position and orientation of F A wrt F W. The subset of W, occupied by A, at posture, is A(). The configuration space C of A is the space of all postures of A C= {( x, y) W, θ + F W [ π, π]} Α() F θ A x = y θ W R 2 is used here to denote the posture of a robot. It should not 9 be confused with the, previously used, DOFs of a robotic mechanism.

10 Η έννοια της πορείας (path) A path of A from the initial posture init to goal is a continuous mapping τ(s):[0,1] C withτ(0)= init andτ(1)= goal. Continuity of τ(s) means : ( ( ) ( ( )) ) [ ] ( ) α A s 0,1 lim max α τ s α τ s = 0 0 s s α Α 0 0 Obviously, the position of each α A depends on the posture = τ(s), of A. A being a «free object» means that if there are no obstacles then every path defined as above is feasible. Limitations: If s parameterization is wrt arc length then point rotations are not allowed Other types of parameterization (e.g. time) do not allow decomposition to path and trajectory planning. τ(s 0 ) α s=s 0 s=0 τ(s 0 +ds) α s=s 0 +ds s=0 s=1 α ds=0 10 α s=1

11 The Basic Motion Planning Problem cont. Initial position and orientation Α B 2 Α B 3 B 1 B 2 Final position and orientation B 1 W R 2 11

12 Τα Εµπόδια στο Χώρο Στάσεων An obstacle B i in workspace W, is represented in C by CB = { C / A( ) I B } i called «C-obstacle». U i= 1 CB «C-obstacle space» «Free space» i : «free pose». U Free path between two (free) poses init & goal : a continuous mapping i Configuration space C of A: i.e. the space of all postures of A 1 C free = C \ CBi = { C / A( ) I ( Bi ) } i= 1 i= 1 C free A( 1 ) B i τ(s):[0,1] C free, τ(0)= init and τ(1)= goal. U B j CB j CB CB j j A( 2 ) CB i CB CB C C C free i i free free

13 Παράδειγµα Robot A (triangle) freely translating without changing orientation. F A : coordinate frame attached at A. The pose of A can be represented by the coordinates of F A wrt F W i.e. =(x,y). Thus, the configuration space of A is C=R 2. The C-obstacle CB i is the set of all those that can be approached by F A without collision of A with B i. CB i is obtained by enlarging obstacle B i using A as it moves from posture 0 to 1, 2, 3, 4, 5, 6 following the direction of red arrows Path planning of A wrt B i is euivalent to motion planning of the specific corner of A wrt CB i. 13

14 Roadmap: visible path Visible path in 2-D space with polygonal obstacles. The graph nodes are the initial and final poses and the vertices of the obstacles. Two nodes are connected only if their line segment does not intersect the obstacles. 14

15 Roadmap: ιάγραµµα Voronoi Voronoi diagram for a configuration space of polygonal C-obstacles. The free space is bounded by a rectangle. 15

16 Accurate Cell Decomposition Free space and obstacles (fig. α). 1 st step : decomposition of free space in trapezoidal and traingular cells (fig. β). 2 nd step : connectivity graph (fig. γ) and search over it to find a path (bold line). 16

17 Accurate Cell Decomposition cont. The path on the connectivity graph defines the cell seuence. 3 rd step from the cell seuence to a free path using an optimization criterion. 17

18 Approximate Cell Decomposition The free space bounded by an outer suare and 3 polygons (fig. α). R is decomposed in 4 eual suares. If a suare is wholly found in the free space or in the C-obstacle space then it is not decomposed further. Otherwise it is decomposed in 4 suare until a specific resolution is achieved. The result is presented on fig. β. White cells are not located in the C-obstacle space. Black cells are located in the C-obstacle space. Grey cells are intersected by the C-obstacle space. This is a "uadtree decomposition. The bold face line represents a free channel. 18

19 Introduction to Artificial Potential Basic Idea Free Space C free Fields) The point robot moves following the model & = u We consider A virtual (positive) charge on the robot found at C free A virtual (negative) charge on the destination goal results to an attractive potential ϕ attr ( ) on the robot found at C free virtual (positive) charges on the obstacles CB i introduce repulsive potential ϕ ( ) on the robot found at C free, rep And we obtain an artificial potential field ( ) ( ) + ( ) ϕ ϕ ϕ attr rep (hoping that) it will lead the robot to goal based on the control law ( ) & = u= ϕ 19

20 Τεχνητά υναµικά Πεδία: Παράδειγµα 2-D space with 2 polygonal obstacles (fig. Α). Fig. β represents the attractive potential φ attr () due to final posture goal. goal Fig. γ represents the repulsive potential φ rep () due to C-obstacles. Fig. δ represents φ()=φ attr ()+φ rep () Fig. ε represents the euipotential line of the tatal potential and the path obtained using stepest descent. & = ϕ ( ) Fig. ζ represents the vector field found at each point of the free space. 20

21 Artificial Potential Fields: Simulation Example The positive robot is led to the negative destination avoiding the positive obstacle 21

22 Artificial Potential Fields: Steepest Descent 3 2 [ ] T = x y : robot position 1 0 & = u ϕ(, d ): Potential Function ϕ u = K 22

23 Artificial Potential Fields: Do they always work? The positive robot is NOT lead to the negative destination because it is trapped in a local minimu due to the positive obstacle 23

24 Artificial Potential Fields: Why they do not always work? 3 2 ϕ u= K ϕ = 0 24

25 Navigation Functions: curing the Artificial Potential Fields Η NF φ()είναι µια ειδική µορφή συνάρτησης δυναµικού, η οποία έχει ένα µοναδικό ελάχιστο, το τελικό configuration G του ροµπότ.. ηλαδή αν το ροµπότ κινηθεί µε το νόµο ελέγχου. τότε t G u = ϕ ( ) Προφανώς, ως συνάρτηση δυναµικού, η συνάρτηση πλοήγησης χαρακτηρίζεται από µια συνάρτηση «ελκτικού» δυναµικού, που έλκει το ροµπότ προς το τελικό configuration G, και από µια συνάρτηση «απωστικού» δυναµικού,, που ορίζεται γύρω από τα εµπόδια του χώρου και ωθεί το ροµπότ µακριά από αυτά Η NF ορίζεται για χώρους που περιλαµβάνουν εµπόδια γνωστής γεωµετρίαςκαι βρίσκονται σε γνωστές, σταθερές θέσεις. 25

26 Navigation Functions: curing the Artificial Potential Fields Η απλούστερη µορφή χώρου είναι σφαιρικός χώρος, αποτελούµενος από µια µεγάλη σφαίρα που περιέχει µικρότερες σφαίρες στο εσωτερικό της, οι οποίες αναπαριστούν τα φυσικά εµπόδια. D 0 (0,ρ 0 ) Ο χώρος εργασίας (workspace) του ροµπότ οριοθετείται από τον κυκλικό δίσκο D 0 (0,ρ 0 ) στον οποίο περιέχονται Μ (µικροί) D j ( j,ρ j ) κυκλικοί D j ( j,ρ j ) δίσκοι, που οριοθετούν τα εµπόδια. Τα j, ρ j είναι γνωστά. Οι µικροί δίσκοι δεν τέµνονται µεταξύ τους και βρίσκονται εξολοκλήρου µέσα στο µεγαλύτερο δίσκο. Oελεύθερος χώρος W free, στον οποίο µπορεί να κινηθεί το ροµπότ αποφεύγοντας τη σύγκρουση µε τα εµπόδια, προκύπτει αν από το χώρο εργασίας Wαφαιρεθεί ο συνολικός χώρος των 26 εµποδίων. ρ 0 ρ j

27 Navigation Functions: curing the Artificial Potential Fields Οι Rimon και Koditschek απέδειξαν ότι η NFφ()πρέπει να πληροί συγκεκριµένες µαθηµατικές ιδιότητες, ώστε να εγγυάται τη σύγκλιση του ροµπότ στο επιθυµητό configuration, µε ταυτόχρονη αποφυγή των εµποδίων του χώρου. Η συνάρτηση πρέπει να περιλαµβάνει Ελκτικό υναµικό: Συνάρτηση απόστασης από το τελικό σηµείο γ = ( ) - G Έλκει το ροµπότ στο µοναδικό ελάχιστο της συνάρτησης (το τελικό 2κ configuration), µε κτην «ενταση της έλξης», και Απωστικό δυναµικό: ωθεί το ροµπότ µακριά από τα όρια των εµποδίων. 27

28 Navigation Functions: curing the Artificial Potential Fields Απωστικό υναµικό: Συνάρτηση εµποδίων D 0 (0,ρ 0 ) Παράγων από όρια workspace β ( ) = ρ0 2 ( ) ( D% ) β = ρ β = ρ 0 ρ j Παράγοντες από εµπόδια β ( ) 2 2 j = - j ρ j β = ρ β % = 0 β > 0 ( 2 ) ( D ) ( D ) j j j j j j j D j ( j,ρ j ) Συνολική «Συνάρτηση Εµποδίων» β M = ( ) ( ) i= 0 β i 28

29 Navigation Functions: curing the Artificial Potential Fields Αν επιλεχθεί ως Συνάρτηση Πλοήγησης (Navigation Function NF )η 2κ ελκτικό δυναµικό ( ) ˆ ϕ ( ) = = απωθητικό δυναµικό β ( ) παρατηρείστε ότι ( ) ( ) γ = M i= 0 - G ˆ ϕ = 0 = lim ˆ ϕ = j= 0,1, K, M G D% Επειδη θέλουµε το πεδίο τιµών της NF να είναι το [0,1], προτείνουµε την NF : ϕ ' = σ o ˆ ϕ= σ( ˆ ϕ) όπου x σ ( x ) 1+ x Εποµένως παίρνουµε την αναλυτική συνάρτηση ϕ ' = σ o ˆ ϕ = σ ˆ ϕ = j ( ) γ β+ γ ( ) ( ) G ϕ j β i ( ) ϕ ' = 0 = ' D% = 1 j= 0,1, K, M 29

30 Navigation Functions: curing the Artificial Potential Fields Η αναζητούµενη NFφ()πρέπει να είναι συνάρτηση Morse : δηλ. στα κρίσιµα σηµεία της (δηλ. στα σηµεία που ϕ( ) = 0 ) 2 πρέπει ϕ( ) 0 (Γιατί?). 1 κ Η συνάρτηση ϕ= σ oϕ ' όπου σ d( x) x είναι d 1 γ κ 2κ - G ϕ = σ ˆ d oσ oϕ = = M γ + β 2κ - G βi( ) + i= 0 και αποδεικνύενται ότι υπάρχει Ν, όπου όταν κ Ν, η φ()είναι συνάρτηση Morse και εποµένως είναι συνάρτηση πλοήγησης. & = u u= K ϕ( ) Έτσι, όταν η είσοδος στο σύστηµα είναι το ροµπότ συγκλίνει στο τελικό configuration, δηλ. t G, ξεκινώντας σχεδόν από οποιαδήποτε αρχική συνθήκη, µε 30 ταυτόχρονη αποφυγή των εµποδίων. 1 κ

31 Navigation Functions: curing the Artificial Potential Fields Η σύγκλιση στο τελικό configuration είναι από «σχεδόν όλες» τις αρχικές συνθήκες. Αυτό συµβαίνει γιατί κάθε εµπόδιο του χώρου εισάγει τουλάχιστον ένα σηµείο σάγµατος (saddle point) στη NF, που είναι όµως σηµείο ασταθούςισορροπίας.. Η µεθοδολογία κατάστρωσης των συναρτήσεων πλοήγησης επεκτείνεται και εφαρµόζεται σε χώρους που περιέχουν εµπόδια (ή σύνολα επικαλυπτόµενων εµποδίων) µε γεωµετρία αστεροειδούς. Με χρήση κατάλληλων συµµόρφων µετασχηµατισµών, ένας τέτοιος χώρος µετασχηµατίζεται σε σφαιρικό και, αντίστοιχα, όταν ευρεθεί η συνάρτηση πλοήγησης του σφαιρικού κόσµου µετασχηµατίζεται κατάλληλα σε συνάρτηση πλοήγησης τού πραγµατικού κόσµου. Έτσι, οι συναρτήσεις πλοήγησης εφαρµόζονται επιτυχώς στον προγραµµατισµό της πορείας ενός ροµπότ σε έναν πραγµατικό κόσµο που περιλαµβάνει εµπόδια αρκετά πολύπλοκης γεωµετρίας. 31

32 Navigation Functions: The Global info is the cure ϕ = = 0 d 32

33 Navigation Functions: curing the Artificial Potential Fields The robot is NOT trapped in a local minimum because its potential is based at glolal information. 33

34 Multiple Robots Centralized vs De-centralized planning 34

35 Articulated Robots Robotic manipulator: typical case. Fig. α : cartesian robot with 3 prismatic and 3 rotational joints. Fig. β : represents a robot with 6 rotational joints. 35

36 Articulated Robots cont. A seuence of postures for a planar 8-joint manipulator using the artificial potential fields approach. 36

37 Non-Holonomic Constraints Car-Robot A modeled as a rectangle. State of A described by 3 parameters (x,y,θ) R 2 [0,2π], with modulo 2π for angle θ. At every instant we have : -sinθ dx+cosθ dy=0 This is a non holonomic euality constraint. In general, φ φ max <π/2. 37

38 Non-Holonomic Constraints cont. Parallel parking for a nonholonomic car-robot. 38