Le η-splines

Generazione di curve con continuità geometrica del terzo ordine

Sommario:

Il principio di scomposizione velocità-percorso

Primitive di pianificazione delle curve

Le η-splines

Parma Gennaio 2005

C. Guarino Lo Bianco

• Particolarmente adatto alla tecnica di controllo proposta

• Flessibilità

Kant, K. and S.W. Zucker, “Toward efficient trajectory planning:

the path-velocity decomposition,” Int. J. of Robotics Research 5(3), pp. 72–89, 1986

PATH-VELOCITY DECOMPOSITION

ripianificazione velocità

• Pianificazione percorso

• Pianificazione velocità

ostacolo inatteso

• Flessibilità

PATH-VELOCITY DECOMPOSITION

ripianificazione velocità

• Pianificazione percorso

• Pianificazione velocità

obiettivo in ritardo o anticipo

( ) v t

( ) v t′

• Flessibilità:

possibilità di ripianificazione indipendente di percorso e velocità

continuità geometrica e dei segnali di controllo garantita Generazione di un

controllo smooth e flessibile per veicoli

autonomi

• Smoothness:

OBIETTIVO GENERALE DEL PROGETTO

1 3

( ), ( ) ( )

v t t C

u G

ω

∈ p

Aggiornamento ^• Continua rispetto alla curva

precedente;

• Tangente continua

• Curvatura continua

• Derivata della

curvatura continua Generare una curva, eventualmente raccordata,

con continuità geometrica G³

OBIETTIVO DELLA PIANIFICAZIONE

Soluzione proposta η-splines

• Data una curva planare regolare si può valutare la coordinata curvilinea come

• Associati ad ogni curva regolare vi sono due vettori PRINCIPALI RELAZIONI DELLE CURVE PLANARI

( ) u ( ) u τ

ν

2 2

( ) 1

cos sin

( ) ( )

c

d d du d

u ds du ds du ds du

d d

u u ds ds

θ θ

κ

= = = =

 

=  

 

= =

p p p p

τ p

p τ

ν

ɺ ɺ

0 1

0

:[ , ] [0, ]

( )

f u

f u u s

u s ξ dξ

→

→ =

∫

( ) : [ ( )u = α u β( )]u ^T p

θ

1 κc

d θ τ

1 κc

d θ

ds d τ

θ ds

κ ⁼

3

2 2 2

d ds

θ θ κ α β α β α β

= = = −

 + 

 

ɺɺ ɺ

ɺ ɺɺ

ɺ

ɺ ɺ

( ) cos

( ) ( ) sin

d d s

u ds ds s

α θ

β θ

   

= =   =  

   

τ p

0

0

0

0

( ) cos ( )

( ) sin ( )

s

s

s d

s d

α α θ ξ ξ

β β θ ξ ξ

= +

= +

∫

∫

PRINCIPALI RELAZIONI DELLE CURVE PLANARI

• Ottimalità

Curve a lunghezza minima

• Primi lavori di pianificazione del percorso

L. Dubins, “On curves of minimal length with a constraint on average curvature and with prescribed initial and terminal positions and tangents,” American Journal of Mathematics, vol. 79, pp. 497–517, 1957.

CENNI STORICI

κ

s

x

y

• Continuità geometrica del secondo ordine

W. Nelson, “Continuous steering-function control of robot carts,” IEEE Transactions on Industrial Electronics, vol. 36, no. 3, pp. 330–337, August 1989.

polar splines

Y. Kanayama and B. Hartman, “Smooth local path planning for autonomous vehicles,” in Proc. of the IEEE Int. Conf. on Robotics and Automation,

ICRA89, vol. 3, Scottsdale, AZ (US), May 1989, pp. 1265–1270.

cubic spirals

• Primitive di uso comune: clotoidi o cornu spirals CENNI STORICI

• Non ammettono espressioni in forma chiusa per

• Sono tipicamente usate per raccordare rette con archi di circonferenza raccordi G²

LE CLOTOIDI (CORNU SPIRALS)

1

( ) 2 ( ) ( )

s As Bs C

s s As B

θ

θ κ

= + +

= = +

ɺ

0

0

0

0

( ) cos ( )

( ) sin ( )

s

s

s d

s d

α α θ ξ ξ

β β θ ξ ξ

= +

= +

∫

∫

( ), ( ) s s

α β

x y

s

κ

• Quattro gradi di libertà nella pianificazione: A,B,C,s_f

mancano gradi di libertà per imporre

