上一次，我们实现了激光雷达+自研底盘的SLAM建图和导航。这次将带大家学习如何使用深度相机实现机器人的视觉SLAM建图及导航。

一、 概述

1.深度相机

随着机器视觉，自动驾驶等颠覆性的技术逐步发展，采用3D相机进行物体识别，行为识别，场景建模的相关应用越来越多，可以说深度相机就是终端和机器人的眼睛，那么什么是深度相机呢，跟之前的普通相机（2D）想比较，又有哪些差别？深度相机又称之为3D相机，顾名思义，就是通过该相机能检测出拍摄空间的景深距离，这也是与普通摄像头最大的区别。

普通的彩色相机拍摄到的图片能看到相机视角内的所有物体并记录下来，但是其所记录的数据不包含这些物体距离相机的距离。仅仅能通过图像的语义分析来判断哪些物体离我们比较远，哪些比较近，但是并没有确切的数据。而深度相机则恰恰解决了该问题，通过深度相机获取到的数据，我们能准确知道图像中每个点离摄像头距离，这样加上该点在2D图像中的(x,y)坐标，就能获取图像中每个点的三维空间坐标。通过三维坐标就能还原真实场景，实现场景建模等应用。

深度相机也具备普通相机的一些不足，容易受视差影响：包括黑色物体（特征点少）、透明物体（光的穿透）、光滑物体（光反射太强）、图像无纹理(特征点少)、过度曝光(特征点少)等。

2.常见的深度相机

目前市面上常有的深度相机方案有以下三种：

2.1 双目视觉(Stereo)

双目立体视觉(Binocular Stereo Vision)是机器视觉的一种重要形式，他是基于视差原理并利用成像设备从不同的位置获取被测物体的两幅图像，通过计算图像对应点间的位置偏差，来获取物体三维几何信息的方法。

双目相机的主要优点有：

• 1）硬件要求低，成本也低。普通 CMOS 相机即可
• 2）室内外都适用。只要光线合适，不要太昏暗

但是双目的缺点也是非常明显：

• 1）对环境光照非常敏感。光线变化导致图像偏差大，进而会导致匹配失败或精度低
• 2）不适用单调缺乏纹理的场景。双目视觉根据视觉特征进行图像匹配，没有特征会导致匹配失败
• 3）计算复杂度高，需要很高的计算资源。该方法是纯视觉的方法，对算法要求高，实时性差
• 4）基线限制了测量范围。测量范围和基线（两个摄像头间距）成正比，导致无法小型化

代表公司：

• Leap Motion， ZED， 大疆

2.2 结构光(Structured-light)

通过近红外激光器，将具有一定结构特征的光线投射到被拍摄物体上，再由专门的红外摄像头进行采集。这种具备一定结构的光线，会因被摄物体的不同深度区域，而采集不同的图像相位信息，然后通过运算单元将这种结构的变化换算成深度信息，以此来获得三维结构。简单来说就是，通过光学手段获取被拍摄物体的三维结构，再将获取到的信息进行更深入的应用。通常采用特定波长的不可见的红外激光作为光源，它发射出来的光经过 一定的编码投影在物体上，通过一定算法来计算返回的编码图案的畸变来得到物体的位置和深度信息。下图是一个典型的结构光相机的示意图:

结构光（散斑）的优点主要有：

• 1）方案成熟，相机基线可以做的比较小，方便小型化。
• 2）资源消耗较低，单帧 IR 图就可计算出深度图，功耗低。
• 3）主动光源，夜晚也可使用。
• 4）在一定范围内精度高，分辨率高，分辨率可达 1280×1024,帧率可达 60FPS。

散斑结构光的缺点与结构光类似：

• 1）容易受环境光干扰，室外体验差。
• 2）随检测距离增加，精度会变差。

代表公司：

• 奥比中光，苹果(Prime Sense)，微软 Kinect-1，英特尔 RealSense, Mantis Vision 等。

2.3 光飞行时间法(TOF)

