optimal path planning under different norms in …kalton/presentations/icra2006.pdfoptimal path...
TRANSCRIPT
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