rbpf粒子滤波slam matlab程序_2D激光雷达SLAM建图定位与路径规划

简介

SLAM（同步定位与地图构建），是指运动物体根据传感器的信息，一边计算自身位置，一边构建环境地图的过程，解决机器人等在未知环境下运动时的定位与地图构建问题。目前，SLAM的主要应用于机器人、无人机、无人驾驶、AR、VR等领域。其用途包括传感器自身的定位，以及后续的路径规划、运动性能、场景理解。

激光SLAM是目前最稳定、最主流的定位导航方法，但在无结构的环境中，由于缺少足够的几何特征，激光雷达SLAM将发生错误。目前室内主要采用 二维激光雷达，室外采用三维激光雷达。

激光SLAM关键模块介绍

激光SLAM所需要的传感器一般有激光雷达（Lidar）、惯性测量单元（IMU）、里程计（Odometry）。由于 IMU 具有较高的角速度测量精度，相比IMU里程计具有较高的局部位置测量精度，一般用IMU计算角度信息，里程计计算位置信息，配合激光雷达进行SLAM过程。

激光SLAM框架一般分为前端扫描匹配、后端优化、闭环检测、地图构建四个关键模块。

前端扫描匹配是激光SLAM的核心步骤，工作内容是已知前一帧位姿并利用相邻帧之间的关系估计当前帧的位姿；前端扫描匹配能给出短时间内的位姿和地图，但由于不可避免的误差累积，后端优化正是当长时间增量式扫描匹配后优化里程计及地图信息；闭环检测负责通过检测闭环而减少全局地图的漂移现象，以便生成全局一致性地图；地图构建模块负责生成和维护全局地图。

激光SLAM方案介绍

根据所采用的数学优化框架，激光SLAM可分为两大类：基于滤波器（Filter-based）和基于图优化（Graph-based）的激光SLAM。下面分别介绍两种框架下的激光SLAM方案。

Gmapping-SLAM(基于粒子滤波)

Gmapping算法是目前基于激光雷达和里程计方案里面比较可靠和成熟的一个算法，它基于粒子滤波，采用RBPF的方法效果稳定，目前许多基于ROS的机器人都跑的是gmapping_slam。

Gmapping的作用是根据激光雷达和里程计（Odometry）的信息，对环境地图进行构建，并且对自身状态进行估计。因此它得输入应当包括激光雷达和里程计的数据，而输出应当有自身位置和地图。

Gmapping消息流

从消息流向的角度看，gmapping算法实际运行中的结构，如下图所示：

位于中心的是我们运行的slam_gmapping节点，这个节点负责整个gmapping SLAM的工作。下面看下它的输入输出信息。

输入

• /tf以及/tf_static：坐标变换，类型为第一代的tf/tfMessage或第二代的tf2_msgs/TFMessage，其中一定得提供的有两个tf，一个是base_frame与laser_frame之间的tf，即机器人底盘和激光雷达之间的变换；一个是base_frame与odom_frame之间的tf，即底盘和里程计原点之间的坐标变换。odom_frame可以理解为里程计原点所在的坐标系。
• /scan：激光雷达数据，类型为sensor_msgs/LaserScan。

/scan是Gmapping SLAM所必须的激光雷达数据。

/tf维护了整个ROS三维世界里的转换关系，而slam_gmapping要从中读取的数据是base_frame与laser_frame之间的tf,只有这样才能够把周围障碍物变换到机器人坐标系下，更重要的是base_frame与odom_frame之间的tf，这个tf反映了里程计（电机的光电码盘、视觉里程计、IMU）的监测数据，也就是机器人里程计测得走了多少距离，它会把这段变换发布到odom_frame和laser_frame之间。因此slam_gmapping会从/tf中获得机器人里程计的数据。

输出

• /tf： 主要是输出map_frame和odom_frame之间的变换。
• /slam_gmapping/entropy：std_msgs/Float64类型，反映了机器人位姿估计的分散程度。
• /map：slam_gmapping建立的地图。
• /map_metadata： 地图的相关信息。

输出的/tf里有一个很重要的信息，就是map_frame(地图坐标系)和odom_frame(里程计坐标系)之间的变换，这其实就是对机器人的定位。通过连通map_frame和odom_frame，这样map_frame与base_frame甚至与laser_frame都连通了。这样便实现了机器人在地图上的定位。同时，输出的Topic里还有/map，在SLAM场景中，地图是作为SLAM的结果被不断地更新和发布。

Cartographer-SLAM(基于图优化)