LE CLOTOIDI (CORNU SPIRALS)

(0) ; ( ) ; (0) ; ( )

i

s

s

θ = θ θ = θ θ ɺ = κ θ ɺ = κ

x y

(0), (0), ( ), ( )

f f

s s

α β α β

Possibile alternativa: composizione primitive di pianificazione Esempio: profilo G² per raccordare due tratti rettilinei

• Operando su s_f1 e su s_f2 si può cercare di imporre il punto di partenza o di arrivo (per tentativi)

LE CLOTOIDI (CORNU SPIRALS)

x y

2

2

1

1

1 0 2

1 2

1 2

1 2

(0) ; ( ) ;

(0) 0; ( ) 0;

( ) (0);

( ) (0).

f f

f f

f

s s s

s

θ θ θ θ

θ θ

θ θ

θ θ

= =

= =

=

=

ɺ ɺ

ɺ ɺ

θ

•Filone di studi per comporre primitive di pianificazione

• Percorsi G²

• Obstacle avoidance

• Lunghezza minima di percorso

• Limitatezza della curvatura

LE CLOTOIDI (CORNU SPIRALS)

x y

θ

arco di circonferenza clotoidi

• Non ammettono espressioni in forma chiusa per

• Sono tipicamente usate per raccordare rette: raccordi G²

3 2

2

1 1

( ) 6 2

( ) ( ) 1

2

s As Bs Cs D

s s As Bs C

θ

θ κ

= + + +

= = + +

ɺ

0

0

0

0

( ) cos ( )

( ) sin ( )

s

s

s d

s d

α α θ ξ ξ

β β θ ξ ξ

= +

= +

∫

∫

( ), ( ) s s

α β

y s

κ

x y

θ

cubic spiral

LE CUBIC SPIRALS (Kanayama & Hartman 1989)

LE CUBIC SPIRALS (Kanayama & Hartman 1989)

• cinque gradi di libertà nella pianificazione: A,B,C,D,s_f

anche considerando s_f mancano gradi di libertà per imporre

• Sintesi alternativa:

(0) ; ( ) ; (0) ; ( )

f f f

i

s

s

θ = θ θ = θ θ ɺ = κ θ ɺ = κ (0), (0), ( ), ( )

f f

s s

α β α β

3 2

2

1 1

( ) 6 2

( ) ( ) 1

2 ( ) ( )

s As Bs Cs D

s s As Bs C

s s As B

θ

θ κ

θ κ

= + + +

= = + +

= = +

ɺ

ɺɺ ɺ

(0) ; ( ) ;

(0) ; ( ) ;

f

f

i f

i f

s s

θ κ θ κ

θ κ θ κ

= =

= =

ɺ ɺ

ɺɺ ɺ ɺɺ ɺ

LE POLAR SPLINES (Nelson 1989)

• raccordi di tipo G²

2 3 4

( ) 1

2

2

r ϕ R ϕ ϕ ϕ

θ θ

 

=   + + +  

 

2 3

0 1 2 3

( ) ...

r ϕ = + a a ϕ + a ϕ + a ϕ +

(0) ; ( ) ;

(0) 0; ( ) 0;

(0) 0; ( ) 0

f

f

f

r R r R

dr dr

d d

θ

ϕ ϕ θ

κ κ θ

= =

= =

= =

x y

θ

R

R

( ) r ϕ ϕ

θ

raccordo tratti rettilinei

LE POLAR SPLINES (Nelson 1989)

• caratteristiche espressioni algebriche per

( ) sin

( ) [1 cos ]

i i

R R

α ϕ α ϕ

β ϕ β ϕ

= +

= + −

ϕ

κ α ϕ ^{( ) e ( )} β ϕ

• cambi di corsia:

Nessuna delle primitive presentate gestisce direttamente i cambi di corsia

sono sempre necessarie composizioni di curve

CAMBIO DI CORSIA (Nelson 1989)

la derivata della curvatura è ancora discontinua

x κ

• spline

y x ( ) = + a

a x

+ a x

+ a x

+ a x

+ a x

(0) 0; ( ) ;

(0) 0; ( ) 0;

(0) 0; ( ) 0

f f

f

f

y y x y

dy dy

dx dx x

κ κ x

= =

= =

= =

y

x

H. Delingette, M. Hèbert, and K. Ikeuchi, “Trajectory generation with

curvature constraint based on energy minimization.” in Proc. of the IEEE- RSJ Int. Conf. on Intelligent Robots and Systems, Osaka, Japan,

November 1991, pp. 206–211

