Autonomous navigation and exploration in a rescue environment