# Clothoid path planning — PythonRobotics documentation

# 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