Efficient Terrain Map Using Planar Regions for Footstep Planning on Humanoid Robots

Humanoid robots possess the ability to perform complex tasks in challenging environments. However, they require a model of the surroundings in a representation that is sufficient enough for downstream tasks such as footstep planning. The maps generated by existing mapping algorithms are either sparse, insufficient for footstep planning, memory intensive, or too slow for dynamic humanoid behaviors. In this work, we develop a mapping algorithm that combines planar region measurements along with kinematic-inertial state estimates to build a dense but efficient map of bounded planar surfaces. We present novel algorithms for plane feature matching, tracking and registration for mapping within a factor graph framework. The generated map is not only memory efficient, but also offers higher reliability and speed in bipedal footstep planning, than was possible earlier. The complete algorithm is also demonstrated using a full-scale humanoid robot, Nadia, walking over both flat ground and rough terrain utilizing the generated terrain map.