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