当前位置:   article > 正文

Fast planner 基本原理学习(一)

fast planner

一、主题:Fast planner 基本原理学习

二、目标:

  • 理解Fast planner轨迹规划处理流程
  • 理解hybrid A*的改进点
  • B样条曲线定义、性质、以及所带来的便利

三、正文:

1、Fast planner轨迹规划处理流程在这里插入图片描述

主要思想:前端考虑动力学进行规划,后端轨迹优化利用B样条曲线的性质。

  • 前端考虑动力学的作用:1、为了后端优化能得到效果更好的轨迹。2、利用Forward direction:discrete (sample) in control space可以很好的几何到A*算法中。
  • 后端采用B样条曲线作轨迹规划,在位置上,可以利用几个控制点描述一条曲线,利用B样条曲线的性质,可以将对轨迹的约束、动力学的约束加在控制点上,从而简化了计算。处理顺序是:1、先通过esdf地图提供梯度场信息,设计惩罚函数,使轨迹向障碍物少的地方移动。然后轨迹几何位置不变。在进行时间重分配,使轨迹符合动力学约束(最大速度、最大加速度在允许范围内。)
  • 备注:运动规划的轨迹是一条带时间参数的轨迹,同时需要符合避障、动力学的约束。一般工程上前端用来完成满足避障的几何路线。动力学的优化一般放在后端。

优点
1、使轨迹向梯度场小的方向移动,设计出来的轨迹在障碍物中间,相较其他方法会更加安全。
2、利用了B样条曲线,带来了很大的便利,因为B样条曲线具有以下性质:

  •   改变任意控制点,只改变有限时间段的轨迹。
    
    • 1
  •   B样条曲线的导数仍为B曲线。
    
    • 1

缺点

2、前端处理流程

3、B样条曲线

贝塞尔曲线(Bezier Curve)

  1. 定义
    B ( t ) = ∑ i = 0 n − 1 R i , n ( t ) P i R i , n ( t ) = C n i t i ( 1 − t ) n − i t ∈ [ 0 , 1 ] \boldsymbol{B}(t)=\sum_{i=0}^{n-1} R_{i, n}(t) \boldsymbol{P}_{i} \quad R_{i, n}(t)=C_{n}^{i} t^{i}(1-t)^{n-i} t \in[0,1] B(t)=i=0n1Ri,n(t)PiRi,n(t)=Cniti(1t)nit[0,1]
  2. 性质
  • 曲线的阶数随着控制点的增加而增加。
  • 改变任意一点会影响到整段轨迹。

