最近没啥可写博客了的，而且自己的毕业设计刚好是ROS的SLAM+MoveIt相关的，怕写的太多论文查重没法通过。

最近有小伙伴联系到我，问了我八个ROS问题，我觉得很有意义，所以在这里给大家整理一下供大家一起学习。

大家在学习有困惑可以发送问题给我（1692697820@qq.com），我会找时间整理并回复大家。也欢迎大家在“泡泡”里面进行提问~

这里我们来讲解第八个问题，进行多传感器融合的思路？（视觉SLAM和激光SLAM结合？）

在ROS系统当中，一般机器人所用到的传感器有轮式编码器、陀螺仪、激光雷达、深度相机。大多数使用的使用的时候，大多ROS机器人以轮式编码器+激光雷达实现，只要topic和tf对的上，时间、环境维护好，配置相应的launch文件就可以跑起来！

我们接触较多的是使用直流编码电机的两轮差速小车底盘，订阅cmd_vel话题，通过运动学正解算得到两个轮子的速度值，再通过PID亮速度值转成PWM值给单片机执行；编码器在这里除了PID部分的部分，同时也需要进行里程计Odom的计算。通过轮子编码器得到的里程计也叫做轮式里程计。

陀螺仪也是常用的模块，六轴陀螺仪可以获取机器人在X、Y、Z三个的方向的加速度和角速度，九轴陀螺仪会在此基础上多一个磁力计（感知地球磁场，通过磁场来标定数据）。在ROS系统下的IMU数据除了加速度和角速度之外，还需要一组四元数（表示当前姿态的），这部分需要通过DMP算法解算得到。在陀螺仪这部分得到的里程计叫做惯导里程计。

我们以右手坐标系位参考，在SLAM建图导航过程当中，我们是以XoY平面位准，所需要的也只是X和Y方向的线速度、Z轴的角度。所以很多在做编码器融合陀螺仪发布Odom的时候，这也是一种采取的方案。

激光雷达就不过多描述，这个大家都不会陌生，它只有一个scan的话题，就是雷达的点云数据。深度相机是近几年兴起的新技术，相比较传统的相机，深度相机在功能上添加了一个深度测量，从而更方便准确的感知周围的环境及变化。通过RGB颜色数值+Deep深度数值，我们可以直接构建三维地图。不过也有使用depthtolaser功能包，将深度相机的点云数据转换模拟得到雷达点云的操作。

同时深度相机通过视觉算法也可以作为一个里程计——VIO视觉里程计。

多传感融合，我用较多的是robot_pose_ekf功能包，实现轮式编码器的里程计（Odom）和陀螺仪的里程计（IMU）融合，得到一个新的里程计Odom_Combined。

<launch>
	<!-- 发布雷达base_laser_link和底盘base_link的TF坐标关系 -->
	<node pkg="tf" type="static_transform_publisher" name="laser_static_broadcaster" args="0.15 0 0.05 0 0 0 base_link base_laser_link 200" />

	<!-- 发布陀螺仪base_imu_link和底盘base_link的TF坐标关系 -->
	<node pkg="tf" type="static_transform_publisher" name="imu_static_broadcaster" args="-0.05 0 0.05 0 0 0 base_link base_imu_link 200" />

	<!-- 启动robot_pose_ekf节点 -->
	<node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
  		<param name="output_frame" value="odom_combined"/>	
  		<!-- 输出的融合后的odom名为odom_conbined -->
    
  		<param name="base_footprint_frame" value="base_link"/>
  		<!-- 输出的融合后的参考坐标系为base_link -->
    
  		<param name="freq" value="30.0"/>
  		<!-- 新的里程计发布频率为30Hz -->
    
  		<param name="sensor_timeout" value="1.0"/>  
  		<!-- 数据超时 -->  
  
  		<param name="odom_used" value="true"/>
  		<!-- 启用轮子odom数据 -->
    
  		<param name="imu_used" value="true"/>
  		<!-- 启用陀螺仪imu数据 -->
    
  		<param name="vo_used" value="false"/>
  		<!-- 禁用视觉里程计数据 -->
    
		</node>
