磨蹭了好久終于開始寫本行SLAM系列了。鄙人目前水平一般谓着，但接下來的幾年也都會在這個方向繼續(xù)深入下去泼诱，所以打算盡我所能地寫一個完整的SLAM從入門到精(fang)通(qi)的系列。文章的內容大概會分為三個大部分
1：2維平面的SLAM系統(tǒng)
2: 視覺SLAM
3：視覺慣性SLAM
每一個大部分都會分為代碼和理論兩個小部分赊锚，以幫助大家理解如何實現(xiàn)治筒。
絕大多數的代碼會在ROS的幫助下完成，這也方便大家在學習完ROS入門系列之后看看利用ROS寫稍微大一點的程序是什么樣子舷蒲。
2維平面的SLAM系統(tǒng)主要參考<probabilistic robotics>
視覺SLAM會主要參考<multiple view geometry>, 一些關于非線性最小二乘優(yōu)化的書籍以及<視覺SLAM十四講>耸袜。代碼參考ORBSLAM2等。視覺SLAM的教學代碼有高翔博士的<一起來做RGB-D SLAM>牲平，但是也有一些年份了堤框，可以用更新的了。
視覺慣性SLAM主要參考一些論文了，比如和VINS-Fusion,OKVIS以及預積分相關的論文胰锌，我們會在以后說到骗绕。
可能好多人會直接想搞視覺SLAM部分而不想從2d部分學起，但是對于入門者來說资昧，2d的理論和代碼都會更簡單一些酬土，方便看懂為后面進階鋪路。
最后說一下作者是興趣使然的更新格带，看心情更新撤缴，更新時間很長，也說不定斷更叽唱。

基本要求

理論上你需要有一些基本的高數知識屈呕，比如怎么求積分怎么求偏導數什么的，基本線性代數知識棺亭，比如矩陣的初級操作虎眨，求逆什么的，以及概率論的基本知識镶摘，高斯分布嗽桩，多變量高斯分布，全概率公式凄敢，貝葉斯法則什么的碌冶，編程上你需要對C++有基本的了解，知道并能編寫簡單的函數涝缝，類扑庞。不了解也不用擔心，稍微深入的地方我都會講到拒逮。

基本概念

