一、全局规划器概述
对于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 全局规划
更多推荐
Navigation之global_planner框架学习及参数配置分析
发布评论