Using visual odometry to create 3D maps for online footstep planning

This paper describes an online system for footstep planning using a 3D map reconstructed by visual odometry. This system consists of two key components: 3D reconstruction via visual odometry from a stereo image sequence to obtain a dense local world model, and a footstep planner for biped robots using the reconstructed 3D map. Visual odometry is a method to connect 3D image sequences to obtain 6DOF camera motion and dense 3D environment information. The method described in this paper consists of three components: stereo depth map calculation, 3D flow calculation from tracking raw image features, and 6DOF camera motion estimation from RANSAC. Using the resulting 3D data, an optimal sequence of footstep locations is planned. The footstep planner is provided a height map of the terrain and a discrete set of possible footstep motions. The planner then evaluates footstep locations for viability using a collection of heuristic metrics designed to encode the relative safety, effort required, and overall motion complexity. Finally, we implemented this system on the humanoid robot H7. A local 3D map is reconstructed using visual odometry at about 10 Hz and the footstep planner replans at intervals of four steps. The robot walked across a floor, avoiding obstacles and reaching the goal.