Optimization-Based Planning for Autonomous Traversal of Obstacles with Mobile Ground Robots