【自动驾驶】路径规划——ReedsShepp 曲线总结(python实现 | c++实现)
文章目录
- 参考资料
- 1. Reeds-Shepp 曲线
- 1.1 基本概念
- 1.2 字段组合
- 1.3 时间翻转(timeflip)、反射(reflect)和向后变换(backwards)
- 1.3.1 时间翻转(timeflip)
- 1.3.2 反射(reflect)
- 1.3.3 向后变换(backwards)
- 1.4 48 个字段组合
- 2. RS曲线求解
- 2.1 三段圆弧组成
- 2.2 两段圆弧与直线组成
- 2.3 四段圆弧组成
- 2.4 三段圆弧与一条直线段组成
- 2.5 四段圆弧与一条直线段组成
- 3. python实现
- 4. c++实现
参考资料
Reeds-Shepp和Dubins曲线简介
路径规划 - ReedsShepp 曲线
原论文:OPTIMAL PATHS FOR A CAR THAT GOES BOTH FORWARDS AND BACKWARDS
Reeds Shepp planning
一个实例了解自动驾驶路径规划 —— (四)HybridA*算法中RS曲线详解
乘用车自动泊车系统的路径规划及跟踪控制研究_陶鹏
前文
- Dubins 曲线推导(基于向量的方法)
- Dubins 曲线公式总结及python代码实现(基于几何的方法)
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 曲线更优的解。
曲线示例如下:
图片来源:https://blog.csdn.net/robinvista/article/details/95137143
如上图可以知道,在 Reeds-Shepp 曲线的路径中,允许尖瓣存在。
1.2 字段组合
在字段中的字母上加入上标,用来表示运动方向,表示如下:
符号 | 含义 | 绕单位圆 |
---|---|---|
L + L^+ L+ | 向前左转 | 逆时针 |
L − L^- L− | 向后左转 | 顺时针 |
R + R^+ R+ | 向前右转 | 逆时针 |
R − R^- R− | 向后右转 | 顺时针 |
S + S^+ S+ | 向前直走 | / |
S − S^- S− | 向后直走 | / |
使用 C 、 S C、S C、S 字符可以给出如下集合:
C C C ← { C + C − C + C + C − C − C + C + C − C + C β + C β − C − C + C β − C β − C + } C S C ← { C + S + C + C − C π / 2 + S + C + C + S + C π / 2 + C − C − C π / 2 + S + C π / 2 + C − } (1) \tag{1} \begin{aligned} &C C C \leftarrow\left\{\quad C^{+} C^{-} C^{+} \quad C^{+} C^{-} C^{-} \quad C^{+} C^{+} C^{-} \quad C^{+} C_{\beta}^{+} C_{\beta}^{-} C^{-} \quad C^{+} C_{\beta}^{-} C_{\beta}^{-} C^{+}\right\}\\ &C S C \leftarrow\left\{\quad C^{+} S^{+} C^{+} \quad C^{-} C_{\pi / 2}^{+} S^{+} C^{+} \quad C^{+} S^{+} C_{\pi / 2}^{+} C^{-} \quad C^{-} C_{\pi / 2}^{+} S^{+} C_{\pi / 2}^{+} C^{-}\right\} \end{aligned} CCC←{C+C−C+C+C−C−C+C+C−C+Cβ+Cβ−C−C+Cβ−Cβ−C+}CSC←{C+S+C+C−Cπ/2+S+C+C+S+Cπ/2+C−C−Cπ/2+S+Cπ/2+C−}(1)
通过反转等式 (1) 的符号可以获得新的的字段。上式 (1) 中, C π / 2 + C^+_{\pi/2} Cπ/2+表示相应的 L L L或 R R R的转过的角度为 π / 2 \pi/2 π/2 , C β C β C_{\beta}C_{\beta} CβCβ组合表示相应的弧段拥有相等的长度。
为了以紧凑的表示,避免 ± \pm ± ,可以写出充分路径的列表如下:
C C C ← { C ∣ C ∣ C C ∣ C C C C ∣ C C C β ∣ C β C C ∣ C β C β ∣ C } C S C ← { C S C C ∣ C π / 2 S C C S C π / 2 ∣ C C ∣ C π / 2 S C π / 2 ∣ C } (2) \tag{2} \begin{aligned} &C C C \leftarrow\left\{\begin{array}{ccccc} C|C| C & C \mid C C & C C \mid C & C C_{\beta} \mid C_{\beta} C & C\left|C_{\beta} C_{\beta}\right| C \end{array}\right\}\\ &C S C \leftarrow\left\{\begin{array}{llll} C S C & C \mid C_{\pi / 2} S C & C S C_{\pi / 2} \mid C & C\left|C_{\pi / 2} S C_{\pi / 2}\right| C \end{array}\right\} \end{aligned} CCC←{C∣C∣CC∣CCCC∣CCCβ∣CβCC∣CβCβ∣C}CSC←{CSCC∣Cπ/2SCCSCπ/2∣CC∣∣Cπ/2SCπ/2∣∣C}(2)
其中, ∣ | ∣表示车辆运动朝向由正向转为反向或者由反向转为正向。
将上述word分别带下标,如 C α ∣ C β ∣ C γ C_{\alpha}|C_{\beta}| C_{\gamma} Cα∣Cβ∣Cγ,其中 α , β , γ \alpha, \beta, \gamma α,β,γ分别表示旋转的角度(弧度制)。 S S S带下标 d d d表示行走的直线距离为 d d d。它们的范围如下表所示:
将 C C C替换为 L L L或 R R R ,通过简单的变换,则共有 48 种字段组合。这种简单变换包括时间翻转(timeflip)、反射(reflect)和向后变换(backwards)。
1.3 时间翻转(timeflip)、反射(reflect)和向后变换(backwards)
1.3.1 时间翻转(timeflip)
将计算出的曲线按照其运动方向进行取反,得到的新的曲线为原曲线相反的曲线。
如图,蓝色曲线 L − R + S + L + L^-R^+S^+L^+ L−R+S+L+与红色曲线 L + R − S − L − L^+R^-S^-L^- L+R−S−L−关于Y 轴左右对称,两条路径曲线上的路径航向角相反。从起始点 O ( 0 , 0 , 0 ) O(0, 0, 0) O(0,0,0)到目标点 A ( x , y , θ ) A(x, y,\theta) A(x,y,θ)的路径可以通过起始点 O ( 0 , 0 , 0 ) O(0, 0, 0) O(0,0,0)到目标点 B ( − x , − y , − θ ) B(-x,-y,-\theta) B(−x,−y,−θ)的路径取反计算获得。
1.3.2 反射(reflect)
第二种转换关系:“reflect”,即将计算的曲线按照其沿圆周运动方向取反,得到的曲线与原来的曲线长度相同的新曲线。
如图所示,红色曲线 R − L + S + R + R^- L^+ S^+ R^+ R−L+S+R+与蓝色曲线 L − R + S + L + L^- R^+ S^+ L^+ L−R+S+L+关于 X 轴左右对称,两条路径曲线上的路径航向角相反。从起始点 O ( 0 , 0 , 0 ) O(0,0,0) O(0,0,0) 到目标点 A ( x , y , θ ) A(x, y, \theta) A(x,y,θ)的路径可以通过起始点 O ( 0 , 0 , 0 ) O(0,0,0) O(0,0,0) 到目标点 B ( x , − y , − θ ) B(x,-y,-\theta) B(x,−y,−θ)的路径圆周方向取反计算获得。
1.3.3 向后变换(backwards)
第三种转换关系:“backwards”,即将原曲线的路径逆序转换,得到的曲线与原来的曲线长度相同的新曲线。
“backwards”实例图如图所示,蓝色曲线 L − S − R − L + L^-S^-R^- L^+ L−S−R−L+与红色曲线 L + R − S − L − L^+ R^- S^- L^- L+R−S−L−不具有以上两种的堆成关系,但是很直观的可以看出 L − S − R − L + L^-S^-R^- L^+ L−S−R−L+的逆序排列得到 L + R − S − L − L^+ R^- S^- L^- L+R−S−L−,两条路径曲线上的路径进行逆向排列。从起始点 O ( 0 , 0 , 0 ) O(0,0,0) O(0,0,0)到目标点 A ( x , y , θ ) A(x, y,\theta) A(x,y,θ) 的路径可以通过起始点 O ( 0 , 0 , 0 ) O(0,0,0) O(0,0,0) 到目标点 B ( x ∗ cos θ + y ∗ sin θ , x ∗ sin θ − y ∗ cos θ , θ ) B(x*\cos \theta +y*\sin\theta, x*\sin\theta -y*\cos\theta,\theta) B(x∗cosθ+y∗sinθ,x∗sinθ−y∗cosθ,θ) 的路径圆周方向取反计算获得。
1.4 48 个字段组合
所有的48个字段组合一一枚举如下:
图中,角标 β \beta β代表C旋转的弧度是 β \beta β,同理 π / 2 \pi/2 π/2也是代表旋转弧度,意思就是这个圆必须转 π / 2 \pi/2 π/2。
所以,Dubins曲线是从6个曲线中选出最短的那条,而Reeds-Shepp曲线是从48个曲线中选出最短的那个(最开始的论文认为最短的曲线一定在48条曲线中,后人研究发现有两条曲线不会是最短的,所以后来搜索范围减小到46条)。
至于这些字段组合具体的推导,请看原论文。
2. RS曲线求解
Reeds-Shepp 曲线的求解公式在原论文的式(8.1)~式(8.11)中。这边将其总结整理。
假设起点为 ( 0 , 0 , 0 ) (0,0,0) (0,0,0),终点为 ( x , y , θ ) (x,y,\theta) (x,y,θ)。
2.1 三段圆弧组成
- 第一类基础曲线: C ∣ C ∣ C C|C| C C∣C∣C, 即三段圆弧且每段圆弧两两圆弧方向相反, 基于此, 可以扩展出四种曲线形式: L + R − L + 、 L − R + L − 、 R + L − R + 、 R − L + R − L^{+} R^{-} L^{+} 、 L^{-} R^{+} L^{-} 、 R^{+} L^{-} R^{+} 、 R^{-} L^{+} R^{-} L+R−L+、L−R+L−、R+L−R+、R−L+R−。
- 第二类基础曲线: C C ∣ C C C \mid C CC∣C, 即三段圆弧且后两段圆弧的圆弧方向相反, 基于此, 可 以扩展出四种曲线形式: L + R + L − 、 L − R − L + 、 R + L + R − 、 R − L − R + L^{+} R^{+} L^{-} 、 L^{-} R^{-} L^{+} 、 R^{+} L^{+} R^{-} 、 R^{-} L^{-} R^{+} L+R+L−、L−R−L+、R+L+R−、R−L−R+。
- 第三类基础曲线: C ∣ C C C \mid C C C∣CC, 即三段圆弧且前两段圆弧的圆弧方向相反, 基于此, 可 以扩展出四种曲线形式: L + R − L − 、 L − R + L + 、 R + L − R − 、 R − L + R + L^{+} R^{-} L^{-} 、 L^{-} R^{+} L^{+} 、 R^{+} L^{-} R^{-} 、 R^{-} L^{+} R^{+} L+R−L−、L−R+L+、R+L−R−、R−L+R+。
通过时间翻转、反射和向后三种曲线变换方式, 上述 3 种公式的符号变换成 了 12 种曲线的运动方式, 可以利用公式(1)进行求解 。
{ ( u 1 , t 1 ) = R ( x − sin θ , y − 1 + cos θ ) { u 1 2 > 4 → L = ∞ u 1 2 ≤ 4 → { A = arcsin ( u 1 2 4 ) u = M ( A + t 1 ) ( u 2 , t 2 ) = R ( x − sin θ , y − 1 + cos θ ) t = t 2 v = M ( θ + t − u ) L = ∣ t ∣ + ∣ u ∣ + ∣ v ∣ 其中 A ∈ [ π 2 , π ] (1) \tag{1} \left\{\begin{array}{l} \left(u_{1}, t_{1}\right)=R(x-\sin \theta, y-1+\cos \theta) \\ \left\{\begin{array}{l} u_{1}^{2}>4 \rightarrow L=\infty \\ u_{1}^{2} \leq 4 \rightarrow\left\{\begin{array}{l} A=\arcsin \left(\frac{u_{1}^{2}}{4}\right) \\ u=M\left(A+t_{1}\right) \\ \left(u_{2}, t_{2}\right)=R(x-\sin \theta, y-1+\cos \theta) \\ t=t_{2} \\ v=M(\theta+t-u) \\ L=|t|+|u|+|v| \end{array}\right. \\ \text { 其中 } A \in\left[\frac{\pi}{2}, \pi\right] \end{array}\right. \end{array}\right. ⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧(u1,t1)=R(x−sinθ,y−1+cosθ)⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧u12>4→L=∞u12≤4→⎩⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎧A=arcsin(4u12)u=M(A+t1)(u2,t2)=R(x−sinθ,y−1+cosθ)t=t2v=M(θ+t−u)L=∣t∣+∣u∣+∣v∣ 其中 A∈[2π,π](1)
其中 t 、 u 、 v t 、 u 、 v t、u、v 代表公式中每次转向的弧度值大小(其实也是基础曲线三段圆弧的弧长,因为圆弧半径为1,单位圆), L L L为路径总长度;公式中的函数 R R R 是将笛卡尔坐标系中的 ( x , y ) (x, y) (x,y) 转化成极坐标系下的 ( u 1 , t 1 ) \left(u_{1}, t_{1}\right) (u1,t1), 其中径向坐标值有 u 1 、 u 2 u_{1} 、 u_{2} u1、u2, 角坐标值有 t 1 、 t 2 t_{1} 、 t_{2} t1、t2, 函数 M M M 用于对 2 π 2\pi 2π取模运算,并将弧度值限定在 [ − π , π ] [-\pi, \pi] [−π,π]。
函数 ( r , θ ) = R ( x , y ) (r,\theta)=R(x, y) (r,θ)=R(x,y)即笛卡尔坐标系中的 ( x , y ) (x, y) (x,y) 与极坐标系 ( r , θ ) (r,\theta) (r,θ)的关系可以表示为: { r cos θ = x r sin θ = y \begin{aligned} \left\{\begin{array}{l} r\cos{\theta}=x\\ r\sin{\theta}=y \end{array}\right.\\ \end{aligned} {rcosθ=xrsinθ=y
ψ = M ( θ ) \psi=M(\theta) ψ=M(θ)即表示 { ψ = θ m o d 2 π − π ≤ ψ < π \begin{aligned} \left\{\begin{array}{l} \psi=\theta \mod 2\pi\\ -\pi\le\psi<\pi \end{array}\right.\\ \end{aligned} {ψ=θmod2π−π≤ψ<π
后文除了新定义的符号,不再赘述已定义的符号。
2.2 两段圆弧与直线组成
- 第四类基础曲线形式: C S C C S C CSC, 即两段圆弧与直线组成, 基于此, 可以扩展出八种曲 线形式: L + S + L + 、 L − S − L − 、 R + S + R + 、 R − S − R − 、 L + S + R + 、 L − S − R − 、 R + S + L + 、 R − S − L − L^{+} S^{+} L^{+} 、 L^{-} S^{-} L^{-} 、 R^{+} S^{+} R^{+} 、 R^{-} S^{-} R^{-} 、 L^{+} S^{+} R^{+} 、 L^{-} S^{-} R^{-} 、 R^{+} S^{+} L^{+} 、 R^{-} S^{-} L^{-} L+S+L+、L−S−L−、R+S+R+、R−S−R−、L+S+R+、L−S−R−、R+S+L+、R−S−L−。
{ ( u , t ) = R ( x − sin θ , y − 1 + cos θ ) v = M ( θ − t ) L = ∣ t ∣ + ∣ u ∣ + ∣ v ∣ 其中 A ∈ [ 0 , π ] (2) \tag{2} \begin{aligned} \left\{\begin{array}{l} (u, t)=R(x-\sin \theta, y-1+\cos \theta) \\ v=M(\theta-t) \\ L=|t|+|u|+|v| \\ \text { 其中 } A \in[0, \pi] \end{array}\right. \end{aligned} ⎩⎪⎪⎨⎪⎪⎧(u,t)=R(x−sinθ,y−1+cosθ)v=M(θ−t)L=∣t∣+∣u∣+∣v∣ 其中 A∈[0,π](2)
{ ( u 1 , t 1 ) = R ( x − sin θ , y − 1 + cos θ ) { u 1 2 > 4 → L = ∞ u 1 2 ≤ 4 → { u = u 1 2 − 4 ( u 2 , t 2 ) = R ( u , 2 ) t = M ( t 1 + t 2 ) v = M ( t − u ) L = ∣ t ∣ + ∣ u ∣ + ∣ v ∣ (3) \tag{3} \begin{aligned} \left\{\begin{array}{l} \left(u_{1}, t_{1}\right)=R(x-\sin \theta, y-1+\cos \theta) \\ \left\{\begin{array}{l} u_{1}^{2}>4 \rightarrow L=\infty \\ u_{1}^{2} \leq 4 \rightarrow\left\{\begin{array}{l} u=\sqrt{u_{1}^{2}-4} \\ \left(u_{2}, t_{2}\right)=R(u, 2) \\ t=M\left(t_{1}+t_{2}\right) \\ v=M(t-u) \\ L=|t|+|u|+|v| \end{array}\right. \end{array}\right. \end{array}\right.\\ \end{aligned} ⎩⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎧(u1,t1)=R(x−sinθ,y−1+cosθ)⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧u12>4→L=∞u12≤4→⎩⎪⎪⎪⎪⎨⎪⎪⎪⎪⎧u=u12−4 (u2,t2)=R(u,2)t=M(t1+t2)v=M(t−u)L=∣t∣+∣u∣+∣v∣(3)
通过上述三种曲线变换方式,将第 4 种公式的符号变换成了 8 种曲线,这 8种曲线中前 4 种使用式(2) 进行求解,后 4 种使用式(3) 进行求解。
2.3 四段圆弧组成
第五类基础曲线形式: C C β ∣ C β C C C_{\beta} \mid C_{\beta} C CCβ∣CβC, 即四段圆弧组成, 第一段与第二段圆弧的圆弧方向相反, 第三段与第四段圆弧的圆弧方向相反, 前两段与后两段圆弧方向相反, 基于此, 可以扩展出四种曲线形式: L + R β + L β − R − 、 L − R β − L β + R + 、 R + L β + R β − L − 、 R − L β − R β + L + L^{+} R_{\beta}{ }^{+} L_{\beta}{ }^{-} R^{-} 、 L^{-} R_{\beta}{ }^{-} L_{\beta}{ }^{+} R^{+} 、 R^{+} L_{\beta}{ }^{+} R_{\beta}{ }^{-} L^{-} 、 R^{-} L_{\beta}{ }^{-} R_{\beta}{ }^{+} L^{+} L+Rβ+Lβ−R−、L−Rβ−Lβ+R+、R+Lβ+Rβ−L−、R−Lβ−Rβ+L+。
第六类基础曲线形式: C ∣ C β C β ∣ C C\left|C_{\beta} C_{\beta}\right| C C∣CβCβ∣C, 即四段圆弧组成, 第一段与第二段圆弧的圆弧方向相同, 第三段与第四段圆弧的圆弧方向相同, 前两段与后两段圆弧方向相反, 基于此, 可以扩展出四种曲线形式: L + R β − L β − R + 、 L − R β + L β + R − 、 R + L β − R β − L + 、 R − L β + R β + L − L^{+} R_{\beta}{ }^{-} L_{\beta}{ }^{-} R^{+} 、 L^{-} R_{\beta}{ }^{+} L_{\beta}{ }^{+} R^{-} 、 R^{+} L_{\beta}{ }^{-} R_{\beta}{ }^{-} L^{+} 、 R^{-} L_{\beta}{ }^{+} R_{\beta}{ }^{+} L^{-} L+Rβ−Lβ−R+、L−Rβ+Lβ+R−、R+Lβ−Rβ−L+、R−Lβ+Rβ+L−。
{ ξ = x + sin ( θ ) η = y − 1 − cos ( θ ) ρ 1 = 2 + ξ 2 + η 2 4 ρ 2 = 20 − ξ 2 − η 2 16 0 ≤ ρ 1 、 ρ 2 ≤ 1 → { u 1 = arccos ( ρ 1 ) u 2 = − arccos ( ρ 2 ) u = u 1 o r u 2 ( t , v ) = τ ω ( u , − u , ξ , η , θ ) L = ∣ t ∣ + 2 ∗ ∣ u ∣ + ∣ v ∣ 其中 u ∈ [ 0 , π / 2 ] 其它 L = ∞ (4) \tag{4} \left\{\begin{array}{l} \xi=x+\sin (\theta) \\ \eta=y-1-\cos (\theta) \\ \rho_{1}=\frac{2+\sqrt{\xi^{2}+\eta^{2}}}{4} \\ \rho_{2}=\frac{20-\xi^{2}-\eta^{2}}{16} \\ 0 \leq \rho_{1} 、 \rho_{2} \leq 1 \rightarrow\left\{\begin{array}{l} u_{1}=\arccos \left(\rho_{1}\right) \\ u_{2}=-\arccos \left(\rho_{2}\right) \\ u=u_1 \quad or \quad u_2\\ (t, v)=\tau \omega(u,-u, \xi, \eta, \theta) \\ L=|t|+2 *|u|+|v| \\ \text { 其中 } u \in[0, \pi / 2] \end{array}\right. \\ \text { 其它 } L=\infty \end{array}\right. ⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧ξ=x+sin(θ)η=y−1−cos(θ)ρ1=42+ξ2+η2 ρ2=1620−ξ2−η20≤ρ1、ρ2≤1→⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧u1=arccos(ρ1)u2=−arccos(ρ2)u=u1oru2(t,v)=τω(u,−u,ξ,η,θ)L=∣t∣+2∗∣u∣+∣v∣ 其中 u∈[0,π/2] 其它 L=∞(4)
这 8 种曲线可以通过式(4)求解, 上述公式中的 ρ 1 , u 1 \rho_{1},u_1 ρ1,u1 用于第 5 类公式的路径计算, ρ 2 , u 2 \rho_{2},u_2 ρ2,u2 用于第 6 类公式的路径计算,式中的 τ ω \tau \omega τω 是曲线弧度计算函数,一般地,函数的曲线弧度值 ( t , v ) = τ ω ( u , v 0 , ξ , η , θ ) (t,v)=\tau \omega(u,v_0,\xi,\eta,\theta) (t,v)=τω(u,v0,ξ,η,θ)是通过式(5) 进行计算。
{ σ = M ( u − v 0 ) ζ = sin ( u ) − sin ( σ ) ψ = cos ( u ) − cos ( σ ) − 1 t 1 = arctan η ∗ ζ − ξ ∗ ψ ξ ∗ ζ + η ∗ ψ λ = 2 ∗ ( cos ( σ ) − cos ( v 0 ) − cos ( u ) ) + 3 t = { M ( t 1 + π ) , λ < 0 M ( t 1 ) , λ ≥ 0 v = M ( t − ( u − v 0 ) − θ ) (5) \tag{5} \left\{\begin{array}{l} \sigma=M(u-v_0) \\ \zeta=\sin (u)-\sin (\sigma) \\ \psi=\cos (u)-\cos (\sigma)-1 \\ t_{1}=\arctan \frac{\eta * \zeta-\xi * \psi}{\xi * \zeta+\eta * \psi} \\ \lambda=2 *(\cos (\sigma)-\cos (v_0)-\cos (u))+3 \\ t=\left\{\begin{array}{l} M\left(t_{1}+\pi\right), \lambda<0 \\ M\left(t_{1}\right), \lambda \geq 0 \end{array}\right. \\ v=M(t-(u-v_0)-\theta) \end{array}\right. ⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧σ=M(u−v0)ζ=sin(u)−sin(σ)ψ=cos(u)−cos(σ)−1t1=arctanξ∗ζ+η∗ψη∗ζ−ξ∗ψλ=2∗(cos(σ)−cos(v0)−cos(u))+3t={M(t1+π),λ<0M(t1),λ≥0v=M(t−(u−v0)−θ)(5)
2.4 三段圆弧与一条直线段组成
第七类基础曲线形式: C ∣ C π / 2 S C C\mid C_{\pi / 2} SC C∣Cπ/2SC, 即三段圆弧与一条直线段组成, 第二段圆弧为 π / 2 \pi / 2 π/2 圆弧, 第三段为直线路径, 第一段与后三段圆弧方向相反, 基于此, 可以扩展出八种曲线形式: L + R π / 2 − S − R − 、 L − R π / 2 + S + R + 、 R + L π / 2 − S − L − 、 R − L π / 2 + S + L + 、 L + R π / 2 − S − L − L^{+} R_{\pi / 2}^{-} S^{-} R^{-} 、 L^{-} R_{\pi / 2}^{+} S^{+} R^{+} 、 R^{+} L_{\pi / 2}^{-} S^{-} L^{-} 、 R^{-} L_{\pi / 2}^{+} S^{+} L^{+} 、 L^{+} R_{\pi / 2}^{-} S^{-} L^{-} L+Rπ/2−S−R−、L−Rπ/2+S+R+、R+Lπ/2−S−L−、R−Lπ/2+S+L+、L+Rπ/2−S−L−、 L − R π / 2 + S + L + L^{-} R_{\pi / 2}^{+} S^{+} L^{+} L−Rπ/2+S+L+、 R + L π / 2 − S − R − R^{+} L_{\pi / 2}^{-} S^{-} R^{-} R+Lπ/2−S−R−、 R − L π / 2 + S + R + R^{-} L_{\pi / 2}^{+} S^{+} R^{+} R−Lπ/2+S+R+。
第八类基础曲线形式: C S C π / 2 ∣ C C S C_{\pi / 2} \mid C CSCπ/2∣C, 即三段圆弧与一段直线组成, 第三段圆弧为 π / 2 \pi / 2 π/2 圆弧, 第二段为直线路径, 第四段与前三段圆弧方向相反, 基于此, 可以扩展出八种曲线形式: L + S + L π / 2 + R − 、 L − S − L π / 2 − R + 、 R + S + R π / 2 + L − 、 R − S − R π / 2 − L + 、 R + S + L π / 2 + R − 、 R − S − L π / 2 − R + L^{+} S^{+} L_{\pi / 2}^{+} R^{-} 、 L^{-} S^{-} L_{\pi / 2}^{-} R^{+} 、 R^{+} S^{+} R_{\pi / 2}^{+} L^{-} 、 R^{-} S^{-} R_{\pi / 2}^{-} L^{+} 、 R^{+} S^{+} L_{\pi / 2}^{+} R^{-} 、 R^{-} S^{-} L_{\pi / 2}^{-} R^{+} L+S+Lπ/2+R−、L−S−Lπ/2−R+、R+S+Rπ/2+L−、R−S−Rπ/2−L+、R+S+Lπ/2+R−、R−S−Lπ/2−R+、 L + S + R π / 2 + L − 、 L − S − R π / 2 − L + L^{+} S^{+} R_{\pi / 2}^{+} L^{-} 、 L^{-} S^{-} R_{\pi / 2}^{-} L^{+} L+S+Rπ/2+L−、L−S−Rπ/2−L+。
{ ζ = x + sin ( θ ) η = y − 1 − cos ( θ ) ( ρ , ϑ ) = R ( − η , ζ ) { ( 1 ) ρ ≥ 2 → { ( T , ϑ 1 ) = R ( ρ 2 − 4 , − 2 ) t = M ( ϑ − ϑ 1 ) u = 2 − ϑ 1 v = M ( θ − t − π / 2 ) ( 2 ) { t = ϑ u = 2 − ρ v = M ( t + π / 2 − θ ) L = ∣ t ∣ + ∣ u ∣ + ∣ v ∣ + π / 2 其他, L = ∞ (6) \tag{6} \left\{\begin{array}{l} \zeta=x+\sin (\theta) \\ \eta=y-1-\cos (\theta) \\ \left(\rho, \vartheta\right)=R(-\eta, \zeta) \\ \left\{\begin{array}{l} (1) \rho \geq 2 \rightarrow\left\{\begin{array}{l} \left(T, \vartheta_{1}\right)=R\left(\sqrt{\rho^{2}-4},-2\right) \\ t=M\left(\vartheta-\vartheta_{1}\right) \\ u=2-\vartheta_{1} \\ v=M(\theta-t-\pi / 2) \end{array}\right. \\ (2)\left\{\begin{array}{l} t=\vartheta \\ u=2-\rho \\ v=M(t+\pi / 2-\theta) \end{array}\right. \\ L=|t|+|u|+|v|+\pi / 2 \\ \text { 其他,} L=\infty \end{array}\right. \end{array}\right. ⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧ζ=x+sin(θ)η=y−1−cos(θ)(ρ,ϑ)=R(−η,ζ)⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧(1)ρ≥2→⎩⎪⎪⎪⎨⎪⎪⎪⎧(T,ϑ1)=R(ρ2−4 ,−2)t=M(ϑ−ϑ1)u=2−ϑ1v=M(θ−t−π/2)(2)⎩⎨⎧t=ϑu=2−ρv=M(t+π/2−θ)L=∣t∣+∣u∣+∣v∣+π/2 其他,L=∞(6)
式中 u u u 表示直线运动长度, t 、 v t、v t、v 表示圆弧运动的弧度值,式(6)中,当圆弧路经由两段顺时针方向的曲线构成就利用(1)子式计算,当圆弧路径由两段逆时针方向的曲线构成就利用(2)子式计算。
2.5 四段圆弧与一条直线段组成
- 第九类基础曲线形式: C ∣ C π / 2 S C π / 2 ∣ C C\left|C_{\pi / 2} S C_{\pi / 2}\right| C C∣∣Cπ/2SCπ/2∣∣C, 即四段圆弧与一条直线段组成, 第二段圆弧与第四段圆弧为 π / 2 \pi / 2 π/2 圆弧, 中间路径为直线路径, 第二、三、四段圆弧方向与第一段、 第五段路径方向相反, 基于此, 可以扩展出四种曲线形式: L + R π / 2 − S − L π / 2 − R + 、 L − R π / 2 + S + L π / 2 + R − L^{+} R_{\pi / 2}^{-} S^{-} L_{\pi / 2}^{-} R^{+} 、 L^{-} R_{\pi / 2}^{+} S^{+} L_{\pi / 2}^{+} R^{-} L+Rπ/2−S−Lπ/2−R+、L−Rπ/2+S+Lπ/2+R−、 R + L π / 2 − S − R π / 2 − L + 、 R − L π / 2 + S + R π / 2 + L − R^{+} L_{\pi / 2}^{-} S^{-} R_{\pi / 2}^{-} L^{+} 、 R^{-} L_{\pi / 2}^{+} S^{+} R_{\pi / 2}^{+} L^{-} R+Lπ/2−S−Rπ/2−L+、R−Lπ/2+S+Rπ/2+L−。
{ ζ = x + sin ( θ ) η = y − 1 − cos ( θ ) ( ρ , ϑ ) = R ( ζ , η ) ρ ≥ 2 → { t = M ( ϑ − arccos ( − 2 ρ ) ) , t ∈ [ − π 2 , π 2 ] { t ≤ 0 → L = ∞ t > 0 → { u = 4 − ζ + 2 ∗ cos ( t ) sin ( t ) v = M ( t − θ ) L = ∣ t ∣ + ∣ u ∣ + ∣ v ∣ + 2 ∗ π 2 其他, L = ∞ (7) \tag{7} \left\{\begin{array}{l} \zeta=x+\sin (\theta) \\ \eta=y-1-\cos (\theta) \\ (\rho, \vartheta)=R(\zeta, \eta) \\ \rho \geq 2 \rightarrow\left\{\begin{array}{l} t=M\left(\vartheta-\arccos \left(-\frac{2}{\rho}\right)\right), t \in\left[-\frac{\pi}{2}, \frac{\pi}{2}\right] \\ \left\{\begin{array}{l} t \leq 0 \rightarrow L=\infty\\ t>0 \rightarrow\left\{\begin{array}{l} u=4-\frac{\zeta+2 * \cos (t)}{\sin (t)} \\ v=M(t-\theta) \\ L=|t|+|u|+|v|+2 * \frac{\pi}{2} \end{array}\right. \\ \end{array}\right. \\ \end{array}\right.\\ \text{其他, }L=\infty \end{array}\right. ⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧ζ=x+sin(θ)η=y−1−cos(θ)(ρ,ϑ)=R(ζ,η)ρ≥2→⎩⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎧t=M(ϑ−arccos(−ρ2)),t∈[−2π,2π]⎩⎪⎪⎨⎪⎪⎧t≤0→L=∞t>0→⎩⎨⎧u=4−sin(t)ζ+2∗cos(t)v=M(t−θ)L=∣t∣+∣u∣+∣v∣+2∗2π其他, L=∞(7)
3. python实现
由于自己写的代码bug实在太多了。。。不想改了,所以这边贴出几个开源的github代码:
python:
- https://github.com/ghliu/pyReedsShepp
- https://github.com/nathanlct/reeds-shepp-curves
- PythonRobotics
4. c++实现
C++:
- https://ompl.kavrakilab.org/ReedsSheppStateSpace_8cpp_source.html
【自动驾驶】路径规划——ReedsShepp 曲线总结(python实现 | c++实现)相关推荐
- 自动驾驶路径规划论文解析(5)
解析论文:A Sampling-Based Local Trajectory Planner for Autonomous Driving along a Reference Path 文章依然采用了 ...
- 自动驾驶路径规划:A*(Astar)算法
来源丨古月居 点击进入->3D视觉工坊学习交流群 1. 最佳优先搜索(Best-First Search) 最佳优先搜索(BFS),又称A算法,是一种启发式搜索算法(Heuristic Algo ...
- 自动驾驶路径规划技术-高速公路路径规划
Path Planning - Highway Driving project Github: https://github.com/williamhyin/CarND-Path-Planning E ...
- 自动驾驶路径规划概况
文章目录 前言 介绍 1. 路径规划在自动驾驶系统架构中的位置 2. 全局路径规划的分类 2.1 基础图搜索算法 2.1.1 Dijkstra算法 2.1.2 双向搜索算法 2.1.3 Floyd算法 ...
- 自动驾驶路径规划五大常用算法(Dijkstra/人工势场/图搜索等)
编辑 | 希骥智能网联汽车 点击下方卡片,关注"自动驾驶之心"公众号 ADAS巨卷干货,即可获取 点击进入→自动驾驶之心[规划控制]技术交流群 后台回复[规划控制综述]获取自动驾驶 ...
- 汇总!自动驾驶路径规划中的障碍物处理方法总结
作者 | 西涯先生 编辑 | 汽车人 原文链接:https://zhuanlan.zhihu.com/p/635971229 点击下方卡片,关注"自动驾驶之心"公众号 ADAS巨 ...
- 自动驾驶路径规划——路径规划入门须知
目录 前言 1.无人驾驶关键技术 2.路径规划基本概念与分类 2.1 路径规划基本概念 2.1.1 路径规划需要解决的问题 2.1.2 路径规划--现在的研究 2.2路径规划的分类 2.3路径规划的流 ...
- 自动驾驶路径规划论文解析(4)
本文解析论文Runtime-Bounded Tunable Motion Planning for Autonomous Driving 论文的两个点我认为都比较值得应用,接下来有时间我会进行测试,但 ...
- 自动驾驶路径规划论文解析(3)
本文解析文章:On-Road Trajectory Planning for General Autonomous driving with enhanced tunability 文章稀松平常,没什 ...
最新文章
- 【面向对象编程】(3) 类之间的交互,依赖关系,关联关系
- 讨论SELinux对系统安全的影响(转)
- Day 8: Harp.JS——现代静态web服务器
- BZOJ2752: [HAOI2012]高速公路(road)(线段树 期望)
- MATLAB如何把图片显示在GUI上
- android studio 2.0 导入工程
- JavaScript中的数据结构及实战系列(1):队列
- Copula函数初了解
- mysql alter操作
- ubuntu linux qq闪退
- 小米pro15拆机_小米笔记本拆解
- 利用python进行微信好友数据分析
- 菜鸟教程python3 mysql_MySQL菜鸟教程
- 苹果平替笔哪款好用?性价比最高的苹果平替笔
- 每天一道算法题——拼音翻译成阿拉伯数字(只有数字拼音)
- 漫画 | 抱歉,你可能看到了假的通信史!
- 什么是智慧交通研究(ITS)
- listview 点击 item 字体变颜色
- AIoT在智慧景区中的应用
- 详解pdfFactory的页面管理功能
热门文章
- 2018北邮计算机硕士就业,2018北京高校毕业生就业率排名,北邮第一,清华北大不在前五...
- 搜索技巧:怎样在网上能找到好图片?
- 学校电脑计算机排序摆放,录取:电脑排序两分钟搞定
- 2022中级会计分录
- springboot xjar 安全加密运行工具
- springboot 使用shiro集成阿里云短信验证码
- Value '0000-00-00 00:00:00' can not be represented as java.sql.Timestamp
- QDUOJ二次开发记录(前端、后端)
- TCGA的拷贝数变异CNV可视化
- 卡巴斯基被列入黑名单,转用免费的麦咖啡