如何使用深度相機實現(xiàn)視覺slam建圖和導(dǎo)航

一、 概述

1.深度相機

隨著機器視覺，自動駕駛等顛覆性的技術(shù)逐步發(fā)展，采用3D相機進行物體識別，行為識別，場景建模的相關(guān)應(yīng)用越來越多，可以說深度相機就是終端和機器人的眼睛，那么什么是深度相機呢，跟之前的普通相機（2D）想比較，又有哪些差別？深度相機又稱之為3D相機，顧名思義，就是通過該相機能檢測出拍攝空間的景深距離，這也是與普通攝像頭最大的區(qū)別。

普通的彩色相機拍攝到的圖片能看到相機視角內(nèi)的所有物體并記錄下來，但是其所記錄的數(shù)據(jù)不包含這些物體距離相機的距離。僅僅能通過圖像的語義分析來判斷哪些物體離我們比較遠，哪些比較近，但是并沒有確切的數(shù)據(jù)。而深度相機則恰恰解決了該問題，通過深度相機獲取到的數(shù)據(jù)，我們能準(zhǔn)確知道圖像中每個點離攝像頭距離，這樣加上該點在2D圖像中的(x,y)坐標(biāo)，就能獲取圖像中每個點的三維空間坐標(biāo)。通過三維坐標(biāo)就能還原真實場景，實現(xiàn)場景建模等應(yīng)用。

深度相機也具備普通相機的一些不足，容易受視差影響：包括黑色物體（特征點少）、透明物體（光的穿透）、光滑物體（光反射太強）、圖像無紋理(特征點少)、過度曝光(特征點少)等。

2.常見的深度相機

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

2.1 雙目視覺(Stereo)

雙目立體視覺(Binocular Stereo Vision)是機器視覺的一種重要形式，他是基于視差原理并利用成像設(shè)備從不同的位置獲取被測物體的兩幅圖像，通過計算圖像對應(yīng)點間的位置偏差，來獲取物體三維幾何信息的方法。

雙目相機的主要優(yōu)點有：

1）硬件要求低，成本也低。普通 CMOS 相機即可

2）室內(nèi)外都適用。只要光線合適，不要太昏暗

但是雙目的缺點也是非常明顯：

1）對環(huán)境光照非常敏感。光線變化導(dǎo)致圖像偏差大，進而會導(dǎo)致匹配失敗或精度低

2）不適用單調(diào)缺乏紋理的場景。雙目視覺根據(jù)視覺特征進行圖像匹配，沒有特征會導(dǎo)致匹配失敗

3）計算復(fù)雜度高，需要很高的計算資源。該方法是純視覺的方法，對算法要求高，實時性差

4）基線限制了測量范圍。測量范圍和基線（兩個攝像頭間距）成正比，導(dǎo)致無法小型化

代表公司：

Leap Motion， ZED， 大疆

2.2 結(jié)構(gòu)光(Structured-light)

通過近紅外激光器，將具有一定結(jié)構(gòu)特征的光線投射到被拍攝物體上，再由專門的紅外攝像頭進行采集。這種具備一定結(jié)構(gòu)的光線，會因被攝物體的不同深度區(qū)域，而采集不同的圖像相位信息，然后通過運算單元將這種結(jié)構(gòu)的變化換算成深度信息，以此來獲得三維結(jié)構(gòu)。簡單來說就是，通過光學(xué)手段獲取被拍攝物體的三維結(jié)構(gòu)，再將獲取到的信息進行更深入的應(yīng)用。通常采用特定波長的不可見的紅外激光作為光源，它發(fā)射出來的光經(jīng)過 一定的編碼投影在物體上，通過一定算法來計算返回的編碼圖案的畸變來得到物體的位置和深度信息。下圖是一個典型的結(jié)構(gòu)光相機的示意圖:

結(jié)構(gòu)光（散斑）的優(yōu)點主要有：

1）方案成熟，相機基線可以做的比較小，方便小型化。

2）資源消耗較低，單幀 IR 圖就可計算出深度圖，功耗低。

3）主動光源，夜晚也可使用。

4）在一定范圍內(nèi)精度高，分辨率高，分辨率可達 1280×1024,幀率可達 60FPS。

散斑結(jié)構(gòu)光的缺點與結(jié)構(gòu)光類似：

1）容易受環(huán)境光干擾，室外體驗差。

