Safety and efficiency in autonomous vehicles through planning with uncertainty