Motion Planning in Partially Known Dynamic Environments

Transcription

1 Motion Planning in Partially Known Dynamic Environments Movie Workshop Laas-CNRS, Toulouse (FR), January 7-8, 2005 Dr. Thierry Fraichard e-motion Team Inria Rhône-Alpes & Gravir-CNRS Laboratory Movie Workshop Laas-CNRS, Toulouse (FR), January 7-8, 2005 p.1/24

2 Motivation, Problem European IST Cybercars project [ ] Motion autonomy for: Robotic vehicles (wheeled, car-like), [high speed] Partially known dynamic environments: [fast] moving obstacles Movie Workshop Laas-CNRS, Toulouse (FR), January 7-8, 2005 p.2/24

3 Motion Planning Perspective Motion planning: a priori computation of a complete motion to the goal based upon models of the robotic system and its environment Fast robotic system: system dynamics Moving obstacles: knowledge of their future behaviour Model of the future: a priori known / predicted partially known environment Prediction: validity duration? Dynamic environment: limited response time (blind / fast / hostile moving obstacles) r = f(dynamicity) real-time constraint Movie Workshop Laas-CNRS, Toulouse (FR), January 7-8, 2005 p.3/24

4 State-Time Space Framework [Fraichard, 92] t q Moving obstacles: time Dynamics: state space q Movie Workshop Laas-CNRS, Toulouse (FR), January 7-8, 2005 p.4/24

5 State-Time Space Framework [Fraichard, 92] t T q s g Moving obstacles: time Dynamics: state space s 0 q Movie Workshop Laas-CNRS, Toulouse (FR), January 7-8, 2005 p.4/24

6 Previous Approaches t s g s 0 s 1. Reactive approaches: no lookahead convergence & safety problem [Vector Field Histogram, Dynamic Windows, Nearness Diagram... ] Movie Workshop Laas-CNRS, Toulouse (FR), January 7-8, 2005 p.5/24

7 Previous Approaches t s g t s g s 0 s s 0 s 1. Reactive approaches: no lookahead convergence & safety problem [Vector Field Histogram, Dynamic Windows, Nearness Diagram... ] 2. Planning approaches: time-consuming process real-time problem [Fraichard 92; Hsu 00; Bruce & Veloso 02; Van den Berg, 04] Movie Workshop Laas-CNRS, Toulouse (FR), January 7-8, 2005 p.5/24

8 Previous Approaches t s g t s g t s g s 0 s s 0 s s 0 s 1. Reactive approaches: no lookahead convergence & safety problem [Vector Field Histogram, Dynamic Windows, Nearness Diagram... ] 2. Planning approaches: time-consuming process real-time problem [Fraichard 92; Hsu 00; Bruce & Veloso 02; Van den Berg, 04] 3. Mixed approaches: Upgraded reactive approaches [VFH, Global DW, Global ND... ] Downgraded planning approaches [Frazzoli, 00; Large, 03] Movie Workshop Laas-CNRS, Toulouse (FR), January 7-8, 2005 p.5/24

9 Proposed Approach Argument: in a dynamic environment, motion planning must take into account the real-time constraint Observation: best motion planning techniques today are randomised (graph / tree based) no running-time upper-bound Solution: Iterative Partial Trajectory Planning (IPTP) 1. Get model of the future (a priori known / observation & prediction) 2. Expand exploration tree until r is over 3. Return best partial trajectory 4. Repeat until goal is reached Note: IPTP doubly required in a partially known environment Prediction validity duration (partial) Prediction update (iterative) Movie Workshop Laas-CNRS, Toulouse (FR), January 7-8, 2005 p.6/24

10 Iterative Partial Trajectory Planning Scheme Observation & Prediction model root (t 1 ) B( t 0, ) δ p0 modelb ( t 1, ) modelb ( t 2, ) modelb( t, n 1 ) root (t 2 ) root (t 3 ) root (t n ) (success) δp 1 δ p 2 PLANNING PLANNING PLANNING... δ pn 1 PLANNING (success) (goal) time φ 0 EXECUTION δ h1 φ 1 EXECUTION φ 2 EXEC. EXECUTION φ n 1 EXECUTION δ h2 δ h3 δ hn 1 t 0 δ c t1 δ 2 c t δc 3 t tn 1 δc tn δ t n+ 1 c step 0 step 1 step 2 step n-1 step n t f r Movie Workshop Laas-CNRS, Toulouse (FR), January 7-8, 2005 p.7/24

11 Iterative Partial Trajectory Planning Issues Same as reactive/mixed approaches, related to the fact that only a partial motion is computed: Convergence: will the system ever reach the goal? Safety: won t the system ever collide with an obstacle? Movie Workshop Laas-CNRS, Toulouse (FR), January 7-8, 2005 p.8/24

12 Iterative Partial Trajectory Planning Issues Same as reactive/mixed approaches, related to the fact that only a partial motion is computed: Convergence: will the system ever reach the goal? Safety: won t the system ever collide with an obstacle? Solutions proposed wrt safety Braking trajectory [reactive approaches] Evasive trajectory [Hsu 00; ATC literature] τ-safety [Frazzoli, 00; Large, 03] Movie Workshop Laas-CNRS, Toulouse (FR), January 7-8, 2005 p.8/24

