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}