Reeds Shepp planning

A sample code with Reeds Shepp path planning.

https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/ReedsSheppPath/animation.gif?raw=true

Mathematical Description of Individual Path Types

Here is an overview of mathematical derivations of formulae for individual path types.

In all the derivations below, radius of curvature of the vehicle is assumed to be of unit length and start pose is considered to be at origin. (In code we are removing the offset due to start position and normalising the lengths before passing the values to these functions.)

Also, (t, u, v) respresent the measure of each motion requried. Thus, in case of a turning maneuver, they represent the angle inscribed at the centre of turning circle and in case of straight maneuver, they represent the distance to be travelled.

Left-Straight-Left

We can deduce the following facts using geometry.

AGHC is a rectangle.
\(∠LAC = ∠BAG = t\)
\(t + v = φ\)
\(C(x - sin(φ), y + cos(φ))\)
\(A(0, 1)\)
\(u, t = polar(vector<AC>)\)

Hence, we have:

\(u, t = polar(x - sin(φ), y + cos(φ) - 1)\)
\(v = φ - t\)

Left-Straight-Right

With followng notations:

\(∠MBD = t1\)
\(∠BDF = θ\)
\(BC = u1\)

We can deduce the following facts using geometry.

D is mid-point of BC and FG.
\(t - v = φ\)
\(C(x + sin(φ), y - cos(φ))\)
\(A(0, 1)\)
\(u1, t1 = polar(vector<AC>)\)
\(\frac{u1^2}{4} = 1 + \frac{u^2}{4}\)
\(BF = 1\) [Radius Of Curvature]
\(FD = \frac{u}{2}\)
\(θ = arctan(\frac{BF}{FD})\)
\(t1 + θ = t\)

Hence, we have:

\(u1, t1 = polar(x + sin(φ), y - cos(φ) - 1)\)
\(u = \sqrt{u1^2 - 4}\)
\(θ = arctan(\frac{2}{u})\)
\(t = t1 + θ\)
\(v = t - φ\)

LeftxRightxLeft

With followng notations:

\(∠CBD = ∠CDB = A\) [BCD is an isoceles triangle]
\(∠DBK = θ\)
\(BD = u1\)

We can deduce the following facts using geometry.

\(t + u + v = φ\)
\(D(x - sin(φ), y + cos(φ))\)
\(B(0, 1)\)
\(u1, θ = polar(vector<BD>)\)
\(A = arccos(\frac{BD/2}{CD})\)
\(u = (π - 2*A)\)
\(∠ABK = \frac{π}{2}\)
\(∠KBD = θ\)
\(t = ∠ABK + ∠KBD + ∠DBC\)

Hence, we have:

\(u1, θ = polar(x - sin(φ), y + cos(φ) - 1)\)
\(A = arccos(\frac{u1/2}{2})\)
\(t = \frac{π}{2} + θ + A\)
\(u = (π - 2*A)\)
\(v = (φ - t - u)\)

LeftxRight-Left

With followng notations:

\(∠CBD = ∠CDB = A\) [BCD is an isoceles triangle]
\(∠DBK = θ\)
\(BD = u1\)

We can deduce the following facts using geometry.

\(t + u - v = φ\)
\(D(x - sin(φ), y + cos(φ))\)
\(B(0, 1)\)
\(u1, θ = polar(vector<BD>)\)
\(A = arccos(\frac{BD/2}{CD})\)
\(u = (π - 2*A)\)
\(∠ABK = \frac{π}{2}\)
\(∠KBD = θ\)
\(t = ∠ABK + ∠KBD + ∠DBC\)

Hence, we have:

\(u1, θ = polar(x - sin(φ), y + cos(φ) - 1)\)
\(A = arccos(\frac{u1/2}{2})\)
\(t = \frac{π}{2} + θ + A\)
\(u = (π - 2*A)\)
\(v = (-φ + t + u)\)

Left-RightxLeft

With followng notations:

\(∠CBD = ∠CDB = A\) [BCD is an isoceles triangle]
\(∠DBK = θ\)
\(BD = u1\)

We can deduce the following facts using geometry.

