mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-01-23 03:28:00 -05:00
d74a91e062
* Rearchitecture document structure * Rearchitecture document structure * Rearchitecture document structure * Rearchitecture document structure * Rearchitecture document structure * Rearchitecture document structure * Rearchitecture document structure
81 lines
2.2 KiB
ReStructuredText
81 lines
2.2 KiB
ReStructuredText
.. _clothoid-path-planning:
|
|
|
|
Clothoid path planning
|
|
--------------------------
|
|
|
|
.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/ClothoidPath/animation1.gif
|
|
.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/ClothoidPath/animation2.gif
|
|
.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/ClothoidPath/animation3.gif
|
|
|
|
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.
|
|
|
|
.. math::
|
|
|
|
g(A):=Y(2A, \delta-A, \phi_{s})
|
|
|
|
Where
|
|
|
|
* :math:`\delta`: the orientation difference between start and goal pose.
|
|
* :math:`\phi_{s}`: the orientation of the start pose.
|
|
* :math:`Y`: :math:`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 :math:`A`,
|
|
|
|
:math:`L`: path length
|
|
|
|
.. math::
|
|
|
|
L=\frac{R}{X\left(2 A, \delta-A, \phi_{s}\right)}
|
|
|
|
where
|
|
|
|
* :math:`R`: the distance between start and goal pose
|
|
* :math:`X`: :math:`X(a, b, c)=\int_{0}^{1} \cos \left(\frac{a}{2} \tau^{2}+b \tau+c\right) d \tau`
|
|
|
|
|
|
- :math:`\kappa`: curvature
|
|
|
|
.. math::
|
|
|
|
\kappa=(\delta-A) / L
|
|
|
|
|
|
- :math:`\kappa'`: curvature rate
|
|
|
|
.. math::
|
|
|
|
\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.
|
|
|
|
.. math::
|
|
\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}
|
|
|
|
|
|
References
|
|
~~~~~~~~~~
|
|
|
|
- `Fast and accurate G1 fitting of clothoid curves <https://www.researchgate.net/publication/237062806>`__
|