Tutorial on Fast Marching Method - Simon Fraser University

122/09/08LIST – DTSI – Service Robotique Interactive

Tutorial on Fast Marching Method

Application to Trajectory Planning for

Autonomous Underwater Vehicles

Clement Petres

Email: [email protected]

This work has been supported by the Ocean Systems Laboratory

http://www.eece.hw.ac.uk/research/oceans/

mailto:[email protected]

205/02/008DTSI LIST – DTSI – Service Robotique Interactive

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


Introduction: a short story

Costs

• Obstacles: 11

• Rocky ground: 2

• Free space: 1

• Wind: 0.5


4-connexity Breadth-First algorithm without obstacle (cost function = constant = 1)

Grid-search algorithms


Some demos (priority queue) …


4-connexity Breadth-First

8-connexity Breadth-First

4-connexity Fast Marching (Sethian, 1996)

Towards Fast Marching algorithm…


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*

10 20 30 40 50 60 70 80 90 100

10

20

30

40

50

60

70

80

90

100

A* = Breadth-First + heuristicFM* = Fast Marching + heuristic

A* versus 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


Non-convex obstacles: Fast Marching method

Cost = 0

Cost = 10

Cost map

Distance map (Fast Marching) and optimal trajectory (gradient descent)


Non-convex obstacles: dilatation + Fast Marching

Trajectory avoiding obstacle Safer trajectory avoiding dilated obstacle


Application: harbour obstructed by a net

Distance map computation Gradient descent computation

Harbour 2D simulation


Application: 3D trajectory planning

3D optimal trajectory using 3D Fast Marching algorithm










Anisotropic Fast Marching: theory

τ(x)u(x)

Eikonal equation

i

j

Upwind scheme

B

A

uu

uuu

Quadratic equation for u

22

B

2

A τuuuu

u) (x,τ~u(x)

Hamilton-Jacobi equation

Anisotropic cost function

u)(x,τ(x)τu) (x,τ~ FO

Quadratic equation for u

FO

2

F

2

O

2

B

2

A τ2τττuuuu

Q(x)

u(x).F(x)1 αu)(x,τF

Q(x)

(x)F uu(x)F uu1 αu)(x,τ

jBiA

F

linear for u :Fτ

Fsup 2ατ(x)Q(x) Ω 1Q(x)

u(x).F(x)Ω,x

so that


Isotropic Fast Marching


Anisotropic Fast Marching: simulation


Anisotropic Fast Marching


Anisotropic Fast Marching: simulation


Anisotropic Fast MarchingIsotropic Fast Marching

Gain = 10 %

Anisotropic Fast Marching: results










R : trajectory curvature radius

r : turning radius of the vehicle

Trajectory planning under curvature constraints


Metric (cost function t)

1,0

x,x21 ds (s)Cτ)x,ρ(x21

Trajectory

C(s)s

Ω0,1:C

startxC(0) endxC(1)

0τ.NR

τ

Euler-Lagrange equation

(Cohen and Kimmel, 1997)rτsup

τinfR

Ω

Ω

)x,ρ(xinf)x,u(x 212121 x,xc

Functional minimization problem

N

Smoothing t to decrease τsupΩ



KO

Smoothing

KO

Smoothing

FM

OK ?

FM

OK ?

FM

OK ?



Lower bounds on the curvature radius

Isotropic case

0x

F

y

F

Q

α.Nτ

R

ττ yiO

FO

r

F

Ω

OΩ

OΩ

JQinf

2ατsup

τinfR r

τsup

τinfR

Ω

Ω

u)(x,τ(x)τu) (x,τ~ FO

Q(x)

u(x).F(x)1 αu)(x,τF

Cost function

Euler-Lagrange equation

0τ.NR

τ

• Smoothing tO to decrease

• Smoothing the field of force F to decrease FJ

OΩ τsup

(Petres and Pailhas, 2005)










1) 1000x1000 grid of pixels

Operations:

2) FM on the grid

Multiresolution FM based trajectory planning


3) Mesh of 1400 vertices

2) Quadtree decomposition

4) FM on the mesh

Multiresolution FM based trajectory planning

1) 1000x1000 grid of pixels

Operations:


x3

x2

x4

x5

i

j

x1

x

22

B

2

A τuuuu

Cartesian grid

xA

xB

x

i

j

Unstructured mesh

i

ii

xx

xxT

2TT2T τQbbuQb2auQaa

n21 T,,T,TT 1TTTQ

i

ixx

1a

i

ii

xx

ub

n

2

1

a...

a

a

a

n

2

1

b...

b

b

b 321 u,u,uminu

Notations:

Multiresolution FM based trajectory planning: upwind scheme

(Sethian and Vladimirsky, 2000)


Trajectory using FM on the mesh

Trajectory using FM on the gridOriginal 1000x1000 image

Quadtree decompositionMesh with 1400 vertices

100 seconds

Recapitulative










E* algorithm

FM based trajectory planning in dynamic environments

