Optimal Path Planning under Different Norms in …kalton/presentations/icra2006.pdfOptimal Path...

Optimal Path Planning under Different Norms in Continuous

State SpacesKen Alton and Ian M. Mitchell

Department of Computer ScienceUniversity of British Columbia

A Typical Path Planning Example

• A robot navigating through known corridors of a building to a goal

Outline

• We investigate use of optimality metrics other than Euclidean distance

• We compare Dijkstra’s Algorithm with the Fast Marching Method, which is better suited for path planning in continuous spaces

• We compare the use of an adaptive simplicial grid to that of a uniform orthogonal grid

Optimal Path Planning in a Continuous Domain

Value or cost-to-go function:

Dynamic programming principal:

• Constraint on robot action:

• Eikonal equation:

• Let ∆t go to 0 (Hamilton-Jacobi-Bellman equation):

Optimal Path Planning in a Continuous Domain (2)

A Typical Path Planning Example

• The robot is constrained by the Euclidean distance it can travel in a time instant

A Robot Arm Example

• Each joint can be actuated independently at maximum speed

Related Work

• R. Kimmel and J. A. Sethian, “Optimal algorithm for shape from shading and path planning,” 2001.

• K. Konolige, “A gradient method for realtime robot control,” 2000.

• S. M. LaValle, “Numerical computation of optimal navigation functions on a simplicial complex,” 1998.

• J. Rosell and P. Iniguez, “Path planning using harmonic functions and probabilistic cell decomposition,” 2005.

Generic Shortest Path Dynamic Programming Algorithm

Dijkstra’s Algorithm –Update Function

• To find shortest paths on a discrete graph, use generic dynamic programming with the update equation:

• Values and actions are not defined for states that are not nodes in the discrete graph

• Actions only include those corresponding to edges leading to neighboring states

• Interpolation of actions to points that are not grid nodes may not lead to actions optimal under

Dijkstra’s Algorithm and Continuous Path Planning: Some Concerns

Dijkstra’s and Continuous Path Planning

−0.5 0 0.5

−0.8

−0.6

−0.4

−0.2

0

0.2

0.4

0.6

0.8

−0.5 0 0.5

−0.8

−0.6

−0.4

−0.2

0

0.2

0.4

0.6

0.8

−0.5 0 0.5

−0.8

−0.6

−0.4

−0.2

0

0.2

0.4

0.6

0.8

Fast Marching Method -Update Function

Fast Marching Method

• Finite difference approximation for the gradient of V in:

• E.g., for p = 2:

• Solve the quadratic to get:

Fast Marching Method – Other Norms

Dijkstra vs. Fast Marching Method

• Dijkstra: solid contours and trajectories• FMM: dashed contours and trajectories

Adaptive Simplicial Mesh

• J. M. Maubach, 1995.• J. Sethian and A. Vladimirsky, 2000.

Simplicial vs. Orthogonal GridDijkstra’s vs. Fast Marching

Mixed Norms – Two Robots• Red robot may move in any direction in 2D• Blue robot constrained to 1D circular path• Cost encodes black obstacles and collision

states• 2D robot action constrained in ||····||2 and

combined action of both robots in ||····||∞∞∞∞

Conclusions

• The Fast Marching Method is like Dijkstra’s but with an update formula better suited for continuous spaces

• Update equations in various norms were developed for both algorithms

• Dijkstra’s Algorithm and the Fast Marching Method benefit from refinement of the grid near obstacles

Thank you