A*算法在ROS中3维空间的实现_a*算法解决三维空间

代码流程

已知起始点
定义启发式函数（加入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中不予考虑
结束路径规划后，从目标点开始溯源，按照标记好的父系节点最终得到一条完整路径
1
2
3
4
5
6
7
8
9
10
11
12
13

部分代码实现

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;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35

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); i++){
        for(int j = (current_y - 1);j <= (current_y + 1); j++){
            for(int k = (current_z - 1);k <= (current_z + 1); k++){
                //currentPtr不算入neighborPtrSets
                if(i == current_x && j ==current_y && k == current_z)
                {
                    continue;
                }
                //防止x,y,z索引越界
                if(i < 0 || i  > (GLX_SIZE - 1) || j < 0 || j > (GLY_SIZE - 1) || k < 0 || k > (GLZ_SIZE - 1))
                {
                    continue;
                }
                //周围栅格若存在障碍物则跳过
                Vector3i tmpIdx(i,j,k);
                if(isOccupied(tmpIdx))
                {
                    continue;
                }
                //若存在closelist中
                if(GridNodeMap[i][j][k]->id == -1)
                {
                    continue;
                }
                neighborPtrSets.push_back(GridNodeMap[i][j][k]);    
                edgeCostSets.push_back(sqrt(pow(i-idx(0),2)+pow(j-idx(1),2)+pow(k-idx(2),2)));
                Vector3d test = gridIndex2coord(tmpIdx);
                GridNodeMap[i][j][k]->coord = test;
            }
        }
    }
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51

AstarGraphSearch函数