2）隨檢測距離增加，精度會變差。

代表公司：

奧比中光，蘋果(Prime Sense)，微軟 Kinect-1，英特爾 RealSense, Mantis Vision 等。

2.3 光飛行時間法(TOF)

顧名思義是測量光飛行時間來取得距離，具體而言就是通過給目標(biāo)連續(xù)發(fā)射激光脈沖，然后用傳感器 接收從反射光線，通過探測光脈沖的飛行往返時間來得到確切的目標(biāo)物距離。因為光速激光，通過直接測 光飛行時間實際不可行，一般通過檢測通過一定手段調(diào)制后的光波的相位偏移來實現(xiàn)。TOF 法根據(jù)調(diào)制方法的不同，一般可以分為兩種：脈沖調(diào)制（Pulsed Modulation）和連續(xù)波調(diào)制 （Continuous Wave Modulation）。脈沖調(diào)制需要非常高精度時鐘進行測量，且需要發(fā)出高頻高強度激光，目 前大多采用檢測相位偏移辦法來實現(xiàn) TOF 功能。簡單來說就是，發(fā)出一道經(jīng)過處理的光，碰到物體以后會反射回來，捕捉來回的時間，因為已知光速和調(diào)制光的波長，所以能快速準(zhǔn)確計算出到物體的距離。

其原理示意圖：

因為 TOF 并非基于特征匹配，這樣在測試距離變遠時，精度也不會下降很快，目前無人駕駛以及一些高端的消費類 Lidar 基本都是采用該方法來實現(xiàn)。

TOF 的優(yōu)點主要有：

1）檢測距離遠。在激光能量夠的情況下可達幾十米

2）受環(huán)境光干擾比較小

但是 TOF 也有一些顯而易見的問題：

1）對設(shè)備要求高，特別是時間測量模塊。

2）資源消耗大。該方案在檢測相位偏移時需要多次采樣積分，運算量大。

3）邊緣精度低。

4）限于資源消耗和濾波，幀率和分辨率都沒辦法做到較高。

代表公司：

微軟 Kinect-2，PMD，SoftKinect， 聯(lián)想 Phab。

傳感器技術(shù)不是很成熟，因此，分辨率較低，成本高，但由于其原理與另外兩種完全不同，實時性高，不需要額外增加計算資源，幾乎無算法開發(fā)工作量，是未來。

2.4 綜合對比

從上面三種主流的 3D 相機成像方案來看，各有優(yōu)劣，但是從實際應(yīng)用場景來看，在非無人駕駛域，結(jié)構(gòu)光，特別是散斑結(jié)構(gòu)光的用途是最廣泛。因為從精度，分辨率，還有應(yīng)用場景的范圍來看雙目和 TOF 都沒有辦法做到最大的平衡。而且對于結(jié)構(gòu)光容易受環(huán)境光干擾，特別是太陽光影響問題，鑒于此類相機都有紅外激光發(fā)射模塊，非常容易改造為主動雙目來彌補該問題。

結(jié)構(gòu)光與TOF的對比：

對比來看，結(jié)構(gòu)光技術(shù)功耗更小，技術(shù)更成熟，更適合靜態(tài)場景。而TOF方案在遠距離下噪聲較低，同時擁有更高的FPS，因此更適合動態(tài)場景。

目前，結(jié)構(gòu)光技術(shù)主要應(yīng)用于解鎖以及安全支付等方面，其應(yīng)用距離受限。而TOF技術(shù)主要用于智能機后置攝影，并在AR、VR等領(lǐng)域（包括3D拍照、體感游戲等）有一定的作用。

3D結(jié)構(gòu)光和TOF兩者其實各有優(yōu)劣勢。結(jié)構(gòu)光最大的優(yōu)勢是發(fā)展的較為成熟，成本比較低，劣勢是只適合中短距離使用。ToF優(yōu)勢是抗干擾性較好，視角較寬，缺陷是功耗高，造價貴，精度及深度圖分辨率較低。兩項技術(shù)各有側(cè)重點和適配使用場景。

結(jié)構(gòu)光相機硬件說明：

綜合考慮成本，本文將采用此類相機，在這里單獨說明下。

Kinect（xbox游戲機配件）：

樂視體感相機（樂視tv配件）：

相機一般有三只眼睛，從左到右分別為：

投射紅外線pattern的IR Projector（左）

攝像頭Color Camera（中）

讀取反射紅外線的IR Camera（右）

Depth傳感器讀取左右兩邊投射的紅外線pattern，通過pattern的變形來取得Depth的信息。

二、 攝像頭通路打通

1.硬件連接

考慮成本，我們在上一次組裝的ROS+底盤套裝的基礎(chǔ)上，新增一個深度相機即可：

樂視體感三合一攝像頭 * 1

usb子母延長線 * 1

2.安裝驅(qū)動

# 安裝相關(guān)依賴 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相關(guān)依賴 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 # 創(chuàng)建udev規(guī)則 cd ros_astra_camera chmod +x ./scripts/create_udev_rules ./scripts/create_udev_rules # 配置并加載環(huán)境變量(機器人端執(zhí)行) 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.機器人端啟動攝像頭(啟動后會彈出幾個警告，不用管它，這是由于驅(qū)動中的一些參數(shù)攝像頭不支持) #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...，是因為沒有權(quán)限，執(zhí)行以下命令 #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 中顯示會是全黑，這和圖像格式有關(guān)，并非設(shè)備問題) rviz 或 rqt_image_view

4.點云顯示

這一部分為了讓大家了解一下 rviz 中的各類插件的使用，將帶著大家一起搭建一個 rviz 顯示環(huán)境。（如果不想進行手動配置，也可以直接使用預(yù)先配置好的文件：/robot_vslam/rviz/astrapro.rviz）

首先我們添加一個點云顯示插件，點擊”Add”，在彈出的界面中雙擊” PointCloud2”：

展開左側(cè)PointCloud2選型卡，選擇Topic：

將 Fixed Frame 選擇為 camera_link：

此時界面中就會顯示出點云圖：

在 Color Transformer 中選擇不同的顏色模式可以給點云按照不同的規(guī)則著色：

最后通過左上角的”File”選項卡中 Save Config As 可以將當(dāng)前 rviz 配置保存到指定位置，下次打開 rviz 候可以通過 Open Config 直接打開配置文件避免再次手動配置界面。

5.轉(zhuǎn)激光雷達數(shù)據(jù)測試

# 啟動深度相機（機器人端） roslaunch robot_vslam camera.launch # 啟動深度圖到雷達數(shù)據(jù)的轉(zhuǎn)換 roslaunch robot_vslam depth_to_laser.launch # pc端可視化 rviz

配置一下界面就可以正常顯示雷達數(shù)據(jù)了

這里可以注意到，相對于機器人的 TF 坐標(biāo)位置，雷達數(shù)據(jù)并不是像我們常見的激光雷達數(shù)據(jù)是 360 度的，這是由于深度相機的視場角(即 FOV，我們這款相機水平 FOV 為 58.4 度，垂直FOV 為 45.5 度)是有限的，所以轉(zhuǎn)換出的雷達數(shù)據(jù)角度范圍和相機是水平 FOV是一致的，簡單理解就是，只能看到相機前方一定角度范圍內(nèi)的東西。

