【自动驾驶】路径规划——ReedsShepp 曲线总结（python实现 | c++实现）_reeds-shepp

参考资料

• Reeds-Shepp和Dubins曲线简介

• 路径规划 - ReedsShepp 曲线

• 原论文：OPTIMAL PATHS FOR A CAR THAT GOES BOTH FORWARDS AND BACKWARDS

• Reeds Shepp planning

• 一个实例了解自动驾驶路径规划 —— （四）HybridA*算法中RS曲线详解

• 乘用车自动泊车系统的路径规划及跟踪控制研究_陶鹏

---

前文

• Dubins 曲线推导(基于向量的方法)
• Dubins 曲线公式总结及python代码实现(基于几何的方法)

---

1. Reeds-Shepp 曲线

1.1 基本概念

Reeds-Shepp 算法简称 RS，由 J.A.Reeds 和 L.A.Shepp 于 1990 年发表的论文 ( optimal path for a car that goes both forward and backwards)。该方法基于 Dubins 算法进行改进，将反向运动（汽车允许后退，挂倒挡）加入到规划中，这就使得在某些情况下可以得出比 Dubins 曲线更优的解。

曲线示例如下：

图片来源：https://blog.csdn.net/robinvista/article/details/95137143

---

如上图可以知道，在 Reeds-Shepp 曲线的路径中，允许尖瓣存在。

1.2 字段组合

在字段中的字母上加入上标，用来表示运动方向，表示如下：

使用 $C、S$ 字符可以给出如下集合：

C C C ← { C + C − C + C + C − C − C + C + C − C + C β + C β − C − C + C β − C β − C + } C S C ← { C + S + C + C − C π / 2 + S + C + C + S + C π / 2 + C − C − C π / 2 + S + C π / 2 + C − } (1) \tag{1}

$$\begin{aligned} &C C C \leftarrow\left\{\quad C^{+} C^{-} C^{+} \quad C^{+} C^{-} C^{-} \quad C^{+} C^{+} C^{-} \quad C^{+} C_{\beta}^{+} C_{\beta}^{-} C^{-} \quad C^{+} C_{\beta}^{-} C_{\beta}^{-} C^{+}\right\}\\ &C S C \leftarrow\left\{\quad C^{+} S^{+} C^{+} \quad C^{-} C_{\pi / 2}^{+} S^{+} C^{+} \quad C^{+} S^{+} C_{\pi / 2}^{+} C^{-} \quad C^{-} C_{\pi / 2}^{+} S^{+} C_{\pi / 2}^{+} C^{-}\right\} \end{aligned}$$ ​CCC←{C+C−C+C+C−C−C+C+C−C+Cβ+​Cβ−​C−C+Cβ−​Cβ−​C+}CSC←{C+S+C+C−Cπ/2+​S+C+C+S+Cπ/2+​C−C−Cπ/2+​S+Cπ/2+​C−}​(1)

通过反转等式 (1) 的符号可以获得新的的字段。上式 (1) 中， $C_{\pi /2}^{+}$表示相应的$L$或$R$的转过的角度为$\pi /2$ ， $C_{\beta }{C}_{\beta }$组合表示相应的弧段拥有相等的长度。

为了以紧凑的表示，避免$\pm$ ，可以写出充分路径的列表如下：
C C C ← { C ∣ C ∣ C C ∣ C C C C ∣ C C C β ∣ C β C C ∣ C β C β ∣ C } C S C ← { C S C C ∣ C π / 2 S C C S C π / 2 ∣ C C ∣ C π / 2 S C π / 2 ∣ C } (2) \tag{2}

$$\begin{aligned} &C C C \leftarrow\left\{\begin{array}{ccccc} C|C| C & C \mid C C & C C \mid C & C C_{\beta} \mid C_{\beta} C & C\left|C_{\beta} C_{\beta}\right| C \end{array}\right\}\\ &C S C \leftarrow\left\{\begin{array}{llll} C S C & C \mid C_{\pi / 2} S C & C S C_{\pi / 2} \mid C & C\left|C_{\pi / 2} S C_{\pi / 2}\right| C \end{array}\right\} \end{aligned}$$ ​CCC←{C∣C∣C​C∣CC​CC∣C​CCβ​∣Cβ​C​C∣Cβ​Cβ​∣C​}CSC←{CSC​C∣Cπ/2​SC​CSCπ/2​∣C​C ​Cπ/2​SCπ/2​ ​C​}​(2)