B样条曲线(B-Spline)

  1. 定义
    C ( t ) = ∑ i = 0 n N i , p ( t ) Q i \mathbf{C}(t)=\sum_{i=0}^{n} N_{i, p}(t) \mathbf{Q}_{i} C(t)=i=0nNi,p(t)Qi
    [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-3DrQwf6o-1654346880211)(https://note.youdao.com/yws/res/9115/WEBRESOURCEb4bce5177d243739e738735aa7493741)]

  2. 性质

  • 更改控制点(control point)只能影响有限的时间,如图,如果 Q 3 Q_{3} Q3 的点发生移动。只有在时间段u1~u5内, N 1 , 3 N_{1,3} N1,3不为0。所以改变Q3的位置影响的时间段是有限的。
    [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-ieOUPM9O-1654346880212)(https://note.youdao.com/yws/res/9022/WEBRESOURCE66f35e9767e8147370c1de50d0b69df1)]

时间范围为什么两边缩小Pb的偏移,由下图可以很容易看出来。
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-Z1sfvvWd-1654346880212)(https://note.youdao.com/yws/res/9031/WEBRESOURCE607aca02d779f56235890a3769d23752)]

  • 一个时间段的轨迹由有限的点决定
    如图, [ u 3 , u 4 ) [u_{3},u_{4}) [u3,u4)的时间段只受到 Q 0 、 Q 1 、 Q 2 、 Q 3 Q_{0}、Q_{1}、Q_{2}、Q_{3} Q0Q1Q2Q3控制点的作用。
    [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-9QxElULu-1654346880213)(https://note.youdao.com/yws/res/9031/WEBRESOURCE607aca02d779f56235890a3769d23752)]

  • p>=1具有轨迹连续性质
    推到出C(t)的表达式易证。

  • B样条的导数还是B样条

    • 1阶导
      C ( ˙ u ) = ∑ i = 0 n − 1 N i + 1 , p − 1 ( u ) V i \mathbf {C} \dot(u)=\sum_{i=0}^{n-1} N_{i+1, p-1}(u) \mathbf{V}_{i} C(˙u)=i=0n1Ni+1,p1(u)Vi
    • 2阶导
      C ( ¨ u ) = ∑ i = 0 n − 2 N i + 2 , p − 2 ( u ) A i \mathbf{C} \ddot(u)=\sum_{i=0}^{n-2} N_{i+2, p-2}(u) \mathbf{A}_{i} C(¨u)=i=0n2Ni+2,p2(u)Ai
    • 控制点的计算
      V i = p ( Q i + 1 − Q i ) u i + p + 1 − u i + 1 \mathbf{V}_{i}=\frac{p\left(\mathbf{Q}_{i+1}-\mathbf{Q}_{i}\right)}{u_{i+p+1}-u_{i+1}} Vi=ui+p+1ui+1p(Qi+1Qi)
      A i = ( p − 1 ) ( V i + 1 − V i ) u i + p + 1 − u i + 2 \mathbf{A}_{i}=\frac{(p-1)\left(\mathbf{V}_{i+1}-\mathbf{V}_{i}\right)}{u_{i+p+1}-u_{i+2}} Ai=ui+p+1ui+2(p1)(Vi+1Vi)
    • 通过对高阶的轨迹表达式求导,可得到物理含义的速度、加速度等信息,且可是B样条曲线,方便利用B样条曲线的凸包性质。
  • 凸包性质(Convex Hull Property)

    • 直观上看,如下图,最左边的橙色时间段段为控制点$Q_{0}、Q_{1}、Q_{2}、Q_{3}$控制,可见这段轨迹是在控制点的连线区域内的。时间段以此类推,都在其对应控制点连线的多边行区域内。因此容易将轨迹的障碍物的距离约束,速度、加速度约束,转化到对控制点的约束,从而简化计算。
      [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-DCsNMUGU-1654346880214)(https://note.youdao.com/yws/res/9092/WEBRESOURCE24533b9b9a853cecbff80fb98b729f05)]
  • 时间均匀的B样条可以化成特殊的表达形式(Fast Planner中使用
    通式:
    [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-2ILWokeD-1654346880215)(https://note.youdao.com/yws/res/9133/WEBRESOURCE98c3430718259f8f1ef7755fb0b3f92a)]

    当p = 3, m = 3:
    p ( s ( u ) ) = s ( u ) T M 4 q 3 s ( u ) = [ 1 s ( u ) s 2 ( u ) s 3 ( u ) ] T q 3 = [ Q 0 Q 1 Q 2 Q 3 ] T s ( u ) = ( u − u 3 ) / Δ u

    p(s(u))=s(u)TM4q3s(u)=[1s(u)s2(u)s3(u)]Tq3=[Q0Q1Q2Q3]Ts(u)=(uu3)/Δu
    p(s(u))s(u)q3s(u)=s(u)TM4q3=[1s(u)s2(u)s3(u)]T=[Q0Q1Q2Q3]T=(uu3)/Δu
    推导:

    [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-75fmrpar-1654346880215)(https://note.youdao.com/yws/res/9137/WEBRESOURCE78be1357248d75e6332e2d6d0b1ace82)]

N 0 , 3 ( t ) = 1 6 { t 3 t ∈ [ 0 , 1 ) t 2 ( 2 − t ) + t ( 3 − t ) ( t − 1 ) + ( 4 − t ) ( t − 1 ) 2 t ∈ [ 1 , 2 ) t ( 3 − t ) 2 + ( t − 1 ) ( 3 − t ) ( 4 − t ) + ( 4 − t ) 2 ( t − 2 ) t ∈ [ 2 , 3 ) ( 4 − t ) 3 t ∈ [ 3 , 4 ) N_{0,3}(t)=\frac{1}{6}\left\{

t3t[0,1)t2(2t)+t(3t)(t1)+(4t)(t1)2t[1,2)t(3t)2+(t1)(3t)(4t)+(4t)2(t2)t[2,3)(4t)3t[3,4)
\right. N0,3(t)=61t3t2(2t)+t(3t)(t1)+(4t)(t1)2t(3t)2+(t1)(3t)(4t)+(4t)2(t2)(4t)3t[0,1)t[1,2)t[2,3)t[3,4)
N 1 , 3 ( t ) = 1 6 { ( t − 1 ) 3 t ∈ [ 1 , 2 ) ( t − 1 ) 2 ( 1 − t ) + ( t − 1 ) ( 2 − t ) ( t − 2 ) + ( 3 − t ) ( t − 2 ) 2 t ∈ [ 2 , 3 ) ( t − 1 ) ( 2 − t ) 2 + ( t − 2 ) ( 2 − t ) ( 3 − t ) + ( 3 − t ) 2 ( t − 3 ) t ∈ [ 3 , 4 ) ( 3 − t ) 3 t ∈ [ 4 , 5 ) . . . N_{1,3}(t)=\frac{1}{6}\left\{
(t13t[1,2)t12(1t)+t1(2t)(t2)+(3t)(t2)2t[2,3)t1(2t)2+(t2)(2t)(3t)+(3t)2(t3)t[3,4)(3t)3t[4,5)
\right. \\...
N1,3(t)=61(t13t12(1t)+t1(2t)(t2)+(3t)(t2)2t1(2t)2+(t2)(2t)(3t)+(3t)2(t3)(3t)3t[1,2)t[2,3)t[3,4)t[4,5)...

可计算 t ∈ [ u 3 , u 4 ) t \in[u_{3},u_{4}) t[u3,u4)段的轨迹:
推广到 t ∈ [ u m , u m + 1 ) t \in[u_{m},u_{m+1}) t[um,um+1)段的轨迹:
结论:由公式看出,只需要 Q m − 3 、 Q m − 2 、 Q m − 1 、 Q m Q_{m-3}、Q_{m-2}、Q_{m-1}、Q_{m} Qm3Qm2Qm1Qm的点,分配时间 Δ u \Delta u Δu,就可以确定` t ∈ [ u m , u m + 1 ) t \in[u_{m},u_{m+1}) t[um,um+1)段轨迹。

p ( s ( u ) ) = s ( u ) T M 4 q m s ( u ) = [ 1 s ( u ) s 2 ( u ) s 3 ( u ) ] T q m = [ Q m − 3 Q m − 2 Q m − 1 Q m ] T s ( u ) = ( u − u m ) / Δ u

p(s(u))=s(u)TM4qms(u)=[1s(u)s2(u)s3(u)]Tqm=[Qm3Qm2Qm1Qm]Ts(u)=(uum)/Δu
p(s(u))s(u)qms(u)=s(u)TM4qm=[1s(u)s2(u)s3(u)]T=[Qm3Qm2Qm1Qm]T=(uum)/Δu
M 4 = 1 3 ! [ 1 4 0 0 − 3 0 3 0 3 − 6 3 0 − 1 3 − 3 1 ] \mathbf{M}_{4}=\frac{1}{3 !}\left[
1400303036301331
\right]
M4=3!11331406303330001


  1. 理解:B样条曲线是贝塞尔曲线的一种特殊情况,规定了基函数的形式,使B样条有一些新的性质。由三个重要元素组成: degree(阶数)、control points(控制点)、knot vector(时间向量)。

  1. 基函数计算
    N i , 0 ( u ) = { 1  if  u i ≤ u < u i + 1 0  otherwise  N i , p ( u ) = u − u i u i + p − u i N i , p − 1 ( u ) + u i + p + 1 − u u i + p + 1 − u i + 1 N i + 1 , p − 1 ( u ) u 代 表 时 间 变 量
    \begin{array}{l} N_{i, 0}(u)=\left\{\begin{array}{lc} 1 & \text { if } u_{i} \leq u<u_{i+1} \\ 0 & \text { otherwise } \end{array}
    \right. \\ N_{i, p}(u)=\frac{u-u_{i}}{u_{i+p}-u_{i}} N_{i, p-1}(u)+\frac{u_{i+p+1}-u}{u_{i+p+1}-u_{i+1}} N_{i+1, p-1}(u) \end{array} u代表时间变量
    Ni,0(u)={10 if uiu<ui+1 otherwise Ni,p(u)=ui+puiuuiNi,p1(u)+ui+p+1ui+1ui+p+1uNi+1,p1(u)u
  • 不同阶的基函数间的计算关系
    [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-zTVGjvvA-1654346880216)(https://note.youdao.com/yws/res/8986/WEBRESOURCE770de27d10ca273bfa5aee2d14d71b0b)]

  • 0阶
    [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-xl309Lpu-1654346880217)(https://note.youdao.com/yws/res/8999/WEBRESOURCE433151066d5283f20fb94650b21e0e28)]

  • 1阶
    [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-gTlpMUaI-1654346880217)(https://note.youdao.com/yws/res/9001/WEBRESOURCE5450b7c8dc1da5438cfba6a9f0202b9e)]

  • 2阶
    [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-yfZSKZS1-1654346880218)(https://note.youdao.com/yws/res/9004/WEBRESOURCEa788c0231d44fd724bcbb4f1a6767d75)]

可以看出: 0阶时轨迹就是控制点。1阶时控制点连线的直线。2阶导时控制点有光滑曲线连接(轨迹连续可导)。

  • 注意:Faster Planner选用了三阶基函数。通过各阶基函数的计算关系,很好推导B样条曲线的性质。

4、后端处理流程

  1. 代价函数的设计

    • smoothness cost f s f_{s} fs
      f s = ∑ i = p b − 1 N − p b + 1 ∥ ( Q i + 1 − Q i ) + ( Q i − 1 − Q i ) ∥ 2 f_{s}=\sum_{i=p_{b}-1}^{N-p_{b}+1}\left\|\left(\mathbf{Q}_{i+1}-\mathbf{Q}_{i}\right)+\left(\mathbf{Q}_{i-1}-\mathbf{Q}_{i}\right)\right\|^{2} fs=i=pb1Npb+1(Qi+1Qi)+(Qi1Qi)2
      [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-tdARbn11-1654346880218)(https://note.youdao.com/yws/res/9212/WEBRESOURCE960200fd3e8289c35d25d80b6fc72542)]
    1. dynamic feasibility cost f v + f a f_{v}+f_{a} fv+fa
      设计惩罚函数
      [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-J8GogIjs-1654346880219)(https://note.youdao.com/yws/res/9220/WEBRESOURCEdbde5f889c636b0305e4fda94fc6efff)]
      请添加图片描述

      注意:因为是软约束,所以后续需要进行时间调整

    2. collision cos f c f_{c} fc

      f c = ∑ i = p b N − p b F c ( d ( Q i ) ) f_{c}=\sum_{i=p_{b}}^{N-p_{b}} F_{c}\left(d\left(\mathbf{Q}_{i}\right)\right) fc=i=pbNpbFc(d(Qi))
      可见,由B样条曲线的凸包性质,轨迹的约束都可以转化到控制点上,简化了计算。
      在这里插入图片描述

  2. 时间调整

参考:

《深蓝学院》移动机器人运动规划第8章

声明:本文内容由网友自发贡献,不代表【wpsshop博客】立场,版权归原作者所有,本站不承担相应法律责任。如您发现有侵权的内容,请联系我们。转载请注明出处:https://www.wpsshop.cn/w/繁依Fanyi0/article/detail/458883
推荐阅读
相关标签
  

闽ICP备14008679号