Planification de mouvement et suivi de trajectoireprojects.laas.fr/gepetto/uploads/Members/2015-isae...Contexte robot industriel drone v ehicule autonome Syst emes mobiles autonomes

Planification de mouvement et suivi de trajectoire

Florent Lamiraux

CNRS-LAAS, Toulouse, France




Contexte

robot industriel drone vehicule autonome

Systemes mobiles autonomes

I se deplacant dans un environnement encombre d’obstacles,

I soumis a des contraintes cinematiques ou dynamiques.

Planification de mouvement : calculer automatiquement unmouvement sans collisions entre deux configurations du systeme.


Contexte







Contexte







Robot

Un robot est un ensemble de corps solidesB0, · · · Bm, relies entre eux par des articula-tions.

T3

R3

R1 R1 R1

R1 R1 R1

q0

...

...

q1q2

qiqi+1qi+2

B0

B1

B2

Articulation : deplacement rigide parametreentre deux corps et a valeur dans SE(3).


Deplacement rigide

Definitions

I SO(3) : groupe des matrices de rotation de dimension 3.

R ∈ SO(3)⇔ RTR = I3 et det(R) = 1

I SE(3) : groupe des deplacement rigides

T ∈ SE (3)⇔ ∃t ∈ R3, ∃R ∈ SO(3)

∀x ∈ R3 T (x) = Rx + t

On notera T = T(R,t).


Articulation

Une articulation est representee par une application d’unesous-variete de Rp dans SE(3), ou p ≥ 1 est un entier.Exemples :

I Translation T1 :

R → SE (3)

t → T(I3,(t 0 0))

translation selon x


Articulation


I Translation T3 :

R3 → SE (3)

t → T(I3,t)

translation


Articulation


I Rotation R1 :

R → SE (3)

t → T(R,0)

R =

cos t − sin t 0sin t cos t 0

0 0 1


Articulation


I Rotation R3 :

R4 → SE (3)

t → T(R,0)

‖t‖ = 1

R = 1− 2(t22 + t2

3 ) 2t2t1 − 2t3t0 2t3t1 + 2t2t02t2t1 + 2t3t0 1− 2(t2

1 + t23 ) 2t3t2 − 2t1t0

2t3t1 − 2t2t0 2t3t2 + 2t1t0 1− 2(t21 + t2

2 )

t0 + t1 i + t2j + t3k est un quaternion.


Quaternions

Corps non commutatif isomorphe a R4, engendrepar trois elements i , j , k satisfaisant les relationssuivantes :

i2 = j2 = k2 = ijk = −1

dont on deduit immediatement

ij = k, jk = i , ki = j Hamilton (1843)


Quaternions

Corps non commutatif isomorphe a R4, engendrepar trois elements i , j , k satisfaisant les relationssuivantes :

i2 = j2 = k2 = ijk = −1

dont on deduit immediatement

ij = k, jk = i , ki = j Hamilton (1843)


Quaternions unitaires et rotations

Soit q = q0 + q1i + q2j + q3k un quaternion unitaire :

q20 + q2

3 + q22 + q2

3 = 1

∀x = (x0, x1, x2) ∈ R3, soit u = x0i + x1j + x2k

q . u . q∗ = y0i + y1j + y2k

ou q∗ = q0 − q1i − q2j − q3k est le conjugue de q.y = (y0, y1, y2) est l’image de x par la rotation de matrice 1− 2(q2

2 + q23) 2q2q1 − 2q3q0 2q3q1 + 2q2q0

2q2q1 + 2q3q0 1− 2(q21 + q2

3) 2q3q2 − 2q1q0

2q3q1 − 2q2q0 2q3q2 + 2q1q0 1− 2(q21 + q2

2)




q20 + q2

3 + q22 + q2

3 = 1


q . u . q∗ = y0i + y1j + y2k


2 + q23) 2q2q1 − 2q3q0 2q3q1 + 2q2q0

