Plane-Aware Heightmap for Adaptable Footstep Planning in Discontinuous Coplanar Terrain

Planar information is crucial for determining the support plane of a candidate footstep, enabling reliable traversability checks during footstep planning. Due to the imprudent determination of the support plane caused by insufficient planar information, current heightmap-based algorithms struggle to plan footsteps in discontinuous coplanar terrain. In this paper, we propose a plane-aware heightmap that integrates the results of planar region detection, allowing each cell to direct retrieval of its corresponding plane parameters. Building on this, the support plane of a candidate footstep is determined by evaluating the consistency between the support planes of the four corner subregions of the foot. Specifically, the support plane considered for each corner subregion is the one with the highest elevation within that region, provided it is sufficiently large to support the corresponding area. We evaluate the enhancement of footstep planning achieved by the proposed heightmap and support-plane determination methods, which enable planning across diverse discontinuous coplanar terrain in both simulated and real-world settings.