即時(shí)定位與地圖構(gòu)建(SLAM)的相關(guān)研究

1、即時(shí)定位與地圖構(gòu)建（SLAM）的相關(guān)研究即時(shí)定位與地圖構(gòu)建（SimultaneousLocalization AndMapping）指的是機(jī)器人在自身位置不確定的條件下,在完全未知環(huán)境中創(chuàng)建地圖,同時(shí)利用地圖進(jìn)行自主定位和導(dǎo)航。SLAM問(wèn)題可以描述為:機(jī)器人在未知環(huán)境中從一個(gè)未知位置開(kāi)始移動(dòng),在移動(dòng)過(guò)程中根據(jù)位置估計(jì)和傳感器數(shù)據(jù)進(jìn)行自身定位,同時(shí)建造增量式地圖。（1）定位(localization)：機(jī)器人必須知道自己在環(huán)境中位置。（2）建圖(mapping)：機(jī)器人必須記錄環(huán)境中特征的位置（如果知道自己的位置）（3）SLAM：機(jī)器人在定位的同時(shí)建立環(huán)境地圖。其基本原理是運(yùn)過(guò)概率統(tǒng)計(jì)的方法，通

2、過(guò)多特征匹配來(lái)達(dá)到定位和減少定位誤差的。圖 SLAM的基本過(guò)程移動(dòng)機(jī)器人自定位與環(huán)境建模問(wèn)題是緊密相關(guān)的。環(huán)境模型的準(zhǔn)確性依賴(lài)于定位精度，而定位的實(shí)現(xiàn)又離不開(kāi)環(huán)境模型。在未知環(huán)境中，機(jī)器人沒(méi)有什么參照物，只能依靠自己并不十分準(zhǔn)確的傳感器來(lái)獲取外界信息，如同一個(gè)盲人在一個(gè)陌生環(huán)境中摸索的情況。這種情況下，定位是比較困難的。有地圖的定位和有定位的地圖創(chuàng)建都是容易解決的，但無(wú)地圖的定位和未解決定位的地圖創(chuàng)建如同"雞-蛋"問(wèn)題，無(wú)從下手。已有的研究中對(duì)這類(lèi)問(wèn)題的解決方法可分為兩類(lèi)：一類(lèi)利用自身攜帶的多種內(nèi)部傳感器(包括里程儀、羅盤(pán)、加速度計(jì)等)，通過(guò)多種傳感信息的融合減少

3、定位的誤差，使用的融合算法多為基于卡爾曼濾波的方法。這類(lèi)方法由于沒(méi)有參考外部信息，在長(zhǎng)時(shí)間的漫游后誤差的積累會(huì)比較大。另一類(lèi)方法在依靠?jī)?nèi)部傳感器估計(jì)自身運(yùn)動(dòng)的同時(shí)，使用外部傳感器(如激光測(cè)距儀、視覺(jué)等)感知環(huán)境，對(duì)獲得的信息進(jìn)行分析提取環(huán)境特征并保存，在下一步通過(guò)對(duì)環(huán)境特征的比較對(duì)自身位置進(jìn)行校正。但這種方法依賴(lài)于能夠取得環(huán)境特征。SLAM的三個(gè)基本問(wèn)題Leonard和Durrant-Whyte將移動(dòng)機(jī)器人完成任務(wù)定義為三個(gè)問(wèn)題“Wheream I?”、“Wheream I going?”和“Howdo I getthere”，就是定位、目標(biāo)識(shí)別和路徑規(guī)劃，為了能實(shí)現(xiàn)導(dǎo)航，移動(dòng)機(jī)器人需要靠本體

4、感受傳感器和環(huán)境感知傳感器來(lái)實(shí)現(xiàn)對(duì)本體位姿估計(jì)和外部環(huán)境位姿的定位。依據(jù)環(huán)境空間的描述方法，Desouza等將視覺(jué)導(dǎo)航的方法化為三類(lèi)：(1)已知地圖的導(dǎo)航(Map-BasedNavigation)：表示地圖的方法幾何特征(GeometricPrimitives)、拓?fù)涮卣?TopologicalFeatures)或占據(jù)柵格(OccupancyGrids)移動(dòng)機(jī)器人依據(jù)這些已知的環(huán)境地圖進(jìn)行導(dǎo)航。(2)地圖建立的導(dǎo)航(Map-Building-BasedNavigation)：在沒(méi)有已知環(huán)境地圖的情況下，移動(dòng)機(jī)器人通過(guò)自身的導(dǎo)航運(yùn)動(dòng)和傳感器的不斷感知更新來(lái)進(jìn)行導(dǎo)航。(3)未知環(huán)境的導(dǎo)航(Mapl

5、essNavigation)：相對(duì)于上面兩種方法，在實(shí)時(shí)的動(dòng)態(tài)環(huán)境中無(wú)法建立明確的地圖表達(dá)形式，更多的是通過(guò)傳感器獲得的觀(guān)測(cè)信息用來(lái)識(shí)別或者跟蹤環(huán)境中的物體來(lái)導(dǎo)航。但是由于感知信息的不確定性，移動(dòng)機(jī)器人很難實(shí)現(xiàn)定位的準(zhǔn)確，因而，在未知環(huán)境中的定位成為最關(guān)鍵的問(wèn)題定位(Wheream I?)是實(shí)現(xiàn)自主能力的最基本問(wèn)題，是為了確定機(jī)器人在運(yùn)行環(huán)境中相對(duì)于世界坐標(biāo)系的位置及其本身的位姿。移動(dòng)機(jī)器人的定位與其它領(lǐng)域研究課題的關(guān)系如圖所示：圖 SLAM與各領(lǐng)域關(guān)系圖現(xiàn)有的移動(dòng)機(jī)器人自主定位方法主要是局部定位和全局定位。局部是通過(guò)測(cè)量相對(duì)于機(jī)器人初始位姿的距離和方向來(lái)確定當(dāng)前的位姿，但隨著時(shí)間的

6、累計(jì)造成定位的誤差較大，無(wú)法精確定位。全局定位則通過(guò)測(cè)機(jī)器人的絕對(duì)位置來(lái)定位，定位的精度較高，并且可以用來(lái)修正局部定位的定位誤差。現(xiàn)在移動(dòng)機(jī)器人定位的方法大致可分為三類(lèi)(1)相對(duì)定位(RelativePositionMeasurements)：主要依靠?jī)?nèi)部本體感受傳感器如里程計(jì)（Odometry）、陀螺儀(Gyroscopes)等，通過(guò)給定初始位姿，來(lái)測(cè)量相對(duì)于機(jī)器人初始位姿的距離和方向來(lái)確定當(dāng)前機(jī)器人的位姿，也叫做航跡推測(cè)(DeadReckoning, DR)。(2)絕對(duì)定位(AbsolutePosition Measurements)：主要采用主動(dòng)或被動(dòng)標(biāo)識(shí)(Activeor Passiv

7、e Beacons)、地圖匹配(MapMatching)、全球定位系統(tǒng)(GlobalPositioning System,GPS)、或?qū)Ш叫艠?biāo)(LandmarkNavigation)進(jìn)行定位。位置的計(jì)算方法包括有三角測(cè)量法(Triangulation)、三邊測(cè)量法(Trilateration)和模型匹配算法(ModelMatching)等。(3)組合定位(CombinedPositionMethod)：雖然相對(duì)定位這種方法能夠根據(jù)運(yùn)動(dòng)學(xué)模型的自我推算移動(dòng)機(jī)器人的位姿和軌跡而且具有自包含的有點(diǎn)。但是不可避免地會(huì)存在隨時(shí)間的增加和距離的增加而增加的累積航跡誤差。在絕對(duì)定位中，地圖匹配技術(shù)處理數(shù)據(jù)速

