Reeds-Shepp曲线公式推导及代码实现-第一部分（共三部分）

注：由于优快云篇幅限制，文章只能拆分成三部分，完整的文章可见Reeds-Shepp曲线公式推导及代码实现

优快云剩余部分：
Reeds-Shepp曲线公式推导及代码实现-第一部分（共三部分）
Reeds-Shepp曲线公式推导及代码实现-第二部分（共三部分）
Reeds-Shepp曲线公式推导及代码实现-第三部分（共三部分）

1. 前言

Reeds-Shepp曲线是基于Dubins曲线进行改进的一种规划算法，其将后退也加入到了规划中，这样就使得在某些情况下可以得到比Dubins曲线更优的路径。

1.1 Dubins曲线

Dubins路径是一个简单汽车模型的解析路径规划（不考虑后退）算法。它可以生成具有最大曲率约束和切线（偏航角）约束的二维平面姿态之间的最短路径，其生成的路径由3段最大曲率或者一段直线组成，每段路段类型可以分为3种类型：

符号	含义	绕圆方向
$L$	左转	逆时针
$R$	右转	顺时针
$S$	直行	/

由于在路径规划的时候，起点和终点都是存在航向的，因此两点之间并不一定是直线最短，《On Curves of Minimal Length with a Constraint on Average Curvature, and with Prescribed Initial and Terminal Positions and Tangents》中证明了对于每种特定终点情况下的最佳路径集合中，最多只有6条可选曲线，其分别为：$LRL$、$LSL$、$LSR$、$RLR$、$RSR$和$RSL$。

其中$LSL$、$RSR$、$RSL$和$LSR$可以看作是一种情况，即一段圆弧、一段直线和一段圆弧，可以统一用$CSC$来表示，这里$C$代表圆弧、$S$代表一条直线；则对于$LRL$和$RLR$是另外一种都是圆弧的情况，可以用$CCC$来表示，综上可记为
C S C ⇒ { L S L 、 R S R 、 R S L 、 L S R } C C C ⇒ { L R L 、 R L R } \begin{align} CSC&\Rightarrow\{LSL、RSR、RSL、LSR\}\\ CCC&\Rightarrow\{LRL、RLR\} \end{align} CSCCCC​⇒{LSL、RSR、RSL、LSR}⇒{LRL、RLR}​​

1.2 Reeds-Shepp曲线

在Reeds-Shepp曲线路径中，允许反方向运动，因此相当于在Dubins曲线上区分了运动方向，我们将字母上添加正负号来进行区分前进和后退，因此其有以下6种类型：

符号	含义	绕圆方向
$L^{+}$	向前左转	逆时针
$L^{-}$	向后左转	顺时针
$R^{+}$	向前右转	顺时针
$R^{-}$	向后右转	逆时针
$L^{+}$	向前直行	/
$L^{-}$	向后直行	/

同样的我们使用$C$代表圆弧($L^{\pm }$、$S^{\pm }$)、$S$代表一条直线($S^{\pm }$)。

Reeds-Shepp算法在论文 《optimal path for a car that goes both forward and backwards》中证明了从起点到终点的最短路径一定是以下组合中的一个：$C|C|C$、$CC|C$、$C|CC$、$CSC$、$C{C}_{\beta }|C_{\beta }C$、$C|C_{\beta }{C}_{\beta }|C$、$C|C_{\pi /2}SC$、$CS{C}_{\pi /2}|C$和$C|C_{\pi /2}S{C}_{\pi /2}|C$，这里$|$表示方向的反转。

其对应48的组合如下所示：

