Overcoming Nonsmoothness and Control Chattering in Nonconvex Optimal Control Problems

Finding the shortest path for a differential-drive robot while avoiding obstacles is a difficult nonlinear and nonconvex optimal control problem. Numerical solvers can struggle with this task, often failing due to a non-differentiable cost function or producing jerky, "chattering" control signals. These issues are addressed by introducing a small modification to the cost calculation to ensure