vSLAM開發指南：從技術框架、開源演算法到硬體選型！

• 2019 年 12 月 24 日
• 筆記

出品 | 智東西公開課 講師 | 小覓智慧 CTO 楊瑞翾 編輯 | 王鑫

今天要和大家分享的主題是《如何利用vSLAM幫助機器人實現不同場景下的精準導航避障》。將從以下四個方面來和大家分享：

1、vSLAM發展歷程

2、vSLAM技術原理及不同演算法實現

3、vSLAM在不同場景下的應用挑戰與解決辦法

4、vSLAM在機器人實時導航避障中的應用實踐

vSLAM 的發展歷程

vSLAM的發展歷程也可以說是視覺感測器的進化史，兩者是相互依賴的。最早誕生的是單目相機，於是有了單目的vSLAM演算法，XBox的Kinect相機把結構光和ToF都變成了消費類電子產品，並得到了大規模的量產。單目相機加上一組深度相機，於是有了RGB-D的SLAM，也有把深度圖轉換為Laser-Scan當雷射來使用去做鑒圖的。

雙目相機由於對算力的要求比較高，得益於近些年來CPU和GPU的算力越來越高，逐步發展成了主流的感測器。視覺里程計對於視覺和IMU的同步精度要求是比較高的，所以我們也做了專門的雙目慣導相機。多目相機可以簡單的理解為是多組的雙目，對視角和不同方向進行了擴充。

下面我簡單介紹一下每個相機的特點：

-單目相機

單目相機的最大的問題是沒有尺度資訊，比如圖片里的比例關係,人之所以可以區分是因為我們經過了大量的訓練，類似於深度學習。但是機器視覺是不能從一張圖中得到空間關係的，這就需要對相機進行移動來估計不同位姿下的影像關係，從而得到尺度資訊，相當於這就是一個可變基準線的雙目相機。

-結構光相機

結構光相機是靠投射出一些紅外的圖案，再利用IR攝影機接收到影像的形變來計算出來深度，IR的發射器和接收器之間的距離也類似於雙目的基準線,測距的關係、原理也類似於三角測量。用到的圖案也有各式各樣的，比如條紋狀圖案等。在Kinect中用的是隨機的散斑，Kinect也提供了三種不同距離下的三種不同尺寸的散斑，用於測量不同的區間。

-ToF相機

ToF相機是通過發射連續的光脈衝，然後利用感測器接收反射光，通過光飛行的時間推算出距離。ToF相機現在的解析度普遍不高，基本上是QVGA(QuarterVGA)，當然現在也有VGA解析度的，但是隨著它的解析度的提升，成本也會越來越高。

-雙目相機

雙目相機的原理類似於人眼，我們人眼是通過左眼和右眼看到物體的不同視差，來得到深度資訊，雙目相機也是同樣的原理。

這張圖展示的是一個雙目三角測距的簡化模型，P代表被測的物體，z代表物體和相機之間的距離，b代表基準線，f代表焦距，d代表視差。通過相似三角形的原理：z/f=b/d。實際上雙目視差的演算法要更複雜一些，像OpenCV中就有BM演算法和SGBM演算法。

因為雙目的視差計算是不依賴於其它感測器的，所以雙目相機在室內和室外都可以很好應用。上圖就是在室內應用中看到的效果。對於一些弱紋理的表面，比如白牆或者沒有紋理的桌面等，我們可以通過疊加一些IR結構光的紋理去增強它表面的紋理資訊，這樣可以方便計算出雙目視差。

上面是雙目相機在室外的效果，結構光的相機主要發射和接受IR光，太陽光里有大量的IR光會被覆蓋掉，所以大部分結構光相機和ToF相機在室外都不能很好的工作，但是雙目沒有這個問題，所以現在很多汽車的ADAS里都用到了雙目相機。

我們把雙目結構光再加上IMU融合到一個相機里叫雙目結構光慣導相機，通過多感測器融合和互補來達到雙目在室內、室外不同的紋理，即使在旋轉的情況下也可以達到很好的應用效果。

