A*算法笔记及C++实现

本文记录最常见的基于图搜索的A*算法的原理和代码实现效果。由于A*算法是在Dijkstra算法基础上加入了“贪心”的启发式函数，所以会先顺带介绍下Dijkstra算法。

1. Dijkstra算法和A*算法流程

便于理解，先上算法伪代码流程，针对流程逐一介绍

第1步：创建一个优先级队列（也叫 open list），用于存储所有需要被扩展的节点，这个优先级队列中节点以到起始点的路径代价g(n)进行排序；
第2步：讲起始点放入优先级队列中，令起始节点g值为0，图中其他节点g值为无穷大；
第3步：进入循环，进行路径搜索：
（1）如果队列为空，说明未找到可行路径，循环结束；
（2）将g值最小的点n从队列中弹出；
（3）将节点n标记为已扩展（也叫存入closed list）；
（4）如果节点n是目标节点，路径找到，循环结束；
（5）对于节点n周围的每个邻居节点m:
*如果m不在队列中，将m加入队列，并更新g(m)=g(n)+cost(n,m)
*如果m已在队列中且g(m)>g(n)+cost(n,m)，更新g(m)=g(n)+cost(n,m)
结束循环。

可以看出，Dijkstra本质上就是BFS算法，从起始节点开始一层层往外递进寻找目标节点。优点在于必可以找到1条最优路径（若存在），缺点也很明显，即搜索是无方向的，访问了大量的无效节点，造成资源浪费，降低效率。在此基础上，加入与目标节点相关的启发式函数，即成为使搜索具有方向性的A*算法，流程如下

可以看出，与Dijkstra唯一的区别就在于优先级队列中用于节点排序的成本函数变成了f(n)=g(n)+h(n)，即增加了启发式函数h(n)。h(n)表示当前节点到目标节点预估的cost。

2. 启发式函数H类型

启发式函数h(n)表示当前节点到目标节点的预计cost，一般都不考虑障碍物，主要有以下3种估计方法：

（1）欧拉距离
即当前节点与目标节点之间的直线距离
（2）曼哈顿距离
即只能沿网格横纵两个方向移动到达目标节点的最短距离

（3）Diagonal 距离
一般网格地图中，运动对象既可以沿网格横纵方向移动，亦可以沿对角线方向移动，此时到目标节点的最短距离即为Diagonal距离

其中，使用欧拉距离和Diagonal距离作为启发式函数可以找到最优路径，而使用曼哈顿距离作为启发式函数却不能保证找到最优路径。因为只有保证预计的cost小于或等于实际cost，才能寻找到左右路径

如图所示，由于h比实际cost大，有可能找的路径为非最短路径

3. C++代码实现

借鉴深蓝学院《移动机器人运动规划》课程的作业代码结构，重新写了二维平面内的A*算法，并通过调用opencv库展示路径搜索效果。具体代码见：
https://github.com/immune1993/path_planning_algorithm/tree/main/Search_based/Astar
下面仅贴出核心的搜索部分代码和启发式函数计算部分代码：

