https://123dok.net/document/zwvdl21q-maneuver-based-obstacle-avoiding-trajectory-optimization.html