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

文章目录

• 参考资料
• 1. Reeds-Shepp 曲线
• • 1.1 基本概念
  • 1.2 字段组合
  • 1.3 时间翻转(timeflip)、反射(reflect)和向后变换(backwards)
  • • 1.3.1 时间翻转(timeflip)
    • 1.3.2 反射(reflect)
    • 1.3.3 向后变换(backwards)
  • 1.4 48 个字段组合
• 2. RS曲线求解
• • 2.1 三段圆弧组成
  • 2.2 两段圆弧与直线组成
  • 2.3 四段圆弧组成
  • 2.4 三段圆弧与一条直线段组成
  • 2.5 四段圆弧与一条直线段组成
• 3. python实现
• 4. c++实现

参考资料

• 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$ 字符可以给出如下集合：

$$\begin{array}{rl}& CCC\leftarrow \left\{C^{+}{C}^{-}{C}^{+}{C}^{+}{C}^{-}{C}^{-}{C}^{+}{C}^{+}{C}^{-}{C}^{+}{C}_{\beta }^{+}{C}_{\beta }^{-}{C}^{-}{C}^{+}{C}_{\beta }^{-}{C}_{\beta }^{-}{C}^{+}\right\}\\ & CSC\leftarrow \left\{C^{+}{S}^{+}{C}^{+}{C}^{-}{C}_{\pi /2}^{+}{S}^{+}{C}^{+}{C}^{+}{S}^{+}{C}_{\pi /2}^{+}{C}^{-}{C}^{-}{C}_{\pi /2}^{+}{S}^{+}{C}_{\pi /2}^{+}{C}^{-}\right\}\end{array}\text{\hspace{1em}(1)}$$

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

为了以紧凑的表示，避免$\pm$ ，可以写出充分路径的列表如下：
$$\begin{array}{rl}& CCC\leftarrow \left\{\begin{array}{ccccc}C|C|C& C\mid CC& CC\mid C& C{C}_{\beta }\mid C_{\beta }C& C\left|C_{\beta }{C}_{\beta }\right|C\end{array}\right\}\\ & CSC\leftarrow \left\{\begin{array}{llll}CSC& C\mid C_{\pi /2}SC& CS{C}_{\pi /2}\mid C& C\left|C_{\pi /2}S{C}_{\pi /2}\right|C\end{array}\right\}\\ & \end{array}\text{\hspace{1em}(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)进行求解 。
$$\{\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}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}\\ \text{ 其中 }A\in \left[\frac{\pi }{2},\pi \right]\end{array}\end{array}\text{\hspace{1em}(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 )$的关系可以表示为： $$\begin{array}{r}\{\begin{array}{l}r\cos \theta =x\\ r\sin \theta =y\end{array}\end{array}$$

$\psi =M(\theta )$即表示 $$\begin{array}{r}\{\begin{array}{l}\psi =\theta \mathrm{m}\mathrm{o}\mathrm{d}2\pi \\ -\pi \le \psi <\pi \end{array}\end{array}$$

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

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}^{-}$。

$$\begin{array}{r}\{\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}\end{array}\text{\hspace{1em}(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}^{-}$。

$$\{\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\le {\rho }_1、{\rho }_2\le 1\to \{\begin{array}{l}{u}_1=\arccos \left({\rho }_1\right)\\ u_2=-\arccos \left({\rho }_2\right)\\ u=u_{1}or{u}_2\\ (t,v)=\tau \omega (u,-u,\xi ,\eta ,\theta )\\ L=|t|+2\ast |u|+|v|\\ \text{ 其中 }u\in [0,\pi /2]\end{array}\\ \text{ 其它 }L=\infty \end{array}\text{\hspace{1em}(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) 进行计算。

$$\{\begin{array}{l}\sigma =M(u-v_0)\\ \zeta =\sin (u)-\sin (\sigma )\\ \psi =\cos (u)-\cos (\sigma )-1\\ t_1=\arctan \frac{\eta \ast \zeta -\xi \ast \psi }{\xi \ast \zeta +\eta \ast \psi }\\ \lambda =2\ast (\cos (\sigma )-\cos (v_0)-\cos (u))+3\\ t=\{\begin{array}{l}M\left(t_1+\pi \right),\lambda <0\\ M\left(t_1\right),\lambda \ge 0\end{array}\\ v=M(t-(u-v_0)-\theta )\end{array}\text{\hspace{1em}(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}^{+}$。

$$\{\begin{array}{l}\zeta =x+\sin (\theta )\\ \eta =y-1-\cos (\theta )\\ \left(\rho ,\vartheta \right)=R(-\eta ,\zeta )\\ \{\begin{array}{l}(1)\rho \ge 2\to \{\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}\\ (2)\{\begin{array}{l}t=\vartheta \\ u=2-\rho \\ v=M(t+\pi /2-\theta )\end{array}\\ L=|t|+|u|+|v|+\pi /2\\ \text{ 其他，}L=\infty \end{array}\end{array}\text{\hspace{1em}(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}^{-}$。

$$\{\begin{array}{l}\zeta =x+\sin (\theta )\\ \eta =y-1-\cos (\theta )\\ (\rho ,\vartheta )=R(\zeta ,\eta )\\ \rho \ge 2\to \{\begin{array}{l}t=M\left(\vartheta -\arccos \left(-\frac{2}{\rho }\right)\right),t\in \left[-\frac{\pi }{2},\frac{\pi }{2}\right]\\ \{\begin{array}{l}t\le 0\to L=\infty \\ t>0\to \{\begin{array}{l}u=4-\frac{\zeta +2\ast \cos (t)}{\sin (t)}\\ v=M(t-\theta )\\ L=|t|+|u|+|v|+2\ast \frac{\pi }{2}\end{array}\end{array}\end{array}\\ \text{其他, }L=\infty \end{array}\text{\hspace{1em}(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

来源地址：https://blog.csdn.net/weixin_42301220/article/details/125382518