无人驾驶学习笔记--路径规划（二）【Dubins曲线和Reeds-Shepp曲线】

在自动驾驶行驶过程中，包含Free space场景和停车场景，在这种不规则道路结构中，无人驾驶车辆可以使用不同的曲线来表示车辆的行驶轨迹，而常见且经典的曲线则就是Dubins曲线和Reeds-Shepp曲线。

I. Dubins 曲线

参考文章：Dubins曲线详细笔记
https://zhuanlan.zhihu.com/p/414753861

1. 基本理论

Dubins曲线限制车辆只能向前行驶，规划出的曲线由三段直线或圆弧组成，能够满足车辆的最小转弯半径、起点航向角、终点航向角和车辆动力学约束，但规划出的曲线在两段交点处曲率不连续。
对于指定起点$q_i$到终点 $q_g$ 的最短路径，可以写成求解如下优化问题：
$$\min L(q,u)=\min {\int }_0^{t_F}\sqrt{\dot{x}(t{)}^2+\dot{y}(t{)}^2}\text{d}t$$
假设车辆运行速度恒定，那么，上式中的代价函数 $L(q,u)$ 只与 $t_F$ 相关。

已经证明，给定任意的起点和终点，Dubin car的最短路径总是可以表示为不超过三种的motion primitives.

Dubin car 的action 只有 $u\in \{-1,0,1\}$. 并且可以证明，只有六种motion primitives 是最优的。
$$\{L_{\alpha }{R}_{\beta }{L}_{\gamma },R_{\alpha }{L}_{\beta }{R}_{\gamma },L_{\alpha }{S}_d{L}_{\gamma },L_{\alpha }{S}_d{R}_{\gamma },R_{\alpha }{S}_d{L}_{\gamma },R_{\alpha }{S}_d{R}_{\gamma }\}$$
其中 $\alpha \in [0,2\pi ),\gamma \in [0,2\pi ),\beta \in (\pi ,2\pi ),d\ge 0$.

2. 曲线计算

2.1 坐标转换

设起点为$s(x_i,y_i,{\alpha }_i)$，终点为$g(x_g,y_g,{\beta }_g)$，通过坐标变换将起点和终点放置于x轴，则起点终点连线逆时针旋转$\theta$角，此时起点坐标为$s(0,0,\alpha )$，终点坐标为$g(d,0,\beta )$，则

$$\begin{gathered} \theta =\arctan (\frac{y_g-y_i}{x_g-x_i}) \\ D=\sqrt{(x_i-x_g{)}^2+(y_i-y_g{)}^2} \\ d=\frac{D}{R} \\ \alpha =mod({\alpha }_i-\theta ,2\pi ) \\ \beta =mod({\beta }_g-\theta ,2\pi ) \end{gathered}$$
$\theta$为起点和终点的航向差，以上角度的取值均在$[0,2\pi ]$之间。上述第三式是为了将半径归一化，在后续的计算中将转弯半径都作为1，便于从角度计算弧长，简化计算。

2.2 LSL

$$\begin{gathered} t_{lsl}=-\alpha +\arctan (\frac{\cos \beta -\cos \alpha }{d+\sin \alpha -\sin \beta })[mod2\pi ] \\ {p}_{lsl}=\sqrt{2+d^2-2\cos (\alpha -\beta )+2d(\sin \alpha -\sin \beta )} \\ {q}_{lsl}=\beta -\arctan \frac{\cos \beta -\cos \alpha }{d+\sin \alpha -\sin \beta }[mod2\pi ] \end{gathered}$$
总长度等于：
$$L_{lsl}=t_{lsl}+p_{lsl}+q_{lsl}=-\alpha +\beta +p_{lsl}$$

2.3 LRL

$$\begin{gathered} t_{lrl}=(-\alpha +\arctan (\frac{\cos \beta -\cos \alpha }{d+\sin \alpha -\sin \beta })+\frac{p_{lrl}}{2})[mod2\pi ] \\ {p}_{lrl}=\arccos \frac{1}{8}(6-d^2+2\cos (\alpha -\beta )+2d(\sin \alpha -\sin \beta ))[mod2\pi ] \\ {q}_{lrl}=\beta [mod2\pi ]-\alpha +2p_{lrl}[mod2\pi ] \\ {L}_{lrl}=t_{lrl}+p_{lrl}+q_{lrl}=-\alpha +\beta +2p_{lrl} \end{gathered}$$

