SLAM框架-常見方案對比

Simultaneous Localization and Mapping，即時定位與地圖構(gòu)建技術(shù)。無論在室內(nèi)、野外、空中還是水下，SLAM是機(jī)器人進(jìn)入未知環(huán)境遇到的第一個問題。本期將給大家介紹SLAM的基礎(chǔ)知識：傳感器與視覺SLAM框架。

近來年，智能機(jī)器人技術(shù)在世界范圍內(nèi)得到了大力發(fā)展。人們致力于把機(jī)器人用于實(shí)際場景：從室內(nèi)的移動機(jī)器人，到野外的自動駕駛汽車、空中的無人機(jī)、水下環(huán)境的探測機(jī)器人等等，均得到了廣泛的關(guān)注。

在大多數(shù)場合中，我們研究機(jī)器人會碰到一個基礎(chǔ)性的困難，那就是定位和建圖，也就是所謂的SLAM技術(shù)。沒有準(zhǔn)確的定位與地圖，掃地機(jī)就無法在房間自主地移動，只能隨機(jī)亂碰；家用機(jī)器人就無法按照指令準(zhǔn)確到達(dá)某個房間。此外，在虛擬現(xiàn)實(shí)（Virtual Reality）和增強(qiáng)現(xiàn)實(shí)技術(shù)（Argument Reality）中，沒有SLAM提供的定位，用戶就無法在場景中漫游。在這幾個應(yīng)用領(lǐng)域中，人們需要SLAM向應(yīng)用層提供空間定位的信息，并利用SLAM的地圖完成地圖的構(gòu)建或場景的生成。

傳感器

當(dāng)我們談?wù)揝LAM時，最先問到的就是傳感器。SLAM的實(shí)現(xiàn)方式與難度和傳感器的形式與安裝方式密切相關(guān)。傳感器分為激光和視覺兩大類，視覺下面又分三小方向。下面就帶你認(rèn)識這個龐大家族中每個成員的特性。

1. 傳感器之激光雷達(dá)

激光雷達(dá)是最古老，研究也最多的SLAM傳感器。它們提供機(jī)器人本體與周圍環(huán)境障礙物間的距離信息。常見的激光雷達(dá)，例如SICK、Velodyne還有我們國產(chǎn)的rplidar等，都可以拿來做SLAM。激光雷達(dá)能以很高精度測出機(jī)器人周圍障礙點(diǎn)的角度和距離，從而很方便地實(shí)現(xiàn)SLAM、避障等功能。 主流的2D激光傳感器掃描一個平面內(nèi)的障礙物，適用于平面運(yùn)動的機(jī)器人（如掃地機(jī)等）進(jìn)行定位，并建立2D的柵格地圖。這種地圖在機(jī)器人導(dǎo)航中很實(shí)用，因?yàn)槎鄶?shù)機(jī)器人還不能在空中飛行或走上臺階，仍限于地面。在SLAM研究史上，早期SLAM研究幾乎全使用激光傳感器進(jìn)行建圖，且多數(shù)使用濾波器方法，例如卡爾曼濾波器與粒子濾波器等。 激光的優(yōu)點(diǎn)是精度很高，速度快，計(jì)算量也不大，容易做成實(shí)時SLAM。缺點(diǎn)是價格昂貴，一臺激光動輒上萬元，會大幅提高一個機(jī)器人的成本。因此激光的研究主要集中于如何降低傳感器的成本上。對應(yīng)于激光的EKF-SLAM理論方面，因?yàn)檠芯枯^早，現(xiàn)在已經(jīng)非常成熟。與此同時，人們也對EKF-SLAM的缺點(diǎn)也有較清楚的認(rèn)識，例如不易表示回環(huán)、線性化誤差嚴(yán)重、必須維護(hù)路標(biāo)點(diǎn)的協(xié)方差矩陣，導(dǎo)致一定的空間與時間的開銷，等等。

2. 傳感器之視覺SLAM

