Optimal Trajectory Planning with Quintic G2-splines