Upload
rosaire-fremont
View
105
Download
1
Tags:
Embed Size (px)
Citation preview
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é.