Transcript Slides

Search-Based Footstep Planning
Wesley Chu
With slides taken from Armin Hornung
Humanoid Path Planning
• Humanoids have large number of DOF
• Planning full body movements not
computationally feasible
• Alternative: plan for footstep locations, and
use predefined motions to execute on these
footsteps
2D Path Planning
• Compute collision-free 2D path, then plan
footsteps in local area
• But 2D planners do not consider the full
capabilities of a humanoid robot
• Humanoids can step on, over, or close to
obstacles
Footstep Planning





State
Footstep action
Fixed set of footstep actions
Successor state
Transition costs reflect execution time:
Euclidean distance
constant step cost
costs based on the
distance to obstacles
Footstep Planning
start
Footstep Planning
start
Footstep Planning
start
Footstep Planning
estimated costs
from s’ to goal
s’
transition costs
s
path costs from
start to s
start
Footstep Planning
?
s’
planar obstacle
s
start
Obstacle Representation
• Some obstacles are planar and shallow, allowing the
robot to either step over or on it.
• Other obstacles are non-traversable (e.g. walls), and
must be navigated around
• Map representation must make a distinction
between the two
Search Based Footstep Planning
• Chaining footstep actions builds a lattice
graph in the global search space
• Only add valid states after a collision check
• With graph of footstep states, can now
perform heuristic search
Quick Review: A*
• Similar to Dijkstra
• Best-first search to find optimal path
• Paths are explored in a priority queue
– f(n) = g(n) + h(n)
• Weighted A*: multiply h(n) by some constant
weight w
– Faster – favors states closer to the goal
– At most w times worse than optimal solution
Footstep planning with A*
•
•
•
•
•
Search space: (x, y, theta)
Discrete footsteps
Optimal solution with A*
But expanding all states is slow!
Computationally infeasible for
cluttered/complicated environments
• Heuristic usually Euclidean distance to goal or
2D Dijkstra path to goal
Heuristic
Estimates the costs to the goal
 Critical for planner performance
 Usual choices:



Euclidean distance
2D Dijkstra path
expanded
state s'
h(s')
goal
state
Randomized Footstep Planning
• Search space of footstep actions with RRT /
PRM
• Fast
• But no guarantees on optimality or
completeness
Anytime Repairing A* (ARA*)
• Basic idea: run A* with a large weight, to
quickly find a suboptimal solution (but
bounded in it’s sub-optimality)
• Then, as time permits, run A* with
successively smaller weights to find better
solutions
• Key: reuse information from previous
iterations to speed up subsequent searches
Randomized A* (R*)
• Iteratively construct graph of sparsely placed
random sub-goals (subgoals should be closer
to final goal than previous subgoal)
• Run wA* between subgoals – prefer easy to
plan sequences
• Iteratively lower weight as time allows
Planning in Dense Clutter Until
First Solution
A*
Euclidean heur.
11.9 sec.
R*
Euclidean heur.
0.4 sec.
ARA*
Euclidean heur.
2.7 sec.
ARA*
Dijkstra heur.
0.7 sec.
Planning in Dense Clutter Until
First Solution



12 random start and goal locations
ARA* finds fast results only with the 2D Dijkstra
heuristic, leading to longer paths due to its
inadmissibility
R* finds fast results even with the Euclidean
heuristic
Planning with Time Limit 5s
ARA*
Euclidean heuristic
ARA*
Dijkstra heuristic
R*
Euclidean heuristic
goal
start
fails, requires 43 sec.
final w=1.4
final w=7
goal
start
fails, requires 92 sec.
clutter
final w=1.4
final w=8
Extension to 3D




•
Depth camera for visual perception
Scan matching to reduce drift of odometry
Heightmap as environment representation
Footstep planning and collision-checking
on heightmap
Maier et al. (to appear IROS 2013)]
Depth Cameras for
Robot Navigation

Dense depth
information

Lightweight

Cheap
Pose Estimation
Odometry estimate is error-prone due to
slippage of the feet and noisy sensors
 Accordingly, consecutive depth camera
observations may not align
 Error accumulates over time
 Scan matching to reduce the error

odometry
estimate
Pose Estimation
Odometry estimate is error prone due to
slippage of the feet and noisy sensors
 Accordingly, consecutive depth camera
observations may not align
 Error accumulates over time
 Scan matching to reduce the error

scan
matching
Heightmap Learned from
Depth-Camera Observations






2D gridmap
Probabilistic
height estimate
for each cell
Conservative
updates
Quick access
Memory efficient
High resolution
Action Set for a Nao Humanoid
T-Step
Standard
planar steps
step over
step onto
Extended 3D stepping capabilities
Safe Stepping Actions

Allow only states where all
cells covered by the footprint
have a small height difference

Height difference between
s and s 0= a(s ) must be within
the limits [ ¢ zm in ; ¢ zm ax ]a
allowed by the action a
s
Whole-Body Collision Checking
• In addition to (x, y, theta), footsteps also have a
height associated with them
• Inverse Height Map of step motions determine the
clearance needed for full body clearance
• Mark obstacles that would violate inverse height
map as unavailable to step on (but can still
potentially step over)
Adaptive Level-of-Detail Planning
Planning the whole path with footsteps may
not always be desired in large open spaces
 Adaptive level-of-detail planning: Combine
fast grid-based 2D planning in open spaces
with footstep planning near obstacles

Adaptive planning
[Hornung & Bennewitz (ICRA ‘11)]
Adaptive Level-of-Detail Planning
When near an obstacle,
switch to footstep
planning
 When not near an
obstacle, find paths
with a 2D path search
algorithm

Adaptive Planning Results
2D Planning
Footstep Planning
Adaptive Planning
29 s planning time
<1s planning time,
costs only 2% higher
goal
start
<1 s planning time
High path costs
Fast planning times and efficient solutions
with adaptive level-of-detail planning
Summary
• ARA*, R*
• Extensions to 3D
• Adaptive Level-of-Detail Planning
Thank you!