Algoritmo di ottimizzazione

Pianificazione di velocità ottima per veicoli autonomi

Sommario:

Pianificazione di velocità

Algoritmo di ottimizzazione

Esistenza della soluzione

Esempi

Conclusioni e sviluppi futuri

Corsi brevi di Dottorato Parma

Gennaio 2005

C. Guarino Lo Bianco

1

2

CENNI STORICI

V. Muñoz, A. Ollero, M. Prado, and A. Simón, “Mobile robot trajectory planning with dynamic and kinematic constraints,” in Proc. of the 1994 IEEE Int. Conf. on Robotics and Automation, vol. 4, San Diego, CA, May 1994, pp. 2802–2807

• Combinazione di spline cubiche

• Vincoli di pianificazione sia cinematici che dinamici

• Continuità delle accelerazioni metodi numerici di soluzione

• Soluzione a tempo minimo

• Problema poco studiato:

2 3

0 1 2 3

( ) , [0; ]

i i i i i i

s t = a +a t a t+ +a t t∈ t

ti

amax

max, max

v a

• Caratteristiche generali

smoothness del controllo minori slittamenti delle ruote

maggiore precisione nell’inseguimento del percorso

3

LA PIANIFICAZIONE DI VELOCITÀ ( ) 1

v t ∈C

è una caratteristica sufficiente per assicurare la smoothness del controllo?

in generale NO ( ) 1

v t ∈C

4

VALUTAZIONE DELLE ACCELERAZIONI

v

ˆ _A x ˆ _A

y

θ ( , )x y

cos sin

x v

y

θ θ

   

=   =  

   

v ɺ ɺ

cos sin

sin cos

x v v

y v v

θ θ θ θ θ θ

 − 

=     =   +

   

a ɺɺ ɺ ɺ

ɺɺ ɺ ɺ

v

κ = ω

a

Slittamento delle ruote e comfort di viaggio sono

strettamente correlati al modulo della accelerazione planare

• Velocità

• Accelerazione

2 2 4

v κ v

= +

a ɺ

2 2 2 2 2 2

v θ v v ω v

= + = +

a ɺ ɺ ɺ

• Pianificazione percorso contenimento del curve a curvatura limitata

le η-splines minimizzazione curvatura

• Pianificazione di velocità contenimento di

• Altre ragioni per il contenimento di

• limiti degli attuatori velocità limitate coppie limitate

5

VINCOLI ALLA PIANIFICAZIONE

κ

, v v ɺ

, v v ɺ

= m

F a T = J ω ɺ

• Percorso assegnato assegnata

• Obstacle avoidance/intercettazione oggetti assegnato

• Possibilità creazione di un profilo complessivo assegnabilità condizioni di interpolazione

• Positività della velocità

• Esecuzione dell’algoritmo in real time

6

ALTRI VINCOLI ALLA PIANIFICAZIONE sf

t

( ) 1

v t ∈C

( ) 0, (0, _f ) v t > ∀ ∈t t

• Inversione dinamica azione in avanti

riduzione tempi latenza algoritmi efficienti

7

IL PROBLEMA DEL REAL-TIME

t

Percorso teorico Percorso reale

ripianificazione attuazione

istanti di campionamento

latenza ripianificazione

PIANIFICAZIONE DI VELOCITA’

• Caratteristiche profilo

• Continuo assieme alla sua derivata

• Tempo di percorrenza

• Lunghezza di percorso

• Condizioni di interpolazione

• Velocità limitata

• Accelerazione limitata

8

, , ,

s f s f

v v a a

s

t

(0,

] v ∈ v

t

v

f

,

v a

s

,

v a s

Tutte le condizioni devono essere simultaneamente

soddisfatte e, possibilmente, il jerk deve essere minimizzato

max max

[ , ]

a ∈ − a a

UNA POSSIBILE SOLUZIONE

• Il profilo

• Cinque spline polinomiali propriamente raccordate

• Valutazione coefficienti

• Garanzia di continuità della velocità

• Garanzia di continuità dell’accelerazione

• Condizioni al contorno soddisfatte

9

t

,

v a

Espressioni in forma chiusa sono state proposte per la valutazione efficiente dei coefficienti

