Online Multi-Contact Motion Replanning for Humanoid Robots with Semantic 3D Voxel Mapping: ExOctomap

This study introduces a rapid motion-replanning technique driven by a semantic 3D voxel mapping system, essential for humanoid robots to autonomously navigate unknown territories through online environmental sensing. Addressing the challenges posed by the conventional approach based on polygon mesh or primitive extraction for mapping, we adopt semantic voxel mapping, utilizing our innovative Extended-Octomap (ExOctomap). This structure archives environmental normal vectors, outcomes of Euclidean Cluster Extraction, and principal component analysis within an Octree structure, facilitating an O (log N) efficiency in semantic accessibility from a position query [Formula: see text]. This strategy reduces the 6D contact pose search to simple 3D grid sampling. Moreover, voxel representation enables the search of collision-free trajectories online. Through experimental validation based on simulations and real robotic experiments, we demonstrate that our framework can efficiently adapt multi-contact motions across diverse environments, achieving near real-time planning speeds that range from 13.8 ms to 115.7 ms per contact.