自动驾驶运动规划-Reeds Shepp曲线
相比于Dubins Car只允许车辆向前运动，Reeds Shepp Car既允许车辆向前运动，也允许车辆向后运动。
1、车辆模型
车辆运动模型仍然采用Simple Car Model，但增加对车辆运动方向的描述，运动方程如下：
$\dot{x} = u_1 cos{\theta}$
$\dot{y} = u_1 sin{\theta}$
$\dot{\theta} = u_1 u_2$
其中，$u_1 \in \{-1, 1\}$，$u_2 \in [-tan \phi_{max}, tan \phi_{max}]$。当$u_1 = 1$时，表示车辆向前运动；$u_1=-1$时，表示车辆向后运动。
2、Reeds-Shepp Car
J Reeds和L Shepp证明Reeds Shepp Car从起点$q_I$到终点$q_G$的最短路径一定是下面的word的其中之一。word中的”|”表示车辆运动朝向由正向转为反向或者由反向转为正向。
每个word都由$L^{+}$，$L^{-}$，$R^{+}$，$R^{-}$，$S^{+}，$S^{-}$这六种primitives组成，其中$L^{+}$表示车辆左转前进；$L^{-}$表示车辆左转后退；$R^{+}$表示车辆右转前进；$R^{-}$表示车辆右转后退；$S^{+}$表示车辆直行前进；$S^{-}$表示车辆直行后退。
Reeds and Shepp曲线的word所有组合不超过48种，所有的组合一一枚举如下：
3、计算过程优化
3.1 位置姿态统一化
车辆的起点和终点的位置姿态是难以穷举的，所以一般在计算之前，会将车辆的姿态归一化：
起始姿态：$q_I = (0, 0, 0)$；
目标姿态：$q_G=(x, y, \phi)$；其中，$\phi=\theta_2 – \theta_1$
车辆的转弯半径： r = 1;
假设车辆的初始姿态为$q_I = (x_1, y_1, \theta_1)$，目标姿态$q_G=(x_2, y_2, \theta_2)$，车辆的转向半径为r = $\rho$，如何实现姿态的归一化呢，实际上归一化的过程就是向量的平移和旋转过程。
首先将向量$\vec{q_I q_G}$平移到坐标原点(0,0)。平移$q_I$到O(0, 0)，平移向量为$(-x_1, -y_1)$；对$q_G$应用同样的平移向量:$q_G = [x_2 – x_1, y_2 – y_1]$，最后得到平移后的向量：
$$
\vec{q_I q_G} =
\begin{bmatrix}
x_2 – x_1 \\
y_2 – y_1 \\
\end{bmatrix}
=\begin{bmatrix}
d_x \\
d_y \\
\end{bmatrix}
$$
应用旋转矩阵，将车辆的起点朝向转到x轴正向：
$$
\begin{bmatrix}
cos \theta_1 & sin \theta_1 \\
-sin \theta_1 & cos \theta_1 \\
\end{bmatrix}
\begin{bmatrix}
d_x \\
d_y \\
\end{bmatrix}
\begin{bmatrix}
d_x cos \theta_1 + d_y sin \theta_1 \\
-d_x sin \theta_1 + d_y cos \theta_1 \\
\end{bmatrix}
$$
旋转之后，目标位置朝向更新为$\phi = \theta_2 – \theta_1$。
将车辆转向半径缩放到1，于是最终得到车辆运动的起始姿态：
$$
I =
\begin{bmatrix}
0 \\
0 \\
0
\end{bmatrix}
$$
目标姿态：
$$
G=
\begin{bmatrix}
x \\
y \\
\phi
\end{bmatrix}
=\begin{bmatrix}
(d_x cos \theta_1 + d_y sin \theta_1) / \rho \\
(-d_x sin \theta_1 + d_y cos \theta_1 ] / \rho \\
\theta2 – \theta_1
\end{bmatrix}
$$
代码如下:
double x1 = s1->getX(), y1 = s1->getY(), th1 = s1->getYaw(); double x2 = s2->getX(), y2 = s2->getY(), th2 = s2->getYaw(); double dx = x2 - x1, dy = y2 - y1, c = cos(th1), s = sin(th1); double x = c * dx + s * dy, y = -s * dx + c * dy, phi = th2 - th1; return ::reedsShepp(x / rho_, y / rho_, phi)
3.2 利用对称关系降低求解复杂度
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.2.1 timefilp对称性
假设我们推导出从起始姿态$q_I(x_1,y_1, \theta_1)$达到目标姿态$(x_2, y_2, \theta_2)$的路径计算方法：
path = calc_path($x_1$, $y_1$, $\theta_1$, $x_2$, $y_2$, $\theta_2$)
利用对称性，将目标Pose修改为$(-x_2, y_2, -\theta_2)$，代入同样的Path计算函数：
path = calc_path($x_1$, $y_1$, $\theta_1$, -$x_2$, $y_2$, -$\theta_2$)
就得到从$(x_1, y_1, \theta_1)$到$(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.2 reflect对称性
将目标姿态修改为$(x_2, -y_2, -\theta_2)$，代入同样的Path计算函数：
path = calc_path($x_1$, $y_1$, $\theta_1$, $x_2$, -$y_2$, -$\theta_2$)
就得到从$(x_1, y_1, \theta_1)$到$(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.2.3 timeflip + reflect
结合timeflip对称性和reflect对称性，将目标姿态修改为$(-x_2, -y_2, \theta_2)$，代入同样的Path计算函数：
path = calc_path($x_1$, $y_1$, $\theta_1$, -$x_2$, -$y_2$, $\theta_2$)
就得到从$(x_1, y_1, \theta_1)$到$(x_2, y_2, \theta_2)$的$R^{-}S^{-}L^{-}$类型的运动路径。
计算出的$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个函数就可以得到全部运动路径。
参考链接
1、Optimal paths for a car that goes both forwards and backwards, J Reeds, L Shepp – Pacific journal of mathematics, 1990
2、OMPL的Reeds Sheep实现代码。(https://ompl.kavrakilab.org/ReedsSheppStateSpace_8cpp_source.html)
3、Reeds Sheep的Python代码实现。(https://github.com/nathanlct/reeds-shepp-curves/blob/master/reeds_shepp.py)