SLAM全稱Simultaneous Localization and Mapping罐氨，即同時定位與制圖。定位的概念很好理解消恍，如果一個機器人來到陌生的環(huán)境岂昭，它需要知道自己在哪兒，數學上來說就是知道自己的坐標狠怨，機器人如果在移動，就需要時刻的坐標更新邑遏。制圖的意思是指對周圍環(huán)境的了解佣赖，對周圍環(huán)境的了解能幫助你更好地定位自己。制圖分為稀疏制圖(sparse mapping)和稠密建圖(dense mapping)记盒。稀疏制圖表示你對周圍的環(huán)境只有部分的了解憎蛤，而稠密建圖則表示你對周圍的環(huán)境的每一個點都清楚。打個比方，你和父母一起出游俩檬，結果走散了萎胰，你給父母打電話，他們問你在哪兒棚辽，這時候你需要定位了(咳咳假設你不能直接發(fā)定位給他們)技竟，你告訴他們:"我在xx路上，后面有一個xx樓屈藐，上面寫著xxx榔组。"你說的內容，提取了當前環(huán)境比較特征的部分联逻，而這些比較特征的部分搓扯，這些就足夠幫住確定你的位置了，這就屬于稀疏建圖包归。如果你告訴父母:"我在xx路上锨推，前面是xx，后面是xx公壤，左邊是xx换可，右邊是xx，斜前方是xx境钟，斜后方是xx....."總之你把你周圍的一切東西都描述了出來锦担，這就就是稠密建圖。稠密建圖當然能幫助你定位慨削，但很顯然洞渔，如果你只是想單純地定位，你給出的信息就太冗余了缚态。所以SLAM中的定位只需要稀疏建圖就可以了磁椒，你了解周圍環(huán)境的一些特征，就足夠能確定你的坐標了玫芦。
實際的稀疏建圖浆熔，是提取環(huán)境中一些比較特征的點桥帆。如果你有一個相機，在獲取一張照片之后老虫，根據特定的算法叶骨，能把照片上一些比較有特色的點找出來，這些比較特色的點就能幫助你定位祈匙『龉簦總的來說SLAM里的地圖不是大家想象中的百度地圖高德地圖google地圖那樣天揖，而是一些從環(huán)境中提取出來的稀疏的比較有特點的點。
關于定位，還得介紹一下室內外的定位方法。在室外的話载城，利用GPS直接定位是很好的選擇蘸吓，不過普通民用GPS精度一般，誤差可以有3米左右，也有高精度的GPS，誤差可以到達厘米級，但是價格很貴赖钞。關于SLAM的室外應用，很多人肯定很感興趣無人車方面的技術聘裁。無人車現(xiàn)在分級是L0~L5相信這個大家很多都聽說過雪营。其中有部分公司達到了L4級的無人車系統(tǒng)，這意味著在提供一定的輔助條件下衡便，汽車可以自動駕駛献起。而這個輔助條件，比如說有預先制好的地圖镣陕。再用前面那個你需要給父母提供你位置信息的例子來說谴餐，你的父母如果對當地很熟悉，也就是他們腦海中已經有地圖了呆抑，你打電話的內容就可能更省略:"我在XX樓旁邊“他們就能找到你了岂嗓。回到無人車的例子鹊碍，如果周圍的環(huán)境已經在出發(fā)前就被知道了厌殉，無人車就能省掉一部分計算力并且定位的精度更高。如何預先知道環(huán)境呢侈咕？在現(xiàn)實中公罕，可能專門有一輛車，利用激光雷達(lidar)在一條路上先開一次耀销，獲取到周圍環(huán)境的3D點云楼眷，儲存起來，這樣地圖就預先構建好了熊尉，能省掉很大的計算力(就好像之前的例子你給父母提供位置信息時能少說話)罐柳。所以說嚴格來講目前大多數公司的無人車在室外并不是完全使用SLAM的技術，他們不僅有SLAM的影子(利用激光雷達或者相機等幫助定位并提取路人狰住，車子等特征)硝清，同時也借助了預先制好的地圖和GPS等其他傳感器輔助定位，以達到盡可能好的效果转晰。雖然不是完完全全的自動化，但是L4級的無人駕駛還是很有意義的查邢。比如說應用于公交車或者其他只在限定范圍內運動的車輛，我們完全可以對周圍的道路進行預先制圖從而節(jié)省汽車在真正行駛過程中的計算量扰藕。甚至普通家用車缓苛，你如果大多數時候只在某個城市什么的行駛邓深，也完全可以有專門的公司去預先制圖。
而L5級的無人駕駛芥备，其實才是真正的SLAM應用冬耿。L5級的無人駕駛不限環(huán)境和條件萌壳，意味沒有預先制定的地圖，我們必須真正地同時定位和制圖袱瓮。一般也需要GPS的輔助定位缤骨。當然無人駕駛除了SLAM還需要有很多其他技術，比如控制绊起，決策，利用深度學習提取路人等信息什么的燎斩，要把他們融合起來非常復雜。L5級的無人駕駛據我目前所知沒有公司達到瘫里，室外的SLAM應用也并沒有那么成熟(等一下不要砸自己飯碗啊喂Ｊ当巍谨读！)局装，理論雖然很成熟了劳殖，但是面對千奇百怪的實際環(huán)境，那些很fancy的算法也是有些捉襟見肘哆姻。L3,L4級無人駕駛能滿足很多無人駕駛的需求了宣增，他們需要但不完全依賴于SLAM矛缨。至于L5級目前是一個目標帖旨，沖鴨！灵妨！
室內的定位就沒有GPS的用武之地了，完全SLAM的地盤了泌霍。有名的應用領域比如掃地機器人什么的货抄。

basic_idea.png

基本原理