void AstarPathFinder::AstarGraphSearch(Vector3d start_pt, Vector3d end_pt)
{  
    ros::Time time_1 = ros::Time::now();    
    //index of start_point and end_point
    ROS_INFO("start_pt_z = %.2f",start_pt(2));
    Vector3i start_idx = coord2gridIndex(start_pt);
    Vector3i end_idx   = coord2gridIndex(end_pt);
    ROS_INFO("goal_idx(%d,%d,%d)",end_idx[0],end_idx[1],end_idx[2]);
    goalIdx = end_idx;

    start_pt = gridIndex2coord(start_idx);
    end_pt   = gridIndex2coord(end_idx);

    //Initialize the pointers of struct GridNode which represent start node and goal node
    GridNodePtr startPtr = new GridNode(start_idx, start_pt); //结构体指针建立
    GridNodePtr endPtr   = new GridNode(end_idx,   end_pt);

    //openSet is the open_list implemented through multimap in STL library
    openSet.clear();
    // currentPtr represents the node with lowest f(n) in the open_list
    GridNodePtr currentPtr  = NULL;//open_list中f（n）最小的节点指针为currentPtr
    GridNodePtr neighborPtr = NULL;//open_list中f（n）最小的节点相邻节点的结构体指针为neighborPtr

    //put start node in open set
    startPtr -> gScore = 0;
    startPtr -> fScore = getHeu(startPtr,endPtr);   
    //STEP 1: finish the AstarPathFinder::getHeu , which is the heuristic function
    startPtr -> id = 1; 
    startPtr -> coord = start_pt;
    openSet.insert( make_pair(startPtr -> fScore, startPtr) );
    //ROS_INFO("fScore = %.2f",startPtr -> fScore);
    /*
    *
    STEP 2 :  some else preparatory works which should be done before while loop
    please write your code below
    *
    *
    */
   //Step2中不知道该写啥，最后也实现了规划路径的效果
    vector<GridNodePtr> neighborPtrSets;
    vector<double> edgeCostSets;
    // this is the main loop
    while ( !openSet.empty() ){
        /*
        *
        *
        step 3: Remove the node with lowest cost function from open set to closed set
        please write your code below
        
        IMPORTANT NOTE!!!
        This part you should use the C++ STL: multimap, more details can be find in Homework description
        *
        *
        */
       //弹出一个代价最小的节点作为currentPtr加入close list
       currentPtr = openSet.begin()->second;//注意
       openSet.erase(openSet.begin());
       currentPtr->id = -1; //closeset
       ROS_INFO("currentPtr_idx(%d,%d,%d)",currentPtr->index[0],currentPtr->index[1],currentPtr->index[2]);

        // if the current node is the goal 
        if( currentPtr->index == goalIdx ){
            ros::Time time_2 = ros::Time::now();
            terminatePtr = currentPtr;
            ROS_WARN("[A*]{sucess}  Time in A*  is %f ms, path cost if %f m", (time_2 - time_1).toSec() * 1000.0, currentPtr->gScore * resolution );            
            return;
        }
        //get the succetion
        ROS_INFO("start count neighborcost and edgecost");
        AstarGetSucc(currentPtr, neighborPtrSets, edgeCostSets);  //STEP 4: finish AstarPathFinder::AstarGetSucc yourself

        /*
        *
        *
        STEP 5:  For all unexpanded neigbors "m" of node "n", please finish this for loop
        please write your code below
        *        
        */
        //进行邻居节点的遍历         
        for(int i = 0; i < (int)neighborPtrSets.size(); i++){
            /*
            *
            *
            Judge if the neigbors have been expanded
            please write your code below
            
            IMPORTANT NOTE!!!
            neighborPtrSets[i]->id = -1 : unexpanded 关列表
            neighborPtrSets[i]->id = 1 : expanded, equal to this node is in close set 开列表
            *        
            */
            ROS_INFO("neighborPtrSets[%d]->id = %d ",i,neighborPtrSets[i] -> id);
            if(neighborPtrSets[i] -> id == 0){ //discover a new node, which is not in the closed set and open set 不在开列表也不在关列表
                /*
                *
                *
                STEP 6:  As for a new node, do what you need do ,and then put neighbor in open set and record it
                please write your code below
                *        
                */
                //ROS_INFO("[node] neighborid = 0");
                neighborPtrSets[i]->id = 1;
                neighborPtrSets[i]->gScore = currentPtr->gScore +(double)edgeCostSets[i];
                neighborPtrSets[i]->fScore = neighborPtrSets[i]->gScore + getHeu(neighborPtrSets[i],endPtr);
                neighborPtrSets[i]->cameFrom = currentPtr;
                openSet.insert( make_pair(neighborPtrSets[i] -> fScore, neighborPtrSets[i]) );
                continue;
            }   
               /*
                *
                STEP 7:  As for a node in open set, update it , maintain the openset ,and then put neighbor in open set and record it
                please write your code below
                *        
                */
            if(neighborPtrSets[i] -> id == 1 )//this node is in open set and need to judge if it needs to update, the "0" should be deleted when you are coding
            {
                if((neighborPtrSets[i]->gScore )>( currentPtr->gScore +edgeCostSets[i]))
                { 
                    auto ptr = openSet.equal_range(neighborPtrSets[i]->fScore);
                    neighborPtrSets[i]->gScore = currentPtr->gScore + edgeCostSets[i];
                    neighborPtrSets[i]->fScore = neighborPtrSets[i]->gScore + getHeu(neighborPtrSets[i],endPtr);
                    neighborPtrSets[i]->cameFrom = currentPtr;
                }
                continue;
            }
            //this node is in closed set
            else{
                continue;
            }
        }      
    }
    //if search fails
    ros::Time time_2 = ros::Time::now();
    ROS_INFO("get time_2");
    if((time_2 - time_1).toSec() > 0.1)
    ROS_WARN("Time consume in Astar path finding is %f", (time_2 - time_1).toSec() );
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138

getPath函数

vector<Vector3d> AstarPathFinder::getPath() 
{   
    vector<Vector3d> path;
    vector<GridNodePtr> gridPath;
    GridNodePtr currentNodePtr = terminatePtr;
    /*
    *
    *
    STEP 8:  trace back from the curretnt nodePtr to get all nodes along the path
    please write your code below
    *      
    */
    while (currentNodePtr != nullptr) {
        gridPath.push_back(currentNodePtr);
        currentNodePtr = currentNodePtr->cameFrom;
    }
    for (auto ptr: gridPath) //遍历容器中所有的元素
        path.push_back(ptr->coord);
    reverse(path.begin(),path.end());

    return path;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22

代码运行

1. 将启动rviz的节点写入demo.launch中，比较方便运行

<node pkg="rviz" type="rviz" name="rviz" args="-d $(find grid_path_searcher)/launch/rviz_config/demo.rviz" output="screen" />
1

2. ctrl+alt+t打开终端，运行demo.launch文件

roslaunch grid_path_searcher demo.launch
1

3. rviz中3D nav goal插件设置目标点，鼠标左键按下先设置x，y坐标位置以及方向，同时按下右键上下拖动设置z坐标位置
4. 结果如图
   
   整个栅格地图大小长50宽50高10，图中坐标轴O点位于（25，25，0），起点为（25，25，5），从该点开始的那条绿色较连贯的线为规划出的路线
   

其他

Diagonal 函数三维情况的推导