GBFS，Dijkstra，A*路径规划比较与C++实例

最新推荐文章于 2024-04-30 10:16:38 发布

NieBP

最新推荐文章于 2024-04-30 10:16:38 发布

阅读量667

点赞数 1

分类专栏：高精度地图文章标签：自动驾驶人工智能

本文链接：https://blog.csdn.net/weixin_43891708/article/details/121469561

版权

高精度地图专栏收录该内容

10 篇文章 11 订阅

订阅专栏

一家之言，仅作分享，如有不合理或需要改进的地方，欢迎各位讨论。

图搜索的基本流程

创建一个容器，一般称为openlist，用来存储将要访问的节点
将起点加入容器开始循环：
---- 弹出：从容器中取出一个节点
---- 扩展：获取该节点周围的节点，将这些节点放入容器

一、贪婪最佳优先算法（Greedy Best First Search, GBFS）

GBFS中采用的openlist是一种优先队列（Priority Queue），在优先队列中元素被赋予了优先级，最高优先级元素优先删除。这里的优先级高低判断在路径规划中就是代价值。
在图搜索算法中，使用代价函数 $f (n)$ 来作为优先级判断的标准， $f (n)$ 越小，优先级越高，反之优先级越低。GBFS作为一种启发式搜索算法，使用启发评估函数 $h (n)$ 来作为代价函数，也就是 $f (n) = h (n)$ ，其中 $h (n)$ 是当前节点到终点的代价，它可以指引搜索算法往终点靠近，主要用欧氏距离(Euclidean Distance)或者曼哈顿距离(Manhattan Distance)来表示。
在这里插入图片描述优点：运行速度快。
缺点：不一定能找到最短路径。问题在于GBFS是基于贪心策略的，它试图向目标移动尽管这不是正确的路径。由于它仅仅考虑到达目标的代价，而忽略了当前已花费的代价，于是尽管路径变得很长，它仍然继续走下去。

二、Dijkstra算法

Dijkstra算法与GBFS算法使用了一样的openlist，但是不同点在于Dijkstra算法的代价函数是 $f (n) = g (n)$ ，其中 $g (n)$ 是从起始点到当前点的代价，它可以确保可以找到距离最短的路径。这里的代价计算不同于上述 $h (n)$ （当前点到终点的曼哈顿距离或欧式距离），而是有权重的，在实际路径规划中，这里的权重可以是每条道路的长度，拓扑中的转向关系，道路属性等等。
优点：能够找到最短路径。
缺点：检查节点数目多，效率低。

三、A*算法

A*算法是将上述两者算法进行了融合，即代价函数是 $f (n) = g (n) + h (n)$ ，其中其中 $g (n)$ 为起点到当前扩展节点的移动代价函数， $h (n)$ 是启发函数，当前节点到目标点的距离代价函数。这样可以同时具有**(1)运行效率高，(2)找到最短路径**两个优点。
两个list：
$o p e n l i s t$ ：存放被检查来寻找最短路径的节点列表。
$c l o s e l i s t$ ：存放不会再被检查的节点列表。
两个代价函数：
$g (n)$ ：从起始点到当前节点的代价，例如所有途径节点的道路长度和，转向权重，道路属性权重等。
$h (n)$ ：从当前节点到终点的代价，例如当前节点与终点的曼哈顿距离或欧式距离。
步骤：
1、从起点S开始，把S作为一个等待检查的节点，放入到 $o p e n l i s t$ 中。
2、寻找S节点的拓出节点，将它们放入到 $o p e n l i s t$ 中，并设置它们的父节点为S。
3、从 $o p e n l i s t$ 中删除起点S，并将其放入到 $c l o s e l i s t$ 中。
4、计算 $o p e n l i s t$ 中每个节点的 $f (n)$ 值。
5、从 $o p e n l i s t$ 中选择 $f (n)$ 值最小的节点A，将其从 $o p e n l i s t$ 中删除，并放入到 $c l o s e l i s t$ 中。
6、检查节点A的拓出节点：
       a、 $c l o s e l i s t$ 中的节点不考虑；
       b、如果节点不在 $o p e n l i s t$ 中，则放入到 $o p e n l i s t$ 中，并计算此节点的 $f (n)$ 值，并设置其父节点为A；
       c、如果某拓出的节点B已经在 $o p e n l i s t$ 中，计算新的路径从S到B（即经过A的路径）
             判断是否需要更新： $g (n)$ 值是否更小
             如果新的 $g (n)$ 值更小，则修改父节点为A，重新计算 $f (n)$ 值， $h (n)$ 值不用改变，因为节点到终点的预计代价是固定不变的；
             如果新的 $g (n)$ 值更大，则说明新的路径代价更高，则值不做改变（ $g (n)$ 值不变也不更新）。
