A star算法

1.A star算法流程

A star是一种启发式算法，用于在图像中或者网格中寻找从起点到终点的最短路径，以下是A star算法的流程

1.1 AstarSearch()

1. 初始化 ：

        维护

        一个开集( OpenList) :存储待遍历的点

        一个闭集(CloseList)：存储已经遍历过的点

2.将起点以及它的fn值加入到OpenList 中

3.从OpenList中弹出一个最小的fn值的节点，作为当前遍历的节点

4.扩展该节点：生产该节点的所有邻居的节点

        判断： a.如果该邻居节点是终点，则结束搜索

                    b.如果该邻居节点不在OpenList中 ， 将其加入到OpenList中，并且计算它的fn值

                    c.如果该节点已经在OpenList或者CloseList中，检查当前路径是否最优（即具有更小 的fn值）

                                c1.如果当前最优，则更新节点的信息

                                c2.否则的话，保持节点的信息不变

5.将当前的节点加入到CloseList中。

6.重复3-5直到查到终点或者OpenList为空

7.若开放集为空但仍未找到终点，算法失败，无可行路径

1.2 getPath()

如果A star找到了终点 ，我们回溯找到起点得到最短的路径

1.从终点开始反向回溯：

      a. 将终点作为当前节点

       b.获取当前节点的父节点（cameFrom 指针）

        c.将当前节点加入路径中

        d.将父节点作为当前节点 继续回溯 直到回溯到起点

2.反向回溯后，我们得到了终点到起点的路径，再反转一下得到起点到终点的路径

2.代码：AstarSearch()

