一、SLAM建图

“SLAM”，英文全称是“Simultaneous Localization And Mapping”，翻译过来就是“即时定位与地图构建”。SLAM最早由Smith、Self和Cheeseman于1988年提出，因为它重要的理论和应用价值，所以被认为是使机器人实现真正自主移动的重点。激光雷达的数据特点为雷达激光所在平面与障碍物相切所得的切面图，即激光雷达数据的点阵形状。

而激光雷达在环境中某一位置的扫描范围具有局限性，只能扫描到对应位置的障碍物信息。但是又因为相邻位置所扫描到的障碍物有一部分是重合的，所以可以将相邻位置所扫描到的障碍物轮廓进行组合，即将连续的激光雷达扫描到的障碍物轮廓组合起来，这样就形成了一个完整的环境地图，这个地图是根据激光雷达扫描平面上所有障碍物的轮廓得到的。系统根据构建地图时障碍物轮廓的重合关系，可以反推出机器人对应的位置关系和机器人位于地图中的位置，这样就实现了机器人在进行地图构建的同时进行自身实时定位的功能，这就是SLAM的由来。
ROS所支持的SLAM算法有很多，大多数用户使用的是Hector SLAM和Gmapping，这两者的区别为：Hector SLAM可以仅依靠激光雷达进行工作；Gmapping则因为需要多传感器的数据进行融合使用，所以建图质量优于Hector SLAM。本章中使用Gmapping进行地图的构建

1 在仿真环境中实现SLAM建图

（1）确保在前面已经对两个开源项目完成了部署。

（2）启动开源项目“wpr_simulation”中SLAM建图的仿真场景，在终端程序中输入如下指令。

roslaunch wpr_simulation wpb_gmapping.launch

指令运行后会弹出Gazebo仿真界面，以及Rviz工具界面

在仿真场景中用隔板将场景分为了4片区域，模拟了一个正常的家庭环境，将整场景分为厨房、餐厅、客厅和卧室，并在每个房间中放置了对应的家具。
从Rviz工具界面中可以看到，由激光雷达所扫描到的区域为灰白色，激光雷达扫描到的障碍物为黑色，未扫描到的区域为深灰色。
（3）控制机器人在环境中移动，以便于扫描完整地图，一般情况下会使用遥控手柄控制机器人移动。启动的launch文件中已经包含了手柄控制的节点，可以直接使用手柄遥控。
如果没有遥控手柄，可以使用键盘控制的方式移动机器人，打开一个新的终端程序，输入如下指令。

rosrun wpr_simulation keyboard_vel_ctrl

指令运行后，终端中会提示控制机器人移动所用的按键，需要注意的是，必须保证此终端为当前选中窗口，否则不能控制机器人移动，控制机器人移动的对应按键为加速制，即按下对应按键后，机器人速度为累加制，当需要转换机器人运动方向时，需先停止，再按下对应方向按键。

控制机器人扫描整个场景之后，在系统中可以看到机器人所扫描的地图

将地图保存下来，打开一个新的终端程序，输入如下指令。

rosrun map_server map_saver -f map

这条指令的意思是，启动map_server包的map_saver程序，将当前SLAM建好的图保存为名为“map”的地图。按下“Enter”键，确认保存，此时系统会提示地图保存完成信息，同时在系统的主文件夹中会出现两个名为“map”的文件，一个名为“map.pgm”，另一个名为“map.yaml”。其中“map.pgm”为图片格式，双击可以查看图片内容，它的内容就是建好的地图图案。

二、Navigation自主导航

使用之前建好的地图进行机器人导航，在进行实际操作之前，先来了解一下机器人导航的原理。在前面的实验中，使用激光雷达SLAM建立的地图为栅格地图，这种地图是室内移动机器人进行导航时常用的地图格式，它是一种在二维空间上描述障碍物分布状况的地图形式。栅格地图将整个环境状况在一个切面上分割成一个横竖排列的栅格小空间，每一个小空间用不同的颜色标记出这个空间的情况。

Rviz中的二维地图是用ROS的SLAM算法建立的二维平面地图在Rviz里的显示，如果放大观察，这个二维地图的细节部分是由一个个正方形的栅格组成的。

白色表示该栅格空间内无障碍物，机器人可以通行；黑色表示该栅格空间内存在障碍物，机器人不能通行；灰色表示该栅格空间尚未探索，可通行情况不明

有了栅格地图，机器人的地图导航问题就变成在栅格地图中寻找机器人能通过的空间区域并驱动机器人从起点移动到终点，这里面包含了两个部分的任务。
（1）机器人定位。机器人需要知道自己当前在地图中的位置，才能确定导航的起点在哪。机器人在移动过程中，也需要时刻确定自己的位置是否贴合规划的路径。
（2）路径规划与导航。路径规划算法就是在栅格地图中寻找一条可通行的路径，即连续的可通行栅格，其从机器人当前位置一直延伸到导航的目标终点