13 Safety Issue Inevitable Collision State (ICS): state for which, no matter what the future trajectory followed by the system is, a collision will occur [Fraichard, 03] A : (x, y, v) v Wall Collision States Movie Workshop Laas-CNRS, Toulouse (FR), January 7-8, 2005 p.9/24

14 Safety Issue Inevitable Collision State (ICS): state for which, no matter what the future trajectory followed by the system is, a collision will occur [Fraichard, 03] A : (x, y, v) v d(v) Wall Collision States Inevitable Collision States Movie Workshop Laas-CNRS, Toulouse (FR), January 7-8, 2005 p.9/24

15 Safety Issue Inevitable Collision State (ICS): state for which, no matter what the future trajectory followed by the system is, a collision will occur [Fraichard, 03] A : (x, y, v) v d(v) Wall Collision States Inevitable Collision States v v S y S Inevitable collision states S x S d(v) Collision states Movie Workshop Laas-CNRS, Toulouse (FR), January 7-8, 2005 p.9/24

16 Inevitable Collision States General concept: robotic systems, fixed & moving obstacles Key to a robotic system safety: never ever end up in an inevitable collision state! Key to safe IPTP: ICS-free partial trajectories Main drawback: ICS characterisation is complex (infinite number of possible future trajectories), but... Conservative approximation property: by considering a subset I of the whole set of possible trajectories Movie Workshop Laas-CNRS, Toulouse (FR), January 7-8, 2005 p.10/24

17 Case Study: Car-Like System A : s = (x, y, θ, v), u = {u ξ, u v } v u ξ = ξ (steering angle): u ξ ξ max ξ θ u v (acceleration): u v u v max b F y R x Movie Workshop Laas-CNRS, Toulouse (FR), January 7-8, 2005 p.11/24

18 Case Study: Car-Like System v ξ θ A : s = (x, y, θ, v), u = {u ξ, u v } u ξ = ξ (steering angle): u ξ ξ max u v (acceleration): u v u v max b F Infinite number of possible trajectories φu Approximation property: y R I: φ u with constant steering angle u ξ x u ξ = 0, u v = 0 u ξ = 0, u v 0 u ξ 0, u v = 0 u ξ 0, u v 0 Movie Workshop Laas-CNRS, Toulouse (FR), January 7-8, 2005 p.11/24

19 Car-Like System: ICS Characterisation (1) B i d(v) B i B y A y A y A x x x Movie Workshop Laas-CNRS, Toulouse (FR), January 7-8, 2005 p.12/24

20 Car-Like System: ICS Characterisation (2) B B B y A y A y A x x x Movie Workshop Laas-CNRS, Toulouse (FR), January 7-8, 2005 p.13/24

21 Car-Like System: ICS Characterisation (3) A B θ = 0, v = 1 C Movie Workshop Laas-CNRS, Toulouse (FR), January 7-8, 2005 p.14/24

22 Iterative Partial Trajectory Planning Algorithm Exploration using a Rapidly Exploring Random Tree [Lavalle 98] s c s s n i δ s r Output trajectory: feasible, collision-free & ICS-free Movie Workshop Laas-CNRS, Toulouse (FR), January 7-8, 2005 p.15/24

23 IPTP Results: 1D Case Film Movie Workshop Laas-CNRS, Toulouse (FR), January 7-8, 2005 p.16/24

24 IPTP Results: 2D Case Film Movie Workshop Laas-CNRS, Toulouse (FR), January 7-8, 2005 p.17/24

25 Conclusion General motion planning technique designed for: Robotic systems, [high speed] Partially known dynamic environments, [fast] moving obstacles Iterative partial trajectory planning scheme: Real-time constraint Environment model update Safety through the Inevitable Collision State concept Movie Workshop Laas-CNRS, Toulouse (FR), January 7-8, 2005 p.18/24

26 Experimental Perspectives Movie Workshop Laas-CNRS, Toulouse (FR), January 7-8, 2005 p.19/24

27 Theoretical Perspectives Inevitable Collision States General characterisation Efficient computation Movie Workshop Laas-CNRS, Toulouse (FR), January 7-8, 2005 p.20/24

28 Moving Obstacles Future Behaviour Prediction Required for safety checking (collision, ICS) B( t 2 ) time t B( t 1 ) t 0 x ( 2 x 2, y ) moving obstacle model y ( x 0, y0) B( t 0 ) Long-term prediction desirable Movie Workshop Laas-CNRS, Toulouse (FR), January 7-8, 2005 p.21/24

29 Moving Obstacles Future Behaviour Prediction Key assumption: the moving obstacles have typical behaviours Two-step statistical approach [Vasquez & Fraichard 03] 1. Learning stage: determine the typical behaviours though observation 2. Prediction stage: determine an obstacle future behaviour through current vs typical behaviour matching Movie Workshop Laas-CNRS, Toulouse (FR), January 7-8, 2005 p.22/24

30 Moving Obstacles Future Behaviour Prediction Learning stage: clustering Movie Workshop Laas-CNRS, Toulouse (FR), January 7-8, 2005 p.23/24

31 Moving Obstacles Future Behaviour Prediction Prediction stage: film, film Movie Workshop Laas-CNRS, Toulouse (FR), January 7-8, 2005 p.24/24