Optimisation de trajectoire pour la navigation bathymétrique

Optimisation de trajectoire pour la navigation
bathymétrique
Kieran Delamotte, Carlo de Franchis, David Gontier, Antoine
Levitt, François Madiot, Carlo Marcati
Sujet proposé par Jérémy Nicola, iXBlue
SEME, Vendredi 16 Janvier 2015
1
Présentation du problème
2
Algorithme de recalage
3
Optimisation de trajectoire
Gloutonne
Dijkstra
Résultats
4
Pistes continues
5
Pistes probabilistes
6
Conclusion
7
Bibliographie et ouvertures
Sommaire
1
Présentation du problème
2
Algorithme de recalage
3
Optimisation de trajectoire
4
Pistes continues
5
Pistes probabilistes
6
Conclusion
7
Bibliographie et ouvertures
Problématique du sous-marin
• Un sous-marin se déplace à profondeur constante (pas de GPS).
• Problème : A cause des imprécisions de ses appareils de mesure,
et de la dérive, il peut se perdre.
• Idée : il peut se recaler en mesurant le fond marin, et en
comparant avec des cartes bathymétriques
Question :
Quelles sont les trajectoires qui permettent un recalage précis ?
Présentation du problème
Sous-marin parfait :
connait parfaitement la trajectoire de A à B.
calcule parfaitement (vx (t), vy (t), h(t)).
vx
t
A
vy
B
t
h
t
Présentation du problème
Sous-marin parfait :
connait parfaitement la trajectoire de A à B.
calcule parfaitement (vx (t), vy (t), h(t)).
vx
t
t
A
vy
B
t
h
t
Présentation du problème
Sous-marin parfait :
connait parfaitement la trajectoire de A à B.
calcule parfaitement (vx (t), vy (t), h(t)).
vx
t
A
vy
B
t
h
t
Présentation du problème
Sous-marin réel : (va suivre la même trajectoire)
connait parfaitement A, et la carte de fond.
e
Il mesure (vfx (t), vfy (t), h(t))
=
(vx (t), vy (t), h(t)) + (evx (t), evy (t), eh (t)).
But : estimer où est B !
vx
A
t
vy
t
h
t
Présentation du problème
Sous-marin réel : (va suivre la même trajectoire)
connait parfaitement A, et la carte de fond.
e
Il mesure (vfx (t), vfy (t), h(t))
=
(vx (t), vy (t), h(t)) + (evx (t), evy (t), eh (t)).
But : estimer où est B !
vx
t
t
A
vy
?
t
h
t
Présentation du problème
Sous-marin réel : (va suivre la même trajectoire)
connait parfaitement A, et la carte de fond.
e
Il mesure (vfx (t), vfy (t), h(t))
=
(vx (t), vy (t), h(t)) + (evx (t), evy (t), eh (t)).
But : estimer où est B !
vx
A
t
t
vy
t
h
t
Présentation du problème
Sous-marin réel : (va suivre la même trajectoire)
connait parfaitement A, et la carte de fond.
e
Il mesure (vfx (t), vfy (t), h(t))
=
(vx (t), vy (t), h(t)) + (evx (t), evy (t), eh (t)).
But : estimer où est B !
vx
t
A
B?
vy
t
h
t
Notre problème
Problème :
Quel est le chemin de A à B qui minimise "l’incertitude" en B ?
Notre approche :
1
Définition de la notion d’incertitudes
2
Calcul effectif du coût d’un chemin du sous-marin parfait
3
Optimisation de chemin
Enoncé du problème sous forme continue
Soit γ0 ∈ C 0 ([0, T ], R2 ) la trajectoire du sous-marin parfait, on
note Aγ0 l’ensemble des trajectoires admissibles:
Aγ0 :=
n
γ ∈ C 0 ([0, T ], R2 ),
γ(0) = A,
∀ 0 ≤ t ≤ T,
kγ 0 (t) − γ00 (t)k∞ ≤ v ,
∀ 0 ≤ t ≤ T,
|H(γ(t)) − H(γ0 (t))| ≤ h .
o
L’incertitude de γ0 (fonction coût) est
c(γ0 ) = Vol {γ(T ), γ ∈ Aγ0 } .
On cherche alors
n
o
arg min c(γ0 ), γ0 ∈ C 0 ([0, T ], R2 ), γ0 (0) = A, γ0 (T ) = B .
Sommaire
1
Présentation du problème
2
Algorithme de recalage
3
Optimisation de trajectoire
4
Pistes continues
5
Pistes probabilistes
6
Conclusion
7
Bibliographie et ouvertures
Algorithme de recalage
Algorithme de recalage (pour le sous-marin réel) :
Entrée : γ0 := {A, A1 , A2 , . . . , AN = B}1 la trajectoire du
sous-marin parfait.
Sortie : L’incertitude c(γ0 ).
Idée :
On pose
Jk := γ
kT
N
, γ ∈ Aγ0 ,
l’ensemble des positions admissibles au temps k.
On a J0 = {A}, et on peut calculer les Jk par récurrences.
On a c(γ0 ) = ] (JN ) (incertitude finale), ou
P
c(γ0 ) = N
k=1 ] (Jk ) (incertitude globale).
1
Ici, Ak = γ0 (kT /N)
Une étape de recalage
Jk
Ak
On part de l’ensemble Jk (avec Ak ∈ Jk ).
Une étape de recalage
Jk
Ak
vk
Ak+1
On lit une vitesse vfk = vk + ev ,k , avec |ev ,k | < v .
Une étape de recalage
Jk
Ak
T (Jk )
Ak+1
On translate l’ensemble par vk , et on prend son v -voisinage.
Une étape de recalage
Jk
Ak
{H −1 (hk+1 )}
{H −1 (hk+1 − h )} {H −1 (hk+1 + h )}
Ak+1
On lit une hauteur hg
k+1 = hk+1 + eh,k+1 , avec |eh,k+1 | < h .
Une étape de recalage
Jk
Ak
Ak+1
On prend l’intersection.
Une étape de recalage
Jk
Ak
Jk+1
Ak+1
On obtient Jk comme l’intersection de 2 ensembles.
Algorithme de recalage sur données réelles
Algorithme de recalage sur données réelles
Algorithme de recalage sur données réelles
Algorithme de recalage sur données réelles
Algorithme de recalage sur données réelles
Algorithme de recalage sur données réelles
Algorithme de recalage sur données réelles
Sommaire
1
Présentation du problème
2
Algorithme de recalage
3
Optimisation de trajectoire
Gloutonne
Dijkstra
Résultats
4
Pistes continues
5
Pistes probabilistes
6
Conclusion
7
Bibliographie et ouvertures
Stratégie gloutonne
Remarque : Il faut trouver des forts gradients, qui sont
successivement "orthogonaux" !
Jk−1
Stratégie gloutonne
Remarque : Il faut trouver des forts gradients, qui sont
successivement "orthogonaux" !
Jk−1
T (Jk−1 )
Stratégie gloutonne
Remarque : Il faut trouver des forts gradients, qui sont
successivement "orthogonaux" !
Jk
∇H(Ak )
Stratégie gloutonne
Remarque : Il faut trouver des forts gradients, qui sont
successivement "orthogonaux" !
T (Jk )
Jk
Stratégie gloutonne
Remarque : Il faut trouver des forts gradients, qui sont
successivement "orthogonaux" !
∇H(Ak+1 )
Jk+1
Algorithme glouton
On en déduit une stratégie gloutonne :
On pose γ = [A], et P = A.
Tant que P 6= B,
Calculer
EP = {P 0 , rmin ≤ d(P, P 0 ) ≤ rmax , d(P 0 , B) < d(P, B)}.
Si B ∈ EP , γ = [γ, B], P = B.
Sinon, poser
n
o
Pnext = arg min ∇H(P 0 ) ∧ ∇H(P), P 0 ∈ EP
puis γ = [γ, Pnext ], et P = Pnext .
Remarque : Les constantes rmin et rmax prennent en compte la
vitesse minimale et maximale du sous-marin, et la fréquence
d’échantillonnage !
Stratégie globale
La stratégie gloutonne est locale: on cherche un algorithme global.
On considère les pixels de la carte comme des noeuds et on calcule
le coût de passage de l’un à l’autre.
En pratique, on définit
c(i, j) = (|∇H(i) ∧ ∇H(j)| − α (d(j, B) − d(i, B)))−1 .
Stratégie globale
La stratégie gloutonne est locale: on cherche un algorithme global.
On considère les pixels de la carte comme des noeuds et on calcule
le coût de passage de l’un à l’autre.
En pratique, on définit
c(i, j) = (|∇H(i) ∧ ∇H(j)| − α (d(j, B) − d(i, B)))−1 .
Stratégie globale
La stratégie gloutonne est locale: on cherche un algorithme global.
On considère les pixels de la carte comme des noeuds et on calcule
le coût de passage de l’un à l’autre.
En pratique, on définit
c(i, j) = (|∇H(i) ∧ ∇H(j)| − α (d(j, B) − d(i, B)))−1 .
Algorithme global
On peut donc définir un nouvel algorithme
Pour tout (P, P 0 ) tels que
rmin ≤ d(P, P 0 ) ≤ rmax , d(P 0 , B) < d(P, B),
on calcule c(P, P 0 ).
On construit la matrice de coût du graphe.
On applique l’algorithme de Dijkstra et on obtient la
trajectoire du coût minimal entre A et B.
Croissance en N 2 (rmax − rmin )2 du coût de l’algorithme : trop lent
pour des cartes de taille elevée.
Autre version
C
Soit r ∈ (0, 1). Dans chaque sous partie C , sélectionner
l’ensemble des points P tels que
k∇h(P)k ≥ k∇hkL∞ (C ) (1 − r ).
Coût c(P, Q) = c({P, Q}) pour tout Q ∈ EP .
Résultats
Glouton
Dijkstra 1
Dijkstra 2
Résultats
Glouton
Dijkstra 1
Dijkstra 2
Sommaire
1
Présentation du problème
2
Algorithme de recalage
3
Optimisation de trajectoire
4
Pistes continues
5
Pistes probabilistes
6
Conclusion
7
Bibliographie et ouvertures
Modèle jouet
On cherche à aller sur des zones de grande pente:
c(γ) = −
Z 1
|∇h(γ(t))|2 dt
t=0
Mal posé (les suites minimisantes se localisent sur les maxima de
∇h) : régularisation ?
Z 1
c(γ) =
a|γ 0 (t)|2 − b|∇h(γ(t))|2 dt
t=0
Bien posé ? Minimiseurs ? Comment prendre en compte
l’heuristique que γ doit alterner les directions de ∇h pour se
localiser ?
Un modèle un peu plus complexe
Modèle effectif pour les incertitudes en x et y σx et σy :
c(γ) = σx (1) + σy (1), où
σ̇x = a − bσx (∂x h(γ(t)))2
σ̇y = a − bσy (∂y h(γ(t)))2 .
Amortissement exponentiel vers un point d’équilibre
σx ∼ 1/(∂y h(γ(t)))2 . Sous la forme générale d’un contrôle
optimal. Problème : modèle non isotrope (trajectoires
diagonales ?)
Généralisation : représenter l’incertitude par une matrice SDP A.
Si on pose G = ∇h∇hT ,
√ √
Ȧ = aA − b GA G.
On modifie les incertitudes dans la direction ∇h, on n’y touche pas
dans ∇h⊥ . Problème : A ne reste pas forcément SDP. Y a-t-il une
bonne généralisation ?
Sommaire
1
Présentation du problème
2
Algorithme de recalage
3
Optimisation de trajectoire
4
Pistes continues
5
Pistes probabilistes
6
Conclusion
7
Bibliographie et ouvertures
Modélisation des incertitudes
Sources d’incertitude : précision de l’accéléromètre, erreurs
d’intégration (fréquence finie), erreur numérique (stockage en
virgule flottante)
Dérive de l’estimation de position
Modélisation délicate
Si on fait une erreur initiale sur l’accélération, dérive en t 2
Si on fait une erreur initiale sur la vitesse, dérive en t
Compensation des erreurs ? Marche aléatoire, dérive en
t 3/2 ? t 5/2 ?
Non trivial en pratique
√
t ?
Modélisation probabiliste
Une approche par intervalles néglige la compensation des
erreurs et est trop pessimiste. Modèle probabiliste ?
Soit (x0 , x1 . . . , xN ) le chemin suivi par le sous-marin, supposé
exact.
Soit Xn la position estimée du sous-marin. Loi de Xn+1 ?
Sans bathymétrie, on met à jour la position avec le vecteur
vitesse vn = xn+1 − xn , entaché (pour simplifier) d’une erreur
aléatoire εv ∼ N(0, σv )
La bathymétrie nous donne h(Xn+1 ) = h(xn+1 ) + εh , avec une
erreur εh ∼ N(0, σh )
Xn+1 ∼ L(Xn + vn + εv h(Xn + vn + εv ) = h(xn+1 ) + εh )
Modélisation probabiliste
Xn+1 ∼ L(Xn + vn + εv h(Xn + vn + εv ) = h(xn+1 ) + εh )
En notant formellement P(X = x ) pour la densité de X en x ,
P(Xn+1 = x ) = P(Xn + vn + εv = x h(Xn + vn + εv ) = h(xn+1 ) + εh )
1
P(Xn + vn + εv = x ∩ h(Xn + vn + εv ) = h(xn+1 ) + εh
N
1
= P(Xn + vn + εv = x ) P(h(x ) = h(xn+1 ) + εh ),
N
=
où N est choisi pour normaliser Xn+1 .
On calcule P(h(x ) = h(xn+1 ) + εh ) (facile si εh est gaussienne), on
calcule P(Xn + vn + εv = x ) (translation et convolution), on
multiplie et on normalise. En théorie bien plus précis que
l’approche par boites, mais quel traitement des incertitudes de
vitesse ? Lien avec les filtres de Kalman ?
Sommaire
1
Présentation du problème
2
Algorithme de recalage
3
Optimisation de trajectoire
4
Pistes continues
5
Pistes probabilistes
6
Conclusion
7
Bibliographie et ouvertures
Conclusion (ce qu’on a fait)
Comprendre et “mathématiser” le problème:
Traduction du problème dans un langage mathématique grâce
aux précisions de l’encadrant.
Développement d’un algorithme de recalage pour affiner
l’intuition.
Traduction des données physiques (vitesse, carte
bathymétrique ) en conditions algorithmiques/mathématiques :
couronne d’admissibilité, choix d’une direction de fort gradient.
Proposition de deux algorithmes discrets testés sur les cartes
fournies :
Stratégie gloutonne, locale et incrémentale, d’après les
données physiques (gradient, distance, vitesse).
Stratégie globale basée sur un graphe décrivant les relations
entre des points d’intérêts de la carte. La trajectoire de coût
minimal est trouvée grâce à l’algorithme de Dijkstra.
Conclusion (pistes futures)
Modélisation continue (modèle intégral, contrôle d’équations
différentielles)
Modélisation probabiliste (calcul de lois)
Sommaire
1
Présentation du problème
2
Algorithme de recalage
3
Optimisation de trajectoire
4
Pistes continues
5
Pistes probabilistes
6
Conclusion
7
Bibliographie et ouvertures
Quelques pistes bibliographiques
S.Barkby, S.B. Williams O.Pizarro, M.Y. Jakuba (2006).
Amélioration d’une carte existante lors de la navigation.
Contrôle avec une méthode de SLAM et un filtre de Rao.
D.Wettergreen (2008). Repositionnement sous l’eau.
Découpe du volume à l’aide d’un octree, et filtre de Kalman.
E.Galceran & M.Carreras (2013) : article de review en CPP
(Coverage Path Planning). Déterminer le chemin passant par
les points d’un volume (une aire) tout en évitant les obstacles
éventuels. Algorithme de slicing. Applications en bathymétrie.
E. Galceran, R. Campos N.Palomeras, D. Ribas, M. Carreras
& P. Ridao (2014). Trajectoire pour la cartographie du
domaine. Séparation des régions selon le module du gradient.
Régions de gradient fort traitées par un algorithme de slicing,
autres régions traitées par l’algorithme de Boutrosphedon.
Merci pour votre attention !
et merci à Jérémy Nicola et aux organisateurs.