8、度較慢，而信標(biāo)或標(biāo)識(shí)牌的建設(shè)和維護(hù)成本太高，GPS又只能在室外使用。由于單一定位的方法的缺陷，移動(dòng)機(jī)器人定位仍然是基于航跡的推算與絕對(duì)位姿和軌跡矯正相結(jié)合起來(lái)。-我已在Github Pages 上搭建了個(gè)人博客，歡迎訪(fǎng)問(wèn)：http:/xiongxiaoxx.github.io/視覺(jué)SLAM漫談（一）1.    前言開(kāi)始做SLAM（機(jī)器人同時(shí)定位與建圖）研究已經(jīng)近一年了。從一年級(jí)開(kāi)始對(duì)這個(gè)方向產(chǎn)生興趣，到現(xiàn)在為止，也算是對(duì)這個(gè)領(lǐng)域有了大致的了解。然而越了解，越覺(jué)得這個(gè)方向難度很大?？傮w來(lái)講有以下幾個(gè)原因：· 入門(mén)資料很少。雖然國(guó)內(nèi)也有不少人在做，

9、但這方面現(xiàn)在沒(méi)有太好的入門(mén)教程。SLAM for dummies可以算是一篇。中文資料幾乎沒(méi)有。· SLAM研究已進(jìn)行了三十多年，從上世紀(jì)的九十年代開(kāi)始。其中又有若干歷史分枝和爭(zhēng)論，要把握它的走向就很費(fèi)工夫。· 難以實(shí)現(xiàn)。SLAM是一個(gè)完整的系統(tǒng)，由許多個(gè)分支模塊組成?，F(xiàn)在經(jīng)典的方案是“圖像前端，優(yōu)化后端，閉環(huán)檢測(cè)”的三部曲，很多文獻(xiàn)看完了自己實(shí)現(xiàn)不出來(lái)。· 自己動(dòng)手編程需要學(xué)習(xí)大量的先決知識(shí)。首先你要會(huì)C和C+，網(wǎng)上很多代碼還用了11標(biāo)準(zhǔn)的C+。第二要會(huì)用Linux。第三要會(huì)cmake，vim/emacs及一些編程工具。第四要會(huì)用openCV, PCL, Eig

10、en等第三方庫(kù)。只有學(xué)會(huì)了這些東西之后，你才能真正上手編一個(gè)SLAM系統(tǒng)。如果你要跑實(shí)際機(jī)器人，還要會(huì)ROS。當(dāng)然，困難多意味著收獲也多，坎坷的道路才能鍛煉人（比如說(shuō)走著走著才發(fā)現(xiàn)Linux和C+才是我的真愛(ài)之類(lèi)的。）鑒于目前網(wǎng)上關(guān)于視覺(jué)SLAM的資料極少，我于是想把自己這一年多的經(jīng)驗(yàn)與大家分享一下。說(shuō)的不對(duì)的地方請(qǐng)大家批評(píng)指正。這篇文章關(guān)注視覺(jué)SLAM，專(zhuān)指用攝像機(jī)，Kinect等深度像機(jī)來(lái)做導(dǎo)航和探索，且主要關(guān)心室內(nèi)部分。到目前為止，室內(nèi)的視覺(jué)SLAM仍處于研究階段，遠(yuǎn)未到實(shí)際應(yīng)用的程度。一方面，編寫(xiě)和使用視覺(jué)SLAM需要大量的專(zhuān)業(yè)知識(shí)，算法的實(shí)時(shí)性未達(dá)到實(shí)用要求；另一方面，視覺(jué)SLAM生

11、成的地圖（多數(shù)是點(diǎn)云）還不能用來(lái)做機(jī)器人的路徑規(guī)劃，需要科研人員進(jìn)一步的探索和研究。以下，我會(huì)介紹SLAM的歷史、理論以及實(shí)現(xiàn)的方式，且主要介紹視覺(jué)（Kinect）的實(shí)現(xiàn)方式。2.    SLAM問(wèn)題SLAM，全稱(chēng)叫做Simultaneous Localization and Mapping，中文叫做同時(shí)定位與建圖。啊不行，這么講下去，這篇文章肯定沒(méi)有人讀，所以我們換一個(gè)講法。3.    小蘿卜的故事從前，有一個(gè)機(jī)器人叫“小蘿卜”。它長(zhǎng)著一雙烏黑發(fā)亮的大眼睛，叫做Kinect。有一天，它被邪惡的科學(xué)家關(guān)進(jìn)了一間空屋

12、子，里面放滿(mǎn)了雜七雜八的東西。 小蘿卜感到很害怕，因?yàn)檫@個(gè)地方他從來(lái)沒(méi)來(lái)過(guò)，一點(diǎn)兒也不了解。讓他感到害怕的主要是三個(gè)問(wèn)題：1.          自己在哪里？2.          這是什么地方？3.          怎么離開(kāi)這個(gè)地方？在SLAM理論中，第一個(gè)問(wèn)題稱(chēng)為定位 (L

13、ocalization)，第二個(gè)稱(chēng)為建圖 (Mapping)，第三個(gè)則是隨后的路徑規(guī)劃。我們希望借助Kinect工具，幫小蘿卜解決這個(gè)難題。各位同學(xué)有什么思路呢？4.    Kinect數(shù)據(jù)要打敗敵人，首先要了解你的武器。不錯(cuò)，我們先介紹一下Kinect。眾所周知這是一款深度相機(jī)，你或許還聽(tīng)說(shuō)過(guò)別的牌子，但Kinect的價(jià)格便宜，測(cè)量范圍在3m-12m之間，精度約3cm，較適合于小蘿卜這樣的室內(nèi)機(jī)器人。它采到的圖像是這個(gè)樣子的（從左往右依次為rgb圖，深度圖與點(diǎn)云圖）： Kinect的一大優(yōu)勢(shì)在于能比較廉價(jià)地獲得每個(gè)像素的深度值，不管

14、是從時(shí)間上還是從經(jīng)濟(jì)上來(lái)說(shuō)。OK，有了這些信息，小蘿卜事實(shí)上可以知道它采集到的圖片中，每一個(gè)點(diǎn)的3d位置。只要我們事先標(biāo)定了Kinect，或者采用出廠(chǎng)的標(biāo)定值。我們把坐標(biāo)系設(shè)成這個(gè)樣子，這也是OpenCV中采用的默認(rèn)坐標(biāo)系。 o-uv是圖片坐標(biāo)系，o-xyz是Kinect的坐標(biāo)系。假設(shè)圖片中的點(diǎn)為(u,v)，對(duì)應(yīng)的三維點(diǎn)位置在(x,y,z)，那么它們之間的轉(zhuǎn)換關(guān)系是這樣的： 或者更簡(jiǎn)單的： 后一個(gè)公式給出了計(jì)算三維點(diǎn)的方法。先從深度圖中讀取深度數(shù)據(jù)（Kinect給的是16位無(wú)符號(hào)整數(shù)），除掉z方向的縮放因子，這樣你就把一個(gè)整數(shù)變到了以米為單位的數(shù)據(jù)。然后，x,y用