</launch>

这个是我在做ekf实现陀螺仪和轮式编码器融合得到odom_combined的一个launch文件。

这个是在进行gampping建图时候的tf树。雷达坐标和陀螺仪坐标通过静态tf关系连接到base_link，base_link坐标到odom_combined坐标是通过robot_pose_ekf融合得到，odom_combined融合后的里程计通过gmapping算法连接到map坐标系下。

我们启动底盘，订阅cmd_vel，发布轮式里程计odom；我们启动雷达，发布scan；我们启动陀螺仪，发布imu。

我们启动robot_pose_ekf功能包，发布雷达坐标到base_link坐标的关系、发布陀螺仪坐标到base_link坐标的关系，它们都是静态的、固定不动的坐标关系，robpt_pose_ekf会订阅imu和odom，发布一个融合好的里程计——odom_combined。

建图，或者导航，启动相应的功能包就可以，注意要把topic和tf对上！

激光SLAM只能建立2D栅格地图，典型的功能包就是gmapping、cartographer，其他的hector什么的感觉效果并不是很好；视觉SLAM可以建立3D立体地图，典型的功能包就是rtabmap，rtabmap对于多传感器的组合也是多种多样。

这个是使用rtabmap将深度相机、雷达以及里程计融合的一个框架图，这里的里程计只是轮式里程计，你可以按照我们上面说的使用轮式里程计+陀螺仪融合的方式得到一个更好的里程计，只要你在launch文件能对上就可以。

<launch> 
	<group ns="rtabmap"> 

		<!-- Use RGBD synchronization --> 
		<!-- Here is a general example using a standalone nodelet, but it is recommended to attach this nodelet to nodelet manager of the camera to avoid topic serialization --> 

		<node pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_ros/rgbd_sync" output="screen"> 
			<remap from="rgb/image" to="/camera/rgb/image_rect_color"/> 
			<remap from="depth/image" to="/camera/depth_registered/image_raw"/> 
			<remap from="rgb/camera_info" to="/camera/rgb/camera_info"/> 
			<remap from="rgbd_image" to="rgbd_image"/> <!-- output --> 
			<!-- Should be true for not synchronized camera topics (e.g., false for kinectv2, zed,realsense, true for xtion, kinect360)--> 
		
			<param name="approx_sync" value="true"/> 
		</node> 

		<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start"> 
			<param name="frame_id" type="string" value="base_link"/>
			<param name="subscribe_depth" type="bool" value="false"/>
			<param name="subscribe_rgbd" type="bool" value="true"/> 
			<param name="subscribe_scan" type="bool" value="true"/> 
			<remap from="odom" to="/base_controller/odom"/> 
			<remap from="scan" to="/base_scan"/> 
			<remap from="rgbd_image" to="rgbd_image"/> 
			<param name="queue_size" type="int" value="10"/>
			<!-- RTAB-Map's parameters --> 
		
			<param name="RGBD/NeighborLinkRefining" type="string" value="true"/> 
			<param name="RGBD/ProximityBySpace" type="string" value="true"/> 
			<param name="RGBD/AngularUpdate" type="string" value="0.01"/> 
			<param name="RGBD/LinearUpdate" type="string" value="0.01"/> 
			<param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/> 
			<param name="Grid/FromDepth" type="string" value="false"/> 
		
			<!-- occupancy grid from lidar --> 
			<param name="Reg/Force3DoF" type="string" value="true"/>
			<param name="Reg/Strategy" type="string" value="1"/> <!--1=ICP --> 
		
			<!-- ICP parameters --> 
			<param name="Icp/VoxelSize" type="string" value="0.05"/> 
			<param name="Icp/MaxCorrespondenceDistance" type="string" value="0.1"/> 
		</node> 
	</group> 