7、继续从openlist中找出 $f (n)$ 值最小的节点，从 $o p e n l i s t$ 中删除，放入到 $c l o s e l i s t$ 中，再继续检查其拓出节点，如此循环
8、结束条件：
       a、当 $o p e n l i s t$ 中出现终点节点E时，说明路径已经找到；
       b、当 $o p e n l i s t$ 中没有了数据，则说明没有合适的路径。

四、C++代码

伪代码

初始化两个空的列表openlist和closelist，将起点加入openlist中，并设置代价值为0。
while(1)
{
    if(openlist != null)
    {
         从openlist中选取代价值最小的节点n.
         1. 如果节点n为终点，则从终点开始逐步追踪parent节点，一直达到起点，返回找到的结果路径，
            跳出循环，算法结束，break;
         2. 如果节点n不是终点，
            2.1 将节点n从openlist中删除，加入到closelist中.
            2.2 遍历节点n的8个相邻无碰撞节点
                2.2.1 如果相邻节点在closelist中，则跳过该节点，计算下一个节点。
                2.2.2 如果相邻节点不在openlist中，则设置该节点父节点为n，通过f(n)计算该节点的
                      代价值，并将该节点放入openlist中。
                2.2.3 如果相邻节点在openlist中，则重新计算g(n)值，判断是否更新：如果更小，则重新计算f(n)；
                      反之，不做更新。 
     }
     else
     {
         break; //不能找到一条最优路径
     }
}

下面是头文件中需要定义的搜索节点结构体与二元堆的类

struct SearchNode
{
    RoadInfo stRoadinfo;

    //cost of Current Node
    int iCost;

    //All estimates of node f(n) = g(n) + h(n)
    int iAllEstimateCost;

    //The local optimum value of node A*->g(n),the actual distance from the starting point to that point
    int iPartOptimalCost;

    //Estimated h(n) Spherical Distance or Manhattan Distance of Node
    int iPartEstmateCost;

    //ForwardSearchNode
    int iForwardRoadid;

    SearchNode() {
        iCost = 0;
        iAllEstimateCost = 0;
        iPartOptimalCost = 0;
        iPartEstmateCost = 0;
        iForwardRoadid = 0;
    }
};

struct CmpSearchNode{
    bool operator()(IN const SearchNode& a, IN const SearchNode& b)
    {
        return a.iAllEstimateCost < b.iAllEstimateCost;
    }
};

class RHeap{
public:
    RHeap()
    {
        std::make_heap(m_pData.begin(),m_pData.end(),CmpSearchNode());
    }

    ~RHeap(){
        m_pData.clear();
        std::vector<SearchNode>().swap(m_pData);
    }

    void push(IN SearchNode& node)
    {
        m_pData.emplace_back(node);
        std::push_heap(m_pData.begin(),m_pData.end(),CmpSearchNode());
        std::sort_heap(m_pData.begin(),m_pData.end(),CmpSearchNode());
    }

    void GetMin(OUT SearchNode& node)
    {
        std::pop_heap(m_pData.begin(),m_pData.end(),CmpSearchNode());
        node = m_pData.back();
        m_pData.pop_back();
        std::sort_heap(m_pData.begin(),m_pData.end(),CmpSearchNode());
    }

