Autonomous robot planning and control algorithms