专业财税服务推荐

精选优质财税服务,为企业提供专业、可靠的财税解决方案,助力企业健康发展

零报税代理记账
零申报代理记账
报税做账算帐财务报表老会计做账
代理记账
咨询微信:lhy_happyday
工商营业执照年度年报年检公示
全国个体、企业、公司、合作社工商年审年报服务!
个体/10元/次 企业/20元/次
咨询微信:lhy_happyday
财税咨询服务
一对一专业财税咨询,解决企业财税难题,提供定制方案
咨询微信:lhy_happyday
财务分析服务
小规模个体报税0申报税务年报工商年报月报季报报税代理记账
咨询微信:lhy_happyday
立即咨询专业财税顾问
微信号: lhy_happyday
会计从业9年,管理多家个体工商、小规模、一般纳税人等企业的财务、税务等相关工作!。
扫码或搜索添加微信,备注"财税咨询"获取专属优惠
知方号 知方号

自动驾驶路径规划 pp曲线和cc曲线

自动驾驶路径规划

文章目录 参考资料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 ) sleft(x_{i}, y_{i}, alpha_{i} ight) s(xi​,yi​,αi​) ,终点为 g ( x g , y g , β g ) gleft(x_{g}, y_{g}, eta_{g} ight) g(xg​,yg​,βg​) , 先坐标变换将起点平移至原点,并旋转 θ heta θ 角,则终点也落在 x mathrm{x} x 轴上,起点和终点的坐标为 s ( 0 , 0 , α ) , g ( d , 0 , β ) s(0,0, alpha), g(d, 0, eta) 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) ag{1} egin{gathered} heta=operatorname{atan} 2left(frac{y_{g}-y_{i}}{x_{g}-x_{i}} ight) mod{2pi} \ D=sqrt{left(x_{i}-x_{g} ight)^{2}+left(y_{i}-y_{g} ight)^{2}} \ d=D / R \ alpha= left(alpha_{i}- heta ight) mod{2pi} \ eta= left(eta_{g}- heta ight) mod{2pi} end{gathered} θ=atan2(xg​−xi​yg​−yi​​)mod{2π}D=(xi​−xg​)2+(yi​−yg​)2 ​d=D/Rα=(αi​−θ)mod{2π}β=(βg​−θ)mod{2π}​(1)

几点说明:

其中 θ heta θ 为起点和终点航向角度差,上面的角度都在 [ 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 π = π mod(3pi,2pi)=3pi mod 2 pi=pi mod(3π,2π)=3πmod2π=π。下述的 β (   m o d   2 π ) eta(mod 2 pi) β(mod2π)也是这个意思,即 β eta β对 2 π 2pi 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) ag{2} egin{aligned} &t_{l s l}=-alpha+arctan frac{cos eta-cos alpha}{d+sin alpha-sin eta}{mod 2 pi} \ &p_{l s l}=sqrt{2+d^{2}-2 cos (alpha-eta)+2 d(sin alpha-sin eta)} \ &q_{l s l}=eta-arctan frac{cos eta-cos alpha}{d+sin alpha-sin eta}{mod 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) ag{3} mathcal{L}_{l s l}=t_{l s l}+p_{l s l}+q_{l s l}=-alpha+eta+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 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仓库。

版权声明:本文内容由互联网用户自发贡献,该文观点仅代表作者本人。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如发现本站有涉嫌抄袭侵权/违法违规的内容, 请发送邮件至lizi9903@foxmail.com举报,一经查实,本站将立刻删除。