s

,

v a

t

t

t

t

t

2

1 2 3

( ) 2 3 , [0; ]

i i i i i

v t = +a a t + a t t∈ t

10

UNA POSSIBILE SOLUZIONE

C. Guarino Lo Bianco, M. Romano, “Bounded velocity planning for autonomous vehicles. IEEE/RSJ Int. Conf. on Intelligent Robots and Systems 2005 , IROS’05, Edmonton, Canada, August 2-6, 2005

• Vincolo sulle accelerazioni non ancora considerato

• Soluzione non specializzata per un particolare veicolo vincoli dovuti alla curvatura non considerati

• Gradi di libertà disponibili

Tempo di percorso

11

t

,

v a

s

,

v a

t

t

t

t

t

1 2 3 4 5 2 3 2

: [ , , , , , , ,= t t t t t v v a ]^T h

v2

v3

a2

3

1,2,4,5

f i

i

t t t

=

= −

∑

1 2 4 5 2 3 2

: [ , , , , , ,= t t t t v v a ]^T h

UNA POSSIBILE SOLUZIONE

I rimanenti gradi di libertà sono usati per soddisfare gli ultimi

vincoli e ottimizzare il profilo

IL PROBLEMA DI OTTIMIZZAZIONE

12

Obiettivo dell’ottimizzazione:

Minimizzare il massimo jerk longitudinale

in modo che

[0, ]

min max ( )

H t tf j t

∈ ∈

h

0 max

( )

0 ( ) , (0, )

te

f

f

s v d

v t v t t

τ τ

=

< ≤ ∀ ∈

∫

Il problema è convertibile in una ottimizzazione standard La soluzione può essere valutata efficientemente?

RISULTATO DI FATTIBILITA’

13

Proprietà:

E’ sempre possibile risolvere il problema di ottimizzazione se

Dimostrazione (cenno):

Necessità: La soluzione è feasible ma assurdo

Sufficienza: Basta trovare una prima soluzione che rispetti i vincoli.

• È possibile valutarla in modo efficiente?

0 ^f max f

s v

< t <

max

0

1 ( )

tf

f

f f

v s v d

t t

τ τ

≤ =

∫

VALUTAZIONE DELLA SOLUZIONE INIZIALE

14

Step 1 (limitatezza velocità):

Si ponga e e

con

1 2 4 5 : min{ , } t = = = =t t t t t_{α β}

2 3

v = v a₂ = 0

t₁

v t₂( )

v t₁( )

v t₄( )

v t₅( ) ( , )v a_{2 2}

( , )v a_{0 0}

t₂ t₃ t₄ t₅

t_f t ( , )v a_{3 3}

(v₄( )h ,a₄( )h )

( , )v a_{5 5} (v₁( )h ,a₁( )h )

v t( )

max

min , 2 se 0 5

2( )

: min , se 0

5

altrimenti 5

f s

s s

f s

s s

f

t v

a a

t v v

t a

a t

α

  

− <

 

  

  − 

=   −  >

 





max

min ,2 se 0 5

2( )

: min , se 0

5

altrimenti 5

f f

f f

f f

f f

f

t v

a a

t v v

t a

a t

β

  

>

  

 

  

  − 

  

=   <

 

  





t₁

v t₂( )

v t₁( )

v t₄( )

v t₅( ) ( , )v a_{2 2}

( , )v a_{0 0}

t₂ t₃ t₄ t₅

t_f t ( , )v a_{3 3}

(v₄( )h ,a₄( )h )

( , )v a_{5 5} (v₁( )h ,a₁( )h )

v t( )

Step 1 (cenno di dimostrazione):

• Condizioni imposte tratto intermedio retta altri tratti parabole

• Vertice seconda parabola fine secondo intervallo seconda parabola monotona massimo nel primo intervallo il massimo non dipende da .v₂

VALUTAZIONE DELLA SOLUZIONE INIZIALE

15

16

Step 2 (lunghezza del percorso):

Si imponga v₂ = 0

t₁

v t₂( ) v t₁( )

v t₄( ) v t₅( )

v v₂= =0₃ ( , )v a_{0 0}

t₂ t₃ t₄ t₅

t_e t ( , )v a5 5

