refer to ：https://github.com/studentlemon/path_smooth_with_kappa
This is used for smoothing path. SQP optimization model is established in Cartesian coordinate system.
State space:
In this repo, θ is the bias between the angle of reference point and decision variable. k is the curvature. In this model, speed is considered as an constant value, is therefore replaced by a constant distance ds.
- Objective function:
- Constraints:
In the kinematic model constrians, Taylor expansion is used based on reference points.
As shown in figure below, SQP is the smoothing result, where IHA* represents the original path.
