https://123dok.net/document/6zk5jwpz-improving-relaxation-based-constrained-path-planning-via-quadratic-programming.html