Reeds-Shepp和Dubins曲线中的关键问题解析

9大关键性问题：

1，什么是Reeds-Shepp曲线？

2，Reeds-Shepp曲线是什么样的曲线？

3，什么是Dubins曲线？

4，Reeds-Shepp曲线和Dubins曲线对任意的起止位姿都存在吗？

 5，当环境中存在障碍物时，Reeds-Shepp曲线和Dubins曲线对任意的起止位姿都存在吗？

6，值函数

7，Reeds-Shepp全部曲线

8，Mathematica代码下载地址

9，参考资料 

1，什么是Reeds-Shepp曲线？

　　想象你下班开车回家，到了小区后把车停到车位里。作为一个喜欢追求挑战的老司机，你想找一条最短的路径把车停进去。那么这样的路径是什么呢？答案就是Reeds-Shepp曲线。Reeds-Shepp曲线由Reeds和Shepp二人在1990年的论文《Optimal paths for a car that goes both forwards and backwards》[ 1 ] 中提出。

2，Reeds-Shepp曲线是什么样的曲线？

　　既然是最短路径，我们首先想到的就是直线段，那么它是直线吗？嗯…，在某些情况下，它确实是直线。比如下图左所示的情况，汽车车头刚好对准了停车位（绿色表示停车位，红色表示汽车的起始状态，灰色表示汽车）。可是实际显然不会这么简单，会有各种可能，比如下图右所示的情况：车在停车位的右侧，且车头和停车位平行（侧位停车）。由于汽车都有一个最小转向半径，所以你不能让汽车像螃蟹一样横着开进去，这时求最短路径可就没那么容易了。图中汽车运动形成的黑色曲线就是Reeds-Shepp曲线。Reeds-Shepp曲线由几段半径固定的圆弧和一段直线段拼接组成，而且圆弧的半径就是汽车的最小转向半径。这里的路径长度是指汽车中心运动轨迹的长度，也就是所有圆弧的弧长和直线段的长度之和。

  

 　下图展示了更一般的情况，汽车从不同的初始位置和朝向进入同一个停车位:

 

3，什么是Dubins曲线？

　　Dubins曲线和Reeds-Shepp曲线差不多，只不过多了一个约束条件：汽车只能朝前开，不能后退（不能挂倒挡）。Dubins曲线由Dubins在1957年的论文《On curves of minimal length with a constraint on average curvature and with prescribed initial and terminal positions and tangents》[ 2 ] 中提出。Dubins曲线如下图所示。

4，Reeds-Shepp曲线和Dubins曲线对任意的起止位姿都存在吗？

答案是肯定的，任意的起始状态和终止状态之间都存在这样的曲线。如下图所示。这时为了清晰起见，我用箭头表示汽车的朝向，蓝色箭头是出发位姿，绿色箭头是目标位姿，黑色曲线是Reeds-Shepp曲线，红色曲线是Dubins曲线。注意二者有时是重合的。

 5，当环境中存在障碍物时，Reeds-Shepp曲线和Dubins曲线对任意的起止位姿都存在吗？

Reeds-Shepp曲线和Dubins曲线特指没有障碍物时的最短路径。如果存在障碍物，那么这样的曲线不再是传统意义上的RS和Dubins曲线了，不过为了保持一致我们还是这么称呼它们吧。
　　生活从来没那么简单，你在停车时周围可能会有各种障碍物，比如其它车辆、垃圾桶。这时是否仍然存在RS曲线和Dubins曲线似乎没那么容易回答了。不过庆幸的是这个问题已经被人解决了，答案是：只要存在连接起止位姿的无碰撞路径，那么就存在无碰撞的Reeds-Shepp曲线。然而这个结果对Dubins曲线却不适用。这个答案好像没什么出人意料，不过稍微品味一下却让人很吃惊。注意这里的前提：“连接起止位姿的无碰撞路径”，除了无碰撞的要求以外，我没说其它任何的要求，比如存在一条类似“Z”这样完全由直线组成的折线路径也可以。很奇妙吧？不管你的路径由什么线（直线/任意曲线）组成，不管它有多怪异多扭曲，只要你能找到一条这样的路径，那么就一定存在满足要求的Reeds-Shepp曲线，即连接起止位姿，并且汽车不会碰到障碍物。在电影《车手》中就出现了神奇的一幕——汽车原地直角拐弯。其实理论上，不需要原地拐弯，汽车也能通过狭窄的直角胡同。
　　理论上存在没有给我们提供实际寻找的方法，这时我们可以采用随机搜索的方法，见下图（黑色多边形表示障碍物）。
 

6，值函数

 如果我们把终点（位姿）固定在原点：( 0 , 0 , 0 )，那么曲线的长度可以看成是起点的函数。我们可以对任意起点( x , y , θ )计算函数的值，这样的函数被称为值函数。想象最简单的情况（运动不受约束，不考虑角度变化引起的），值函数就是点到点的距离，这样的值函数等值面图像就是个圆柱。对于Dubins曲线和Reeds-Shepp曲线，它们的值函数图像如下图所示。如果你知道强化学习，这里的值函数就是强化学习中出现的值函数，即最优的累积回报。如果我们知道值函数，那么一直沿着值函数的梯度下降的方向运动到达终点的曲线是什么样的呢？不是别的，就是Dubins曲线和Reeds-Shepp曲线，不信你可以仿真看看。值函数的一个用处是作为启发函数，路径搜索算法需要这样的启发函数加快搜索速度，比如混合A* 和RRT* 等。
 

Dubins曲线的值函数 

 Reeds-Shepp曲线的值函数

7，Reeds-Shepp全部曲线

Reeds-Shepp曲线是从46个曲线中选出最短的那个，这46个曲线如下图所示。

 

8，Mathematica代码下载地址

源代码我上传到了github：Reeds-Shepp和Dubins曲线.rar，没有版权，随便用。你可以对起点进行扰动，观察最短曲线的不连续变化。

9，参考资料 

[1]　Optimal Paths for a Car That Goes both Forwards and Backwards，J. A. Reeds and L. A. Shepp，Pacific Journal of Mathematics.
[2]　On Curves of Minimal Length with a Constraint on Average Curvature, and with Prescribed Initial and Terminal Positions and Tangents，Lester E. Dubins，American Journal of Mathematics.