Clothoid path planning

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:

Step1: Solve g function

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\)

Step2: Calculate path parameters

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}\]

Step3: Construct a path with Fresnel integral

The final clothoid path can be calculated with the path parameters and Fresnel integrals.

\[\begin{split}\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}\end{split}\]

References

• Fast and accurate G1 fitting of clothoid curves