vSLAM 技術原理和不同的演算法實現

下面為大家介紹一下vSLAM的技術原理和一些開源的VSLAM演算法的實現。vSLAM的技術框架主要包含了感測器的預處理、前端、後端、迴環檢測、建圖等。

-感測器

感測器部分我們經常用到的就是單目、雙目和RGBD。由於單目相機尺度的不確定性，所以初始化的時間一般比較長，而且估計的位姿會有累積的誤差；而雙目相機本身是有尺度資訊的，所以初始化很快，但是對標定有一定的要求；RGBD相機主要受室外陽光的影響比較大，包括反光物體的表面對RGBD相機的干擾也是比較大的，如果再加入IMU，就需要考慮IMU和影像之間同步的問題。

-前端

前端又稱為視覺里程計，主要是做一些相鄰幀的影像估算，還有幀間相機的運動，其實是解決了定位的問題，裡邊又涉及到特徵提取和位姿估計的演算法。

這裡提到的視覺里程計只能做相鄰幀的運動，相當於做一個局部的估計，運行時間長了會有累積的誤差，因此需要後端的優化和迴環檢測去做閉環處理。

-後端

後端優化主要有兩類演算法：基於濾波理論的優化和非線性的優化。近些年來基於非線性優化的vSLAM越來越多，而且逐漸成為主流的方法，但是實際上基於濾波理論的發展時間比較長，而且之前也有很好的應用案例，所以也是非常有生命力的。

-迴環檢測

迴環檢測和我們人類用眼睛來判斷兩個地方是否相同是一樣的道理，檢測到了相同的點就會做一次迴環修正。但是這裡有一個問題需要注意，由於場景裡邊可能出現高度相似的情況導致錯誤的閉環，比如貼著相同重複紋理的牆紙，這種情況我們需要增加一些其他的約束來避免它的錯誤閉環。

-建圖

vSLAM建立的地圖有幾種

（1）比如特徵地圖會把幾何特徵點和一些關鍵幀存儲起來，這些可以做定位導航用，也可以保存成地圖載入做導航。

（2）拓撲地圖也叫統計地圖，它是一種保持點與線相對位置關係正確但是不關心面積距離和方向是否正確的一種抽象地圖。

（3）柵格地圖就是加入了尺寸資訊的一種二維地圖，比如大家可以看到上面圖片背景的小格子，每一個格子就代表1個固定單位的尺寸資訊，整個地圖都是按照尺寸去描述的。二維的地圖實際上比較適用於在地面運動且沒有太多高度的機器人。

（4）三維地圖可以把三維的環境和場景進行還原，比如Octomap或者是點雲的地圖，有了這個地圖，我們就可以做空間的三維重建或者路徑規劃，比如無人機的路徑規劃和導航。

下面為大家快速介紹幾種開源的SLAM：

-ORB_SLAM

ORB_SLAM的核心是使用ORB作為vSLAM的核心特徵，是一個完整的SLAM系統，包含了視覺里程計、跟蹤、閉環檢測，是一種完全基於稀疏特徵點的單目SLAM系統。同時也有單目、雙目和RGBD相機的介面。

ORB特徵實際上是將FAST特徵的檢測方法和BRIEF特徵描述子的結合起來的，並在他們原來的基礎上做了改進和優化，在效果和速度上都有很大的提升。上面的地址是我們的雙目產品接入了ORB_SLAM後的一個歷程，ORB_SLAM並沒有加入IMU的計算。

ORB的作者發了一篇基於慣導的單目vSLAM的文章，可以解決純旋轉的問題，中國的大神王京把它進行了簡單的實現，但是這個僅僅是驗證性質的，裡邊還存在一些Bug，下圖裡有程式碼地址，大家可以去學習下。

-VINS

VINS-Mono和VINS-Mobile是香港科技大學的沈劭劼老師團隊開源的單目視覺慣導SLAM方案，也是非常經典而且我個人非常推薦的一個單目VIO的項目。對整體算力的要求不高，精度也不錯，尤其是VINS-Mobile可以在iOS系統運行。