顾名思义是测量光飞行时间来取得距离，具体而言就是通过给目标连续发射激光脉冲，然后用传感器 接收从反射光线，通过探测光脉冲的飞行往返时间来得到确切的目标物距离。因为光速激光，通过直接测 光飞行时间实际不可行，一般通过检测通过一定手段调制后的光波的相位偏移来实现。 TOF 法根据调制方法的不同，一般可以分为两种：脉冲调制（Pulsed Modulation）和连续波调制 （Continuous Wave Modulation）。脉冲调制需要非常高精度时钟进行测量，且需要发出高频高强度激光，目 前大多采用检测相位偏移办法来实现 TOF 功能。简单来说就是，发出一道经过处理的光，碰到物体以后会反射回来，捕捉来回的时间，因为已知光速和调制光的波长，所以能快速准确计算出到物体的距离。

其原理示意图：

因为 TOF 并非基于特征匹配，这样在测试距离变远时，精度也不会下降很快，目前无人驾驶以及一些高端的消费类 Lidar 基本都是采用该方法来实现。

TOF 的优点主要有：

• 1）检测距离远。在激光能量够的情况下可达几十米
• 2）受环境光干扰比较小

但是 TOF 也有一些显而易见的问题：

• 1）对设备要求高，特别是时间测量模块。
• 2）资源消耗大。 该方案在检测相位偏移时需要多次采样积分，运算量大。
• 3）边缘精度低。
• 4）限于资源消耗和滤波，帧率和分辨率都没办法做到较高。

代表公司：

• 微软 Kinect-2，PMD，SoftKinect， 联想 Phab。
• • 传感器技术不是很成熟，因此，分辨率较低，成本高，但由于其原理与另外两种完全不同，实时性高，不需要额外增加计算资源，几乎无算法开发工作量，是未来。

2.4 综合对比

从上面三种主流的 3D 相机成像方案来看，各有优劣，但是从实际应用场景来看，在非无人驾驶域，结构光，特别是散斑结构光的用途是最广泛。因为从精度，分辨率，还有应用场景的范围来看双目和 TOF 都没有办法做到最大的平衡。而且对于结构光容易受环境光干扰，特别是太阳光影响问题，鉴于此类相机都有红外激光发射模块，非常容易改造为主动双目来弥补该问题。

结构光与TOF的对比：

对比来看，结构光技术功耗更小，技术更成熟，更适合静态场景。而TOF方案在远距离下噪声较低，同时拥有更高的FPS，因此更适合动态场景。

目前，结构光技术主要应用于解锁以及安全支付等方面，其应用距离受限。而TOF技术主要用于智能机后置摄影，并在AR、VR等领域（包括3D拍照、体感游戏等）有一定的作用。

3D结构光和TOF两者其实各有优劣势。结构光最大的优势是发展的较为成熟，成本比较低，劣势是只适合中短距离使用。ToF优势是抗干扰性较好，视角较宽，缺陷是功耗高，造价贵，精度及深度图分辨率较低。两项技术各有侧重点和适配使用场景。

结构光相机硬件说明：

综合考虑成本，本文将采用此类相机，在这里单独说明下。

Kinect（xbox游戏机配件）：

乐视体感相机（乐视tv配件）：

相机一般有三只眼睛，从左到右分别为：

• 投射红外线pattern的IR Projector（左）
• 摄像头Color Camera（中）
• 读取反射红外线的IR Camera（右）

Depth传感器读取左右两边投射的红外线pattern，通过pattern的变形来取得Depth的信息。

二、 摄像头通路打通

1.硬件连接

考虑成本，我们在上一次组装的ROS+底盘套装的基础上，新增一个深度相机即可：

• 乐视体感三合一摄像头 * 1
• usb子母延长线          * 1

2.安装驱动

# 安装相关依赖
sudo apt install ros-$ROS_DISTRO-rgbd-launch ros-$ROS_DISTRO-libuvc-camera ros-$ROS_DISTRO-libuvc-ros ros-$ROS_DISTRO-camera-info-manager   # ros-$ROS_DISTRO-libuvc 
# 安装vslam相关依赖
sudo apt install ros-$ROS_DISTRO-rtabmap ros-$ROS_DISTRO-rtabmap-ros ros-$ROS_DISTRO-depthimage-to-laserscan

# 克隆功能包源码
cd ~/catkin_ws/src
git clone git@github.com:orbbec/ros_astra_camera.git
git clone git@github.com:yanjingang/robot_vslam.git

# 创建udev规则
cd ros_astra_camera
chmod +x ./scripts/create_udev_rules
./scripts/create_udev_rules
# 配置并加载环境变量(机器人端执行)
cd ~/catkin_ws/src
./robot_vslam/script/setrgbd.sh astrapro
source ~/.bashrc          # export CAMERA_TYPE=astrapro

