Plane extraction for navigation of humanoid robot