\(t - u - v = φ\)
\(D(x - sin(φ), y + cos(φ))\)
\(B(0, 1)\)
\(u1, θ = polar(vector<BD>)\)
\(BC = CD = 2\) [2 * radius of curvature]
\(cos(2π - u) = \frac{BC^2 + CD^2 - BD^2}{2 * BC * CD}\) [Cosine Rule]
\(\frac{sin(A)}{BC} = \frac{sin(u)}{u1}\) [Sine Rule]
\(∠ABK = \frac{π}{2}\)
\(∠KBD = θ\)
\(t = ∠ABK + ∠KBD - ∠DBC\)

Hence, we have:

\(u1, θ = polar(x - sin(φ), y + cos(φ) - 1)\)
\(u = arccos(1 - \frac{u1^2}{8})\)
\(A = arcsin(\frac{sin(u)}{u1}*2)\)
\(t = \frac{π}{2} + θ - A\)
\(v = (t - u - φ)\)

Left-RightxLeft-Right

With followng notations:

\(∠CLG = ∠BCL = ∠CBG = ∠LGB = A = u\) [BGCL is an isoceles trapezium]
\(∠KBG = θ\)
\(BG = u1\)

We can deduce the following facts using geometry.

\(t - 2u + v = φ\)
\(G(x + sin(φ), y - cos(φ))\)
\(B(0, 1)\)
\(u1, θ = polar(vector<BG>)\)
\(BC = CL = LG = 2\) [2 * radius of curvature]
\(CG^2 = CL^2 + LG^2 - 2*CL*LG*cos(A)\) [Cosine rule in LGC]
\(CG^2 = CL^2 + LG^2 - 2*CL*LG*cos(A)\) [Cosine rule in LGC]
From the previous two equations: \(A = arccos(\frac{u1 + 2}{4})\)
\(∠ABK = \frac{π}{2}\)
\(t = ∠ABK + ∠KBG + ∠GBC\)

Hence, we have:

\(u1, θ = polar(x + sin(φ), y - cos(φ) - 1)\)
\(u = arccos(\frac{u1 + 2}{4})\)
\(t = \frac{π}{2} + θ + u\)
\(v = (φ - t + 2u)\)

LeftxRight-LeftxRight

With followng notations:

\(∠GBC = A\) [BGCL is an isoceles trapezium]
\(∠KBG = θ\)
\(BG = u1\)

We can deduce the following facts using geometry.

\(t - v = φ\)
\(G(x + sin(φ), y - cos(φ))\)
\(B(0, 1)\)
\(u1, θ = polar(vector<BG>)\)
\(BC = CL = LG = 2\) [2 * radius of curvature]
\(CD = 1\) [radius of curvature]
D is midpoint of BG
\(BD = \frac{u1}{2}\)
\(cos(u) = \frac{BC^2 + CD^2 - BD^2}{2*BC*CD}\) [Cosine rule in BCD]
\(sin(A) = CD*\frac{sin(u)}{BD}\) [Sine rule in BCD]
\(∠ABK = \frac{π}{2}\)
\(t = ∠ABK + ∠KBG + ∠GBC\)

Hence, we have:

\(u1, θ = polar(x + sin(φ), y - cos(φ) - 1)\)
\(u = arccos(\frac{20 - u1^2}{16})\)
\(A = arcsin(2*\frac{sin(u)}{u1})\)
\(t = \frac{π}{2} + θ + A\)
\(v = (t - φ)\)

LeftxRight90-Straight-Left

With followng notations:

\(∠FBM = A\) [BGCL is an isoceles trapezium]
\(∠KBF = θ\)
\(BF = u1\)

We can deduce the following facts using geometry.

\(t + \frac{π}{2} - v = φ\)
\(F(x - sin(φ), y + cos(φ))\)
\(B(0, 1)\)
\(u1, θ = polar(vector<BF>)\)
\(BM = CB = 2\) [2 * radius of curvature]
\(MD = CD = 1\) [CGDM is a rectangle]
\(MC = GD = u\) [CGDM is a rectangle]
\(MF = MD + DF = 2\)
\(BM = \sqrt{BF^2 - MF^2}\) [Pythagoras theorem on BFM]
\(tan(A) = \frac{MF}{BM}\)
\(u = MC = BM - CB\)
\(t = ∠ABK + ∠KBF + ∠FBC\)

Hence, we have:

\(u1, θ = polar(x - sin(φ), y + cos(φ) - 1)\)
\(u = arccos(\sqrt{u1^2 - 4} - 2)\)
\(A = arctan(\frac{2}{\sqrt{u1^2 - 4}})\)
\(t = \frac{π}{2} + θ + A\)
\(v = (t - φ + \frac{π}{2})\)

