planification de mouvement et suivi de...
TRANSCRIPT
Planification de mouvement et suivi de trajectoire
Florent Lamiraux
CNRS-LAAS, Toulouse, France
Planification de mouvement et suivi de trajectoire
Planification de mouvement et suivi de trajectoire
Planification de mouvement et suivi de trajectoire
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.
Planification de mouvement et suivi de trajectoire
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.
Planification de mouvement et suivi de trajectoire
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.
Planification de mouvement et suivi de trajectoire
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).
Planification de mouvement et suivi de trajectoire
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).
Planification de mouvement et suivi de trajectoire
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
Planification de mouvement et suivi de trajectoire
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 T3 :
R3 → SE (3)
t → T(I3,t)
translation
Planification de mouvement et suivi de trajectoire
Articulation
Une articulation est representee par une application d’unesous-variete de Rp dans SE(3), ou p ≥ 1 est un entier.Exemples :
I Rotation R1 :
R → SE (3)
t → T(R,0)
R =
cos t − sin t 0sin t cos t 0
0 0 1
Planification de mouvement et suivi de trajectoire
Articulation
Une articulation est representee par une application d’unesous-variete de Rp dans SE(3), ou p ≥ 1 est un entier.Exemples :
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.
Planification de mouvement et suivi de trajectoire
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)
Planification de mouvement et suivi de trajectoire
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)
Planification de mouvement et suivi de trajectoire
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)
Planification de mouvement et suivi de trajectoire
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)
Planification de mouvement et suivi de trajectoire
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)
Planification de mouvement et suivi de trajectoire
Quaternions unitaires et rotations
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.
Planification de mouvement et suivi de trajectoire
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
Planification de mouvement et suivi de trajectoire
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)
Planification de mouvement et suivi de trajectoire
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 .
Planification de mouvement et suivi de trajectoire
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 .
Planification de mouvement et suivi de trajectoire
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 .
Planification de mouvement et suivi de trajectoire
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 .
Planification de mouvement et suivi de trajectoire
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 .
Planification de mouvement et suivi de trajectoire
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 .
Planification de mouvement et suivi de trajectoire
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 .
Planification de mouvement et suivi de trajectoire
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 .
Planification de mouvement et suivi de trajectoire
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 .
Planification de mouvement et suivi de trajectoire
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 et suivi de trajectoire
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 et suivi de trajectoire
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 et suivi de trajectoire
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 et suivi de trajectoire
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 et suivi de trajectoire
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 et suivi de trajectoire
Planification de mouvement
I Un cas simple : un point dans un plan.
qinit
qgoal
Planification de mouvement et suivi de trajectoire
Planification de mouvement
I Espace des configurations = espace de travail
qinit
qgoal
Planification de mouvement et suivi de trajectoire
Planification de mouvement
I Decomposition de Cfree en cellules convexes
qinit
qgoal
Planification de mouvement et suivi de trajectoire
Planification de mouvement
I Decomposition de Cfree en cellules convexes
qinit
qgoal
Planification de mouvement et suivi de trajectoire
Planification de mouvement
I Decomposition de Cfree en cellules convexes
qinit
qgoal
Planification de mouvement et suivi de trajectoire
Planification de mouvement
I Decomposition de Cfree en cellules convexes
qinit
qgoal
Planification de mouvement et suivi de trajectoire
Planification de mouvement
I Decomposition de Cfree en cellules convexes
qinit
qgoal
Planification de mouvement et suivi de trajectoire
Planification de mouvement
I Decomposition de Cfree en cellules convexes
qinit
qgoal
Planification de mouvement et suivi de trajectoire
Planification de mouvement
I Decomposition de Cfree en cellules convexes
qinit
qgoal
Planification de mouvement et suivi de trajectoire
Planification de mouvement
I Decomposition de Cfree en cellules convexes
qinit
qgoal
Planification de mouvement et suivi de trajectoire
Planification de mouvement
I Decomposition de Cfree en cellules convexes
qinit
qgoal
Planification de mouvement et suivi de trajectoire
Planification de mouvement
I Complexite : O(nlog(n))
qinit
qgoal
Planification de mouvement et suivi de trajectoire
Planification de mouvement
I Un peu plus difficile : un polygone en translation.
qinit
qgoal
Planification de mouvement et suivi de trajectoire
Planification de mouvement
I Construction de l’espace des configurations
qinit
qgoal
Planification de mouvement et suivi de trajectoire
Planification de mouvement
I Minkowski : Cobst = {o− r, o ∈ obstacle, r ∈ robot}
qinit
qgoal
Planification de mouvement et suivi de trajectoire
Planification de mouvement
I Minkowski : Cobst = {o− r, o ∈ obstacle, r ∈ robot}
qinit
qgoal
Planification de mouvement et suivi de trajectoire
Planification de mouvement
I Minkowski : Cobst = {o− r, o ∈ obstacle, r ∈ robot}
qinit
qgoal
Planification de mouvement et suivi de trajectoire
Planification de mouvement
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)),
Planification de mouvement et suivi de trajectoire
Planification de mouvement
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)),
Planification de mouvement et suivi de trajectoire
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.
Planification de mouvement et suivi de trajectoire
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.
Planification de mouvement et suivi de trajectoire
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.
Planification de mouvement et suivi de trajectoire
Inconvenients
I problemes plans,
I manque de robustesse des calculs de geometrie algorithmiqueen precision finie.
Planification de mouvement et suivi de trajectoire
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.
Planification de mouvement et suivi de trajectoire
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.
Planification de mouvement et suivi de trajectoire
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.
Planification de mouvement et suivi de trajectoire
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.
Planification de mouvement et suivi de trajectoire
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.
Planification de mouvement et suivi de trajectoire
Ensemble semi-algebrique
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.
Planification de mouvement et suivi de trajectoire
Ensemble semi-algebrique
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.
Planification de mouvement et suivi de trajectoire
Ensemble semi-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.
Planification de mouvement et suivi de trajectoire
Ensemble semi-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.
Planification de mouvement et suivi de trajectoire
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.
Planification de mouvement et suivi de trajectoire
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.
Planification de mouvement et suivi de trajectoire
Robot et environnement semi-algebriques
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.
Planification de mouvement et suivi de trajectoire
Robot et environnement semi-algebriques
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.
Planification de mouvement et suivi de trajectoire
Robot et environnement semi-algebriques
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.
Planification de mouvement et suivi de trajectoire
Robot et environnement semi-algebriques
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.
Planification de mouvement et suivi de trajectoire
Robot et environnement semi-algebriques
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
Planification de mouvement et suivi de trajectoire
Robot et environnement semi-algebriques
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
Planification de mouvement et suivi de trajectoire
Robot et environnement semi-algebriques
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
Planification de mouvement et suivi de trajectoire
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
Planification de mouvement et suivi de trajectoire
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
Planification de mouvement et suivi de trajectoire
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
Planification de mouvement et suivi de trajectoire
Elimination de quantificateurs
I Un exemple simple :
∃x , x2 + bx + c = 0 ⇔ b2 − 4c ≥ 0
Planification de mouvement et suivi de trajectoire
Elimination de quantificateurs
I Un exemple simple :
∃x , x2 + bx + c = 0 ⇔ b2 − 4c ≥ 0
Planification de mouvement et suivi de trajectoire
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,
Planification de mouvement et suivi de trajectoire
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,
Planification de mouvement et suivi de trajectoire
Decomposition de Cfree en cellules semi-algebriques
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).
Planification de mouvement et suivi de trajectoire
Decomposition de Cfree en cellules semi-algebriques
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).
Planification de mouvement et suivi de trajectoire
Decomposition de Cfree en cellules semi-algebriques
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).
Planification de mouvement et suivi de trajectoire
Decomposition de Cfree en cellules semi-algebriques
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).
Planification de mouvement et suivi de trajectoire
Decomposition de Cfree en cellules semi-algebriques
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).
Planification de mouvement et suivi de trajectoire
Decomposition de Cfree en cellules semi-algebriques
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).
Planification de mouvement et suivi de trajectoire
Decomposition de Collins
I Exemple : P(x , y , z) = x2 + y 2 + z2 − 1
x
y
x
y
z
Planification de mouvement et suivi de trajectoire
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.
Planification de mouvement et suivi de trajectoire
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.
Planification de mouvement et suivi de trajectoire
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.
Planification de mouvement et suivi de trajectoire
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.
Planification de mouvement et suivi de trajectoire
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.
Planification de mouvement et suivi de trajectoire
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.
Planification de mouvement et suivi de trajectoire
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.
Planification de mouvement et suivi de trajectoire
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.
Planification de mouvement et suivi de trajectoire
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.
Planification de mouvement et suivi de trajectoire
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.
Planification de mouvement et suivi de trajectoire
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.
Planification de mouvement et suivi de trajectoire
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.
Planification de mouvement et suivi de trajectoire
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.
Planification de mouvement et suivi de trajectoire
Probabilistic roadmap (PRM) 1994
qinit
qgoal
Planification de mouvement et suivi de trajectoire
Probabilistic roadmap (PRM) 1994
qinit
qgoal
Planification de mouvement et suivi de trajectoire
Probabilistic roadmap (PRM) 1994
qgoal
qinit
Planification de mouvement et suivi de trajectoire
Probabilistic roadmap (PRM) 1994
qgoal
qinit
Planification de mouvement et suivi de trajectoire
Probabilistic roadmap (PRM) 1994
qgoal
qinit
Planification de mouvement et suivi de trajectoire
Probabilistic roadmap (PRM) 1994
qgoal
qinit
Planification de mouvement et suivi de trajectoire
Probabilistic roadmap (PRM) 1994
qgoal
qinit
Planification de mouvement et suivi de trajectoire
Probabilistic roadmap (PRM) 1994
qgoal
qinit
Planification de mouvement et suivi de trajectoire
Probabilistic roadmap (PRM) 1994
qgoal
qinit
Planification de mouvement et suivi de trajectoire
Probabilistic roadmap (PRM) 1994
qgoal
qinit
Planification de mouvement et suivi de trajectoire
Probabilistic roadmap (PRM) 1994
qgoal
qinit
Planification de mouvement et suivi de trajectoire
Probabilistic roadmap (PRM) 1994
qgoal
qinit
Planification de mouvement et suivi de trajectoire
Probabilistic roadmap (PRM) 1994
qgoal
qinit
Planification de mouvement et suivi de trajectoire
Probabilistic roadmap (PRM) 1994
qgoal
qinit
Planification de mouvement et suivi de trajectoire
Probabilistic roadmap (PRM) 1994
qgoal
qinit
Planification de mouvement et suivi de trajectoire
Probabilistic roadmap (PRM) 1994
qgoal
qinit
Planification de mouvement et suivi de trajectoire
Probabilistic roadmap (PRM) 1994
qgoal
qinit
Planification de mouvement et suivi de trajectoire
Probabilistic roadmap (PRM) 1994
qgoal
qinit
Planification de mouvement et suivi de trajectoire
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.
Planification de mouvement et suivi de trajectoire
Visibility-based probabilistic roadmap (Visi-PRM) 1999
qinit
qgoal
Planification de mouvement et suivi de trajectoire
Visibility-based probabilistic roadmap (Visi-PRM) 1999
qgoal
qinit
Planification de mouvement et suivi de trajectoire
Visibility-based probabilistic roadmap (Visi-PRM) 1999
qgoal
qinit
Planification de mouvement et suivi de trajectoire
Visibility-based probabilistic roadmap (Visi-PRM) 1999
qgoal
qinit
Planification de mouvement et suivi de trajectoire
Visibility-based probabilistic roadmap (Visi-PRM) 1999
qgoal
qinit
Planification de mouvement et suivi de trajectoire
Visibility-based probabilistic roadmap (Visi-PRM) 1999
qgoal
qinit
Planification de mouvement et suivi de trajectoire
Visibility-based probabilistic roadmap (Visi-PRM) 1999
qgoal
qinit
Planification de mouvement et suivi de trajectoire
Visibility-based probabilistic roadmap (Visi-PRM) 1999
qgoal
qinit
Planification de mouvement et suivi de trajectoire
Visibility-based probabilistic roadmap (Visi-PRM) 1999
qgoal
qinit
Planification de mouvement et suivi de trajectoire
Visibility-based probabilistic roadmap (Visi-PRM) 1999
qgoal
qinit
Planification de mouvement et suivi de trajectoire
Visibility-based probabilistic roadmap (Visi-PRM) 1999
qgoal
qinit
Planification de mouvement et suivi de trajectoire
Visibility-based probabilistic roadmap (Visi-PRM) 1999
qgoal
qinit
Planification de mouvement et suivi de trajectoire
Visibility-based probabilistic roadmap (Visi-PRM) 1999
qgoal
qinit
Planification de mouvement et suivi de trajectoire
Visibility-based probabilistic roadmap (Visi-PRM) 1999
qgoal
qinit
Planification de mouvement et suivi de trajectoire
Visibility-based probabilistic roadmap (Visi-PRM) 1999
qgoal
qinit
Planification de mouvement et suivi de trajectoire
Rapidly exploring Random Tree (RRT) 2000
qinit
qgoal
Planification de mouvement et suivi de trajectoire
Rapidly exploring Random Tree (RRT) 2000
qgoal
qinit
Planification de mouvement et suivi de trajectoire
Rapidly exploring Random Tree (RRT) 2000
qgoal
qinit
Planification de mouvement et suivi de trajectoire
Rapidly exploring Random Tree (RRT) 2000
qgoal
qinit
Planification de mouvement et suivi de trajectoire
Rapidly exploring Random Tree (RRT) 2000
qgoal
qinit
Planification de mouvement et suivi de trajectoire
Rapidly exploring Random Tree (RRT) 2000
qgoal
qinit
Planification de mouvement et suivi de trajectoire
Rapidly exploring Random Tree (RRT) 2000
qgoal
qinit
Planification de mouvement et suivi de trajectoire
Rapidly exploring Random Tree (RRT) 2000
qgoal
qinit
Planification de mouvement et suivi de trajectoire
Rapidly exploring Random Tree (RRT) 2000
qgoal
qinit
Planification de mouvement et suivi de trajectoire
Rapidly exploring Random Tree (RRT) 2000
qgoal
qinit
Planification de mouvement et suivi de trajectoire
Rapidly exploring Random Tree (RRT) 2000
qgoal
qinit
Planification de mouvement et suivi de trajectoire
Rapidly exploring Random Tree (RRT) 2000
qgoal
qinit
Planification de mouvement et suivi de trajectoire
Rapidly exploring Random Tree (RRT) 2000
qgoal
qinit
Planification de mouvement et suivi de trajectoire
Rapidly exploring Random Tree (RRT) 2000
qgoal
qinit
Planification de mouvement et suivi de trajectoire
Rapidly exploring Random Tree (RRT) 2000
qgoal
qinit
Planification de mouvement et suivi de trajectoire
Rapidly exploring Random Tree (RRT) 2000
qgoal
qinit
Planification de mouvement et suivi de trajectoire
Rapidly exploring Random Tree (RRT) 2000
qgoal
qinit
Planification de mouvement et suivi de trajectoire
Rapidly exploring Random Tree (RRT) 2000
qgoal
qinit
Planification de mouvement et suivi de trajectoire
Rapidly exploring Random Tree (RRT) 2000
qgoal
qinit
Planification de mouvement et suivi de trajectoire
Rapidly exploring Random Tree (RRT) 2000
qgoal
qinit
Planification de mouvement et suivi de trajectoire
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.
Planification de mouvement et suivi de trajectoire
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.
Planification de mouvement et suivi de trajectoire
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.
Planification de mouvement et suivi de trajectoire
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.
Planification de mouvement et suivi de trajectoire
Hierarchie de volumes englobants
I Arbres binaires de volumes englobants tels queI chaque noeud contient deux fils,I les feuilles sont les facettes triangulaires.
Planification de mouvement et suivi de trajectoire
Hierarchie de volumes englobants
I Arbres binaires de volumes englobants tels queI chaque noeud contient deux fils,I les feuilles sont les facettes triangulaires.
Planification de mouvement et suivi de trajectoire
Hierarchie de volumes englobants
I Arbres binaires de volumes englobants tels queI chaque noeud contient deux fils,I les feuilles sont les facettes triangulaires.
Planification de mouvement et suivi de trajectoire
Hierarchie de volumes englobants
I Arbres binaires de volumes englobants tels queI chaque noeud contient deux fils,I les feuilles sont les facettes triangulaires.
Planification de mouvement et suivi de trajectoire
Hierarchie de volumes englobants
I Arbres binaires de volumes englobants tels queI chaque noeud contient deux fils,I les feuilles sont les facettes triangulaires.
Planification de mouvement et suivi de trajectoire
Hierarchie de volumes englobants
I Arbres binaires de volumes englobants tels queI chaque noeud contient deux fils,I les feuilles sont les facettes triangulaires.
Planification de mouvement et suivi de trajectoire
Hierarchie de volumes englobants
I Arbres binaires de volumes englobants tels queI chaque noeud contient deux fils,I les feuilles sont les facettes triangulaires.
Planification de mouvement et suivi de trajectoire
Hierarchie de volumes englobants
I Arbres binaires de volumes englobants tels queI chaque noeud contient deux fils,I les feuilles sont les facettes triangulaires.
Planification de mouvement et suivi de trajectoire
Hierarchie de volumes englobants
I Arbres binaires de volumes englobants tels queI chaque noeud contient deux fils,I les feuilles sont les facettes triangulaires.
Planification de mouvement et suivi de trajectoire
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.
Planification de mouvement et suivi de trajectoire
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.
Planification de mouvement et suivi de trajectoire
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.
Planification de mouvement et suivi de trajectoire
Tests de collision pour configurations
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.
Planification de mouvement et suivi de trajectoire
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.
Planification de mouvement et suivi de trajectoire
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.
Planification de mouvement et suivi de trajectoire
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)
Planification de mouvement et suivi de trajectoire
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)
Planification de mouvement et suivi de trajectoire
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
Planification de mouvement et suivi de trajectoire
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
Planification de mouvement et suivi de trajectoire
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
Planification de mouvement et suivi de trajectoire
Vitesse d’une articulation
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.
Planification de mouvement et suivi de trajectoire
Vitesse d’une articulation
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.
Planification de mouvement et suivi de trajectoire
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
Rparent(i)Mparent(i)(q) Ri
Mi (q)
Mi/parent Ti (q)
Planification de mouvement et suivi de trajectoire
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
Rparent(i)Mparent(i)(q) Ri
Mi (q)
Mi/parent Ti (q)
Planification de mouvement et suivi de trajectoire
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
Planification de mouvement et suivi de trajectoire
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
Planification de mouvement et suivi de trajectoire
Majoration de la vitesse d’un corps
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 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‖
Planification de mouvement et suivi de trajectoire
Majoration de la vitesse d’un corps
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 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‖
Planification de mouvement et suivi de trajectoire
Majoration de la vitesse d’un corps
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 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‖
Planification de mouvement et suivi de trajectoire
Majoration de la vitesse d’un corps
On peut majorer la norme des composantes du torseur cinematique
‖vi+1/0‖ ≤ ‖vi+1‖+ ‖vi/0‖+ ‖ωi/0‖.max ‖ ~OiOi+1‖‖ωi+1/0‖ ≤ ‖ωi/0‖+ ‖ωi+1‖
De (2), on deduit immediatement
‖ω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‖
Planification de mouvement et suivi de trajectoire
Majoration de la vitesse d’un corps
On peut majorer la norme des composantes du torseur cinematique
‖vi+1/0‖ ≤ ‖vi+1‖+ ‖vi/0‖+ ‖ωi/0‖.max ‖ ~OiOi+1‖‖ωi+1/0‖ ≤ ‖ωi/0‖+ ‖ωi+1‖
De (2), on deduit immediatement
‖ω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‖
Planification de mouvement et suivi de trajectoire
Majoration de la vitesse d’un corps
Par recurrence, on peut montrer que
‖vi/0‖ ≤i∑
j=1
‖vj‖+i−1∑k=1
k∑j=1
‖ωj‖.max ‖ ~OkOk+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.
Planification de mouvement et suivi de trajectoire
Majoration de la vitesse d’un corps
Par recurrence, on peut montrer que
‖vi/0‖ ≤i∑
j=1
‖vj‖+i−1∑k=1
k∑j=1
‖ωj‖.max ‖ ~OkOk+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.
Planification de mouvement et suivi de trajectoire
Majoration de la vitesse d’un corps
Par recurrence, on peut montrer que
‖vi/0‖ ≤i∑
j=1
‖vj‖+i−1∑k=1
k∑j=1
‖ωj‖.max ‖ ~OkOk+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.
Planification de mouvement et suivi de trajectoire
Majoration de la vitesse d’un corps
Par recurrence, on peut montrer que
‖vi/0‖ ≤i∑
j=1
‖vj‖+i−1∑k=1
k∑j=1
‖ωj‖.max ‖ ~OkOk+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.
Planification de mouvement et suivi de trajectoire
Majoration de la vitesse d’un corps
Par recurrence, on peut montrer que
‖vi/0‖ ≤i∑
j=1
‖vj‖+i−1∑k=1
k∑j=1
‖ωj‖.max ‖ ~OkOk+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.
Planification de mouvement et suivi de trajectoire
Majoration de la vitesse d’un corps
‖ ~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.
Planification de mouvement et suivi de trajectoire
Majoration de la vitesse d’un corps
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
Planification de mouvement et suivi de trajectoire
Majoration de la vitesse d’un corps
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.
Planification de mouvement et suivi de trajectoire
Majoration de la vitesse d’un corps
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.
Planification de mouvement et suivi de trajectoire
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
Planification de mouvement et suivi de trajectoire