激光SLAM导航系列(四)全局路径规划
全局路径规划简介
机器人移动到目的地需要在做出具体移动策略之前先进行全局路径规划，ROS的navigation中使用global_planner包提供的一系列全局规划的算法接口（包括A*，Dijkstra）。
在本文中我们主要使用A*算法来进行全局路径规划。
A算法*
A*（A-Star)算法是一种静态路网中求解最短路径最有效的直接搜索方法，也是解决许多搜索问题的有效算法。算法中的距离估算值与实际值越接近，最终搜索速度越快。
公式表示为：
f(n)=g(n)+h(n)
其中 f(n) 是从初始状态经由状态n到目标状态的代价估计，g(n) 是在状态空间中从初始状态到状态n的实际代价，h(n) 是从状态n到目标状态的最佳路径的估计代价。对于路径搜索问题，状态就是图中的节点，代价就是距离。
h(n)的选取：
保证找到最短路径（最优解的）条件，关键在于估价函数f(n)的选取（或者说h(n)的选取）。
我们以d(n)表达状态n到目标状态的距离，那么h(n)的选取大致有如下三种情况：
（1）如果h(n)<= d(n)到目标状态的实际距离，这种情况下，搜索的点数多，搜索范围大，效率低。但能得到最优解。
（2）如果h(n)=d(n)，即距离估计h(n)等于最短距离，那么搜索将严格沿着最短路径进行， 此时的搜索效率是最高的。
（3）如果 h(n)>d(n),搜索的点数少，搜索范围小，效率高，但不能保证得到最优解。[1]
全局路径规划模块流程
在move_base节点中，通过类加载模块载入了BaseGlobalPlanner（全局路径规划）的实例，首先这个进行了初始化的操作，获取了一些设置参数，比如是否使用Dijkstra算法等，开启了一个叫makePlanService的服务，一旦有请求就能调用makePlan以生成全局规划路径。在makePlan中，主要分为三个步骤，也就是计算potential值，提取plan，发布plan。
全局路径规划程序架构
本节主要对全局路径规划中重要的两个模块进行程序架构的分析，分别是计算potential值和提取plan。
（1）计算potential值
将整个地图分为许多点，而每个点对应于一组起点和终点都有一个potential值，其实就是计算A*算法中的f(n)，该模块主要步骤如下：
①将起点放入队列
queue_.push_back(Index(start_i, 0))②将所有点的potential都设为一个极大值
std::fill(potential, potential + ns_, POT_HIGH)③将起点的potential设为0
potential[start_i] = 0④进入循环，继续循环的判断条件为只要队列大小大于0且循环次数小于所有格子数的2倍
while (queue_.size() > 0 && cycle < cycles)⑤得到最小cost的索引，并删除它，如果索引指向goal(目的地)则退出算法，返回true
Index top = queue_[0];
std::pop_heap(queue_.begin(), queue_.end(), greater1());
queue_.pop_back();
int i = top.i;
if (i == goal_i)
return true;⑥对前后左右四个点执行add函数
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);add函数中，如果是已经添加的的点则忽略，根据costmap的值如果是障碍物的点也忽略。
potential[next_i]是起点到当前点的cost即g(n),distance * neutral_cost_是当前点到目的点的cost即h(n)。
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);所以计算完这两个cost后，加起来即为f(n)，将其存入队列中。
queue_.push_back(Index(next_i, potential[next_i] + distance * neutral_cost_));返回④继续循环。
（2）提取plan
这个模块负责使用计算好的potential值生成一条全局规划路径。该模块的主要流程如下：
①将goal（目的地）所在的点的(x,y)作为当前点加入path
int start_index = getIndex(start_x, start_y);
path.push_back(current);②进入循环，继续循环的条件为当前点的索引不是起始点
while (getIndex(current.first, current.second) != start_index)③搜索当前点的四周的四个临近点，选取这四个临近点的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;
}
}
}④将potential值最小的点更改为当前点，并加入path
current.first = min_x;
current.second = min_y;
path.push_back(current);返回②，继续循环
至此， 全局规划模块就已经根据起点和终点规划处一条全局路径，再通过最后一步发布plan将其发布为ROS话题。
[1]《A* Pathfinding for Beginners》 Patrick Lester