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

一、 概述

1.深度相機(jī)

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

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

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

2.常見(jiàn)的深度相機(jī)

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

2.1 雙目視覺(jué)(Stereo)

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

雙目相機(jī)的主要優(yōu)點(diǎn)有：

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

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

但是雙目的缺點(diǎn)也是非常明顯：

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

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

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

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

代表公司：

Leap Motion， ZED， 大疆

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

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

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

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

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

3）主動(dòng)光源，夜晚也可使用。

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

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

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

2）隨檢測(cè)距離增加，精度會(huì)變差。

代表公司：

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

2.3 光飛行時(shí)間法(TOF)

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

其原理示意圖：

因?yàn)?TOF 并非基于特征匹配，這樣在測(cè)試距離變遠(yuǎn)時(shí)，精度也不會(huì)下降很快，目前無(wú)人駕駛以及一些高端的消費(fèi)類 Lidar 基本都是采用該方法來(lái)實(shí)現(xiàn)。

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

1）檢測(cè)距離遠(yuǎn)。在激光能量夠的情況下可達(dá)幾十米

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

但是 TOF 也有一些顯而易見(jiàn)的問(wèn)題：

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

2）資源消耗大。該方案在檢測(cè)相位偏移時(shí)需要多次采樣積分，運(yùn)算量大。

3）邊緣精度低。

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

代表公司：

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

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

2.4 綜合對(duì)比

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

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

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

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

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

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

綜合考慮成本，本文將采用此類相機(jī)，在這里單獨(dú)說(shuō)明下。

Kinect（xbox游戲機(jī)配件）：

樂(lè)視體感相機(jī)（樂(lè)視tv配件）：

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

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

攝像頭Color Camera（中）

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

Depth傳感器讀取左右兩邊投射的紅外線pattern，通過(guò)pattern的變形來(lái)取得Depth的信息。

二、 攝像頭通路打通

1.硬件連接

考慮成本，我們?cè)谏弦淮谓M裝的ROS+底盤套裝的基礎(chǔ)上，新增一個(gè)深度相機(jī)即可：

樂(lè)視體感三合一攝像頭 * 1

usb子母延長(zhǎng)線 * 1

2.安裝驅(qū)動(dòng)

# 安裝相關(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)境變量(機(jī)器人端執(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.測(cè)試攝像頭

在完成第一步的功能包編譯之后，將攝像頭連接到機(jī)器人的主機(jī)

# 0.檢查攝像頭是否正常識(shí)別 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.機(jī)器人端啟動(dòng)攝像頭(啟動(dòng)后會(huì)彈出幾個(gè)警告，不用管它，這是由于驅(qū)動(dòng)中的一些參數(shù)攝像頭不支持) #roslaunch astra_camera astra.launch # Use Astra roslaunch robot_vslam astrapro.launch # 如果報(bào)Warning: USB events thread - failed to set priority. This might cause loss of data...，是因?yàn)闆](méi)有權(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圖像只能通過(guò)rviz顯示，在rqt_image_view 中顯示會(huì)是全黑，這和圖像格式有關(guān)，并非設(shè)備問(wèn)題) rviz 或 rqt_image_view

4.點(diǎn)云顯示

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

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

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

將 Fixed Frame 選擇為 camera_link：

此時(shí)界面中就會(huì)顯示出點(diǎn)云圖：

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

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

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

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

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

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

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

1.vSLAM建圖

機(jī)器人端啟動(dòng)建圖：

# 啟動(dòng)底盤base control roslaunch ros_arduino_python arduino.launch # 啟動(dòng)camera roslaunch robot_vslam camera.launch # 啟動(dòng)rtab vslam roslaunch robot_vslam rtabmap_rgbd.launch

在 rtabmap_rgbd.launch 啟動(dòng)完成后，camera 節(jié)點(diǎn)和 rtabmap 的節(jié)點(diǎn)中會(huì)分別輸出以下信息(因?yàn)檫@里輸出的信息較多且有參考價(jià)值，所以我們沒(méi)有將所有的啟動(dòng)文件寫成一個(gè) launch 文件在同一個(gè)終端中輸出)

PC端查看建圖過(guò)程：

# PC端打開rviz終端(在rviz終端中就可以看到所建立的平面地圖和立體地圖) roslaunch robot_vslam rtabmap_rviz.launch # 也可以使用rtabmap自帶的圖形化界面rtabmapviz在PC端來(lái)觀測(cè)建圖過(guò)程（不建議，帶寬占用100M以上） # roslaunch robot_vslam rtabmapviz.launch

鍵盤控制機(jī)器人移動(dòng)：

# 鍵盤控制 rosrun teleop_twist_keyboard teleop_twist_keyboard.py

我們建議是將機(jī)器人的直線運(yùn)動(dòng)速度不超過(guò) 0.2m/s,旋轉(zhuǎn)速度不超過(guò) 0.4rad/s 以保證建圖質(zhì)量。建圖過(guò)程和激光雷達(dá)建圖基本一致，遙控機(jī)器人在環(huán)境中遍歷一遍完成建圖。

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

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

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

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

2.導(dǎo)航

機(jī)器人端打開四個(gè)終端，分別運(yùn)行:

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

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

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

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

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

如果沒(méi)有自動(dòng)定位成功，則可以在 PC 端或機(jī)器人端啟動(dòng)一個(gè)鍵盤控制節(jié)點(diǎn)：

rosrun teleop_twist_keyboard teleop_twist_keyboard.py

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

接下來(lái)就可以在地圖中指定目標(biāo)點(diǎn)讓機(jī)器人導(dǎo)航過(guò)去了。

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

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

1.視覺(jué)+激光雷達(dá)建圖

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

# 啟動(dòng)激光雷達(dá)+base_control roslaunch robot_navigation robot_lidar.launch # 啟動(dòng)深度攝像頭 roslaunch robot_vslam camera.launch # 啟動(dòng)rgbd+lidar聯(lián)合建圖 roslaunch robot_vslam rtabmap_rgbd_lidar.launch # pc端查看 roslaunch robot_vslam rtabmap_rviz.launch # pc端控制移動(dòng) rosrun teleop_twist_keyboard teleop_twist_keyboard.py

我們建議是將機(jī)器人的直線運(yùn)動(dòng)速度不超過(guò) 0.2m/s,旋轉(zhuǎn)速度不超過(guò) 0.4rad/s 以保證建圖質(zhì)量建圖過(guò)程和激光雷達(dá)建圖基本一致，遙控機(jī)器人在環(huán)境中遍歷一遍完成建圖。這里可以看到，由于加入了360 度的激光雷達(dá)數(shù)據(jù)，建圖在 2D 的地圖方面表現(xiàn)要更好一些。

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

機(jī)器人端打開四個(gè)終端，分別運(yùn)行:

# 啟動(dòng)激光雷達(dá)+base_control roslaunch robot_navigation robot_lidar.launch # 啟動(dòng)camera roslaunch robot_vslam camera.launch # 啟動(dòng)rtab并自動(dòng)定位 roslaunch robot_vslam rtabmap_rgbd_lidar.launch localization:=true # 啟動(dòng)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 度的激光雷達(dá)，機(jī)器人具備了全向的感知能力，所以可以將 move_forward_only 參數(shù)設(shè)置為 false 允許機(jī)器人向后運(yùn)動(dòng)。

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

審核編輯 ：李倩