視覺SLAM是21世紀(jì)SLAM研究熱點(diǎn)之一，一方面是因?yàn)橐曈X十分直觀，不免令人覺得：為何人能通過眼睛認(rèn)路，機(jī)器人就不行呢？另一方面，由于CPU、GPU處理速度的增長，使得許多以前被認(rèn)為無法實(shí)時化的視覺算法，得以在10 Hz以上的速度運(yùn)行。硬件的提高也促進(jìn)了視覺SLAM的發(fā)展。

以傳感器而論，視覺SLAM研究主要分為三大類：單目、雙目（或多目）、RGBD。其余還有魚眼、全景等特殊相機(jī)，但是在研究和產(chǎn)品中都屬于少數(shù)。此外，結(jié)合慣性測量器件（Inertial Measurement Unit，IMU）的視覺SLAM也是現(xiàn)在研究熱點(diǎn)之一。就實(shí)現(xiàn)難度而言，我們可以大致將這三類方法排序?yàn)椋簡文恳曈X>雙目視覺>RGBD。

單目相機(jī)： 單目相機(jī)SLAM簡稱MonoSLAM，即只用一支攝像頭就可以完成SLAM。這樣做的好處是傳感器特別的簡單、成本特別的低，所以單目SLAM非常受研究者關(guān)注。相比別的視覺傳感器，單目有個最大的問題，就是沒法確切地得到深度。這是一把雙刃劍。

一方面，由于絕對深度未知，單目SLAM沒法得到機(jī)器人運(yùn)動軌跡以及地圖的真實(shí)大小。直觀地說，如果把軌跡和房間同時放大兩倍，單目看到的像是一樣的。因此，單目SLAM只能估計(jì)一個相對深度，在相似變換空間Sim(3)中求解，而非傳統(tǒng)的歐氏空間SE(3)。如果我們必須要在SE(3)中求解，則需要用一些外部的手段，例如GPS、IMU等傳感器，確定軌跡與地圖的尺度（Scale）。

另一方面，單目相機(jī)無法依靠一張圖像獲得圖像中物體離自己的相對距離。為了估計(jì)這個相對深度，單目SLAM要靠運(yùn)動中的三角測量，來求解相機(jī)運(yùn)動并估計(jì)像素的空間位置。即是說，它的軌跡和地圖，只有在相機(jī)運(yùn)動之后才能收斂，如果相機(jī)不進(jìn)行運(yùn)動時，就無法得知像素的位置。同時，相機(jī)運(yùn)動還不能是純粹的旋轉(zhuǎn)，這就給單目SLAM的應(yīng)用帶來了一些麻煩，好在日常使用SLAM時，相機(jī)都會發(fā)生旋轉(zhuǎn)和平移。不過，無法確定深度同時也有一個好處：它使得單目SLAM不受環(huán)境大小的影響，因此既可以用于室內(nèi)，又可以用于室外。

雙目相機(jī)： 相比于單目，雙目相機(jī)通過多個相機(jī)之間的基線，估計(jì)空間點(diǎn)的位置。與單目不同的是，立體視覺既可以在運(yùn)動時估計(jì)深度，亦可在靜止時估計(jì)，消除了單目視覺的許多麻煩。不過，雙目或多目相機(jī)配置與標(biāo)定均較為復(fù)雜，其深度量程也隨雙目的基線與分辨率限制。通過雙目圖像計(jì)算像素距離，是一件非常消耗計(jì)算量的事情，現(xiàn)在多用FPGA來完成。

RGBD： RGBD = RGB + Depth Map RGBD相機(jī)是2010年左右開始興起的一種相機(jī)，它最大的特點(diǎn)是可以通過紅外結(jié)構(gòu)光或Time-of-Flight（飛行時間）原理，直接測出圖像中各像素離相機(jī)的距離。因此，它比傳統(tǒng)相機(jī)能夠提供更豐富的信息，也不必像單目或雙目那樣費(fèi)時費(fèi)力地計(jì)算深度。目前常用的RGBD相機(jī)包括Kinect/Kinect V2（微軟公司開發(fā)）、Xtion（華碩）等。不過，現(xiàn)在多數(shù)RGBD相機(jī)還存在測量范圍窄、噪聲大、視野小等諸多問題。出于量程的限制，主要用于室內(nèi)SLAM。