//Astar搜索函数
bool AstarPathFinder::AstarGraphSearch(Eigen::Vector2d start_pt, Eigen::Vector2d end_pt) {
    //坐标转换为网格索引
    Eigen::Vector2i start_idx= coord2gridIndex(start_pt);
    Eigen::Vector2i end_idx= coord2gridIndex(end_pt);
    goalIdx=end_idx;

    //初始化起始和目标节点
    GridNodePtr startNode=GridNodeMap[start_idx(0)][start_idx(1)];
    GridNodePtr endNode=GridNodeMap[end_idx(0)][end_idx(1)];

    //起始和目标节点绘制
    startNode->drawNode(picture,Eigen::Vector3i (0,0,255));
    endNode->drawNode(picture,Eigen::Vector3i (255,0,0));

    openset.clear();

    //将开始节点放入open set
    startNode->gScore=0;
    startNode->fScore= getHeu(startNode,endNode);
    startNode->id=1;//1代表在openset中，-1代表在closedset中
    openset.insert(std::make_pair(startNode->fScore,startNode));

    //存放周边节点和周边路径cost
    std::vector<GridNodePtr> neighbors;
    std::vector<double> edgecosts;
    //当前节点
    GridNodePtr currentNode;

    while(!openset.empty()){
        //取出最小cost节点放入close set
        currentNode=openset.begin()->second;
        openset.erase(openset.begin());
        currentNode->id=-1;

        if(currentNode->index==goalIdx){
            goalNode=currentNode;
            std::cout<<"Path has been found!"<<std::endl;
            return true;
        }

        //获取当前节点周边节点
        getneighbors(currentNode,neighbors,edgecosts);
        //更新周边节点到openset中
        for(int i=0;i<neighbors.size();i++){
            auto tmpNode=neighbors[i];
            //如果节点不在openset中
            if(tmpNode->id==0){
                tmpNode->drawNode(picture,Eigen::Vector3i(0,255,0),1);
                tmpNode->id=1;
                tmpNode->gScore=currentNode->gScore+edgecosts[i];
                tmpNode->fScore=tmpNode->gScore+ getHeu(tmpNode,endNode);
                tmpNode->father=currentNode;
                openset.insert(std::make_pair(tmpNode->fScore,tmpNode));
            }else if(tmpNode->id==1){//如果节点在openset中
                if(tmpNode->gScore>currentNode->gScore+edgecosts[i]){
                    tmpNode->gScore=currentNode->gScore+edgecosts[i];
                    tmpNode->fScore=tmpNode->gScore+ getHeu(tmpNode,endNode);
                    tmpNode->father=currentNode;
                    for(auto p=openset.begin();p!=openset.end();p++){
                        if(p->second==tmpNode){
                            openset.erase(p);
                            break;
                        }
                    }
                    openset.insert(std::make_pair(tmpNode->fScore,tmpNode));
                }
            }
            cv::imshow("Astar",picture);
            cv::waitKey(1);
        }
    }
    std::cout<<"Path has been not found!"<<std::endl;
    return false;
}

//计算启发式cost函数
double AstarPathFinder::getHeu(GridNodePtr node1, GridNodePtr node2) {
    double h=0;
    //获取坐标差值向量
    auto coord_diff2=node1->coord-node2->coord;
    Eigen::Vector2d coord_diff;
    coord_diff<<abs(coord_diff2[0]),abs(coord_diff2[1]);
    std::vector<double>  diff;
    switch (h_class)
    {
        case 1://Euclidean
        h=sqrt( coord_diff.transpose()*coord_diff);
        break;
        case 2://Manhattan
        h=coord_diff.sum();
        break;
        case 3://Diagonal
        for (int i=0;i<2;i++){diff.push_back(coord_diff[i]);}
        sort(diff.begin(),diff.end());
        h=diff[0]*sqrt(2)+(diff[1]-diff[0]);
        break;
        case 4://Dijkstra
        h=0;
        break;
    }
    if(tie_break){
        double p = 1.0 / 25.0;
        h *= (1.0 + p);
    }
    return h;
}

4. 路径搜索效果演示

4.1 起始与目标节点中间无障碍
h(n)为欧拉距离

h(n)为曼哈顿距离

h(n)为Diagonal距离

h(n)=0,Dijkstra算法

4.2 起始与目标节点中间有障碍
h(n)为欧拉距离

h(n)为曼哈顿距离

h(n)为Diagonal距离

h(n)=0,Dijkstra算法

从演示效果可以看出，Dijkstra算法遍历了所有节点才找到路径，效率最低。欧拉距离和Diagonal距离作为启发函数都能找到最优路径，且欧拉距离找到的路径更接近于起始与目标点之间的直线。而曼哈顿距离作为启发函数在不复杂的环境中也能找到最短路径，且访问节点最少，效率最高。

5. 参考

[1]深蓝学院课程：移动机器人运动规划