Optimal planning for coordinated vehicles with bounded curvature