HighSpeed Autonomous Navigation with Motion Prediction for Unknown Moving Obstacles

1
High-Speed Autonomous Navigation with Motion
Prediction for Unknown Moving Obstacles

• Dizan Vasquez, Frederic Large,
• Thierry Fraichard and Christian Laugier
• INRIA Rhône-Alpes Gravir Lab. France
• IROS 2004

2
Objective

• To design techniques allowing a vehicle to
  navigate in an environment populated with moving
  obstacles whose future motion is unknown.
• Two constraints
• Limited response time f(Dynamicity).
• Need of reasoning about the future Prediction.
• Prediction Validity?

3
Autonomous NavigationApproaches

• Reactive approaches Arkins, Simmons, Borenstein,
  etc.
• No look-ahead
• Improved reactive approaches Khatib, Montano,
  Ulrich, etc.
• Lack of generality
• Iterative planning approaches Hsu, Veloso
• Too slow for highly dynamic environment
• Iterative partial planning Fraichard, Frazzoli,
  Petti

4
Autonomous NavigationProposed Solution

• Iterative partial planning approach
• Fast Motion Planning. The concept of Velocity
  Obstacle Fiorini, Shiller is used in an
  iterative motion planner which proposes a safe
  plan for a given time interval.
• Motion prediction for Moving obstacles. Typical
  behavior of moving obstacles is learned and then
  applied for motion prediction.

5
Motion PlanningPrinciple

• Iterative planner. Plans computed during a given
  time interval.
• Incremental calculation of a partial trajectory.
• Uses a model of the future (prediction).
• Based on the A algorithm.
• Uses the Non Linear Velocity Obstacle concept to
  speed up the calculation Large, Shiller
• Real Time.
• Adapts to changes.

6
Motion PlanningVelocity Obstacles

• A NLVO is the set of all the linear velocities of
  the robot that are constant on a given interval
  and that induce a collision before
  .

7
Motion PlanningA implementation

• Nodes Dated states.
• Link Motion (velocities).
• Velocities expanded with a two criteria
  heuristic
• Time to Collision cost
• Time to Goal cost

8
Motion PlanningUpdating the Tree

• Instead of rebuilding the tree at each step, we
  update it.
• Past configuration are pruned excepting for the
  currently open node.
• If any collision is detected, another node is
  chosen in the remaining tree, and explored from
  the root.

9
Motion PredictionTraditional Approaches

• Motion Equations and State Estimation
• Example

Zhu90

• Fast.
• Easy to Implement.
• Estimate and . Kalman60
• Short Time Horizon.
• Equations are not general (intentional
  behaviour?).

10
Motion PredictionLearning-Based Approaches

• Hypothesis On a given environment, objects do
  not move randomly but follow a pattern.
• Steps
• Learning.
• Prediction.
• General.
• Long Time Horizon.
• Real-Time Capability.
• Prediction of unobserved behaviors.
• Unstructured Environments

TadokoroEtAl95 KruseEtAl96 BennewitzEtAl02
11
Motion PredictionProposed Approach

• The approach we propose is defined by
• A similarity measure.
• Use of pairwise clustering algorithms.
• A cluster representation.
• Calculation of probability of belonging to a
  cluster.

12
Motion PredictionLearning Stage

• Dissimilarity
• Measure

Observed Trajectories
Dissimilarity Matrix
Cluster Mean Values and Std. Dev.
2. Pairwise Clustering
Algorithm
3. Calculation Of Cluster Representation
Trajectory Clusters
13
Motion PredictionDissimilarity Measure
q
di
.
dj
t
Ti
Tj
14
Motion PredictionCluster Representation

• Cluster Mean-Value
• Cluster Standard Deviation

15
Motion PredictionPrediction Stage

• The probability of belonging to a cluster is
  modeled as a Gaussian
• Where
• Prediction Maximum likelihood or sampling

16
Motion PredictionExperimental Results

• Implementation using Complete-Link Hierarchical
  Clustering and Deterministic Annealing
  Clustering.
• Benchmark using Expectation-Maximization
  Clustering as described in Bennewitz02

17
Motion PredictionExperimental Results

• Evaluation using a performance measure.
• Tests ran with simulated data.

18
Motion PlanningResults

• Experiments have been performed in a simulated
  environment.

19
Conclusions

• In this paper a navigation approach is proposed.
  It consists of two components
• A learning-based motion prediction technique
  able to produce long-term motion estimates.
