A*算法在ROS中3维空间的实现

最新推荐文章于 2024-07-19 17:25:59 发布

m0_60823648

最新推荐文章于 2024-07-19 17:25:59 发布

阅读量1.1k

点赞数

分类专栏：路径规划文章标签：算法

本文链接：https://blog.csdn.net/m0_60823648/article/details/132016868

版权

代码流程

已知起始点
定义启发式函数（加入Tie Breaker效率可能更高）
将起始点加入openSet，计算f值（f=g+h）
定义id=-1为closeSet，id=1为openSet，初始将所有点的id定义为0
开始规划路径
	▲从openSet中取出f值最小的点CurrentNode，并将CurrentNode从openSet中取出放入closeSet
	▲if CurrentNode与目标点一致，说明找到完整路径，结束路径规划进程
	▲得到CurrentNode的相邻点NeighborNode，分别计算CurrentNode到NeighborNode的cost值
	▲遍历所有的NeighborNode
		if id==0 计算f值，加入openSet，设置父系节点
		if id==1 判断当前f值是否小于已知f值，如果小于则更新f值，重新设置父系节点
		if id==-1 在closeSet中不予考虑
结束路径规划后，从目标点开始溯源，按照标记好的父系节点最终得到一条完整路径

部分代码实现

getHeu函数

double AstarPathFinder::getHeu(GridNodePtr node1, GridNodePtr node2)
{
   
    /* 
    choose possible heuristic function you want
    Manhattan, Euclidean, Diagonal, or 0 (Dijkstra)
    Remember tie_breaker learned in lecture, add it here ?
    *
    *
    *
    STEP 1: finish the AstarPathFinder::getHeu , which is the heuristic function
    please write your code below
    *
    *
    */

   //尝试以欧几里得距离作为启发式函数
   double Heu;
   double x1,y1,z1;
   double x2,y2,z2;
   
   x1 = (double)node1->index[0];
   y1 = (double)node1->index[1];
   z1 = (double)node1->index[2];
   x2 = (double)node2->index[0];
   y2 = (double)node2->index[1];
   z2 = (double)node2->index[2];


   Heu = sqrt(pow(x1-x2, 2) + pow(y1-y2, 2) + pow(z1-z2, 2));
//    Tie Breaker 
   Heu = Heu + Heu*0.001;
//    ROS_INFO("point1(%.2f,%.2f,%.2f),point2(%.2f,%.2f,%.2f),Heu = %.2f",x1,y1,z1,x2,y2,z2,Heu);

   return Heu;
}

AstarGetSucc函数

inline void AstarPathFinder::AstarGetSucc(GridNodePtr currentPtr, vector<GridNodePtr> & neighborPtrSets, vector<double> & edgeCostSets)
{
      
    neighborPtrSets.clear();
    edgeCostSets.clear();
    /*
    *
    STEP 4: finish AstarPathFinder::AstarGetSucc yourself 
    please write your code below
    *
    *
    */
    //将currentPtr所有邻居节点写入vector<GridNodePtr> neighborPtrSets;计算对应的vector<double> edgeCostSets;
    if(currentPtr == nullptr)
        ROS_INFO ("Error: Current pointer is null!");

    Vector3i idx = currentPtr->index;
    int current_x = idx[0];
    int current_y = idx[1];
    int current_z = idx[2];
    for(int i = (current_x-1);i <= (current_x + 1