其中，$|$表示车辆运动朝向由正向转为反向或者由反向转为正向。

将上述word分别带下标，如$C_{\alpha }|C_{\beta }|C_{\gamma }$，其中$\alpha ,\beta ,\gamma$分别表示旋转的角度（弧度制）。$S$带下标$d$表示行走的直线距离为$d$。它们的范围如下表所示：

将$C$替换为$L$或$R$ ，通过简单的变换，则共有 48 种字段组合。这种简单变换包括时间翻转(timeflip)、反射(reflect)和向后变换(backwards)。

1.3 时间翻转(timeflip)、反射(reflect)和向后变换(backwards)

1.3.1 时间翻转(timeflip)

将计算出的曲线按照其运动方向进行取反，得到的新的曲线为原曲线相反的曲线。

如图，蓝色曲线 $L^{-}{R}^{+}{S}^{+}{L}^{+}$与红色曲线$L^{+}{R}^{-}{S}^{-}{L}^{-}$关于Y 轴左右对称，两条路径曲线上的路径航向角相反。从起始点$O(0,0,0)$到目标点$A(x,y,\theta )$的路径可以通过起始点$O(0,0,0)$到目标点 $B(-x,-y,-\theta )$的路径取反计算获得。

1.3.2 反射(reflect)

第二种转换关系：“reflect”，即将计算的曲线按照其沿圆周运动方向取反，得到的曲线与原来的曲线长度相同的新曲线。

如图所示，红色曲线 $R^{-}{L}^{+}{S}^{+}{R}^{+}$与蓝色曲线 $L^{-}{R}^{+}{S}^{+}{L}^{+}$关于 X 轴左右对称，两条路径曲线上的路径航向角相反。从起始点$O(0,0,0)$ 到目标点 $A(x,y,\theta )$的路径可以通过起始点$O(0,0,0)$ 到目标点 $B(x,-y,-\theta )$的路径圆周方向取反计算获得。

1.3.3 向后变换(backwards)

第三种转换关系：“backwards”，即将原曲线的路径逆序转换，得到的曲线与原来的曲线长度相同的新曲线。

“backwards”实例图如图所示，蓝色曲线 $L^{-}{S}^{-}{R}^{-}{L}^{+}$与红色曲线 $L^{+}{R}^{-}{S}^{-}{L}^{-}$不具有以上两种的堆成关系，但是很直观的可以看出 $L^{-}{S}^{-}{R}^{-}{L}^{+}$的逆序排列得到 $L^{+}{R}^{-}{S}^{-}{L}^{-}$，两条路径曲线上的路径进行逆向排列。从起始点$O(0,0,0)$到目标点 $A(x,y,\theta )$ 的路径可以通过起始点$O(0,0,0)$ 到目标点 $B(x\ast \cos \theta +y\ast \sin \theta ,x\ast \sin \theta -y\ast \cos \theta ,\theta )$ 的路径圆周方向取反计算获得。

1.4 48 个字段组合

所有的48个字段组合一一枚举如下：

图中，角标$\beta$代表C旋转的弧度是$\beta$，同理$\pi /2$也是代表旋转弧度，意思就是这个圆必须转$\pi /2$。

所以，Dubins曲线是从6个曲线中选出最短的那条，而Reeds-Shepp曲线是从48个曲线中选出最短的那个（最开始的论文认为最短的曲线一定在48条曲线中，后人研究发现有两条曲线不会是最短的，所以后来搜索范围减小到46条）。

至于这些字段组合具体的推导，请看原论文。

2. RS曲线求解

Reeds-Shepp 曲线的求解公式在原论文的式(8.1)~式(8.11)中。这边将其总结整理。

假设起点为$(0,0,0)$，终点为$(x,y,\theta )$。

2.1 三段圆弧组成