2.4 RSR

$$\begin{gathered} t_{rsr}=\alpha -\arctan \frac{\cos \alpha -\cos \beta }{d-\sin \alpha +\sin \beta }[mod2\pi ] \\ {p}_{rsr}=\sqrt{2+d^2-2\cos (\alpha -\beta )+2d(\sin \beta -\sin \alpha )} \\ {q}_{rsr}=-\beta [mod2\pi ]+\arctan (\frac{\cos \alpha -\cos \beta }{d-\sin \alpha +\sin \beta })[mod2\pi ] \\ {L}_{rsr}=t_{rsr}+p_{rsr}+q_{rsr}=\alpha -\beta +p_{rsr} \end{gathered}$$

2.5 LSR

$$\begin{gathered} t_{lsr}=(-\alpha +\arctan (\frac{-\cos \alpha -\cos \beta }{d+\sin \alpha +\sin \beta })-\arctan (\frac{-2}{p_{lsr}}))[mod2\pi ] \\ {p}_{lsr}=\sqrt{-2+d^2+2\cos (\alpha -\beta )+2d(\sin \alpha +\sin \beta )} \\ {q}_{lsr}=-\beta [mod2\pi ]+\arctan (\frac{-\cos \alpha -\cos \beta }{d+\sin \alpha +\sin \beta })-\arctan (\frac{-2}{p_{lsr}})[mod2\pi ] \\ {L}_{lsr}=t_{lsr}+p_{lsr}+q_{lsr}=\alpha -\beta +2t_{lsr}+p_{lsr} \end{gathered}$$

2.6 RSL

$$\begin{gathered} t_{rsl}=\alpha -\arctan (\frac{\cos \alpha +\cos \beta }{d-\sin \alpha -\sin \beta })+\arctan (\frac{2}{p_{rsl}})[mod2\pi ] \\ {p}_{rsl}=\sqrt{d^2-2+2\cos (\alpha -\beta )-2d(\sin \alpha +\sin \beta )} \\ {q}_{rsl}=\beta [mod2\pi ]-\arctan (\frac{\cos \alpha +\cos \beta }{d-\sin \alpha -\sin \beta })+\arctan (\frac{2}{p_{rsl}})[mod2\pi ] \\ {L}_{rsl}=t_{rsl}+p_{rsl}+q_{rsl}=-\alpha +\beta +2t_{rsl}+p_{rsl} \end{gathered}$$

2.7 RLR

$$\begin{gathered} t_{rlr}=\alpha -\arctan (\frac{\cos \alpha -\cos \beta }{d-\sin \alpha +\sin \beta })+\frac{p_{rlr}}{2}[mod2\pi ] \\ {p}_{rlr}=\arccos \frac{1}{8}(6-d^2+2\cos (\alpha -\beta )+2d(\sin \alpha -\sin \beta )) \\ {q}_{rlr}=\alpha -\beta -t_{rlr}+p_{rlr}[mod2\pi ] \\ {L}_{rlr}=t_{rlr}+p_{rlr}+q_{rlr}=\alpha -\beta +2p_{rlr} \end{gathered}$$

II. Reeds-Shepp曲线

参考文章：自动驾驶运动规划-Reeds Shepp曲线
https://zhuanlan.zhihu.com/p/122544884

1. 基本理论

Reeds-Shepp曲线允许车辆向前或向后行驶，对于指定起点$q_i$到终点 $q_g$ 的最短路径，一定是由下面的状态表示的：
$$\{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\}$$
其中，“|” 表示转向。Reeds-Shepp 曲线的word组合通过一一枚举可以表示为：
其中，$L^{+}$ 表示车辆左转前进，$L^{-}$表示车辆左转后退，$R^{+}$表示右转前进，$R^{-}$表示右转后退，$S^{+}$表示直行前进，$S^{-}$表示直行后退。