v_max v t( )

t₁

v t₂( )

v t₁( )

v t₄( )

v t₅( ) v v₂= =v_{3 max}

( , )v a_{0 0}

t₂ t₃ t₄ t₅

t_e t ( , )v a_{5 5} v t( )

2 max

v = v

Si riduca finché

t

^S < ^s

t

^S > ^s

Entrambe disuguaglianze soddisfatte se

0

f

s v

< t <

Si imponga

VALUTAZIONE DELLA SOLUZIONE INIZIALE

17

Step 2 (lunghezza del percorso):

Ricerca per bisezione su v₂ si ottiene

t₁

v t₂( )

v t₁( )

v t₄( )

v t₅( ) v₂

( , )v a_{0 0}

t₂ t₃ t₄ t₅

t_f t ( , )v a_{5 5}

S = sf

Il vincolo sull’area soddisfatto se è rispettata

0

f

s v

< t <

VALUTAZIONE DELLA SOLUZIONE INIZIALE

ALGORITMO DI OTTIMIZZAZIONE

18

Caratteristiche:

• Veloce (real time)

• Soluzione ammissibile sempre disponibile

Algoritmi standard di ottimizzazione non sufficientemente efficienti

La soluzione ottima è ricavata mediante un algoritmo basato su

proprietà fisiche

t

j

t

t

t

t

t

t

j

t

t

t

t

t

ALGORITMO DI OTTIMIZZAZIONE

19

Procedura di ottimizzazione (cenno)

• Allarga l’intervallo temporale relativo al massimo jerk

• Modifica per soddisfare

• Verifica “feasibility” soluzione

• Itera fintanto che si verificano miglioramenti

una soluzione feasible è sempre disponibile

0< v t( ) ≤ vmax, ∀ ∈t (0, )t_f

0

( )

tf

s

= ∫ v τ τ d

1 2 4 5

( , , , ) t t t t

2

, ,

v v a

Tempo di percorso Lunghezza di percorso

Condizioni al contorno Massima velocità

ESEMPIO

0.2

0 1.0

0.4 0.6 0.7

vt(;) (m/s)h

0.8 0.9

0.3 0.5

0.1

2 4 6 8 10

0 12 14 16 18

Time (s)

v_max= 0.7

bounded solution

unbounded solution

20

Soluzione iniziale

J_max = 0.6286 m/s³

0.55; 0.2; 0.2; 0.12

s f s f

v s

= = 10 v = a = a =

max

0.7

v =

f

18 t =

Soluzione finale

J_max = 0.1363 m/s³

0.2

0 1.0

0.4 0.6 0.7

vt(;) (m/s)h

0.8 0.9

0.3 0.5

0.1

2 4 6 8 10

0 12 14 16 18

Time (s)

v_max= 0.7

bounded solution

unbounded solution

I VINCOLI DI ACCELERAZIONE

• Nuova versione algoritmo vincoli di accelerazione

• Nuove condizioni per l’esistenza della soluzione

• Miglioramenti all’algoritmo di ottimizzazione

• Riduzione dei tempi di calcolo

• Ricerca maggiormente estensiva della soluzione

21

max max

[ , ]

a ∈ − a a

Tempo di percorso Lunghezza di percorso

Condizioni al contorno Massima velocità

Accelerazione massima

ESEMPIO

f

10 s =

max

0.7

v =

f

18 t =

0.05; 0.35; 0.2; 0.2

s f s f

v = v = a = − a = −

max

0.2

a =

ESEMPIO

22

Soluzione iniziale

J_max = 0.7781 m/s³

Soluzione finale

J_max = 0.4037 m/s³

0.2

0 1.0

0.4 0.6 0.7

vt(;)(m/s)h

0.8 0.9

0.3 0.5

0.1

2 4 6 8 10

0 12 14 16 18

Time (s)

vmax= 0.7 bounded

acceleration

unbounded acceleration

-0.3

-0.5 0.5

-0.1 0.1 0.2

at(;)(m/s)h2

0.3 0.4

-0.2 0

-0.4

2 4 6 8 10

0 12 14 16 18

Time (s)

a_max= 0.2

bounded acceleration unbounded acceleration 0.2

0 1.0

0.4 0.6 0.7

vt(;)(m/s)h

0.8 0.9

0.3 0.5

0.1

2 4 6 8 10

0 12 14 16 18

Time (s)

v_max= 0.7 bounded

acceleration

unbounded acceleration

-0.6 -1.0 1.0

-0.2 0.2 0.4

at(;)(m/s)h2

0.6 0.8

-0.4 0

-0.8

2 4 6 8 10

0 12 14 16 18

Time (s)

amax= 0.2

bounded acceleration unbounded acceleration

I VINCOLI CINEMATICI E DINAMICI

• Vincoli su accelerazione e velocità costanti algoritmo general purpose

• Specializzazione algoritmo: unicicli, bicicli, omnidirezionali conversione vincoli cinematici e dinamici in vincoli su

il problema diventa di ottimizzazione semi-infinita non riconducibile ad una ottimizzazione standard

23

max, max

v a

IL PROBLEMA DI OTTIMIZZAZIONE

24

Obiettivo dell’ottimizzazione:

Minimizzare il massimo jerk longitudinale

in modo che

dove

[0, ]

min max ( )

H t tf j t

∈ ∈

h

max

max max

0

( )

0 [ ( )] [ ( )], (0, )

[ ( )] [ ( )] [ ( )], (0, )

( ) ( )

f f

f

f

t

s t s

v s t v s t t t

a s t a s t a s t t t

s t v τ τd

=

< ≤ ∀ ∈

− < ≤ ∀ ∈

=

∫

ɶ

ɶ ɶ

CONCLUSIONI

• Pianificazione di velocità tutt’altro che banale

• Algoritmi semplici soluzioni subottime

• Algoritmi complessi soluzioni ricavate iterativamente Garazie di convergenza

• Efficienza: nel peggiore dei casi il tempo di convergenza è stato di 0.02 s

SVILUPPI FUTURI

• Miglioramento del codice

• Studio allargato ad altri veicoli autonomi

• Aggiunta di vincoli addizionali

25

BIBLIOGRAFIA

• GUARINO LO BIANCO C., M. ROMANO. (2005). Bounded velocity planning for autonomous vehicles.

IEEE/RSJ Int. Conf. on Intelligent Robots and Systems 2005 , IROS’05, Edmonton, Canada, August 2-6.

• GUARINO LO BIANCO C., A. PIAZZI, M. ROMANO. (2004). Smooth Motion Generation for Unicycle Mobile Robots Via Dynamic Path Inversion. IEEE TRANSACTIONS ON ROBOTICS. vol. 20, pp. 884-891

• GUARINO LO BIANCO C., A. PIAZZI. (2004). Using semi-infinite optimization for the steering of car-like vehicles. In J. GUDDAT, H. TH. JONGEN, JAN-J. RUCKMANN, MAXIM TODOROV. Parametric

Optimization and Related Topics VII. (pp. 121-132). PUEBLA: Sociedad Matemática Mexicana (MEXICO).

• GUARINO LO BIANCO C., A. PIAZZI, M. ROMANO. (2004). Velocity planning for autonomous vehicles.

IEEE Intelligent Vehicles Symposium, IV2004. June 14-17. (pp. 413-418).

• GUARINO LO BIANCO C., A. PIAZZI, M. ROMANO. (2004). Smooth control of a wheeled omnidirectional robot. 5th IFAC Symposium on Intelligent Autonomous Vehicles, IAV2004. July 5-7.

• A. PIAZZI, M. ROMANO, GUARINO LO BIANCO C. (2003). G³-splines for the path planning of wheeled mobile robots. European Control Conference 2003, ECC2003. September 1-4.

• A. PIAZZI, GUARINO LO BIANCO C., M. BERTOZZI, A. FASCIOLI, ALBERTO BROGGI. (2002). Quintic G2-splines for the Iterative Steering of Vision-based Autonomous Vehicles. IEEE TRANSACTIONS ON INTELLIGENT TRANSPORTATION SYSTEMS. vol. 3, pp. 27-36.

• GUARINO LO BIANCO C., A. PIAZZI. (2002). Inversion-based control of wheeled mobile robots. IEEE Intelligent Vehicle Symposium 2002, IV2002. June 18-20.