cartographer是google开发的实时室内SLAM项目，cartographer采用基于ceres非线性优化的方法，cartographer的量点在于代码规范与工程化，非常适合于商业应用和再开发。并且cartographer基于submap子图构建全局地图的思想，能有效的避免建图过程中环境中移动物体的干扰。并且cartographer支持多传感器数据（odometry、IMU、LaserScan等）建图，支持2D_SLAM和3D_SLAM建图。

cartographer采用的是主流的SLAM框架，也就是特征提取、闭环检测、后端优化的三段式。由一定数量的LaserScan组成一个submap子图，一系列的submap子图构成了全局地图。用LaserScan构建submap的短时间过程累计误差不大，但是用submap构建全局地图的长时间过程就会存在很大的累计误差，所以需要利用闭环检测来修正这些submap的位置，闭环检测的基本单元是submap，闭环检测采用scan_match策略。cartographer的重点内容就是融合多传感器数据（odometry、IMU、LaserScan等）的submap子图创建以及用于闭环检测的scan_match策略的实现。

Cartographer_ros整体代码结构

参考博客： 小虎哥哥爱学习

整体代码结构

顶层的是cartographer_ros，作为rosj接口调用层，通过调用cartographer核心算法，订阅多传感器数据（/scan、/imu、/odom等），并发布地图、机器人位置信息（/map、/tf等）；其次是cartographer，作为SLAM算法的核心实现，特征提取、子图构建、闭环检测、全局优化都在这里实现，其中优化过程需要调用ceres-solver非线性优化库；最后是ceres-solver，非线性优化库，用于求解SLAM中的优化问题。

ROS-Navigation 导航

ROS提供了一整套Navigation的解决方案，包括全局与局部的路径规划、代价地图、异常行为恢复、地图服务器等。

Navigation工作框架

Navigation工作框架图

Navigation工作框架图中位于导航功能正中心的是move_base节点，可以理解为一个强大的路径规划器，在实际的导航任务中，只需要启动这一个node，并且给他提供数据，就可以规划出路径和速度。move_base之所以能做到路径规划，是因为它包含了很多的插件，像图中的白色圆圈global_planner、local_planner、global_costmap、local_costmap、recovery_behaviors。这些插件用于负责一些更细微的任务：全局规划、局部规划、全局地图、局部地图、恢复行为。而每一个插件其实也都是一个package，放在Navigation Stack里。

Navigation Stack

Navigation Stack是一个ROS的metapackage，里面包含了ROS在路径规划、定位、地图、异常行为恢复等方面的package。Navigation Stack的主要作用就是路径规划，输入各传感器的数据，输出速度。

ROS坐标系

在ROS中，坐标轴规定可以参考ROS-REP 103，坐标系可以参考ROS-REP 105，常规用来表示三维位置关系时，使用的是右手坐标系，对于一个三维空间里面的旋转，可以分解成绕着坐标轴的旋转，旋转的方向使用右手法则定义。

右手坐标系定义

把右手放在原点的位置，使大姆指，食指和中指互成直角，把大姆指指向Z轴的正方向，食指指向X轴的正方向时向时，中指所指的方向就是Y轴的正方向。

通常相对于我们的身体而言

• X -> 朝前
• Y -> 朝左
• Z -> 朝上

绕坐标轴旋转定义

用右手握住坐标轴，大拇指的方向朝着坐标轴朝向的正方向，四指环绕的方向定义沿着这个坐标轴旋转的正方向。一般来说

• 绕Z轴旋转，称之为航向角，使用yaw表示;
• 绕X轴旋转，称之为横滚角，使用roll表示;
• 绕Y轴旋转，称之为俯仰角，使用pitch表示;

Autolabor Pro1 坐标系

以机器人四轮接触面为X-Y平面，四轮轴心连线的点做垂线，与x-y平面的交点为base_link坐标系原点，车头朝向方向为X轴，正左方向为Y轴，向上为Z轴。

杉川激光雷达 Delta-1A坐标系

根据杉川激光雷达提供的外观尺寸图文件，找到雷达的零点位置，零点方向为X轴，垂直于零点的正左方向为Y轴，向上为Z轴。

一般在安装时将雷达不带线的一端朝车头方向，雷达USB端的朝车尾方向。如下图所示：

小觅S1030IMU坐标系统

根据小觅官方说明文档，IMU 坐标系统为右手系,坐标轴方向如图所下:

激光SLAM实际应用

激光SLAM基于autolabor pro1小车、delate-1A、小觅双目相机、IntelNUC工控机等硬件，并采用ROS Kinetic机器人操作系统、gmapping-SLAM、Cartographer-SLAM算法、ROS-Navigation导航包等实现了激光SLAM的建图、定位以及路径规划等功能。

硬件组成图

IntelNUC工控机通过USB连接 分别与autolabor pro1小车、激光雷达、小觅双目进行交互。