視覺SLAM框架

視覺SLAM幾乎都有一個基本的框架 。一個SLAM系統(tǒng)分為四個模塊（除去傳感器數(shù)據(jù)讀?。篤O、后端、建圖、回環(huán)檢測。這里我們簡要介紹各模塊的涵義，之后再詳細(xì)介紹其使用方法。

SLAM框架之視覺里程計(jì)

Visual Odometry，即視覺里程計(jì)。它估計(jì)兩個時刻機(jī)器人的相對運(yùn)動（Ego-motion）。在激光SLAM中，我們可以將當(dāng)前的觀測與全局地圖進(jìn)行匹配，用ICP求解相對運(yùn)動。而對于相機(jī)，它在歐氏空間里運(yùn)動，我們經(jīng)常需要估計(jì)一個三維空間的變換矩陣——SE3或Sim3（單目情形）。求解這個矩陣是VO的核心問題，而求解的思路，則分為基于特征的思路和不使用特征的直接方法。

特征匹配

基于特征的方法是目前VO的主流方式。對于兩幅圖像，首先提取圖像中的特征，然后根據(jù)兩幅圖的特征匹配，計(jì)算相機(jī)的變換矩陣。最常用的是點(diǎn)特征，例如Harris角點(diǎn)、SIFT、SURF、ORB。如果使用RGBD相機(jī)，利用已知深度的特征點(diǎn)，就可以直接估計(jì)相機(jī)的運(yùn)動。給定一組特征點(diǎn)以及它們之間的配對關(guān)系，求解相機(jī)的姿態(tài)，該問題被稱為PnP問題（Perspective-N-Point）。PnP可以用非線性優(yōu)化來求解，得到兩個幀之間的位置關(guān)系。

不使用特征進(jìn)行VO的方法稱為直接法。它直接把圖像中所有像素寫進(jìn)一個位姿估計(jì)方程，求出幀間相對運(yùn)動。例如，在RGBD SLAM中，可以用ICP（Iterative Closest Point，迭代最近鄰）求解兩個點(diǎn)云之間的變換矩陣。對于單目SLAM，我們可以匹配兩個圖像間的像素，或者像圖像與一個全局的模型相匹配。直接法的典型例子是SVO和LSD-SLAM。它們在單目SLAM中使用直接法，取得了較好的效果。目前看來，直接法比特征VO需要更多的計(jì)算量，而且對相機(jī)的圖像采集速率也有較高的要求。

SLAM框架之后端

在VO估計(jì)幀間運(yùn)動之后，理論上就可以得到機(jī)器人的軌跡了。然而視覺里程計(jì)和普通的里程計(jì)一樣，存在累積誤差的問題（Drift）。直觀地說，在t1和t2時刻，估計(jì)的轉(zhuǎn)角比真實(shí)轉(zhuǎn)角少1度，那么之后的軌跡就全部少掉了這1度。時間一長，建出的房間可能由方形變成了多邊形，估計(jì)出的軌跡亦會有嚴(yán)重的漂移。所以在SLAM中，還會把幀間相對運(yùn)動放到一個稱之為后端的程序中進(jìn)行加工處理。

早期的SLAM后端使用濾波器方式。由于那時還未形成前后端的概念，有時人們也稱研究濾波器的工作為研究SLAM。SLAM最早的提出者R. Smith等人就把SLAM建構(gòu)成了一個EKF（Extended Kalman Filter，擴(kuò)展卡爾曼濾波）問題。他們按照EKF的形式，把SLAM寫成了一個運(yùn)動方程和觀測方式，以最小化這兩個方程中的噪聲項(xiàng)為目的，使用典型的濾波器思路來解決SLAM問題。