形式	组合
$C|C|C$	$L^{+}{R}^{-}{L}^{+}$、$L^{-}{R}^{+}{L}^{-}$、$R^{+}{L}^{-}{R}^{+}$、$R^{-}{L}^{+}{R}^{-}$
$CC|C$	$L^{+}{R}^{+}{L}^{-}$、$L^{-}{R}^{-}{L}^{+}$、$R^{+}{L}^{+}{R}^{-}$、$R^{-}{L}^{-}{R}^{+}$
$C|CC$	$L^{+}{R}^{-}{L}^{-}$、$L^{-}{R}^{+}{L}^{+}$、$R^{+}{L}^{-}{R}^{-}$、$R^{-}{L}^{+}{R}^{+}$
$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}^{-}$
$C{C}_{\beta }|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|C_{\beta }{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|C_{\pi /2}SC$	$L^{+}{R}_{\pi /2}^{-}{S}^{-}{L}^{-}$、$L^{-}{R}_{\pi /2}^{+}{S}^{+}{L}^{+}$、$R^{+}{L}_{\pi /2}^{-}{S}^{-}{R}^{-}$、$R^{-}{L}_{\pi /2}^{+}{S}^{+}{R}^{+}$ $L^{+}{R}_{\pi /2}^{-}{S}^{-}{R}^{-}$、$L^{-}{R}_{\pi /2}^{+}{S}^{+}{R}^{+}$、$R^{+}{L}_{\pi /2}^{-}{S}^{-}{L}^{-}$、$R^{-}{L}_{\pi /2}^{+}{S}^{+}{L}^{+}$
$CS{C}_{\pi /2}|C$	$L^{+}{S}^{+}{R}_{\pi /2}^{+}{L}^{-}$、$L^{-}{S}^{-}{R}_{\pi /2}^{-}{L}^{+}$、$R^{+}{S}^{+}{L}_{\pi /2}^{+}{R}^{-}$、$R^{-}{S}^{-}{L}_{\pi /2}^{-}{R}^{+}$ $L^{+}{S}^{+}{L}_{\pi /2}^{+}{R}^{-}$、$L^{-}{S}^{-}{L}_{\pi /2}^{-}{R}^{+}$、$R^{+}{S}^{+}{R}_{\pi /2}^{+}{L}^{-}$、$R^{-}{S}^{-}{R}_{\pi /2}^{-}{L}^{+}$
$C|C_{\pi /2}S{C}_{\pi /2}|C$	$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}^{-}$

其中角标$\beta$代表$C$的旋转弧度是$\beta$，角标$\pi /2$代表$C$的旋转弧度是$\pi /2$。

Dubins曲线是从公式1中的6个曲线里选出最短的那条，而Reeds-Shepp曲线是从上面48个曲线中选出最短的那条。

注：最开始的论文认为最短的曲线一定在48条曲线中，后人研究发现有两条曲线不会是最短的，所以后来搜索范围减小到46条。

对于上述的48中组合，实际可以通过简单的变换，实现部分组合的转换，这种变换包括：

• 时间翻转

  时间翻转就将车原行驶方向取反，即通过交换字母上的正负号，例如，$L^{-}{R}^{+}{L}^{-}$可以通过$L^{+}{R}^{-}{L}^{+}$进行时间变换获得，即原始路径从$(0,0,0)$到$(x,y,\phi )$，当经过时间变换（符号取反），得到的新路径关于$Y$轴对称，即得到新的目标点$(-x,y,-\phi )$，因此对于$L^{-}{R}^{+}{L}^{-}$可以通过$L^{+}{R}^{-}{L}^{+}$的求解公式，将目标点变为$(-x,y,-\phi )$获得。

• 反射变换

  反射变换就是通过交换字母$L$和$R$，即车辆通过左右反向转向，例如，$R^{+}{L}^{-}{R}^{+}$可以通过$L^{+}{R}^{-}{L}^{+}$进行反射变换获得，即原始路径从$(0,0,0)$到$(x,y,\phi )$，当经过反射变换（交换字母$L$和$R$），得到的新路径关于$X$轴对称，即得到新的目标点$(x,-y,-\phi )$，因此对于$R^{+}{L}^{-}{R}^{+}$可以通过$L^{+}{R}^{-}{L}^{+}$的求解公式，将目标点变为$(x,-y,-\phi )$获得。

• 逆向变换

  逆向变换就是将时间翻转和反射变换结合起来，即将先将原组合进行时间翻转，然后再对时间翻转后的结果进行反射变换，例如，$R^{-}{L}^{+}{R}^{-}$可以通过$L^{+}{R}^{-}{L}^{+}$进行逆向变换获得，即原始路径从$(0,0,0)$到$(x,y,\phi )$，当经过时间翻转得到$(-x,y,-\phi )$，再对其进行反射变换，即得到新的目标点$(-x,-y,\phi )$，因此对于$R^{-}{L}^{+}{R}^{-}$可以通过$L^{+}{R}^{-}{L}^{+}$的求解公式，将目标点变为$(-x,-y,\phi )$获得。

通过上述变换，我们可以消除48种中的部分字段，最终保留以下12个字段：