Left-Straight-Right90xLeft

With followng notations:

\(∠MBH = A\) [BGCL is an isoceles trapezium]
\(∠KBH = θ\)
\(BH = u1\)

We can deduce the following facts using geometry.

\(t - \frac{π}{2} - v = φ\)
\(H(x - sin(φ), y + cos(φ))\)
\(B(0, 1)\)
\(u1, θ = polar(vector<BH>)\)
\(GH = 2\) [2 * radius of curvature]
\(CM = DG = 1\) [CGDM is a rectangle]
\(CD = MG = u\) [CGDM is a rectangle]
\(BM = BC + CM = 2\)
\(MH = \sqrt{BH^2 - BM^2}\) [Pythagoras theorem on BHM]
\(tan(A) = \frac{HM}{BM}\)
\(u = MC = BM - CB\)
\(t = ∠ABK + ∠KBH - ∠HBC\)

Hence, we have:

\(u1, θ = polar(x - sin(φ), y + cos(φ) - 1)\)
\(u = arccos(\sqrt{u1^2 - 4} - 2)\)
\(A = arctan(\frac{2}{\sqrt{u1^2 - 4}})\)
\(t = \frac{π}{2} + θ - A\)
\(v = (t - φ - \frac{π}{2})\)

LeftxRight90-Straight-Right

With followng notations:

\(∠KBG = θ\)
\(BG = u1\)

We can deduce the following facts using geometry.

\(t - \frac{π}{2} - v = φ\)
\(G(x + sin(φ), y - cos(φ))\)
\(B(0, 1)\)
\(u1, θ = polar(vector<BG>)\)
\(BD = 2\) [2 * radius of curvature]
\(DG = EF = u\) [DGFE is a rectangle]
\(DG = BG - BD = 2\)
\(∠ABK = \frac{π}{2}\)
\(t = ∠ABK + ∠KBG\)

Hence, we have:

\(u1, θ = polar(x + sin(φ), y - cos(φ) - 1)\)
\(u = u1 - 2\)
\(t = \frac{π}{2} + θ\)
\(v = (t - φ - \frac{π}{2})\)

Left-Straight-Left90xRight

With followng notations:

\(∠KBH = θ\)
\(BH = u1\)

We can deduce the following facts using geometry.

\(t + \frac{π}{2} + v = φ\)
\(H(x + sin(φ), y - cos(φ))\)
\(B(0, 1)\)
\(u1, θ = polar(vector<BH>)\)
\(GH = 2\) [2 * radius of curvature]
\(DC = BG = u\) [DGBC is a rectangle]
\(BG = BH - GH\)
\(∠ABC= ∠KBH\)

Hence, we have:

\(u1, θ = polar(x + sin(φ), y - cos(φ) - 1)\)
\(u = u1 - 2\)
\(t = θ\)
\(v = (φ - t - \frac{π}{2})\)

LeftxRight90-Straight-Left90xRight

With followng notations:

\(∠KBH = θ\)
\(∠HBM = A\)
\(BH = u1\)

We can deduce the following facts using geometry.

\(t - v = φ\)
\(H(x + sin(φ), y - cos(φ))\)
\(B(0, 1)\)
\(u1, θ = polar(vector<BH>)\)
\(GF = ED = 1\) [radius of curvature]
\(BD = GH = 2\) [2 * radius of curvature]
\(FN = GH = 2\) [ENMD is a rectangle]
\(NH = GF = 1\) [FNHG is a rectangle]
\(MN = ED = 1\) [ENMD is a rectangle]
\(DO = EF = u\) [DOFE is a rectangle]
\(MH = MN + NH = 2\)
\(BM = \sqrt{BH^2 - MH^2}\) [Pythagoras theorem on BHM]
\(DO = BM - BD - OM\)
\(tan(A) = \frac{MH}{BM}\)
\(∠ABC = ∠ABK + ∠KBH + ∠HBM\)

Hence, we have:

\(u1, θ = polar(x + sin(φ), y - cos(φ) - 1)\)
\(u = /sqrt{u1^2 - 4} - 4\)
\(A = arctan(\frac{2}{u1^2 - 4})\)
\(t = \frac{π}{2} + θ + A\)
\(v = (t - φ)\)

Ref: