Navigation之global_planner框架学习及参数配置分析

一、全局规划器概述

对于global planner，可以采用以下三种实现之一：

"navfn/NavfnROS", "global_planner/GlobalPlanner", "carrot_planner/CarrotPlanner"

本文分析其中一种实现：global_planner/GlobalPlanner。
move_base调用global_planner需要修改的文件：
1、bgp_plugin.xml
2、package.xml
以上两个文件都在/global_planner文件夹内

二、根据nav_core提供的BaseGlobalPlanner接口：

initialize(name, costmap) ——算法实例的选取
makePlan(start, goal, plan)——两个步骤完成路径的生成（①计算可行点矩阵potential_array (planner_->calculatePotentials) → ②从可行点矩阵中提取路径plan (path_maker_->getPath)）
主要是以下三个实例:
1.计算“一个点”的可行性 —— p_calc_：PotentialCalculator::calculatePotential()、 QuadraticCalculator::calculatePotential()
2.计算“所有”的可行点 —— planner_：DijkstraExpansion::calculatePotentials()、 AStarExpansion::calculatePotentials()
3.从可行点中“提取路径” —— path_maker_：GridPath::getPath()、 GradientPath::getPath()

涉及到四个算法程序：A*, Dijkstra；gradient_path, grid_path

可以总结出global_planner框架:

三、例子 

3.1  举例——A*算法：

bool AStarExpansion::calculatePotentials(unsigned char* costs, double start_x, double start_y, double end_x, double end_y,
int cycles, float* potential) 
{
    queue_.clear();
    int start_i = toIndex(start_x, start_y);

    //1.将起点放入队列
    queue_.push_back(Index(start_i, 0));//push the start point into queue_

    //2.将所有点的potential都设为一个极大值
    std::fill(potential, potential + ns_, POT_HIGH);//initial all the potential as very large value

    //3.将起点的potential设为0
    potential[start_i] = 0;//initial the start position at potential as 0

    int goal_i = toIndex(end_x, end_y);
    int cycle = 0;

    //4.进入循环，继续循环的判断条件为只要队列大小大于0且循环次数小于所有格子数的2倍
    while (queue_.size() > 0 && cycle < cycles) 
    {

        //5.得到最小cost的索引，并删除它，如果索引指向goal(目的地)则退出算法，返回true
        Index top = queue_[0];//get the Index that with mini cost
        std::pop_heap(queue_.begin(), queue_.end(), greater1());//build the heap sort
        queue_.pop_back();//remove the Index with mini cost

        int i = top.i;//the Index's i from (i,cost)
        if (i == goal_i)//stop condition
            return true;

        //6.对前后左右四个点执行add函数
        //add the neighborhood 4 points into the search scope
        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);
    }
    return false;
}

//add函数中，如果是已经添加的的点则忽略，根据costmap的值如果是障碍物的点也忽略。
void AStarExpansion::add(unsigned char* costs, float* potential, float prev_potential, int next_i, int end_x,int end_y)
 {
    if (potential[next_i] < POT_HIGH)//it means the potential cell has been explored
        return;

    if(costs[next_i]>=lethal_cost_ &&
     !(unknown_ && costs[next_i]==costmap_2d::NO_INFORMATION))//it means this cell is obstacle
        return;
    // compute the next_i cell in potential
    // costs[next_i] + neutral_cost_:original cost + extra cost
    //AStarExpansion::calculatePotentials()计算每一个点的potential时，用的是p_calc_实例！！
    potential[next_i] = p_calc_->calculatePotential(potential, costs[next_i] + neutral_cost_, next_i, prev_potential);

    int x = next_i % nx_, y = next_i / nx_;
    float distance = abs(end_x - x) + abs(end_y - y);

    queue_.push_back(Index(next_i, potential[next_i] + distance * neutral_cost_));
    //potential[next_i]：    是起点到当前点的cost即g(n)
    //distance * neutral_cost_：   是当前点到目的点的cost即h(n)。
    //f(n)=g(n)+h(n)：  计算完这两个cost后，加起来即为f(n)，将其存入队列中。

    std::push_heap(queue_.begin(), queue_.end(), greater1());//sort about the previous insert element
    //返回，继续循环
}

A* 算法是策略寻路，不保证一定是最短路径。 Dijkstra 算法是全局遍历，确保运算结果一定是最短路径。 Dijkstra 需要载入全部数据，遍历搜索。 ROS的DijkstraExpansion实现，代码明显不如A*的实现优雅，还使用了宏定义函数。

