路径规划图解A 、Dijkstra、GBFS算法的异同(附C++ Python Matlab仿真)

最新推荐文章于 2024-05-23 00:22:50 发布

2301_76190672

最新推荐文章于 2024-05-23 00:22:50 发布

阅读量704

点赞数 18

分类专栏：程序员文章标签：算法 c++ python

本文链接：https://blog.csdn.net/2301_76190672/article/details/138391607

版权

程序员专栏收录该内容

146 篇文章 0 订阅

订阅专栏

在栅格地图中，常见的**邻域(neighbor)**模式如下所示，即

8邻域
24邻域
48邻域

栅格的邻域表示了从当前位置出发下一次搜索的集合，例如八邻域法中，当前栅格只能和周围的八个栅格相连形成局部路径。

在这里插入图片描述

下面是一个图搜索问题的例子，可以直观理解什么是搜索问题。

例1：在如下的栅格地图中，设绿色栅格为起点，红色栅格为终点，灰色栅格为障碍，白色栅格为可行点，问如何设计一条由栅格组成的连接起点、终点的路径，并尽可能使路径最短？

接下来，围绕这个问题展开阐述。

在这里插入图片描述

2 贪婪最佳优先搜索

一个朴素的想法是：每一次搜索时就找那些与终点最近的节点，这里衡量最近可以用多种度量方式——曼哈顿距离、欧式距离等。这种方法像一头狼贪婪地望着食物，迫切寻求最近的路径，因此称为贪婪最佳优先搜索(Greedy Best First Search, GBFS)。

假设采用八邻域法，在GBFS思想指导下，在起点的八邻域中就会选择最右侧的节点，如下所示。

在这里插入图片描述
循环地，直到如下所示的节点，因为邻域内有障碍，这些障碍节点不会被候选，所以此时离终点最近的就是下方的方格

在这里插入图片描述

依次类推直至终点

3 Dijkstra算法

Dijkstra算法走向了另一个极端，它完全不考虑扩展节点与终点的关系，而是定义了一个**路径耗散函数

(

)

g(n)

g(n)**，从起点开始，机器人每走一个栅格就会产生一定的代价或耗散，因为Dijkstra算法希望路径最短，所以每次首选那些使路径耗散最小的节点。

依照Dijkstra算法的观点，从起点开始，其八个邻域节点都会被依次探索，因为它们离起点最近，接着再探索这些节点的子节点。

在这里插入图片描述
因此Dijkstra算法会遍历大量的节点，一圈圈地逼近终点

在这里插入图片描述

4 启发式A*搜索

A*算法是非常有效且常用的路径规划算法之一，其是结合Dijsktra算法与GBFS各自优势的启发式搜索算法，其搜索代价评估函数为

(

)

(

)

(

)

f(n)=g(n)+h(n)

其中

(

)

g(n)

g(n)代表路径耗散，是Dijsktra算法分量；

(

)

h(n)

h(n)代表下一个搜索节点与终点的距离，启发式地引导机器人朝着终点拓展，是GBFS算法分量。

在这里插入图片描述
兼具两个算法特点的A*算法既保持完备性，又在一定条件下体现出最优性，被广泛应用于路径规划中。

5 A*、Dijkstra、GBFS算法的异同

特别地

(

)

g\left( n \right) =0

g(n)=0时，启发函数影响占据主导，A*算法退化为GBFS算法——完全不考虑状态空间本身的固有属性，不择手段地追求对目标的趋近，此时算法搜索效率将得到提升，但最优性无法保证；

(

)

h(n)=0

h(n)=0时，路径耗散函数影响占据主导，A*算法退化为Dijsktra算法——无先验信息搜索，此时算法搜索效率下降，但最优性上升。

三个算法的直观比较如下所示

在这里插入图片描述

6 算法仿真与实现

6.1 算法流程

在这里插入图片描述

6.2 ROS C++实现

核心代码如下

std::tuple<bool, std::vector<Node>> AStar::plan(const unsigned char\* costs, const Node& start,
                                                  const Node& goal, std::vector<Node> &expand) {
    // open list
    std::priority_queue<Node, std::vector<Node>, compare_cost> open_list;
    open_list.push(start);

    // closed list
    std::unordered_set<Node, NodeIdAsHash, compare_coordinates> closed_list;

    // expand list
    expand.clear();
    expand.push\_back(start);

    // get all possible motions
    const std::vector<Node> motion = getMotion();

    // main loop
    while (!open_list.empty()) {
      // pop current node from open list
      Node current = open_list.top();
      open_list.pop();
      current.id = this->grid2Index(current.x, current.y);
      
      // current node do not exist in closed list
      if (closed_list.find(current) != closed_list.end())
        continue;

      // goal found
      if (current==goal) {
        closed_list.insert(current);
        return {true, this->\_convertClosedListToPath(closed_list, start, goal)};
      }

      // explore neighbor of current node
      for (const auto& m : motion) {
        Node new_point = current + m;

        // current node do not exist in closed list
        if (closed_list.find(new_point) != closed_list.end())
          continue;

        // explore a new node
        new_point.id = this->grid2Index(new_point.x, new_point.y);
        new_point.pid = current.id;

        // if using dijkstra implementation, do not consider heuristics cost
        if(!this->is_dijkstra_)


现在能在网上找到很多很多的学习资源，有免费的也有收费的，当我拿到1套比较全的学习资源之前，我并没着急去看第1节，我而是去审视这套资源是否值得学习，有时候也会去问一些学长的意见，如果可以之后，我会对这套学习资源做1个学习计划，我的学习计划主要包括规划图和学习进度表。



分享给大家这份我薅到的免费视频资料，质量还不错，大家可以跟着学习

![](https://img-blog.csdnimg.cn/img_convert/21b2604bd33c4b6713f686ddd3fe5aff.png)



**网上学习资料一大堆，但如果学到的知识不成体系，遇到问题时只是浅尝辄止，不再深入研究，那么很难做到真正的技术提升。**

**[需要这份系统化学习资料的朋友，可以戳这里无偿获取](https://bbs.csdn.net/topics/618317507)**

**一个人可以走的很快，但一群人才能走的更远！不论你是正从事IT行业的老鸟或是对IT行业感兴趣的新人，都欢迎加入我们的的圈子（技术交流、学习资源、职场吐槽、大厂内推、面试辅导），让我们一起学习成长！**

2301_76190672

关注

18
点赞
踩
11

收藏

觉得还不错? 一键收藏
1
评论
路径规划图解A 、Dijkstra、GBFS算法的异同(附C++ Python Matlab仿真)

在栅格地图中，常见的**邻域(neighbor)**模式如下所示，即栅格的邻域表示了，例如八邻域法中，当前栅格只能和周围的八个栅格相连形成局部路径。下面是一个图搜索问题的例子，可以直观理解什么是搜索问题。：在如下的栅格地图中，设绿色栅格为起点，红色栅格为终点，灰色栅格为障碍，白色栅格为可行点，问如何设计一条由栅格组成的连接起点、终点的路径，并尽可能使路径最短？接下来，围绕这个问题展开阐述。
复制链接

扫一扫