2. 坐标转换

起始姿态：$q_I=(0,0,0)$；
目标姿态：$q_G=(x,y,\varphi )$；其中，$\varphi ={\theta }_2-{\theta }_1$
车辆的转弯半径： r = 1;

假设车辆的初始姿态为$q_I=(x_1,y_1,{\theta }_1)$，目标姿态$q_G=(x_2,y_2,{\theta }_2)$，车辆的转向半径为r = $\rho$

通过旋转平移后即可得到目标姿态：
$$G=\left[\begin{array}{c}x\\ y\\ \varphi \end{array}\right]=\left[\begin{array}{c}(d_{x}cos{\theta }_1+d_{y}sin{\theta }_1)/\rho \\ (-d_{x}sin{\theta }_1+d_{y}cos{\theta }_1)/\rho \\ {\theta }_2-{\theta }_1\end{array}\right]$$

3. 对称性

相较于Dubins曲线，Reeds-Shepp曲线有48种组合，为了降低求解的工作量，可以利用其对称性进行求解。

以转向不同的CSC类型为例，它包含4种曲线类型：$L^{+}{S}^{+}{R}^{+}$、$L^{-}{S}^{-}{R}^{-}$、$R^{+}{S}^{+}{L}^{+}$、$R^{-}{S}^{-}{R}^{-}$，我们只需要编码推导得到$L^{+}{S}^{+}{R}^{+}$的计算过程，其它几种直接可以通过对称性关系得到车辆运动路径。

给定车辆起始姿态$q_I=(160,160,0)$，目标姿态$q_G=(190,180,30)$，可以得到$L^{+}{S}^{+}{R}^{+}$的运动路径如下：

{ Steering: left			Gear: forward	distance: 0.63 }
{ Steering: straight		Gear: forward	distance: 4.02 }
{ Steering: right			Gear: forward	distance: 0.11 }

3.1 timeflip 对称性

时间变换通过交换字母上标符号+和−，即车取反向的行进方向。也就是说，$L^{-}{R}^{+}{S}^{+}{L}^{+}$ 表达式可以通过 $L^{+}{R}^{-}{S}^{-}{L}^{-}$ 表达式通过时间变换获得。其中+,-交换。

如果原始路径从(0,0,0)到 $(x,y,\psi )$ ，显而易见时间变换的路径将从 $(0,0,0)$ 到 $(-x,y,-\psi )$ 。因此，如果一个路径表达式 $L^{+}t{R}^{-}{S}^{-}u{L}_v^{-}$ 从点 $(0,0,0)$ 到 $(-x,y,-\psi )$ ，查找的合适弧长为t、u、v。这就等效于路径表达式 $L_t^{-}{R}^{+}{S}_u^{+}{L}_v^{+}$ 从点 $(0,0,0)$ 到 $(x,y,\psi )$ 查找相应的弧长。

所以，通过时间变换将上述列表中第一个字母符号为”−“的字段消除。

假设我们推导出从起始姿态$q_I=(x_1,y_1,{\theta }_1)$达到目标姿态$q_G=(x_2,y_2,{\theta }_2)$的路径计算方法：

$$path=calcPath(x_1,y_1,{\theta }_1,x_2,y_2,{\theta }_2)$$

利用对称性，将目标Pose修改为$q_G=(-x_2,y_2,-{\theta }_2)$，代入同样的Path计算函数：

$$path=calcPath(x_1,y_1,{\theta }_1,-x_2,y_2,-{\theta }_2)$$

就得到从$q_I=(x_1,y_1,{\theta }_1)$到$q_G=(x_2,y_2,{\theta }_2)$的$L^{-}{S}^{-}{R}^{-}$类型的运动路径。

计算出的$L^{-}{S}^{-}{R}^{-}$的车辆运动路径如下：

{ Steering: left	    Gear: backward	distance: -2.85 }
{ Steering: straight	Gear: backward	distance: 4.02 }
{ Steering: right	    Gear: backward	distance: -2.32 }

3.2 reflect 对称性