    std::size_t GetSize()
    {
        return m_pData.size();
    }

    void sort()
    {
        std::sort_heap(m_pData.begin(),m_pData.end(),CmpSearchNode());
    }

private:
    std::vector<SearchNode> m_pData;
};

下面代码是作者自定义的道路路网数据，其中包括拓扑关系，道路长度，以及转向关系，这些都是影响 $g (n)$ 的代价值。

void Astar_RoutePlan::AStar_Strategy(IN const SearchNode src_node, IN const SearchNode dest_node,
                                     OUT std::vector<int> &retRoadlist)
{
    retRoadlist.clear();
    RHeap openlist;
    std::list<SearchNode> closelist;
    std::map<int, SearchNode> map_node;
    std::map<int, SearchNode> map_nodeclose;

    if(src_node.stRoadinfo.id == dest_node.stRoadinfo.id)
    {
        retRoadlist.emplace_back(src_node.stRoadinfo.id);
        return;
    }

    SearchNode node = src_node;
    node.iForwardRoadid = -1;
    openlist.push(node);
    map_node.insert(std::make_pair(node.stRoadinfo.id, node));

    bool bFound = false;

    SearchNode rp_curNode;

    while(openlist.GetSize())
    {
        // Get the node with min cost f(n).
        openlist.GetMin(rp_curNode);
        std::map<int, SearchNode>::iterator iter_map_node = map_node.find(rp_curNode.stRoadinfo.id);

        if(iter_map_node != map_node.end())
        {
            map_node.erase(iter_map_node);
        }

        // Find Destination
        if(rp_curNode.stRoadinfo.id == dest_node.stRoadinfo.id)
        {
            bFound = true;
        }

        if(rp_curNode.iForwardRoadid == dest_node.stRoadinfo.id)
        {
            bFound = true;
        }

        // Get BackWard Adjacent
        std::vector<SearchNode> adjacentNodelist;
        getBackwardAdjacentNodes(rp_curNode,adjacentNodelist);

        closelist.emplace_back(rp_curNode);
        map_nodeclose.insert(std::make_pair(rp_curNode.stRoadinfo.id, rp_curNode));

        for(SearchNode tmpAdjacentNode:adjacentNodelist)
        {
            tmpAdjacentNode.iPartEstmateCost = HeuristicCost(tmpAdjacentNode,dest_node);
            tmpAdjacentNode.iPartOptimalCost = rp_curNode.iPartOptimalCost + tmpAdjacentNode.iCost;
            tmpAdjacentNode.iAllEstimateCost = tmpAdjacentNode.iPartEstmateCost + tmpAdjacentNode.iPartOptimalCost;

            if(tmpAdjacentNode.iForwardRoadid == dest_node.stRoadinfo.id)
            {
                bFound = true;
            }

            iter_map_node = map_node.find(tmpAdjacentNode.stRoadinfo.id);
            if(iter_map_node != map_node.end())
            {
                if(tmpAdjacentNode.iAllEstimateCost < iter_map_node->second.iAllEstimateCost)
                {
                    iter_map_node->second.iAllEstimateCost = tmpAdjacentNode.iAllEstimateCost;
                    iter_map_node->second.iPartEstmateCost = tmpAdjacentNode.iPartEstmateCost;
                    iter_map_node->second.iPartOptimalCost = tmpAdjacentNode.iPartOptimalCost;
                    iter_map_node->second.iForwardRoadid   = tmpAdjacentNode.iForwardRoadid;
                    openlist.sort();
                }
            }
            else if(map_nodeclose.find(tmpAdjacentNode.stRoadinfo.id)!= map_nodeclose.end())
            {
                iter_map_node->second.iAllEstimateCost = tmpAdjacentNode.iAllEstimateCost;
                iter_map_node->second.iPartEstmateCost = tmpAdjacentNode.iPartEstmateCost;
                iter_map_node->second.iPartOptimalCost = tmpAdjacentNode.iPartOptimalCost;
                iter_map_node->second.iForwardRoadid   = tmpAdjacentNode.iForwardRoadid;
            }
            else
            {
                openlist.push(tmpAdjacentNode);
                map_node.insert(std::make_pair(tmpAdjacentNode.stRoadinfo.id, tmpAdjacentNode));
            }

        }
        if(bFound || closelist.size() > 500)  //Calc time >500 then break
        {
            break;
        }
    }

    if(bFound)
    {
        while(rp_curNode.iForwardRoadid != -1)
        {
            retRoadlist.emplace_back(rp_curNode.stRoadinfo.id);
            std::map<int, SearchNode>::iterator iter_nodeclose = map_nodeclose.find(rp_curNode.iForwardRoadid);
            rp_curNode = iter_nodeclose->second;
        }
        retRoadlist.emplace_back(rp_curNode.stRoadinfo.id);
        reverse(retRoadlist.begin(), retRoadlist.end());
    }
}

