Gmapping SLAM and Navigation
1、什么是SLAM？
在研究机器人自动行驶的时候，人们注意到，为了实现自动驾驶功能，机器人必须实现对自身的定位和对周围环境的感知。在逐步探索中，研究者开始借用激光雷达、相机等先进的传感设备完成定位与环境感知功能。
同步定位与建图（Simultaneous Localization and Mapping，SLAM）是在上世纪80年代被提出的，起初发展的算法皆采用激光雷达作为定位与建图的工具，随着稀疏性问题的解决，相机也被引入SLAM领域，如今SLAM技术在向多传感器融合的方向发展，激光雷达、深度相机、IMU惯导等正成为SLAM技术的常见解决方案。
2、Gmapping
2.1、Gmapping的原理介绍
Gmapping是一种用于建立二维地图的概率算法，常用于移动机器人的自主定位和建图过程中。它基于激光雷达数据和机器人的运动信息，通过蒙特卡洛方法进行粒子滤波定位和地图构建。
在Gmapping算法中，首先利用激光雷达获取环境的距离信息，然后通过对扫描数据进行滤波和去噪处理，得到清晰的地图信息。接着，使用粒子滤波定位技术，根据机器人的运动模型和传感器的反馈信息，对机器人在地图上的位置进行估计。
在每个时间步中，gmapping算法会生成一组粒子，代表机器人可能存在的位置。这些粒子根据机器人的运动模型进行更新，并根据激光雷达数据计算出每个粒子在地图上的权重。权重表示了该粒子在地图上的拟合程度，越拟合越高。
通过重采样和权重归一化，算法会选择一部分权重较高的粒子作为新的样本集，并根据它们来更新地图。这样，随着时间的推移，地图会逐渐完善并收敛到真实环境中。
Gmapping算法的核心思想是通过利用机器人的传感器信息和运动模型，结合蒙特卡洛方法进行粒子滤波定位和地图构建，
2.2、Gmapping相关功能包下载与配置
1、首先新建并编译工作空间gampping_ws:
mkdir -p ~/gmapping_ws/src #新建文件夹
cd ~/gmapping_ws/src
catkin_init_workspace #初始化工作空间
cd ..
catkin_make #编译工作空间
2、Gmapping的安装方法有两种，在这里参考张虎的《机器人SLAM导航核心技术与实战》第8章内容：
（1）直接下载ROS系统工作空间功能包：
sudo apt-get install ros-melodic-gmapping*
此时gmapping、slam_gmapping、openslam_gmapping功能包被下载到ROS的系统工作空间opt/ros/melodic/share中，此时只有可执行文件，看不到源码。
（2）如需要查看源码，可以分别安装openslam_gmapping和slam_gmapping：
安装Gmapping的依赖库：
# 安装openslam_gmapping和slam_gmapping功能包及其依赖
sudo apt-get install ros-melodic-openslam-gmapping ros-melodic-gmapping
# 卸载openslam_gmapping和slam_gmapping功能包但保留已经安装的依赖项
sudo apt-get remove ros-melodic-openslam-gmapping ros-melodic-gmapping
然后在自定义的工作空间gmapping_ws下安装openslam_gmapping和slam_gmapping功能包：
cd gmapping_ws/src/
git clone https://github.com/ros-perception/slam_gmapping.git #下载slam_gmapping
cd slam_gmapping
git branch # 查看代码版本是不是melodic，若不是使用git checkout命令切换到对应版本
git clone https://github.com/ros-perception/openslam_gmapping.git # 下载openslam_gmapping
cd openslam_gmapping
git branch # 查看代码版本是不是melodic，若不是使用git checkout命令切换到对应版本
然后编译指定功能包：
cd ~/gmapping_ws
catkin_make -DCATKIN_WHITELIST_PACKAGES="openslam_gmapping"
catkin_make -DCATKIN_WHITELIST_PACKAGES="gmapping"
使系统识别ROS工作空间，然后在命令行输入时可以使用tab键补全：
echo "source gmapping_ws/devel/setup.bash">>~/.bashrc
source ~/.bashrc
*参考文章链接：https://blog.csdn.net/hiram_zhang/article/details/116220680
3、下载basic_localization_stage_indexed.bag数据集进行测试
下载地址：http://download.ros.org/data/amcl/?C=D;O=A
# 用默认launch文件启动gmapping
roslaunch gmapping slam_gmapping_pr2.launch
再打开一个终端，播放basic_localization_stage_indexed.bag数据集：
cd ~/Downloads/
rosbag play basic_localization_stage_indexed.bag
在启动一个终端，运行rviz
rviz
4、因为下载gmapping是为了能够在自己的小车运行，因此必须按照小车的实际情况对gmapping算法中的参数进行配置。
在slamgmapping/gmapping/launch中右键打开终端并新建文件gmapping_robot.launch，参数如下图所示：
touch gmapping_robot.launch
3、在ROS中运行GmappingSLAM
推荐一个不用安装虚拟机和Ubuntu就可以远程ssh登录机器人系统的工具：mobaxterm，下载官网：https://mobaxterm.mobatek.net/download.html
下载后不用安装，解压后直接运行.exe文件
3.1、准备工作
将笔记本端与主控板连接起来，便于使用笔记本查看建图效果和控制小车运动。
1、在远程登录小车之前，必须使主控板上Ubuntu系统与笔记本上Ubuntu系统连接到同一网络下，可以是小车自己携带的网卡发出的wifi，也可以是自己创建的局域网。一般如果虚拟机的ip与主控板的ip的前三位主段是相同的，就可以断定二者共用同一个wifi，或者使用ping命令能ping通的话，也可以表明二者在同一局域网内。
ping 192.168.0.100 //后面是我所使用的小车主控板的ip地址
2、对VMware虚拟网络进行配置，参考该文章：
https://blog.csdn.net/Alezan/article/details/124070178
桥接模式：使虚拟机与主机处于同一地位，虚拟机可以访问与主机在同一局域网下的其他电脑，同时可以在主机联通互联网时上网，适合多台电脑的互联
直连模式：主机相当于虚拟机的路由器，虚拟机的ip主段与主机的ip主段不再一致，解决了桥接模式ip不足的问题，虚拟机也可以访问互联网，但是外部不能访问虚拟机
仅主机模式：只有虚拟机与主机之间的访问，无法连接外部网络
在使用自己的机器人时请选择桥接模式，同时要配置虚拟机的静态ip，不能再使用主机的ip，具体操作可以查看该文章：https://blog.csdn.net/qq_40177571/article/details/124862435
查看Ubuntu系统ip地址的命令：ip a，或者可以用：ifconfig命令查看系统的ip地址，若没法运行该命令，可根据提示安装：sudo apt-get install net-tools，然后运行ifconfig。
在连接局域网时，我自己的Windows主机ip：192.168.0.185，用win+R并输入cmd打开Windows命令行并输入ip查看命令：ipconfig
然后是自己虚拟机的ip：192.168.0.188
3、使用ssh远程登录：
ssh -Y wheeltec@192.168.0.100
wheeltec是主控板上系统的用户名，192.168.0.100是主控板上系统的ip地址，-Y表示可以访问图形化工具。
4、用虚拟机初次远程登录主控板系统时，可能会出现报错，我这里的解决方案是将提示的那一行复制到命令行运行，然后再次尝试ssh命令。
3.2、建图过程
3.2.1、操作指令
1、远程登录成功后，启动建图命令：
roslaunch turn_on_wheeltec_robot mapping.launch
其中turn_on_wheeltec_robot为自定义的功能包，mapping.launch为建图启动文件。
2、打开rviz，点击Add map、laser_scan，分别在topic中订阅/map话题和/scan话题，查看建图效果
rviz
注意，在单独启动rviz时，需要先启动master节点：roscore，然后另启一终端运行：rosrun rviz rviz。但是对.launch文件，运行之后，若master节点没有启动会自动去启动它，因此在第二步运行rviz时，master节点已经被启动了，无需再次运行roscore。
每次打开rviz都要重新配置很麻烦，可以使用ctrl+s命令保存当前配置，保存到路径opt/ros/melodic/share/rviz/default.rviz，若提示无法写入该文件，则是当前文件权限为“only read”，启动一个终端，输入并运行命令sudo chmod 777 /opt/ros/melodic/share/rviz/default.rviz，然后再此尝试ctrl+s保存即可成功。
3、另启一个终端，运行键盘控制小车命令：
roslaunch wheeltec_robot_rc keyboard_teleop.launch
运行成功之后，可以使用笔记本键盘控制小车完成建图。
4、建图完成后，需要保存地图：
roslaunch turn_on_wheeltec_robot map_saver.launch
地图被保存到：~/wheeltec_robot/src/turn_on_wheeltec_robot/map路径下，其中的wheeltec.pgm文件为所建地图的灰度图，在map_saver.launch中也可以自定义保存文件的名字，由于要跑好几个算法模型，故每次都要进行重命名，在这里命名为/gmapping。保存好之后发现其实不止一个文件，.pgm为灰度图文件，.yaml为描述为图片文件，.
3.2.2、遇到问题及解决方案
无论是mobaxterm还是虚拟机，运行建图命令一切正常，但是rviz始终无法打开，虚拟机中报错是无法找到master节点，mobaxterm报错缺少一个驱动swrast。将小车的主控板连接到显示器，尝试在其上运行rviz，结果可以正常打开工作。
查看报错信息：
libGL error: MESA-LOADER: failed to open swrast (search paths /usr/lib/aarch64-linux-gnu/dri:$${ORIGIN}/dri:/usr/lib/dri)
libGL error: failed to load driver: swrast
初步判断是与rviz相关的驱动swrast无法工作，该驱动应该与libGL有关。
在网上搜索报错信息，似乎将问题都指向是libGL和Nvida的驱动，建议更新驱动。但是小车在连接显示器后测试rviz是可以正常打开的，猜测可能跟虚拟机或者自己笔记本有关，或者是二者版本之间存在差异。向老师寻求解决方案无果，自己在网上找了很久也没有可用的解决办法，最终只能选择用笔记本做小车的运动控制，在跑完一圈后用显示器查看建图效果，对于建图效果差的地方可以多跑几次。
但是在导航时小车不可能带个屏幕一起跑，上述方法不可行。在查阅一个视频后，学习到了ROS的多机通讯机制。将小车设置位ROS主机，电脑上的虚拟机设置为ROS从机，两台电脑连接在同一网段下（看ip地址的前三位），分别在主机和从机的home目录下的.bashrc文件（ctrl+h可见）中加入：
# 主机
export ROS_MASTER_URI=http://192.168.0.100:11311 # 主机ip
export ROS_HOSTNAME=192.168.0.100 # 从机ip
# 从机
export ROS_MASTER_URI=http://192.168.0.100:11311 # 主机ip
export ROS_HOSTNAME=192.168.0.188 # 从机的ip
如此便在主机与从机之间建立了通讯机制，ROS主机的master节点作为总的master节点，从机要运行node必须要事先在主机中启动master节点。如此便可完美的避开上述问题。
4、在ROS中实现导航功能
我们先分析日常生活中使用手机进行导航的实际案例，在使用手机进行导航时，我们需要使用到导航软件中已经建好的城市地图，首先打开定位，定位自己当期所处的位置，然后设定目的地，进行路径规划，路径可能不止一条，有距离最短的、有时间最短的、有红灯最少的等等，机器人的导航也与此相似。
前面我们已经构建好了地图，并将其保存在了~/wheeltec_robot/src/turn_on_wheeltec_robot/map路径下，接着就要在场景中对小车自身的位置进行定位，然后设定地图中小车的目标点，小车根据地图自动规划出路径。分析整个过程，我们可以注意到，地图、定位和路径规划是整个导航过程最为重要的三个概念，地图的品质、定位的精度、路径规划的合理性事关导航过程的成败，因此需要重点分析。
4.1、代价地图
前面建图部分得到的灰度地图其数值范围为[-1,100]，其中“-1”代表未知区域，基本是rviz的背景色，“0“代表自由区域，为白色，”100“代表完全占用，为黑色，实际上在建图时只有”-1“、”0“、”100“三种情况，因为建图过程中雷达探测到了障碍物一般就是确定的，不会取0-100之间的数值。
到了执行导航功能时，系统的地图又有所不同，此时使用的并不完全是使用建图的地图，而是使用代价地图。代价地图的取值范围是[0,254]，数值越大，表示占用程度越高，导航时小车越要远离。
代价地图的生成过程可以参考此文章：https://blog.csdn.net/qq_35635374/article/details/120874817
总而言之就是对之前建好的静态层地图和雷达运行探测到的障碍层地图按照膨胀规则进行膨胀和合并，生成数值在[0,254]之间的代价地图，导航时按照地图方格中的数值进行路径规划。
*图源为上面分享的文章，一图便可看懂
4.2、定位
前面提到人使用手机进行导航时，首先必须要知道自己在地图中的位置，机器人在地图中的导航也是如此。机器人在地图中的定位采用自适应蒙特卡洛方法（amcl方法）。
蒙特卡洛方法是一种概率模型，简单来说就是用实验的方式用记录数值来逼近概率，比如用蒙特卡洛方法计算圆周率。在机器人定位中，蒙特卡洛方法有以下几个步骤：
1、初始化：在地图中抛洒粒子
2、计算权重：根据雷达信息计算每个粒子的权重，权重越高，证明匹配度越高
3、重采样：在第二步计算得到的权值较高的粒子附近重新抛洒粒子，此时新粒子的权重与离自己最近的前一步得到的粒子的权重相同
4、状态变换：机器人继续运动，粒子也随之运动，但所有粒子不是以相同的速率运动，粒子又会局部分散开来，然后重复第2步，接着2-3-4步依次循环，直到机器人近似确定自己的位姿。
这种方法也被称为粒子滤波方法。
自适应的目的是要解决两个问题，一个是“粒子数固定”问题，另一个是“机器人绑架”问题。解决“粒子数固定”问题是指当位姿基本确定后，减少粒子数量，减少计算量。解决“机器人绑架”问题是指当机器人定位完成后，被人为放置到其他环境，此时重新计算得到的权值会普遍较低，此时就需要重新抛洒粒子，进行重定位。
amcl的输入是全局地图、雷达和小车的TF信息，最后发布的是TF转换信息。其节点图如下所示：
/amcl广播/map到/odom_combined，/ros_pose_ekf广播/odom_combinde到/base_footprint。
4.3、路径规划
路径规划可以分为全局路径规划和局部路径规划，全局路径规划是指根据已经构建好的静态地图，规划起始点与目标点之间的全局路径，局部路径规划是在静态全局地图上再构建一个小地图，实施探测小车周围的环境情况，应对可能突然出现的动态障碍物，实现动态避障。
4.3.1、全局路径规划
全局路径规划在已经建好的静态地图上进行，在ROS系统工作空间中，global_planner集成到/move_base功能包中，其中有Astar算法和Dijkstra，默认使用Dijkstra算法。Dijkstra算法使用广度优先方式，计算量比较庞大，规划速度比较慢，但保证规划的是最短路径。Astar算法采用深度优先方式，规划速度比较快，但不一定是最短路径。
/move_base中的全局路径规划器有Navfn、GlobalPlanner、CarrotPlanner三种，下面进行简要介绍：
1、Navfn：使用Dijkstra算法，Astar算法有bug用不了；
2、GlobalPlanner：对Navfn进行了优化，且具备A*算法功能，使用的人较多；
3、CarrotPlanner：继承自GlobalPlanner，其特点是当目标点在障碍物内时，它会沿着起始点与目标点之间的向量往回走一段，直到找到一个不在障碍物内的点，然后将这个目标点传递给局部路径规划器，自己不进行路径规划。
*图来自ROS官网：http://wiki.ros.org/carrot_planner
使用GlobalPlanner，如何切换Astar算法和Dijkstra算法呢？
在wheeltec_robot/src/navigation_melodic/global_planner/src中可以找到Astar算法和Dijkstra算法的源码，如图所示：
在本车上的turn_on_wheeltec_robot.launch文件中，调用的是teb_local_planner.launch文件，该文件将local_costmap_params.yaml、global_costmap_params.yaml、move_base_params.yaml、base_global_planner_param.yaml等参数文件上传到参数服务器中，其中全局路径规划器选择的是global_planner/GlobalPlanner，在base_global_planner_param.yaml文件中，对使用哪种算法进行了设置，如下图所示：
其中5-6行清晰的说明了如何切换Astar算法和Dijkstra算法。
4.3.2、局部路径规划
局部路径规划将全局规划的路径分成了很多小段，并根据是否探测到新的障碍物重新进行局部范围内的路径规划。/move_base中集成的局部路径规划器有BaseLocalPlanner、DWAPlanner、TebPlanner，下面进行简要介绍：
1、BaseLocalPlanner：实现了Trajectory Rollout和DWA两种局部路径规划；
2、DWAPlanner：基本上是BaseLocalPlanner的优化版；
DWAPlanner的基本原理是
3、TebPlanner：使用Timed Elastic Band方法进行局部路径规划。
TebPlanner的基本原理是
在使用自己的小车进行局部路径规划时，三种局部路径规划器如何选择呢？在本车上的turn_on_wheeltec_robot.launch文件中，调用的是teb_local_planner.launch文件，该文件将local_costmap_params.yaml、global_costmap_params.yaml、move_base_params.yaml、base_global_planner_param.yaml等参数文件上传到参数服务器中，其中在move_base_params.yaml中局部路径规划器选择的是teb_local_planner/Teb
LocalPlannerROS。根据项目需求，可以考虑将两种都使用进来，方便进行切换。
4.3、导航过程
1、启动一个终端，打开导航指令：
roslaunch turn_on_wheeltec_robot navigation.launch
2、打开rviz，Add添加两个map，分别订阅global_costmap、local_costmap话题，添加laser_scan订阅scan话题，添加两个path，分别订阅global_path、local_path话题，添加TF和RobotModel：
rviz
3、地图的原点与小车导航时的初始位置可能并不相同，可以长按鼠标使用2D Pose Estimate将初始位姿调整为小车此时在场地中的初始位姿；长按鼠标使用2D Nav Goal设置小车导航的目标点和目标位姿，然手后小车自动规划路线；使用Publish Point可以设置多个目标点，小车按照目标点顺序自动规划路径，依次前往，最终回到出发点，并以此循环往复，Add makerarray并订阅？可以显示目标点的序号。
使用rviz导航的目标点话题由rviz发布，由move_base订阅。
除了使用rviz，也可以在命令行手动发布目标点信息：
rostopic pub /move_base_simple/goal gemotry_msgs/PoseStamped
其中的参数要自己设置，可参考下面的方法：
frame_id:map
position:（目标点位置）
x:前进方向
y:符合右手坐标系
z:2D图一般为0
orientation:（目标点的姿态）
x:表示绕x轴旋转
y:表示绕y轴旋转
z:表示绕z轴旋转
w:
这里的position是采用笛卡尔坐标系，比较容易理解。方向则采用的是哈密顿四元数（xi+yj+zk+w），可以将其理解为四维复数，
5、总结
至此，Gmapping SLAM and Navigation全部结束，Gmapping在SLAM中算是比较简单的算法模型，对像我这样的初学者很友好，但是对Linux、ROS的新手而言配置过程还是很累的，很多报错比较难解决。希望自己多阅读一些ROS相关的书籍和文章，多学学前辈的经验，对机器人的开发过程多一些知识储备。学海无涯，唯苦可达！