反射变换通过交换字母L和R，即取反车辆转向。也就是说，一个路径表达式 $R^{+}{L}^{-}{S}^{-}{R}^{-}$ 的解可以通过反射变换从路径表达式 $L^{+}{R}^{-}{S}^{-}{L}^{-}$ 的解中获得，即沿着该路径交换L和R。相应的参考路径由 $(x,y,\psi )$ 变为 $(x,-y,-\psi )$ 。

假设已知路径 $L^{+}t{R}^{-}{S}_u^{-}{L}_v^{-}$ 解的表达式，从点 $(0,0,0)\to (x,-y,-\psi )$ 的最优弧长为t、u、v。

则从点 $(0,0,0)\to (x,y,\psi )$ 的最优弧长为t、u、v，对应路径 $R^{+}t{L}^{-}{S}_u^{-}{R}_v^{-}$ 的解， 所以，通过反射变换可以将上述列表中以 $L^{+}$ 开头的字段消除。

将目标姿态修改为$q_G=(x_2,-y_2,-{\theta }_2)$，代入同样的Path计算函数：
$$path=calcPath(x_1,y_1,{\theta }_1,x_2,-y_2,-{\theta }_2)$$

就得到从$q_I=(x_1,y_1,{\theta }_1)$到$q_G=(x_2,y_2,{\theta }_2)$的$R^{+}{S}^{+}{L}^{+}$类型的运动路径。

计算出的$R^{+}{S}^{+}{L}^{+}$的车辆运动路径如下：

{ Steering: right	    Gear: forward	distance: -0.56 }
{ Steering: straight	Gear: forward	distance: 5.28 }
{ Steering: left	    Gear: forward	distance: -0.03 }

3.3 timeflip + reflect

结合timeflip对称性和reflect对称性，将目标姿态修改为$q_G=(-x_2,-y_2,{\theta }_2)$，代入同样的Path计算函数：
$$path=calcPath(x_1,y_1,{\theta }_1,-x_2,-y_2,{\theta }_2)$$

就得到从$q_I=(x_1,y_1,{\theta }_1)$到$q_G=(x_2,y_2,{\theta }_2)$的[公式]类型的运动路径。

计算出的$R^{-}{S}^{-}{L}^{-}$的车辆运动路径如下：

{ Steering: right		Gear: backward	distance: -1.86 }
{ Steering: straight	Gear: backward	distance: 5.28 }
{ Steering: left		Gear: backward	distance: -2.38 }

通过对称性，48种不同的Reeds Shepp曲线通过不超过12个函数就可以得到全部运动路径。

3.4 backwards

逆向变换通过将原路径按照相反方向行走。也就是说，路径 $L^{-}{S}^{-}{R}^{-}{L}^{+}$ 的公式可以使用逆向变换从路径 $L^{+}{R}^{-}{S}^{-}{L}^{-}$ 的公式中获得，即按照相反的顺序运动。逆向变换将目标点 $(x,y,\psi )$ 转化为
$$x^{\prime }=(x\cos (\psi )+y\sin (\psi ),x\sin (\psi )-y\cos (\psi ),\psi )$$
所以可以通过公式 $L^{+}{R}^{-}{S}^{-}{L}^{-}$ 到达点 $x^{\prime }$，从而获得 $L^{-}{S}^{-}{R}^{-}{L}^{+}$ 到达点 $(x,y,\psi )$ 的解。通过这个变换可以消除上述列表中一些字段

4. 公式计算

参考论文：Optimal paths for a car that goes both forwards and backwards, J Reeds, L Shepp - Pacific journal of mathematics, 1990

论文中将不同的状态，按照Base word分类为了以下几种：

(8.1) $L^{+}{S}^{+}{L}^{+}$
$$\begin{gathered} (u,t)=R(x-sin\varphi ,y-1+cos\varphi ) \\ v=M(\varphi -t) \\ L=|t|+|u|+|v| \end{gathered}$$

