Search-Based Footstep Planning Armin Hornung, Daniel Maier, Maren Bennewitz Presentation by Dominique Gordon
Introduction
Humanoid Robots vs. Wheeled Robots Step over obstacles Many degrees of freedom Not yet feasible to plan whole body motions in real world
Overview A* search algorithm wA* search algorithm ARA* search algorithm R* search algorithm Extensions to 3D Adaptive-Level-of-Detail Planning
Today! First evaluation of anytime search-based footstep planning over long distances!
Search-Based Footstep Planning
Robot’s State State s = (x, y, 𝛳) of stance foot Footstep action a = (∆x, ∆y, ∆𝛳) relative to stance foot Expanding a state s - determine all successor states s’ s’s checked for collisions and discarded if invalid
A* Search Expands states according to f(s) = g(s) + h(s) g(s) = actual cost of best path to current state h(s) = heuristic that estimates cost to final goal Heuristics must be ADMISSIBLE h(s) ≤ the actual cost
A* Search with SLD as Heuristic f(x) = 75 + 374 = 449 f(x) = 140 + 253 = 393 f(x) = 118 + 329 = 447
A* Search Results Randomized Method Finds optimal path Performance dependent on quality of the heuristic Slow Randomized Method Fast No guarantees on solution quality
Anytime Search-Based Footstep Planning
Weighted A* Search (wA*) Inflates h with a factor w≥1 w=1 is standard A* Finds paths faster, expands fewer states Bias towards states that are closer to goal Trades quality of solution for efficiency
wA* Search with SLD as Heuristic, w=10 f(x) = 75 + 10(374) = 3815 f(x) = 140 + 10(253) = 2670 f(x) = 118 + 10(329) = 3408
Anytime Repairing A* (ARA*) Series of wA* searches while reusing previous information Initially large w As time allows, search runs with lower values of w With enough time, ARA* reaches w = 1 Cost of best solution is guaranteed no worse than w times optimal solution speedup is achieved by not re-computing the state values that have been correctly computed in the previous iterations
Anytime Repairing A* (ARA*) explained A* searches with decreasing w. w = 2.5 w = 1.5 w =1 The corresponding ARA* search iterations. w = 2.5 w = 1.5 w =1
ARA* Results Still depends on quality of heuristic Heuristic of euclidean distance create local minima around obstacles Heuristic of 2D Dijkstra path potentially inadmissible because no stepover
Randomized A* Search (R*) Depends less on quality of heuristic Series of short-range wA* searches Generates k successor states at distance ∆ in random direction Goal states within ∆ is added to successors as well Collision checked Finds “easy” local path - least number of expansions If requires too many expansions, marked AVOID Iteratively lowers w as time permits In our experiments, we used a threshold of 100 expansions to detect that a path can not be found easily
R* explained by me K = 4, ∆ = 1, w = 1 (for demo purposes) Heuristic is SLD ∆=1 g(S2)=5 ∆=1 g(S1)=7 S2 h(S2)=4 S1 S3 h(S3)=3 h(S1)=5 Goal ∆=1 g(S3)=5 Start h(S4)=3.5 ∆=1 g(S4)=3 S4 f(S1) = 7 + 5 = 12 f(S2) = 5 +4 = 9 f(S3) = 5 +3 = 8 f(S4) = 3 + 3.5 = 6.5 At every iteration, R* selects the next state s to expand from While normal A* expands s by generating all the immediate successors of state s, R* expands s by generating K states Only looks at 4 states -- so fast!
R* with Euclidean distance heuristic
ARA* vs. R* Experiments ARA* with 2D Dijkstra heuristic expands non-optimal path Inadmissible ARA* with Euclidean distance heuristic fails to find a plan in time R* with Euclidean finds good initial solutions quickly
Extensions to 3D 3D sensing builds elevation map Footstep state now also has height Body of robot collision-checked against environment to execute step Non-traversable if obstacle exceeds the max step height Extended Dijkstra heuristic
https://www.youtube.com/watch?v=g2NZ_EasJv0&feature=youtu.be
Adaptive-Level-of-Detail Planning
Large open spaces with no obstacles Close to obstacles 2D path easy to compute Easy for humanoid to follow with corresponding walking controller Close to obstacles Footstep planning allows robots to step close to or over obstacles More efficient than 2D path
fast 2D planning in open spaces Adaptive-Level-of-Detail planning combines fast 2D planning in open spaces with footstep planning in areas with obstacles
Preprocessing: Classify the environment open area vs. close to obstacles traversable vs. non traversable obstacles
Execution of Adaptive-Level-of-Detail Planning A walking controller to execute planned footsteps A velocity controller to follow the 2D path
Experiments with Adaptive-Level-of-Detail Planning 2D-only path = fastest computation, not optimal Global footstep plan = longest to compute, efficient path Adaptive-Level-of-Detail planning combines benefits Footstep planning only invoked where necessary Path costs 2% higher than optimal, 51% lower than 2D
Summary Anytime algorithms ARA* R* 3D sensing and collision checking extends footstep planning to 3D Adaptive level-of-detail planning
Personal Opinions Impressed by adaptive-level-of-detail planning Great application of AI search algorithms Didn’t explain ARA* and R* algorithms well Lack of resources for algorithms
Questions?
Works Cited Search-Based Footstep Planning by Armin Hornung, Daniel Maier, Maren Bennewitz Anytime Search-Based Footstep Planning with Suboptimality Bounds by Armin Hornung, Andrew Dornbush, Maxim Likhachev, Maren Bennewitz ARA*: Anytime A* with Provable Bounds on Sub-Optimality by Maxim Likhachev, Geogg Gorgon, Sebastian Thrun R* Search by Maxim Likhachev and Anthony Stentz