Tutorial on Fast Marching Method Application to Trajectory Planning - PowerPoint PPT Presentation

Tutorial on Fast Marching Method Application to Trajectory Planning for Autonomous Underwater Vehicles Clement Petres Email: clementpetres@yahoo.fr This work has been supported by the Ocean Systems Laboratory http://www.eece.hw.ac.uk/research/oceans/ LIST – DTSI – Service Robotique Interactive 1 22/09/08

Presentation overview I. Introduction to FM based trajectory planning II. Trajectory planning under directional constraints III. Trajectory planning under curvature constraints IV. Multiresolution FM based trajectory planning V. FM based trajectory planning in dynamic environments VI. Trajectory planning under visibility constraints DTSI TSI LIST – DTSI – Service Robotique Interactive 2 05/02/008

Introduction: a short story Costs • Obstacles: 11 • Rocky ground: 2 • Free space: 1 • Wind: 0.5 DTSI TSI LIST – DTSI – Service Robotique Interactive 3 05/02/008

Grid-search algorithms 4-connexity Breadth-First algorithm without obstacle (cost function = constant = 1) DTSI TSI LIST – DTSI – Service Robotique Interactive 4 05/02/008

Some demos (priority queue) … DTSI TSI LIST – DTSI – Service Robotique Interactive 5 05/02/008

Towards Fast Marching algorithm… 4-connexity Breadth-First 4-connexity Fast Marching (Sethian, 1996) 8-connexity Breadth-First DTSI TSI LIST – DTSI – Service Robotique Interactive 6 05/02/008

A* versus FM* A* = Breadth-First + heuristic FM* = Fast Marching + heuristic 100 100 90 90 80 80 70 70 60 60 50 50 40 40 30 30 20 20 10 10 10 20 30 40 50 60 70 80 90 100 10 20 30 40 50 60 70 80 90 100 4-connexity A* (Nilsson, 1968) 4-connexity FM* On a grid with N points: FM complexity = A* complexity = O(N.log(N)): • Maximum cost of sorting the queue: log(N) • Maximum number of iterations: N DTSI TSI LIST – DTSI – Service Robotique Interactive 7 05/02/008

Non-convex obstacles: Fast Marching method Cost = 0 Cost = 10 Cost map Distance map (Fast Marching) and optimal trajectory (gradient descent) DTSI TSI LIST – DTSI – Service Robotique Interactive 8 05/02/008

Non-convex obstacles: dilatation + Fast Marching Trajectory avoiding obstacle Safer trajectory avoiding dilated obstacle DTSI TSI LIST – DTSI – Service Robotique Interactive 9 05/02/008

Application: harbour obstructed by a net Harbour 2D simulation Distance map computation Gradient descent computation DTSI TSI LIST – DTSI – Service Robotique Interactive 10 05/02/008

Application: 3D trajectory planning 3D optimal trajectory using 3D Fast Marching algorithm DTSI TSI LIST – DTSI – Service Robotique Interactive 11 05/02/008

Presentation overview I. Introduction to FM based trajectory planning II. Trajectory planning under directional constraints III. Trajectory planning under curvature constraints IV. Multiresolution FM based trajectory planning V. FM based trajectory planning in dynamic environments VI. Trajectory planning under visibility constraints DTSI TSI LIST – DTSI – Service Robotique Interactive 12 05/02/008

Anisotropic Fast Marching: theory Eikonal equation Hamilton-Jacobi equation ~  u(x)     τ(x) τ u(x) (x, u) Anisotropic cost function Upwind scheme ~     τ τ τ (x, u) (x) (x, u)   O F    u u   A    u      u u   B    u(x).F(x)      τ F α (x, u) 1     Q(x)      u(x).F(x)      τ(x) 2 α Ω, Q(x) sup F x 1 so that Ω Q(x) τ linear for u : F           j u u F (x) u u F (x)      A i B j τ α (x, u) 1   F   Q(x) i Quadratic equation for u Quadratic equation for u                   2 2 τ 2 2 τ 2 τ 2 2 τ τ 2 u u u u u u u u A B A B O F O F DTSI TSI LIST – DTSI – Service Robotique Interactive 13 05/02/008

Anisotropic Fast Marching: simulation Isotropic Fast Marching Distance map computation Gradient descent computation DTSI TSI LIST – DTSI – Service Robotique Interactive 14 05/02/008

Anisotropic Fast Marching: simulation Anisotropic Fast Marching Distance map computation Gradient descent computation DTSI TSI LIST – DTSI – Service Robotique Interactive 15 05/02/008