2q2q1 + 2q3q0 1− 2(q21 + q2

3) 2q3q2 − 2q1q0

2q3q1 − 2q2q0 2q3q2 + 2q1q0 1− 2(q21 + q2

2)




q20 + q2

3 + q22 + q2

3 = 1


q . u . q∗ = y0i + y1j + y2k


2 + q23) 2q2q1 − 2q3q0 2q3q1 + 2q2q0

2q2q1 + 2q3q0 1− 2(q21 + q2

3) 2q3q2 − 2q1q0

2q3q1 − 2q2q0 2q3q2 + 2q1q0 1− 2(q21 + q2

2)



I A noter que q et −q representent la meme rotation.

I SO(3) est isomorphe a Sp(1)/{±1}, la demi-sphere de R4.


Configuration d’un robot

La configuration q d’un robot est represen-tee par la concatenation des parametres dechaque articulation.

T3

R3

R1 R1 R1

R1 R1 R1

q0

...

...

q1q2

qiqi+1qi+2

B0

B1

B2


Cinematique directe

Calcul de la position de chaque articulationdans le repere global.

Mi (q) = Mparent(i)(q) Mi/parent Ti (q)

R0

Rparent(i)Mparent(i)(q) Ri

Mi (q)

Mi/parent Ti (q)


Definitions

I Espace de travail : W = R2 or R3 : espace dans lequel lerobot se deplace.

I Obstacle dans l’espace de travail : partie compacte de W,note O.

I Espace des configurations : C.

I Position en configuration q d’un point M ∈ Bi : xi (M,q).

I Obstacle dans l’espace des configurations :

Cobst = {q ∈ C, ∃i ∈ {1, · · · ,m}, ∃M ∈ Bi , xi (M,q) ∈ O ou

∃i , j ∈ {1, · · · ,m}, ∃Mi ∈ Bi , ∃Mj ∈ Bj ,xi (Mi ,q) = xj(Mj ,q)}

I Espace des configurations libres : Cfree = C \ Cobst .


Definitions










Definitions










Definitions










Definitions










Definitions










Mouvement

I Espace des configurations :I variete differentielle

I Mouvement :I function continue de [0, 1] dans C.

I Mouvement sans collision :I fonction continue de [0, 1] dans Cfree .


Mouvement





Mouvement





Probleme de planification de mouvement

DefinitionI Etant donnes

I un robot,I des obstacles dans l’espace de travail,I une configuration initiale qinit ,I une configuration finale qgoal ,

I calculer un mouvement sans collision de qinit a qgoal .

Principe de resolution

I transformer le probleme continu en probleme combinatoire.





































Planification de mouvement

I Un cas simple : un point dans un plan.

qinit

qgoal



I Espace des configurations = espace de travail

qinit

qgoal



I Decomposition de Cfree en cellules convexes

qinit

qgoal




qinit

qgoal




qinit

qgoal




qinit

qgoal




qinit

qgoal




qinit

qgoal




qinit

qgoal




qinit

qgoal




qinit

qgoal



I Complexite : O(nlog(n))

qinit

qgoal



I Un peu plus difficile : un polygone en translation.

qinit

qgoal



I Construction de l’espace des configurations

qinit

qgoal



I Minkowski : Cobst = {o− r, o ∈ obstacle, r ∈ robot}

qinit

qgoal




qinit

qgoal




qinit

qgoal



I Complexite de la construction de Cobst :I nr nombre de sommets du robot,I no nombre de sommets de l’obstacle.I polygones convexes : O(nr + no),I polygones non-convexes : O(nrno log(nrno)),



I Complexite de la construction de Cobst :I nr nombre de sommets du robot,I no nombre de sommets de l’obstacle.I polygones convexes : O(nr + no),I polygones non-convexes : O(nrno log(nrno)),


Motion planning

I polygone en translation et rotation dans le plan :

I Espace des configurations delimite par des sufaces regleesdans R2 × S1,

