Reeds-Shepp 曲线已成为自动驾驶与机器人运动规划领域的基础算法之一，本文介绍相关信息。
Reeds-Shepp 曲线由 J.A. Reeds 和 L.A. Shepp 于1990年提出，通过扩展 Dubins曲线 的运动约束（Dubins仅支持单向行驶），显著提升了路径优化能力.
Reeds-Shepp曲线（简称RS曲线）是一种用于路径规划的数学模型，专门描述平面内具备前进/后退能力的车辆（如汽车、机器人）从起点到终点的最短可行路径。其特点在于允许车辆通过最小转弯半径进行转向，并支持方向切换（前进变后退或反之）.
对比Dubins曲线
Simple Car Model是指一种用于描述车辆运动的简化模型。具体来说，Simple Car Model将车辆视为一个二维平面上的刚体，其原点位于车辆后轮的中心，x轴沿着车辆主轴方向，与车辆运动方向一致。车辆的状态由位置坐标(x, y)和姿态角（θ）表示。在Simple Car Model中，车辆的速度为s，方向盘的转角为α，前轮和后轮中心的距离为L。如果方向盘转角固定，车辆会在原地转圈，转圈的半径为L/sin(α)。在非常短的时间内，车辆可以近似视为沿着后轮指向的方向前进，当方向盘转角趋于0时，车辆的运动方向与x轴一致。Simple Car Model通过引入Action变量，假设车辆的运动速度和方向盘转角由这些变量控制，从而得到车辆的运动模型。这个模型适用于自动驾驶运动规划中的运动方程，可以描述车辆的向前或向后运动。
运动方程如下：
$$ \begin{aligned}&\dot{x}=u_{1}cos\theta\\&\dot{y}=u_{1}sin\theta\\&\dot{\theta}=u_{1}u_{2}\end{aligned} $$
其中，$u_1\in{-1,1},:u_2\in[-tan\phi_{max},tan\phi_{max}]$ 。当 $u_1=1$ 时，表示车辆向前运动； $u_1=-1$ 时，表示车辆向后运动。
J Reeds和L Shepp证明Reeds Shepp Car从起点 $𝑞_𝐼$到终点 $𝑞_𝐺$ 的最短路径一定是下面的 word 的其中之一。word中的"|"表示车辆运动朝向由正向转为反向或者由反向转为正向。
$$ \{C|C|C,\:CC|C,\:C|CC,\:CSC,\:CC_{\beta}|C_{\beta}C,\:C|C_{\beta}C_{\beta}|C,\C|C_{\pi/2}SC,\:CSC_{\pi/2}|C,\:C|C_{\pi/2}SC_{\pi/2}|C\}. $$
每个曲线都由$𝐿+，𝐿−，𝑅+，𝑅−，𝑆+，𝑆−$这六种基本运动组合组成，每种运动对应固定转向半径（通常取最小半径），其中$𝐿+$表示车辆左转前进；$𝐿−$表示车辆左转后退；$𝑅+$表示车辆右转前进；$𝑅−$表示车辆右转后退；$𝑆+$表示车辆直行前进；$𝑆−$表示车辆直行后退。
Reeds and Shepp曲线的word所有组合不超过48种，所有的组合一一枚举如下：
车辆的起点和终点的位置姿态是难以穷举的，所以一般在计算之前，会将车辆的姿态归一化.
假设车辆的初始姿态为 $𝑞_𝐼=(𝑥_1,𝑦_1,𝜃_1)$，目标姿态 $𝑞_𝐺=(𝑥_2,𝑦_2,𝜃_2)$，车辆的转向半径为 $r = 𝜌$，如何实现姿态的归一化呢，实际上归一化的过程就是向量的平移和旋转过程。
首先将向量 $\overrightarrow{q_{I}q_{G}}$ 平移到坐标原点 $(0,0)$。平移 $𝑞_𝐼$到 $O(0, 0)$，平移向量为 $(−𝑥_1,−𝑦_1)$；对 $𝑞_𝐺$ 应用同样的平移向量: $𝑞_𝐺=[𝑥_2−𝑥_1,𝑦_2−𝑦_1]$，最后得到平移后的向量：
$$ \vec{q_I}\vec{q_G}=\left[\begin{array}{c}x_2-x_1\\y_2-y_1\end{array}\right]=\left[\begin{array}{c}d_x\\d_y\end{array}\right] $$ 应用旋转矩阵，将车辆的起点朝向转到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_xcos\theta_1+d_ysin\theta_1\\-d_xsin\theta_1+d_ycos\theta_1\end{bmatrix} $$
旋转之后，目标位置朝向更新为 $𝜙=𝜃_2−𝜃_1$。
将车辆转向半径缩放到1，于是最终得到车辆运动的起始姿态： $q_𝐼=[0,0,0]$
目标姿态：
$$ G=\begin{bmatrix}x\\y\\ \phi\end{bmatrix}=\begin{bmatrix}(d_xcos\theta_1+d_ysin\theta_1)/\rho\\(-d_xsin \theta_1+d_ycos\theta_1]/\rho\\ \theta_2-\theta_1\end{bmatrix} $$
归一化后得到：
起始姿态： $q_{I}=(0,0,0)$
目标姿态：$𝑞_𝐺=(𝑥,𝑦,𝜙) $；其中，$𝜙=𝜃_2−𝜃_1$
车辆的转弯半径：$ r = 1$;
Reeds Shepp曲线有48种组合，编程时一一编码计算比较麻烦，因此可以利用其对称性降低求解工作量。
以转向不同的CSC类型为例，它包含4种曲线类型：𝐿+𝑆+𝑅+、𝐿−𝑆−𝑅−、𝑅+𝑆+𝐿+、𝑅−𝑆−𝑅−，我们只需要编码推导得到𝐿+𝑆+𝑅+的计算过程，其它几种直接可以通过对称性关系得到车辆运动路径。
给定车辆起始姿态 $𝑞_𝐼$，目标姿态 $𝑞_𝐺$，可以得到𝐿+𝑆+𝑅+的运动路径如下：
利用对称性求解其它几种路径.
假设我们推导出从起始姿态 $𝑞_𝐼(𝑥_1,𝑦_1,𝜃_1)$ 达到目标姿态 $(𝑥_2,𝑦_2,𝜃_2)$ 的路径计算方法：
1 | |
利用对称性，将目标Pose修改为 $(−𝑥_2,𝑦_2,−𝜃_2)$，代入同样的Path计算函数：
1 | |
就得到从 $(𝑥_1,𝑦_1,𝜃_1)$ 到 $(𝑥_2,𝑦_2,𝜃_2)$ 的 𝐿−𝑆−𝑅−类型的运动路径。
将目标姿态修改为 $(𝑥_2,−𝑦_2,−𝜃_2)$，代入同样的Path计算函数：
1 | |
就得到从 $(𝑥_1,𝑦_1,𝜃_1)$ 到 $(𝑥_2,𝑦_2,𝜃_2)$ 的𝑅+𝑆+𝐿+类型的运动路径。
结合timeflip对称性和reflect对称性，将目标姿态修改为 $(−𝑥_2,−𝑦_2,𝜃_2)$，代入同样的Path计算函数：
1 | |
就得到从 $(𝑥_1,𝑦_1,𝜃_1)$ 到 $(𝑥_2,𝑦_2,𝜃_2)$ 的𝑅−𝑆−𝐿−类型的运动路径。
通过对称性，48种不同的Reeds Shepp曲线通过不超过12个函数就可以得到全部运动路径。
Dubins曲线是从6个曲线中选出最短的那条，而Reeds-Shepp曲线是从46个曲线中选出最短的那个，这46个曲线如下图所示。（最开始的论文认为最短的曲线一定在48条曲线中，后人研究发现有两条曲线不会是最短的，所以搜索范围减小到46条）
https://github.com/zhm-real/CurvesGenerator
Reeds-Shepp曲线和Dubins曲线对任意的起止位姿都存在吗？
答案是肯定的，任意的起始状态和终止状态之间都存在这样的曲线，如下图所示。箭头表示汽车的朝向，红色曲线是Reeds-Shepp曲线，而黑色曲线是Dubins曲线。
注意二者有时是重合的。
当环境中存在障碍物时，Reeds-Shepp曲线和Dubins曲线对任意的起止位姿都存在吗？
Reeds-Shepp曲线和Dubins曲线特指没有障碍物时的最短路径。如果存在障碍物，那么这样的曲线不再是传统意义上的RS和Dubins曲线了，不过为了保持一致我们还是这么称呼它们吧。
生活从来没那么简单，你在停车时可能周围会有各种障碍物，比如其它车辆、垃圾桶、树木。这时是否仍然存在RS曲线和Dubins曲线似乎没那么容易回答了。不过庆幸的是这个问题已经被人解决了，答案是：只要存在连接起止位姿的无碰撞路径，那么就存在无碰撞的Reeds-Shepp曲线。然而这个结果对Dubins曲线却不适用。这个答案好像没什么出人意料，不过稍微品味一下却让人很吃惊。注意这里的前提：“连接起止位姿的无碰撞路径”，除了无碰撞的要求以外，我没说其它任何的要求，比如存在一条类似“Z”这样完全由直线组成的折线路径也可以。很奇妙吧？不管你的路径由什么线（直线/任意曲线）组成，不管它有多怪异多扭曲，只要你能找到一条这样的路径，那么就一定存在满足要求的Reeds-Shepp曲线，即连接起止位姿，并且汽车不会碰到障碍物。在电影《车手》中就出现了神奇的一幕——汽车原地直角拐弯。其实理论上，不需要原地拐弯，汽车也能通过狭窄的直角胡同。
理论上存在没有给我们提供实际寻找的方法，这时我们可以采用随机搜索的方法，如下图。（黑色表示障碍物）
Dubins曲线和Reeds-Shepp曲线有什么用？
我们以汽车为例介绍了最短路径。实际上，汽车只是一类更广泛系统的特例，这类系统叫做“非完整约束系统”。非完整约束系统就是受到非完整约束的系统（废话）。可以用直观的例子解释，人站在平地上就不受非完整约束，因为人可以往任意方向移动，前后左右随便你怎么动都可以。可是如果你骑在自行车上就受非完整约束了，因为你的运动方向受到前后轮的限制，只能沿着前轮指向的方向运动，尽管你可以通过调整车把改变前轮的朝向，但是你无法向车轮的侧方移动。当然，如果别人从侧面踹了你一脚或者路太滑，自行车可能会向侧方移动，这时你就违反非完整约束了，但是我们暂时不考虑这些特例。这些特例，比如路滑，确实有可能发生。可是为了让问题简单一点，我们不得不理想化处理，认为自行车车轮不会发生侧滑，哪怕一毫米也不会有。因此，具有车轮的东西都属于非完整约束系统，比如独轮车。注意这里的车轮是指普通的车轮，不包括特殊的车轮（比如麦克纳姆轮）。那些带挂斗有很多轮子的大货车属于非完整约束系统吗？这就要看情况了。一般货车挂斗上的轮子不能转向，在货车拐弯的时候，有些轮子必然会侧滑，至于哪些会侧滑取决于路面的条件。因此严格来说，它不是一个非完整约束系统，虽然看上去的表现像受到非完整约束。即便这样，我们也可以按照非完整约束处理。
非完整约束有什么特别的地方吗？给车辆或移动机器人规划轨迹的工程师都不喜欢它，它也因为难以处理而臭名昭著。对于简单的系统，例如前面的汽车模型（相对于真实的汽车已经简化了），我们还能得到满足非完整约束的最短路径，可是对于复杂的模型就很难了，比如变量更多、约束形式更复杂的模型，或者环境中存在障碍物的情况。所以人们一般用Dubins曲线和Reeds-Shepp曲线作为复杂模型的近似最短路径，在实际执行时并不一定严格地遵循这些曲线。
Dubins曲线和Reeds-Shepp曲线的值函数是什么样的？
如果我们把终点（位姿）固定在原点：$(0,0,0)$，那么曲线的长度可以看成是起点的函数。我们可以对任意起点$(x,y,\theta)$ 计算函数的值，这样的函数是以位姿为自变量的函数，它的函数值则是曲线的长度，我们就叫它值函数吧。想象最简单的情况（运动不受约束，不考虑角度变化引起的），值函数就是点到点的距离，即$\sqrt{x2+y2} $ ，这样的值函数图像就是个圆锥。
对于Dubins曲线和Reeds-Shepp曲线，它们的值函数图像如下图所示。如果你听说过强化学习，这里的值函数就是强化学习中出现的值函数，即最优的累积回报。值函数的一个用处是作为启发函数，路径搜索算法需要这样的启发函数加快搜索速度，比如混合A* 和RRT* 等。
动画是如何制作的？
动画都是用Mathematica制作的，所使用的源代码在github上可以找到：https://github.com/robinvista/Mathematica。读者可以对程序任意修改，例如对起点进行扰动，观察最短曲线产生的不连续变化。
是否最优曲线只能用 Dubins曲线和Reeds-Shepp曲线？
其实，Reeds-Shepp和Dubins曲线只不过是最优曲线的两个特殊情况。我们还可以考虑各种各样的机器人约束或者目标函数，这时的曲线就更有意思了，当然也更难求解了。Reeds-Shepp和Dubins曲线之所以有名，是因为它们刚好存在解析形式，而且形式还不是太复杂，类似的曲线还有Balkcom-Mason曲线。其它更复杂的最优曲线要想求解析解是非常困难的。
文章链接：
https://www.zywvvd.com/notes/study/algorithm/graph/reeds-shepp/reeds-shepp/
此内容由惯性聚合(RSS阅读器)自动聚合整理，仅供阅读参考。 原文来自 — 版权归原作者所有。