void Astar_RoutePlan::getBackwardAdjacentNodes(IN const SearchNode cur_node, OUT std::vector<SearchNode> &retAdjacentNodes)
{
    retAdjacentNodes.clear();
    std::multimap< int, std::pair<int,TurnInfo> >  topoinfo;
    mParse.GetTopoInfo(topoinfo);

    std::multimap< int, std::pair<int,TurnInfo> >::iterator iter_adjacent = topoinfo.find(cur_node.stRoadinfo.id);

    if(iter_adjacent == topoinfo.end())
    {
        return;
    }

    for(size_t i=0; i<topoinfo.count(cur_node.stRoadinfo.id); i++, iter_adjacent++)
    {
        RoadInfo adjacentRoadinfo = mParse.GetRoad(iter_adjacent->second.first);
        SearchNode adjacentNode;
        if(adjacentRoadinfo.length > 0)
        {
            adjacentNode.stRoadinfo = adjacentRoadinfo;

            adjacentNode.iCost = adjacentNode.stRoadinfo.length;

            if(iter_adjacent->second.second == TURN_LEFT)
            {
                adjacentNode.iCost += 40;
            }
            else if(iter_adjacent->second.second == TURN_RIGHT)
            {
                adjacentNode.iCost += 20;
            }
            else if(iter_adjacent->second.second == GO_STRAIGHT)
            {
                adjacentNode.iCost += 0;
            }
            double dx = adjacentNode.stRoadinfo.startPoint.x - cur_node.stRoadinfo.endPoint.x;
            double dy = adjacentNode.stRoadinfo.startPoint.y - cur_node.stRoadinfo.endPoint.y;
            double abs = std::hypot(dx,dy);
            adjacentNode.iCost += abs;
            adjacentNode.iForwardRoadid = cur_node.stRoadinfo.id;
            retAdjacentNodes.emplace_back(adjacentNode);
        }
    }
}

double Astar_RoutePlan::HeuristicCost(IN const SearchNode node, IN const SearchNode dest_node)
{
    return std::fabs(node.stRoadinfo.startPoint.x - dest_node.stRoadinfo.endPoint.x)
            + fabs(node.stRoadinfo.startPoint.y - dest_node.stRoadinfo.endPoint.y);
}

NieBP

关注

1
点赞
踩
7

收藏

觉得还不错? 一键收藏
0
评论
GBFS，Dijkstra，A*路径规划比较与C++实例

图搜索的基本流程创建一个容器，一般称为openlist，用来存储将要访问的节点将起点加入容器开始循环：---- 弹出：从容器中取出一个节点---- 扩展：获取该节点周围的节点，将这些节点放入容器一、贪婪最佳优先算法（Greedy Best First Search, GBFS）GBFS中采用的openlist是一种优先队列（Priority Queue），在优先队列中元素被赋予了优先级，最高优先级元素优先删除。这里的优先级高低判断在路径规划中就是代价值。在图搜索算法中，使用代价函数f(n)
复制链接

扫一扫