Legged robots face significant challenges in navigating complex environments, as they require precise real-time decisions for foothold selection and contact planning. While existing research has explored methods to select footholds based on terrain geometry or kinematics, a critical gap remains: few existing methods efficiently validate the existence of a non-collision swing trajectory. This paper addresses this gap by introducing KCFRC, a novel approach for efficient foothold reachability analysis. We first formally define the foothold reachability problem and establish a sufficient condition for foothold reachability. Based on this condition, we develop the KCFRC algorithm, which enables robots to validate foothold reachability in real time. Our experimental results demonstrate that KCFRC achieves remarkable time efficiency, completing foothold reachability checks for a single leg across 900 potential footholds in an average of 2 ms. Furthermore, we show that KCFRC can accelerate trajectory optimization and is particularly beneficial for contact planning in confined spaces, enhancing the adaptability and robustness of legged robots in challenging environments.
Figures & tables
Figure 1: Kinematic Collision-Aware Foothold Reachability Criteria demonstration in barrier traversal. Blue cubes show validated reachable footholds, orange/red curves show initialized/optimized swing trajectories. The hexapod robot must carefully select footholds to navigate through the confined space. More supplemental materials are available at KCFRC Webpage .
Figure 2: Foothold Evaluation Criteria. (a) Foot Collision checking for foot-terrain interference, (b) Kinematic Feasibility ensuring joint limits compliance, and (c) Leg Collision detecting segment-obstacle interference. These criteria require explicit trajectory generation for validation.
Symbols
Description
p
Current foothold
q
Candidate foothold
X
Space used in the FRP
Df
Feasible domain in the FRP
Do
Occupied domain in the FRP
fP:E3→SE(3)
Torso pose query function
Table I: Symbol & Abbreviation
Figure 3: Reachability of the expected foothold. The foothold is reachable if feasible domain is not split by obstacles, and vice versa.
Figure 5: Diagram of the Sufficient Condition for Reachability (Theorem 1 ).
Figure 6: Collision detection method. (a) Collision detection under SDF. (b) Foot-collision-only, joint-collision-only and link-subdivision collision models.
Figure 7: Approximate feasible domain PITD.
Figure 8: Grid map and generated guiding surfaces. (a) Keypoint-weighted guiding surface creates smooth interpolation between key points with k=1,wi=1 . (b) Convolutional guiding surface smooths terrain features using n=5,l=0.03 m kernel . The grid map resolution is 0.05 m.
Figure 9: 2D intersection border search. (a) Intersection of Df and Sa . (b) schematic diagram of Algorithm 1 .
Figure 10: Shortest path in projected 2D feasible domain. The shortest path in this domain goes through concave points and connects start and goal points.
Figure 11: Reachability check experiment. Random convex polyhedra corridor is generated as the feasible domain, and stochastic fractal grid map as obstacles. Meanwhile, the shortest polygon path, visibility graph, and initialized trajectory are visualized.
Figure 12: Reachability check time w.r.t intersection border length. Linear fitting of the data is y=ax+b,a=0.00077,b=0.02722 .
Figure 13: Representative planning scenes for foothold reachability check comparison. Top Left&Mid: Hexapod dense planning scenes. Top Right: Hexapod sparse planning scenes. Mid-Row: Quadruped dense planning scenes. Bottom Row: Hexapod confined planning scenes. Planned contact states are recorded for foothold reachability check benchmarking. The grid map resolution is set to 2.5 cm/grid.
Figure 14: Foothold reachability check methods participate in the reachability check experiment for comparison. (a) FEC with predefined swing trajectory. (b) KCFRC. (c) RRT-Connect. (d) STOMP.
Parameters
Value
Description
Robot
ElAir Knee
0.05 m
Knee collision ball radius.
ElAir Foot
0.03 m
Foot collision ball radius.
A1 Knee
0.03 m
Knee collision ball radius.
A1 Foot
0.02 m
Foot collision ball radius.
Ground Truth
Table II: Key Parameters of the Methods
Method
Ave. Time(ms)
Max. Time(ms)
Ave. Accuracy
Ave. Precision
Ave. Recall
Hexapod Confined Planning Scenes
Ground Truth
827.852
18941.200
1.000
1.000
1.000
KCFRC(key)
50.574
78.740
0.996
0.998
0.986
KCFRC(conv)
2.580
4.070
0.993
0.999
0.974
RRT-C(1ms)
303.257
1085.290
0.998
1.000
0.992
RRT-C(50us)
209.600
948.055
0.972
1.000
0.889
Table III: Foothold Reachability Check Comparison
Figure 15: Comparing our proposed method with other methods in a U-shape clamped barrier planning scene considering both knee and foot collision, the final result trajectory is drawn in red. (b) The proposed planner returns a smooth swing trajectory in one shot. (c) MINCO-LBFGS optimizer returns success after many retries. (d) RRT-Connect finds a feasible swing trajectory with low quality even after interpolation. (e) STOMP returns success after many retries.
Figure 16: ElSpider 4 Air climbs across a barrier. (a) The robot is getting across a barrier 0.12m in width and 0.19m in height. (b) Snapshots of the robot and Rviz visualization. (c) Foot SDF curves with time of the snapshots annotated. The SDF value of each foot during the swing phase is no less than 0.02m, claiming no unexpected collision.
Planners in Config Space
Time ( ms )
Len. ( rad )
Cost ( s−3 )
Succ. Num.
Succ. Rate
Proposed
Mean
0.818
2.440
140.117
493
0.992
Max.
12.509
8.031
6079.370
Std.
0.871
1.010
365.728
MINCO LBFGS
Mean
1.447
3.296
147.635
490
0.986
Max.
9.025
5.352
697.719
Std.
0.909
0.647
71.668
Table IV: Comparison of swing trajectory planning performance
Figure 17: Real-world experimental hardware setups demonstrating KCFRC-equipped hexapod locomotion across diverse terrains. (a1, a2) Timber piles: ElSpider 4 Air navigating across irregular timber obstacles with varying heights from 0.1m to 0.4m. (b1, b2) Hole navigation: Hexapod climbing through confined spaces requiring precise foothold planning and collision avoidance. (c1, c2) Stairs: Robot ascending and descending stair structures with 0.12m step height and 0.35m depth, showcasing multi-level terrain traversal capabilities.
FZI Research Center for Information Technology, Karlsruhe, Germany · Machine Intelligence and Robotics Lab (MaiRo), Karlsruhe Institute of Technology (KIT), Karlsruhe, Germany