I resolution par discretisation de l’orientation.


Motion planning





Motion planning





Inconvenients

I problemes plans,

I manque de robustesse des calculs de geometrie algorithmiqueen precision finie.


Resolution exacte du polygone en translation et rotation

I Decomposition cellulaire exacteI formulation semi-algebrique du problemeI decidabilite : theoreme de Tarski (1901-1983),I resolution : algorithme de Collins.

I PrincipeI Decomposer Cfree en cellules simplement connexes et construire

un graphe d’adjacence des cellules.


Resolution exacte du polygone en translation et rotation

I Decomposition cellulaire exacteI formulation semi-algebrique du problemeI decidabilite : theoreme de Tarski (1901-1983),I resolution : algorithme de Collins.

I PrincipeI Decomposer Cfree en cellules simplement connexes et construire

un graphe d’adjacence des cellules.


Ensemble semi-algebrique

I Expression polynomiale atomique.I Une expression polynomiale atomique sur Rn est une

expression de la forme :

P(x) ./ 0

ou P ∈ Q[X1, ...,Xn], x ∈ Rn et ./ est un comparateur parmi{=, 6=, >,<,≥,≤}.

I Expression polynomialeI Une expression polynomiale sur Rn est une combinaison

booleenne finie d’expressions polynomiales atomiques.

I Ensemble semi-algebriqueI Un ensemble semi-algebrique est un domaine de Rn defini par

une expression polynomiale.





P(x) ./ 0










P(x) ./ 0








I Expression de TarskiI Une expression de Tarski est une expression polynomiale

prefixee par un nombre fini de quantificateurs ∃ ou ∀.I Une expression polynomiale est donc une expression de Tarski

sans quantificateurs.



I Expression de TarskiI Une expression de Tarski est une expression polynomiale

prefixee par un nombre fini de quantificateurs ∃ ou ∀.I Une expression polynomiale est donc une expression de Tarski

sans quantificateurs.



I Exemple : l’aire delimitee par un polygone convexe dans leplan est un sous-ensemble semi-algebrique de R2 :∧i∈{1,...,p}(aix + biy + ci > 0)

∆ 2

∆ 3

∆ 4

∆ 5

∆ 1

I Une aire polygonale non-convexe peut etre decomposee enaires polygonales convexes et est donc un ensemblesemi-algebrique.



I Exemple : l’aire delimitee par un polygone convexe dans leplan est un sous-ensemble semi-algebrique de R2 :∧i∈{1,...,p}(aix + biy + ci > 0)

∆ 2

∆ 3

∆ 4

∆ 5

∆ 1

I Une aire polygonale non-convexe peut etre decomposee enaires polygonales convexes et est donc un ensemblesemi-algebrique.


Robot et environnement semi-algebriques

x

y

a = (x ,y )aa

Γ(a,q)

Reference configuration

θ

I Espace des configurations : R2 × S1.

I Une configuration peut etre representee par q = (x , y , θ).Un point a = (xa, ya) du robot se trouve en

x(q, a) ,

(cos θ − sin θsin θ cos θ

)(xaya

)+

(xy

)dans la configuration q.



x

y

a = (x ,y )aa

Γ(a,q)

Reference configuration

θ

I Espace des configurations : R2 × S1.

I Une configuration peut etre representee par q = (x , y , θ).Un point a = (xa, ya) du robot se trouve en

x(q, a) ,

(cos θ − sin θsin θ cos θ

)(xaya

)+

(xy

)dans la configuration q.



x

y

Γ(a,q)

θ

I C-obstacle :I Soit Λrobot(a), a ∈ R2 l’expression polynomiale definissant le

robot.I Soit Λobst(b), b ∈ R2 l’expression polynomiale definissant les

obstacles.

q ∈ Cobst ⇔ ∃a ∈ R2, ∃b ∈ R2,Λrobot(a)∧Λobst(b)∧x(q, a) = b