(8.2) $L^{+}{S}^{+}{R}^{+}$
$$(u_1,t)=R(x+sin\varphi ,y-1-cos\varphi )$$
if $u_1^2<4$, $L=\infty$, else:
$$\begin{gathered} u=sqrt(u_1^2-4) \\ (T,\theta )=R(u,2) \\ t=M(t_1+\theta ) \\ v=M(t-\varphi ) \\ L=|t|+|u|+|v| \end{gathered}$$

(8.3) $L^{+}{R}^{-}L$
$$\begin{gathered} \xi =x-sin\varphi \\ \eta =y-1+cos\varphi \\ (u_1,\theta )=R(\xi ,\eta ) \end{gathered}$$
if $u_1^2<4$, $L=\infty$, else:
$$\begin{gathered} A=arcsin(u_1^2/4),\pi /2\le A\le \pi \\ u=M(A+\theta ) \\ (T,v)=R(2-\xi sinu+\eta cosu,\xi cosu+\eta sinu) \\ w=M(\varphi +v-u) \\ L=|t|+|u|+|v| \end{gathered}$$

(8.7) $L^{+}{R}_u^{+}{L}_u^{-}{R}^{-}$
$$\rho =\frac{2+\sqrt{{\xi }^2+{\eta }^2}}{4}$$
if $0\le \rho \le 1$,
$$\begin{gathered} u=arccos(\rho ),0\le u\le \pi /2 \\ t=\tau (u,-u) \\ u=w(u,-u) \\ L=|t|+2|u|+|v| \end{gathered}$$
else: $L=\infty$

(8.8) $L^{+}{R}_u^{-}{L}_u^{-}{R}^{+}$
$$\rho =(20-{\xi }^2-{\eta }^2)/16$$
if $0\le \rho \le 1$,
$$\begin{gathered} u=-arccos(\rho ),0\le u\le \pi /2 \\ t=\tau (u,u) \\ v=w(u,u) \end{gathered}$$
else: $L=\infty$

(8.9) $L^{+}{R}_{-\pi /2}^{-}{S}^{-}{L}^{-}$
$$\begin{gathered} \xi =x+sin\varphi \\ \eta =y-1-cos\varphi \\ (\rho ,\theta )=R(-\eta ,\xi ) \end{gathered}$$
if $\rho <2$, $L=\infty$, else:
$$\begin{gathered} (T,{\theta }_1)=R(\sqrt{{\rho }^2-4}-2) \\ t=M(\theta -{\theta }_1) \\ u=2-{\theta }_1 \\ v=M(\varphi -\pi /2-t) \\ L=|t|+\pi /2+|u|+|v| \end{gathered}$$

(8.10) $L^{+}{R}_{-\pi /2}^{-}{S}^{-}{R}^{-}$
$$\begin{gathered} t=\theta \\ u=2-\rho \\ v=M(t+\pi /2-\varphi ) \\ L=|t|+\pi /2+|u|+|v| \end{gathered}$$

(8.11) $L^{+}{R}_{-\pi /2}^{-}{L}_{-\pi /2}^{-}{R}^{+}$
$$\begin{gathered} \xi =x+sin\varphi \\ \eta =y-1-cos\varphi \\ (\rho ,\theta )=R(\xi ,\eta ) \end{gathered}$$
if $\rho <2$, $L=\infty$, else:
$$t=M(\theta -arccos(-2/\rho )),-\pi /2\le t\le \pi /2$$
if $t\le 0$, $L=\infty$, else:
$$\begin{gathered} u=4-(\xi +2cost)/sint \\ w=M(t-\varphi ) \end{gathered}$$

疑似论文印刷错误？ 代码中公式如下：
$$\begin{gathered} \xi =x+sin\varphi \\ \eta =y-1-cos\varphi \\ (\rho ,\theta )=R(\xi ,\eta ) \end{gathered}$$
if $\rho <2$, $L=\infty$, else:
$$u=4-\sqrt{{\rho }^2-4}$$
if $u<0$, $L=\infty$, else:
$$\begin{gathered} t=M(arctan(\frac{(4-u)\xi -2\eta }{-2\xi +(u-4)\eta })) \\ v=M(t-\varphi ) \\ L=|t|+|u|+|v| \end{gathered}$$