VINS-Fusion是香港科技大學VINS-Mono的一個擴展，它支援多感測器的整合，包括單目+IMU、雙目+IMU甚至純雙目，同時也提供了增加GPS的版本，也非常有幸我們和秦通大神一起探討感測器在VINS上面的優化，在這方面提供了優化方案和支援，這個也幫助VINS在前期發布的時候有一個很好的感測器。

-OKVIS

OKVIS是蘇黎世聯邦理工學院發布的一個雙目VIO項目，這也是一個非常經典的雙目VIO，但是它只輸出六自由度的位姿，並沒有迴環檢測和地圖，因此嚴格意義上說並不是一個完整的SLAM，雖然它精度不錯，但是如果長時間靜止會出現位姿飄走的情況，這也是該項目里的一個問題。

OKVIS的程式碼結構框架是非常清晰的，但只限於定位功能這一部分，當然它也包含了緊耦合和多感測器融合，程式碼也是非常推薦大家去學習。

-maplab

maplab是蘇黎世理工大學繼OKVIS之後推出的另一個vSLAM框架，它包含了兩部分，一部分是ROVIO，另一部分是SLAM離線處理的console.

-MSCKF

MSCKF是一個經典的基於濾波理論優化的vSLAM，它的優點是對算力要求不高，而且精度也不錯。

vSLAM 在不同場景下的應用挑戰

與解決辦法

下面主要是針對幾種常見的VIO的性能做一些對比介紹，以及感測器選型時的一些注意事項。

測試對比用到的機型分別是Intel NUC、Odroid XU4、Up Board。這裡NUC和UpBoard都是X86架構的，XU4使用的是一個ARM架構的主板。

在這幅圖裡橫坐標代表了位姿的誤差，這個誤差越小越好，縱坐標分別是CPU的佔用率、記憶體的佔用率和處理的時間。從這個結果來看VINS、OKVIS和ROVIO的表現效果還是不錯的，隨著一些新的演算法推出，也會有一些新的評測，大家可以關注一下。

-影像感測器

對於vSLAM全局快門要比捲簾快門好，因為全局快門一次是曝光一整幅影像，而捲簾快門是按行去做曝光。這裡可能有個誤區，有人會覺得全局快門不會模糊，其實並不是這樣的，是否模糊跟曝光時間的長短有關係，如果是在比較暗的環境，並且自動曝光時間拉長的情況下，即使移動也是會模糊的，這個和影像感測器的感光面積、鏡頭的光圈等因素有關，全局快門實際上主要解決的是果凍效應問題。

-鏡頭

對於鏡頭來講，視場角對於SLAM系統來說越大越好，因為視野里資訊越多，能抓的特徵點就越多，但是在選擇大廣角的鏡頭時，畫面的畸變不能太怪異，需要匹配標定和矯正的模型才可以使用。

-雙目感測器

另外，雙目感測器一定要注意雙目之間的同步，左目和右目必須在同一時間內曝光，曝光的同時也要有相同的AE（自動白平衡）和AWE（自動曝光）。

-影像+IMU

影像和IMU之間的同步也有嚴格要求，最好的情況是IMU和影像能夠完全的對齊，就像第一種情況，這種實際上是很難的，但是我們的感測器基本上已經可以做到了，這也是我們在市面上比一些競品做得好的一個非常重要的原因；

第二種情況是Sensor和IMU的時鐘同步，由於二者時鐘是同步的，所以它的offset相對來說是固定的，這種也是可以用的，因為在初始化的時候，我們能夠把offset估計出來然後固定化；但是第三種情況是不同步的，這對於vSLAM來說是致命問題，是不能使用的。

在產品化的過程中，實際上我們還要對演算法進行加速，包括CPU指令集的加速、GPU里CUDA或者Open CL的加速以及FPGA、DSP、ASIC等晶片的加速。這裡有一個誤區，很多公司希望通過開源項目稍加修改來實現產品化，但這是非常困難的。因為演算法需要對場景進行優化，對不同感測器進行融合，同時也要對於特定的平台去做加速和優化，降低一些主機的配置降低成本，才有可能把產品做好。