Anisotropic Fast Marching: results Anisotropic Fast Marching Isotropic Fast Marching Gain = 10 % DTSI TSI LIST – DTSI – Service Robotique Interactive 16 05/02/008

Presentation overview I. Introduction to FM based trajectory planning II. Trajectory planning under directional constraints III. Trajectory planning under curvature constraints IV. Multiresolution FM based trajectory planning V. FM based trajectory planning in dynamic environments VI. Trajectory planning under visibility constraints DTSI TSI LIST – DTSI – Service Robotique Interactive 17 05/02/008

Trajectory planning under curvature constraints R : trajectory curvature radius r : turning radius of the vehicle DTSI TSI LIST – DTSI – Service Robotique Interactive 18 05/02/008

Trajectory planning under curvature constraints Trajectory    Ω 0,1 C(0)  C(1)  x x C :  start end s C(s) Metric (cost function t )   N   ρ(x τ , x ) C (s) ds   1 2 x , x 1 2 0 , 1 Functional minimization problem  ρ(x   u(x , x ) inf , x ) c 1 2 1 2 x 1 x , 2 Euler-Lagrange equation τ    τ.N 0 R τ inf   Ω R r (Cohen and Kimmel, 1997)  τ sup Ω sup Ω  τ Smoothing t to decrease DTSI TSI LIST – DTSI – Service Robotique Interactive 19 05/02/008

Trajectory planning under curvature constraints KO KO Smoothing Smoothing OK ? OK ? OK ? FM FM FM DTSI TSI LIST – DTSI – Service Robotique Interactive 20 05/02/008

Lower bounds on the curvature radius Cost function ~     τ τ τ (x, u) (x) (x, u) O F    u(x).F(x)      τ F α (x, u) 1     Q(x) Euler-Lagrange equation Isotropic case      τ τ τ α F F           τ.N τ y O F i 0 .N 0     O   R R Q y x (Petres and Pailhas, 2005) τ τ inf inf     Ω Ω O R R r r  2 α τ sup Ω   τ sup J Ω  O F inf Q Ω  • Smoothing t O to decrease τ sup Ω O • Smoothing the field of force F to decrease J F  DTSI TSI LIST – DTSI – Service Robotique Interactive 21 05/02/008

Presentation overview I. Introduction to FM based trajectory planning II. Trajectory planning under directional constraints III. Trajectory planning under curvature constraints IV. Multiresolution FM based trajectory planning V. FM based trajectory planning in dynamic environments VI. Trajectory planning under visibility constraints DTSI TSI LIST – DTSI – Service Robotique Interactive 22 05/02/008

Multiresolution FM based trajectory planning Operations: 1) 1000x1000 grid of pixels 2) FM on the grid DTSI TSI LIST – DTSI – Service Robotique Interactive 23 05/02/008

Multiresolution FM based trajectory planning Operations: 1) 1000x1000 grid of pixels 2) Quadtree decomposition 3) Mesh of 1400 vertices 4) FM on the mesh DTSI TSI LIST – DTSI – Service Robotique Interactive 24 05/02/008

Multiresolution FM based trajectory planning: upwind scheme Cartesian grid j     x     2 2 τ x A 2 u u u u A B i x B Unstructured mesh x 1        τ T 2 T T 2 a Qa u 2a Qb u b Qb x 2 (Sethian and Vladimirsky, 2000) x x 5 j Notations: i      x x  1    T T i  Q T T T , T , , T T  x 3 i 1 2 n x 4 x x i     a b     1 1 1 u a b            i u  a a b b 2 2 min u , u , u   i i  ...   ...  x x x x     1 2 3 i i     a b n n DTSI TSI LIST – DTSI – Service Robotique Interactive 25 05/02/008

Recapitulative 100 seconds Trajectory using FM on the grid Original 1000x1000 image Quadtree decomposition Mesh with 1400 vertices Trajectory using FM on the mesh DTSI TSI LIST – DTSI – Service Robotique Interactive 26 05/02/008

Presentation overview I. Introduction to FM based trajectory planning II. Trajectory planning under directional constraints III. Trajectory planning under curvature constraints IV. Multiresolution FM based trajectory planning V. FM based trajectory planning in dynamic environments VI. Trajectory planning under visibility constraints DTSI TSI LIST – DTSI – Service Robotique Interactive 27 05/02/008

FM based trajectory planning in dynamic environments E* algorithm (Philippsen and Siegwart, 2005) Cost map Updated grid points DTSI TSI LIST – DTSI – Service Robotique Interactive 28 05/02/008

Dynamic replanning: video of trials in Scotland DTSI TSI LIST – DTSI – Service Robotique Interactive 29 05/02/008