Cost map

Updated grid points

(Philippsen and Siegwart, 2005)


Dynamic replanning: video of trials in Scotland










Visibility map: homogeneous media

Trajectory planning under visibility constraints

Cost map

Cost map

Shadow


Visibility map: hetrogeneous media


ShadowWithout obstacle With obstacle

Cost = 5

Cost = 5

Cost = 1

Cost = 1

Cost = 1

Cost = 5

Cost = 5

Cost = 5

Cost = 1

Cost = 1

Cost = 1

Cost = 5


1 sentry in the 4 corners

Cost of covered areas : 1

Cost of exposed areas : 10

Cost of obstacles : 100


Application in covert robotics


Conclusion

Main advantages of Fast Marching methods applied to trajectory planning

• Accuracy, robustness reliability

• Curvature constraints underactuated AUV

• Fields of force new domains of applicability

• Simplicity easy integration on real systems

Recap of the talk

• FM* algorithm: FM + heuristic

• Directional constrained TP: anisotropic FM

• Curvature constrained TP: isotropic and anisotropic media

• Multiresolution TP: FM + adaptive mesh generation

• Dynamic replanning: E* algorithm, comparative study, in-water trials

• Visibility-based TP: E* based visibility map, heterogeneous environments


• E. W. Dijkstra, « A Note on Two Problems in Connexion with Graphs »,Numerische Mathematik, vol. 1, pp. 269–271, 1959.

• P. E. Hart, N. J. Nilsson, B. Raphael, « A Formal Basis for the HeuristicDetermination of Minimum Cost Paths », IEEE Transactions on SystemsScience and Cybernetics, vol. 4(2), pp. 100–107, 1968.

• J. A. Sethian, « Level Set Methods and Fast Marching Methods »,Cambridge University Press, 1999.

References on chapter 1


• A. Vladimirsky, Ordered Upwind Methods for Static Hamilton-JacobiEquations: Theory and Algorithms, SIAM Journal on Numerical Analyzis,vol. 41(1), pp. 325-363, 2003.

• M. Soulignac, P. Taillibert, M. Rueher, « Adapting the WavefrontExpansion in Presence of Strong Currents », IEEE InternationalConference on Robotics and Automation, pp. 1352-1358, 2008.

• B. Garau, A. Alvarez, G. Oliver, « Path Planning of AUV in CurrentFields with Complex Spatial Variability: an A* approach », IEEEInternational Conference on Robotics and Automation, pp. 194-198,2005.



• L. D. Cohen, Ron Kimmel, « Global Minimum for Active ContourModels: A Minimal Path Approach », International Journal on ComputerVision, vol. 24(1), pp. 57-78, 1997.

• S. M. Lavalle, « Planning Algorithms », Cambridge University Press,2006.



• J. A. Sethian, A. Vladimirsky, « Fast Methods for the Eikonal andRelated Hamilton-Jacobi Equations on Unstructured Meshes », AppliedMathematics, vol. 97(11), pp. 5699-5703, 2000.

• D. Ferguson, A. Stentz, « Multi-Resolution Field D* », InternationalConference on Intelligent Autonomous Systems, 2006.

• A. Yahja, A. Stentz, S. Singh, B. Brumitt, « Framed-Quadtree PathPlanning for Mobile Robots Operating in Sparse Environments », IEEEInternational Conference on Robotics and Automation, vol. 1, pp. 650-655, 1998.



• R. Philippsen, R. Siegwart, « An Interpolated Dynamic NavigationFunction », IEEE International Conference on Robotics and Automation,pp. 3782-3789, 2005.

• D. Ferguson, A. Stentz, « Using Interpolation to Improve PathPlanning: The Field D* Algorithm », Journal of Field Robotics, vol. 23(2),pp. 79-101, 2006.

• A. Stentz, « Optimal and Efficient Path Planning for Partially-KnownEnvironment », IEEE International Conference on Robotics andAutomation, pp. 3310-3317, 1994.



• J. A. Sethian, « Level Set Methods and Fast Marching Methods »,Cambridge University Press, 1999.

• Y.-H. Tsai, L.-T. Cheng, S. Osher, P. Burchard, G. Sapiro, « Visibility andIts Dynamics in a PDE Based Implicit Framework », Journal ofComputational Physics, vol. 199(1), pp. 260-290, 2004.



• C. Pêtrès, Y. Pailhas, P. Patron, Y.Petillot, J. Evans, D. Lane, « PathPlanning for Autonomous Underwater Vehicles », IEEE Transactions onRobotics, vol. 23(2), pp. 331-341, 2007.

• C. Pêtrès, « Trajectory Planning for Autonomous UnderwaterVehicles », Ph.D. Thesis, Heriot-Watt University, Ocean SystemsLaboratory, Edinburgh, Scotland, 2007.

Main sources

Webpage: http://clement.petres.googlepages.com/

http://clement.petres.googlepages.com/

Documents

Tutorial on Fast Marching Method - Simon Fraser University