intrinsic splines

S. Fleury, P. Souères, J.-P. Laumond, and R. Chatila, “Primitives for

smoothing paths of mobile robots,” Proc. of the IEEE Int. Conf. on Rob. and Autom., Atlanta, GA, September 1993, pp. 832-839

anticlothoids

O. Pinchard, A. Liégeois, and F. Puognet, “Generalized polar polynomials for vehicle path generation with dinamic constraints,” Proc. of the IEEE Int.

Conf. on Rob. and Autom., Minneapolis, MN, April 1996, pp. 915-920

generalized polar polynomials ALTRI LAVORI

• Espressioni per la curva, tangente alla curva,

curvatura, derivata della curvatura di tipo algebrico

• Esistenza della soluzione indipendentemente dalle condizioni al contorno

• Generazione della curva con un singolo raccordo

• Flessibilità della soluzione

le η-splines

SPECIFICHE PER LA SINTESI DI CURVE G³

2 3 4 5 6 7

0 1 2 3 4 5 6 7

2 3 4 5 6 7

0 1 2 3 4 5 6 7

( ) : ( )

( )

u u u u u u u u

u u u u u u u u u

α α α α α α α α α

β β β β β β β β β

 + + + + + + + 

 

=   =  

+ + + + + + +

   

p

con

u ∈ [ 0 , 1 ]

Controllo del moto di car-like vehicles G²-splines ORIGINE DELLE G³-SPLINES

• A. Piazzi and C. Guarino Lo Bianco, “Quintic G²-splines for

trajectory planning of autonomous vehicles,” in Proceedings of the IEEE Intelligent Vehicles Symposium, Dearborn (MI), USA,

October 2000, pp. 198–203.

• C. Guarino Lo Bianco and A. Piazzi, “Optimal trajectory planning with quintic G²-splines,” Proceedings of the IEEE Intelligent

Vehicles Symposium, Dearborn (MI), USA, October 2000, pp. 620- 625.

Controllo del moto di unicicli G³-splines

• A. Piazzi, M. Romano, C. Guarino Lo Bianco, “G³-splines for the path planning of wheeled mobile robots,” European Control

Conference 2003, ECC2003, September 2003.

VALUTAZIONE DEI PARAMETRI

α β

,

x y

p_A,κ

A

θ

A

θ

B

pB,κ

B

, , , , , , ,

A B

θ θ κ κ κ κ

p p ɺ ɺ

1 2 3 4 5 6

: [

η η η η η η ]

η ℝ ℝ ℝ ℝ ℝ ℝ

Sono state proposte

equazioni algebriche in funzione di

e di altri quattro parametri liberi

,

; , ( , ];

, , ,

A B A B

A B A B

θ θ π π κ κ κ κ

∈ ∈ −

∈ p p ℝ

ɺ ɺ ℝ con

Non vi sono limitazioni nella scelta dei parametri di interpolazione o degli η

VALUTAZIONE DEI PARAMETRI

α β

,

PROPRIETÀ DELLE ηηηη-SPLINES

Dato un generico insieme di dati di interpolazione

la curva soddisfa le condizioni al contorno per ogni valore di ηηηη.

Dimostrazione: (cenno) È una verifica:

• Si fissano dati di interpolazione arbitrari

• Si valutano i parametri delle spline

• Si valuta

)

;

(

p u

, , , , , , ,

A B

θ θ κ κ κ κ

p p ɺ ɺ

, , , , , , ,

A B

θ θ κ κ κ κ

p p ɺ ɺ

( , )

( ), ( )

i i

α

β

(0, )

p η p p

(1, )

PROPRIETÀ DELLE ηηηη-SPLINES Dimostrazione: (continua)

• Si valutano

•Si valutano

arg[ (1, )]

θ

3

2 2 2

(1, ) (1, ) (1, ) (1, ) (1, )

(1, ) (1, )

c

α β α β

κ

α β

= −

 + 

 

η η η η

η

η η

ɺɺ ɺ

ɺ ɺɺ

ɺ ɺ

κ

2 2 2

(0, ) (0, ) (0, ) (0, ) (0, )

(0, ) (0, )

c

α β α β

κ

α β

= −

 + 

 

η η η η

η

η η

ɺɺ ɺ

ɺ ɺɺ

ɺ ɺ

κ

(1, ) (1, )

(1, )

= p η τ η

p η ɺ

ɺ

arg[ (0, )]

θ

(0, )

(0, )

(0, )

= p η τ η

p η ɺ

ɺ

PROPRIETÀ DELLE ηηηη-SPLINES Dimostrazione: (continua)

• Si valutano

(1, ) (1, ) (1, ) (1, )

1 (1, )

c c

d d

d du

ds du ds du

κ κ

κ

η η η η pɺ η

κ

(0, ) (0, ) (0, ) (0, )

1 (0, )

c c

d d

d du

ds du ds du

κ κ

κ

η η η η pɺ η

κ

PROPRIETÀ DELLE ηηηη-SPLINES

Data una generica curva polinomiale del settimo ordine che soddisfa le condizioni al contorno e tale che , esiste certamente un valore di ηηηη tale che .

ovvero

Le η-splines sono una parametrizzazione completa di tutte le possibili curve polinomiali fino al settimo ordine che soddisfano i dati di interpolazione.

( ) ,

[ 0 , 1]

q

, , , , , , ,

A B

θ θ κ κ κ κ

p p ɺ ɺ

( ,

)

( )

p η q

( 0 )

0 e (1)

0

PROPRIETÀ DELLE ηηηη-SPLINES Dimostrazione: (cenno)

• Si suppone nota

• Si impongono le stesse condizioni al contorno per e

• Si verifica che e che

2 2

1 4

2 2

2 5

3 6

: (0) (0) : (1) sin (1) cos

: (1) (1) : (0) sin (0) cos

: (1) sin (1) cos : (0) sin (0) cos

B B

A A

B B

A A

η γ δ η δ θ γ θ

η γ δ η δ θ γ θ

η δ θ γ θ

η δ θ γ θ

= + = +

= + = +

= +

= +

ɺ ɺ ɺɺ ɺɺ

ɺ ɺɺɺ

ɺ ɺɺɺ

ɺɺ ɺɺ ɺɺɺ ɺɺɺ

( ) ,

[ 0 , 1]

q

2 3 4 5 6 7

0 1 2 3 4 5 6 7

2 3 4 5 6 7

0 1 2 3 4 5 6 7

( ) : ( )

( )

u u u u u u u u

u u u u u u u u u

γ γ γ γ γ γ γ γ γ

δ δ δ δ δ δ δ δ δ

 + + + + + + + 

 

=   =  

+ + + + + + +

   

q

)

;

(

p u

i i

α

γ β

δ

,

0 , . . . , 7

PROPRIETÀ DELLE ηηηη-SPLINES

La curva è la curva polinomiale di ordine minimo che può interpolare qualunque insieme di condizioni al contorno

, , , , , , ,

A B

θ θ κ κ κ κ

p p ɺ ɺ

)

;

(

p u

Dimostrazione: (cenno)

• Le η-splines sono una parametrizzazione completa di tutte le possibili curve polinomiali fino al settimo ordine impongo le condizioni al contorno per generare un cambio di corsia

non dipende da ηηηη ed è del 7°ordine

4 5 6 7

( ; ) ( ) :

35 84 70 20

u u

u u u u

α

 

=  − + −  p η

( )

β

PROPRIETÀ DELLE ηηηη-SPLINES Simmetria:

Si assumano

(1

; )

( ; )

p η p p p η

1 2

;

;

;

; 0 ; 0

A B A B A B

η η η η η η

θ θ κ κ κ κ

= = − =

= = = ɺ = ɺ =

allora la curva generata è simmetrica, ovvero

p(1- ; )u ´

p_A

p_B y

x x_A

y_A

d₁ d₂

p( ; )u ´

µ

x_B

y_B µ

PROPRIETÀ DELLE ηηηη-SPLINES

Generazione di segmenti rettilinei:

Si definisca e si assumano

c o s s i n

; 0 ; 0

B A

B A

A B A B A B

x x d

y y d

θ θ

θ θ θ κ κ κ κ

= +

= +

= = = = ɺ = ɺ =

allora la curva generata è una retta per ogni valore di ηηηη scelto

B A

d = p − p

ESEMPIO

Generazione di un percorso G³ misto:

• Cambio di corsia

• Tratto rettilineo

• Cubic spiral

• Raccordo generico

• Arco di circonferenza

2 5

1

0 4

[m]

[m]

3 2

5

1.5 1 4.5

0.5 0 4 5.5

3 2.5 3.5

6 7 2

5

1

4

3

SVILUPPI FUTURI

• Scelta dei parametri ηηηη che soddisfino appropriati criteri di ottimalità

• Obstacle avoidance

• Sintesi delle G^k-splines

• ηηηη-splines in 3D?