【自动驾驶】路径规划 |
您所在的位置:网站首页 › 圆弧线计算公式是什么 › 【自动驾驶】路径规划 |
文章目录
参考资料1. Dubins 曲线计算1.1 坐标变换1.2 LSL路径1.3 RSR路径1.4 RSL路径1.5 LSR路径1.6 RLR路径1.7 LRL路径
参考资料
Classification of the Dubins setDubins 曲线计算笔记Dubins path planning
前文 Dubins 曲线推导(基于向量的方法) 前文已经从向量的角度进行了Dubins曲线公式的推导,这篇博客主要基于这篇论文《Classification of the Dubins set》介绍基于几何的关系下的dubins曲线公式,论文每一种情况都有详细的推导,因此这里只给出每种情况的公式,推导过程大家参考论文即可。 1. Dubins 曲线计算dubins路径集合为 { L S L , R S R , R S L , L S R , R L R , L R L } \{LSL, RSR, RSL, LSR, RLR, LRL\} {LSL,RSR,RSL,LSR,RLR,LRL}。 L L L表示向左转的圆弧运动, R R R表示向右转的圆弧运动, S S S表示沿直线运动。 1.1 坐标变换设起点为 s ( x i , y i , α i ) s\left(x_{i}, y_{i}, \alpha_{i}\right) s(xi,yi,αi) ,终点为 g ( x g , y g , β g ) g\left(x_{g}, y_{g}, \beta_{g}\right) g(xg,yg,βg) , 先坐标变换将起点平移至原点,并旋转 θ \theta θ 角,则终点也落在 x \mathrm{x} x 轴上,起点和终点的坐标为 s ( 0 , 0 , α ) , g ( d , 0 , β ) s(0,0, \alpha), g(d, 0, \beta) s(0,0,α),g(d,0,β) ,其中: θ = atan 2 ( y g − y i x g − x i ) m o d { 2 π } D = ( x i − x g ) 2 + ( y i − y g ) 2 d = D / R α = ( α i − θ ) m o d { 2 π } β = ( β g − θ ) m o d { 2 π } (1) \tag{1} \begin{gathered} \theta=\operatorname{atan} 2\left(\frac{y_{g}-y_{i}}{x_{g}-x_{i}}\right) \bmod\{2\pi\} \\ D=\sqrt{\left(x_{i}-x_{g}\right)^{2}+\left(y_{i}-y_{g}\right)^{2}} \\ d=D / R \\ \alpha= \left(\alpha_{i}-\theta\right) \bmod\{2\pi\} \\ \beta= \left(\beta_{g}-\theta\right) \bmod\{2\pi\} \end{gathered} θ=atan2(xg−xiyg−yi)mod{2π}D=(xi−xg)2+(yi−yg)2 d=D/Rα=(αi−θ)mod{2π}β=(βg−θ)mod{2π}(1) 几点说明: 其中 θ \theta θ 为起点和终点航向角度差,上面的角度都在 [ 0 , 2 π ] [0,2 \pi] [0,2π] 之间。 上面用 D D D 除上 R R R这样处理可以使每个最小转弯半径 R R R都为 1 ,由角度计算弧长时更方便,弧长即等于角度的弧度,因此在后面看到的 cos ( α ) \cos (\alpha) cos(α) ,其实是前面省略了 R R R 。 m o d ( ) mod() mod()是取模运算,例如: m o d ( 3 π , 2 π ) = 3 π m o d 2 π = π \bmod(3\pi,2\pi)=3\pi \bmod 2 \pi=\pi mod(3π,2π)=3πmod2π=π。下述的 β ( m o d 2 π ) \beta(\bmod 2 \pi) β(mod2π)也是这个意思,即 β \beta β对 2 π 2\pi 2π取模。python实现方式很简单,如下: def mod2pi(theta): """对2pi取模运算 """ return theta - 2.0 * math.pi * math.floor(theta / 2.0 / math.pi)坐标变换的python实现很简单,如下: # 坐标变换 dx = g_x-s_x dy = g_y-s_y D = math.hypot(dx, dy) d = D * curvature theta = mod2pi(math.atan2(dy, dx)) alpha = mod2pi(s_yaw - theta) beta = mod2pi(g_yaw - theta)设车辆在起始圆上走过的长度为 t t t ,直线段长度为 p p p ,第二个圆上的圆弧长度为 q q q,整个路径长度 L = t + p + q L=t+p+q L=t+p+q。下面依次给出6种情况的轨迹公式。 1.2 LSL路径![]() L S L LSL LSL路径的轨迹长度公式如下: t l s l = − α + arctan cos β − cos α d + sin α − sin β { m o d 2 π } p l s l = 2 + d 2 − 2 cos ( α − β ) + 2 d ( sin α − sin β ) q l s l = β − arctan cos β − cos α d + sin α − sin β { m o d 2 π } (2) \tag{2} \begin{aligned} &t_{l s l}=-\alpha+\arctan \frac{\cos \beta-\cos \alpha}{d+\sin \alpha-\sin \beta}\{\bmod 2 \pi\} \\ &p_{l s l}=\sqrt{2+d^{2}-2 \cos (\alpha-\beta)+2 d(\sin \alpha-\sin \beta)} \\ &q_{l s l}=\beta-\arctan \frac{\cos \beta-\cos \alpha}{d+\sin \alpha-\sin \beta}\{\bmod 2 \pi\} \end{aligned} tlsl=−α+arctand+sinα−sinβcosβ−cosα{mod2π}plsl=2+d2−2cos(α−β)+2d(sinα−sinβ) qlsl=β−arctand+sinα−sinβcosβ−cosα{mod2π}(2) 总长度等于: L l s l = t l s l + p l s l + q l s l = − α + β + p l s l (3) \tag{3} \mathcal{L}_{l s l}=t_{l s l}+p_{l s l}+q_{l s l}=-\alpha+\beta+p_{l s l} Llsl=tlsl+plsl+qlsl=−α+β+plsl(3) python实现如下: def left_straight_left(alpha, beta, d): """LSL路径 """ sa = math.sin(alpha) sb = math.sin(beta) ca = math.cos(alpha) cb = math.cos(beta) c_ab = math.cos(alpha - beta) tmp0 = d + sa - sb mode = ["L", "S", "L"] p_squared = 2 + (d * d) - (2 * c_ab) + (2 * d * (sa - sb)) if p_squared mod2π}(4)总长度等于: L r s r = t r s r + p r s r + q r s r = α − β + p r s r (5) \tag{5} \mathcal{L}_{r s r} =t_{r s r}+p_{r s r}+q_{r s r}=\alpha-\beta+p_{r s r} Lrsr=trsr+prsr+qrsr=α−β+prsr(5) python实现如下: def right_straight_right(alpha, beta, d): """RSR路径 """ sa = math.sin(alpha) sb = math.sin(beta) ca = math.cos(alpha) cb = math.cos(beta) c_ab = math.cos(alpha - beta) tmp0 = d - sa + sb mode = ["R", "S", "R"] p_squared = 2 + (d * d) - (2 * c_ab) + (2 * d * (sb - sa)) if p_squared mod2π}(6)总长度等于: L r s l = t r s l + p r s l + q r s l = − α + β + 2 t r s l + p r s l (7) \tag{7} \mathcal{L}_{r s l} =t_{r s l}+p_{r s l}+q_{r s l}=-\alpha+\beta+2 t_{r s l}+p_{r s l} Lrsl=trsl+prsl+qrsl=−α+β+2trsl+prsl(7) python实现如下: def right_straight_left(alpha, beta, d): """RSL路径 """ sa = math.sin(alpha) sb = math.sin(beta) ca = math.cos(alpha) cb = math.cos(beta) c_ab = math.cos(alpha - beta) p_squared = (d * d) - 2 + (2 * c_ab) - (2 * d * (sa + sb)) mode = ["R", "S", "L"] if p_squared mod2π}(8)总长度等于: L l s r = t l s r + p l s r + q l s r = α − β + 2 t l s r + p l s r (9) \tag{9} \mathcal{L}_{l s r} =t_{l s r}+p_{l s r}+q_{l s r}=\alpha-\beta+2 t_{l s r}+p_{l s r} Llsr=tlsr+plsr+qlsr=α−β+2tlsr+plsr(9) python实现如下: def left_straight_right(alpha, beta, d): """LSR路径 """ sa = math.sin(alpha) sb = math.sin(beta) ca = math.cos(alpha) cb = math.cos(beta) c_ab = math.cos(alpha - beta) p_squared = -2 + (d * d) + (2 * c_ab) + (2 * d * (sa + sb)) mode = ["L", "S", "R"] if p_squared mod2π}(10)总长度等于: L r l r = t r l r + p r l r + q r l r = α − β + 2 p r l r (11) \tag{11} \mathcal{L}_{r l r}=t_{r l r}+p_{r l r}+q_{r l r}=\alpha-\beta+2 p_{r l r} Lrlr=trlr+prlr+qrlr=α−β+2prlr(11) python实现如下: def right_left_right(alpha, beta, d): """RLR路径 """ sa = math.sin(alpha) sb = math.sin(beta) ca = math.cos(alpha) cb = math.cos(beta) c_ab = math.cos(alpha - beta) mode = ["R", "L", "R"] tmp_rlr = (6.0 - d * d + 2.0 * c_ab + 2.0 * d * (sa - sb)) / 8.0 if abs(tmp_rlr) > 1.0: return None, None, None, mode p = mod2pi(math.acos(tmp_rlr)) t = mod2pi(alpha - math.atan2(ca - cb, d - sa + sb) + mod2pi(p / 2.0)) q = mod2pi(alpha - beta - t + p) return t, p, q, mode上述实现方式得不到最优结果,很奇怪,下面这种写法倒是可以得到最优结果: def right_left_right(alpha, beta, d): """RLR路径,这部分的实现与公式不一致,改成与公式一致后反而得不到最优路径 """ sa = math.sin(alpha) sb = math.sin(beta) ca = math.cos(alpha) cb = math.cos(beta) c_ab = math.cos(alpha - beta) mode = ["R", "L", "R"] tmp_rlr = (6.0 - d * d + 2.0 * c_ab + 2.0 * d * (sa - sb)) / 8.0 if abs(tmp_rlr) > 1.0: return None, None, None, mode p = mod2pi(2 * math.pi - math.acos(tmp_rlr)) t = mod2pi(alpha - math.atan2(ca - cb, d - sa + sb) + mod2pi(p / 2.0)) q = mod2pi(alpha - beta - t + mod2pi(p)) return t, p, q, mode 1.7 LRL路径![]() L R L LRL LRL路径的轨迹长度公式如下: t l r l = ( − α + arctan ( − cos α + cos α d + sin α − sin β ) + p l r l 2 ) { m o d 2 π } p l r l = arccos 1 8 ( 6 − d 2 + 2 cos ( α − β ) + 2 d ( sin α − sin β ) ) { m o d 2 π } q l r l = β ( m o d 2 π ) − α + 2 p l r l { m o d 2 π } (12) \tag{12} \begin{aligned} &t_{l r l}=\left(-\alpha+\arctan \left(\frac{-\cos \alpha+\cos \alpha}{d+\sin \alpha-\sin \beta}\right)+\frac{p_{l r l}}{2}\right)\{\bmod 2 \pi\} \\ &p_{l r l}=\arccos \frac{1}{8}\left(6-d^{2}+2 \cos (\alpha-\beta)+2 d(\sin \alpha-\sin \beta)\right)\{\bmod 2 \pi\} \\ &q_{l r l}=\beta(\bmod 2 \pi)-\alpha+2 p_{l r l}\{\bmod 2 \pi\} \end{aligned} tlrl=(−α+arctan(d+sinα−sinβ−cosα+cosα)+2plrl){mod2π}plrl=arccos81(6−d2+2cos(α−β)+2d(sinα−sinβ)){mod2π}qlrl=β(mod2π)−α+2plrl{mod2π}(12) 总长度等于: L l r l = t l r l + p l r l + q l r l = − α + β + 2 p l r l . (13) \tag{13} \mathcal{L}_{l r l}=t_{l r l}+p_{l r l}+q_{l r l}=-\alpha+\beta+2 p_{l r l} . Llrl=tlrl+plrl+qlrl=−α+β+2plrl.(13) python实现如下: def left_right_left(alpha, beta, d): """LRL路径 """ sa = math.sin(alpha) sb = math.sin(beta) ca = math.cos(alpha) cb = math.cos(beta) c_ab = math.cos(alpha - beta) mode = ["L", "R", "L"] tmp_lrl = (6.0 - d * d + 2.0 * c_ab + 2.0 * d * (- sa + sb)) / 8.0 if abs(tmp_lrl) > 1: return None, None, None, mode p = mod2pi(math.acos(tmp_lrl)) t = mod2pi(-alpha - math.atan2(ca - cb, d + sa - sb) + p / 2.0) q = mod2pi(mod2pi(beta) - alpha +2*p) return t, p, q, mode同样上述实现方式得不到最优结果,下面这种写法可以得到最优结果: def left_right_left(alpha, beta, d): """LRL路径,这部分的实现与公式不一致,改成与公式一致后反而得不到最优路径 """ sa = math.sin(alpha) sb = math.sin(beta) ca = math.cos(alpha) cb = math.cos(beta) c_ab = math.cos(alpha - beta) mode = ["L", "R", "L"] tmp_lrl = (6.0 - d * d + 2.0 * c_ab + 2.0 * d * (- sa + sb)) / 8.0 if abs(tmp_lrl) > 1: return None, None, None, mode p = mod2pi(2 * math.pi - math.acos(tmp_lrl)) t = mod2pi(-alpha - math.atan2(ca - cb, d + sa - sb) + p / 2.0) q = mod2pi(mod2pi(beta) - alpha - t + mod2pi(p)) return t, p, q, mode以上完整代码文件见github仓库 由于在自动驾驶中算法实现一般使用C++,所以我也使用C++实现了相关功能,代码结构与python代码实现类似,这边就不再做相关代码解释了。完整代码详见另一个github仓库。 |
今日新闻 |
点击排行 |
|
推荐新闻 |
图片新闻 |
|
专题文章 |
CopyRight 2018-2019 实验室设备网 版权所有 win10的实时保护怎么永久关闭 |