三、 視覺 vSLAM 建圖及導(dǎo)航

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 節(jié)點和 rtabmap 的節(jié)點中會分別輸出以下信息(因為這里輸出的信息較多且有參考價值，所以我們沒有將所有的啟動文件寫成一個 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,旋轉(zhuǎn)速度不超過 0.4rad/s 以保證建圖質(zhì)量。建圖過程和激光雷達建圖基本一致，遙控機器人在環(huán)境中遍歷一遍完成建圖。

地圖是默認(rèn)保存在機器人端~/.ros/rtabmap.db文件中，由于這個文件包含了建圖過程中的圖片和特征點的信息，所以文件會比較大。

注意:啟動 roslaunch robot_vslam rtabmap_rgbd.launch 都會覆蓋掉 rtabmap.db 文件，如果需要保留上次建圖的結(jié)果，注意備份文件。

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

這里我們也可以修改點云顯示的顏色來實現(xiàn)其他效果，例如這樣

2.導(dǎo)航

機器人端打開四個終端，分別運行:

# 啟動底盤base control roslaunch ros_arduino_python arduino.launch # 啟動camera roslaunch robot_vslam camera.launch # 啟動rtab并自動定位 roslaunch robot_vslam rtabmap_rgbd.launch localization:=true # 啟動movebase（純前向視覺無法后退） roslaunch robot_vslam move_base.launch planner:=dwa move_forward_only:=true

這里 move_base 的 planner 參數(shù)可以選擇 dwa 和 teb，這部分在激光雷達導(dǎo)航部分已經(jīng)介紹過，這里就不再贅述，move_forward_only 參數(shù)是為了只允許機器人向前運動(視覺傳感器只有前向感知) 。啟動后檢查各個節(jié)點有無報錯，正常應(yīng)該是可以直接定位成功的。

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

# pc端rtabmap查看（需手動下載全局地圖） roslaunch robot_vslam rtabmapviz.launch

在 rtabmapviz 中點擊紅框中的按鈕同步地圖數(shù)據(jù)庫的信息，可能會花費一點時間，取決于你的局域網(wǎng)條件。如果左下位置中沒有出現(xiàn)圖像且圖像兩側(cè)處為黑色，則機器人尚未完成初始位置的確定。如果如下圖所示，則機器人已經(jīng)完成定位，可以開始導(dǎo)航無需執(zhí)行下面的步驟(通常機器人從紋理比較豐富且上次已經(jīng)到達過的位置啟動，都可以直接完成定位，這里文檔中是為了說明初始定位這一過程故意從一處幾乎沒有紋理的區(qū)域啟動使機器人不能完成初始定位)

如果沒有自動定位成功，則可以在 PC 端或機器人端啟動一個鍵盤控制節(jié)點：

rosrun teleop_twist_keyboard teleop_twist_keyboard.py

控制慢速移動機器人，直到左下角位置出現(xiàn)圖像且兩側(cè)為綠色表示定位成功

接下來就可以在地圖中指定目標(biāo)點讓機器人導(dǎo)航過去了。

注意：rgbd 相機的視場角較小，且深度信息存在較大的盲區(qū)，在導(dǎo)航中有可能會發(fā)生碰撞，可以通過調(diào)節(jié)增大 robot_vslam/param/MarsRobot/costmap_common_params.yaml中的inflation_radius 參數(shù)來改善，但是很難達到和激光雷達導(dǎo)航或rgbd相機+激光雷達的效果。

四、視覺+激光雷達建圖及導(dǎo)航

1.視覺+激光雷達建圖

這里操作過程和第三章節(jié)完全一致，只是部分啟動指令有差異，所以這里只列出啟動指令，不再描述操作過程。
機器人端打開三個終端，分別運行:

# 啟動激光雷達+base_control roslaunch robot_navigation robot_lidar.launch # 啟動深度攝像頭 roslaunch robot_vslam camera.launch # 啟動rgbd+lidar聯(lián)合建圖 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,旋轉(zhuǎn)速度不超過 0.4rad/s 以保證建圖質(zhì)量建圖過程和激光雷達建圖基本一致，遙控機器人在環(huán)境中遍歷一遍完成建圖。這里可以看到，由于加入了360 度的激光雷達數(shù)據(jù)，建圖在 2D 的地圖方面表現(xiàn)要更好一些。

2.視覺vSLAM +激光雷達導(dǎo)航

機器人端打開四個終端，分別運行:

# 啟動激光雷達+base_control roslaunch robot_navigation robot_lidar.launch # 啟動camera roslaunch robot_vslam camera.launch # 啟動rtab并自動定位 roslaunch robot_vslam rtabmap_rgbd_lidar.launch localization:=true # 啟動movebase（允許后退） roslaunch robot_vslam move_base.launch planner:=dwa move_forward_only:=false # pc端查看 roslaunch robot_vslam rtabmap_rviz.launch roslaunch robot_vslam rtabmapviz.launch

這里由于加入了 360 度的激光雷達，機器人具備了全向的感知能力，所以可以將 move_forward_only 參數(shù)設(shè)置為 false 允許機器人向后運動。

今天帶大家學(xué)習(xí)了如何使用深度相機實現(xiàn)視覺slam建圖和導(dǎo)航，同時我們也嘗試了視覺+激光雷達的協(xié)同使用，機器人在障礙物感知和避障方面有了明顯改善，同時通過攝像頭我們也能更方便的實現(xiàn)遠程監(jiān)控和控制。下一次你想看到什么呢？歡迎留言，我將帶大家一起實現(xiàn)～！

審核編輯 ：李倩