Navigation中A*算法源码解释

A*算法原理

A*算法原理在之前的文章中也有提到，这次主要就是和Navigation中对应起来。
A*算法原理

A*源码

示例图
nx_ = 6, ny_ = 6;
其中A为起始点，(1,2)。B为goal_i 为(4,4)。

calculatePotential 函数是对堆中F值最小的索引值取出，然后进行下一次的更新。

其中toIndex函数返回值为 start_x + nx_*start_y; 即索引

	queue_.clear(); //首先将堆清空
	//x,y点是全部数据的第几个，示例中即13。
    int start_i = toIndex(start_x, start_y);  
    //首先将start点以及其代价压入，即（13，0） 
    queue_.push_back(Index(start_i, 0));
	//potential，填充个数是ns_，值是POT_HIGH很大的一个值1e10.
    std::fill(potential, potential + ns_, POT_HIGH);  
    potential[start_i] = 0;//将起始点的代价设置为0
	//得到目标点的index即28。
    int goal_i = toIndex(end_x, end_y);
    int cycle = 0; //一个计数器，在dadu某个值就不再计算potentail

接下来就是计算potential，然后根据代价值大小进行排序。

	while (queue_.size() > 0 && cycle < cycles) {
        //将最小的代价的index取出给i，并且判断是否为目标点，若是return true。
        Index top = queue_[0];
        //进行小顶堆的排序。即有小到大排序。然后弹出并且删除第一个
        //对应于将这个点从open_list拿到close_list
        std::pop_heap(queue_.begin(), queue_.end(), greater1()); 
        queue_.pop_back();

        int i = top.i; //
        if (i == goal_i) //已经到达目标点
            return true;

        //只检测上下左右四个点.
        add(costs, potential, potential[i], i + 1, end_x, end_y);
        add(costs, potential, potential[i], i - 1, end_x, end_y);
        add(costs, potential, potential[i], i + nx_, end_x, end_y);
        add(costs, potential, potential[i], i - nx_, end_x, end_y);

        cycle++;
    }

add函数中首先是三个if的判断原因在注释中。

	//溢出
	if (next_i < 0 || next_i >= ns_) 
        return;
	// 其实只要不是POT_HIGH就行，表示之前已经检索过这个栅格
    if (potential[next_i] < POT_HIGH) 
        return;
	//有障碍或者是无信息的点
    if(costs[next_i]>=lethal_cost_ && !(unknown_ && costs[next_i]==costmap_2d::NO_INFORMATION)) 
        return;

接下来是计算potential值

 potential[next_i] = p_calc_->calculatePotential(potential, costs[next_i] + neutral_cost_, next_i, prev_potential);

calculate_potential函数如下：

		virtual float calculatePotential(float* potential, unsigned char cost, int n, float prev_potential=-1){
            if(prev_potential < 0){
                // get min of neighbors
                float min_h = std::min( potential[n - 1], potential[n + 1] ),
                      min_v = std::min( potential[n - nx_], potential[n + nx_]);
                prev_potential = std::min(min_h, min_v);
            }

            return prev_potential + cost;
        }

在不小于0的时候，返回的是prev_potential + cost,以start_cost为例，最开始给其赋值是0，因此返回就是cost，可以理解为到下一个栅格的距离，或者是分辨率。因此potential就是A*算法中 F = G + H中的G值。
在计算完potential后开始计算H值，即distance,利用的就是移动多少格子，只能上下左右移动

	// 得到在栅格地图中的（x,y）
	int x = next_i % nx_, y = next_i / nx_;  
    // F = G+H 中的H
    float distance = abs(end_x - x) + abs(end_y - y);  

还是以刚开始的图为例，A的i值是13，则add中的next_i分别是12，14，7，19。以14为例，则x = 2, y = 2。而B为(4,4)。因此distance = 2+2 =4; 由于这只是格子的个数，还有乘上每个格子的真实距离或者是分辨率，所以最后的H = distance *neutral_cost_。因此最后的F = potential[next_i] + distance *neutral_cost_

//将next_i以及对应的cost压入
queue_.push_back(Index(next_i, potential[next_i] + distance * neutral_cost_)); 

最后再进行排序，小顶堆。

 std::push_heap(queue_.begin(), queue_.end(), greater1()); 
 //堆排序，生成小顶堆。（数据结构还不熟悉）

在calculatepotential中对start_i周围的四个点进行add之后，进入下一次循环，这次循环的起始点是上一个点中四个点周围中最小的点。
然后依次进行，直到搜索到goal_i点。此时的potential赋值情况如下，阴影表示已经赋新值，空白是之前最开始的POT_HIGH。（以A的下一个点是14算，其实19也是，但是只弹出一个最小值的Index）。1/4表示的是potential为1，distance为4。

加入getPath函数

此时A*算法就完成了。
在得到路径方面，假如采用的grid_path。(gradient的插补还不太懂)则其就是检测goal_i周围8个点中最小的Potential。然后将此点加入Path,并且开始下一次计算。因此最后得到的轨迹如下图：

参考博客

ROS navigation planner–GlobalPlanner