我們在Jetson TX 2上面也跑過VINS相關的演算法，而且值得一提的是，VINS有一個專門的項目是VINS-Fusion對於GPU的優化項目，大家可以看一下，這也使得其在NVIDIA 的GPU產品上能有更好的表現。

vSLAM 在不同場景下的應用

vSLAM發展到現在，在很多領域都有了很成熟的應用。下邊我們看一下vSLAM在哪些產品中都得到了應用？

對於現在市面上不同的定位導航和避障方案來講，雙目+IMU的產品逐漸成為大家選型的主流方向，因為它的精度和布置成本相對來說都比較好，同時也能提供視覺感測器里的識別等部分。

像AR、VR之前基本上是使用外部設備進行定位的，或者在環境中部署一些二維碼，但是對於環境和部署的要求比較高。

現在一些新的VR、AR定位產品都在使用VIO技術，讓頭盔能夠實現自主定位，主要關注的是影像的高幀率，定位的幀率要高，感測器不能太重，而且功耗也不能太高。定位的高幀率，和頭盔的影像刷新率有關，因為它要有很好的響應速度，才不會讓人感到眩暈。

-無人機避障

針對於無人機的避障現在也越來越多的方案用到了雙目，因為雙目能夠很好的去用作室外的一些避障和導航，並且解析度越高，對於物體的檢測效果會越好。

-無人駕駛

在無人駕駛的應用里大部分都是在藉助於多感測器融合的方案，裡面包含了高精度的GPS、毫米波雷達、相機、光學雷達、慣導等感測器。對於裡面的視覺部分，我們比較關心影像的動態範圍，可以讓相機在低光或者背光的環境下（比如隧道）能有很好的表現。同時也希望相機能夠用到這種車規級的感測器，因為在夏天陽光直射的情況下，車內的溫度會非常高。

下圖是VINS融合GPS做的數據集的定位導航，可以看到雙目+GPS也可以達到很好的效果。

vSLAM 在機器人實時定位導航

避障中的應用實踐

安防機器人我們用到了掃地機的一種底盤，然後使用我們的雙目相機做定位、導航和避障，最上面有一個2D的光學雷達去做建圖，同時還有一個三攝影機的全景相機做全景影像的採集，下面我們說一下不同感測器在這套系統裡邊起到的作用。

-超聲波感測器

超聲波感測器實際上是一個精度不高的感測器，而且易受外界環境的干擾，在這裡的主要是解決視覺和雷射以及一些感測器對於玻璃等透明物體識別無效的問題，超聲波可以很好的探測到玻璃。

-雙目感測器

雙目感測器主要用來提供位姿資訊，包括避障的點雲和深度，同時也可以做迴環檢測。主要起到定位、導航和避障的作用。

-IMU感測器

IMU感測器：我們是和視覺做了一些融合，也放到了vSLAM VIO的相關演算法里，我們用到的IMU實際上只是這種消費級的IMU，精度並不是很高，但是達到了非常好的效果。

-光學雷達

現在的產品里大多數用的都是單線光學雷達，因為多線的光學雷達價格太高，光學雷達在這種建圖的精度上和一些特殊場景上有很好的表現。至於這套系統要做多少感測器的冗餘和配合，需要結合應用場景和產品的定位來決定，在一些場景裡邊可以完全不用光學雷達也能得到很好的效果。

-底盤裡程針

另外，要注意的是底盤裡程計，相對來說，底盤輪子的轉動是比較精確的，但是打滑或者地面有坑坑窪窪的情況除外，這種情況需要一個多感測器融合的方案。

-主機

對於主機現在比較常見的是GPU或X86的主機，我們用到的基本是NVIDIA的嵌入式平台，通過ARM+GPU的優化方式，把我們的演算法放進去。同時對於低成本的要求，我們也會去別的平台做一些移植和優化。