bool AStar::AstarSearch(const double step_size, Vector3d start_pt, Vector3d end_pt)
{
    ros::Time time_1 = ros::Time::now(); //为了判断开始搜索的时间
    ++rounds_; //一个标志位 用于判断是否搜索成功

    step_size_ = step_size; //步长 0.1 
    inv_step_size_ = 1 / step_size;
    center_ = (start_pt + end_pt) / 2;

    Vector3i start_idx, end_idx; //起点和终点的索引向量 三维

    /*将欧式空间坐标转换为grid_map索引值   并且修正起点和终点位置*/
    if (!ConvertToIndexAndAdjustStartEndPoints(start_pt, end_pt, start_idx, end_idx))
    {
        ROS_ERROR("Unable to handle the initial or end point, force return!");
        return false;
    }

    // if ( start_pt(0) > -1 && start_pt(0) < 0 )
    //     cout << "start_pt=" << start_pt.transpose() << " end_pt=" << end_pt.transpose() << endl;

    //初始化起点和终点的节点（指针）   GridNodePtr 在头文件中定义为 GridNode的三重指针 这里通过索引值找到对应的节点
    GridNodePtr startPtr = GridNodeMap_[start_idx(0)][start_idx(1)][start_idx(2)];
    GridNodePtr endPtr = GridNodeMap_[end_idx(0)][end_idx(1)][end_idx(2)];

    std::priority_queue, NodeComparator> empty;
    /*这段代码定义了一个名为empty的空优先队列

        1.数据类型： GridNodePtr 即 GridNode的三重指针
        2.使用 std::vector 作为底层的容器来存储数据
        3.使用 NodeComparator 作为比较器，用于比较两个GridNodePtr的大小

        4.std::priority_queue 是一个容器适配器，它提供了常数时间复杂度下的
                最大元素查找，对元素的插入和移动操作都是对数时间复杂度。在这个队列中，元素会被按优先级排序，队列中最优先的元素位于其顶部。
        这里，NodeComparator 中的 operator() 函数定义了优先级的比较方式。在给定的代码中，NodeComparator 的 operator() 函数是这样定义的：

    bool operator()(GridNodePtr node1, GridNodePtr node2)
      {
              return node1->fScore > node2->fScore; //从堆底到堆顶 降序排列  也就是小的优先级高 优先级队列
      }
它比较了两个节点的 fScore 属性，根据 fScore 的大小来确定节点在优先队列中的优先级。此处，fScore 值较小的节点会被视为具有更高的优先级。*/





    openSet_.swap(empty);//清空openSet_ 优先队列

    GridNodePtr neighborPtr = NULL;
    GridNodePtr current = NULL;//current 代表当前openSet_中的f(n)最小的节点

    startPtr->index = start_idx;
    startPtr->rounds = rounds_;
    startPtr->gScore = 0;
    startPtr->fScore = getHeu(startPtr, endPtr); //获得启发式函数值 采用对角函数 
    startPtr->state = GridNode::OPENSET; //把初始节点放入openset中
    startPtr->cameFrom = NULL;//此时父节点为空
    openSet_.push(startPtr); //把初始节点放入openset中  优先队列

    endPtr->index = end_idx;//终点节点的索引值

    double tentative_gScore; //临时的g(n)值

    int num_iter = 0;
    /*---------------------开始进入主循环---------------------------------------------------*/
    while (!openSet_.empty()) /*---------------------------------首先判断openSet_是否为空-------------------------------------------*/
    {
        num_iter++;
        current = openSet_.top();//访问最小的f(n)cost节点
        openSet_.pop(); //把访问过的节点从openSet_中删除

        // if ( num_iter < 10000 )
        //     cout << "current=" << current->index.transpose() << endl;

        /*判断是否到达终点*/
        if (current->index(0) == endPtr->index(0) && current->index(1) == endPtr->index(1) && current->index(2) == endPtr->index(2))
        {
            // ros::Time time_2 = ros::Time::now();
            // printf("\033[34mA star iter:%d, time:%.3f\033[0m\n",num_iter, (time_2 - time_1).toSec()*1000);
            // if((time_2 - time_1).toSec() > 0.1)
            //     ROS_WARN("Time consume in A star path finding is %f", (time_2 - time_1).toSec() );
            
            //如果到达终点 返回true 和  路径
            gridPath_ = retrievePath(current);
            return true;
        }

        /*如果不是终点 我们继续  我们把当前的着一个节点放入闭集中去 */
        current->state = GridNode::CLOSEDSET; //move current node from open set to closed set.
        /*八连通搜索3*3*3 */
        for (int dx = -1; dx <= 1; dx++)
            for (int dy = -1; dy <= 1; dy++)
                for (int dz = -1; dz <= 1; dz++)
                {
                    if (dx == 0 && dy == 0 && dz == 0)
                        continue; //搜索到自己时 终止当前循环 开启下一次的循环 也就是说不搜索自己

                    Vector3i neighborIdx;
                    neighborIdx(0) = (current->index)(0) + dx;
                    neighborIdx(1) = (current->index)(1) + dy;
                    neighborIdx(2) = (current->index)(2) + dz;
                    /*超出map大小跳出循环 */
                    if (neighborIdx(0) < 1 || neighborIdx(0) >= POOL_SIZE_(0) - 1 || neighborIdx(1) < 1 || neighborIdx(1) >= POOL_SIZE_(1) - 1 || neighborIdx(2) < 1 || neighborIdx(2) >= POOL_SIZE_(2) - 1)
                    {
                        continue;
                    }
                    /*初始化 拓展邻近节点*/
                    neighborPtr = GridNodeMap_[neighborIdx(0)][neighborIdx(1)][neighborIdx(2)];
                    neighborPtr->index = neighborIdx;

                    bool flag_explored = neighborPtr->rounds == rounds_;
                    /*判断拓展的节点是否在闭集中 如果已经在了 则跳出循环*/
                    if (flag_explored && neighborPtr->state == GridNode::CLOSEDSET)
                    {
                        continue; //in closed set.
                    }

                    neighborPtr->rounds = rounds_;
                    /*检查拓展的邻近节点索引是否在障碍物中 如果在 则跳出循环*/
                    if (checkOccupancy(Index2Coord(neighborPtr->index)))
                    {
                        continue;
                    }

                    double static_cost = sqrt(dx * dx + dy * dy + dz * dz); //搜索过程中产生的代价
                    tentative_gScore = current->gScore + static_cost; 
                    /*如果是第一次搜索 发现新的节点（不再闭集合中 加入到开集合中 */
                    if (!flag_explored)
                    {
                        //discover a new node
                        neighborPtr->state = GridNode::OPENSET;
                        neighborPtr->cameFrom = current;//设置邻近节点的父节点为当前节点
                        neighborPtr->gScore = tentative_gScore;
                        neighborPtr->fScore = tentative_gScore + getHeu(neighborPtr, endPtr);
                        openSet_.push(neighborPtr); //put neighbor in open set and record it.
                    }
                    /*此时openset 已经加入到了拓展邻近节点 同时也在搜索新的邻近节点 更新cost 寻找最小的cost 内循环完成后继续从openset中提取最小的节点cost开始搜索*/
                    else if (tentative_gScore < neighborPtr->gScore)
                    { //in open set and need update
                        neighborPtr->cameFrom = current;
                        neighborPtr->gScore = tentative_gScore;
                        neighborPtr->fScore = tentative_gScore + getHeu(neighborPtr, endPtr);//更新完成
                    }
                }
        ros::Time time_2 = ros::Time::now();//结束搜索的时间
        if ((time_2 - time_1).toSec() > 0.2) //time_1 是开始搜索的时间 如果超过0.2s 则跳出循环
        {
            ROS_WARN("Failed in A star path searching !!! 0.2 seconds time limit exceeded.");
            return false;
        }
    }

    ros::Time time_2 = ros::Time::now();//

    if ((time_2 - time_1).toSec() > 0.1)//如果超过0.1s 则跳出循环
        ROS_WARN("Time consume in A star path finding is %.3fs, iter=%d", (time_2 - time_1).toSec(), num_iter);

    return false;
}

 3.代码：getPath()

vector AStar::getPath()
{
    vector path;

    for (auto ptr : gridPath_)
        path.push_back(Index2Coord(ptr->index));

    reverse(path.begin(), path.end()); //反转一下
    return path;
    //在 bspline_optimizer.cpp 大概521行 处用到
}

4.为什么在算法中回经常欧式坐标和网格地图索引值相互转换呢？

1.搜索效率优化：A star在搜索时需要对邻居节点进行扩展和判断，这通常需要在网格中进行，从而提升了效率

2.坐标和索引的匹配：实际中我们会以欧式空间给出指定点和终点，而在网格中需要用索引值来表示位置

3.路径构建和可视化： 在找到最优路径后，需要将网格索引值转换为实际的欧式坐标，以便进行路径的可视化，导航等，反过来将欧式坐标转换为地图索引值可以用来在网格地图上进行标记

4.判断避障 ： 在判断节点是否为障碍物，这可能需要在网格地图上进行判断，通过欧式空间转换为地图索引值，可以方便检查节点是否为障碍物

综上所述，欧式空间坐标和网格地图索引值的相互转换使得A*算法可以在不同的表示之间进行无缝切换，从而在搜索、路径规划和路径处理等方面更加灵活和高效。