# 编译
cd ~/catkin_ws
catkin_make --pkg astra_camera robot_vslam
source ~/catkin_ws/devel/setup.bash

3.测试摄像头

在完成第一步的功能包编译之后，将摄像头连接到机器人的主机

# 0.检查摄像头是否正常识别
ll /dev/astra*
  lrwxrwxrwx 1 root root 15 Jan  6 12:50 /dev/astra_pro -> bus/usb/001/014
  lrwxrwxrwx 1 root root 15 Jan  6 12:50 /dev/astrauvc -> bus/usb/001/015

# 1.机器人端启动摄像头(启动后会弹出几个警告，不用管它，这是由于驱动中的一些参数摄像头不支持)
#roslaunch astra_camera astra.launch  # Use Astra
roslaunch robot_vslam astrapro.launch
# 如果报Warning: USB events thread - failed to set priority. This might cause loss of data...，是因为没有权限，执行以下命令
#sudo -s
#source /home/work/.bashrc
#roslaunch robot_vslam astrapro.launch

# 2.PC 端查看图像(常用topic：深度：/camera/depth/image；彩色：/camera/rgb/image_raw/compressed；红外：/camera/ir/image；注:ir图像只能通过rviz显示，在rqt_image_view 中显示会是全黑，这和图像格式有关，并非设备问题)
rviz  或 rqt_image_view

4.点云显示

这一部分为了让大家了解一下 rviz 中的各类插件的使用，将带着大家一起搭建一个 rviz 显示环境。（如果不想进行手动配置，也可以直接使用预先配置好的文件：/robot_vslam/rviz/astrapro.rviz）

首先我们添加一个点云显示插件，点击”Add”，在弹出的界面中双击” PointCloud2”：

展开左侧PointCloud2选型卡，选择Topic：

将 Fixed Frame 选择为 camera_link：

此时界面中就会显示出点云图：

在 Color Transformer 中选择不同的颜色模式可以给点云按照不同的规则着色：

最后通过左上角的”File”选项卡中 Save Config As 可以将当前 rviz 配置保存到指定位置，下次打开 rviz 候可以通过 Open Config 直接打开配置文件避免再次手动配置界面。

5.转激光雷达数据测试

# 启动深度相机（机器人端）
roslaunch robot_vslam camera.launch
# 启动深度图到雷达数据的转换 (！初次安装相机驱动的板子，需要重启下pi这个功能才能生效！)
roslaunch robot_vslam depth_to_laser.launch

# pc端可视化
rviz

三、 视觉 vSLAM 建图及导航

1.vSLAM建图

机器人端启动建图：

# 启动底盘base control
roslaunch ros_arduino_python arduino.launch
# 启动camera
roslaunch robot_vslam camera.launch
# 启动rtab vslam
roslaunch robot_vslam rtabmap_rgbd.launch

在 rtabmap_rgbd.launch 启动完成后，camera 节点和 rtabmap 的节点中会分别输出以下信息(因为这里输出的信息较多且有参考价值，所以我们没有将所有的启动文件写成一个 launch 文件在同一个终端中输出)

PC端查看建图过程：

# PC端打开rviz终端(在rviz终端中就可以看到所建立的平面地图和立体地图)
roslaunch robot_vslam rtabmap_rviz.launch
# 也可以使用rtabmap自带的图形化界面rtabmapviz在PC端来观测建图过程（不建议，带宽占用100M以上）
# roslaunch robot_vslam rtabmapviz.launch

键盘控制机器人移动：

# 键盘控制
rosrun teleop_twist_keyboard teleop_twist_keyboard.py

我们建议是将机器人的直线运动速度不超过 0.2m/s,旋转速度不超过 0.4rad/s 以保证建图质量。建图过程和激光雷达建图基本一致，遥控机器人在环境中遍历一遍完成建图。

地图是默认保存在机器人端~/.ros/rtabmap.db 文件中，由于这个文件包含了建图过程中的图片和特征点的信息，所以文件会比较大。

注意:启动 roslaunch robot_vslam rtabmap_rgbd.launch 都会覆盖掉 rtabmap.db 文件，如果需要保留上次建图的结果，注意备份文件。

