the university of north carolina at chapel hill constraint-based motion planning using voronoi...
Post on 21-Dec-2015
213 views
TRANSCRIPT
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Constraint-Based Motion Planning using Voronoi Diagrams
Maxim Garber and Ming C. LinDepartment of Computer Sciencehttp://gamma.cs.unc.edu/cplan/
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Introduction
• A motion planning method ♦ for rigid and articulated objects♦ in dynamic environments♦ using Voronoi Diagrams
• Allowing incorporation of various geometric, physical and mechanical constraints
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Previous Work
• Roadmap Based Planning♦ Randomized
• PRM: Kavraki & Latombe 1994, Kavraki et al. 1996
• OBPRM: Amato et al. 1998• MAPRM: Wilmarth et al. 1999
♦ Voronoi Based• Ó Dúnlaing 1983• Choset et al. 1995, 1996• vPlan: Foskey et al. 2001
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Previous Work
• Motion Planning in Dynamic Environments♦ Artificial Potential Fields
• Khatib 1986
♦ Industrial Applications• Ahrentsen et al. 1997
♦ Using Graphics Hardware• Hoff et al. 1999
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Previous Work
• Voronoi Diagrams in Motion Planning♦ Voronoi Graph
• Ó Dúnlaing 1983• Choset et al. 1995, 1996• vPlan: Foskey et al. 2001
♦ Random Sampling • Pisula et al. 2000• MAPRM: Wilmarth et al. 1999
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Basic ApproachCharacteristics:
• Reactive Planning -- handling dynamic scenes and moving obstacles/robots
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Basic ApproachCharacteristics:
• Reactive Planning -- handling dynamic scenes and moving obstacles/robots
• Estimated Roadmap -- providing global information through estimated paths
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Basic ApproachCharacteristics:
• Reactive Planning -- handling dynamic scenes and moving obstacles/robots
• Estimated Roadmap -- providing global information through estimated paths
• Voronoi Diagrams -- capturing a useful characterization of workspace
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Basic ApproachCharacteristics:
• Reactive Planning -- handling dynamic scenes and moving obstacles/robots
• Estimated Roadmap -- providing global information through estimated paths
• Voronoi Diagrams -- capturing a useful characterization of workspace
…… combine these in a general and extensible constraint-based motion planning framework
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Framework Objectives
• Portable♦ Handle rigid, articulated, and
deformable (future work) robots
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Framework Objectives
• Portable♦ Handle rigid, articulated, and
deformable (future work) robots
• Dynamic♦ Allow scenes with dynamic obstacles
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Framework Objectives
• Portable♦ Handle rigid, articulated, and
deformable (future work) robots
• Dynamic♦ Allow scenes with dynamic obstacles
• General♦ Allow a wide range of relationships
between objects to be specified
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Planning Framework
• Formulate motion planning as a constrained dynamical system
• Introduce both hard and soft constraints ♦ guide the robot(s) to their goal(s)♦ avoiding collision with other robot(s)
and obstacles
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Framework Example• Environment
contains obstacles
• The obstacles may be dynamic
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
• The robot is a collection of rigid objects
• Each rigid object has state:
• position
• rotation
• linear velocity
• angular velocity
Framework Example
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
• The objects are subject to various constraints.
• Constraints that define the problem:
• Non-Penetration
Framework Example
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
• The objects are subject to various constraints.
• Constraints that define the problem:
• Non-Penetration
• Joint Connectivity
Framework Example
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
• The objects are subject to various constraints.
• Constraints that define the problem:
• Non-Penetration
• Joint Connectivity
• Joint Angle Limits
Framework Example
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
• Given a planning goal
• Define constraints that encourage planning behavior:
Framework Example
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
• Given a planning goal
• Define constraints that encourage planning behavior:
• Estimated Path
Framework Example
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
• Given a planning goal
• Define constraints that encourage planning behavior:
• Estimated Path
• Obstacle Avoidance
Framework Example
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Simulation Loop:
Framework Example
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Simulation Loop:
• Update Obstacles
Framework Example
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Simulation Loop:
• Update Obstacles
Framework Example
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Simulation Loop:
• Update Obstacles
• Apply Planning Constraints
Framework Example
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Framework ExampleSimulation Loop:
• Update Obstacles
• Apply Planning Constraints
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Simulation Loop:
• Update Obstacles
• Apply Planning Constraints
• Enforce Problem Constraints
Framework Example
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Simulation Loop:
• Update Obstacles
• Apply Planning Constraint Forces
• Enforce Problem Constraints
• Repeat Until Goal is Achieved
Framework Example
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
General Framework
Simulation Loop
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
General Framework
Simulation Loop
Robots, Obstacles, Goals
…
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
General Framework
Simulation Loop
Robots, Obstacles, Goals
…
Constraints
C1
C2
C3…
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
General Framework
q
qCEf ic
))((
Simulation Loop
INPUT: Robots, Obstacles, Goals
…
Constraints
C1
C2
C3…
q
qCEf ic
))((Constraint Force Energy Function
)()())((21 qCqCkqCE iisi
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
General Framework
Simulation Loop
Robots, Obstacles, Goals
…
Constraints
C1
C2
C3
…
Constraint
Solvers
S1
S2
S3…
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
General Framework
Simulation Loop
Robots, Obstacles, Goals
…
Constraints
C1
C2
C3
…
Constraint
Solvers
S1
S2
S3… Run Simulation
Planned Path
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Types of Constraints
• Hard Constraints
• Soft Constraints
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Hard Constraints
• Must be enforced throughout the entire simulation
• Solved using Gauss-Seidel Iteration
• Examples:♦ object non-penetration ♦ joint connectivity♦ joint angle limits
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Gauss-Seidel Iteration
• For each hard constraint we require an Instance Solver , Relax()
• After applying Relax(Ci) the residual of the constraint Ci, Res(Ci) = 0
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Gauss-Seidel Iteration
let S be the state of the simulation
Repeat{
for each hard constraint Ci {
S Relax(Ci)
}
} until |Res(Ci)| = 0
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Non-Penetration
• In the event of collision, prevent object penetration
• Use Proximity Query Package (Gottschalk et al. 1996, Larsen et al. 2000 )
• Apply impulse based rigid body dynamics to resolve penetrations
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Joint Constraints
• Simple Atomic Constraints
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Joint Constraints
• Simple Atomic Constraints♦ point distance constraint
p1
p2d
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Joint Constraints
• Simple Atomic Constraints♦ point distance constraint
♦ point planar angle constraint
p1
p2d
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Residuals
• Simple Atomic Constraints♦ point distance constraint
♦ point planar angle constraint
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Joint Constraints
• Combine atomic constraints to form joints
Example1 : A Ball Joint
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Joint Constraints
Example 2: A Revolute Joint
• Combine atomic constraints to form joints
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Soft Constraints
• Encourage planning behavior• Solved using penalty forces• Examples:♦ goal seeking♦ obstacle avoidance♦ estimated path following
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Voronoi Diagrams
• Partition space into regions by closest primitive
• Discretized version can be computed quickly using graphics hardware [Hoff et al. 1999]
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Voronoi Diagrams
• Provide key planning constraints:
♦ Global Estimated Paths
♦ Local Obstacle Avoidance
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Estimated Paths
Based On vPlan [Foskey et al. 2001]
• Extract estimated path from a 3D Voronoi Diagram of obstacles computed using graphics HW
• This estimated path can be recomputed and updated as objects in the scene move
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Obstacle Avoidance
• Distance Fields
♦ Computed in 3D
♦ A byproduct of the graphics hardware based Voronoi Diagram computation
♦ For each point in space, provide the distance to the nearest obstacle
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Obstacle Avoidance Example
R1 must be farther from R2 than a specified threshold distance
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Obstacle Avoidance Example
Localize computation using bounding boxes
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Obstacle Avoidance Example
Compute distance field of R2 in local region
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Obstacle Avoidance Example
Apply forces at sample points on R1
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Obstacle Avoidance Example
Resultant force pushes R1 away from R2
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Obstacle Avoidance
• Distance Field can be recomputed every frame
• Applicable to deformable robots & obstacles whose shape changes every frame
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Results
• Applied to 3 planning scenes♦ Maintainability Study♦ Automated Car Painting♦ Assembly Line Planning
• Timings Taken On:♦ Pentium3 933MHz, 256MB RAM,
NVIDIA GeForce2 GPU
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Maintainability StudyStart Goal
• Scene: ♦ static environment with 2 moving robots♦ 20,000 polygons
• Constraints♦ Non-Penetration, Estimated Path, Obstacle Avoidance
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Maintainability Study
• Performance:♦ Average Time Step 0.093 seconds♦ Total Time 67 seconds♦ The main bottleneck is the distance
field calculation
• Video
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Automotive Painting
• Scene: ♦ static environment and 6 linked moving objects (robot arm)♦ 25,000 polygons
• Constraints♦ Non-Penetration, Estimated Path, Obstacle Avoidance, 40
atomic joint constraints
Start Goal
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Automotive Painting
• Performance:♦ Average Time Step 0.038 seconds♦ Total Time 18 seconds
• Video
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Assembly Line Planning
• Scene: ♦ static environment, 2 moving obstacles, and 6 linked moving
objects (robot arm)♦ 17,000 polygons
• Constraints♦ Non-Penetration, Goal Seeking, Obstacle Avoidance, 40 atomic
joint constraints
Start Goal
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Assembly Line Planning
• Performance:♦ Average Time Step 0.0085 seconds♦ Total Time 16 seconds
• Video
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Conclusion
• Planner♦ Dynamic scenes using local
constraints♦ Global planning, using estimated
path constraints♦ Articulated objects represented
using constraints
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Conclusion
• Framework♦ Static and dynamic environments♦ General relationships between
objects♦ Extensible to many application areas
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Future Work
• More Challenging Scenes♦ Narrow Passages♦ Many Dynamic Obstacles♦ Deformable Objects
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Future Work
• Constraints♦ More sophisticated constraint solver
• Optimization based• Hybrid combination of global & local
techniques
♦ More Constraint Types:• Non-holonomic• Line of sight• Direct human interaction