Capturability-Aware Footstep Planning in Cluttered Environments: A Recursively Feasible Sequential MPC on Axis-Aligned Safe Corridors
Authors: Athanasios Kyriakopoulos, Shubham S. Kumbhar, Kunal S. Narkhede, Abhijeet M. Kulkarni, Ioannis Poulakakis
Organizations: A. Kyriakopoulos was with the School of Mechanical Engineering, National Technical University of Athens, Athens, Greece, when this work was conducted. He is now with the Department of Mechanical Engineering, University of California, Berkeley, CA 94720, USA · S. S. Kumbhar, K. S. Narkhede, and A. M. Kulkarni are with the Department of Mechanical Engineering, University of Delaware, Newark, DE 19716, USA · I. Poulakakis is with the School of Mechanical Engineering, National Technical University of Athens, Athens, Greece, and HERON—Hellenic Center of Excellence in Robotics, Athena Research Center, Athens, Greece
A fundamental aspect of humanoid footstep planning is to jointly guarantee obstacle avoidance and locomotion stability. This paper presents a capturability-aware sequential Model Predictive Control (MPC) framework for Linear Inverted Pendulum (LIP)-based humanoid footstep planning in the presence of obstacles. The proposed method combines obstacle avoidance with capturability constraints through safe corridors constructed as sequences of intersecting rectangles, yielding a sequential MPC framework with recursive feasibility guarantees for the LIP-based planning layer. The planner autonomously adapts to constrained scenarios, switching to in-place walking when obstructions---such as doors assumed to be open during planning but unexpectedly found to be closed during execution---block forward motion. Performance is largely independent of the prediction horizon, allowing high success rates while keeping the computational cost low. The approach is validated in high-fidelity simulations with the humanoid robot Digit, demonstrating a practical solution for capturable humanoid footstep planning in cluttered environments.
Figures & tables
Figure 1: Overview of the proposed capturability-aware planning framework: Axis-aligned SC s enable a sequential MPC scheme that reliably drives the robot to its destination, avoiding obstacles and ensuring recursive feasibility.
Figure 2: (a) Linear Inverted Pendulum ( LIP ) model: COM moves in a constant-height plane (pink), with the kinematically feasible foot placement region in yellow. (b) 3D evolution of COM (red), DCM offset (blue), and CCM offset (orange dashed); rectangular regions B (green) and S (blue). (c) Top view of COM , DCM , CCM trajectories and the rectangles B , S .
Figure 3: Rectangle generation via Algorithm 1 . Starting from pseed , the algorithm iteratively selects the separating hyperplane farthest from the seed for each obstacle, intersecting the resulting half-spaces to build an obstacle-free rectangle. Redundant obstacles (e.g., O3 , O6 ) are discarded at each iteration. The final rectangle (red) encloses a local obstacle-free region around pseed .
Figure 4: Failed (left) vs successful (right) switching policy activation. Switching from Si to Si+1 occurs once the virtual stopping point (purple), the COM (red), and the current footstep (yellow) enter Si∩Si+1 . The current waypoint and DCM are also shown in green and blue, respectively.
Figure 5: (a) MPC success rates of the proposed and prior methods. Proposed method outcomes: success (green), in-place walking (yellow), and failure to compute a SC (purple). Prior method outcomes: success (blue) and infeasibility. (b) MPC runtimes for the proposed method.
Figure 6: Digit navigating an office-like environment (rooms are labeled I, II and III) with two doors and three static obstacles. Start and goal points are in magenta and green; doors in red, obstacles in brown. The active SC rectangle is shown in blue and the COM trail in red. (b)-(c) Digit executing capturability-aware footsteps at closed doorways; upon door-opening, the planner resumes corridor execution. (d) Top view with capturability-aware SC , start, and goal. (e) COM speed: red boxes mark near-zero-velocity door-waiting phases; green dotted lines indicate entry into successive corridor intersections.
Walking over disconnected footholds requires selecting landing regions while adapting step timing to the robot's motion. We present a perceptive model predictive planner that combines discrete region selection, foot placement, and current-step duration in a mixed-integer quadratic program using step-to-step Divergent Component of Motion (DCM) dynamics. Onboard depth data are fused into a probabilistic heightmap in the stance foot frame. Convex steppable regions are extracted and shrunk by margins that increase with estimated height uncertainty. Linear constraints impose capturability-based bounds on DCM evolution and limit swing-foot retargeting using the remaining commanded swing time. Within-step replanning updates the initial DCM from measurements to respond to model mismatch and disturbances. We evaluate the framework on Digit in simulation over randomized footholds and on hardware over separated plates, with mean planner solve times of 11.4ms and 4.0ms, respectively.
Zhaoyang Xiang, Upama Pant, Ayonga Hereid
Mechanical and Aerospace Engineering, The Ohio State University, Columbus, OH 43210, USA.
Safe motion planning in uncertain, time-varying environments is challenging because the safe region can change unpredictably across planning steps, often causing a loss of recursive feasibility. In this work, we present a Probabilistic Recursively Feasible Model Predictive Control (PRF-MPC) framework that guarantees recursive feasibility with a specified probability. We introduce properties that an ideal predictor should satisfy to ensure distributional consistency, and use these properties to derive closed-form expressions for the means and covariances of trajectories predicted at future time steps. Building on this analysis, we construct safety constraints that ensure, with high probability, that the current safe set is contained within the safe sets at future time steps, thereby probabilistically guaranteeing recursive feasibility. Simulation results on a lane-change scenario demonstrate that the proposed method significantly improves recursive feasibility.
Hyeontae Sung, Hyeongchan Ham, Junyoung Park +2
School of Electrical Engineering, Korea Advanced Institute of Science and Technology, Daejeon, Korea · The SYCAMORE Lab, ´Ecole Polytechnique F´ed´erale de Lausanne (EPFL), Switzerland
Navigating humanoid robots through dynamic, non-convex environments requires tightly coupling onboard perception with footstep planning and dynamic locomotion control. Existing methods frequently discard complex free space through simplified convex obstacle bounds and fail to account for real-time dynamic occupancy predictions. This paper presents a real-time framework for humanoid navigation that directly constructs spatio-temporal safe corridors from LiDAR-based particle representations and couples them with Linear Inverted Pendulum (LIP)-based planning and Whole Body Control (WBC) to enable navigation around and within moving non-convex structures without restrictive geometric assumptions. Simulations on the Digit humanoid yield a 96% success rate in navigating around and inside moving non-convex obstacles in a 24 by 24 m^2 arena. Ablation studies confirm that the proposed iterative polytope generation significantly outperforms single-iteration baselines in narrow corridors, while evaluations demonstrate robustness to clutter, sensor noise, control delays, and force perturbations.
Shubham S. Kumbhar, Panagiotis Artemiadis
the Mechanical Engineering Department, at the University of Delaware, Newark, DE 19716, USA