目錄?

一、 概述

????1.深度相機(jī)

????2.常見的深度相機(jī)

????????2.1 雙目視覺(Stereo)

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

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

????2.4 綜合對(duì)比

二、 攝像頭通路打通

????1.硬件連接

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

????3.測(cè)試攝像頭

????4.點(diǎn)云顯示

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

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

????1.vSLAM建圖

????2.導(dǎo)航

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

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

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

一、 概述

1.深度相機(jī)

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

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

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

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

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

2.1 雙目視覺(Stereo)

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

雙目相機(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)景。雙目視覺根據(jù)視覺特征進(jìn)行圖像匹配，沒有特征會(huì)導(dǎo)致匹配失敗

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

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

代表公司：

• Leap Motion， ZED， 大疆

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

通過近紅外激光器，將具有一定結(jié)構(gòu)特征的光線投射到被拍攝物體上，再由專門的紅外攝像頭進(jìn)行采集。這種具備一定結(jié)構(gòu)的光線，會(huì)因被攝物體的不同深度區(qū)域，而采集不同的圖像相位信息，然后通過運(yùn)算單元將這種結(jié)構(gòu)的變化換算成深度信息，以此來獲得三維結(jié)構(gòu)。簡(jiǎn)單來說就是，通過光學(xué)手段獲取被拍攝物體的三維結(jié)構(gòu)，再將獲取到的信息進(jìn)行更深入的應(yīng)用。通常采用特定波長(zhǎng)的不可見的紅外激光作為光源，它發(fā)射出來的光經(jīng)過 一定的編碼投影在物體上，通過一定算法來計(jì)算返回的編碼圖案的畸變來得到物體的位置和深度信息。下圖是一個(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í)間來取得距離，具體而言就是通過給目標(biāo)連續(xù)發(fā)射激光脈沖，然后用傳感器 接收從反射光線，通過探測(cè)光脈沖的飛行往返時(shí)間來得到確切的目標(biāo)物距離。因?yàn)楣馑偌す猓ㄟ^直接測(cè) 光飛行時(shí)間實(shí)際不可行，一般通過檢測(cè)通過一定手段調(diào)制后的光波的相位偏移來實(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è)相位偏移辦法來實(shí)現(xiàn) TOF 功能。簡(jiǎn)單來說就是，發(fā)出一道經(jīng)過處理的光，碰到物體以后會(huì)反射回來，捕捉來回的時(shí)間，因?yàn)橐阎馑俸驼{(diào)制光的波長(zhǎng)，所以能快速準(zhǔn)確計(jì)算出到物體的距離。

其原理示意圖：

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

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

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

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

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

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

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

• 3）邊緣精度低。

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

代表公司：

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

• 
  

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

2.4 綜合對(duì)比

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

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

對(duì)比來看，結(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ī)硬件說明：

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

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

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

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

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

• 攝像頭Color Camera（中）

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

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

?

二、 攝像頭通路打通

1.硬件連接

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

• 樂視體感三合一攝像頭 * 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 [email protected]:orbbec/ros_astra_camera.git
git clone [email protected]: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)闆]有權(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 中顯示會(huì)是全黑，這和圖像格式有關(guān)，并非設(shè)備問題)
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ī)則著色：

最后通過左上角的”File”選項(xiàng)卡中 Save Config As 可以將當(dāng)前 rviz 配置保存到指定位置，下次打開 rviz 候可以通過 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ù)并不是像我們常見的激光雷達(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)的東西。

?

三、 視覺 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à)值，所以我們沒有將所有的啟動(dòng)文件寫成一個(gè) launch 文件在同一個(gè)終端中輸出)

PC端查看建圖過程：

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

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

# 鍵盤控制
rosrun teleop_twist_keyboard teleop_twist_keyboard.py

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

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

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

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

這里我們也可以修改點(diǎn)云顯示的顏色來實(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（純前向視覺無法后退）
roslaunch robot_vslam move_base.launch planner:=dwa move_forward_only:=true

這里 move_base 的 planner 參數(shù)可以選擇 dwa 和 teb，這部分在激光雷達(dá)導(dǎo)航部分已經(jīng)介紹過，這里就不再贅述，move_forward_only 參數(shù)是為了只允許機(jī)器人向前運(yùn)動(dòng)(視覺傳感器只有前向感知) 。啟動(dòng)后檢查各個(gè)節(jié)點(diǎn)有無報(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ù)庫的信息，可能會(huì)花費(fèi)一點(diǎn)時(shí)間，取決于你的局域網(wǎng)條件。如果左下位置中沒有出現(xiàn)圖像且圖像兩側(cè)處為黑色，則機(jī)器人尚未完成初始位置的確定。如果如下圖所示，則機(jī)器人已經(jīng)完成定位，可以開始導(dǎo)航無需執(zhí)行下面的步驟(通常機(jī)器人從紋理比較豐富且上次已經(jīng)到達(dá)過的位置啟動(dòng)，都可以直接完成定位，這里文檔中是為了說明初始定位這一過程故意從一處幾乎沒有紋理的區(qū)域啟動(dòng)使機(jī)器人不能完成初始定位)

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

rosrun teleop_twist_keyboard teleop_twist_keyboard.py

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

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

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

?

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

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

這里操作過程和第三章節(jié)完全一致，只是部分啟動(dòng)指令有差異，所以這里只列出啟動(dòng)指令，不再描述操作過程。
機(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)速度不超過 0.2m/s,旋轉(zhuǎn)速度不超過 0.4rad/s 以保證建圖質(zhì)量建圖過程和激光雷達(dá)建圖基本一致，遙控機(jī)器人在環(huán)境中遍歷一遍完成建圖。這里可以看到，由于加入了360 度的激光雷達(dá)數(shù)據(jù)，建圖在 2D 的地圖方面表現(xiàn)要更好一些。

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