當(dāng)一個幀到達(dá)時，我們能（通過碼盤或IMU）測出該幀與上一幀的相對運(yùn)動，但是存在噪聲，是為運(yùn)動方程。同時，通過傳感器對路標(biāo)的觀測，我們測出了機(jī)器人與路標(biāo)間的位姿關(guān)系，同樣也帶有噪聲，是為觀測方程。通過這兩者信息，我們可以預(yù)測出機(jī)器人在當(dāng)前時刻的位置。同樣，根據(jù)以往記錄的路標(biāo)點(diǎn)，我們又能計(jì)算出一個卡爾曼增益，以補(bǔ)償噪聲的影響。于是，對當(dāng)前幀和路標(biāo)的估計(jì)，即是這個預(yù)測與更新的不斷迭代的過程。

21世紀(jì)之后，SLAM研究者開始借鑒SfM（Structure from Motion）問題中的方法，把捆集優(yōu)化（Bundle Adjustment）引入到SLAM中來。優(yōu)化方法和濾波器方法有根本上的不同。它并不是一個迭代的過程，而是考慮過去所有幀中的信息。通過優(yōu)化，把誤差平均分到每一次觀測當(dāng)中。在SLAM中的Bundle Adjustment常常以圖的形式給出，所以研究者亦稱之為圖優(yōu)化方法（Graph Optimization）。圖優(yōu)化可以直觀地表示優(yōu)化問題，可利用稀疏代數(shù)進(jìn)行快速的求解，表達(dá)回環(huán)也十分的方便，因而成為現(xiàn)今視覺SLAM中主流的優(yōu)化方法。

SLAM框架之回環(huán)檢測

回環(huán)檢測，又稱閉環(huán)檢測（Loop closure detection），是指機(jī)器人識別曾到達(dá)場景的能力。如果檢測成功，可以顯著地減小累積誤差?；丨h(huán)檢測實(shí)質(zhì)上是一種檢測觀測數(shù)據(jù)相似性的算法。對于視覺SLAM，多數(shù)系統(tǒng)采用目前較為成熟的詞袋模型（Bag-of-Words, BoW）。詞袋模型把圖像中的視覺特征（SIFT, SURF等）聚類，然后建立詞典，進(jìn)而尋找每個圖中含有哪些“單詞”（word）。也有研究者使用傳統(tǒng)模式識別的方法，把回環(huán)檢測建構(gòu)成一個分類問題，訓(xùn)練分類器進(jìn)行分類。

回環(huán)檢測的難點(diǎn)在于，錯誤的檢測結(jié)果可能使地圖變得很糟糕。這些錯誤分為兩類：1.假陽性（False Positive），又稱感知偏差（Perceptual Aliasing），指事實(shí)上不同的場景被當(dāng)成了同一個；2.假陰性（False Negative），又稱感知變異（Perceptual Variability），指事實(shí)上同一個場景被當(dāng)成了兩個。感知偏差會嚴(yán)重地影響地圖的結(jié)果，通常是希望避免的。一個好的回環(huán)檢測算法應(yīng)該能檢測出盡量多的真實(shí)回環(huán)。研究者常常用準(zhǔn)確率-召回率曲線來評價一個檢測算法的好壞。 參考文章

雙目立體視覺的數(shù)學(xué)原理

雙目立體視覺是基于視差原理，由多幅圖像獲取物體三維幾何信息的方法。在機(jī)器視覺系統(tǒng)中，雙目視覺一般由雙攝像機(jī)從不同角度同時獲取周圍景物的兩幅數(shù)字圖像，或有由單攝像機(jī)在不同時刻從不同角度獲取周圍景物的兩幅數(shù)字圖像，并基于視差原理即可恢復(fù)出物體三維幾何信息，重建周圍景物的三維形狀與位置。 雙目視覺有的時候我們也會把它稱為體視，是人類利用雙眼獲取環(huán)境三維信息的主要途徑。從目前來看，隨著機(jī)器視覺理論的發(fā)展，雙目立體視覺在機(jī)器視覺研究中發(fā)回來看了越來越重要的作用。本篇帖子主要研究了雙目視覺的數(shù)學(xué)原理。

雙目立體視覺的數(shù)學(xué)原理

