1 High-Speed Autonomous Navigation with Motion Prediction for Unknown Moving Obstacles Dizan Vasquez, Frederic Large, Thierry Fraichard and Christian Laugier.

1

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

Dizan Vasquez, Frederic Large, Thierry Fraichard and Christian LaugierINRIA 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 Navigation:Approaches

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 Navigation:Proposed 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 Planning:Principle

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 Planning:Velocity 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 .TH

],[ 0 THt

})()(],,[|{ 0 tBtATHttVvNLVO i

7

Motion Planning:A* implementation

Nodes: Dated states. Link: Motion (velocities). Velocities expanded with a

two criteria heuristic: 1. Time to Collision cost :

2. Time to Goal cost:

]1,0[)( vCosttc

]1,0[)( vCostopt

8

Motion Planning:Updating 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 Prediction:Traditional Approaches

Motion Equations and State Estimation

Example dqqtq

T

0

)()0()(

dqqtqT

0

)()0()(

Fast.Easy to Implement.Estimate and . [Kalman60]

Short Time Horizon.Equations are not general (intentional behaviour?).

dqqtqT

0

)()0()(

dqqtqT

0

)()0()(

dqqtqT

0

)()0()(

dqqtqT

0

)()0()(

[Zhu90]

10

Motion Prediction:Learning-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 Prediction:Proposed 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 Prediction:Learning Stage

1. DissimilarityMeasure

Observed Trajectories

DissimilarityMatrix

2. Pairwise Clustering Algorithm

3. CalculationOf Cluster

Representation

TrajectoryClusters

ClusterMean Valuesand Std. Dev.

13

Motion Prediction:Dissimilarity Measure

2/1),max(

0

2)()(),max(

1),(

dttdtdTT

ddji TT

t

jiji

ji

t

q

.

di

dj

Ti Tj

14

Motion Prediction:Cluster Representation

Cluster Mean-Value:

Cluster Standard Deviation:

kN

ii

kk td

Nt

1

)(1

)(

2/1

1

2),(1

kN

iki

kk d

N

15

Motion Prediction:Prediction Stage

The probability of belonging to a cluster is modeled as a Gaussian:

Where:

Prediction: Maximum likelihood or sampling

2/1

0

2)()(1

),(

dttdtoT

ddpartialT

t

ipartialpartial

jipartial

22

),(2

1

2

1)|(

kpartialpartial o

k

kpartial eCoP

16

Motion Prediction:Experimental Results

Implementation using Complete-Link Hierarchical Clustering and Deterministic Annealing Clustering.

Benchmark using Expectation-Maximization Clustering as described in [Bennewitz02]

17

Motion Prediction:Experimental Results

Evaluation using a performance measure.

Tests ran with simulated data.

18

Motion Planning:Results

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 laboratory’s parking.

Research on unknown behavior’s prediction.

21

Thank You!

22

PWE: Calcul du Nombre de Clusters

23

Résultats Expérimentaux: Génération de l’ensemble d’entraînement (cont…)

1. Les points correspondant aux points de control sont génères en utilisant des distributions gaussiennes avec un écart type fixe.

2. Le mouvement a été simulé en avançant en pas fixes depuis le dernier point de control dans la direction du prochain d’accord a une distribution gaussienne. On considère avoir arrivé dans le prochain point de control quand on est plus près qu’un certain seuil.

3. 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

L’appartenance dans un cluster est calculée de façon itérative:

INITIALISER et AU HAZARD;température T←T₀;WHILE T>Tfinal

s←0; REPEAT

Estimation: Calculer en fonction de ;

Maximisation: Calculer a partir de ; s←s+1;

UNTIL tous ( , ) convergent;

T←ηT; ← ; ← ;END;

1siM

si*

1* si

s

iM

0* i

0

iM

1siM

1* si

1siM

0* i0

iMsi*

iM

26

Experimental Results:Performance Measure

Test Trajectory 1. Select StartingFraction

Tra

jec

tory

Fra

cti

on

Cluster Set 2. Select Cluster with Max

Likelihood

Error Value

Cluster Mean

3. CalculateDistance

Test Trajectory

27

Experimental Results: Learning stage results

Alg.. Parameter Number

Clusters

CL maxDistance=40cm

59

CL maxDistance=30cm 101

CL maxDistance=20cm 205

DA K=59 59

DA K=101 99

DA K=205 138

EM σ=20cm 36

EM σ=15cm 53

EM σ=10cm 133

28

Experimental Results: Learning stage results

Résultats Expérimentaux:

29

Experimental Results: Cluster Examples

30

Conclusions:Contributions

We have proposed an approach based on three calculations:

Dissimilarity Measure. Cluster Mean-Value. Probability of Belonging to a Cluster.

31

Conclusions:Contributions (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 l’algorithme 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 l’algorithme 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 l’art pour notre problème:Apprentissage:

Trouve les groupes et ses représentations (séquences de gaussiennes) simultanément.

Utilise l’algorithme EM (Expectation-Maximization) Trouve le nombre de clusters en utilisant un algorithme

incrémental.

Estimation: Basé sur le calcul de la vraisemblance d’une trajectoire

partielle observé opartial sous chaque un des chemins θk comme une multiplication de probabilités.

T

t

ti

tiki dPdP

1

)|()|(

34

Estimation basé sur EM (EME):Algorithme EM

Calcule les assignations ci

k et les chemins θk

1. Expectation: Calcule la valeur espéré E[ci

k] sous les chemins courants θk.

2. Maximization: Assume que ci

k= E[cik] et

calcule des nouveaux chemins θ’k

3. Fait θk=θ’k et recommence

θk1

θk2 θk

10

..

..

.

.

.

35

Estimation basé sur EM (EME):Estimation

La vraisemblance d’une trajectoire di sous un chemin θk est:

θk1

θk2 θk

10

..

..

.

.

.di1

di2

di5

T

t

ti

tiki dPdP

1

)|()|(

36

Résultats Expérimentaux:Mesure de Performance

Fonction PerformanceMetric( χ,C,percentage) result←0;

FOR chaque trajectoire χi in the test set χ DO calculate χi

percentage;

trouver le cluster Ck ayant la majeur vraisemblance pour χi

percentage;

result←result+δ(χ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 n’est pas capable de prédire des trajectoires qu’il n’a jamais observé.