Our cost function (REF ) minimizes the sum of squared linear and angular accelerations at different time instants. Constraint () enforces the boundary constraints on the linear and angular positions and their derivatives. Note that every problem in the batch has the same boundary conditions. Inequality () represents the bounds on the total velocity and acceleration. Inequality () is the standard collision avoidance model wherein a rectangular footprint is approximated through a combination of overlapping circles (see Fig.REF (a)) [1]}, [2]}. The \(r_i\) 's locates the center of the circle in the robot's local frame. The obstacle's locations are given by \(x_j(t), y_j(t)\)  and they are assumed to be axis-alligned ellipses with dimensions \((a, b)\) .
