D´etermination des param`etres initiaux de la trajectoire 102

u

∗

(et la trajectoire d’´etat ˜s

∗

associ´ee) garantissant que toutes les

con-traintes consid´er´ees soient respect´ees. Le processus de saturation est

d´ecrit en Section 4.5.3.

– Etape 2 : Mise `a jours des param`etres pet t

f

afin de converger

vers l’´etat but

Une fois qu’une trajectoire faisable ˜u

∗

est obtenue, il est possible de

v´erifier si son ´etat finals

f

(d´efini au tempst

f

) correspond `a l’´etat but

s

g

. La fonction de coˆutJd´efinie lors du probl`eme d’optimisation (4.15)

est con¸cue pour ˆetre minimale lorsque cette condition est satisfaite. Si

l’´etat but n’a pas encore ´et´e atteint, les variations de J par rapport

aux param`etres (p, t

f

) sont ´etudi´ees afin de la faire d´ecroˆıtre. Une

cor-rection cor

(p,tf)

est alors calcul´ee et appliqu´ee sur ces param`etres (cf.

Section 4.5.4).

La convergence vers le but est atteinte lorsque la valeur de la fonction

de coˆut J est suffisamment faible. Si ce crit`ere n’est pas assur´e apr`es

un nombre fix´e d’it´erations de l’algorithme (dans le cas o`u la m´ethode

serait tomb´ee dans un minimum local), le processus d’optimisation est

stopp´e.

4.5.2 D´etermination des param`etres initiaux de la

tra-jectoire

Au cours de la premi`ere ´etape de l’algorithme 5, un ensemble de

param`etres permettant de calculer la trajectoire initiale doit ˆetre fourni. Le

succ`es de la m´ethode d’optimisation (dˆu aux minima locaux) et le nombre

d’´etapes requis pour converger vers le but d´ependent fortement du choix de

ces param`etres initiaux, il ne doit donc pas ˆetre n´eglig´e.

Une solution simple consiste `a d´eterminer ces param`etres analytiquement

`a partir d’une intuition sur le contrˆole `a appliquer : Par exemple, en calculant

la distance approximative `a parcourir par le robot entre les ´etats initiaux et

finaux, il est possible d’´evaluer la vitesse et le temps requis pour rejoindre

l’´etat but et d’en d´eduire le contrˆole `a appliquer. Cette m´ethode a

l’avan-tage d’ˆetre rapide `a calculer et de n´ecessiter peu de m´emoire, n´eanmoins il

peut en r`egle g´en´erale s’av´erer tr`es difficile de trouver une bonne heuristique

permettant de d´eterminer ces param`etres.

CHAPITRE 4. G ´EN ´ERATION DE TRAJECTOIRES AVEC

CONTRAINTE SUR LE TEMPS FINAL 103

initiaux contenant un ´echantillonnage de l’espace d’´etats-temps ainsi que

les param`etres requis pour atteindre chaque ´etat-temps de cette base. On