I mais x(q, a) n’est pas une expression polynomiale.



x

y

Γ(a,q)

θ



obstacles.





x

y

Γ(a,q)

θ



obstacles.





x

y

Γ(a,q)

θ



obstacles.





x

y

Γ(a,q)

θ

I Parametrage de S1 : on pose t = tan θ2 , on obtient

I

x(q, a) =

(1−t2

1+t2 − 2t1+t2

2t1+t2

1−t2

1+t2

)(xaya

)+

(xy

)avec q = (x , y , t) et C-obstacle est defini par une expression deTarski :

q ∈ Cobst ⇔ ∃a ∈ R2,∃b ∈ R2,

Λrobot(a) ∧ Λobst(b) ∧ (1 + t2)x(q, a) = (1 + t2)b



x

y

Γ(a,q)

θ


I

x(q, a) =

(1−t2

1+t2 − 2t1+t2

2t1+t2

1−t2

1+t2

)(xaya

)+

(xy


q ∈ Cobst ⇔ ∃a ∈ R2,∃b ∈ R2,




x

y

Γ(a,q)

θ


I

x(q, a) =

(1−t2

1+t2 − 2t1+t2

2t1+t2

1−t2

1+t2

)(xaya

)+

(xy


q ∈ Cobst ⇔ ∃a ∈ R2,∃b ∈ R2,



Theoreme de Tarski [Tarski 1951]

I Toute expression de Tarski est equivalente a une expressionpolynomiale.

I Pour un robot et des obstacles definis par des expressionspolynomiales, Cfree et Cobst sont donc des ensemblessemi-algebriques de R3.

I Il existe donc une expression polynomiale P telle que

q ∈ Cfree ⇔ P(q) is true














Elimination de quantificateurs

I Un exemple simple :

∃x , x2 + bx + c = 0 ⇔ b2 − 4c ≥ 0


Elimination de quantificateurs

I Un exemple simple :

∃x , x2 + bx + c = 0 ⇔ b2 − 4c ≥ 0


Decomposition de Cfree en cellules semi-algebriques

I Decomposition semi-algebrique de Rn

I partition en sous-ensembles semi-algebriques appeles cellules,pour chaque cellule, ∃j ∈ N, 1 ≤ j ≤ n tel que la cellule esthomeomorphe a Rj .

I Decomposition semi-algebrique cylindrique de Rn

I decomposition semi-algebrique de Rn definie recursivement par

1. pour n = 1, K1 est une partition de R en un ensemble fini denombres et d’intervalles ouverts,

2. pour n > 1, il existe une decomposition semi-algebriquecylindrique Kn−1 de Rn−1 telle que pour chaque celluleκ ∈ Kn, la projection de κn sur Rn−1 est une cellule de Kn−1,



I Decomposition semi-algebrique de Rn

I partition en sous-ensembles semi-algebriques appeles cellules,pour chaque cellule, ∃j ∈ N, 1 ≤ j ≤ n tel que la cellule esthomeomorphe a Rj .

I Decomposition semi-algebrique cylindrique de Rn

I decomposition semi-algebrique de Rn definie recursivement par

1. pour n = 1, K1 est une partition de R en un ensemble fini denombres et d’intervalles ouverts,

2. pour n > 1, il existe une decomposition semi-algebriquecylindrique Kn−1 de Rn−1 telle que pour chaque celluleκ ∈ Kn, la projection de κn sur Rn−1 est une cellule de Kn−1,



I Soit P un ensemble fini de polymomes de Q[X1, · · · ,Xm].I decomposition semi-algebrique P-invariante de Rn

I decomposition semi-algebrique de Rn telle que le signe dechaque polynome de P reste constant sur chaque cellule.

I Theoreme de CollinsI Il existe un algorithme qui construit une decomposition

semi-algebrique P-invariante Kn de Rn. La complexite de cetalgorithme est

I polynomiale dans le nombre de polynomes de P,I polynomiale dans le plus haut degre de P,I doublement exponentiel dans la dimension n (22n).





































Decomposition de Collins

I Exemple : P(x , y , z) = x2 + y 2 + z2 − 1

x

y

x

y

z


Solution exacte au probleme de planification de mouvement

I Algorithme :I A partir de la decomposition de Collins, construire le graphe

d’adjacence des cellules :I les noeuds sont les cellules de dimension n ou n − 1 incluses

dans Cfree ,I les aretes connectent deux noeuds si l’un est contenu dans la

frontiere de l’autre.

I Si les configurations initiales et finales sont dans la memecomposante connexe du graphe, alors il existe une solution auprobleme de planification de mouvement.

I Dans ce cas, il est possible de trouver une solution.


































Historique

I J. Schwartz and M. Sharir, On the “piano movers’”problem I. The case of a two-dimensional rigid polygonalbody moving amidst polygonal barriers, Communicationson Pure and Applied Mathematics, Volume 36, Issue 3, pages345–398, May 1983.

I J. Schwartz and M. Sharir, On the “piano movers”problem II. General techniques for computing topologicalproperties of real algebraic manifolds, Advances in AppliedMathematics, Volume 4, Issue 3, Pages 298–351, September1983.


Conclusion concernant les methodes exactes

I D’un point de vue pratique, les methodes exactesI souffrent d’une tres grande complexite algorithmique,

I croissante avec le nombre d’obstacles,I croissante avec la dimension de l’espace des configurations

I peu robustes.I Elles necessitent une precision infinie.

I Pour cette raison, elles n’ont jamais reellement eteimplementees.


Conclusion concernant les methodes exactes

I D’un point de vue pratique, les methodes exactesI souffrent d’une tres grande complexite algorithmique,

I croissante avec le nombre d’obstacles,I croissante avec la dimension de l’espace des configurations

I peu robustes.I Elles necessitent une precision infinie.

I Pour cette raison, elles n’ont jamais reellement eteimplementees.


Methodes aleatoires

I Au debut des annees 1990 sont apparues des methodesaleatoires

I PrincipeI tirer aleatoirement des configurations,I tester si elles sont en collision,I construire un graphe dont les noeuds sont des configurations

sans collisionsI et dont les aretes sont des interpolations lineaires sans

collisions.


Methodes aleatoires




collisions.


Methodes aleatoires




collisions.


Methodes aleatoires




collisions.


Methodes aleatoires




collisions.


Probabilistic roadmap (PRM) 1994

qinit

qgoal



qinit

qgoal



qgoal

qinit



qgoal

qinit



qgoal

qinit



qgoal

qinit



qgoal

qinit



qgoal

qinit



qgoal

qinit



qgoal

qinit



qgoal

qinit



qgoal

qinit



qgoal

qinit



qgoal

qinit



qgoal

qinit



qgoal

qinit



qgoal

qinit



qgoal

qinit


Probabilistic roadmap (PRM)

I Beaucoup de noeuds inutiles sont crees.I Cela rend la connexion de nouveaux noeuds au graphe existant

de plus en plus couteux.

I Variante : visibility-based PRMI On ne garde que les noeuds interessants.


Visibility-based probabilistic roadmap (Visi-PRM) 1999

qinit

qgoal



qgoal

qinit



qgoal

qinit



qgoal

qinit



qgoal

qinit



qgoal

qinit



qgoal

qinit



qgoal

qinit



qgoal

qinit



qgoal

qinit



qgoal

qinit



qgoal

qinit



qgoal

qinit



qgoal

qinit



qgoal

qinit


Rapidly exploring Random Tree (RRT) 2000

qinit

qgoal



qgoal

qinit



qgoal

qinit



qgoal

qinit



qgoal

qinit



qgoal

qinit



qgoal

qinit



qgoal

qinit



qgoal

qinit



qgoal

qinit



qgoal

qinit



qgoal

qinit



qgoal

qinit



qgoal

qinit



qgoal

qinit



qgoal

qinit



qgoal

qinit



qgoal

qinit



qgoal

qinit



qgoal

qinit


Methodes aleatoires

I Avantages :I pas de calcul explicite de l’espace des configurations,I faciles a implementer,I robustes.

I Inconvenients :I pas de propriete de completude, completude en probabilite

seulementI difficile de trouver les passages etroits.

I Operateurs requis :I Test de collision

I pour les configurations,I pour les mouvements d’interpolation.


Methodes aleatoires







Methodes aleatoires







Tests de collision

I pour configurationsI probleme : etant donnes

I deux ensembles rigides de facettes triangulaires,I la position relative de l’un par rapport a l’autre,

determiner si l’intersection entre les ensembles est vide, oubien calculer la distance entre les deux ensembles.


Hierarchie de volumes englobants

I Arbres binaires de volumes englobants tels queI chaque noeud contient deux fils,I les feuilles sont les facettes triangulaires.


























Tests de collision pour configurations

I AlgorithmeI tester les noeuds racine de chaque arbre,I si deux volumes sont en collision, tester l’un avec les fils de

l’autre.




l’autre.




l’autre.



I AlgorithmeI tester les noeuds racines de chaque arbre,I si deux volumes sont en collision, tester l’un avec les fils de

l’autre.


Tests de collision pour trajectoires

I on considere un robot avec m corps rigides :I pour i ∈ {1, ...,m} et q ∈ C, on note Bi (q) la partie de

l’espace de travail occupee par le corps i en configuration q.

I Soit Γ : [0, 1]→ C une trajectoire.

I Γ est sans collision si et seulement si

∀ t ∈ [0, 1], ∀i , j ∈ {1, ...,m}, Bi (Γ(t))⋂O = ∅

if i 6= j , Bi (Γ(t))⋂Bj(Γ(t)) = ∅

I Test by intervals.


Tests de collision pour trajectoires

I on considere un robot avec m corps rigides :I pour i ∈ {1, ...,m} et q ∈ C, on note Bi (q) la partie de

l’espace de travail occupee par le corps i en configuration q.

I Soit Γ : [0, 1]→ C une trajectoire.

I Γ est sans collision si et seulement si

∀ t ∈ [0, 1], ∀i , j ∈ {1, ...,m}, Bi (Γ(t))⋂O = ∅

if i 6= j , Bi (Γ(t))⋂Bj(Γ(t)) = ∅

I Test by intervals.


Vitesse d’une articulation

Soit M un mouvement rigide dependant du temps et x un point deR3. Le mouvement du point x au cours du temps est donne par

X (t) = M(t).x = R(t).x + t(t)

La vitesse du point X est donc

X (t) = R(t).x + t(t)



Soit M un mouvement rigide dependant du temps et x un point deR3. Le mouvement du point x au cours du temps est donne par

X (t) = M(t).x = R(t).x + t(t)

La vitesse du point X est donc

X (t) = R(t).x + t(t)


Derivee d’une matrice de rotation

RRT = I3

Par derivation,

RRT + RRT = 0

La matrice RRT est antisymetrique. On la note [ω]×. Ainsi

R = [ω]×R [ω]× =

0 −ω2 ω1

ω2 0 −ω0

−ω1 ω0 0



RRT = I3

Par derivation,

RRT + RRT = 0


R = [ω]×R [ω]× =

0 −ω2 ω1

ω2 0 −ω0

−ω1 ω0 0



RRT = I3

Par derivation,

RRT + RRT = 0


R = [ω]×R [ω]× =

0 −ω2 ω1

ω2 0 −ω0

−ω1 ω0 0



X (t) = R(t).x + t(t)

= [ω(t)]×R.x + t(t)

= ω(t)× (X (t)− t(t)) + t(t)

R0

x

M(t)X (t)

t(t)

ω

On retrouve la formule du torseur cinematique.



X (t) = R(t).x + t(t)

= [ω(t)]×R.x + t(t)

= ω(t)× (X (t)− t(t)) + t(t)

R0

x

M(t)X (t)

t(t)

ω

On retrouve la formule du torseur cinematique.


Tests de collision par intervalles

On considere des trajectoires a vitesses lineaires et angulairesconstantes :

I on note Vi = (vi , ωi ) le torseur cinematique de vitesse dureferentiel Ri dans le referentiel Rparent(i).

R0


Mi (q)

Mi/parent Ti (q)


Tests de collision par intervalles

On considere des trajectoires a vitesses lineaires et angulairesconstantes :

I on note Vi = (vi , ωi ) le torseur cinematique de vitesse dureferentiel Ri dans le referentiel Rparent(i).

R0


Mi (q)

Mi/parent Ti (q)


Majoration de la vitesse d’un corps

On considere une chaıne cinematique a p articulations telle que :

I parent (i+1) = i, pour i entre 0 et p-1.

R0

Ri

Mi

Ri+1

Mi+1

Mi+1/i

Vi/0

Vi+1

OiOi+1

Le torseur de vitesse de Ri+1 dans R0 est donnee par :

vi+1/0 = Rivi+1 + vi/0 + ωi/0 × ~OiOi+1

ωi+1/0 = ωi/0 + Riωi+1



On considere une chaıne cinematique a p articulations telle que :

I parent (i+1) = i, pour i entre 0 et p-1.

R0

Ri

Mi

Ri+1

Mi+1

Mi+1/i

Vi/0

Vi+1

OiOi+1


vi+1/0 = Rivi+1 + vi/0 + ωi/0 × ~OiOi+1

ωi+1/0 = ωi/0 + Riωi+1




vi+1/0 = Rivi+1 + vi/0 + ωi/0 × ~OiOi+1

ωi+1/0 = ωi/0 + Riωi+1

On peut majorer la norme des composantes du torseur cinematique

‖vi+1/0‖ ≤ ‖vi+1‖+ ‖vi/0‖+ ‖ωi/0‖.max ‖ ~OiOi+1‖ (1)

‖ωi+1/0‖ ≤ ‖ωi/0‖+ ‖ωi+1‖ (2)

De (2), on deduit immediatement

‖ωi+1/0‖ ≤i+1∑j=1

‖ωj‖




vi+1/0 = Rivi+1 + vi/0 + ωi/0 × ~OiOi+1

ωi+1/0 = ωi/0 + Riωi+1



‖ωi+1/0‖ ≤ ‖ωi/0‖+ ‖ωi+1‖ (2)


‖ωi+1/0‖ ≤i+1∑j=1

‖ωj‖




vi+1/0 = Rivi+1 + vi/0 + ωi/0 × ~OiOi+1

ωi+1/0 = ωi/0 + Riωi+1



‖ωi+1/0‖ ≤ ‖ωi/0‖+ ‖ωi+1‖ (2)


‖ωi+1/0‖ ≤i+1∑j=1

‖ωj‖




‖vi+1/0‖ ≤ ‖vi+1‖+ ‖vi/0‖+ ‖ωi/0‖.max ‖ ~OiOi+1‖‖ωi+1/0‖ ≤ ‖ωi/0‖+ ‖ωi+1‖


‖ωi+1/0‖ ≤i+1∑j=1

‖ωj‖

Par recurrence, on peut montrer que

‖vi/0‖ ≤i∑

j=1

‖vj‖+i−1∑k=1

k∑j=1

‖ωj‖.max ‖ ~OkOk+1‖




‖vi+1/0‖ ≤ ‖vi+1‖+ ‖vi/0‖+ ‖ωi/0‖.max ‖ ~OiOi+1‖‖ωi+1/0‖ ≤ ‖ωi/0‖+ ‖ωi+1‖


‖ωi+1/0‖ ≤i+1∑j=1

‖ωj‖


‖vi/0‖ ≤i∑

j=1

‖vj‖+i−1∑k=1

k∑j=1





‖vi/0‖ ≤i∑

j=1

‖vj‖+i−1∑k=1

k∑j=1


En intervertissant les indices j et k, on obtient

‖vi/0‖ ≤i∑

j=1

‖vj‖+i−1∑j=1

i−1∑k=j

max ‖ ~OkOk+1‖

‖ωj‖

‖ωi/0‖ ≤i∑

j=1

‖ωj‖

On obtient des majorations sous forme de combinaison lineaire des‖vj‖, ‖ωj‖ qui sont constants le long du mouvement.




‖vi/0‖ ≤i∑

j=1

‖vj‖+i−1∑k=1

k∑j=1



‖vi/0‖ ≤i∑

j=1

‖vj‖+i−1∑j=1

i−1∑k=j

max ‖ ~OkOk+1‖

‖ωj‖

‖ωi/0‖ ≤i∑

j=1

‖ωj‖





‖vi/0‖ ≤i∑

j=1

‖vj‖+i−1∑k=1

k∑j=1



‖vi/0‖ ≤i∑

j=1

‖vj‖+i−1∑j=1

i−1∑k=j

max ‖ ~OkOk+1‖

‖ωj‖

‖ωi/0‖ ≤i∑

j=1

‖ωj‖





‖vi/0‖ ≤i∑

j=1

‖vj‖+i−1∑k=1

k∑j=1



‖vi/0‖ ≤i∑

j=1

‖vj‖+i−1∑j=1

i−1∑k=j

max ‖ ~OkOk+1‖

‖ωj‖

‖ωi/0‖ ≤i∑

j=1

‖ωj‖





‖vi/0‖ ≤i∑

j=1

‖vj‖+i−1∑k=1

k∑j=1



‖vi/0‖ ≤i∑

j=1

‖vj‖+i−1∑j=1

i−1∑k=j

max ‖ ~OkOk+1‖

‖ωj‖

‖ωi/0‖ ≤i∑

j=1

‖ωj‖




‖ ~OkOk+1‖I si l’articulation k est une rotation R1 ou R3, ‖ ~OkOk+1‖ est

constant,

I si l’articulation k est une translation, ddt

~OkOk+1 est constant.Le maximum est atteint aux bornes du mouvement.



Si tous les points deplaces par l’articulation i sont dans une boulede rayon ri centree sur l’origine de R, la vitesse de chaque pointest majoree par

vi/0max = ‖vi/0‖+ ri ‖ωi/0‖

R0

Ri

Vi/0 = (vi/0, ωi/0)

Oi

O0

ri



R0

Ri

Vi/0 = (vi/0, ωi/0)

Oi

O0

ri

B0 Bi

d

I Si a l’abscisse t = t0, la distance entre B0 et Bi est notee d ,

I alors sur l’intervalle

]t0 −d

vi/0max, t0 +

d

vi/0max[

les corps B0 et Bi sont sans collision.



R0

Ri

Vi/0 = (vi/0, ωi/0)

Oi

O0

ri

B0 Bi

d

I Si a l’abscisse t = t0, la distance entre B0 et Bi est notee d ,

I alors sur l’intervalle

]t0 −d

vi/0max, t0 +

d

vi/0max[

les corps B0 et Bi sont sans collision.


Test de collision par intervallesAlgorithme :

t ← 0 ; dtmax ←∞while t < tmax doq← Γ(t) ; computeForwardKinematics (q)for all pair (Bi , Bj) do

d ← distmin (Bi , Bj)if d == 0 then

return trueelseif d/vij max < dtmax then

dtmax ← d/vij max

end ifend if

end fort ← t + dtmax

end whilereturn false


Documents

Planification de mouvement et suivi de trajectoireprojects.laas.fr/gepetto/uploads/Members/2015-isae...Contexte robot industriel drone v ehicule autonome Syst emes mobiles autonomes