形式	组合
$C|C|C$	$L^{+}{R}^{-}{L}^{+}$、$\bcancel{L^{-}{R}^{+}{L}^{-}}$、$\bcancel{R^{+}{L}^{-}{R}^{+}}$、$\bcancel{R^{-}{L}^{+}{R}^{-}}$
$CC|C$	$L^{+}{R}^{+}{L}^{-}$、$L\bcancel{{}^{-}{R}^{-}{L}^{+}}$、$\bcancel{R^{+}{L}^{+}{R}^{-}}$、$\bcancel{R^{-}{L}^{-}{R}^{+}}$
$C|CC$	$L^{+}{R}^{-}{L}^{-}$、$L\bcancel{{}^{-}{R}^{+}{L}^{+}}$、$\bcancel{R^{+}{L}^{-}{R}^{-}}$、$\bcancel{R^{-}{L}^{+}{R}^{+}}$
$CSC$	$L^{+}{S}^{+}{L}^{+}$、$L\bcancel{{}^{-}{S}^{-}{L}^{-}}$、$\bcancel{R^{+}{S}^{+}{R}^{+}}$、$\bcancel{R^{-}{S}^{-}{R}^{-}}$ $L^{+}{S}^{+}{R}^{+}$、 $\bcancel{L^{-}{S}^{-}{R}^{-}}$、 $\bcancel{R^{+}{S}^{+}{L}^{+}}$、 $\bcancel{R^{-}{S}^{-}{L}^{-}}$
$C{C}_{\beta }|C_{\beta }C$	$L^{+}{R}_{\beta }^{+}{L}_{\beta }^{-}{R}^{-}$、$L\bcancel{{}^{-}{R}_{\beta }^{-}{L}_{\beta }^{+}{R}^{+}}$、$\bcancel{R^{+}{L}_{\beta }^{+}{R}_{\beta }^{-}{L}^{-}}$、$\bcancel{R^{-}{L}_{\beta }^{-}{R}_{\beta }^{+}{L}^{+}}$
$C|C_{\beta }{C}_{\beta }|C$	$L^{+}{R}_{\beta }^{-}{L}_{\beta }^{-}{R}^{+}$、$L\bcancel{{}^{-}{R}_{\beta }^{+}{L}_{\beta }^{+}{R}^{-}}$、$\bcancel{R^{+}{L}_{\beta }^{-}{R}_{\beta }^{-}{L}^{+}}$、$\bcancel{R^{-}{L}_{\beta }^{+}{R}_{\beta }^{+}{L}^{-}}$
$C|C_{\pi /2}SC$	$L^{+}{R}_{\pi /2}^{-}{S}^{-}{L}^{-}$、$L\bcancel{{}^{-}{R}_{\pi /2}^{+}{S}^{+}{L}^{+}}$、$\bcancel{R^{+}{L}_{\pi /2}^{-}{S}^{-}{R}^{-}}$、$\bcancel{R^{-}{L}_{\pi /2}^{+}{S}^{+}{R}^{+}}$ $L^{+}{R}_{\pi /2}^{-}{S}^{-}{R}^{-}$、$\bcancel{L^{-}{R}_{\pi /2}^{+}{S}^{+}{R}^{+}}$、$\bcancel{R^{+}{L}_{\pi /2}^{-}{S}^{-}{L}^{-}}$、$\bcancel{R^{-}{L}_{\pi /2}^{+}{S}^{+}{L}^{+}}$
$CS{C}_{\pi /2}|C$	$L^{+}{S}^{+}{R}_{\pi /2}^{+}{L}^{-}$、$L\bcancel{{}^{-}{S}^{-}{R}_{\pi /2}^{-}{L}^{+}}$、$\bcancel{R^{+}{S}^{+}{L}_{\pi /2}^{+}{R}^{-}}$、$\bcancel{R^{-}{S}^{-}{L}_{\pi /2}^{-}{R}^{+}}$ $L^{+}{S}^{+}{L}_{\pi /2}^{+}{R}^{-}$、$\bcancel{L^{-}{S}^{-}{L}_{\pi /2}^{-}{R}^{+}}$、$\bcancel{R^{+}{S}^{+}{R}_{\pi /2}^{+}{L}^{-}}$、$\bcancel{R^{-}{S}^{-}{R}_{\pi /2}^{-}{L}^{+}}$
$C|C_{\pi /2}S{C}_{\pi /2}|C$	$L^{+}{R}_{\pi /2}^{-}{S}^{-}{L}_{\pi /2}^{-}{R}^{+}$、$L\bcancel{{}^{-}{R}_{\pi /2}^{+}{S}^{+}{L}_{\pi /2}^{+}{R}^{-}}$、$\bcancel{R^{+}{L}_{\pi /2}^{-}{S}^{-}{R}_{\pi /2}^{-}{L}^{+}}$、$\bcancel{R^{-}{L}_{\pi /2}^{+}{S}^{+}{R}_{\pi /2}^{+}{L}^{-}}$