在实际的机器人路径规划中，除了考虑白色可通行栅格的连通情况，还需要考虑机器人本身体积所占用的空间，只有白色栅格空间大于机器人的底盘直径，机器人才能安全通过。因此通常会在黑色障碍物域膨胀出一条碰撞安全边界，这条边界的宽度为机器人底盘半径长度，它的意义是：当机器人位于这条安全边界里时提示它会与障碍物发生碰撞。这样，机器人的可通行路径会避开这一条安全边界，在剩余的可通行区域里规划。
在ROS中启动Navigation导航系统后，可以在Rviz里看到障碍物栅格周围有一圈淡蓝色的安全边界。机器人的路径规划会避开黑色障碍物栅格和淡蓝色的安全边界，在剩余的白色可通行区域里寻找出一条最短路径，图中的曲线就是Navigation导航系统规划出的路径。

ROS的Navigation导航系统按照上述功能分为两个主要部分。
（1）AMCL：英文全称为“Adaptive Monte Carlo Localization”（蒙特卡洛自适应定位算法），这是一种使用概率理论在已知地图中对机器人自定位置进行估计的方法。这种方法会先在机器人可能通过的位置周围假设多个位置，然后在机器人行进过程中，依据激光雷达和电机码盘里程计等设备输出的信息对这些假设位置进行筛选，逐步剔除明显不可信的假设位置，留下可信度较高的定位位置。比如在图中，地面上那些分散的绿色箭头就是机器人的假设位置，一开始的时候绿色箭头很多、很分散，在机器人运动过程中，这些箭头会逐渐收敛，最终汇聚成一个箭头，它就是机器人最可信的定位位置。
（2）Move_Base：这是Navigation系统里扮演核心中枢的ROS包，它将机器人导航需要用到的地图、坐标、路径和行为规划器连接到了一起，同时还提供了导航参数的设置接口。

具体如下。
（1）map_server在主题“/map”里提供全局地图信息，global_costmap从这个主题里获得全局地图信息，再加上从sensor sources获得的激光雷达和点云等的信息，融合成一个全局的栅格代价地图。
（2）全局的栅格代价地图交给global_planner进行全局路径规划，再结合外部节点发送到主题“/move_base_simple/goal”里的移动终点得出紫色的全局移动路径。
（3）local_costmap从global_costmap中截取机器人周围一定距离内的地图，结合传感器信息生成一个局部的代价地图。
（4）local_planner从global_planner获得全局规划路径，结合从local_costmap获得的局部代价地图及amcl提供的机器人位置信息，计算出机器人当前应该执行的速度，发送到主题“cmd_vel”里，驱动机器人沿着全局路径进行移动。
从上面部分可以看出，ROS的Navigation系统已经包含了导航需要的大部分功能，只需要提供全局地图和导航目的地即可进行Navigation导航。

（1）复制地图文件

建立的两个地图文件复制到工作空间“~/catkin_ws/src/wpr_simulation/maps”内。

(2）在终端程序中输入如下指令启动仿真程序

roslaunch wpr_simulation wpb_navigation.launch

仿真程序运行后会弹出导航仿真场景，场景内的布局和建图的仿真场景一致，只是机器人初始位置发生了改变。

启动Gazebo的同时，Rviz也会启动，可以在左侧任务栏进行窗口切换，在Rviz界面中就可以看到前面创建好的地图。

在这个界面中可以看到规划器根据地图固定障碍物膨胀出来的安全边界。
（3）在Rviz刚启动时，机器人的默认位置是地图的起始点，就是建图时机器人出发的地方。而在导航开始时，现实世界中机器人很有可能并不在之前建图时机器人出发的位置。这一点也可以通过观察Rviz中红色激光雷达数据点和静态障碍物轮廓是否贴合判断出来。如果现实世界中的机器人位置和Rviz中显示的位置有偏差，那么需要在导航前先纠正这个偏差，具体过程如下。
先单击Rviz界面上方工具栏里的“2D Pose Estimate”按钮，然后单击Rviz界面里现实机器人所处的位置。这时，界面里会出现一个箭头，代表的是机器人在初始位置的朝向。按住鼠标左键不放，在屏幕上画圈，可以控制箭头的朝向，选定方向后即可松开按键。

4）设置好机器人的初始位置后，就可以为机器人指定导航的目标地点。先单击Rviz工具栏里的“2D Nav Goal”按钮，然后单击Rviz界面上的导航目标点（通常在白色区域里选择一个地点）。此时会再次出现箭头，和前面的操作一样，按住鼠标左键不放在屏幕上拖动，设置机器人移动到终点后的朝向。
选择完目标朝向后，松开鼠标左键即可看到路径规划，全局规划器会自动规划出一条路径，机器人会沿着此路径移动到目标点，并在调整朝向后停止

同时，我们在Gazebo仿真界面中也可以看到机器人在场景中移动到目标点位。接下来可以尝试设置其他位置目标点，观察机器人规划的路径及移动过程

来源: 部分来自网络