</launch> 

在这个例子中，因为 rtabmap节点同步来⾃不同传感器的主题，我们使⽤rgbd_sync nodelet来确保我们的图像主题在将它们提供给 之前正确同步在⼀起 rtabmap 。输出rgbd_image是rtabmap_ros/RGBDImage消息。参数approx_sync当相机节点尚未同步相机主题时应为真，例如此处对于Xbox 360的或openni2驱动 freenect approx_sync 程序。对于Kinect v2、ZED或realsense 的相机驱动程序， 应为false，因为它们发布 rgb 和深度主题已经同步（相同的时间戳）。

<!-- RTAB-Map的参数--> 
<param name="RGBD/NeighborLinkRefining" type="string" value="true"/> 
<param name="RGBD/ProximityBySpace" type="string" value="true"/> 
<param name="RGBD/AngularUpdate" type="string" value="0.01"/> 
<param name="RGBD/LinearUpdate" type="string" value="0.01"/> 
<param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/> 
<param name="Grid/FromDepth" type="string" value="false"/> 
<param name="Reg/Force3DoF" type="string" value="true"/> 
<param name="Reg/Strategy" type="string" value="1"/> <!-- 1=ICP --> 

<!-- ICP参数--> 
<param name="Icp/VoxelSize" type="string" value="0.05"/> 
<param name="Icp/MaxCorrespondenceDistance" type="string" value="0.1"/> 

此部分设置 RTAB-Map 的参数（与独⽴版本中的⾸选项对话框相同）。要知道RTAB-Map的所有可以

设置的参数，加上⼀些描述，执⾏这个命令：rosrun rtabmap_ros rtabmap --params

以下是此处设置的主要参数的简要概述：

RGBD/NeighborLinkRefining 里程计 ：使⽤ ICP 使⽤输⼊激光雷达主题校正 。

RGBD/ProximityBySpace ：根据地图中的机器⼈位置查找局部闭环。 例如，当机器⼈以相反的方向返回时，它很有⽤。 相机朝后，⽆法找到全局闭环。 因此，使⽤位置和先前添加到地图的激光扫描，我们使⽤ ICP 找到变换。

RGBD/AngularUpdate ：机器⼈应该移动以更新地图（如果不是 0）。

RGBD/LinearUpdate ：机器⼈应该移动以更新地图（如果不是 0）。RGBD/OptimizeFromGraphEnd ：通过设置为 false（这是默认值），在循环闭包时，将从地图中的第⼀个姿势开始优化图形。 ，TF /map -> /odom 发⽣这种情况时 会发⽣变化。 当设置为 false时，图形从添加到地图的最新节点而不是第⼀个节点进⾏优化。 通过从最后⼀个优化，最后⼀个姿势保持其值，所有先前的姿势都根据它进⾏校正（因此 /odom 和 /map 将始终匹配在⼀起）。

Grid/FromDepth ：如果为 true，则占⽤⽹格是从深度相机⽣成的云中创建的。 如果为 false，则根据激光扫描创建占⽤⽹格。

Reg/Force3DoF ：强制 3DoF 注册： roll 、 pitch 和 z 不会估计 。

Reg/Strategy ：我们选择 ICP 来改进使⽤激光扫描使⽤ ICP 发现的全局闭环。

Icp/VoxelSize ：在做 ICP 之前，扫描被过滤到 5 厘⽶的体素。

Icp/MaxCorrespondenceDistance : ICP 注册期间点之间的最⼤距离。

无雷达，通过深度相机模拟得到

视觉+雷达，IPC模拟得到里程计‘

只有视觉，视觉里程计

这里只是介绍了一种轮式编码器+陀螺仪+雷达+深度相机的融合，在机器人常用的传感器还有超声波、GPS等等，这部分可以使用robot_localization功能包来实现，从而达到更加精准的定位效果。（前段时间见到有大佬用这个进行双陀螺仪，确实很秀！）