雙目立體視覺是基于視差，由三角法原理進(jìn)行三維信息的獲取，即由兩個攝像機(jī)的圖像平面和北側(cè)物體之間構(gòu)成一個三角形。已知兩個攝像機(jī)之間的位置關(guān)系，便可以獲得兩攝像機(jī)公共視場內(nèi)物體的三維尺寸及空間物體特征點(diǎn)的三維坐標(biāo)。所以，雙目視覺系統(tǒng)一般由兩個攝像機(jī)構(gòu)成。

雙目立體視覺三維測量原理

上圖所示為簡單的平視雙目立體成像原理圖，兩攝像機(jī)的投影中心連線的距離，即基線距離B。兩攝像機(jī)在同一時刻觀看時空物體的同一特征點(diǎn)P，分別在“左眼”和“右眼”上獲取了點(diǎn)P的圖像，他們的坐標(biāo)分別為Pleft=（Xleft,Yleft）；Pright=（Xright,Yright）。將定兩攝像機(jī)的圖像在同一平面上，則特征點(diǎn)P的圖像坐標(biāo)的Y坐標(biāo)一定是相同的，即Yleft = Yright =Y。由三角幾何關(guān)系可以得到如下關(guān)系式：

則視差為：Disparity=Xleft-Xright.由此可以計(jì)算出特征點(diǎn)P在攝像機(jī)坐標(biāo)系下的三維坐標(biāo)：

因此，左攝像機(jī)像面上的任意一點(diǎn)只要能在右攝像機(jī)像面上找到對應(yīng)的匹配點(diǎn)，就完全可以確定該點(diǎn)的三維坐標(biāo)。這種方法是點(diǎn)對點(diǎn)的運(yùn)算，像平面上所有點(diǎn)只要存在相應(yīng)的匹配點(diǎn)，就可以參與上述運(yùn)算，從而獲取對應(yīng)的三維坐標(biāo)。

雙目立體視覺數(shù)學(xué)模型

在分析了最簡單的平視雙目立體視覺的三維測量原理基礎(chǔ)上，現(xiàn)在我們就有能力來考慮一般情況。如上圖所示，設(shè)左攝像機(jī)O-xyz位于世界坐標(biāo)系原點(diǎn)，且沒有發(fā)生旋轉(zhuǎn)，圖像坐標(biāo)系為Ol-X1Y1，有效焦距為fl；右攝像機(jī)坐標(biāo)系為Or-xyz，圖像坐標(biāo)系為Or-XrYr，有效焦距為fr。那么根據(jù)攝像機(jī)的投射模型我們就能得到如下關(guān)系式：

因?yàn)镺-xyz坐標(biāo)系與Or-xryrzr坐標(biāo)系之間的位置關(guān)系可通過空間轉(zhuǎn)換矩陣MLr表示為：

同理，對于O-xyz坐標(biāo)系中的空間點(diǎn)，兩個攝像機(jī)面點(diǎn)之間的對應(yīng)關(guān)系可以表示為：

于是，空間點(diǎn)三維坐標(biāo)可以表示為

因此，只要我們通過計(jì)算機(jī)標(biāo)定技術(shù)獲得左右計(jì)算機(jī)內(nèi)參數(shù)/焦距fr,fl和空間點(diǎn)在左右攝像機(jī)中的圖像坐標(biāo)，就能夠重構(gòu)出被測點(diǎn)的三維空間坐標(biāo)。

1.雙目方案，最大的問題在于實(shí)現(xiàn)算法需要很高的計(jì)算資源，導(dǎo)致實(shí)時性很差，而且基本跟分辨率，檢測精度掛鉤。也就是說，分辨率越高，要求精度越高，則計(jì)算越復(fù)雜，同時，純雙目方案受光照，物體紋理性質(zhì)影響。

2.結(jié)構(gòu)光方案，目的就是為了解決雙目中匹配算法的復(fù)雜度和魯棒性問題而提出，該方案解決了大多數(shù)環(huán)境下雙目的上述問題。但是，在強(qiáng)光下，結(jié)構(gòu)光核心技術(shù)激光散斑會被淹沒。因此，不合適室外。同時，在長時間監(jiān)控方面，激光發(fā)射設(shè)備容易壞，重新?lián)Q設(shè)備后，需要重新標(biāo)定。

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

審核編輯：劉清