15、上面的公式算出。一點(diǎn)都不難，就是一個(gè)中心點(diǎn)位置和一個(gè)焦距而已。f代表焦距，c代表中心。如果你沒(méi)有自己標(biāo)定你的Kinect，也可以采用默認(rèn)的值：s=5000, cx = 320, cy=240, fx=fy=525。實(shí)際值會(huì)有一點(diǎn)偏差，但不會(huì)太大。5.    定位問(wèn)題知道了Kinect中每個(gè)點(diǎn)的位置后，接下來(lái)我們要做的，就是根據(jù)兩幀圖像間的差別計(jì)算小蘿卜的位移。比如下面兩張圖，后一張是在前一張之后1秒采集到的：   你肯定可以看出，小蘿卜往右轉(zhuǎn)過(guò)了一定的角度。但究竟轉(zhuǎn)過(guò)多少度呢？這就要靠計(jì)算機(jī)來(lái)求解了。這個(gè)問(wèn)題稱(chēng)為相機(jī)相對(duì)姿態(tài)

16、估計(jì)，經(jīng)典的算法是ICP（Iterative Closest Point，迭代最近點(diǎn)）。這個(gè)算法要求知道這兩個(gè)圖像間的一組匹配點(diǎn)，說(shuō)的通俗點(diǎn)，就是左邊圖像哪些點(diǎn)和右邊是一樣的。你當(dāng)然看見(jiàn)那塊黑白相間的板子同時(shí)出現(xiàn)在兩張圖像中。在小蘿卜看來(lái)，這里牽涉到兩個(gè)簡(jiǎn)單的問(wèn)題：特征點(diǎn)的提取和匹配。如果你熟悉計(jì)算機(jī)視覺(jué)，那你應(yīng)該聽(tīng)說(shuō)過(guò)SIFT, SURF之類(lèi)的特征。不錯(cuò)，要解決定位問(wèn)題，首先要得到兩張圖像的一個(gè)匹配。匹配的基礎(chǔ)是圖像的特征，下圖就是SIFT提取的關(guān)鍵點(diǎn)與匹配結(jié)果：  對(duì)實(shí)現(xiàn)代碼感興趣的同學(xué)請(qǐng)Google“OpenCV 匹配”即可，在opencv的教程上也有很明白

17、的例子。上面的例子可以看出，我們找到了一些匹配，但其中有些是對(duì)的（基本平等的匹配線(xiàn)），有些是錯(cuò)的。這是由于圖像中存在周期性出現(xiàn)的紋理（黑白塊），所以容易搞錯(cuò)。但這并不是問(wèn)題，在接下來(lái)的處理中我們會(huì)將這些影響消去。得到了一組匹配點(diǎn)后，我們就可以計(jì)算兩個(gè)圖像間的轉(zhuǎn)換關(guān)系，也叫PnP問(wèn)題。它的模型是這樣的： R為相機(jī)的姿態(tài)，C為相機(jī)的標(biāo)定矩陣。R是不斷運(yùn)動(dòng)的，而C則是隨著相機(jī)做死的。ICP的模型稍有不同，但原理上也是計(jì)算相機(jī)的姿態(tài)矩陣。原則上，只要有四組匹配點(diǎn)，就可以算這個(gè)矩陣。你可以調(diào)用openCV的SolvePnPRANSAC函數(shù)或者PCL的ICP算法來(lái)求解。openCV提供的算法是R

18、ANSAC（Random Sample Consensus，隨機(jī)采樣一致性）架構(gòu)，可以剔除錯(cuò)誤匹配。所以代碼實(shí)際運(yùn)行時(shí)，可以很好地找到匹配點(diǎn)。以下是一個(gè)結(jié)果的示例。 上面兩張圖轉(zhuǎn)過(guò)了16.63度，位移幾乎沒(méi)有。有同學(xué)會(huì)說(shuō)，那只要不斷匹配下去，定位問(wèn)題不就解決了嗎？表面上看來(lái)，的確是這樣的，只要我們引入一個(gè)關(guān)鍵幀的結(jié)構(gòu)（發(fā)現(xiàn)位移超過(guò)一個(gè)固定值時(shí)，定義成一個(gè)關(guān)鍵幀）。然后，把新的圖像與關(guān)鍵幀比較就行了。至于建圖，就是把這些關(guān)鍵幀的點(diǎn)云拼起來(lái)，看著還有模有樣，煞有介事的： 1200幀的匹配結(jié)果然而，如果事情真這么簡(jiǎn)單，SLAM理論就不用那么多人研究三十多年了（它是從上世紀(jì)90年代

19、開(kāi)始研究的）（上面講的那些東西簡(jiǎn)直隨便哪里找個(gè)小碩士就能做出來(lái)）。那么，問(wèn)題難在什么地方呢？6.    SLAM端優(yōu)化理論最麻煩的問(wèn)題，就是“噪聲”。這種漸近式的匹配方式，和那些慣性測(cè)量設(shè)備一樣，存在著累積噪聲。因?yàn)槲覀冊(cè)诓粩嗟馗玛P(guān)鍵幀，把新圖像與最近的關(guān)鍵幀比較，從而獲得機(jī)器人的位移信息。但是你要想到，如果有一個(gè)關(guān)鍵幀出現(xiàn)了偏移，那么剩下的位移估計(jì)都會(huì)多出一個(gè)誤差。這個(gè)誤差還會(huì)累積，因?yàn)楹竺娴墓烙?jì)都基于前面的機(jī)器人位置哇！這后果簡(jiǎn)直不堪設(shè)想啊（例如，你的機(jī)器人往右轉(zhuǎn)了30度，再往左轉(zhuǎn)了30度回到原來(lái)的位置。然而由于誤差，你算成了向右轉(zhuǎn)29度，再向左轉(zhuǎn)

20、31度，這樣你構(gòu)建的地圖中，會(huì)出現(xiàn)初始位置的兩個(gè)“重影”）。我們能不能想辦法消除這個(gè)該死的誤差呢？朋友們，這才是SLAM的研究，前面的可以說(shuō)是“圖像前端”的處理方法。我們的解決思路是：如果你和最近的關(guān)鍵幀相比，會(huì)導(dǎo)致累計(jì)誤差。那么，我們最好是和更前面的關(guān)鍵幀相比，而且多比較幾個(gè)幀，不要只比較一次。我們用數(shù)學(xué)來(lái)描述這個(gè)問(wèn)題。設(shè)： 不要怕，只有借助數(shù)學(xué)才能把這個(gè)問(wèn)題講清楚。上面的公式中，xp是機(jī)器人小蘿卜的位置，我們假定由n個(gè)幀組成。xL則是路標(biāo)，在我們的圖像處理過(guò)程中就是指SIFT提出來(lái)的關(guān)鍵點(diǎn)。如果你做2D SLAM，那么機(jī)器人位置就是x, y加一個(gè)轉(zhuǎn)角theta。如果是3D SLA

21、M，就是x,y,z加一個(gè)四元數(shù)姿態(tài)（或者rpy姿態(tài)）。這個(gè)過(guò)程叫做參數(shù)化（Parameterization）。不管你用哪種參數(shù)，后面兩個(gè)方程你都需要知道。前一個(gè)叫運(yùn)動(dòng)方程，描述機(jī)器人怎樣運(yùn)動(dòng)。u是機(jī)器人的輸入，w是噪聲。這個(gè)方程最簡(jiǎn)單的形式，就是你能通過(guò)什么方式（碼盤(pán)等）獲得兩幀間的位移差，那么這個(gè)方程就直接是上一幀與u相加即得。另外，你也可以完全不用慣性測(cè)量設(shè)備，這樣我們就只依靠圖像設(shè)備來(lái)估計(jì)，這也是可以的。后一個(gè)方程叫觀(guān)測(cè)方程，描述那些路標(biāo)是怎么來(lái)的。你在第i幀看到了第j個(gè)路標(biāo)，產(chǎn)生了一個(gè)測(cè)量值，就是圖像中的橫縱坐標(biāo)。最后一項(xiàng)是噪聲。偷偷告訴你，這個(gè)方程形式上和上一頁(yè)的那個(gè)方程是一模一樣的

