文章目录

参考资料
1. Reeds-Shepp 曲线
2. RS曲线求解
3. python实现
4. c++实现

参考资料

前文

1. Reeds-Shepp 曲线

1.1 基本概念

Reeds-Shepp 算法简称 RS，由 J.A.Reeds 和 L.A.Shepp 于 1990 年发表的论文 ( optimal path for a car that goes both forward and backwards)。该方法基于 Dubins 算法进行改进，将反向运动（汽车允许后退，挂倒挡）加入到规划中，这就使得在某些情况下可以得出比 Dubins 曲线更优的解。

曲线示例如下：

【自动驾驶】路径规划——ReedsShepp 曲线总结（python实现 | c++实现）

图片来源：https://blog.csdn.net/robinvista/article/details/95137143

如上图可以知道，在 Reeds-Shepp 曲线的路径中，允许尖瓣存在。

1.2 字段组合

在字段中的字母上加入上标，用来表示运动方向，表示如下：

符号	含义	绕单位圆
	向前左转	逆时针
	向后左转	顺时针
	向前右转	逆时针
	向后右转	顺时针
	向前直走	/
	向后直走	/

使用【自动驾驶】路径规划——ReedsShepp 曲线总结（python实现 | c++实现）字符可以给出如下集合：

【自动驾驶】路径规划——ReedsShepp 曲线总结（python实现 | c++实现）

通过反转等式 (1) 的符号可以获得新的的字段。上式 (1) 中，【自动驾驶】路径规划——ReedsShepp 曲线总结（python实现 | c++实现）表示相应的或的转过的角度为，组合表示相应的弧段拥有相等的长度。

为了以紧凑的表示，避免【自动驾驶】路径规划——ReedsShepp 曲线总结（python实现 | c++实现），可以写出充分路径的列表如下：

其中，【自动驾驶】路径规划——ReedsShepp 曲线总结（python实现 | c++实现）表示车辆运动朝向由正向转为反向或者由反向转为正向。

将上述word分别带下标，如【自动驾驶】路径规划——ReedsShepp 曲线总结（python实现 | c++实现），其中分别表示旋转的角度（弧度制）。带下标表示行走的直线距离为。它们的范围如下表所示：

将【自动驾驶】路径规划——ReedsShepp 曲线总结（python实现 | c++实现）替换为或，通过简单的变换，则共有 48 种字段组合。这种简单变换包括时间翻转(timeflip)、反射(reflect)和向后变换(backwards)。

1.3 时间翻转(timeflip)、反射(reflect)和向后变换(backwards)

1.3.1 时间翻转(timeflip)

将计算出的曲线按照其运动方向进行取反，得到的新的曲线为原曲线相反的曲线。

如图，蓝色曲线【自动驾驶】路径规划——ReedsShepp 曲线总结（python实现 | c++实现）与红色曲线关于Y 轴左右对称，两条路径曲线上的路径航向角相反。从起始点到目标点的路径可以通过起始点到目标点的路径取反计算获得。

1.3.2 反射(reflect)

第二种转换关系：“reflect”，即将计算的曲线按照其沿圆周运动方向取反，得到的曲线与原来的曲线长度相同的新曲线。

如图所示，红色曲线【自动驾驶】路径规划——ReedsShepp 曲线总结（python实现 | c++实现）与蓝色曲线关于 X 轴左右对称，两条路径曲线上的路径航向角相反。从起始点到目标点的路径可以通过起始点到目标点的路径圆周方向取反计算获得。

1.3.3 向后变换(backwards)

第三种转换关系：“backwards”，即将原曲线的路径逆序转换，得到的曲线与原来的曲线长度相同的新曲线。

“backwards”实例图如图所示，蓝色曲线【自动驾驶】路径规划——ReedsShepp 曲线总结（python实现 | c++实现）与红色曲线不具有以上两种的堆成关系，但是很直观的可以看出的逆序排列得到，两条路径曲线上的路径进行逆向排列。从起始点到目标点的路径可以通过起始点【自动驾驶】路径规划——ReedsShepp 曲线总结（python实现 | c++实现）到目标点的路径圆周方向取反计算获得。

1.4 48 个字段组合

所有的48个字段组合一一枚举如下：

