The Robotics Institute 2014 Master of Science in Robotic ...
Introduction to Robotic Motion Planningnereus.mech.ntua.gr/courses/robotics/robotics_pdf/... ·...
Transcript of Introduction to Robotic Motion Planningnereus.mech.ntua.gr/courses/robotics/robotics_pdf/... ·...
Introduction to Robotic Motion Introduction to Robotic Motion Planning
1
Robot Planning & Control Architecture
Trajectory (i.e. “geometry”+time)
RobotRobot
Environment
,q q&Control:Trajectory / Interaction
uTrajectoryGenerator
( )dq t
( )dh t ,f µ
Path (i.e. “geometry”)
Trajectory (i.e. “geometry”+time)
PathPlanner
TaskPlanner
( ) [ ]0,1i s sτ ∈
( ) [ ]0,1ih s s∈
( ) ( )0 , 1 1, ,i i i Nτ τ = K
UserRequirements
&Specifications
Path (i.e. “geometry”)Task
2
Το Πρόβληµα του Προγραµµατισµού της Κίνησης• 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
Το Πρόβληµα του Προγραµµατισµού της Κίνησης
• Motion Planning is quite more involved than obstacle avoidance.obstacle avoidance.
• Computational complexity is a central issue in motion planning.
4
Motion Planning Issues
• This simple assembly is definedby the 3 composing parts and asequence of 2 spatial relations.sequence of 2 spatial relations.
• Every relation can be defined as aset of elementary relationshipsbetween geometric features e.g.“Face Α of obj. 1 must fit face Β ofobj. 2", “Shaft C of obj. 3 fits holeD of obj. 1"
• Then it can be translated to aquantitative formulation e.g. usinga “homogeneous transformation”
5
The Basic Motion Planning Problem
• ΑΑ : solid object (robot) moving in the subspaceWW (work-space)of theeuclideanspaceℜℜ NN, whereNN = 2 oror 3.
ΑΑ ΑΑ
space)of theeuclideanspaceℜℜ , whereNN = 2 oror 3.•• ΑΑ is a «free object», i.e. there is no motion constraint.
6WW∈∈∈∈ℜℜℜℜℜℜℜℜ 2
The Basic Motion Planning Problem
•• BB11,, BB22,, ……,, BBmm solid objects distributed overWW. BBii are calledobstacles.
BB33
ΑΑ ΑΑ
BB
BB22
obstacles.• The geometry ofΑΑ,, BB11,, BB22,, ……,, BBmm and the positions ofBBii in WW
are accurately known.
7
BB22
BB11 WW∈∈∈∈ℜℜℜℜℜℜℜℜ 2
BB11
The Basic Motion Planning Problem
• The problem is:– Considering an initialinitialinitialinitial position & orientation and a finalfinalfinalfinal position & orientation of
ΑΑ in WW, produce a path ττ defining a continuous sequence of positions andorientations of ΑΑ avoiding collisions with BBii , starting from the initial position &
Initial position
and orientation
BB
ΑΑ ΑΑ
BB22
orientations of ΑΑ avoiding collisions with BBii , starting from the initial position &orientation and arriving at the final position & orientation. Report failure ofdetermining such a path.
8
Final position
and orientation
BB22
BB11
BB33
WW∈∈∈∈ℜℜℜℜℜℜℜℜ 2
BB11
The configuration space
•• FFA A , , FFWW : Cartesian systems attached at AA and WW.
ΑΑ(q)(q)
( ){ , ,C x y W= ∈attached at AA and WW.• The posture qq of AA is the
position and orientation of FFAA wrt FFWW .
• The subset of WW , occupied by AA , at posture qq , is AA((q)q) .
FFA A x
q y
θ
=
θ
( ){[ ]}
, ,,
C x y Wθ π π= ∈∈ − +
by AA , at posture qq , is AA((q)q) .
• The configuration spaceCC of AA is the space of all postures qq of AA
9
WW∈∈∈∈ℜℜℜℜℜℜℜℜ 2
«qq» is used here to denote the posture of a robot. It should not beconfused with the, previously used, DOFs of a robotic mechanism.
FFWW
θ
Η έννοια της πορείας (path)• A path of AA from the initial posture qinit to qgoal is a continuous mapping
τ(s):[0,1]→→→→CCwith τ(0)=qinit and τ(1)=qgoal .
• Continuity of τ(s)means :
Obviously, the position of each depends on the posture q = τ(s), of AA.
[ ] ( )( ) ( )( )( )0
0 00,1 lim max 0s s
A s s sα
α α τ α τ→ ∈Α
∀ ∈ ∀ ∈ − =Aα∀ ∈Obviously, the position of each depends on the posture q = τ(s), of AA.
•• AA 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
s=1
s=s0 s=s0+ds
τ(s0)τ(s0+ds)
α α
Aα∀ ∈
α
10
– Other types of parameterization (e.g. time) do not allow decomposition to path and trajectory planning.
s=0
s=0
s=1
αds=0
The Basic Motion Planning Problem cont.
Initial position
and orientation
ΑΑ ΑΑ
BB22
Final position
orientation
BB22
BB11
BB33 BB11
position and
orientation
BB11 WW∈∈∈∈ℜℜℜℜℜℜℜℜ 2
11
Τα Εµπόδια στο Χώρο Στάσεων• An obstacle BBii in workspace WW, is
represented in CC by
called «C-obstacle».
{ / ( ) }i iCB q C A q B= ∈ ≠ ∅I
Configuration space CC of AA: i.e. the space of all postures qq of AA 1
2
3
i
i
i
q CB
q CB
q CB
∉
∉
∈called «C-obstacle».
• «C-obstacle space»
• «Free space»
• : «free pose». • Free path between two (free) poses qinit &
qgoal : a continuous mapping
1
q
ii
CB=U
1 1
\ { / ( ) ( ) }q q
free i ii i
C C CB q C A q B= =
= = ∈ ≠∅IU Ufreeq C∈ A(q1) Bi
qgoal : a continuous mapping τ(s):[0,1]→→→→Cfree, τ(0)=qinit andτ(1)=qgoal .
12A(q2)
Bj
1
2
3
j
j
j
q CB
q CB
q CB
∈
∉
∉
1
2
3
free
free
free
q C
q C
q C
∉
∈
∉
Παράδειγµα
1
2 3
4
• Robot AA (triangle) freely translating without changing orientation.• FFAA : coordinate frame attached at AA.
• The pose qq of AA can be represented by the coordinates of FFAA wrt FFWW i.e.
0
5
6
• The pose qq of AA can be represented by the coordinates of FFAA wrt FFWW i.e.q=(x,y) . Thus, the configuration space of AA is C=ℜℜℜℜ 2.
• The C-obstacle CBi is the set of all those qq that can be approached by FFAAwithout collision of AA with BBii .
• CBi is obtained by enlarging obstacle BBii using AA as it moves from posture 0to 1, 2, 3, 4, 5, 6 following the direction of red arrows
• Path planning of AA wrt BBii is equivalent to motion planning of the specificcorner of AA wrt CBi. 13
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 connected only if their line segment does not intersect the obstacles.
14
Roadmap: ∆ιάγραµµα Voronoi
• Voronoi diagram for a configuration space of polygonal C-obstacles.
• The free space is bounded by a • The free space is bounded by a rectangle.
15
Accurate Cell Decomposition
• Free space and obstacles (fig.α).
• 1st step : decomposition of freespace in trapezoidal andtraingular “cells” (fig. β).
• 2nd step : connectivity graph (fig.γ) and search over it to find apath (bold line).
16
Accurate Cell Decomposition cont.
• The path on theconnectivity graphdefines the cellconnectivity graphdefines the cellsequence.
• 3rd step from the cellsequence to a free pathusing an optimizationcriterion . 17
Approximate Cell Decomposition
• The free space bounded by an outer square and 3 polygons (fig. α). • R is decomposed in 4 equal squares. • If a square 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 square until a specific resolution is • Otherwise it is decomposed in 4 square 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 "quadtree“ decomposition.• The bold face line represents a free channel.
18
Introduction to Artificial Potential Fields)
Basic Idea• Free Space Cfree• The point robot moves following the model q u=&• The point robot moves following the model• We consider
– A virtual (positive) charge on the robot found at q ∈∈∈∈ Cfree
– A virtual (negative) charge on the destination qgoal results to an attractive potential on the robot found at q ∈∈∈∈ Cfree
– virtual (positive) charges on the obstacles CBi introduce repulsive potential
on the robot found at q ∈∈∈∈ Cfree,
q u=&
( )attr qϕ
( )rep qϕ on the robot found at q ∈∈∈∈ Cfree,
And we obtain an artificial potential field
• …(hoping that) it will lead the robot to qgoal based on the control law
( ) ( ) ( )attr repq q qϕ ϕ ϕ+�
( )qq u qϕ= = −∇&
( )rep qϕ
19
Τεχνητά ∆υναµικά Πεδία: Παράδειγµα• 2-D space with 2 polygonal
obstacles (fig. Α).• Fig. β represents the
attractive potential φattr(q)due to final posture qgoal.
• Fig. γ represents thegoal
• Fig. γ represents therepulsive potential φrep(q)due to C-obstacles.
• Fig. δ representsφ(q)=φattr(q)+φrep(q)
• Fig. ε represents theequipotential line of the tatalpotential and the pathobtained using stepestdescent.descent.
• Fig. ζ represents the vectorfield found at each point ofthe free space.
( )qq qϕ= −∇&
20
Artificial Potential Fields: Simulation Example
• The positive robot is led to the negative destination avoiding the positive obstacle…
21
0
1
2
3
Artificial Potential Fields: Steepest Descent
[ ] : robot positionT
q x y=
q u=&
-3 -2 -1 0 1 2 3-3
-2
-1
0
ϕ∂= −
( , ): Potential Functiondq qϕ
u Kq
ϕ∂= −
∂
22
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
0
1
2
3
Artificial Potential Fields: Why they do not always work?
u Kq
ϕ∂= −
∂
-4 -3 -2 -1 0 1 2 3-3
-2
-1
0
0q
ϕ∂=
∂
24
• Η NF φ(q) είναι µια ειδική µορφή συνάρτησης δυναµικού, η οποία έχει ένα µοναδικό ελάχιστο, το τελικό configuration qGτου ροµπότ. ∆ηλαδή αν το ροµπότ κινηθεί µε το νόµο ελέγχου
Navigation Functions: “curing” the Artificial Potential Fields
( )ϕ= −∇ q q→του ροµπότ. ∆ηλαδή αν το ροµπότ κινηθεί µε το νόµο ελέγχου. τότε
• Προφανώς, ως συνάρτηση δυναµικού, η συνάρτηση πλοήγησης χαρακτηρίζεται– από µια συνάρτηση «ελκτικού» δυναµικού, που έλκει το ροµπότ προς το τελικό configuration qG, και
– από µια συνάρτηση «απωστικού» δυναµικού, που ορίζεται γύρω από τα
( )u qϕ= −∇ Gtq q→∞→
– από µια συνάρτηση «απωστικού» δυναµικού, που ορίζεται γύρω από τα εµπόδια του χώρου και ωθεί το ροµπότ µακριά από αυτά
• Η NF ορίζεται για χώρους που περιλαµβάνουν εµπόδια γνωστής γεωµετρίας και βρίσκονται σε γνωστές, σταθερές θέσεις.
25
ρ0
D0 (0,ρ0)
• Η απλούστερη µορφή χώρου είναι σφαιρι-κός χώρος, αποτελούµενος από µια µε-γάλη σφαίρα που περιέχει µικρότερες σφαίρες στο εσωτερικό της, οι οποίες
Navigation Functions: “curing” the Artificial Potential Fields
ρ
ρj
Dj (qj,ρj)
σφαίρες στο εσωτερικό της, οι οποίες αναπαριστούν τα φυσικά εµπόδια.
• Ο χώρος εργασίας (workspace) του ροµπότ οριοθετείται από τον κυκλικό δίσκο D0(0,ρ0) στον οποίο περιέχονται Μ (µικροί) κυκλικοί Dj(qj,ρj) δίσκοι , που οριοθετούν τα εµπόδια. εµπόδια.
• Τα qj , ρj είναι γνωστά. Οι µικροί δίσκοι δεν τέµνονται µεταξύ τους και βρίσκονται εξολοκλήρου µέσα στο µεγαλύτερο δίσκο.
• O ελεύθερος χώρος Wfree, στον οποίο µπορεί να κινηθεί το ροµπότ αποφεύγοντας τη σύγκρουση µε τα εµπόδια, προκύπτει αν από το χώρο εργασίας W αφαιρεθεί ο συνολικός χώρος των εµποδίων.
26
• Οι Rimon και Koditschek απέδειξαν ότι η NF φ(q) πρέπει να πληροί συγκεκριµένες µαθηµατικές ιδιότητες, ώστε να εγγυάται τη σύγκλιση του ροµπότ στο επιθυµητό configuration, µε ταυτόχρονη αποφυγή των εµποδίων του χώρου.
Navigation Functions: “curing” the Artificial Potential Fields
ταυτόχρονη αποφυγή των εµποδίων του χώρου.• Η συνάρτηση πρέπει να περιλαµβάνει
– Ελκτικό ∆υναµικό: Συνάρτηση απόστασης από το τελικό σηµείο
Έλκει το ροµπότ στο µοναδικό ελάχιστο της συνάρτησης (το τελικό
κ
( ) 2- Gq q q
κγ =
configuration), µε κ την «ενταση της έλξης», και
– Απωστικό δυναµικό: ωθεί το ροµπότ µακριά από τα όρια των εµποδίων.
27
ρ0
D0 (0,ρ0)• Απωστικό ∆υναµικό: Συνάρτηση εµποδίων• Παράγων από όρια workspace
Navigation Functions: “curing” the Artificial Potential Fields
( ) 2 2-q q qβ ρ= − + ρ
ρj
Dj (qj,ρj)
• Παράγοντες από εµπόδια
( )( ) ( )
20 0 0
20 0 0 0 0
-
0
q q q
q q D
β ρ
β ρ β
= − +
= ∈ =%
( )
( ) ( ) ( )
2 2
2
-
0 0
j j j
j j j j j j j
q q q
q q D q D
β ρ
β ρ β β
= −
= − ∈ = ∉ >%
• Συνολική «Συνάρτηση Εµποδίων»
( ) ( ) ( )0 0j j j j j j jq q D q Dβ ρ β β= − ∈ = ∉ >%
( ) ( )0
M
ii
q qβ β=
=∏28
• Αν επιλεχθεί ως Συνάρτηση Πλοήγησης (Navigation Function –NFNF ) η
Navigation Functions: “curing” the Artificial Potential Fields
( ) ελκτικό δυναµικόˆ
απωθητικό δυναµικόqϕ = ( )
( )q
q
γβ=
( )
2- G
M
q qκ
=
∏παρατηρείστε ότι
• Επειδη θέλουµε το πεδίο τιµών της NF NF να είναι το [0,1], προτείνουµε την NFNF : όπου
απωθητικό δυναµικό ( )qβ ( )0
M
ii
qβ=∏
( ) ( )ˆ ˆ0 lim 0,1, ,j
Gq D
q q q q j Mϕ ϕ→
= ⇔ = = ∞ =%
K
( )ˆ ˆ'ϕ σ ϕ σ ϕ= =o( )
1
xx
xσ
+�
• Εποµένως παίρνουµε την αναλυτική συνάρτηση
( )1
xx
σ+
�
( )
( ) ( )
ˆ ˆ'
' 0 ' 1 0,1, ,G jq q q q D j M
γϕ σ ϕ σ ϕ
β γ
ϕ ϕ
= = =+
= ⇔ = ∈ = =
o
% K 29
• Η αναζητούµενη NFNF φ(q) πρέπει να είναι συνάρτηση Morse : δηλ. στα κρίσιµα σηµεία της (δηλ. στα σηµεία που ) πρέπει (Γιατί?).
Navigation Functions: “curing” the Artificial Potential Fields
( )2 0qϕ∇ ≠( ) 0qϕ∇ =
( )1
'ϕ σ ϕ=• Η συνάρτηση όπου είναι
Ν φ(q)
( )( )
1
d x xσ � κ'dϕ σ ϕ= o
1
ˆd
κγϕ σ σ ϕ
γ β
= = + o o
( )
1
2
2
0
-
-
GM
G ii
q q
q q q
κ
κ
κ β=
= +
∏και αποδεικνύενται ότι υπάρχει Ν, όπου όταν κ ≥ Ν, η φ(q) είναι συνάρτηση Morse και εποµένως είναι συνάρτηση πλοήγησης.
• Έτσι, όταν η είσοδος στο σύστηµα είναι το ροµπότ συγκλίνει στο τελικό configuration, δηλ. , ξεκινώντας σχεδόν από οποιαδήποτε αρχική συνθήκη, µε ταυτόχρονη αποφυγή των εµποδίων.
q u=& ( )u K qϕ= − ⋅∇Gt
q q→∞→30
• Η σύγκλιση στο τελικό configuration είναι από «σχεδόν όλες» τις αρχικές συνθήκες. Αυτό συµβαίνει γιατί κάθε εµπόδιο του χώρου εισάγει τουλάχιστον ένα σηµείο σάγµατος (saddle point) στη NFNF, που είναι όµως σηµείο ασταθούς ισορροπίας.
Navigation Functions: “curing” the Artificial Potential Fields
σηµείο ασταθούς ισορροπίας. • Η µεθοδολογία κατάστρωσης των συναρτήσεων πλοήγησης επεκτείνεται και
εφαρµόζεται σε χώρους που περιέχουν εµπόδια (ή σύνολα επικαλυπτόµενων εµποδίων) µε γεωµετρία αστεροειδούς. Με χρήση κατάλληλων συµµόρφων µετασχηµατισµών, ένας τέτοιος χώρος µετασχηµατίζεται σε σφαιρικό και, αντίστοιχα, όταν ευρεθεί η συνάρτηση πλοήγησης του σφαιρικού κόσµου µετασχηµατίζεται κατάλληλα σε συνάρτηση πλοήγησης τού πραγµατικού κόσµου.
• Έτσι, οι συναρτήσεις πλοήγησης εφαρµόζονται επιτυχώς στον προγραµµατισµό της πορείας ενός ροµπότ σε έναν πραγµατικό κόσµο που περιλαµβάνει εµπόδια αρκετά πολύπλοκης γεωµετρίας.
31
1
2
3
4
Navigation Functions: The Global info is the “cure”
-5 -4 -3 -2 -1 0 1 2 3 4 5-4
-3
-2
-1
0
0 dq qq
ϕ∂= ⇔ =
∂
32
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
Multiple Robots
• Centralized vs De-centralized planning
34
Articulated Robots
• Robotic manipulator:typical case.typical case.
• Fig. α : cartesianrobot with 3 prismaticand 3 rotationaljoints.
• Fig. β : represents a• Fig. β : represents arobot with 6 rotationaljoints.
35
Articulated Robots cont.
• A sequence of postures for a postures for a planar 8-joint manipulator using the artificial potential fields approach.
36
Non-Holonomic Constraints• At every instant we
have : -sinθ dx+cosθ dy=0
• Car-Robot A modeled as a rectangle.• State of A described by 3 parameters
(x,y,θ) ∈ ℜ2×[0,2π], with modulo 2π for angle θ.
-sinθ dx+cosθ dy=0
• This is a non holonomic equality constraint.
• In general, |φ|≤φmax<π/2.
37
Non-Holonomic Constraints cont.
• Parallel parking for a nonholonomic car-robot.
38