精华内容
下载资源
问答
  • 基于采样路径规划算法总结
    千次阅读
    2021-04-18 12:05:27

    基于采样的路径规划算法总结

    路径规划算法大致可以分为两类,一类是基于搜索的规划,另一类就是本文将要涉及的基于采样的规划。一般而言,基于搜索的规划(如A*)通常是运行在栅格地图上的。当栅格的分辨率越高时,算法搜索的路径就会越优。

    还有一类算法是基于采样的,主要就是RRT和它的变种算法。这类算法的核心在于随机采样,从父节点开始,随机在地图上生成子节点,连接父子节点并进行碰撞检测,若无碰撞,就扩展该子节点。就这样,不断地随机扩展样本点,直到生成一条连接起点和终点的路径。如下图所示,RRT算法的扩展图与盘根错节的树枝十分相似。

    在这里插入图片描述

    RRT算法是一种快速搜索算法,但是它却是以牺牲最优性为代价的。RRT算法搜索到的路径往往不是最优路径。为了解决这个问题,后来出现了RRT* ,使得RRT拥有了渐进最优的特性。如下图所示,随着节点数量的增加,RRT* 会不断地优化路径,最终趋近于最优。

    在这里插入图片描述

    虽然 RRT* 拥有了在理论上可以找到最优路径的能力,但这种向最优逼近的速度并不快。因此,为了提高 RRT* 的收敛速度,Informed-RRT* 出现了,提高了初始路径向最优路径的收敛速度。

    Informed-RRT* 的主要思路是在于椭圆的一个特性:椭圆上的点到椭圆两个焦点的距离之和相同,而椭圆外的点到椭圆两个焦点的距离之和大于前者,内点则反之。

    当Informed-RRT* 第一次找到一条可用路径时,就会根据该路径长度画出一个椭圆,该椭圆上的点到两焦点的距离就是该路径长度。因此,为了优化当前路径,算法只需要在该椭圆内扩展样本即可。每当找到一条更短路径,椭圆的范围也会随着变小,如下图所示。

    在这里插入图片描述

    本质上,Informed-RRT* 提供了一个向最优路径优化的方向,从而极大地提高它的收敛速度。这种优化方法使得RRT的样本不再是 均匀(uniform) 的随机扩展,而是 有偏向(bias) 的随机扩展。

    下面是我会详细介绍RRT、RRT-CONNECT、RRT* 、Informed-RRT* 算法。算法是使用MATLAB实现的,程序源码放在这里

    1.RRT

    关于RRT的编程实现比较简单,但我初次编程时仍然有很多需要注意的地方。首先理一理RRT的整体执行流程:

    1. 初始化节点扩展的步长 s t e p step step。初始化VERTEX表格,该表格用于储存每次随机生成的且满足条件的节点,然后把起点加入到V表中;

    2. 以整张地图为范围,随机生成一组坐标,以此作为一个新节点 v r a n d v_{rand} vrand。然后根据V表,从中找到离 v r a n d v_{rand} vrand最近的节点作为它的父节点 v n e a r e s t v_{nearest} vnearest

    3. 计算 v r a n d v_{rand} vrand v n e a r e s t v_{nearest} vnearest的距离,取与步长相比较后的最小值,防止两节点之间的距离过长;

      dist = min([norm(new_vertex-near_vertex),step_len]);  % 两节点之间的距离
      theta = atan2(new_node.ny-near_node.ny,new_node.nx-near_node.nx);  % 两节点之间的方向
      new_node.nx = near_node.nx + dist*cos(theta);
      new_node.ny = near_node.ny + dist*sin(theta);
      
    4. 对于edge( v r a n d v_{rand} vrand, v n e a r e s t v_{nearest} vnearest)进行碰撞检测,若满足要求,则将该节点加入到V表中。反之,则返回第二步;

    5. 判断 v r a n d v_{rand} vrand与终点的距离,若其小于步长并且之间无碰撞,则把终点加入到V表中,并设置 v r a n d v_{rand} vrand为终点的父节点,然后根据父节点提取出一条可行路径。反之,则返回第二步。

    观察代码和可视化图像,可以发现,RRT算法并没有设置任何优化路径的程序。当RRT发现一条可连接起点与终点的可行路径时,就直接把这条路径作为最终解。也就是说,RRT只能找到一个可行解,但不保证该解就是最优解。

    在这里插入图片描述

    2.RRT-CONNECT

    观察可视化图像,可以发现RRT的均匀采样(uniform sample)很没有效率,会把大部分的采样点放在无意义的区域。如果我们可以缩小这个区域,那么就可以极大提高算法寻径的效率,这也就是所谓的偏置采样(bias sample)。

    RRT-CONNECT为了提高RRT算法的效率,做出了两点改进:

    The method is based on two ideas: the Connect heuristic that attempts to move over a longer distance, and the growth of RRTs from both q i n i t q_{init} qinit and q g o a l q_{goal} qgoal.

    1. 采用贪婪策略进行采样,算法会尝试往距离更远的地方移动;
    2. 采用双向生长,即起点和终点均作为生长树的起点。

    关于第一点可能比较难以理解。简单来说,因为该算法是双向生长的,当两个树的某两个节点之间存在可生长空间(即不会与障碍物碰撞)时,算法会优先在这两个节点之间扩展新节点。这样就可以让两个生长树快速相交。

    在这里插入图片描述

    这里主要说明RRT-CONNECT与RRT算法不同的地方。首先,由于前者是双向生长的,因此需要两个VERTEX表格,用于储存从起点和从终点扩展的节点。接下来,详细介绍RRT-CONNECT扩展节点的过程:

    1. 首先,在地图范围内随机确定一个节点 v r a n d v_{rand} vrand,然后找到新节点与V1表(从起点开始)中所有节点的最近节点作为 v n e a r e s t v_{nearest} vnearest
    2. 由于刚开始V1表中只有起点,若两点之间无碰撞,则会从起点附近扩展出一个新节点 v n e w v_{new} vnew(规范化距离后的 v r a n d v_{rand} vrand);
    3. 接下来,算法会从V2表(从终点开始)中找到与 v n e w v_{new} vnew最近的节点 v n e w 2 v_{new2} vnew2,同样V2表中只有终点,当两点无碰撞时,会从终点附近扩展出一个新节点 v n e w 2 v_{new2} vnew2(规范化);
    4. 此时,算法判断节点 v n e w v_{new} vnew v n e w 2 v_{new2} vnew2之间存在可扩展的空间。于是不再随机扩展节点,而是从这两点之间扩展节点。具体操作就是以 v n e w 2 v_{new2} vnew2为起点,朝 v n e w v_{new} vnew的方向以一定步长扩展新节点 v n e w 3 v_{new3} vnew3,若节点 v n e w 2 v_{new2} vnew2 v n e w 3 v_{new3} vnew3之间仍然无碰撞,则继续扩展下一个节点,直到节点 v n e w v_{new} vnew v n e w i v_{newi} vnewi接触或与障碍物碰撞才结束这个贪婪扩展过程。

    如下图,当节点扩展到 v n e w 3 v_{new3} vnew3时, v n e w 3 v_{new3} vnew3 v n e w 2 v_{new2} vnew2之间与障碍物碰撞,故放弃节点 v n e w 3 v_{new3} vnew3并结束贪婪扩展的过程。在第一次扩展结束后,交换两个V表的位置,即下一次扩展时把V1表的地方用V2表替换,把V2表的地方用V1表替换。如此反复,直到找到两个生长树相连。

    在这里插入图片描述

    3.RRT*

    RRT-CONNECT通过贪婪生长和双向生长技术极大提高了RRT寻找可行路径的速度,但是仍然没有考虑如何去优化可行解。RRT* 弥补了这个问题,它使得RRT算法获得了渐进最优的能力,即随着采样点数量的增加,算法获得的路径会逐渐向最优路径靠拢。

    RRT* 是如何实现这一功能的呢?

    RRT* 考虑了每一个节点到起点的代价,代价越低就越可能被选择为路径节点。这种方式在基于搜索的路径规划中早已经被广泛使用了。RRT* 寻找初始路径的过程与RRT相同,区别在于前者找到初始路径后程序并没有结束,而是会继续生成采样点并不断更新初始路径。随着样本点的增加,初始路径会逐渐向最优路径靠近。

    下面主要描述RRT* 优化初始路径的过程。

    1. 在地图上随机产生一个样本点 v r a n d v_{rand} vrand,找到最近点 v n e a r e s t v_{nearest} vnearest并经过碰撞检测后,算法会以 v r a n d v_{rand} vrand为圆心、以一定半径生成一个圆形区域,从V表中找到所有在该区域内的节点,若其通过碰撞检测,则将其作为 v r a n d v_{rand} vrand的相邻节点 v n e a r v_{near} vnear。关于半径的计算方式如下:
      r = min ⁡ ( γ R R T ∗ ( ln ⁡ ( c a r d ( V ) ) c a r d ( V ) ) 1 d , η ) r = \min(\gamma_{RRT*} (\frac{\ln(card(V))}{card(V)})^{\frac{1}{d}}, \eta) r=min(γRRT(card(V)ln(card(V)))d1,η)
      上式中,V表示节点集合, c a r d ( V ) card(V) card(V)表示集合V的元素个数,也就是节点的数量。d表示构型空间的维数,由于路径规划是在二维空间寻径的,故 d = 2 d=2 d=2 η \eta η表示RRT的节点扩展步长, γ R R T ∗ \gamma_{RRT*} γRRT是一个系数。

    2. **从所有相邻节点中,为 v r a n d v_{rand} vrand找到一个父节点。**具体方式是先计算由相邻节点那条路径移动到 v r a n d v_{rand} vrand的代价,即从起点开始的代价( v s t a r t → . . . → v n e a r → v r a n d v_{start} \rightarrow ... \rightarrow v_{near} \rightarrow v_{rand} vstart...vnearvrand)。然后,取最小代价的节点作为 v r a n d v_{rand} vrand的父节点,并且更新 v r a n d v_{rand} vrand的代价;

    3. **根据代价修剪(rewire)生长树。**仍然是对相邻节点操作,这次是从 v r a n d v_{rand} vrand开始。需要注意的是此时的 v r a n d v_{rand} vrand已经有父节点了,假设其父节点为 v n e a r 1 v_{near1} vnear1,那么与相邻点的路径应该是 v s t a r t → . . . → v n e a r 1 → v r a n d → v n e a r v_{start} \rightarrow ... \rightarrow v_{near1} \rightarrow v_{rand} \rightarrow v_{near} vstart...vnear1vrandvnear,若代价小于 v n e a r v_{near} vnear原来的代价,则更新 v n e a r v_{near} vnear的父节点为 v r a n d v_{rand} vrand并更新原代价值。

    注:仅比较圆内节点的代价,而不是所有节点的代价,虽然可以减少计算代价,但是也可能会遗漏一些潜在的更好的路径。当然,这个问题只会出现在迭代次数不多时,随着样本点逐渐增多,算法仍然可以找到趋近于最佳路径的结果。

    在这里插入图片描述

    RRT* 在提取路径时与RRT不同,因为生成的路径不止一条,需要根据代价找到代价最小的那条路径作为结果。关键在于要找到代价最小的节点作为终点的父节点。具体寻找过程如下:

    1. 以终点为圆心、扩展步长为半径生成一个圆形区域,排除所有圆外节点和不通过碰撞检测的圆内节点;
    2. 计算剩余的所有节点代价,取最小代价的节点作为终点的父节点。

    虽然RRT* 根据节点代价会逐渐更新路径,但是它在初始路径的生成和路径的更新过程中,仍然采用了uniform sample的方式进行采样。在实验过程中,我明显的感觉到RRT* 算法向最优路径的收敛速度太慢。虽然理论上,它可以找到最优路径,但是我们仍然希望它向最优路径的收敛速度可以更快一点。

    4.Informed-RRT*

    关于RRT* 虽然保证了其路径渐进最优的特性,但是当采样环境维数较大时,其向最优路径收敛的速度会变得很慢。这是因为RRT* 是对集合 X X X(地图上的所有点的集合)进行采样的,但是集合 X X X中的大部分点对于路径改善是不起作用的。

    但是如果我们可以缩小采样集合的范围,那么是否就可以提高算法的收敛速度呢?

    这就是Informed-RRT* 提高收敛速度的方法。它把采样范围缩小到了一个椭圆的范围,算法只要在这个椭圆中采样就可以了。

    关于为什么可以这么做就需要先介绍一下启发函数(heuristic function)的作用。假设存在一个启发函数 f ( ⋅ ) f(\cdot) f(),它可以真实描述节点x的最低代价(从起点到终点且经过节点x的最优路径的代价)。但是通常我们是无法知道这个启发函数 f ( ⋅ ) f(\cdot) f(),故只能设计一个函数 f ^ ( ⋅ ) \hat{f}(\cdot) f^()来估计它。

    当然, f ^ ( ⋅ ) \hat{f}(\cdot) f^()并不是随便设置的,它存在一定的限制,必须满足下式:
    ∀ x ∈ X , f ^ ( x ) ≤ f ( x ) \forall x \in X, \hat{f}(x)\le f(x) xX,f^(x)f(x)
    也就是说, f ^ ( ⋅ ) \hat{f}(\cdot) f^()估计的代价不能高于被估计路径的真实代价。这很好理解,假设当前存在一条路径 p p p,其代价为 c c c(该路径是已知的,故可以计算真实代价)。假设在所有可行路径中,代价比它低有m条路径。当我们使用 f ^ ( ⋅ ) \hat{f}(\cdot) f^()对路径估计代价时,由于不知道其真实代价,必然会导致两种情况:

    • f ^ ( ⋅ ) \hat{f}(\cdot) f^()高估了真实代价,那么就有可能会把原本代价比路径 p p p低的其它路径设置的比 p p p更高了。也就是说在这种启发函数下,比当前路径更优的路径数量会小于m条,这就会存在漏掉最优路径的风险;
      X f ^ ⊆ X f X_{\hat{f}} \subseteq X_f Xf^Xf

    • f ^ ( ⋅ ) \hat{f}(\cdot) f^()低估了真实代价,那么就有可能会把原本代价比路径 p p p高的其它路径设置的比 p p p更低了。也就是说在这种启发函数下,比当前路径更优的路径数量会大于m条,虽然我们要搜索的范围变大了,但不会漏掉最优路径。
      X f ^ ⊇ X f X_{\hat{f}} \supseteq X_f Xf^Xf
      注: X f X_f Xf表示可以改善当前路径代价的节点的集合, X f ^ X_{\hat{f}} Xf^是对其的估计。

    经过以上分析可知,第二种情况更好。因此,当启发函数满足第二种情况时,可以称启发函数是 “admissible”。满足第二种情况的启发函数很好找,直接用欧几里得距离描述就可以了,因为两点之间直线最短。

    由上可知,可以改善当前路径的节点均存在于集合 X f ^ X_{\hat{f}} Xf^。Informed-RRT* 巧妙的设计了一种启发函数,可以直接从集合 X f ^ X_{\hat{f}} Xf^进行采样,故效率会远高于RRT* 。Informed-RRT* 在找到初始路径后,会根据初始路径的长度设置一个椭圆,如下图所示。
    X f ^ = { x ∈ X    ∣    ∣ ∣ x s t a r t − x ∣ ∣ 2 + ∣ ∣ x − x g o a l ∣ ∣ 2 ≤ c b e s t } X_{\hat{f}}=\{ x \in X \; | \; ||x_{start}-x||_2 + ||x-x_{goal}||_2 \le c_{best} \} Xf^={xXxstartx2+xxgoal2cbest}

    在这里插入图片描述

    通过把椭圆的长轴设置为初始路径的长度,椭圆外的点到两焦点(起点和终点)的距离之和必然会大于初始路径长度。而若某点的最短路径长度都大于初始路径长度,那么这个点就没有采样的必要了。这样就可以把采样范围缩小到一个椭圆, X f ^ X_{\hat{f}} Xf^就变成了椭圆内的点集,最优路径必然存在于椭圆内。

    有了理论基础后,接下来需要解决的就是如何对一个椭圆实现均匀采样。关于这个问题,原论文有详细介绍,这里就不再赘述。

    在这里插入图片描述

    上图是Informed-RRT* 的伪代码,它是直接在RRT* 的基础上修改的,上图中红色标注的就是修改部分。可以发现,修改部分很少,基本上只是修改了采样的部分。

    在这里插入图片描述

    更多相关内容
  • 关注《混沌无形》,获取更多硬核干货。
  • 基于采样路径规划算法 1.快速搜索随机树 快速搜索随机树(RRT)算法从起始点开始,在地图上进行随机采样,然后根据采样点信息,结合障碍物检测等约束条件,构建一棵搜索书,直到树的枝叶延伸至目标点或者达到预设...

    基于采样的路径规划算法

    1.PRM算法

    PRM(Probabilistic Road Map)算法,即概率路线图算法是一种融合了采样和图搜索的综合查询方法,该方法首先构建路线图,通过采样和碰撞检测建立完整的无向图,从而得到环境的完整连接属性,然后再通过图搜索得到可行的路径。PRM算法包括学习阶段和查询阶段两部分。
    学习阶段 (learning phase)

    • 初始化,假设G(V,E)为无向图,顶点集V为环境中无碰撞的节点,E表示无碰撞路径,初始状态设定为空集;
    • 采样,从环境空间中采样一个无碰撞的点a(i)加入到顶点集V中;
    • 计算邻域,定于距离p,对于已经存在于顶点集V中的点,如果它与a(i)的距离小于p。称其为a(i)的邻域点;
    • 边线连接:将a(i)与其邻域点相连,生成连线t;
    • 碰撞检测,检测连线t与障碍物是否发生了碰撞,若未发生碰撞,将其加入连线集E中;
    • 结束条件,当所有采样点完成上述步骤后结束,否则重复上一步

    查询阶段 (query phase)
    学习阶段建立了无向图,查询阶段使用Dijkstra算法或A*等图搜索算法对无向图G进行搜索,若能找到起始点到终点的路线,表明存在可行的运动规划。
    在这里插入图片描述

    2.RRT(快速搜索随机树)

    快速搜索随机树(RRT)算法通过对状态空间中的采样点进行碰撞检测,避免对空间进行建模,可以有效解决高维空间和复杂约束的路径规划问题。该方法是一种增量式采样的搜索方法,可以快速有效地搜索高维空间,通过状态空间的随机采样点,把搜索导向空包区域,从而寻找到一条起始点到目标点的规划路径。

    RRT算法思想
    RRT以一个初始点作为根节点,通过随机采样增加子节点的方式,生成一个随机扩展树,当随机数的叶子节点包含了目标点或进入了目标区域,便可以在随机树中找到一条由初始点到目标点的路径。具体步骤如下:

    • 初始化随机数,仅包含一个根节点 q i n i t q_{init} qinit
    • 使用随机采样函数从状态空间随机选择一个采样点 q r a n d q_{rand} qrand
    • 使用nearest函数从随机树中选择一个距离 q r a n d q_{rand} qrand最近的节点 q n e a r e s t q_{nearest} qnearest
    • 通过扩展函数extend从 q n e a r e s t q_{nearest} qnearest q r a n d q_{rand} qrand扩展一段距离,得到新节点 q n e w q_{new} qnew
    • 如果 q n e w q_{new} qnew与障碍物发生了碰撞,扩展函数extend返回空值,放弃生长,否则将 q n e w q_{new} qnew加入到随机树
    • 充数上述步骤,当 q n e a r e s t q_{nearest} qnearest与目标点 q g o a l q_{goal} qgoal小于一个阈值时,标明随机树到达了目标点

    简单的改进
    在随机树的每次生长过程中,设置一个随机概念,确定 q r a n d q_{rand} qrand是目标点还是随机点,设置参数prob,每次得到一个0与1之间的随机值p,当0<p<prob,随机树朝目标点生长;当prob<P<1时,随机树朝着随机方向生长。这种方法可以加快随机树到达目标点的速度。
    RRT缺点
    难以在有狭窄通道的环境找到路径,因为狭窄通道的面积小,被碰到的概率较低

    RRT Connect
    RRT Connect算法从初始状态点和目标状态点同时生长两棵RRT搜索状态空间。与原始RRT算法相比,该算法在目标点建立了第二棵树,在每次迭代中,开始步骤与原始RRT相同,通过采样随机点进行扩展,在扩展完第一棵树的新节点 q n e w q_{new} qnew后,以 q n e w q_new qnew为第二棵树的扩展方向,第二棵树的扩展方式略有不同,该算法首先会扩展第一步得到 q n e w 1 q^1_{new} qnew1,若没有产生碰撞,继续向相同的方向扩展第二步,知道扩展失败或者 q n e w 1 q_{new}^1 qnew1= q n e w q_{new} qnew(表示与第一棵树相连),算法结束。

    每次迭代中考虑两棵树的平衡性,即两棵树节点的多少,交换次序小的树进行扩展。
    在这里插入图片描述

    算法代码地址:RRT

    3. RRT*

    RRT算法不能保证得到的可行路径是相对优化的, R R T ∗ RRT^{*} RRT算法针对路径优化问题进行了改进。 R R T ∗ RRT^{*} RRT的主要特征是快速找到初始路径,之后随着采样点的增加,不断进行优化直到找到目标点或者设定的最大循环次数。 R R T ∗ RRT^{*} RRT算法的收敛时间更长,但计算出路径的代价比RRT算法有所降低,二者的区别主要在于针对新节点 x n e w x_{new} xnew的重计算过程:

    • x n e w x_{new} xnew选择父节点的过程与RRT算法相比多了rewire操作
    • 重布线随机树

    3.1重新选择父节点
    在这里插入图片描述

    上图表示随机树在扩展过程中的一个时刻,节点上的标号为节点的顺序,节点9为新产生的节点 x n e w x_{new} xnew,节点6为产生节点9的 x n e a r x_{near} xnear(图中标错了),节点之间连接的边上数字表示两个节点之间的欧氏距离,代表路径代价。

    重新选择父节点的过程中,以节点9为圆心,按照事先规定的半径找到圆内的近邻,包括节点4、5、8。原有的0-4-6-9的路径代价和为10+5+1=16;备选的三个节点中,节点5与节点9组成的路径为0-1-5-9,代价和为3+5+3=11;
    节点8与节点9组成的路径为0-1-5-8-9,代价和为3+5+1+3=12;节点4与节点9的路径为0-4-9,代价和为10+4=14。

    可以看出,如果将节点5作为节点9的新的父节点,路径代价相对最小,因此将节点9的父节点由原来的节点4修改为节点5,生成随机树如下图:
    在这里插入图片描述

    3.2 重新布线随机树
    x n e w x_{new} xnew重新选择父节点后,对随机树进行重新布线可以使得随机树节点间的代价尽量小,即若邻近节点的父节点修改为 x n e w x_{new} xnew可以减小路径代价,就对其进行更改。

    如下图所示,节点9为新生成的节点 x n e w x_{new} xnew,节点4、6、8为其邻近节点,他们的父节点分别为节点0、4、5,构成的路径分别为0-4、0-4-6、0-1-5-8,代价分别为10、15和9。
    在这里插入图片描述
    当修改节点4的父节点为节点9时,到达节点为0-1-5-9-4,代价为3+5+3+4大于原来的路径代价10,因此不需要改变节点4的父节点;同理修改节点8的父节点,路径代价将变为14,大于原来的路径代价9,因此不需要修改节点8的父节点;对于节点6,修改其父节点之后,代价为12,小于原来的路径代价,因此将节点6的父节点修改为节点9。由此重新布线后的随机树如下图所示:
    在这里插入图片描述
    R R T ∗ RRT^{*} RRT的核心在于上述的重新选择父节点和重新布线两个过程,重新选择父节点可以使得新生成的节点路径代价尽可能小、重新布线可以使得生成新节点后的随机树减少冗余通路和路径代价,二者相辅相成。

    避障策略
    对于圆形障碍物,圆形边界的判断属于非线性问题,一般情况下将其转化为多边形进行非线性处理,只需要判断新生成节点 x n e w x_{new} xnew的坐标是否是在圆内,如果其位于圆形障碍物的外接正方形内,判断碰撞产生。假设圆形障碍物圆心坐标为(x0,y0),半径为r,考虑到移动机器人的尺寸,对障碍物进行膨化处理,膨化尺寸为inf,碰撞条件可以表述为:
    x 0 − r − i n f < x n e w , x < x 0 + r + i n f x_0-r-inf<x_{new,x}<x_0+r+inf x0rinf<xnew,x<x0+r+inf
    y 0 − r − i n f < y n e w , y < y 0 + r + i n f y_0-r-inf<y_{new,y}<y_0+r+inf y0rinf<ynew,y<y0+r+inf

    实际仿真环境中障碍物主要选择为长方形,其碰撞机制需要满足 x n e a r x_{near} xnear x n e w x_{new} xnew的连线不能与矩形相交,若二者均位于矩形任意一条边的同侧, 必不相交;若二者分别位于矩形某条边的两侧,则需要进行第二步判断。

    若分别位于不同册,有两种情况:

    • x n e w x_{new} xnew位于矩形内部,则二者必定相交
    • 二者均位于矩形外部,需要利用直线与矩形的性质

    在这里插入图片描述
    如上图所示,二者均位于AD边两侧且位于矩形外侧,两点连线与矩形相交, D x n e a r Dx_{near} Dxnear A x n e a r Ax_{near} Axnear为两个边界,当 x n e a r x_{near} xnear x n e w x_{new} xnew的连线位于 D x n e a r Dx_{near} Dxnear A x n e a r Ax_{near} Axnear两条直线的下方时,二者与矩形相交,反正不相交;对于AD边而言,必定发生碰撞的过程可以表述为:
    k x n e a r x n e w < k D x n e a r k_{x_{near}x_{new}}<k_{Dx_{near}} kxnearxnew<kDxnear && k x n e a r x n e w > k A x n e a r k_{x_{near}x_{new}}>k_{Ax_{near}} kxnearxnew>kAxnear

    其中k表示直线斜率。

    碰撞检测的逻辑如下:
    对矩形的一条边设定布尔值b,当b=1时表示发生碰撞;定义判断函数judge(M,N,P),其中:
    M = ( x 1 , y 1 ) , N = ( x 2 , y 2 ) , P = ( x , y ) M=(x_1,y_1),N=(x_2,y_2),P=(x,y) M=(x1,y1),N=(x2,y2),P=(x,y)
    j u d g e ( M , N , P ) = ( ( y − y 1 ) ( x 2 − x 1 ) > ( y 2 − y 1 ) ( x − x 1 ) ) judge(M,N,P)=((y-y_1)(x_2-x_1)>(y_2-y_1)(x-x_1)) judge(M,N,P)=((yy1)(x2x1)>(y2y1)(xx1))
    judge函数为布尔函数,等式右边为真时值为1,否则为0。对于每一条边,其判断逻辑为:
    b = ( j u d g e ( x n e a r , V e r t e x 1 , V e r t e x 2 ) ! = j u d g e ( x n e w , V e r t e x 1 , V e r t e x 2 ) ) b=(judge(x_{near},Vertex_1,Vertex_2)!=judge(x_{new},Vertex_1,Vertex_2)) b=(judge(xnear,Vertex1,Vertex2)!=judge(xnew,Vertex1,Vertex2)) and ( j u d g e ( x n e a r , x n e w , V e r t e x 1 ) ! = j u d g e ( x n e a r , x n e w , V e r t e x 2 ) ) (judge(x_{near},x_{new},Vertex_1)!=judge(x_{near},x_{new},Vertex_2)) (judge(xnear,xnew,Vertex1)!=judge(xnear,xnew,Vertex2))
    V e r t e x i Vertex_i Vertexi表示矩形障碍物某一条边的两个定点。

    针对前图案例,布尔函数为:
    b 1 = ( j u d g e ( x n e a r , A , D ) ! = j u d g e ( x n e w , A , D ) ) b_1=(judge(x_{near},A,D)!=judge(x_{new},A,D)) b1=(judge(xnear,A,D)!=judge(xnew,A,D)) and b = ( j u d g e ( x n e a r , x n e w , A ) ! = j u d g e ( x n e a r , x n e w , D ) ) b=(judge(x_{near},x_{new},A)!=judge(x_{near},x_{new},D)) b=(judge(xnear,xnew,A)!=judge(xnear,xnew,D))

    判断逻辑左半部分:
    j u d g e ( x n e a r , A , D ) = ( ( y D − y 1 ) ( x A − x 1 ) > ( y A − y 1 ) ( x A − x 1 ) ) = 0 judge(x_{near},A,D)=((y_D-y_1)(x_A-x_1)>(y_A-y_1)(x_A-x_1))=0 judge(xnear,A,D)=((yDy1)(xAx1)>(yAy1)(xAx1))=0
    j u d g e ( x n e w , A , D ) = ( ( y D − y 1 ) ( x 2 − x 1 ) > ( y 2 − y 1 ) ( x D − x 1 ) ) = 1 judge(x_{new},A,D)=((y_D-y_1)(x_2-x_1)>(y_2-y_1)(x_D-x_1))=1 judge(xnew,A,D)=((yDy1)(x2x1)>(y2y1)(xDx1))=1

    因此 ( j u d g e ( x n e a r , A , D ) ! = j u d g e ( x n e w , A , D ) ) = 1 (judge(x_{near},A,D)!=judge(x_{new},A,D))=1 (judge(xnear,A,D)!=judge(xnew,A,D))=1,表示 x n e a r x_{near} xnear x n e w x_{new} xnew位于AD边两侧;

    判断逻辑右半部分:
    j u d g e ( x n e a r , x n e w , A ) = ( ( y A − y 1 ) ( x 2 − x 1 ) > ( y 2 − y 1 ) ( x A − x 1 ) ) = 0 judge(x_{near},x_{new},A)=((y_A-y_1)(x_2-x_1)>(y_2-y_1)(x_A-x_1))=0 judge(xnear,xnew,A)=((yAy1)(x2x1)>(y2y1)(xAx1))=0
    j u d g e ( x n e a r , x n e w , D ) = ( ( y D − y 1 ) ( x 2 − x 1 ) > ( y 2 − y 1 ) ( x D − x 1 ) ) = 0 judge(x_{near},x_{new},D)=((y_D-y_1)(x_2-x_1)>(y_2-y_1)(x_D-x_1))=0 judge(xnear,xnew,D)=((yDy1)(x2x1)>(y2y1)(xDx1))=0
    所以 ( j u d g e ( x n e a r , x n e w , A ) ! = j u d g e ( x n e a r , x n e w , D ) ) = 0 (judge(x_{near},x_{new},A)!=judge(x_{near},x_{new},D))=0 (judge(xnear,xnew,A)!=judge(xnear,xnew,D))=0;因此b1=0
    综上所述, x n e a r x_{near} xnear x n e w x_{new} xnew连线不会与矩形这一条边相交,因此不会发生碰撞。

    其他边的判断方法相同,当且仅当
    ( b 1 = 0 b1=0 b1=0 and b 2 = 0 b2=0 b2=0 and b 3 = 0 b3=0 b3=0 and b 4 = 0 b4=0 b4=0)=1时,表明 x n e a r x_{near} xnear x n e w x_{new} xnew与矩形整体不相交。

    4.RRT*算法的改进

    4.1 Informed RRT*
    R R T ∗ RRT^* RRT提高规划路径质量的同时,消耗了更长的时间,在保证规划的路径质量的同时,缩短搜索时间是一个探索方向, I n f o r m e d − R R T ∗ Informed-RRT^* InformedRRT是一个可行的方向。
    在这里插入图片描述
    如上图最左侧所示, R R T ∗ RRT* RRT在得到一条可行的路径后,将采样空间限制在一个椭圆形的区域中,并且随着路径长度的不断缩短,逐渐缩小该椭圆形的区域, I n f o r m e d − R R T ∗ Informed-RRT^* InformedRRT就是对这个椭圆形区域直接采样的方法。算法伪代码如下所示:
    在这里插入图片描述

    伪代码中标红的部分是在 R R T ∗ RRT^* RRT基础上进行修改的地方,主要修改内容在第7行采样部分,采样函数的伪代码如下:
    在这里插入图片描述

    采样具体过程如下图所示:
    在这里插入图片描述
    I n f o r m e d − R R T ∗ Informed-RRT^* InformedRRT将目前已经搜索到的最短路径作为 c b e s t c_{best} cbest,起点和终点的距离作为 c m i n c_{min} cmin,以此构建椭圆,当没有规划结果时, c b e s t c_{best} cbest为inf;椭圆中采样涉及到均匀采样方法,是首先在一个单位大小的n维超平面进行采样,然后再映射到超椭球中,具体采样方式见原始论文Informed RRT*

    4.2 Cross-entropy motion planning
    该算法主要对RRT*采样方式进行了改进,算法思想如下:
    在这里插入图片描述
    如图所示,首先生成路径,然后在路径的节点附近进行多高斯模型采样,得到更多的路径
    在这里插入图片描述
    得到更多路径后,算法在更多路径的节点做均值,对均值后的节点重新采样,继续得到更多路径;如此反复。

    5.状态栅格搜索器(State Lattice Search)

    前述的几种路径搜索算法规划的路径往往没有考虑到机器人的动力学模型,不符合机器人的动力学约束。状态栅格搜索对被控对象进行了运动学模型分析,在此基础上进行路径规划。算法具体思想及理论见下篇State Lattice Search

    参考
    [1] https://zhuanlan.zhihu.com/p/65673502
    [2] https://www.cnblogs.com/21207-iHome/p/7210543.html
    [3] https://zhuanlan.zhihu.com/p/51087819
    [4] https://blog.csdn.net/weixin_43890711/article/details/107516647

    展开全文
  • 之前的文章都是基于搜索的路径算法,这两天在又学习了一下基于采样路径规划算法,这里做一下记录,最后会奉上大神的链接 基于采样路径规划算法大致可以分为综合查询方法和单一查询方法两种。 前者首先构建路线...

    之前的文章都是基于搜索的路径算法,这两天在又学习了一下基于采样的路径规划算法,这里做一下记录,最后会奉上大神的链接

    基于采样的路径规划算法大致可以分为综合查询方法和单一查询方法两种。

    1. 前者首先构建路线图,先通过采样和碰撞检测建立完整的无向图,以得到构型空间的完整连接属性,再通过图搜索即可得到可行的路径。
    2. 后者则从特定的初始构型出发局部建立路线图,在构型空间中延伸树型数据结构,最终使它们相连。

    其中,综合查询方法的代表性的方法就是概率路线图(Probabilistic Roadmap,PRM),单一查询方法则以快速扩展随机树算法(Rapidly-exploring Random Tree)。下面将对这两种方式都做一个详细的描述。

    概率路线图PRM

    PRM主要分为两个步骤:

    1. 第一步,预处理,讲需要规划的区域处理成一个满足要求的无向图。
    2. 第二步,搜索,采用图搜索的方式对无向图进行搜索,如果能找到一条从起始点到终点的路径,则说明存在可行的路径。

    预处理的步骤

    1. 初始化。设 G ( V , E ) G(V,E) G(V,E)为一个无向图,其中顶点集 V V V代表无碰撞的构型,连线集 E E E代表无碰撞路径。初始状态为空。
    2. 构型采样。从构型空间中采样一个无碰撞的点 α ( i ) \alpha(i) α(i)并加入到顶点集 V V V中。
    3. 领域计算。定义距离 ρ \rho ρ,对于已经存在于顶点集 V V V中的点,如果它与 α ( i ) \alpha(i) α(i)的距离小于 ρ \rho ρ,则将其称作点 α ( i ) \alpha(i) α(i)的邻域点。
    4. 边线连接。将点 α ( i ) \alpha(i) α(i)与其领域点相连,生成连线 τ \tau τ
    5. 碰撞检测。检测连线 τ \tau τ是否与障碍物发生碰撞,如果无碰撞,则将其加入到连线集 E E E中。
    6. 结束条件。当所有采样点(满足采样数量要求)均已完成上述步骤后结束,否则重复2-5。

    搜索

    采用图搜索的方式对无向图 G G G进行搜索,如果能找到一条从起始点到终点的路线,就说明有可行的路径。

    下面结合示意图对整个算法就行说明:

    1. 初始化,首先构造地图,如下图所示,图片中,黑色区域为障碍物,白色表示可行驶区间,
      在这里插入图片描述

    2. 构型采样,在图中随机采样一定数量的点,并且去掉其中与障碍物有干涉的点,图中最左上角和右下角的点为规划的起点和终点。
      在这里插入图片描述

    3. 邻域计算,边线连接与碰撞检测。对每一个点,取其领域内(需要设置邻域的范围,比如点半径为)的所有点进行连线,对连线进行碰撞检测,去掉经过障碍物的连线,将结果存放在邻接矩阵中。
      在这里插入图片描述

    4. 使用选择的搜索算法,一般来说选择A*,对上图进行搜索,找到从左上到右下的最短路径,即为目标路径。
      在这里插入图片描述

    分析

    采样数量的影响

    显然,对同一地图,采样点的数量越多,找到合理路径以及更优路径的概率就越大。但同时,采样点数量越多,计算与搜索时间也会更长。

    如果只设置10个采样点,邻域200。可以看到,并不能找到可行路径,这也说明了抽样规划算法存在的完备性弱的问题。
    在这里插入图片描述而若设置较多采样点,虽然都能找到可行路径,但耗时却会增加很多。

    邻域设置的影响

    邻域的设置影响着连线的建立与检测。当邻域设置过小,由于连线路径太少,可能找不到解;当领域设置太大,会检测太多较远的点之间的连线,而增加耗时。

    如果邻域设置为100,找不到解;设置为1000,耗时4.470s,耗时较长,
    在这里插入图片描述在这里插入图片描述

    快速随机扩展树(RRT)

    RRT算法与PRM算法十分类似,都是通过抽样来在已知的地图上建立无向图,进而通过搜索方法寻找相对最优的路径。不同的地方在于,PRM算法在一开始就通过抽样在地图上构建出完整的无向图,再进行图搜索;而RRT算法则是从某个点出发一边搜索,一边抽样并建图。

    与PRM算法相同,RRT算法也是概率完备的:只要路径存在,且规划的时间足够长,就一定能确保找到一条路径解。注意“且规划的时间足够长”这一前提条件,说明了如果规划器的参数设置不合理(如搜索次数限制太少、采样点过少等),就可能找不到解。

    算法说明

    我们可以把RRT算法比较形象地看做“树型算法”。它从一个起始构型(对于二维图,就是一个点)出发,不断延伸树型数据,最终与目标点相连。先放一张规划的结果可能更加便于理解:

    在这里插入图片描述

    算法步骤

    1. 初始化,和PRM一样都需要先建一张地图,
      在这里插入图片描述
    2. 随机采样

    我们已经确定了规划的起始点,按道理它需要不断地向着目标点进行生长。但需要注意的是,由于存在障碍物,如果我们让树型一味朝着目标点延伸,则可能会因为“撞墙”而失败。因此,我们采取了一种随机采样方法:在每次选择生长方向时,有一定的概率会向着目标点延伸,也有一定的概率会随机在地图内选择一个方向延伸一段距离,关键代码如下:

    # 利用rand()函数在[0,1]区间内随机生成一个数
    if np.random.rand() < 0.5:
        # 如果小于0.5,则在图的范围内随机采样一个点
        sample = np.mat(np.random.randint(0, img_binary.shape[0] - 1, (1, 2)))
    else:
        # 否则用目标点作为采样点
        sample = self.point_goal
    

    我们每一步让RRT树有0.5的概率直接采样终点向目标点前进,有0.5的概率向地图内任意方向前进。

    在这里插入图片描述

    1. 生长点选择与碰撞检测

    从上图可以看到,由于每次生长都存在一定的随机性,因此RRT树会逐渐出现许多分支,那么每一步中我们该如何选择要延伸哪个分支呢?这里我们直接选择RRT树中离采样点最近的点,并向其延伸。

    假设我们采样了空间中随机一个点,接下来从现有的RRT树中选择离采样点最近的一个点,并向采样点延伸一段距离。假如在这段延伸中没有发生碰撞(碰撞检测),而且新点与现有的所有点的距离大于某个判断阈值(防止生长到RRT已经探索过的位置),则将这个新点也加入RRT树。
    个人觉得这一步,和A*的代价方程的意义是一样的,要保证总体向着目标移动。
    在这里插入图片描述

    1. 终止条件

    由于我们每次延伸的距离是固定的,所以并不能保证最后一次延伸能够刚好到达终点的位置,更可能的情况是在终点周围来回跳动。因此我们设定一个阈值,假如本次延伸的新点与终点的距离小于这个阈值,我们就认为已经规划成功。

    下面是随机采样概率0.5,步长20,采样上限20000次的结果
    在这里插入图片描述

    分析

    由于RRT算法是概率完备的,预设参数可能对规划结果造成不同的影响,其中最主要的两个参数就是随机采样的概率和生长步长。

    随机采样概率

    我们每一次采样,都有一定概率朝着任意方向走,或朝着终点走。这个概率显然会影响搜索效果。给人最直接的感觉是,随机采样的概率越大,RRT树的分支也就越多,反之则难以发生新的分支。下面我们修改随机采样概率来看看效果。

    设随机采样的概率为0.01,采样上限20000次。可以看到,直到达到采样上限也没有成功找到解。这是因为RRT产生分支的概率太小,经历了许多次碰撞才能凭借分支绕过障碍物。
    在这里插入图片描述设随机采样的概率为1.0,采样上限20000次。可以看到,虽然规划得以成功,但由于生长缺乏方向性,其实是一种“碰运气”式的搜索。RRT树的分支填充了所有空间直至找到目标点。这样的搜索会消耗大量的时间。
    在这里插入图片描述

    生长步长

    我们每一次RRT树的延伸,都有一个固定的步长。这个步长的设置显然也会影响树的形状。当步长太大时,可能由于太过笨拙而无法成功绕过障碍物;当步长过小时,生长的速度显然会有所减慢(因为同样的距离要生长更多次)。一般来说,空间越复杂,步长越小。这里必须注意的是,生长步长一定要比判断是否为同一个采样点的阈值要大。

    步长10,采样上限20000次。可以看到,采样点极其密集,消耗的时间更长。
    在这里插入图片描述步长200,采样上限20000次。没有搜索到最终结果,可以看到,由于步长太大,生长点在障碍物与终点之间来回跳动,始终不能满足碰撞检测或终止条件的要求。
    在这里插入图片描述

    最后,奉上大神的链接

    https://zhuanlan.zhihu.com/p/66047152
    https://zhuanlan.zhihu.com/p/65673502
    https://www.cnblogs.com/21207-iHome/p/7210543.html

    展开全文
  • Dijkstra、Astar 和动态规划的基于采样的移动机器人路径规划算法在这个存储库中,我们简要介绍了 Dijkstra、Astar 和动态规划方法的完整源代码,以在 2D 图上找到从起始节点到结束节点的最佳路径。 我们还提供了在...
  • 提出一种基于文化算法框架的萤火虫...考察路径采样点数、种群数量和进化迭代次数等参数变化对收敛性能的影响,并将所提出算法与PSO和ACO等进化计算算法进行性能比较,验证了算法更容易搜索到全局最优解,有更好的收敛性能.
  • 路径规划算法

    千次阅读 多人点赞 2021-11-14 10:20:44
    人工势场法二、基于采样路径规划算法1.PRM算法2.RRT算法三、智能仿生算法1.神经网络算法2.蚁群算法3.遗传算法 前言 随着机器人技术、智能控制技术、硬件传感器的发展,机器人在工业生产、军事国防以及日常生活等...


    前言

    随着机器人技术、智能控制技术、硬件传感器的发展,机器人在工业生产、军事国防以及日常生活等领域得到了广泛的应用。而作为机器人行业的重要研究领域之一,移动机器人行业近年来也到了迅速的发展。移动机器人中的路径规划便是重要的研究方向。移动机器人的路径规划方法主要分为传统的路径规划算法、基于采样的路径规划算法、智能仿生算法。传统的路径规划算法主要有A算法、Dijkstra算法、D算法、人工势场法,基于采样的路径规划算法有PRM算法、RRT算法,智能仿生路径规划算法有神经网络算法、蚁群算法、遗传算法等。
    图1 路径规划算法


    一、传统路径规划算法

    1.Dijkstra算法

    Dijkstra算法是Edsger Wybe Dijkstra在1956年提出的一种用来寻找图形中结点之间最短路径的算法。Dijkstra算法的基本思想是贪心思想,主要特点是以起始点为中心向外层层扩展,直到扩展到目标点为止。Dijkstra算法在扩展的过程中,都是取出未访问结点中距离该点距离最小的结点,然后利用该结点去更新其他结点的距离值。
    Dijkstra算法流程:

    1.	将初始点s放入到集合S中,初始时集合S中只有s;
    2.	无自环的初始点s到自己的最短路径为0;
    3.	For(目标点ei不在集合S中):
    4.	   计算集合S中以外的所有结点到集合S中结点的最短距离,即从s出发,到达所有结点且只允许通过初始点s的最短路径。如果没有直达的通路,那么就设置为无穷,意味着暂时到达不了的结点。
    5.	While(集合V-S不是空集):
    6.	   选出第一次for循环之后在集合V-S中,且相对于集合S的最短路径中距离最短的目标点ej。
    7.	   将该目标点ej并入到集合S中。
    8.	   将目标点ej并入集合S之后会对V-S以外的顶点相对于集合S的最短路径长度产生影响。
    9.	   For(更新S中的结点路径)
    10.	      If(dist[s,ej]+wj,i < dist[s,ei]11.	         dist[s,ei] = dist[s,ej]+wj,i
    

    注:该算法不允许图中存在负权边。
    图2 Dijkstra算法
    优点:
    1) 如果最优路径存在,那么一定能找到最优路径
    缺点:
    1) 有权图中可能是负边
    2) 扩展的结点很多,效率低

    2.A*算法

    A算法发表于1968年,A算法是将Dijkstra算法与广度优先搜索算法(BFS)二者结合而成,通过借助启发式函数的作用,能够使该算法能够更快的找到最优路径。A算法是静态路网中求解最短路径最有效的直接搜索方法。
    A
    算法的启发式函数:f(n)=g(n)+h(n)
    f(n)表示结点的综合优先级,在选择结点时考虑该结点的综合优先级;
    g(n)表示起始点到当前结点的代价值;
    h(n)表示当前结点到目标点的代价估计值,启发式函数。
    当h(n)趋近于0时,此时算法退化为Dijkstra算法,路径一定能找到,但速度比较慢;当g(n)趋近于0时,算法退化为BFS算法,不能保证一定找到路径,但速度特别快。我们可以通过调节h(n)的大小来调整算法的精度与速度。
    在A*算法中,采用最多的是欧几里得距离,即dist = srqt((y2-y1)2+(x2-x1)2)。

    A*算法流程:

    1.	将起始点s加入到开启列表openlist中
    2.	重复以下过程:
    a)	遍历开启列表openlist,寻找F值最小的结点,并将其作为当前要处理的结点
    b)	将要处理的结点移到关闭列表closelist
    c)	对当前结点的8个相邻结点的每个结点:
    i.	如果他是不可抵达的或者已经在关闭列表closelist中,忽略; 
    ii.	如果他不在开启列表openlist中,将其加入openlist,并把当前结点设置为其父节点,记录当前结点的F、G、H值;
    iii.	如果他已经在开启列表openlist中,检查这条路径(即经由当前结点到达相邻结点)是否更好,用G值做参考,更小的G值表示这个更好的路径,如果是这样,将其父节点设置为当前结点,并重新计算他的G值和F值,如果开启列表openlist是按F值进行排序,改变后需要重新排序。
    d)	停止,当
    i.	终点加入到了开启列表openlist中,此时路径已经找到
    ii.	查找重点失败,并且开启列表openlist中是空的,此时没有路径
    3.	保存路径,从终点开始,每个结点沿着其父节点移动直到起点。
    

    图3 A*算法环境及路径点

    图4 A*算法规划路径
    图5 平滑处理后的路径
    优点:
    1) 利用启发式函数,搜索范围小,提高了搜索效率
    2) 如果最优路径存在,那么一定能找到最优路径
    缺点:
    1) A算法不适用于动态环境
    2) A
    算法不太适合于高维空间,计算量大
    3) 目标点不可达时会造成大量性能消耗

    3.D*算法

    D算法是卡耐基梅隆机器人中心的Stentz在1994年提出的主要用于机器人探路,并且美国火星探测器上就是应用的D路径规划算法。A算法适用于在静态路网中寻路,在环境变化后,往往需要replan,由于A不能有效利用上次计算的信息,故计算效率较低。D算法由于储存了空间中每个点到终点的最短路径信息,故在重规划时效率大大提升。A是正向搜索,而D特点是反向搜索,即从目标点开始搜索过程。在初次遍历时候,与Dijkstra算法一致,它将每个节点的信息都保存下来。
    D*算法流程:

    1.	先用Dijkstra算法从目标节点G向起始节点搜索。储存路网中目标点到各个节点的最短路和该位置到目标点的实际值h,k(k为所有变化h之中最小的值,当前为k=h)原OPEN和CLOSE中节点信息保存。
    2.	机器人沿最短路开始移动,在移动的下一节点没有变化时,无需计算,利用上一步Dijkstra计算出的最短路信息从出发点向后追述即可,当在Y点探测到下一节点X状态发生改变,如堵塞。机器人首先调整自己在当前位置Y到目标点G的实际值h(Y),h(Y)=X到Y的新权值C(X,Y)+X的原实际值h(X)。X为下一节点(到目标点方向Y->X->G),Y是当前点。K值去h值变化前后的最小。
    3.	用A*或其他算法计算,这里假设用A*算法,遍历Y的子节点,放入CLOSE,调整Y的子节点a的h值,h(a)=h(Y)+Y到子节点a的权重C(Y,a),比较a点是否存在于OPEN和CLOSE中,方法如下:用A*或者其他算法计算,这里假设用A*算法,遍历Y的子节点,放入CLOSE,调整Y的子节点a的h值,h(a)=h(Y)+Y到子节点a的权重C(Y,a),比较a点是否存在于OPEN和CLOSE中,方法如下:
    while()
    {
    从OPEN表中取k值最小的节点Y;
    遍历Y的子节点a,计算a的h值 h(a)=h(Y)+Y到子节点a的权重C(Y,a)
    {
        if(a in OPEN)     比较两个a的h值 
        if( a的h值小于OPEN表a的h值 )
        {
    更新OPEN表中a的h值;k值取最小的h值
              有未受影响的最短路经存在
              break; 
        }
        if(a in CLOSE) 比较两个a的h值 //注意是同一个节点的两个不同路径的估价值
        if( a的h值小于CLOSE表的h值 )
        {
    更新CLOSE表中a的h值; k值取最小的h值;将a节点放入OPEN表
           有未受影响的最短路经存在
           break;
        }
        if(a not in both)
            将a插入OPEN表中; //还没有排序
    }
    放Y到CLOSE表;
    OPEN表比较k值大小进行排序;
    }
    

    图6 D*算法
    优点:
    1)适用于动态环境的路径规划,搜索效率高
    缺点:
    1) 不适用于高维空间,计算量大
    2) 不太适用于在距离较远的最短路径上发生变化的场景

    4.人工势场法

    人工势场法是由Khatib在1985年提出的一种基于虚拟力场的局部路径规划算法,该算法的基本思想是当机器人在周围环境中运动时,将环境设计成一种抽象的人造引力场,目标点对移动机器人产生“引力”,障碍物对移动机器人产生“斥力”,最后通过二者的合力来控制移动机器人的运动。应用人工势场法规划出来的路径一般是比较平滑并且安全的,但是存在着局部最优的问题。
    利用势场函数U来建立人工势场,势场函数是一种可微函数,空间中某点处势场函数值的大小,代表了该点的势场强度。我们采用引力势场函数与斥力势场函数,用U(q)表示二者之和。
    在这里插入图片描述
    引力势场函数:
    在这里插入图片描述
    e表示引力增益,p(q,qgoal)表示当前点到目标点的距离;
    斥力势场函数:
    在这里插入图片描述
    n表示斥力增益,p(q,qgoal)表示当前点到障碍物的距离,p0表示障碍物作用距离阈值。
    图7 人工势场法
    优点:
    1) 规划出的路径一般是比较平滑且安全
    2) 人工势场法是一种反馈控制策略,具有一定的鲁棒性
    缺点:
    1) 容易陷入局部最优的问题
    2) 距离目标点较远时,引力特别大,斥力相对较小,可能会发生碰撞
    3) 当目标点附近有障碍物时,斥力非常大,引力较小,很难到达目标点

    二、基于采样路径规划算法

    1.PRM算法

    随机路线图(PRM)算法是一种基于图搜索的算法,可以将连续状态空间转换成离散状态空间,在利用A*等搜索算法在路线图上寻找路径,提高搜索效率。PRM算法是概率完备且不是最优的算法。
    PRM算法流程:

    1.	初始化,设G(V,E)为一个无向图,其中顶点集V代表无碰撞的状态点,连线集E代表无碰撞的路径。初始状态为空。
    2.	状态点采样,在状态空间中采样无碰撞的状态点加入到无碰撞的状态点V中。
    3.	邻域计算,定义距离p,对于已经存在于无碰撞的状态点V中的点,如果他与无碰撞的点的距离小于p,则将其称作无碰撞状态点的邻域点。
    4.	边线连接,将无碰撞的状态点与其邻域相连,生成连线。
    5.	碰撞检测,检测连线是否与障碍物发生碰撞,如果无碰撞,则将其加入到连线集E中。
    6.	结束条件,当所有采样点均已完成上述步骤后结束,否则重复2~5。
    7.	最后使用A*算法在路线图上寻找最优路径。
    

    图8 PRM算法
    优点:
    1) 适用于高维空间和复杂约束的路径规划问题
    2) 搜索效率高,搜索速度快
    缺点:
    1)概率完备但不是最优

    2.RRT算法

    RRT算法是适用于高维空间,通过对状态空间中的采样点进行碰撞检测,避免了对空间的建模,较好的处理带有非完整约束的路径规划问题,有效的解决了高维空间和复杂约束的路径规划问题。该算法是概率完备但不是最优的算法。
    RRT算法以初始点qinit作为根节点,通过随机采样增加叶子节点的方式,生成一个随机扩展树,当目标点位于随机扩展树上时,能够找到一天初始点到目标点的路径。首先,需要从状态空间中随机选择一个采样点qrand,然后从随机树中选择一个距离qrand最近的结点qnearest,从qnearest向qrand扩展一个步长的距离,得到一个新的结点qnew,如果qnew与障碍物发生碰撞,则返回空;否则,将qnew加入到随机树中,重复上述步骤直到qnearest和qgoal距离小于一个阈值。
    图9 未通过狭长空间
    图10 通过狭长空间
    优点:
    1) 搜索效率比较高,搜索速度比较快
    2) 适用于高维空间,不会产生维度灾难的问题
    3) 只需对状态空间采样点进行碰撞检测,避免了对空间的建模
    缺点:
    1) 规划出的路径质量一般,可能存在棱角、不够光滑
    2) RRT算法不太适用于存在狭长空间的环境
    3) 规划出的路径可能不是最优路径
    4) 不适用于动态环境的路径规划

    三、智能仿生算法

    1.神经网络算法

    随着机器人自主性的不断提高,使其具有环境感知以及环境学习的能力,许多学者提出了深度强化学习算法来解决移动机器人处于动态复杂环境中路径规划的问题。深度强化学习算法充分利用了深度学习的感知能力与强化学习的决策能力,通过机器人与环境的交互过程不断试错,通过环境评价性的反馈,实现系统更加智能的决策控制,帮助移动机器人在某些复杂未知的环境中实现一定程度的自主化与智能化。
    路径规划就是一个标准的MDP问题,强化学习可以通过值迭代等方法建立一个表格,用以存储状态s到动作a的映射。但是在复杂环境中会产生维度灾难的问题,因此神经网络可以解决维度灾难的问题。以DQN算法为例,来讲解神经网络算法在路径规划中的应用。DQN算法是Q-Learning算法与卷积神经网络(CNN)二者进行结合。DQN算法是由两个网络结构、初始参数相同的结构组成,一个是估计网络,用来输出估计值函数;另一个是目标网络,用来输出目标值函数。通过强化学习使机器人与环境进行交互得到样本(s,a,r,s’),将所有的样本集放入到经验回放池中。神经网络进行训练时,随机的从经验回放池中抽取batchsz数量的样本,将样本输入进神经网络,利用神经网络的非线性拟合能力,拟合出非线性函数来表达我们的Q值,利用e-greedy策略来进行选择智能体的动作。智能体执行完相应的动作之后,环境会反馈一个状态和奖励值,最后经过神经网络模型的训练和优化得到网络的训练参数,得到相对准确的动作输出。
    在这里插入图片描述
    其中w表示估计网络的参数, w-表示目标网络的参数。

    优点:
    1) 适合于动态复杂环境
    2) 适用于高维空间,避免维度灾难的问题
    缺点:
    1) 对硬件的要求比较高
    2) 参数调节比较困难

    2.蚁群算法

    蚁群算法(Ant Colony Optimization,ACO)是Dorigo在1992年提出的一种用来寻找优化路径的概率型算法,是由一群无智能或有轻微智能的个体通过相互写作而表现出智能行为,为求解复杂问题提供了一个新的可能性。该算法的主要思想是蚂蚁在寻找食物过程中发现路径的行为。该算法具有分布计算、信息正反馈和启发式搜索的特征,本质上是进化算法中的一种启发式全局优化算法。
    蚁群算法的基本原理是利用蚂蚁在觅食过程中会释放信息素。

    1.	初始时刻,路径没有任何信息素,蚂蚁会以一定的随机性选择任意方向
    2.	更新信息素矩阵,当有信息素时,蚂蚁会优先选择信息素浓度高的路径
    3.	那么在相同时间内,信息素的浓度与路径长度成反比,越短的路径会有更多的信息素,那么后续的蚂蚁会选择信息素浓度高的路径,最优路径上的信息素浓度会越来越高
    4.	随着时间的推移,信息素会自行挥发
    5.	最终,能选择出一条最优路径即信息素浓度高的路径
    

    影响蚁群算法的因素:
    1) 信息素如何撒播
    2) 信息素如何挥发
    3) 以何种方式让蚂蚁选择运动方向,减少盲目性和不必要性
    4) 给予蚂蚁和环境一定的记忆能力能够帮助减少搜索空间
    个体分布越均匀,种群多样性就越好,得到全局最优解的概率就越大,但是寻优时间就越长;个体分布越集中,种群多样性就越差,不利于发挥算法的探索能力。正反馈加快了蚁群算法的收敛速度,却使算法较早地集中于部分候选解,因此正反馈降低了种群的多样性,也不利于提高算法的全局寻优能力。
    图11 蚁群算法
    优点:
    1) 蚁群算法的鲁棒性强
    2) 采用正反馈机制,能够逼近最优解
    3) 易与其他算法进行结合
    缺点:
    1) 盲目的随机搜索,搜索时间较长,搜索速度慢
    2) 蚁群算法容易陷入局部最优
    3) 蚁群算法参数的选择依赖于经验或试错

    3.遗传算法

    遗传算法(Genetic Algorithm,GA)是由John Holland于20世纪70年代提出,该算法是根据大自然中生物体进化规律而设计提出的,来模拟达尔文生物进化论的自然选择和遗传学机理的生物进化过程的计算模型,是一种通过模拟自然进化过程搜索最优解的方法。其本质是一种高效、并行、全局搜索的方法,能在搜索过程中自动获取和积累有关搜索空间的知识,并自适应地控制搜索过程以求得最佳解。
    图12 遗传算法
    遗传算法的流程:

    1.评估每条染色体所对应个体的适应度
    While(未找到满意的解):
    2.遵照适应度越高,选择概率越大的原则,从种群中选择两个个体作为父方和母方
    3.抽取父母双方的染色体,进行交叉,产生子代
    4.对子代的染色体进行变异
    

    优点:
    1) 遗传算法具有广泛的应用领域
    2) 遗传算法具有群体搜索的特性
    3) 遗传算法基于概率规则,搜索更为灵活
    4) 遗传算法直接以目标函数作为搜索信息,不涉及目标函数值求微分的过程
    缺点:
    1) 遗传算法效率比较低
    2) 遗传算法容易过早收敛
    3) 遗传算法在编码时容易出现不规范不准确的问题


    更多内容请关注微信公众号:深度学习与路径规划

    在这里插入图片描述

    展开全文
  • 基于采样路径规划算法——RRT

    千次阅读 2020-02-24 16:11:15
    基于采样路径规划算法——RRT* ​ 在上一篇博客中简介了基于搜索的路径规划算法A*的原理,这篇博客则会从另外一个角度去解决路径规划的问题。首先我们要从RRT算法说起,然后再探讨其优势和缺点然后给出一些改进的...
  • 与基于图搜索的方法相比,基于采样路径规划算法不需要显式构建整个配置空间和边界。 0.概念 Complete Planner(完备规划器) Probabilistic Complete Planner(概率完备的规划器) Resolution Complete ...
  • 基于采样路径规划算法(PRM,RRT)

    千次阅读 2021-03-19 09:48:08
    总结课程《深蓝学院移动机器人路径规划》 1.概率路图 Probabilistic Road Map 分为学习阶段,查询阶段。 通过采样的方式代替整个2D栅格图,通过一个图结构简化地图。 学习阶段:用一定的采样方式撒点,把落在障碍...
  • 通过维护open表和closed表来表示待扩展的节点与已扩展的节点,从起点开始,根据当前节点的代价计算其相邻节点的代价,根据不同情况选择是否加入open表或更新其父节点与代价,当搜索到终点即反向追溯父节点输出路径。...
  • 运动规划是移动机器人自主导航系统中的重要模块之一,相关算法研究成果层出不穷,本文将空间采样算法拆解为四个子类算法:PRM类算法、RRT类算法、CVM类算法和DWA类算法,并沿时间顺序概述相关算法的发展历程,最后从...
  • 机器人路径规划经典算法

    万次阅读 多人点赞 2019-04-17 08:54:45
    机器人经典路径规划算法 基于图论的路径规划算法 基于图论的路径规划规划算法有BFS,DFS,Dijkstra,A*,D*,D*lite等经典算法。 源代码. ...基于采样路径规划算法 ...基于采样的经典路径规划算法有RRT ,PRM。 ...
  • 路径规划算法总结

    万次阅读 多人点赞 2019-08-07 13:36:27
    广度优先算法实际上已经能够找到最短路径,BFS通过一种从起点开始不断扩散的方式来遍历整个图。可以证明,只要从起点开始的扩散过程能够遍历到终点,那么起点和终点之间一定是连通的,因此他们之间至少存在一条路径...
  • DWA算法可以概括为三步:一是根据机器人自身的限制以及环境制约将速度的采样空间约束在一定范围内; 二是根据机器人运动学对采样后的速度进行模拟得到预轨迹; 三是设定评价函数对预轨迹进行评分以获取最优轨迹对应的...
  • DWA算法一般用于局部路径规划,该算法在速度空间内采样线速度和角速度,并根据机器人的运动学模型预测其下一时间间隔的轨迹。 对待评价轨迹进行评分,从而获得更加安全、平滑的最优局部路径。 一、机器人运动学模型 ...
  • 路径GAN 基于采样路径规划启发式生成对抗网络的Pytorch实现 表中的内容 结构 PathGAN的总体结构由两部分组成: ...基于生成式对抗网络的启发式算法,用于基于采样路径规划(arXiv文章) GAN路径查找器(arXiv文章)
  • RRT路径规划算法

    万次阅读 多人点赞 2019-08-29 21:43:42
    RRT(Rapidly-Exploring Random Tree)算法是一种基于采样路径规划算法,常用于移动机器人路径规划,适合解决高维空间和复杂约束下的路径规划问题。基本思想是以产生随机点的方式通过一个步长向目标点搜索前进,...
  • GMR-RRT*:基于采样的高斯回归策略改进RRT算法路径规划研究
  • 简介 ​ 最近,作者参加了关于RMUS 高校 SimReal挑战赛,首次接触到了机器人导航领域,...​ 机器人导航的路径规划问题主要分为全局路径规划和局部路径规划,这两者是根据对环境信息获取程度划分的。 全局规划通常需
  • 路径规划中经典算法和发展趋势介绍
  • 本文将为大家介绍四种常用的路径规划算法,分别是搜索算法、随机采样、曲线插值和人工势场法。1.搜索算法搜索算法主要包括遍历式和启发式两种,其中Dijkstra算法属于传统的遍历式,A*算法属于启发式,在A*算法的...
  • 在文章路径规划 | 随机采样算法:PRM、RRT、RRT-Connect、RRT*中,介绍了具备渐近最优性的RRT*算法。随着采样点数的增多,RRT*算法的规划结果会逐渐收敛到最优。 但是可以观察到,RRT*算法是对自由空间进行均匀采样...
  • 前言:由于后续可能要做一些无人驾驶相关的项目和实验,所以这段时间学习一些路径规划算法并自己编写了matlab程序进行仿真。开启这个系列是对自己学习内容的一个总结,也希望能够和优秀的前辈们多学习经验。 一、...
  • 路径规划与优化学习系列,人生之路在于不断规划和优化,无人机飞行之路也是如此
  •  上一篇介绍了快速搜索随机树(RRT)算法的原理,这是一种基于采样路径规划算法,在地图尺寸较大时,其效率将显著的优于基于图搜索的路径规划算法(如A*)。  然而,RRT也有其局限性,如: 狭窄通道情况下的...
  • 为了解决这些问题,本文将介绍基于随机采样路径规划算法。这类算法适用于高维度空间,它们以概率完备性(当时间接近无限时一定有解)来代替完备性,从而提高搜索效率。 概率路图算法(Probabilistic Road Map, PRM) ...
  • ROS常用局部路径规划算法比较

    千次阅读 多人点赞 2021-06-07 20:07:39
    本博文主要讨论ROS导航包中集成的局部路径规划算法,DWA、TEB、MPC等算法在使用过程中的各自的优缺点。以下均为自己在使用过程中总结的经验及查阅资料得来,如有理解不到位的...动态窗口法与ROS默认局部路径规划算法T

空空如也

空空如也

1 2 3 4 5 ... 20
收藏数 28,395
精华内容 11,358
关键字:

动态路径规划采样算法