22、。在求解SLAM問(wèn)題前，我們要看到，我們擁有的數(shù)據(jù)是什么？在上面的模型里，我們知道的是運(yùn)動(dòng)信息u以及觀(guān)測(cè)z。用示意圖表示出來(lái)是這樣的： 我們要求解的，就是根據(jù)這些u和z，確定所有的xp和xL。這就是SLAM問(wèn)題的理論。從SLAM誕生開(kāi)始科學(xué)家們就一直在解決這個(gè)問(wèn)題。最初，我們用Kalman濾波器，所以上面的模型（運(yùn)動(dòng)方程和觀(guān)測(cè)方程）被建成這個(gè)樣子。直到21世紀(jì)初，卡爾曼濾波器仍在SLAM系統(tǒng)占據(jù)最主要的地位，Davison經(jīng)典的單目SLAM就是用EKF做的。但是后來(lái)，出現(xiàn)了基于圖優(yōu)化的SLAM方法，漸漸有取而代之的地位1。我們?cè)谶@里不介紹卡爾曼濾波器，有興趣的同學(xué)可以在wiki上找卡

23、爾曼濾波器，另有一篇中文的卡爾曼濾波器介紹也很棒。由于濾波器方法存儲(chǔ)n個(gè)路標(biāo)要消耗n平方的空間，在計(jì)算量上有點(diǎn)對(duì)不住大家。盡管08年有人提出分治法的濾波器能把復(fù)雜度弄到O(n) 2，但實(shí)現(xiàn)手段比較復(fù)雜。我們要介紹那種新興的方法: Graph-based SLAM。圖優(yōu)化方法把SLAM問(wèn)題做成了一個(gè)優(yōu)化問(wèn)題。學(xué)過(guò)運(yùn)籌學(xué)的同學(xué)應(yīng)該明白，優(yōu)化問(wèn)題對(duì)我們有多么重要。我們不是要求解機(jī)器人的位置和路標(biāo)位置嗎？我們可以先做一個(gè)猜測(cè)，猜想它們大概在什么地方。這其實(shí)是不難的。然后呢，將猜測(cè)值與運(yùn)動(dòng)模型觀(guān)測(cè)模型給出的值相比較，可以算出誤差： 通俗一點(diǎn)地講，例如，我猜機(jī)器人第一幀在(0,0,0)，第二幀在