建图完成后 rviz 中的地图如下所示

这里我们也可以修改点云显示的颜色来实现其他效果，例如这样

2.导航

机器人端打开四个终端，分别运行:

# 启动底盘base control
roslaunch ros_arduino_python arduino.launch
# 启动camera
roslaunch robot_vslam camera.launch
# 启动rtab+movebase并自动定位（纯前向视觉无法后退）
roslaunch robot_vslam rtabmap_rgbd.launch localization:=true planner:=dwa move_forward_only:=true

这里 move_base 的 planner 参数可以选择 dwa 和 teb，这部分在激光雷达导航部分已经介绍过，这里就不再赘述，move_forward_only 参数是为了只允许机器人向前运动(视觉传感器只有前向感知) 。启动后检查各个节点有无报错，正常应该是可以直接定位成功的。

# pc端rviz查看和控制
roslaunch robot_vslam rtabmap_rviz.launch

# pc端rtabmap查看（需手动下载全局地图）
roslaunch robot_vslam rtabmapviz.launch

在 rtabmapviz 中点击红框中的按钮同步地图数据库的信息，可能会花费一点时间，取决于你的局域网条件。 如果左下位置中没有出现图像且图像两侧处为黑色，则机器人尚未完成初始位置的确定。如果如下图所示，则机器人已经完成定位，可以开始导航无需执行下面的步骤(通常机器人从纹理比较丰富且上次已经到达过的位置启动，都可以直接完成定位，这里文档中是为了说明初始定位这一过程故意从一处几乎没有纹理的区域启动使机器人不能完成初始定位)

如果没有自动定位成功，则可以在 PC 端或机器人端启动一个键盘控制节点：

rosrun teleop_twist_keyboard teleop_twist_keyboard.py

控制慢速移动机器人，直到左下角位置出现图像且两侧为绿色表示定位成功

接下来就可以在地图中指定目标点让机器人导航过去了。

注意：rgbd 相机的视场角较小，且深度信息存在较大的盲区，在导航中有可能会发生碰撞，可以通过调节增大 robot_vslam/param/MarsRobot/costmap_common_params.yaml中的inflation_radius 参数来改善，但是很难达到和激光雷达导航或rgbd相机+激光雷达的效果。

四、视觉+激光雷达建图及导航

1.视觉+激光雷达建图

这里操作过程和第三章节完全一致，只是部分启动指令有差异，所以这里只列出启动指令，不再描述操作过程。
机器人端打开三个终端，分别运行:

# 启动激光雷达+base_control
roslaunch robot_navigation robot_lidar.launch 
# 启动深度摄像头
roslaunch robot_vslam camera.launch 
# 启动rgbd+lidar联合建图
roslaunch robot_vslam rtabmap_rgbd_lidar.launch

# pc端查看
roslaunch robot_vslam rtabmap_rviz.launch
# pc端控制移动
rosrun teleop_twist_keyboard teleop_twist_keyboard.py

我们建议是将机器人的直线运动速度不超过 0.2m/s,旋转速度不超过 0.4rad/s 以保证建图质量建图过程和激光雷达建图基本一致，遥控机器人在环境中遍历一遍完成建图。这里可以看到，由于加入了360 度的激光雷达数据，建图在 2D 的地图方面表现要更好一些。

2.视觉vSLAM +激光雷达导航

机器人端打开四个终端，分别运行:

# 启动激光雷达+base_control
roslaunch robot_navigation robot_lidar.launch 
# 启动camera 
roslaunch robot_vslam camera.launch 
# 启动rtab+movebase并自动定位（允许后退） 
roslaunch robot_vslam rtabmap_rgbd_lidar.launch localization:=true planner:=dwa move_forward_only:=false

# pc端查看
roslaunch robot_vslam rtabmap_rviz.launch
roslaunch robot_vslam rtabmapviz.launch

这里由于加入了 360 度的激光雷达，机器人具备了全向的感知能力，所以可以将 move_forward_only 参数设置为 false 允许机器人向后运动。今天带大家学习了如何使用深度相机实现视觉slam建图和导航，同时我们也尝试了视觉+激光雷达的协同使用，机器人在障碍物感知和避障方面有了明显改善，同时通过摄像头我们也能更方便的实现远程监控和控制。下一次你想看到什么呢？欢迎留言，我将带大家一起实现～！