2B WMR motion planning - unitn.it€¦ · 2B WMR motion planning ... E- Hybrid trajectory and...

A. Cesarini - Navigation and trajectory control for autonomuous vehicles

2B

WMR motion planning

Course coordinator: Prof. M. De Cecco

Course notes: L. Baglivo, M. De Cecco, A. Cesarini, E. Zappia

Teaching: A. Cesarini Mecatronics Department, University of Trento

Email: [email protected]

http://www.miro.ing.unitn.it/


A- Introduction of the motion planning (MP)

B- Path planning

D- Optimal trajectory planning for constrained WMR

E- Hybrid trajectory and control planning via artificial potentials

- Flat outputs - Path planning Planning via the chained form Planning via Cartesian polynomials Planning via parameterized inputs Linear and generalized clotoids (continuous curvature)

Contents

-  Minimum-time trajectory


!x (t ) =v (t )cos!(t )!y (t ) =v (t )sin!(t )!!(t ) =! (t )v (t )!! (t ) = u (t )

!

"

##

$

##

%

!x (t )!y (t )!!(t )!! (t )

&

'

(((

)

*

+++=

cos!(t )sin!(t )! (t )0

&

'

(((

)

*

+++v (t )+

0001

&

'

((

)

*

++u (t )

Control in:

•  rear velocity

•  curvature velocity

! =1R=tan(!)b

!!(t ) =v (t ) ! tan(!)b

=! !v

x

y

O

ICR

XY

bPv

b/sin(α)

δ

α

x

y

O

ICR

XY

bPv

b/sin(α)

δ

α

R

Independent variable: time t !!!

Continuous curvature for car-like


Car-like (RD) kinematic model with control in:

•  curvature

•  geometric derivative of the curvature

Independent variable: length s !!!

'( ) cos ( )'( ) sin ( )'( ) ( )'( ) ( )

x s sy s ss ss u s

δ

δ

δ κ

κ

=⎧⎪ =⎪⎨

=⎪⎪ =⎩

!x (t ) = dxdsdsdt

= x '(s ) !s

!y (t ) = dydsdsdt

= y '(s ) !s

!!(t ) = d !dsdsdt

= ! '(s ) !s =! (s )v (t )

!! (t ) = u (s (t ))

!

"

####

$

####

Continuous curvature for car-like (2)


'( ) cos ( )'( ) sin ( )'( ) ( )'( ) ( )

x s sy s ss ss u s

δ

δ

δ κ

κ

=⎧⎪ =⎪⎨

=⎪⎪ =⎩

“integrating” 0 0

0 0

( ) cos ( ) ; ( ) ( )

( ) sin ( ) ; ( ) ( )

s s

s s

x s d s d

y s d s u d

δ ξ ξ δ κ ξ ξ

δ ξ ξ κ ξ ξ

= =

= =

∫ ∫

∫ ∫

Although such equations are numerically integrables, they do not represent a solution of the differential equation system since the posture (that is one of the configuration variables in the state) shows up inside the integrals !!

Integrating nonholonomic constraints


0k =

0k =

R1/k R=

0s =

Linking 2 segments with a circular arc

Question: Will be the WMR “mechanically” able to follow such path?

1s

2s

s0 1s 2s

( )sκ

1/ R

Continuous curvature for path smoothing


The curvature is linearly dependent to the arc length s :

20 0 0

0 0

20 0 0

0 0

20 0 0

0 0

1( ) ( ) ( )2

1( ) cos ( ) cos2

1( ) sin ( ) sin2

s s

s s

s s

s d c d s cs

x s x d x c d

y s y d y c d

δ δ κ ξ ξ δ λξ ξ δ λ

δ ξ ξ δ λξ ξ ξ

δ ξ ξ δ λξ ξ ξ

= + = + + = + +

⎛ ⎞= + = + + +⎜ ⎟⎝ ⎠

⎛ ⎞= + = + + +⎜ ⎟⎝ ⎠

∫ ∫

∫ ∫

∫ ∫

Curvature smoothing with linear clotoids

! =d !ds

= !s +! (0)

?


The Cartesian trajectory is computed integrating the following equations:

2

0

2

0

1( ) cos2

1( ) sin2

s

s

x s d

y s d

λξ ξ

λξ ξ

⎛ ⎞= ⎜ ⎟⎝ ⎠

⎛ ⎞= ⎜ ⎟⎝ ⎠

∫

∫

Note: “Fresnel” integrals do not possess a closed-form solution. However, they can be computed via power series approximation or via numerical methods

Curvature smoothing with linear clotoids (2)


Respectively, the canonic form of the Fresnel cosine and sine are:

2

0

2

0

( ) cos2

( ) sin2

x

x

CF x t dt

SF x t dt

π

π

⎛ ⎞= ⎜ ⎟⎝ ⎠

⎛ ⎞= ⎜ ⎟⎝ ⎠

∫

∫

Curvature smoothing with linear clotoids (3)

The evolution in the (x,y,s) space is known as the Cornu spiral whereas its projection on the (x,y) plane generates a clotoid arc.


We should highlight that the constant parameter λ is the “geometric” angular acceleration, the derivative in respect to the length of the arc s

(0)d sdsδ

κ λ κ= = +

Example: car-like kinematic model !x (t ) =v (t )cos!(t )!y (t ) =v (t )sin!(t )

!!(t ) =! (t )v (t ) = tan!(t )b

v (t )

!! (t ) = u (t )

!

"

###

$

###

being

max max;κ κ γ γ≤ ≤

Where γ is the angular acceleration

Example: clotids on car-like


! (t ) = d2"dt 2

= !#v = ddt

tan$b

!

"#

$

%&v =

!$vb cos2$

Angular acceleration of the vehicle:

! (t ) = d2!dt 2

=d (!v )dt

= !!v +! !v

Assuming the WMR steering at ν constant :

While, in curvilinear coordinates: 2

2( ) ( )d ds sds dsδ κ

γ κʹ′= = =

Example: clotids on car-like (2)


while the constraint on the angular acceleration is related to the steering velocity and have to be scaled on the velocity modulus

The curvature limit is related to the actual mechanical constraint on the steering relation:

maxmax

tanbα

κ =

!max =!!maxv

b cos2!max

! (t ) = !!vb cos2!

!

"#

$

%&

!! ! !! / cos!It works for ant α < αmax , since:



Imposing a linear trend variation on the curvature , we obtain: (0)d s

dsδ

κ λ κ= = +

! = !kv = dkdsv 2 = "v 2 = !#v

b cos2#! " =

1v

!#b cos2#

Therefore its possible to obtain the characteristics of the curvature suiting the mechanical constraints and performance of the WMR:

!max =1v

!!maxb cos2!max

="maxv 2



•  The radii of the circular arcs are

•  The curvilinear portion begins at Pi (0,0,0,0).

PLACE the origin of the reference system in Pi !!

•  The curvature is null after Pf (xf ,yf , θf ,0)

•  The length is until Pj

< kmax!1

kmax / !max

The curvature of a clotoid is directly proportional to the path length. The curvature of a linear clotoid that variates in the Cartesian space.

2max max max

2max max max

2max max

max

/ ( /( ))

/ ( /( ))

/(2 )

j

j

j

j

x CF k

y SF kPjkk

π λ πλ

π λ πλ

δ λ

κ

⎧ =⎪⎪ =⎪

= ⎨⎪ =⎪

=⎪⎩

Algorithm sketch for applying clotoids


•  When required to impose a final posture to the WMR, , the curvilinear portion can be made by two different clotoids with thecurvature coefficient computed for obtaining the required final posture (final condition):

minfδ θ<

2

2 2

(cos( / 2) ( / ) sin( / 2) ( / ))

sin ( / 2 )C f

f FresnelC f f FresnelS f

r

π θ θ π θ θ πλ

θ µ

⋅ + ⋅=

⋅ +

Algorithm sketch for applying clotoids (2)


2 3 4

2( ) (1 )2 2

r R φ φ φφ

γ γ= + − +

r(φ) R

R φ γ • R : raggio della circonferenza di raccordo

• R(φ) : raggio polare della spline

• φ : angolo corrente

• γ : angolo totale curva

( ) cos( )( ) sin( )x ry rφ φ

φ φ

=

=

Continuous curvature smoothing: polar splines


0 , ' 0, 0, ' 0, 0

r R rr R r

φ κ

φ γ κ

= ⇒ = = =

= ⇒ = = =

•  Initial conditions

2 2

2 2 3/2( ) 2 ' ( ) ( ) '' ( )( )[ ( ) ' ( )]

r r r rr r

φ φ φ φκ φ

φ φ

+ − ⋅=

+

2 2

2

2 2

12 2 2

'( ) / , '' /// 2 arctan( '/ )

'' 2 '1'

( ' )

r dr d r d r dd ds

r r

d rr rd r r

ds r r d

φ φ φ

κ δ

δ π φ

δφ

φ

= =

=

= + −

−= −

+

= +

Continuous curvature smoothing: polar splines

r(φ) R

R φ γ


•  Solution:

•  Unknown parameters:

•  Imposing final position, “assetto”

and curvature:

2 3( )k s a bs cs ds= + + +

p = [a b c d s f ]T

(0)a κ=

k (s ) = a +bs +cs 2 +ds 3 + ...

!(s ) = as + bs2

2+cs 3

3+ds 4

4+ ...

x (s ) = cos(as + bs2

2+cs 3

3+ds 4

4+ ...)

0

s

! ds

y (s ) = sin(as + bs2

2+cs 3

3+ds 4

4+ ...)

0

s

! ds

"

#

$$$$

%

$$$$

•  Increasing the curvature

polynomial degree it is

possible to minimize the

following functional equation:

2

0

1( ) [ ( )]2

fs

kJ p p dsκ= ∫

Continuous curvature smooting: generalized clotoids


0 0, 0, 0

, 0, 0e e

dyx ydxdyx x y ydx

κ

κ

= ⇒ = = =

= ⇒ = = =

•  Initial conditions •  Curvature

! =

d 2 ydx 2

1+ dydx!

"#

$

%&

2'

(

))

*

+

,,

32

•  Solve for the coefficient of the following polynomial equation:

2 3 4 50 1 2 3 4 5( )y x a a x a x a x a x a x= + + + + +

Continuous curvature smooting: quintics

y (x ) = ye 10xxe

!

"##

$

%&&

3

'15 xxe

!

"##

$

%&&

4

+6 xxe

!

"##

$

%&&

5(

)

**

+

,

--

•  Solution


Continuous curvature smooting: quintics

•  Note: the quintics suits posture variations

of max 45 degrees. However, it is possible to

concatenate them for reaching the desired

final posture.

•  Note2: it is always compatible with the

maximum curvature value imposed. It can be

verified by considering the ye/xe ratio

•  Note3: the relation with the controls is not

direct…


02

46

810

0

5

10

!2

!1

0

1

2

3

0 2 4 6 8 100

5

10

!0.5

0

0.5

1

1.5

2

ATTRACTIVE POTENTIAL REPULSIVE POTENTIAL

+

0 2 4 6 80

2

4

6

8

!1

0

1

2

3 RESULTING POTENTIAL FIELD

Potential field hybrid planning-control method

0 2 4 6 80

1

2

3

4

5

6

7

8

9

This example is in the Matlab script “OstacoliQuadrati.m”

THE RESULTING FORCE IS THE GRADIENT AND GIVES DIRECTION TO THE ROBOT !!!


•  Almost automatic treatment for OBSTACLE AVOIDANCE

•  PLANNING & CONTROL STRATEGY

•  Best for local (dynamical) path planning that provide the best solution for ->

unexpected obstacles

•  Be aware to LOCAL MINIMA for path solving (harmonic potential function

has been proven for global minima => ask the solution to advanced control

theory…)

•  This approach does not allow high precision positioning on the target

Potential field features

… allowing to directly apply GRAPH OPTIMIZATION ALGORITHMS !!!!


!U=

"U

"x

"U

"y

#

$

%%%%

&

'

((((

A formulation for potential field approach

dynamically !!

FORMULATION IN THE

CARTESIAN SPACE !!!!!


A formulation for potential field approach (2)



LIMITATIONS IN USING THIS “UNSMOOTHED”

REPULSIVE POTENTIAL DISTRIBUTIONS ???



CONVEX + DIFFERENTIABLE


•  The attractive potential can be defined punctually as desired.

•  Build a vector field that point towards the desired path.

Object picking application for forklifts !!!

Advantages of the potential field approach


Bibliography

1.  Siegwart R., Nourbakhsh I, Scaramuzza D., Introduction to Autonomous Mobile Robots

2.  A. De Luca, Dispense del corso di Robotica I/II, Universita’ “La Sapienza” (Roma 1)

3.  Siciliano and Sciavicco, “Robotica Industriale”, 2000. 4.  Siciliano, Sciavicco, Villani and Oriolo, “Robotics: modeling, planning and

control”, 2009. 5.  A. De Luca e G. Oriolo, Introduction to the chained form, Chapter 7. 6.  A. De Luca, Dispense del Corso di Controlli Automatici II, Roma 3


Dictionary

1.  Chained-formó forma a catena (litt. incatenata) 2. …

2B WMR motion planning - unitn.it€¦ · 2B WMR motion planning ... E- Hybrid trajectory and...

Documents

Transcript of 2B WMR motion planning - unitn.it€¦ · 2B WMR motion planning ... E- Hybrid trajectory and...