Real-time spline trajectory creation and optimization for mobile robots