消除部分的字段均可以过上述12个字段运用上述三个变换性质得到，也就是说所有的48种轨迹均可以通过上述12种轨迹的求解公式直接或者间接的求解出来。

1.3 车辆模型

Reeds-Shepp曲线是基于一种简化的车辆运动学模型：
$$\{\begin{array}{l}\dot{x}=v\cos \phi \\ \dot{y}=v\sin \phi \\ \dot{\phi }=\frac{v}{R}\\ R=\frac{L}{\tan {\delta }_f}\end{array}$$
其中：

• $v$：车辆的速度；
• $y$：车辆后轴中心点的纵坐标；
• $x$：车辆后轴中心点的横坐标；
• $R$：车辆的转弯半径；
• $L$：车辆的轴距；
• $\phi$：车辆的航向角；
• ${\delta }_f$：车辆前轮转角。

有了运动学方程，在给定时间间隔$T$和速度$v$，就可以得到一系列的点，对应的就是满足车辆运动学约束的路径，点的表达式可以表示为：
$$\begin{array}{rl}x& =x_{pre}+vT\cos \phi \\ y& =y_{pre}+vT\sin \phi \\ \phi & ={\phi }_{pre}+\frac{vT}{R}\end{array}$$

1.4 车辆位姿的归一化

车辆的起点和终点的位姿是不能完全穷举的，为了方便计算，我们会先将起点和终点的位姿归一化，假设起点位姿为$P_s(x_s,y_s,{\phi }_s)$，终点位姿为$P_e(x_e,y_e,{\phi }_e)$，车辆的最小转弯半径为$R_{min}$。

首先将车辆起点平移到原点，它后将车辆起点朝向转向到x轴的正方向，然后再按照最小转弯半径对向量$\overrightarrow{P_s{P}_e}$进行缩放，这样就可以把最小转弯半径归一化为1，其变换过程如下：

原始向量$\overrightarrow{P_s{P}_e}$：
$$\overrightarrow{P_s{P}_e}=\left[\begin{array}{c}{x}_e-x_s\\ y_e-y_s\end{array}\right]=\left[\begin{array}{c}dx\\ dy\end{array}\right]$$
将起点朝向转到x轴正方向对应的旋转矩阵为
$$\left[\begin{array}{cc}\cos (-{\phi }_s)& -\sin (-{\phi }_s)\\ \sin (-{\phi }_s)& \cos (-{\phi }_s)\end{array}\right]=\left[\begin{array}{cc}\cos {\phi }_s& \sin {\phi }_s\\ -\sin {\phi }_s& \cos {\phi }_s\end{array}\right]$$
旋转后的向量为
$$\left[\begin{array}{cc}\cos {\phi }_s& \sin {\phi }_s\\ -\sin {\phi }_s& \cos {\phi }_s\end{array}\right]\left[\begin{array}{c}dx\\ dy\end{array}\right]=\left[\begin{array}{c}dx\ast \cos {\phi }_s+dy\ast \sin {\phi }_s\\ -dx\ast \sin {\phi }_s+dy\ast \cos {\phi }_s\end{array}\right]$$
按照最小转弯半径$R_{min}$归一化后起点位姿$q_s$和终点位姿$q_e$分别为
$$\begin{gathered} q_s=\left[\begin{array}{c}0\\ 0\\ 0\end{array}\right] \\ {q}_e=\left[\begin{array}{c}(dx\ast \cos {\phi }_s+dy\ast \sin {\phi }_s)/R_{min}\\ (-dx\ast \sin {\phi }_s+dy\ast \cos {\phi }_s)/R_{min}\\ {\phi }_e-{\phi }_s\end{array}\right] \end{gathered}$$
在归一化后，车辆圆弧的运动都是基于单位圆进行的，车辆形式的弧长和变化的角度一致，方便后面的计算。