• An iterative motion planner based on the concept
  of Non-Linear Velocity Obstacle which adapts its
  scope according to available time.

20
Perspectives

• Work in a real system installed in the
  laboratorys parking.
• Research on unknown behaviors prediction.

21
Thank You!
22
PWE Calcul du Nombre de Clusters
23
Résultats Expérimentaux Génération de
lensemble dentraînement (cont)

• Les points correspondant aux points de control
  sont génères en utilisant des distributions
  gaussiennes avec un écart type fixe.
• Le mouvement a été simulé en avançant en pas
  fixes depuis le dernier point de control dans la
  direction du prochain daccord a une distribution
  gaussienne. On considère avoir arrivé dans le
  prochain point de control quand on est plus près
  quun certain seuil.
• Le pas 2 es répété jusquà on arrive au dernier
  point de control.

24
Quelques Concepts Importantes

• Configuration.
• Mouvement.
• Estimation de Mouvement.
• Horizon Temporelle.

25
PWE Deterministic Annealing

• Lappartenance dans un cluster est
  calculée de façon itérative
• INITIALISER et AU HAZARD
• température T?T0
• WHILE TgtTfinal
• s?0
• REPEAT
• Estimation Calculer en fonction de
• Maximisation Calculer a partir de
• s?s1
• UNTIL tous ( , ) convergent
• T??T ? ?
• END

26
Experimental ResultsPerformance Measure
27
Experimental Results Learning stage results
28
Experimental Results Learning stage results
Résultats Expérimentaux
29
Experimental Results Cluster Examples
30
ConclusionsContributions

• We have proposed an approach based on three
  calculations
• Dissimilarity Measure.
• Cluster Mean-Value.
• Probability of Belonging to a Cluster.

31
ConclusionsContributions (cont)

• We have implemented our approach using
  Complete-Link and Deterministic Annealing
  Clustering
• We have implemented the approach presented on
  Bennewitz 02
• According to our performance measure, our
  technique has a better performance than that
  based on Estimation-Maximization.

32
PWE Comparaison avec EME

• PWE
• Trouve les groupes et leur représentations en
  deux pas.
• Calcule la valeur de K avec lalgorithme
  Complete-Link.
• Peut utiliser tous les algorithmes Pairwise
  Clustering.
• Représente les clusters avec la trajectoire
  moyenne.

• EME
• Trouve les groupes et leur représentations
  simultanément.
• Calcule la valeur de K avec un algorithme
  incrémental.
• Utilise lalgorithme Expectation-Maximization
• Représente les clusters avec des distributions
  gaussiennes.

33
Estimation basé sur EM (EME)

• Nous considérons cette technique Bennewitz 02
  comme létat de lart pour notre problème
• Apprentissage
• Trouve les groupes et ses représentations
  (séquences de gaussiennes) simultanément.
• Utilise lalgorithme EM (Expectation-Maximization)
  
• Trouve le nombre de clusters en utilisant un
  algorithme incrémental.
• Estimation
• Basé sur le calcul de la vraisemblance dune
  trajectoire partielle observé opartial sous
  chaque un des chemins ?k comme une multiplication
  de probabilités.

34
Estimation basé sur EM (EME)Algorithme EM

• Calcule les assignations cik et les chemins ?k
• Expectation Calcule la valeur espéré Ecik sous
  les chemins courants ?k.
• Maximization Assume que cik Ecik et calcule
  des nouveaux chemins ?k
• Fait ?k?k et recommence

.
.
.
.
.
.
.
?k2
?k10
?k1
35
Estimation basé sur EM (EME)Estimation

• La vraisemblance dune trajectoire di sous un
  chemin ?k est

di5
.
.
.
.
di2
.
di1
.
.
?k2
?k10
?k1
36
Résultats ExpérimentauxMesure de Performance

• Fonction PerformanceMetric( ?,C,percentage)
  result?0
• FOR chaque trajectoire ?i in the test set ? DO
  calculate ?ipercentage
• trouver le cluster Ck ayant la majeur
  vraisemblance pour ?ipercentage
• result?resultd(?i,µk)
• END FOR
• result? result/N?

37
Estimation basé sur EM (EME)Avantages /
Inconvénients

• Horizons Temporelles Longs
• Ils ne fait pas de suppositions par rapport a la
  forme des trajectoires
• Il estime le nombre de clusters
• Il nest pas capable de prédire des trajectoires
  quil na jamais observé.