This is a clothoid path planning sample code.
This can interpolate two 2D pose (x, y, yaw) with a clothoid path, which its curvature is linearly continuous. In other words, this is G1 Hermite interpolation with a single clothoid segment.
This path planning algorithm as follows:
Solve the g(A) function with a nonlinear optimization solver.
g(A):=Y(2A, \delta-A, \phi_{s})
Where
- \delta: the orientation difference between start and goal pose.
- \phi_{s}: the orientation of the start pose.
- Y: Y(a, b, c)=\int_{0}^{1} \sin \left(\frac{a}{2} \tau^{2}+b \tau+c\right) d \tau
We can calculate these path parameters using A,
L: path length
L=\frac{R}{X\left(2 A, \delta-A, \phi_{s}\right)}
where
- R: the distance between start and goal pose
- X: X(a, b, c)=\int_{0}^{1} \cos \left(\frac{a}{2} \tau^{2}+b \tau+c\right) d \tau
- \kappa: curvature
\kappa=(\delta-A) / L
- \kappa': curvature rate
\kappa^{\prime}=2 A / L^{2}
The final clothoid path can be calculated with the path parameters and Fresnel integrals.
\begin{aligned}
&x(s)=x_{0}+\int_{0}^{s} \cos \left(\frac{1}{2} \kappa^{\prime} \tau^{2}+\kappa \tau+\vartheta_{0}\right) \mathrm{d} \tau \\
&y(s)=y_{0}+\int_{0}^{s} \sin \left(\frac{1}{2} \kappa^{\prime} \tau^{2}+\kappa \tau+\vartheta_{0}\right) \mathrm{d} \tau
\end{aligned}
.. autofunction:: PathPlanning.ClothoidPath.clothoid_path_planner.generate_clothoid_path