3.2  举例——grid_path算法：

bool GridPath::getPath(float* potential, double start_x, double start_y, double end_x, double end_y, std::vector<std::pair<float, float> >& path) {
    std::pair<float, float> current;
    current.first = end_x;
    current.second = end_y;

    //1.将goal（目的地）所在的点的(x,y)作为当前点加入path
    int start_index = getIndex(start_x, start_y);
    path.push_back(current);

    int c = 0;
    int ns = xs_ * ys_;

    //2.进入循环，继续循环的条件为当前点的索引不是起始点
    while (getIndex(current.first, current.second) != start_index) {
        float min_val = 1e10;
        int min_x = 0, min_y = 0;

        //3.搜索当前点的四周的四个临近点，选取这四个临近点的potential的值最小的点
        for (int xd = -1; xd <= 1; xd++) {
            for (int yd = -1; yd <= 1; yd++) {
                if (xd == 0 && yd == 0)
                    continue;
                int x = current.first + xd, y = current.second + yd;
                int index = getIndex(x, y);
                if (potential[index] < min_val) {
                    min_val = potential[index];
                    min_x = x;
                    min_y = y;
                }
            }
        }


        if (min_x == 0 && min_y == 0)
            return false;

        //4.将potential值最小的点更改为当前点，并加入path
        current.first = min_x;
        current.second = min_y;
        path.push_back(current);

        if(c++>ns*4){
            return false;
        }

    }//返回2，继续循环

    return true;
}

这个模块负责使用计算好的potential值生成一条全局规划路径。算法很好理解，首先将goal所在的点的(x,y)塞到path，然后搜索当前的点的四周的四个临近点，选取这四个临近点的potential的值最小的点min，然后把这点塞到path，然后再将当前点更新为min这点，由于start 点的potential的值是0，所以这是个梯度下降的方法。

四、全局路径规划器global_planner的参数配置与分析

4.1 简介

nav_core中提供了对应的全局路径规划接口。要想使用机器人move_base提供的便利，我们需要按照接口来实现全局路径规划算法。

global_planner是ros提供的一个全局规划器，是原本的全局规划器navfn的替代品。继承nav_core::BaseGlobalPlanner的接口。

4.2 参数说明

/allow_unknown(bool, default:true)
是否选择探索未知区域。如果和costmap配合使用的时候，其对应参数track_unknown_space必须同样设置为true

/default_tolerance(double, default: 0.0)
靠近目标点的周围多少数值就算到了目标点

/visualize_potential(bool, default: false)
可能区域是否可视化

/use_dijkstra(bool, default: true)
如果设置为false就用A*

/use_quadratic(bool, default:true)
是否使用二阶近似

/use_grid_path(bool, default: false)
输出路径是否遵循网格边界，否则就使用梯度下降法

/old_navfn_behavior(bool, default: false)
如果你想要让global_planner跟之前的navfn版本效果一样，就设true

/lethal_cost(int, default: 253)
障碍物致命区域的代价数值（dynamic reconfigure, 可以动态参数配置）

/neutral_cost (int, default: 50)
中立区域的cost值（动态配置）

/cost_factor(double, default: 3.)
cost因子，用于将每层costmap对应的cost数值乘起来（动态配置）

/publish_potential (bool, default: True)
发布可能的代价地图（动态配置）

/orientation_mode (int, default: 0)
设置每个点的朝向。 (None=0, Forward=1, Interpolate=2, ForwardThenInterpolate=3, Backward=4, Leftward=5, Rightward=6)(动态配置)

/orientation_window_size(int, default 1)
根据方向模式(参数12)指定的位置导数，确定使用哪个窗口来确定方向(动态配置)
 

注意： 所有最后标明（动态配置）的参数，都是使用功能包dynamic_reconfigure实现的参数配置。详情可以查看ROS进阶(二)： dynamic_reconfigure功能包详解_little_miya的博客-CSDN博客_ddynamic_reconfigure

我认为出现这种情况是因为，跟机器人设定的方向orientation_mode有关。

总体来说这个global_planner不好用。全局路径规划还是建议使用默认的navfn插件。

参考文献

移动机器人导航技术笔记（1）-global_planner学习 - 知乎

ROS导航系列(四)：全局路径规划器的参数配置分析_little_miya的博客-CSDN博客_ros 全局规划