总体软件结构图

当前软件需要运行在ROS -Kinetic机器人操作系统上。

软件介绍：

• lAutolabor pro1 ros驱动： 由autolabor pro1提供，可以到autolabor pro1官网去下载，下载地址：http://www.autolabor.com.cn/download，下载内容包括：Autolabor Pro1 ROS驱动包、Autolabor ROS键盘控制包。
• Delta-1A ROS驱动:由杉川激光雷达提供。
• Gmapping-SLAM算法：由ROS操作系统提供，安装的ROS桌面完整版本，自带Gmapping算法库。
• Cartographer-SLAM算法：需要自行安装。
• Move_base/acml ：需要安装ROS-Navigation库。
• 小觅ROS驱动：由小觅官方提供，可以到小觅官网去下载。
• Rviz可视化工具： 由ROS操作系统提供。

实际运行效果

下面把通过gmapping算法建图以及Cartographe算法建图效果图进行展示。

Gmapping算法建图结果

gmapping Rviz可视化建图过程

Cartographer算法建图效果

Cartographer Rviz可视化建图过程

工程代码结构说明

GitHub源码地址：

https://github.com/YMAndroid/Autolabor_Delta_1A_SLAM.git​github.com

catkin_ws_lidar_slam ----工程名称

——/src ---源代码目录

————/autolabor_delta_1a_navigation_src --建图、定位、路径规划功能功能包

————/autolabor_description ---建立的小车urdf模型功能包

————/autolabor_keyboard_control ---autolabor pro1 小车键盘控制功能包

————/autolabor_pro1_driver ---autolabor pro1小车驱动功能包

————/delta_1a_ros -- 激光雷达驱动功能包

————/CMakeLists.txt --编译后自动生成的

运行指令

• 建图

采用gmapping与 cartographer_ros建图算法分别进行建图

gmapping建图：(雷达+小车)

打开终端，执行如下指令:
$ cd ~/catkin_ws_lidar_slam/
$ source devel/setup.bash
$ roslaunch autolabor_box_launch create_map_gmapping.launch   ####打开小车urdf模型、驱动、键盘控制节点、打开雷达驱动、打开gmapping节点、打开rviz可视化节点
####此时使用键盘的上下左右键控制小车移动，开始建图
####建图完成后，开启新的终端执行如下指令，保存地图：
$ rosrun map_server map_saver -f map_name    ####保存地图  map_name为保存地图的名称，运行结果，会生成2个文件，后缀名分别为 .pgm  .yaml

cartographer_ros建图:(雷达+小车)

打开终端，执行如下指令：
$ cd ~/catkin_ws_lidar_slam/
$ source devel/setup.bash
$ roslaunch autolabor_box_launch create_map_cartographer.launch
####此时使用键盘的上下左右键控制小车移动，开始建图
####建图完成后，开启新的终端执行如下指令，保存地图：
$ rosrun map_server map_saver -f map_name 

cartographer_ros建图:(雷达+小车+IMU)

$ cd ~/catkin_ws_lidar_slam/
$ roslaunch autolabor_box_launch create_map_cartographer_imu.launch
####此时使用键盘的上下左右键控制小车移动，开始建图
####建图完成后，开启新的终端执行如下指令，保存地图：
$ rosrun map_server map_saver -f map_name 

• 定位与路径规划

分别使用gmapping建图算法与cartographer_ros建图算法建立的地图进行定位与路径规划

gmapping算法建立的地图进行定位与路径规划

###打开终端，执行如下指令：
$ cd ~/catkin_ws_lidar_slam/
$ source devel/setup.bash
$ roslaunch autolabor_box_launch gmap_navigation.launch     ####打开小车urdf模型、驱动、键盘控制节点、打开雷达驱动、打开map_server节点、打开move_base节点、打开acml节点、打开rviz可视化节点

###使用Rviz工具栏中的2D Nav Goal按钮选择Goal，进行路径规划导航。

cartographer_ros算法建立的地图进行定位与路径规划

##打开终端，执行如下指令：
$ cd ~/catkin_ws_lidar_slam/
$ source devel/setup.bash
$ roslaunch autolabor_box_launch carto_navigation.launch

###使用Rviz工具栏中的2D Nav Goal按钮选择Goal，进行路径规划导航。

============================================================

###注意事项######

开始路径规划之前，先用键盘控制小车在原地旋转几圈，加快粒子群收敛，便于小车找准在地图中的位置以及方位。

###注意事项######

未收敛的时候效果：

小车旁边的蓝色箭头是分散的，粒子群收缩后，蓝色的箭头都会在聚集在小车的正中心，为一个点。