24、(0,0,1)。但是u1告訴我機(jī)器人往z方向（前方）走了0.9米，那么運(yùn)動(dòng)方程就出現(xiàn)了0.1m的誤差。同時(shí)，第一幀中機(jī)器人發(fā)現(xiàn)了路標(biāo)1，它在該機(jī)器人圖像的正中間；第二幀卻發(fā)現(xiàn)它在中間偏右的位置。這時(shí)我們猜測(cè)機(jī)器人只是往前走，也是存在誤差的。至于這個(gè)誤差是多少，可以根據(jù)觀(guān)測(cè)方程算出來(lái)。我們得到了一堆誤差，把這些誤差平方后加起來(lái)（因?yàn)閱渭兊恼`差有正有負(fù)，然而平方誤差可以改成其他的范數(shù)，只是平方更常用），就得到了平方誤差和。我們把這個(gè)和記作phi，就是我們優(yōu)化問(wèn)題的目標(biāo)函數(shù)。而優(yōu)化變量就是那些個(gè)xp, xL。 改變優(yōu)化變量，誤差平方和（目標(biāo)函數(shù)）就會(huì)相應(yīng)地變大或變小，我們可以用數(shù)值方法求它

25、們的梯度和二階梯度矩陣，然后用梯度下降法求最優(yōu)值。這些東西學(xué)過(guò)優(yōu)化的同學(xué)都懂的。 注意到，一次機(jī)器人SLAM過(guò)程中，往往會(huì)有成千上萬(wàn)幀。而每一幀我們都有幾百個(gè)關(guān)鍵點(diǎn)，一乘就是幾百萬(wàn)個(gè)優(yōu)化變量。這個(gè)規(guī)模的優(yōu)化問(wèn)題放到小蘿卜的機(jī)載小破本上可解嗎？是的，過(guò)去的同學(xué)都以為，Graph-based SLAM是無(wú)法計(jì)算的。但就在21世紀(jì)06，07年后，有些同學(xué)發(fā)現(xiàn)了，這個(gè)問(wèn)題規(guī)模沒(méi)有想象的那么大。上面的J和H兩個(gè)矩陣是“稀疏矩陣”，于是呢，我們可以用稀疏代數(shù)的方法來(lái)解這個(gè)問(wèn)題。“稀疏”的原因，在于每一個(gè)路標(biāo)，往往不可能出現(xiàn)在所有運(yùn)動(dòng)過(guò)程中，通常只出現(xiàn)在一小部分圖像里。正是這個(gè)稀疏性，使得優(yōu)化思路

26、成為了現(xiàn)實(shí)。優(yōu)化方法利用了所有可以用到的信息（稱(chēng)為full-SLAM, global SLAM），其精確度要比我們一開(kāi)始講的幀間匹配高很多。當(dāng)然計(jì)算量也要高一些。由于優(yōu)化的稀疏性，人們喜歡用“圖”來(lái)表達(dá)這個(gè)問(wèn)題。所謂圖，就是由節(jié)點(diǎn)和邊組成的東西。我寫(xiě)成G=V,E，大家就明白了。V是優(yōu)化變量節(jié)點(diǎn)，E表示運(yùn)動(dòng)/觀(guān)測(cè)方程的約束。什么，更糊涂了嗎？那我就上一張圖，來(lái)自3。 圖有點(diǎn)模糊，而且數(shù)學(xué)符號(hào)和我用的不太一樣，我用它來(lái)給大家一個(gè)圖優(yōu)化的直觀(guān)形象。上圖中，p是機(jī)器人位置，l是路標(biāo)，z是觀(guān)測(cè)，t是位移。其中呢，p, l是優(yōu)化變量，而z,t是優(yōu)化的約束?？雌饋?lái)是不是像一些彈簧連接了一些質(zhì)點(diǎn)呢？

27、因?yàn)槊總€(gè)路標(biāo)不可能出現(xiàn)在每一幀中，所以這個(gè)圖是蠻稀疏的。不過(guò)，“圖”優(yōu)化只是優(yōu)化問(wèn)題的一個(gè)表達(dá)形式，并不影響優(yōu)化的含義。實(shí)際解起來(lái)時(shí)還是要用數(shù)值法找梯度的。這種思路在計(jì)算機(jī)視覺(jué)里，也叫做Bundle Adjustment。它的具體方法請(qǐng)參見(jiàn)一篇經(jīng)典文章4。不過(guò)，BA的實(shí)現(xiàn)方法太復(fù)雜，不太建議同學(xué)們拿C來(lái)寫(xiě)。好在2010年的ICRA上，其他的同學(xué)們提供了一個(gè)通用的開(kāi)發(fā)包：g2o 5。它是有圖優(yōu)化通用求解器，很好用，我改天再詳細(xì)介紹這個(gè)軟件包?？傊?，我們只要把觀(guān)測(cè)和運(yùn)動(dòng)信息丟到求解器里就行。這個(gè)優(yōu)化器會(huì)為我們求出機(jī)器人的軌跡和路標(biāo)位置。如下圖，紅點(diǎn)是路標(biāo)，藍(lán)色箭頭是機(jī)器人的位置和轉(zhuǎn)角（2D SLA

28、M）。細(xì)心的同學(xué)會(huì)發(fā)現(xiàn)它往右偏轉(zhuǎn)了一些。：  7.    閉環(huán)檢測(cè)上面提到，僅用幀間匹配最大的問(wèn)題在于誤差累積，圖優(yōu)化的方法可以有效地減少累計(jì)誤差。然而，如果把所有測(cè)量都丟進(jìn)g2o，計(jì)算量還是有點(diǎn)兒大的。根據(jù)我自己測(cè)試，約10000多條邊，g2o跑起來(lái)就有些吃力了。這樣，就有同學(xué)說(shuō)，能把這個(gè)圖構(gòu)造地簡(jiǎn)潔一些嗎？我們用不著所有的信息，只需要把有用的拿出來(lái)就行了。事實(shí)上，小蘿卜在探索房間時(shí)，經(jīng)常會(huì)左轉(zhuǎn)一下，右轉(zhuǎn)一下。如果在某個(gè)時(shí)刻他回到了以前去過(guò)的地方，我們就直接與那時(shí)候采集的關(guān)鍵幀做比較，可以嗎？我們說(shuō)，可以，而且那是最好的方法。這個(gè)問(wèn)

29、題叫做閉環(huán)檢測(cè)。閉環(huán)檢測(cè)是說(shuō)，新來(lái)一張圖像時(shí)，如何判斷它以前是否在圖像序列中出現(xiàn)過(guò)？有兩種思路：一是根據(jù)我們估計(jì)的機(jī)器人位置，看是否與以前某個(gè)位置鄰近；二是根據(jù)圖像的外觀(guān)，看它是否和以前關(guān)鍵幀相似。目前主流方法是后一種，因?yàn)楹芏嗫茖W(xué)家認(rèn)為前一種依靠有噪聲的位置來(lái)減少位置的噪聲，有點(diǎn)循環(huán)論證的意思。后一種方法呢，本質(zhì)上是個(gè)模式識(shí)別問(wèn)題（非監(jiān)督聚類(lèi)，分類(lèi)），常用的是Bag-of-Words (BOW)。但是BOW需要事先對(duì)字典進(jìn)行訓(xùn)練，因此SLAM研究者仍在探討有沒(méi)有更合適的方法。在Kinect SLAM經(jīng)典大作中6，作者采用了比較簡(jiǎn)單的閉環(huán)方法：在前面n個(gè)關(guān)鍵幀中隨機(jī)采k個(gè)，與當(dāng)前幀兩兩匹配。匹

30、配上后認(rèn)為出現(xiàn)閉環(huán)。這個(gè)真是相當(dāng)?shù)暮?jiǎn)單實(shí)用，效率也過(guò)得去。高效的閉環(huán)檢測(cè)是SLAM精確求解的基礎(chǔ)。這方面還有很多工作可以做。8.    小結(jié)本文我們介紹了SLAM的基本概念，重點(diǎn)介紹了圖優(yōu)化解決SLAM問(wèn)題的思路。我最近正在編寫(xiě)SLAM程序，它是一個(gè)Linux下基于cmake的工程。目前仍在開(kāi)發(fā)當(dāng)中。歡迎感興趣的同學(xué)來(lái)交流研究心得，我的郵件是：gaoxiang12。參考文獻(xiàn)1 Visual SLAM: Why filter? Strasdat et. al., Image and Vision Computing, 2012.2 Divide and C

31、onquer: EKF SLAM in O(n), Paz Lina M et al., IEEE Transaction on Robotics, 20083 Relative bundle adjustment, Sibley, Gabe, 20094 Bundle adjustment - a Modern Synthesis. Triggs B et. el., Springer, 20005 g2o: A General Framework for Graph Optimization, Kummerle Rainer, et. al., ICRA, 20116 3-D Mappin

32、g with an RGB-D Camera, IEEE Transaction on Robotics, Endres et al., 2014視覺(jué)SLAM漫談（二）:圖優(yōu)化理論與g2o的使用1    前言以及回顧 各位朋友，自從上一篇視覺(jué)SLAM漫談寫(xiě)成以來(lái)已經(jīng)有一段時(shí)間了。我收到幾位熱心讀者的郵件。有的希望我介紹一下當(dāng)前視覺(jué)SLAM程序的實(shí)用程度，更多的人希望了解一下前文提到的g2o優(yōu)化庫(kù)。因此我另寫(xiě)一篇小文章來(lái)專(zhuān)門(mén)介紹這個(gè)新玩意。 在開(kāi)始本篇文章正文以前，我們先來(lái)回顧一下圖優(yōu)化SLAM問(wèn)題的提法。至于SLAM更基礎(chǔ)的內(nèi)容，例如SLAM是

33、什么東西等等，請(qǐng)參見(jiàn)上一篇文章。我們直接進(jìn)入較深層次的討論。首先，關(guān)于我們要做的事情，你可以這樣想： l   已知的東西：傳感器數(shù)據(jù)（圖像，點(diǎn)云，慣性測(cè)量設(shè)備等）。我們的傳感器主要是一個(gè)Kinect，因此數(shù)據(jù)就是一個(gè)視頻序列，說(shuō)的再詳細(xì)點(diǎn)就是一個(gè)RGB位圖序列與一個(gè)深度圖序列。至于慣性測(cè)量設(shè)備，可以有也可以沒(méi)有。 l   待求的東西：機(jī)器人的運(yùn)動(dòng)軌跡，地圖的描述。運(yùn)動(dòng)軌跡，畫(huà)出來(lái)應(yīng)該就像是一條路徑。而地圖的描述，通常是點(diǎn)云的描述。但是點(diǎn)云描述是否可用于導(dǎo)航、規(guī)劃等后續(xù)問(wèn)題，還有待研究。 這兩個(gè)點(diǎn)之間還是有挺長(zhǎng)的路要走的。如果

34、我們使用圖優(yōu)化，往往會(huì)在整個(gè)視頻序列中，定義若干個(gè)關(guān)鍵幀： 這個(gè)圖著實(shí)畫(huà)的有點(diǎn)丑，請(qǐng)大家不要吐槽不管怎么說(shuō)，它表達(dá)出我想表達(dá)的意思。在這張圖中，我們有一個(gè)路標(biāo)點(diǎn)（五角星），并在各個(gè)關(guān)鍵幀中都看到了這個(gè)點(diǎn)。于是，我們就能用PnP或ICP求解相鄰關(guān)鍵點(diǎn)的運(yùn)動(dòng)方向。這些在上篇文章都介紹過(guò)了，包括特征選擇，匹配及計(jì)算等等。那么，這個(gè)過(guò)程中有什么問(wèn)題呢？2    為什么要用全局優(yōu)化你一定已經(jīng)注意到，理想的計(jì)算總和實(shí)際有差距的。好比說(shuō)理想的科研就是“看論文產(chǎn)生想法做實(shí)驗(yàn)發(fā)文章”，那么現(xiàn)實(shí)的科研就是“看論文產(chǎn)生想法做實(shí)驗(yàn)發(fā)現(xiàn)該想法在二十年前就有人做過(guò)了”，這樣一個(gè)過(guò)程

35、。實(shí)際當(dāng)中，僅通過(guò)幀間運(yùn)動(dòng)（ego-motion）來(lái)計(jì)算機(jī)器人軌跡是遠(yuǎn)遠(yuǎn)不夠的。如下圖所示：如果你只用幀間匹配，那么每一幀的誤差將對(duì)后面所有的運(yùn)動(dòng)軌跡都要產(chǎn)生影響。例如第二幀往右偏了0.1，那么后面第三、四、五幀都要往右偏0.1，還要加上它們自己的估算誤差。所以結(jié)果就是：當(dāng)程序跑上十幾秒之后早就不知道飛到哪兒去了。這是經(jīng)典的SLAM現(xiàn)象，在EKF實(shí)現(xiàn)中，也會(huì)發(fā)現(xiàn)，當(dāng)機(jī)器人不斷運(yùn)動(dòng)時(shí)，不確定性會(huì)不斷增長(zhǎng)。當(dāng)然不是我們所希望的結(jié)果。那么怎么辦才好呢？想象你到了一個(gè)陌生的城市，安全地走出了火車(chē)站，并在附近游蕩了一會(huì)兒。當(dāng)你走的越遠(yuǎn)，看到許多未知的建筑。你就越搞不清楚自己在什么地方。如果是你，你會(huì)怎么

36、辦？通常的做法是認(rèn)準(zhǔn)一個(gè)標(biāo)志性建筑物，在它周?chē)D(zhuǎn)上幾圈，弄清楚附近的環(huán)境。然后再一點(diǎn)點(diǎn)兒擴(kuò)大我們走過(guò)的范圍。在這個(gè)過(guò)程中，我們會(huì)時(shí)?；氐街耙呀?jīng)見(jiàn)過(guò)的場(chǎng)景，因此對(duì)它周?chē)木跋缶蜁?huì)很熟悉。機(jī)器人的情形也差不多，除了大多數(shù)時(shí)候是人在遙控它行走。因而我們希望，機(jī)器人不要僅和它上一個(gè)幀進(jìn)行比較，而是和更多先前的幀比較，找出其中的相似之處。這就是所謂的回環(huán)檢測(cè)（Loop closure detection）。用下面的示意圖來(lái)說(shuō)明：沒(méi)有回環(huán)時(shí)，由于誤差對(duì)后續(xù)幀產(chǎn)生影響，機(jī)器人路徑估計(jì)很不穩(wěn)定。加上一些局部回環(huán)，幾個(gè)相鄰幀就多了一些約束，因而誤差就減少了。你可以把它看成一個(gè)由彈簧連起來(lái)的鏈條（質(zhì)點(diǎn)-彈簧模型

37、）。當(dāng)機(jī)器人經(jīng)過(guò)若干時(shí)間，回到最初地方時(shí)，檢測(cè)出了大回環(huán)時(shí)，整個(gè)環(huán)內(nèi)的結(jié)構(gòu)都會(huì)變得穩(wěn)定很多。我們就可以籍此知道一個(gè)房間是方的還是圓的，面前這堵墻對(duì)應(yīng)著以前哪一堵墻，等等。相信講到這里，大家對(duì)回環(huán)檢測(cè)都有了一個(gè)感性的認(rèn)識(shí)。那么，這件事情具體是怎么建模，怎么計(jì)算，怎么編程呢？下面我們就一步步來(lái)介紹。3    圖優(yōu)化的數(shù)學(xué)模型SLAM問(wèn)題的優(yōu)化模型可以有幾種不同的建模方式。我們挑選其中較簡(jiǎn)單的一種進(jìn)行介紹，即FrameSLAM，在2008年提出。它的特點(diǎn)是只用位姿約束而不用特征約束，減少了很多計(jì)算量，表達(dá)起來(lái)也比較直觀(guān)。下面我們給出一種6自由度的3D SLAM建模方法。

38、符號(hào)：注意到這里的建模與前文有所不同，是一個(gè)簡(jiǎn)化版的模型。因?yàn)槲覀兗僭O(shè)幀間匹配時(shí)得到了相鄰幀的變換矩陣，而不是把所有特征也放到優(yōu)化問(wèn)題里面來(lái)。所以這個(gè)模型看上去相對(duì)簡(jiǎn)單。但是它很實(shí)用，因?yàn)椴挥靡胩卣?，所以結(jié)點(diǎn)和邊的數(shù)量大大減少，要知道在圖像里提特征動(dòng)輒成百上千的。4    g2o是什么g2o，就是對(duì)上述問(wèn)題的一個(gè)求解器。它原理上是一個(gè)通用的求解器，并不限定于某些SLAM問(wèn)題。你可以用它來(lái)求SLAM，也可以用ICP, PnP以及其他你能想到的可以用圖來(lái)表達(dá)的優(yōu)化問(wèn)題。它的代碼很規(guī)范，就是有一個(gè)缺點(diǎn)：文檔太少。唯一的說(shuō)明文檔還有點(diǎn)太裝叉（個(gè)人感覺(jué)）了，有點(diǎn)擺弄作者數(shù)學(xué)水平的意

39、思，反正那篇文檔很難懂就是了。話(huà)說(shuō)程序文檔不應(yīng)該是告訴我怎么用才對(duì)么言歸正傳。如果你想用g2o，請(qǐng)去它的github上面下載：它的API在：/stable/api/slam/g2o/classg2o_1_1HyperGraph.html4.1     安裝g2o是一個(gè)用cmake管理的C+工程，我是用Linux編譯的，所以不要問(wèn)我怎么在win下面用g2o，因?yàn)槲乙膊粫?huì)不管怎么說(shuō)，你下載了它的zip包或者用Git拷下來(lái)之后，里面有一個(gè)README文件。告訴你它的依賴(lài)項(xiàng)。在ubuntu下，直接鍵入命令：s

40、udo apt-get install cmake libeigen3-dev libsuitesparse-dev libqt4-dev qt4-qmake libqglviewer-qt4-dev我個(gè)人感覺(jué)還要 libcsparse-dev和freeglut3這兩個(gè)庫(kù)，反正多裝了也無(wú)所謂。注意libqglviewer-qt4-dev只在ubuntu 12.04庫(kù)里有，14.04 里換成另一個(gè)庫(kù)了。g2o的可視化工具g2o_viewer是依賴(lài)這個(gè)庫(kù)的，所以，如果你在14.04下面編，要么是去把12.04那個(gè)deb（以及它的依賴(lài)項(xiàng)）找出來(lái)裝好，要么用ccmake，把build apps一項(xiàng)給去

41、掉，這樣就不編譯這個(gè)工具了。否則編譯過(guò)不去。解開(kāi)zip后，新建一個(gè)build文件夾，然后就是：cmake .makesudo make install這樣g2o就裝到了你的/usr/local/lib和/usr/local/include下面。你可以到這兩個(gè)地方去看它的庫(kù)文件與頭文件。4.2     學(xué)習(xí)g2o的使用因?yàn)間2o的文檔真的很裝叉（不能忍），所以建議你直接看它的源代碼，耐心看，應(yīng)該比文檔好懂些。它的example文檔夾下有一些示例代碼，其中有一個(gè)tutorial_slam2d文件夾下有2d slam仿真的一個(gè)程序。值得仔細(xì)閱讀。使用g2o來(lái)實(shí)

42、現(xiàn)圖優(yōu)化還是比較容易的。它幫你把節(jié)點(diǎn)和邊的類(lèi)型都定義好了，基本上只需使用它內(nèi)置的類(lèi)型而不需自己重新定義。要構(gòu)造一個(gè)圖，要做以下幾件事：l   定義一個(gè)SparseOptimizer. 編寫(xiě)方式參見(jiàn)tutorial_slam2d的聲明方式。你還要寫(xiě)明它使用的算法。通常是Gauss-Newton或LM算法。個(gè)人覺(jué)得后者更好一些。l   定義你要用到的邊、節(jié)點(diǎn)的類(lèi)型。例如我們實(shí)現(xiàn)一個(gè)3D SLAM。那么就要看它的g2o/types/slam3d下面的頭文件。節(jié)點(diǎn)頭文件都以vertex_開(kāi)頭，而邊則以edge_開(kāi)頭。在我們上面的模型中，可以選擇vertex_se

43、3作為節(jié)點(diǎn)，edge_se3作為邊。這兩個(gè)類(lèi)型的節(jié)點(diǎn)和邊的數(shù)據(jù)都可以直接來(lái)自于Eigen:Isometry，即上面講到過(guò)的變換矩陣T。l   編寫(xiě)一個(gè)幀間匹配程序，通過(guò)兩張圖像算出變換矩陣。這個(gè)用OpenCV, pcl都可以做。l   把你得到的關(guān)鍵幀作為節(jié)點(diǎn)，變換矩陣作為邊，加入到optimizer中。同時(shí)設(shè)定節(jié)點(diǎn)的估計(jì)值（如果沒(méi)有慣性測(cè)量就設(shè)成零）與邊的約束（變換矩陣）。此外，每條邊還需設(shè)定一個(gè)信息矩陣（協(xié)方差矩陣之逆）作為不確定性的度量。例如你覺(jué)得幀間匹配精度在0.1m，那么把信息矩陣設(shè)成100的對(duì)角陣即可。l   在程序運(yùn)行過(guò)

44、程中不斷作幀間檢測(cè)，維護(hù)你的圖。l   程序結(jié)束時(shí)調(diào)用optimizer.optimize( steps )進(jìn)行優(yōu)化。優(yōu)化完畢后讀取每個(gè)節(jié)點(diǎn)的估計(jì)值，此時(shí)就是優(yōu)化后的機(jī)器人軌跡。代碼這種東西展開(kāi)來(lái)說(shuō)會(huì)變得像字典一樣枯燥，所以具體的東西需要大家自己去看，自己去體會(huì)。這里有我自己寫(xiě)的一個(gè)程序，可以供大家參考。不過(guò)這個(gè)程序需要帶著數(shù)據(jù)集才能跑，學(xué)習(xí)g2o的同學(xué)只需參考里面代碼的寫(xiě)法即可：5    效果最近我跑了幾個(gè)公開(kāi)數(shù)據(jù)集（http:/vision.in.tum.de/data/datasets/rgbd-dataset）上的例子（fr1_desk, fr2

45、_slam)(，感覺(jué)效果還不錯(cuò)。有些數(shù)據(jù)集還是挺難的。最后一張圖是g2o_viewer，可以看到那些關(guān)鍵路徑點(diǎn)與邊的樣子。 視覺(jué)SLAM漫談 (三): 研究點(diǎn)介紹1.前言讀者朋友們大家好！（很久很久）之前，我們?yōu)榇蠹医榻B了SLAM的基本概念和方法。相信大家對(duì)SLAM，應(yīng)該有了基本的認(rèn)識(shí)。在忙完一堆寫(xiě)論文、博士開(kāi)題的事情之后，我準(zhǔn)備回來(lái)繼續(xù)填坑：為大家介紹SLAM研究的方方面面。如果前兩篇文章算是"初識(shí)"，接下來(lái)幾篇就是"漸入佳境"了。在第三篇中，我們要談?wù)凷LAM中的各個(gè)研究點(diǎn)，為研究生們（應(yīng)該是博客的多數(shù)讀者吧）作一個(gè)提綱挈領(lǐng)的摘要。然后，我

46、們?cè)倬透鱾€(gè)小問(wèn)題，講講經(jīng)典的算法與分類(lèi)。我有耐心講，你是否有耐心聽(tīng)呢？在SLAM for Dummy中，有一句話(huà)說(shuō)的好："SLAM并不是一種算法，而是一個(gè)概念。（SLAM is more like a concept than a single algorithm.）"所以，你可以和導(dǎo)師、師兄弟（以及師妹，如果有的話(huà)）說(shuō)你在研究SLAM，但是，作為同行，我可能更關(guān)心：你在研究SLAM中的哪一個(gè)問(wèn)題。有些研究者專(zhuān)注于實(shí)現(xiàn)一個(gè)具體的SLAM系統(tǒng)，而更多的人則是在研究SLAM里某些方法的改進(jìn)。做應(yīng)用和做理論的人往往彼此看不起，不過(guò)二者對(duì)科研都是有貢獻(xiàn)的。作為研究生，我還是建議各位

47、抓住SLAM中一個(gè)小問(wèn)題，看看能否對(duì)現(xiàn)有的算法進(jìn)行改進(jìn)或者比較。不要覺(jué)得這種事情膚淺，它是對(duì)研究有實(shí)際幫助和意義的。同時(shí)，我也有一些朋友，做了一個(gè)基于濾波器/圖優(yōu)化的SLAM實(shí)現(xiàn)。程序是跑起來(lái)了，但他/她不知道自己有哪些貢獻(xiàn)，鉆研了哪個(gè)問(wèn)題，寫(xiě)論文的時(shí)候就很頭疼。所以，作為研究生，我建議你選擇SLAM中的一個(gè)問(wèn)題，改進(jìn)其中的算法，而不是先找一堆程序跑起來(lái)再說(shuō)。那么問(wèn)題來(lái)了：SLAM方面究竟有哪些可以研究的地方呢？我為大家上一個(gè)腦圖。這個(gè)圖是從我筆記本上拍下來(lái)的（請(qǐng)勿吐槽字和對(duì)焦）?？梢钥吹?，以SLAM為中心，有五個(gè)圈連接到它。我稱(chēng)它為Basic Theory（基礎(chǔ)理論）、Sensor（傳感器）

48、、Mapping（建圖）、Loop Detection（回環(huán)檢測(cè)）、Advanced Topic（高級(jí)問(wèn)題）。這可以說(shuō)是SLAM的研究方向。下面我們"花開(kāi)五朵，各表一枝"。2.基本理論SLAM的基本理論，是指它的數(shù)學(xué)建模。也就是你如何用數(shù)學(xué)模型來(lái)表達(dá)這個(gè)問(wèn)題。為什么說(shuō)它"基本"呢？因?yàn)閿?shù)學(xué)模型影響著整個(gè)系統(tǒng)的性能，決定了其他問(wèn)題的處理方法。在早先的研究中（86年提出1至21世紀(jì)前期2），是使用卡爾曼濾波器的數(shù)學(xué)模型的。那里的機(jī)器人，就是一個(gè)位姿的時(shí)間序列；而地圖，就是一堆路標(biāo)點(diǎn)的集合。什么是路標(biāo)點(diǎn)的集合？就是用(x,y,z)表示每一個(gè)路標(biāo)，然后在濾波器更

49、新的過(guò)程中，讓這三個(gè)數(shù)慢慢收斂。那么，請(qǐng)問(wèn)這樣的模型好不好？好處是可以直接套用濾波器的求解方法。卡爾曼濾波器是很成熟的理論，比較靠譜。缺點(diǎn)呢？首先，濾波器有什么缺點(diǎn)，基于它的SLAM就有什么缺點(diǎn)。所以EKF的線(xiàn)性化假設(shè)啊，必須存儲(chǔ)協(xié)方差矩陣帶來(lái)的資源消耗啊，都成了缺點(diǎn)（之后的文章里會(huì)介紹）。然后呢，最直觀(guān)的就是，用(x,y,z)表示路標(biāo)？萬(wàn)一路標(biāo)變了怎么辦？平時(shí)我們不就把屋里的桌子椅子挪來(lái)挪去的嗎？那時(shí)候?yàn)V波器就掛了。所以啊，它也不適用于動(dòng)態(tài)的場(chǎng)合。這種局限性就是數(shù)學(xué)模型本身帶來(lái)的，和其他的算法無(wú)關(guān)。如果你希望在動(dòng)態(tài)環(huán)境中跑SLAM，就要使用其他模型或改進(jìn)現(xiàn)有的模型了。SLAM的基本理論，向來(lái)

50、分為濾波器和優(yōu)化方法兩類(lèi)。濾波器有擴(kuò)展卡爾曼濾波（EKF）、粒子濾波（PF），F(xiàn)astSLAM等，較早出現(xiàn)。而優(yōu)化方向用姿態(tài)圖（Pose Graph），其思想在先前的文章中介紹過(guò)。近年來(lái)用優(yōu)化的逐漸增多，而濾波器方面則在13年出現(xiàn)了基于Random Finite Set的方法3，也是一個(gè)新興的浪潮4。關(guān)于這些方法的詳細(xì)內(nèi)容，我們?cè)诮窈蟮奈恼轮性龠M(jìn)行討論。作為SLAM的研究人員，應(yīng)該對(duì)各種基本理論以及優(yōu)缺點(diǎn)有一個(gè)大致的了解，盡管它們的實(shí)現(xiàn)可能非常復(fù)雜。3.傳感器傳感器是機(jī)器人感知世界的方式。傳感器的選擇和安裝方式，決定了觀(guān)測(cè)方程的具體形式，也在很大程度上影響著SLAM問(wèn)題的難度。早期的SLAM多

51、使用激光傳感器（Laser Range Finder），而現(xiàn)在則多使用視覺(jué)相機(jī)、深度相機(jī)、聲吶（水下）以及傳感器融合。我覺(jué)得該方向可供研究點(diǎn)有如下幾個(gè)：· 如何使用新興傳感器進(jìn)行SLAM。    要知道傳感器在不斷發(fā)展，總有新式的東西會(huì)出來(lái)，所以這方面研究肯定不會(huì)斷。· 不同的安裝方式對(duì)SLAM的影響。    舉例來(lái)說(shuō)，比如相機(jī)，頂視（看天花板）和下視（看地板）的SLAM問(wèn)題要比平視容易很多。為什么容易呢？因?yàn)轫?下視的數(shù)據(jù)非常穩(wěn)定，不像平視，要受各種東西的干擾。當(dāng)然，你也可以研究其他的安裝方式

52、。· 改進(jìn)傳統(tǒng)傳感器的數(shù)據(jù)處理。        這部分就有些困難了，因?yàn)榻?jīng)常傳感器已經(jīng)有很多人在使用，你做的改進(jìn)，未必比現(xiàn)有的成熟方法更好。4.建圖建圖，顧名思議，就是如何畫(huà)地圖唄。其實(shí)，如果知道了機(jī)器人的真實(shí)軌跡，畫(huà)地圖是很簡(jiǎn)單的一件事。不過(guò)，地圖的具體形式也是研究點(diǎn)之一。比如說(shuō)常見(jiàn)的有以下幾種：· 路標(biāo)地圖。       地圖由一堆路標(biāo)點(diǎn)組成。EKF中的地圖就是這樣的。但是，也有人說(shuō)，這真的是地圖嗎（這些零零碎碎的點(diǎn)都是什

53、么啊喂）？所以路標(biāo)圖盡管很方便，但多數(shù)人對(duì)這種地圖是不滿(mǎn)意的，至少看上去不像個(gè)地圖啊。于是就有了密集型地圖（Dense map）。· 度量地圖（Metric map）    通常指2D/3D的網(wǎng)格地圖，也就是大家經(jīng)常見(jiàn)的那種黑白的/點(diǎn)云式地圖。點(diǎn)云地圖比較酷炫，很有種高科技的感覺(jué)。它的優(yōu)點(diǎn)是精度比較高，比如2D地圖可以用0-1表示某個(gè)點(diǎn)是否可通過(guò)，對(duì)導(dǎo)航很有用。缺點(diǎn)是相當(dāng)吃存儲(chǔ)空間，特別是3D，把所有空間點(diǎn)都存起來(lái)了，然而大多數(shù)角角落落里的點(diǎn)除了好看之外都沒(méi)什么意義· 拓?fù)涞貓D（Topological map）  

54、     拓?fù)涞貓D是比度量地圖更緊湊的一種地圖。它將地圖抽象為圖論中的"點(diǎn)"和"邊"，使之更符合人類(lèi)的思維。比如說(shuō)我要去五道口，不知道路，去問(wèn)別人。那人肯定不會(huì)說(shuō)，你先往前走621米，向左拐94.2度，再走1035米（這是瘋子吧）。正常人肯定會(huì)說(shuō)，往前走到第二個(gè)十字路口，左拐，走到下一個(gè)紅綠燈，等等。這就是拓?fù)涞貓D。· 混合地圖。        既然有人要分類(lèi)，就肯定有人想把各類(lèi)的好處揉到一起。這個(gè)就不多說(shuō)了吧。5.

55、回環(huán)檢測(cè)回環(huán)檢測(cè)，又稱(chēng)閉環(huán)檢測(cè)（Loop closure detection），是指機(jī)器人識(shí)別曾到達(dá)場(chǎng)景的能力。如果檢測(cè)成功，可以顯著地減小累積誤差?；丨h(huán)檢測(cè)目前多采用詞袋模型（Bag-of-Word），研究計(jì)算機(jī)視覺(jué)的同學(xué)肯定不會(huì)陌生。它實(shí)質(zhì)上是一個(gè)檢測(cè)觀(guān)測(cè)數(shù)據(jù)相似性的問(wèn)題。在詞袋模型中，我們提取每張圖像中的特征，把它們的特征向量（descriptor）進(jìn)行聚類(lèi)，建立類(lèi)別數(shù)據(jù)庫(kù)。比如說(shuō)，眼睛、鼻子、耳朵、嘴等等（實(shí)際當(dāng)中沒(méi)那么高級(jí)，基本上是一些邊緣和角）。假設(shè)有10000個(gè)類(lèi)吧。然后，對(duì)于每一個(gè)圖像，可以分析它含有數(shù)據(jù)庫(kù)中哪幾個(gè)類(lèi)。以1表示有，以0表示沒(méi)有。那么，這個(gè)圖像就可用10000維的一個(gè)向量來(lái)表達(dá)。而不同的圖像，只要比較它們的向量即可?；丨h(huán)檢測(cè)也可以建成一個(gè)模型識(shí)別問(wèn)題，所以你也可以使用各種機(jī)器學(xué)習(xí)的方法來(lái)做，比如什么決策樹(shù)/SVM，也可以