图中，角标【自动驾驶】路径规划——ReedsShepp 曲线总结（python实现 | c++实现）代表C旋转的弧度是，同理也是代表旋转弧度，意思就是这个圆必须转。

所以，Dubins曲线是从6个曲线中选出最短的那条，而Reeds-Shepp曲线是从48个曲线中选出最短的那个（最开始的论文认为最短的曲线一定在48条曲线中，后人研究发现有两条曲线不会是最短的，所以后来搜索范围减小到46条）。

至于这些字段组合具体的推导，请看原论文。

2. RS曲线求解

Reeds-Shepp 曲线的求解公式在原论文的式(8.1)~式(8.11)中。这边将其总结整理。

假设起点为【自动驾驶】路径规划——ReedsShepp 曲线总结（python实现 | c++实现），终点为。

2.1 三段圆弧组成

第一类基础曲线: , 即三段圆弧且每段圆弧两两圆弧方向相反, 基于此, 可以扩展出四种曲线形式: 。
第二类基础曲线: , 即三段圆弧且后两段圆弧的圆弧方向相反, 基于此, 可以扩展出四种曲线形式: 。
第三类基础曲线: , 即三段圆弧且前两段圆弧的圆弧方向相反, 基于此, 可以扩展出四种曲线形式: 。

通过时间翻转、反射和向后三种曲线变换方式, 上述 3 种公式的符号变换成了 12 种曲线的运动方式, 可以利用公式(1)进行求解。
【自动驾驶】路径规划——ReedsShepp 曲线总结（python实现 | c++实现）

其中【自动驾驶】路径规划——ReedsShepp 曲线总结（python实现 | c++实现）代表公式中每次转向的弧度值大小（其实也是基础曲线三段圆弧的弧长，因为圆弧半径为1，单位圆）, 为路径总长度；公式中的函数是将笛卡尔坐标系中的转化成极坐标系下的【自动驾驶】路径规划——ReedsShepp 曲线总结（python实现 | c++实现） , 其中径向坐标值有 , 角坐标值有 , 函数用于对取模运算，并将弧度值限定在。

函数即笛卡尔坐标系中的与极坐标系的关系可以表示为：
即表示

后文除了新定义的符号，不再赘述已定义的符号。

2.2 两段圆弧与直线组成

第四类基础曲线形式: , 即两段圆弧与直线组成, 基于此, 可以扩展出八种曲线形式: 。

【自动驾驶】路径规划——ReedsShepp 曲线总结（python实现 | c++实现）

通过上述三种曲线变换方式，将第 4 种公式的符号变换成了 8 种曲线，这 8种曲线中前 4 种使用式(2) 进行求解，后 4 种使用式(3) 进行求解。

2.3 四段圆弧组成

第五类基础曲线形式: , 即四段圆弧组成, 第一段与第二段圆弧的圆弧方向相反, 第三段与第四段圆弧的圆弧方向相反, 前两段与后两段圆弧方向相反, 基于此, 可以扩展出四种曲线形式: 。
第六类基础曲线形式: , 即四段圆弧组成, 第一段与第二段圆弧的圆弧方向相同, 第三段与第四段圆弧的圆弧方向相同, 前两段与后两段圆弧方向相反, 基于此, 可以扩展出四种曲线形式: 。

【自动驾驶】路径规划——ReedsShepp 曲线总结（python实现 | c++实现）
这 8 种曲线可以通过式(4)求解, 上述公式中的用于第 5 类公式的路径计算, 用于第 6 类公式的路径计算，式中的是曲线弧度计算函数，一般地，函数的曲线弧度值【自动驾驶】路径规划——ReedsShepp 曲线总结（python实现 | c++实现）是通过式(5) 进行计算。

【自动驾驶】路径规划——ReedsShepp 曲线总结（python实现 | c++实现）

2.4 三段圆弧与一条直线段组成

第七类基础曲线形式: , 即三段圆弧与一条直线段组成, 第二段圆弧为圆弧, 第三段为直线路径, 第一段与后三段圆弧方向相反, 基于此, 可以扩展出八种曲线形式: 、、、。
第八类基础曲线形式: , 即三段圆弧与一段直线组成, 第三段圆弧为圆弧, 第二段为直线路径, 第四段与前三段圆弧方向相反, 基于此, 可以扩展出八种曲线形式: 、。