激光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

激光SLAM导航系列(四)全局路径规划相关推荐

激光SLAM导航系列(五)局部路径规划
局部路径规划局部路径规划简介机器人在获得目的地信息后,首先经过全局路径规划规划出一条大致可行的路线,然后调用局部路径规划器根据这条路线及costmap的信息规划出机器人在局部时做出具体行动策略,R ...
激光SLAM导航系列(二)SLAM与导航系统框架
激光SLAM系统框架在激光SLAM系统中,Gmapping获取扫描的激光雷达信息以及里程计数据可动态的生成2D栅格地图.导航包则利用这个栅格地图,里程计数据和激光雷达数据做出适合的路径规划和定位,最 ...
激光SLAM导航系列(三)Costmap(代价地图)（上）
Costmap(代价地图)(上) Costmap是机器人收集传感器信息建立和更新的二维或三维地图,可以从下图简要了解. 上图中,红色部分代表costmap中的障碍物,蓝色部分表示通过机器人内切圆半径膨 ...
ROS导航系列(四)：全局路径规划器的参数配置分析
全局路径规划器global_planner的参数配置与分析一.global_planner相关参数二.测试 nav_core中提供了对应的全局路径规划接口.要想使用机器人move_base提供的便 ...
苏宁 11.11：仓库内多 AGV 协作的全局路径规划算法研究
本文为『InfoQ x 苏宁 2018双十一』技术特别策划系列文章之一. 1. 背景随着物联网和人工智能的发展,越来越多的任务渐渐的被机器人取代,机器人逐渐在发展中慢慢进入物流领域,"智能 ...
全局路径规划：图搜索算法介绍1(BFS/DFS)
对于全局路径规划的设计,我们先要了解什么是图搜索,在此之前,要先知道什么是图: 可以看到,图有很多种,有无向图,有向图,节点之间还可以有不同的weight, 用于表述从节点与节点直接迁移的代价. 而图 ...
a算法和a*算法的区别_机器人路径规划算法，全局路径规划与局部路径规划究竟有哪些区别？...
若步智能移动这一简单动作,对于人类来说相当容易,但对机器人而言就变得极为复杂,说到机器人移动就不得不提到路径规划,路径规划是移动机器人导航最基本的环节,指的是 ...
视觉学习笔记5——FAR Planner全局路径规划算法学习研究
文章目录一.Far Planner是什么? 二.配置安装Far Planner 1.运行环境 2.下载编译开发环境包 3.编译Far_Planner 一.Far Planner是什么? FAR Pl ...
(一)路径规划算法---Astar实现自定义的全局路径规划插件
Astar实现自定义的全局路径规划插件文章目录 Astar实现自定义的全局路径规划插件 1.插件功能包的建立 2. 相关步骤 2.1 建立工作空间和环境变量的配置 2.2 建立功能包 2.3 添加源 ...
全局路径规划：图搜索算法介绍6(A star)Matlab算法实现
本文接:全局路径规划:图搜索算法介绍2(A star) https://blog.csdn.net/gophae/article/details/103061702 % This is Yunchen ...

激光SLAM导航系列(四)全局路径规划

激光SLAM导航系列(四)全局路径规划相关推荐

最新文章

热门文章