• 第一类基础曲线: $C|C|C$, 即三段圆弧且每段圆弧两两圆弧方向相反, 基于此, 可以扩展出四种曲线形式: $L^{+}{R}^{-}{L}^{+}、L^{-}{R}^{+}{L}^{-}、R^{+}{L}^{-}{R}^{+}、R^{-}{L}^{+}{R}^{-}$。
• 第二类基础曲线: $CC\mid C$, 即三段圆弧且后两段圆弧的圆弧方向相反, 基于此, 可 以扩展出四种曲线形式: $L^{+}{R}^{+}{L}^{-}、L^{-}{R}^{-}{L}^{+}、R^{+}{L}^{+}{R}^{-}、R^{-}{L}^{-}{R}^{+}$。
• 第三类基础曲线: $C\mid CC$, 即三段圆弧且前两段圆弧的圆弧方向相反, 基于此, 可 以扩展出四种曲线形式: $L^{+}{R}^{-}{L}^{-}、L^{-}{R}^{+}{L}^{+}、R^{+}{L}^{-}{R}^{-}、R^{-}{L}^{+}{R}^{+}$。

通过时间翻转、反射和向后三种曲线变换方式, 上述 3 种公式的符号变换成 了 12 种曲线的运动方式, 可以利用公式(1)进行求解 。
{ ( u 1 , t 1 ) = R ( x − sin ⁡ θ , y − 1 + cos ⁡ θ ) { u 1 2 > 4 → L = ∞ u 1 2 ≤ 4 → { A = arcsin ⁡ ( u 1 2 4 ) u = M ( A + t 1 ) ( u 2 , t 2 ) = R ( x − sin ⁡ θ , y − 1 + cos ⁡ θ ) t = t 2 v = M ( θ + t − u ) L = ∣ t ∣ + ∣ u ∣ + ∣ v ∣  其中  A ∈ [ π 2 , π ] (1) \tag{1} \left\{

$$\begin{array}{l} \left(u_{1}, t_{1}\right)=R(x-\sin \theta, y-1+\cos \theta) \\ \left\{\begin{array}{l} u_{1}^{2}>4 \rightarrow L=\infty \\ u_{1}^{2} \leq 4 \rightarrow\left\{\begin{array}{l} A=\arcsin \left(\frac{u_{1}^{2}}{4}\right) \\ u=M\left(A+t_{1}\right) \\ \left(u_{2}, t_{2}\right)=R(x-\sin \theta, y-1+\cos \theta) \\ t=t_{2} \\ v=M(\theta+t-u) \\ L=|t|+|u|+|v| \end{array}$$ \right. \\ \text { 其中 } A \in\left[\frac{\pi}{2}, \pi\right] \end{array}\right. \end{array}\right. ⎩ ⎨ ⎧​(u1​,t1​)=R(x−sinθ,y−1+cosθ)⎩ ⎨ ⎧​u12​>4→L=∞u12​≤4→⎩ ⎨ ⎧​A=arcsin(4u12​​)u=M(A+t1​)(u2​,t2​)=R(x−sinθ,y−1+cosθ)t=t2​v=M(θ+t−u)L=∣t∣+∣u∣+∣v∣​ 其中 A∈[2π​,π]​​(1)

其中 $t、u、v$ 代表公式中每次转向的弧度值大小（其实也是基础曲线三段圆弧的弧长，因为圆弧半径为1，单位圆）, $L$为路径总长度；公式中的函数 $R$ 是将笛卡尔坐标系中的 $(x,y)$ 转化成极坐标系下的 $\left(u_1,t_1\right)$, 其中径向坐标值有 $u_1、u_2$, 角坐标值有 $t_1、t_2$, 函数 $M$ 用于对$2\pi$取模运算，并将弧度值限定在 $[-\pi ,\pi ]$。

函数$(r,\theta )=R(x,y)$即笛卡尔坐标系中的 $(x,y)$ 与极坐标系$(r,\theta )$的关系可以表示为： { r cos ⁡ θ = x r sin ⁡ θ = y

$$\begin{aligned} \left\{\begin{array}{l} r\cos{\theta}=x\\ r\sin{\theta}=y \end{array}\right.\\ \end{aligned}$$ {rcosθ=xrsinθ=y​​

$\psi =M(\theta )$即表示 { ψ = θ m o d    2 π − π ≤ ψ < π

$$\begin{aligned} \left\{\begin{array}{l} \psi=\theta \mod 2\pi\\ -\pi\le\psi<\pi \end{array}\right.\\ \end{aligned}$$ {ψ=θmod2π−π≤ψ<π​​

后文除了新定义的符号，不再赘述已定义的符号。

2.2 两段圆弧与直线组成

• 第四类基础曲线形式: $CSC$, 即两段圆弧与直线组成, 基于此, 可以扩展出八种曲 线形式: $L^{+}{S}^{+}{L}^{+}、L^{-}{S}^{-}{L}^{-}、R^{+}{S}^{+}{R}^{+}、R^{-}{S}^{-}{R}^{-}、L^{+}{S}^{+}{R}^{+}、L^{-}{S}^{-}{R}^{-}、R^{+}{S}^{+}{L}^{+}、R^{-}{S}^{-}{L}^{-}$。

{ ( u , t ) = R ( x − sin ⁡ θ , y − 1 + cos ⁡ θ ) v = M ( θ − t ) L = ∣ t ∣ + ∣ u ∣ + ∣ v ∣  其中  A ∈ [ 0 , π ] (2) \tag{2}

$$\begin{aligned} \left\{\begin{array}{l} (u, t)=R(x-\sin \theta, y-1+\cos \theta) \\ v=M(\theta-t) \\ L=|t|+|u|+|v| \\ \text { 其中 } A \in[0, \pi] \end{array}\right. \end{aligned}$$ ⎩ ⎨ ⎧​(u,t)=R(x−sinθ,y−1+cosθ)v=M(θ−t)L=∣t∣+∣u∣+∣v∣ 其中 A∈[0,π]​​(2)
$$\begin{array}{r}\{\begin{array}{l}\left(u_1,t_1\right)=R(x-\sin \theta ,y-1+\cos \theta )\\ \{\begin{array}{l}{u}_1^2>4\to L=\infty \\ u_1^2\le 4\to \{\begin{array}{l}u=\sqrt{u_1^2-4}\\ \left(u_2,t_2\right)=R(u,2)\\ t=M\left(t_1+t_2\right)\\ v=M(t-u)\\ L=|t|+|u|+|v|\end{array}\end{array}\end{array}\end{array}\text{\hspace{1em}(3)}$$

通过上述三种曲线变换方式，将第 4 种公式的符号变换成了 8 种曲线，这 8种曲线中前 4 种使用式(2) 进行求解，后 4 种使用式(3) 进行求解。

2.3 四段圆弧组成

• 第五类基础曲线形式: $C{C}_{\beta }\mid C_{\beta }C$, 即四段圆弧组成, 第一段与第二段圆弧的圆弧方向相反, 第三段与第四段圆弧的圆弧方向相反, 前两段与后两段圆弧方向相反, 基于此, 可以扩展出四种曲线形式: $L^{+}{R}_{\beta }{}^{+}{L}_{\beta }{}^{-}{R}^{-}、L^{-}{R}_{\beta }{}^{-}{L}_{\beta }{}^{+}{R}^{+}、R^{+}{L}_{\beta }{}^{+}{R}_{\beta }{}^{-}{L}^{-}、R^{-}{L}_{\beta }{}^{-}{R}_{\beta }{}^{+}{L}^{+}$。

• 第六类基础曲线形式: $C\left|C_{\beta }{C}_{\beta }\right|C$, 即四段圆弧组成, 第一段与第二段圆弧的圆弧方向相同, 第三段与第四段圆弧的圆弧方向相同, 前两段与后两段圆弧方向相反, 基于此, 可以扩展出四种曲线形式: $L^{+}{R}_{\beta }{}^{-}{L}_{\beta }{}^{-}{R}^{+}、L^{-}{R}_{\beta }{}^{+}{L}_{\beta }{}^{+}{R}^{-}、R^{+}{L}_{\beta }{}^{-}{R}_{\beta }{}^{-}{L}^{+}、R^{-}{L}_{\beta }{}^{+}{R}_{\beta }{}^{+}{L}^{-}$。

{ ξ = x + sin ⁡ ( θ ) η = y − 1 − cos ⁡ ( θ ) ρ 1 = 2 + ξ 2 + η 2 4 ρ 2 = 20 − ξ 2 − η 2 16 0 ≤ ρ 1 、 ρ 2 ≤ 1 → { u 1 = arccos ⁡ ( ρ 1 ) u 2 = − arccos ⁡ ( ρ 2 ) u = u 1 o r u 2 ( t , v ) = τ ω ( u , − u , ξ , η , θ ) L = ∣ t ∣ + 2 ∗ ∣ u ∣ + ∣ v ∣  其中  u ∈ [ 0 , π / 2 ]  其它  L = ∞ (4) \tag{4} \left\{

$$\begin{array}{l} \xi=x+\sin (\theta) \\ \eta=y-1-\cos (\theta) \\ \rho_{1}=\frac{2+\sqrt{\xi^{2}+\eta^{2}}}{4} \\ \rho_{2}=\frac{20-\xi^{2}-\eta^{2}}{16} \\ 0 \leq \rho_{1} 、 \rho_{2} \leq 1 \rightarrow\left\{\begin{array}{l} u_{1}=\arccos \left(\rho_{1}\right) \\ u_{2}=-\arccos \left(\rho_{2}\right) \\ u=u_1 \quad or \quad u_2\\ (t, v)=\tau \omega(u,-u, \xi, \eta, \theta) \\ L=|t|+2 *|u|+|v| \\ \text { 其中 } u \in[0, \pi / 2] \end{array}$$ \right. \\ \text { 其它 } L=\infty \end{array}\right. ⎩ ⎨ ⎧​ξ=x+sin(θ)η=y−1−cos(θ)ρ1​=42+ξ2+η2 ​​ρ2​=1620−ξ2−η2​0≤ρ1​、ρ2​≤1→⎩ ⎨ ⎧​u1​=arccos(ρ1​)u2​=−arccos(ρ2​)u=u1​oru2​(t,v)=τω(u,−u,ξ,η,θ)L=∣t∣+2∗∣u∣+∣v∣ 其中 u∈[0,π/2]​ 其它 L=∞​(4)
这 8 种曲线可以通过式(4)求解, 上述公式中的 ${\rho }_1,u_1$ 用于第 5 类公式的路径计算, ${\rho }_2,u_2$ 用于第 6 类公式的路径计算，式中的 $\tau \omega$ 是曲线弧度计算函数，一般地，函数的曲线弧度值$(t,v)=\tau \omega (u,v_0,\xi ,\eta ,\theta )$是通过式(5) 进行计算。

{ σ = M ( u − v 0 ) ζ = sin ⁡ ( u ) − sin ⁡ ( σ ) ψ = cos ⁡ ( u ) − cos ⁡ ( σ ) − 1 t 1 = arctan ⁡ η ∗ ζ − ξ ∗ ψ ξ ∗ ζ + η ∗ ψ λ = 2 ∗ ( cos ⁡ ( σ ) − cos ⁡ ( v 0 ) − cos ⁡ ( u ) ) + 3 t = { M ( t 1 + π ) , λ < 0 M ( t 1 ) , λ ≥ 0 v = M ( t − ( u − v 0 ) − θ ) (5) \tag{5} \left\{

$$\begin{array}{l} \sigma=M(u-v_0) \\ \zeta=\sin (u)-\sin (\sigma) \\ \psi=\cos (u)-\cos (\sigma)-1 \\ t_{1}=\arctan \frac{\eta * \zeta-\xi * \psi}{\xi * \zeta+\eta * \psi} \\ \lambda=2 *(\cos (\sigma)-\cos (v_0)-\cos (u))+3 \\ t=\left\{\begin{array}{l} M\left(t_{1}+\pi\right), \lambda<0 \\ M\left(t_{1}\right), \lambda \geq 0 \end{array}$$ \right. \\ v=M(t-(u-v_0)-\theta) \end{array}\right. ⎩ ⎨ ⎧​σ=M(u−v0​)ζ=sin(u)−sin(σ)ψ=cos(u)−cos(σ)−1t1​=arctanξ∗ζ+η∗ψη∗ζ−ξ∗ψ​λ=2∗(cos(σ)−cos(v0​)−cos(u))+3t={M(t1​+π),λ<0M(t1​),λ≥0​v=M(t−(u−v0​)−θ)​(5)

2.4 三段圆弧与一条直线段组成

• 第七类基础曲线形式: $C\mid C_{\pi /2}SC$, 即三段圆弧与一条直线段组成, 第二段圆弧为 $\pi /2$ 圆弧, 第三段为直线路径, 第一段与后三段圆弧方向相反, 基于此, 可以扩展出八种曲线形式: $L^{+}{R}_{\pi /2}^{-}{S}^{-}{R}^{-}、L^{-}{R}_{\pi /2}^{+}{S}^{+}{R}^{+}、R^{+}{L}_{\pi /2}^{-}{S}^{-}{L}^{-}、R^{-}{L}_{\pi /2}^{+}{S}^{+}{L}^{+}、L^{+}{R}_{\pi /2}^{-}{S}^{-}{L}^{-}$、 $L^{-}{R}_{\pi /2}^{+}{S}^{+}{L}^{+}$、 $R^{+}{L}_{\pi /2}^{-}{S}^{-}{R}^{-}$、 $R^{-}{L}_{\pi /2}^{+}{S}^{+}{R}^{+}$。

• 第八类基础曲线形式: $CS{C}_{\pi /2}\mid C$, 即三段圆弧与一段直线组成, 第三段圆弧为 $\pi /2$ 圆弧, 第二段为直线路径, 第四段与前三段圆弧方向相反, 基于此, 可以扩展出八种曲线形式: $L^{+}{S}^{+}{L}_{\pi /2}^{+}{R}^{-}、L^{-}{S}^{-}{L}_{\pi /2}^{-}{R}^{+}、R^{+}{S}^{+}{R}_{\pi /2}^{+}{L}^{-}、R^{-}{S}^{-}{R}_{\pi /2}^{-}{L}^{+}、R^{+}{S}^{+}{L}_{\pi /2}^{+}{R}^{-}、R^{-}{S}^{-}{L}_{\pi /2}^{-}{R}^{+}$、 $L^{+}{S}^{+}{R}_{\pi /2}^{+}{L}^{-}、L^{-}{S}^{-}{R}_{\pi /2}^{-}{L}^{+}$。

{ ζ = x + sin ⁡ ( θ ) η = y − 1 − cos ⁡ ( θ ) ( ρ , ϑ ) = R ( − η , ζ ) { ( 1 ) ρ ≥ 2 → { ( T , ϑ 1 ) = R ( ρ 2 − 4 , − 2 ) t = M ( ϑ − ϑ 1 ) u = 2 − ϑ 1 v = M ( θ − t − π / 2 ) ( 2 ) { t = ϑ u = 2 − ρ v = M ( t + π / 2 − θ ) L = ∣ t ∣ + ∣ u ∣ + ∣ v ∣ + π / 2  其他， L = ∞ (6) \tag{6} \left\{

$$\begin{array}{l} \zeta=x+\sin (\theta) \\ \eta=y-1-\cos (\theta) \\ \left(\rho, \vartheta\right)=R(-\eta, \zeta) \\ \left\{\begin{array}{l} (1) \rho \geq 2 \rightarrow\left\{\begin{array}{l} \left(T, \vartheta_{1}\right)=R\left(\sqrt{\rho^{2}-4},-2\right) \\ t=M\left(\vartheta-\vartheta_{1}\right) \\ u=2-\vartheta_{1} \\ v=M(\theta-t-\pi / 2) \end{array}$$ \right. \\ (2)\left\{ $$\begin{array}{l} t=\vartheta \\ u=2-\rho \\ v=M(t+\pi / 2-\theta) \end{array}$$ \right. \\ L=|t|+|u|+|v|+\pi / 2 \\ \text { 其他，} L=\infty \end{array}\right. \end{array}\right. ⎩ ⎨ ⎧​ζ=x+sin(θ)η=y−1−cos(θ)(ρ,ϑ)=R(−η,ζ)⎩ ⎨ ⎧​(1)ρ≥2→⎩ ⎨ ⎧​(T,ϑ1​)=R(ρ2−4 ​,−2)t=M(ϑ−ϑ1​)u=2−ϑ1​v=M(θ−t−π/2)​(2)⎩ ⎨ ⎧​t=ϑu=2−ρv=M(t+π/2−θ)​L=∣t∣+∣u∣+∣v∣+π/2 其他，L=∞​​(6)

式中 $u$ 表示直线运动长度，$t、v$ 表示圆弧运动的弧度值，式(6)中，当圆弧路经由两段顺时针方向的曲线构成就利用(1)子式计算，当圆弧路径由两段逆时针方向的曲线构成就利用(2)子式计算。

2.5 四段圆弧与一条直线段组成

• 第九类基础曲线形式: $C\left|C_{\pi /2}S{C}_{\pi /2}\right|C$, 即四段圆弧与一条直线段组成, 第二段圆弧与第四段圆弧为 $\pi /2$ 圆弧, 中间路径为直线路径, 第二、三、四段圆弧方向与第一段、 第五段路径方向相反, 基于此, 可以扩展出四种曲线形式: $L^{+}{R}_{\pi /2}^{-}{S}^{-}{L}_{\pi /2}^{-}{R}^{+}、L^{-}{R}_{\pi /2}^{+}{S}^{+}{L}_{\pi /2}^{+}{R}^{-}$、 $R^{+}{L}_{\pi /2}^{-}{S}^{-}{R}_{\pi /2}^{-}{L}^{+}、R^{-}{L}_{\pi /2}^{+}{S}^{+}{R}_{\pi /2}^{+}{L}^{-}$。

{ ζ = x + sin ⁡ ( θ ) η = y − 1 − cos ⁡ ( θ ) ( ρ , ϑ ) = R ( ζ , η ) ρ ≥ 2 → { t = M ( ϑ − arccos ⁡ ( − 2 ρ ) ) , t ∈ [ − π 2 , π 2 ] { t ≤ 0 → L = ∞ t > 0 → { u = 4 − ζ + 2 ∗ cos ⁡ ( t ) sin ⁡ ( t ) v = M ( t − θ ) L = ∣ t ∣ + ∣ u ∣ + ∣ v ∣ + 2 ∗ π 2 其他,  L = ∞ (7) \tag{7} \left\{

$$\begin{array}{l} \zeta=x+\sin (\theta) \\ \eta=y-1-\cos (\theta) \\ (\rho, \vartheta)=R(\zeta, \eta) \\ \rho \geq 2 \rightarrow\left\{\begin{array}{l} t=M\left(\vartheta-\arccos \left(-\frac{2}{\rho}\right)\right), t \in\left[-\frac{\pi}{2}, \frac{\pi}{2}\right] \\ \left\{\begin{array}{l} t \leq 0 \rightarrow L=\infty\\ t>0 \rightarrow\left\{\begin{array}{l} u=4-\frac{\zeta+2 * \cos (t)}{\sin (t)} \\ v=M(t-\theta) \\ L=|t|+|u|+|v|+2 * \frac{\pi}{2} \end{array}$$ \right. \\ \end{array}\right. \\ \end{array}\right.\\ \text{其他, }L=\infty \end{array}\right. ⎩ ⎨ ⎧​ζ=x+sin(θ)η=y−1−cos(θ)(ρ,ϑ)=R(ζ,η)ρ≥2→⎩ ⎨ ⎧​t=M(ϑ−arccos(−ρ2​)),t∈[−2π​,2π​]⎩ ⎨ ⎧​t≤0→L=∞t>0→⎩ ⎨ ⎧​u=4−sin(t)ζ+2∗cos(t)​v=M(t−θ)L=∣t∣+∣u∣+∣v∣+2∗2π​​​​其他, L=∞​(7)

3. python实现

由于自己写的代码bug实在太多了。。。不想改了，所以这边贴出几个开源的github代码：

python:

• https://github.com/ghliu/pyReedsShepp
• https://github.com/nathanlct/reeds-shepp-curves
• PythonRobotics

4. c++实现

C++：

• https://ompl.kavrakilab.org/ReedsSheppStateSpace_8cpp_source.html