• BROGGI, M. BERTOZZI, A. FASCIOLI, GUARINO LO BIANCO C., A. PIAZZI. (2000). Visual perception of obstacles and vehicle for platooning. IEEE TRANSACTIONS ON INTELLIGENT TRANSPORTATION SYSTEMS. vol. 1, pp. 164-176.

• A. PIAZZI, GUARINO LO BIANCO C. (2000). Quintic G²-splines for the Trajectory Planning of Autonomous Vehicles. IEEE Intelligent Vehicle Symposium 2000, IV2000. October 3-5. (pp. 198-203).

• GUARINO LO BIANCO C., A. PIAZZI. (2000). Optimal Trajectory Planning with Quintic G²-splines. IEEE Intelligent Vehicle Symposium 2000, IV2000. October 3-5. (pp. 620-625).

• BROGGI, M. BERTOZZI, A. FASCIOLI, GUARINO LO BIANCO C., A. PIAZZI. (1999). The ARGO Autonomous Vehicle's Vision and Control Systems. INTERNATIONAL JOURNAL OF INTELLIGENT CONTROL AND SYSTEMS. vol. 3, pp. 409-441

26

BIBLIOGRAFIA

• W. Nelson, “Continuous-curvature paths for autonomous vehicles,” in Proc. of the IEEE Conf. on Robotics and Automation, vol. 3, Scottsdale, AZ, May 1989, pp. 1260–1264.

• 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.

• K. Komoriya and K. Tanie, “Trajectory design and control of a wheeledtype mobile robot using b-spline curve,” in IEEE/RSJ Int. Work. on Intelligent Robots and Systems, IROS89, Tsukuba, JP, Sept. 1989, pp. 398–405.

• J. Reuter, “Mobile robots trajectories with continuously differentiable curvature: an optimal control approach,” in Proceedings of the 1998 IEEE/RSJ Int. Conf. on Intelligent Robots and Systems, vol. 1, Victoria, B.C., Canada, Oct.

1998, pp. 38–43.

• V. Muñoz, A. Ollero, M. Prado, and A. Simón, “Mobile robot trajectory planning with dynamic and kinematic

constraints,” in Proc. of the 1994 IEEE Int. Conf. on Robotics and Automation, vol. 4, San Diego, CA, May 1994, pp.

2802–2807.

• H. Delingette, M. H´ebert, 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.

• S. Fleury, P. Sou`eres, J.-P. Laumond, and R. Chatila, “Primitives for smoothing paths of mobile robots,” in

Proceedings of the IEEE Int. Conference on Robotics and Automation, Atlanta, GA, September 1993, pp. 832–839.

• A. Scheuer and T. Fraichard, “Planning continuous-curvature paths for car-like robots,” in Proc. of the 1996 IEEE/RSJ Int. Conf. on Intelligent Robots and Systems, Osaka, Japan, November 1996, pp. 1304–1311.

• ——, “Collision-free and continuous-curvature path planning for car like robots,” in Proceedings of the 1997 IEEE International Conference on Robotics and Automation, Albuquerque, New Mexico, April 1997, pp. 867–873.

• A. Scheuer and C. Laugier, “Planning sub-optimal and continuous curvature paths for car-like robots,” in Proceedings of the 1998 IEEE/RSJ International Conference on Intelligent Robots and Systems, Victoria, B.C., Canada, October 1998, pp. 25–31.

• A. Scheuer and M. Xie, “Continuous-curvature trajectory planning for manoeuvrable non-holonomic robots,” in Proc. of the IEEE/RSJ Int. Conf. on Intelligent Robots and Systems, vol. 3, Kyongju, Korea, Oct. 1999, pp. 1675–1680.

• T. Fraichard and J.-M. Ahuactzin, “Smooth path planning for cars,” in Proceedings of the 2001 IEEE International Conference on Robotics & Automation, Seoul, Korea, May 2001, pp. 3722–3727.

• T. Fraichard and A. Scheuer, “From Reeds and Shepp’s to continuous curvature paths,” IEEE Trans. on Robotics, vol.

20, no. 6, pp. 1025–1035, Dec. 2004.

27