這兒我用一個不算非常恰當但非常簡單的例子幫助初學者對同時定位制圖的計算原理有一個直觀的了解朱转。請參考上面basic_idea.png的圖。
假設黑圓圈是一輛小車藤为，方塊A是環(huán)境中一個固定的點怪与。小車從位置B運動到了位置C。小車在位置B時通過某種傳感器測得A的位置是距離米琼梆，同時通過傳感器和一些小的計算能知道AB和B運動方向之間的夾角即。運動到位置C后測得A的位置距離它是米窿吩，同樣可以知道。這時候如果B點的坐標是纫雁，那三角形知道兩邊兩角你能不能算第三邊呢煌往？
其實不僅我們可以通過測A和小車之間的距離最終了解的長度轧邪，我們當然還有傳感器能測小車的速度之類的，假設小車從B運動到C只經歷了很短的時間忌愚，我們測得小車在B點的速度延BC方向是曲管，這很短的時間內我們可以假設小車的速度沒有發(fā)生改變硕糊。那你能不能算邊的長度呢院水？
可以說這兩個問題都太簡單了简十，但是如果你能完成你就已經完成SLAM即同時定位與制圖了。A屬于環(huán)境中的點螟蝙，你能根據傳感器確定它的位置恢恼，這就是制圖胰默；你根據小車在B的位置和一系列傳感器數據以及小車的運動模型(它沿BC在極短的時間內坐勻速直線運動)推斷出小車在C的位置漓踢，這就是定位。合起來就是Simultaneous Localization and Mapping和簸。
說地這么簡單彭雾，為什么實際想實現(xiàn)一個SLAM系統(tǒng)卻沒那么容易呢锁保？我們來看看如果面臨現(xiàn)實情況會有哪些問題半沽。
Question1:我們說通過測量A相對于B，C的位置構成一個三角形能輔助我們算出者填，通過測得小車在B的速度以及假設它勻速直線運動我們也能算出浩村。如果傳感器完美測量沒有任何誤差占哟，運動模型也完美心墅，這兩種方式算出的應該一樣榨乎。但當然不會完美。假設測得的長度和真實值差0.01m,測得的長度和真實值差0.01m蜜暑，那算出的長度就會和真實值有差距铐姚，我們把這個差距記為肛捍。如果小車在B測得的速度和真實的速度有0.001m/s的差距，小車從B到C也并不是完美的勻速直線運動拙毫，那算出的長度也會和真實值有差距依许，記為。傳感器不同峭跳，肯定會不一樣。那么我們該使用哪個值袍患？
Question2:我們非常完美地假設能直接測得的長度，視覺SLAM中我們有相機作為傳感器诡延，普通的相機拍出的是一張圖片滞欠，即二維平面肆良，A位于這個平面中筛璧，我們怎么能確定這個平面中一個點到自己的具體距離？
Question3:假設我們已經能在小車在B點時算出A的位置夭谤，小車運動到C時算出A距離C的位置棺牧，但我怎么知道這兩個A是一個A??有點繞口朗儒，這么說，小車在B點拍下了當前環(huán)境的照片醉锄，有一棵樹乏悄，在C點拍下了照片，有同樣一顆樹檩小，這兩張照片儲存在電腦中，但是電腦不會知道這是同一棵樹烟勋。圖片作為數字矩陣儲存在電腦中，這兩棵樹在電腦里面都只是一堆不相關數字而已卵惦。
Question4:環(huán)境中不僅有點A阻肿，還有點M,N,L....，假設每一個點我們都能測出到小車的距離冕茅，但是由于傳感器的測量有誤差，并且測出的誤差各不相同蛹找，有的差0.01m有的差0.0095米，有的差0.011米庸疾，環(huán)境中的每一個點都和B,C形成了一個三角形乍楚，每一個三角形都能算出的長度且由于誤差算出的各不相同届慈，那么和Question1類似的問題又來了徒溪，我們使用哪一個 金顿？
Question5:由于我們前面的說的測量的誤差，無論我們選哪個揍拆，或者用某種方式把這些整合起來算一個新的（最簡單的渠概，加起來求個平均什么的），都不會是真實的值播揪，仍然會和真實的值有一定誤差。當我們計算出的C的位置和實際位置差0.01米猪狈，之后小車從C運動到D又算差0.011米箱沦，之后小車從D運動到E又差0.0095米...每一個是0.1s的話雇庙，如此累積谓形，只需要10s小車的位置誤差就能達到米級状共！誤差的累積是不可避免的。面對這種情況峡继，我們又該怎么辦？
等等還有一系列的問題匈挖，其實最主要的還是來自于測量的不準確性和模型的不準確性(就像小車不是完美勻速直線運動)。現(xiàn)實的SLAM如何處理這些問題儡循，我們會在以后的講解中一一解答

符號約定

在理論講解時舶吗，會涉及到很多符號描述择膝。符號這個東西誓琼，有些統(tǒng)一了有些沒統(tǒng)一還真是麻煩肴捉。我們講解的符號約定如下，普通沒加粗的希臘字母表示一個數字齿穗，或者英語數學里常說scalar傲隶，比如窃页；加粗的小寫字符表示向量跺株；大寫加粗表示矩陣畦木；空心的加粗大寫表示集合袖扛，比如實數集。