d´etermine alors les ´etats-temps de cette base (et les param`etres associ´es)

correspondant au mieux aux ´etats-temps initial et final entre lesquels une

trajectoire est g´en´er´ee. Une telle m´ethode permet en g´en´eral de r´eduire

consid´erablement le nombre d’´etapes de convergence n´ecessaires, n´eanmoins

sa taille en m´emoire peut ˆetre consid´erable si la dimension de l’espace d’´etat

du syst`eme est importante.

Cette seconde solution a ´et´e utilis´ee. La Fig. 4.8 illustre la m´ethode de

construction d’une base de param`etres initiaux pour notre g´en´erateur de

tra-jectoires Tiji : Soit l’´etat-temps initial (s

0

, t

0

) = {s

0¹

, . . . , s

0ⁿ

, t

0

} et

l’´etat-temps final (s

g

, t

g

) = {s

g1

, . . . , s

gⁿ

, t

g

}. Nous ´echantillonnons alors chaque

caract´eristiques

i

, i∈[1;n] de ces deux ´etats afin d’obtenir un ´echantillonage

r´egulier de l’espace des param`etres d’entr´ee de l’algorithme de g´en´eration de

trajectoire 5. Afin d’essayer de d´eterminer une trajectoire (et les param`etres

permettant de la g´en´erer) pour chacune de ces valeurs d’entr´ees, nous

util-isons un premier ensemble de trajectoire dont nous connaissons d´ej`a les

param`etres associ´es. Celles-ci peuvent par exemple ˆetre calcul´ees `a partir

d’un ´echantillonnage sur l’espace de contrˆole. Les noeuds (´etats-temps) de la

base les plus proches des ´etats terminaux des trajectoires d´ej`a calcul´ees sont

d´etermin´es (Fig. 4.8(a)). Tiji est alors utilis´e pour calculer les param`etres

requis pour atteindre ces noeuds en partant des param`etres des trajectoires

les plus proches (Fig. 4.8(b)). Nous essayons alors d’atteindre de proche

en proche les noeuds de l’´echantillonnage sur l’espace d’´etat-temps de cette

mani`ere (Fig. 4.8(c)). Le processus s’arrˆete lorsqueTijine trouve plus aucun

noeud atteignable sur l’´echantillonnage de l’espace d’´etats-temps.

4.5.3 Calcul d’une param´etrisation admissible de la

trajectoire

A partir des param`etres d’entr´ee (p, t

f

) les diff´erents profils de la

trajec-toire de contrˆole ˜u peuvent ˆetre calcul´es (cf. (4.12)). Etant donn´e un

´etat-temps initials

0

et l’expression analytique de ˜u ainsi obtenue, chaque ´etats(t)

de la trajectoire d’´etatss suivie par le syst`eme robotique est alors d´etermin´e

par l’int´egration de la dynamique du syst`eme :

s(t) = ˜s(p, t) =s

0

+

Z

t^{f(p, t)dt,∀t ∈[0;t}

^f

^{] (4.16)}

Mˆeme si l’ensemble de param`etres initiaux fournit une trajectoire initiale

faisable, les corrections cons´ecutives appliqu´ees sur ces param`etres peuvent

104

4.5. TIJI : G ´EN ´ERATEUR DE TRAJECTOIRE AVEC CONTRAINTE

SUR LE TEMPS FINAL

(a) Initialisation (vuex×y) (b) Premi`ere it´eration (vuex×y)

(c) Seconde it´eration (vuex×y)

Figure 4.8 – Premi`eres ´etapes de la construction d’une base de param`etres

permettant de d´eterminer les param`etres initiaux `a fournir en entr´ee deTiji

afin de g´en´erer une trajectoire entre deux ´etats-temps : Lors de

l’initiali-sation, un ensemble de trajectoires initiales (bas´e sur un ´echantillonage du

contrˆole d’entr´ee) est fourni. On essaie alors d’atteindre l’ensemble des noeuds

de l’´echantillonnage sur l’espace d’´etats-temps proches des ´etats-temps

ter-minaux des trajectoires pr´ec´edemment calcul´ees. La base de param`etres est

alors construite de proche en proche au cours des it´erations suivantes `a partir

des noeuds de l’´echantillonnage sur l’espace d’´etats-temps d´ej`a atteints.

CHAPITRE 4. G ´EN ´ERATION DE TRAJECTOIRES AVEC

CONTRAINTE SUR LE TEMPS FINAL 105

la rendre infaisable (les contraintes sur le mouvement ou sur le temps final

peuvent ˆetre viol´ees). Afin d’´eviter ce probl`eme, nous proposons une

solu-tion permettant d’obtenir une trajectoire faisable quelque soit l’ensemble

de param`etres d’entr´ee : Pour ce faire, nous proc´edons `a la saturation des

diff´erents profils de contrˆole et d’´etat ne satisfaisant pas les contraintes du

syst`eme. Les diff´erentes ´etapes de ce processus de saturation sont d´ecrites

ci-dessous.

Application de la contrainte sur le temps final

La contrainte sur le temps final est consid´er´ee en premier lieu. Elle est

tr`es simplement prise en compte en empˆechant le param`etre d’entr´eet

f

cor-respondant `a la dur´ee de la trajectoire de prendre une valeur en dehors de

l’intervalle de temps [t

min

;t

max

]. Un temps final appropri´e est donc obtenu

comme suit :

t

∗ f

=







t

min

sih

⁻t

(t

f

) = −t

f

+t

min

>0

t

max

sih

⁺t

(t

f

) = t

f

−t

max

>0

t

f

sinon

(4.17)

Cette nouvelle valeur du temps final t

∗

f

devient alors, durant la suite de

cette ´etape d’optimisation, le temps auquel sera ´evalu´e l’´etat final s

f

de la

trajectoire d’´etats. Il reste donc `a prendre en compte les contraintes sur

le mouvement du syst`eme robotique. La section suivante d´ecrit alors leur

int´egration.

Saturation des profils de contrˆole et d’´etat non admissibles

Etant donn´e les param`etres d’entr´ee (p, t

∗

f

) les profils des trajectoires de

contrˆoles et d’´etats peuvent ˆetre calcul´es `a partir des Eqs. (4.12) et (4.13).

Ces fonctions peuvent cependant d´epasser les contraintes sur le mouvement

i.e.lesbornes sur le contrˆole h

u

et les bornes sur l’´etat du syst`emeh

s

sur un

ou plusieurs intervalles de temps. Nous notons alors I

U

(resp. I

S

) l’ensemble

des intervalles des temps sur lesquels les bornes sur le contrˆole (resp. bornes

sur l’´etat) n’ont pas ´et´e respect´ees. A partir de la formalisation des bornes

sur le contrˆole et sur l’´etat donn´ees en Section 4.3.1, nous pouvons exprimer

ces intervalles comme suit :

n

I

U

=ⁿI

U 1

, . . . ,I

U k

o

|∀t∈I

U i

, i∈[1;k],

∃u

^j

(t), j ∈[1;m], u

^j

(t)< u

^j_min

or u

^j

(t)> u

^j_max

(4.18)

n

I

S

=ⁿI

S 1

, . . . ,I

S l

o

|∀t ∈I

S i

, i∈[1;l],

∃s

^j

(t), j ∈[1;n], s

^j

(t)< s

^j_min

or s

^j

(t)> s

^j_max

(4.19)

106

4.5. TIJI : G ´EN ´ERATEUR DE TRAJECTOIRE AVEC CONTRAINTE

SUR LE TEMPS FINAL

L’id´ee g´en´erale du processus de saturation consiste alors `a d´eterminer ces

intervalles ainsi que les nouveaux profils de contrˆole ou d’´etat devant y ˆetre

d´efinis. Les bornes sur le contrˆole et sur l’´etat sont appliqu´ees successivement,

n´eanmoins les trajectoires de contrˆoles et d’´etats ´etant li´ees, un processus de

saturation en quatre ´etapes a ´et´e ´etabli :

– Etape 1 : Application des bornes du contrˆole sur la trajectoire de

contrˆoles

A partir de la repr´esentation param´etrique du contrˆole donn´ee par

l’Eq. (4.12), une simple ´etude des variations de ces fonctions suffit `a

d´eterminer l’ensemble des intervalles I

U

sur lesquels les bornes sur le

contrˆole sont d´epass´ees.

Cet ensemble d’intervalles repr´esente simplement les intervalles de

temps au cours desquels un ou plusieurs contrˆoles d’entr´ee ont exc´ed´e

leurs bornes minimales ou maximales. Une trajectoire de contrˆole ˜u

_{u}

respectant ces limites est donn´ee par :

˜

u

{u}

(p, t) =

u

extl

(t) si t ∈I

U

˜

u(p, t) sinon ^(4.20)

o`u u

extl

(t) repr´esente le contrˆole extrˆeme (minimal ou maximal)

appli-cable au temps t respectant les bornes sur le contrˆole. Des fonctions

param´etriques par morceaux sont donc n´ecessaires pour repr´esenter ce

nouveau profil du contrˆole ainsi que ceux des fonctions d’´etats propos´ees

ci-dessous.

– Etape 2 : Calcul de la trajectoire d’´etats `a partir de la trajectoire de

contrˆoles born´ee

Une trajectoire d’´etats ˜s

{u}

prenant en compte lesbornes sur le contrˆole

peut ˆetre d´eduite en int´egrant ˜u

_{u}

`a partir de l’´etat initial s

₀

du

syst`eme :

˜

s

_{u}

(p, t) = s

₀

+

Z

t^f(s

⁰

^,u˜

^{u}

^{(p, t), t)dt (4.21)}

– Etape 3 : Application des bornes de l’´etat sur la trajectoire d’´etats

A partir de cet instant, les seules contraintes n’ayant pas ´et´e prises en

compte sont lesbornes sur l’´etat du syst`eme. Comme pour les bornes sur

le contrˆole, l’ensemble des intervallesI

S

d´epassant les bornes sur l’´etat

est d´etermin´e en ´etudiant les variations de l’Eq. (4.13). La trajectoire

d’´etat ˜s

{u}

doit alors ˆetre satur´ee sur ces intervalles afin de prendre en

compte l’ensemble des contraintes souhait´ees. La trajectoire d’´etats ˜s

∗

r´esultante est donc exprim´ee par :

˜

s

∗

(p, t) =

s

extl

(t) si t ∈I

S

˜

s

_{u}

(p, t) sinon ^(4.22)

CHAPITRE 4. G ´EN ´ERATION DE TRAJECTOIRES AVEC

CONTRAINTE SUR LE TEMPS FINAL 107

o`u s

extl

(t) est la valeur extrˆeme (minimale ou maximale) de l’´etat au

temps t respectant les bornes d’´etat. La trajectoire d’´etats ˜s

∗

garantie

donc de respecter l’ensemble des toutes les contraintes sur le

mouve-ment du syst`emeA et par cons´equence d’ˆetre faisable.

– Etape 4 : D´erivation d’une trajectoire de contrˆoles faisable

Enfin, une trajectoire de contrˆoles faisable ˜u

∗

est d´eriv´ee de la

trajec-toire d’´etats ˜s

∗

:

˜

u

∗

(p, t) =







u

extl

(t) sit ∈I

U

−I

S

0 sit ∈I

S

˜

u(p, t) sinon

(4.23)

De la mˆeme mani`ere que pour ˜s

∗

, le mouvement d´ecrit par la trajectoire

de contrˆole ˜u

∗

appliqu´ee `a partir de l’´etat initials

₀

, a la garantie d’ˆetre

faisable pour le syst`eme A.

Exemple

La Fig. 4.9 illustre le processus de saturation des trajectoires d’´etats et

de contrˆoles pr´esent´e au cours de la section pr´ec´edente. Supposez un syst`eme

1D de type double int´egrateur disposant de bornes sur sa vitesse et son

acc´el´eration. Soit le profil param´etrique initial de l’acc´el´eration du syst`eme

donn´e en Fig. 4.9(a). Les bornes d’acc´el´eration ´etant d´epass´ees sur les

inter-vallesI

A

1

et I

A

2

, nous saturons l’acc´el´eration sur ces intervalles afin d’obtenir

un nouveau profil respectant ces contraintes (Fig. 4.9(c)). En int´egrant ce

dernier `a partir de l’´etat initial s

₀

du syst`eme, nous obtenons alors un

pro-fil de vitesse du syst`eme satisfaisant les bornes sur l’acc´el´eration, mais pas

celles sur la vitesse (Fig. 4.9(d)). Nous saturons alors celle-ci sur

l’inter-valle I

V

1

o`u les bornes de vitesse sont d´epass´ees. Le nouveau profil obtenu

(Fig. 4.9(f)) satisfait donc l’ensemble des contraintes sur le mouvement. Afin

d’obtenir le contrˆole d’entr´ee admissible correspondant `a ce profil de vitesse,

nous red´erivons celui-ci. La Fig. 4.9(e) pr´esente alors le profil d’acc´el´eration

admissible pouvant ˆetre pass´e en entr´ee du syst`eme robotique.

4.5.4 Mise `a jour de la param´etrisation de la

trajec-toire

Une fois qu’une trajectoire faisable a ´et´e d´etermin´ee, son ´etat finals

f

est

facilement obtenu comme suit :

108

4.5. TIJI : G ´EN ´ERATEUR DE TRAJECTOIRE AVEC CONTRAINTE

SUR LE TEMPS FINAL

(a) Etape 1 : Profil d’acc´el´eration

lin´eairea(t) avant saturation.

(b) Etape 1 : Profil de vitesse lin´eaire

v(t) avant saturation.

(c) Etape 2 : Profil d’acc´el´eration

lin´eaire a(t) apr`es saturation des

intervalles sur lesquelles les bornes

d’acc´el´eration minimales et maximales

sont d´epass´ees.

(d) Etape 2 : Profil de vitesse lin´eaire

v(t) prenant en compte les bornes

d’acc´el´eration. Il est obtenu par

int´egration du profil d’acc´el´eration

donn´e en Fig. 4.9(c).

(e) Etape 3 : Profil d’acc´el´eration

lin´eairea(t) prenant en compte toutes les

contraintes sur le mouvement. Obtenu

par int´egration du profil de vitesse donn´e

en Fig. 4.9(f).

(f) Etape 3 : Profil de vitesse lin´eaire

v(t) apr`es saturation des intervalles

sur lesquels les bornes de vitesse sont

d´epass´ees.

Figure4.9 – Diff´erentes ´etapes de calcul des profils de vitesse et acc´el´eration

lin´eaire. Les croix repr´esentent les bornes minimales et maximales sur ces

diff´erents profils.

CHAPITRE 4. G ´EN ´ERATION DE TRAJECTOIRES AVEC

CONTRAINTE SUR LE TEMPS FINAL 109

Si s

f

est suffisamment proche de l’´etat but s

g

, i.e. si J(s

0

, p, t

f

, s

g

) ≤ ε,

o`uε est un seuil fix´e, alors l’approche variationnelle a converg´e comme nous

le souhaitions et une solution a ´et´e trouv´ee. Dans le cas contraire, il est

n´ecessaire d’appliquer une correction sur les param`etres d’entr´ee (p, t

f∗

) afin

d’y parvenir.

Afin de proc´eder au calcul d’une correction sur ces param`etres

permet-tant de minimiser la fonction de coˆutJ(s

0

, p, t

f

, s

g

), de nombreuses m´ethodes

d’optimisation existent (voir [NW99, Ber96] pour un r´esum´e de ces

ap-proches). N´eanmoins, elle peuvent g´en´eralement ˆetre classifi´ees en quatre

cat´egories : les m´ethodes de relaxation (optimisation s´equentielle de chaque

param`etre), les m´ethodes de descente de gradient (recherche d’une direction

descendante donn´ee par le gradient de la fonction de coˆut), les m´ethodes

de Newton ou assimil´ees (approximation locale au second ordre de la

fonc-tion de coˆut) et enfin la m´ethode stochastique de recuit simul´e (en anglais :

simulated annealing [KGV83, GKR94]). Les principales diff´erences entre ces

diff´erentes m´ethodes tiennent au choix de la direction et du pas de descente,

et par cons´equence, `a leur coˆut de calcul. Une m´ethode de Newton standard

a ´et´e utilis´ee dans notre cas.

Un minimum local de la fonction de coˆut est trouv´e lorsque ses d´eriv´ees

partielles^h

∂

J

∂(

p

,

t

f)

i

par rapport aux param`etres (p, t

f

) sont ´egales `a z´ero. Une

racine de ^h

∂

J

∂(

p

,

t

f)

i

peut alors ˆetre d´etermin´ee en lin´earisant son expression

comme suit :

∂J

∂(p, t

f

)

≃

∂

2

J

∂(p, t

f

)

2

∆(p, t

f

) (4.25)

o`u ∆(p, t

f

) est l’erreur suppos´ee commise sur les param`etres , et o`u^h

∂2

J

∂(

p

,

t

f)2

i

sont les d´eriv´ees partielles de second ordre de la fonction de coˆut par rapport

`a ces param`etres (matrice de dimensions (dim P+ 1)×(dimP+ 1)). Afin de

minimiser la fonction de coˆutJ, une correction `a appliquer sur les param`etres

(p, t

f

) est alors calcul´ee comme suit :

cor

₍p,tf)

=−τ

∂

2

J

∂(p, t

f

)

2

−1

∂J

∂(p, t

f

)

(4.26)

o`u τ ∈ [0; 1] est le gain de la correction. La matrice inverse des d´eriv´ees

partielles de second ordre repr´esente alors la direction de la correction, et

τ^h

∂

J

∂(

p

,

t

f)

i

le pas de descente.

La Fig. 4.10 illustre la convergence de la m´ethode apr`es application de

plusieurs corrections sur l’ensemble des param`etres d´efinissant le contrˆole

d’un v´ehicule.

Dans le document Navigation autonome en environnement dynamique : une approche par déformation de trajectoire (Page 103-111)