最近一個月開始看關於slam的研究內容，現在把最近的看到的一些內容做個小結，加深印象，促使自己思考，
其中可能有錯誤，希望大家不吝賜教。另外本博文雖然是我整理的，但是大部分都是別人的思想，版權歸原作者
所有，我已經在文中註明作者，在此向原作者表示感謝！

SLAM（simultaneous localization and mapping）也叫即時定位與導航，最早由Hugh Durrant-Whyte 和 John J.Leonard提出。SLAM主要用於解決移動機器人在未知環境中執行時定位導航與地圖構建的問題。典型的slam系統硬體組成主要由測距感測器，odometry 里程計，處理器，IMU,運動機器人等。最簡單的結構為可以移動的機器人平臺並且至少包含一個測距單元。其過程包括特徵提取，資料關聯，狀態估計，狀態更新以及特徵更新等。對於其中每個部分，均存在多種實現方法。

slam 的一般過程：

假設機器人初始化的時候的位置是確定的，從這個確定的位置上機器人觀測到一個特徵點的位置，這個特徵點具有和測距感測器相關的不確定性。
當機器人移動的時候，由於里程計的累積誤差它的位置的不確定度逐漸增加：

當機器人到達此處，看到兩個特徵點m1和m2，並估計出他們的位置和由於機器人的位置的不確定度造成的不確定度。此時我們可以看到，地圖的不確定度是機器人的位置估計的不確定度造成的。同樣的道理，如果機器人的位置估計建立在不準確估計的對特徵點之上，機器人位置的估計結果也是和特徵點的不確定度相關。

機器人繼續移動，它的不確定性由於里程計的累積誤差繼續增加

為了減小不確定度，機器人必須找到位置已知的特徵點。這些特徵點可以是機器人曾經見到過的特徵點。這就是所謂的迴環檢測。
如果迴環檢測成功，機器人的位置的不確定度就會縮小。同時，地圖也會更新，地圖上的特徵點和機器人過去迴環中的位置的不確定度都會減小。

這就是slam的大致過程（翻譯自Sören Schwertfeger的報告）。
上面對機器人位置的更新一般通過里程計來估計，如果不是平穩的過程還會用到IMU來矯正。SLAM按照測距單元分主要分為鐳射SLAM和視覺SLAM。鐳射最常用，視覺快速發展。也有用超聲波的，它價格便宜，測量精度較差。但在水下，由於其穿透力較強，因而，是最為常用的測距方式。最為常用的超聲波測距單元是Polaroid超聲波發生器。

鐳射SLAM

鐳射SLAM是研究比較早的方法，理論與實踐都比較成熟。鐳射SLAM運算量相對較小，易於實現實時建圖，精度較高。其缺點在於價格一般比較昂貴。鐳射測距單元的另外一個問題是其穿過玻璃平面的問題。另外鐳射測距單元不能夠應用於水下測量。幾何區分度小，具有稀疏性。

鐳射SLAM研究歷史

1.反光板定位（二維）
2.ekf特徵定位（二維）
 主要形式為角點、線。
 一般要配以里程計、imu，或者運動模型來提供運動預測
 演算法以ekf、粒子濾波等模型形式。
3.點或特徵的ICP（二維）
    里程計推導用直接的ICP替代了之前的ekf過程。
    定位使用粒子濾波方式。
    最有代表性的是gmapping
4.三維解決方式
    點抽樣->ICP
    特徵（點、線、面）->ekf icp
    NDT

使用的狀態估計和優化演算法：

基於濾波器：Kalman Filter, EKF, PF
基於非線性優化：圖優化、因子圖

• Significant advantage of graph-based SLAM techniques over EKF SLAM:
• EKF SLAM: computation and memory for to update and store the covariance matrix is
quadratic with the number of features.
• Graph-based SLAM: update time of the graph is constant and the required memory is linear
in the number of features.
• However, the final graph optimization can become computationally costly if the
robot path is long.

開源的實現方案：

2D
主要使用，hokuyo或者 sick 的2D 鐳射感測器，適用於平面內的運動。
以下是較成熟的slam方法。
A . HectorSLAM
scan-matching(Gaussian-Newton equation) + 　感測器的要求高
要求: 高更新頻率小測量噪聲的鐳射掃描器. 不需要里程計,使空中無人機與地面小車在不平坦區域執行存在運用的可能性.
利用已經獲得的地圖對鐳射束點陣進行優化, 估計鐳射點在地圖的表示,和佔據網格的概率.
其中掃描匹配利用的是高斯牛頓的方法進行求解. 找到鐳射點集對映到已有地圖的剛體轉換(x,y,theta).
( 接觸的匹配的方法還有最近鄰匹配的方法(ICP) ,gmapping程式碼中的scanmatcher部分有兩種方法選擇. )
為避免區域性最小而非全域性最優的(類似於多峰值模型的,區域性梯度最小了,但非全域性最優)出現,地圖採用多解析度的形式.
　　導航中的狀態估計可以加入慣性測量，進行ＥＫＦ濾波．

B. Gmapping: tutorial
proposed by Grisetti et al. and is a Rao-Blackwellized PF SLAM approach.
adaptive resampling technique
　 目前鐳射2Dslam用得最廣的方法，gmapping採用的是RBPF的方法. 必須得了解粒子濾波(利用統計特性描述物理表示式下的結果)的方法.
粒子濾波的方法一般需要大量的粒子來獲取好的結果,但這必會引入計算的複雜度;粒子是一個依據過程的觀測逐漸更新權重與收斂的過程,這種重取樣的過程必然會代入粒子耗散問題(depletion problem), 大權重粒子顯著,小權重粒子會消失(有可能正確的粒子模擬可能在中間的階段表現權重小而消失).
自適應重取樣技術引入減少了粒子耗散問題 , 計算粒子分佈的時候不單單僅依靠機器人的運動(里程計),同時將當前觀測考慮進去, 減少了機器人位置在粒子濾波步驟中的不確定性. (FAST-SLAM 2.0 的思想，可以適當減少粒子數)

C. KartoSLAM
graph-based SLAM approach developed bySRI International’s Karto Robotics
highly-optimized and non iterative Cholesky matrix decomposition for sparse linear systems as its solver
the Sparse Pose Adjustment (SPA) is responsible for both scan matching and loop-closure procedures
Karto Open Libraries 2.0 SDK(Karto Open Libraries 2.0 is available under the LGPL open source license. You can try the full Karto SDK 2.1 for 30 days.) 後面在詳細研究下(比較下ＭRPT )
圖優化的核心思想我認為主要就是 矩陣的稀疏化與最小二乘..參見graphslam學習
　　KartoSLAM是基於圖優化的方法，用高度優化和非迭代 cholesky矩陣進行稀疏系統解耦作為解．　圖優化方法利用圖的均值表示地圖，每個節點表示機器人軌跡的一個位置點和感測器測量資料集，箭頭的指向的連線表示連續機器人位置點的運動，每個新節點加入，地圖就會依據空間中的節點箭頭的約束進行計算更新．
　　KartoSLAM的ROS版本，其中採用的稀疏點調整（the Spare Pose Adjustment(SPA)）與掃描匹配和閉環檢測相關．landmark越多,記憶體需求越大,然而圖優化方式相比其他方法在大環境下製圖優勢更大.在某些情況下KartoSLAM更有效,因為他僅包含點的圖(robot pose),求得位置後再求map.

D. CoreSLAM
tinySLAM algorithm: two different steps(distance calculation and update of the map
simple and easy
為了簡單和容易理解最小化效能損失的一種slam演算法.將演算法簡化為距離計算與地圖更新的兩個過程, 第一步,每次掃描輸入,基於簡單的粒子濾波演算法計算距離,粒子濾波的匹配器用於鐳射與地圖的匹配,每個濾波器粒子代表機器人可能的位置和相應的概率權重,這些都依賴於之前的迭代計算. 選擇好最好的假設分佈,即低權重粒子消失,新粒子生成..在更新步驟,掃描得到的線加入地圖中,當障礙出現時,圍繞障礙點繪製調整點集,而非僅一個孤立點.

E. LagoSLAM
Linear Approximation for Graph Optimization
the optimization process requires no initial guess
基本的圖優化slam的方法就是利用最小化非線性非凸代價函式.每次迭代, 解決區域性凸近似的初始問題來更新圖配置,過程迭代一定次數直到區域性最小代價函式達到. (假設起始點經過多次迭代使得區域性代價函式最小). LagoSLAM 是線性近似圖優化,不需要初始假設. 優化器的方法可以有三種選擇 Tree-based netORK Optimizer(TORO), g2o,LAGO

3D
3D SLAM技術是目前SLAM技術發展的方向，不僅在機器人領域，在VR（虛擬現實）、AR（增強現實）、室內高精地圖、室內定位與導航、人工智慧和深度學習等相交叉的領域，也是研究熱點，是up to date的技術；而2D SLAM技術是3D SLAM技術的一個子集，應用環境有限。採用3D SLAM技術的鐳射影像揹包式測繪機器人，可以爬山涉水、翻山越嶺，全空間、全地形地在運動中高精度建模，理論難度較大。
3D-slam使用32線或者64線鐳射感測器，典型的為velodyne 鐳射感測器。
3D SLAM的實現手段是多樣的。在室內等小範圍環境識別和構圖方面，可以採用視覺感測器，比如單目或雙目攝像機、微軟的kinect或Intel的real sense類的RGBD深度感測器等；在室外大場景環境中的工業級應用，往往採用鐳射和視覺感測器結合的里程計方法等。

實現演算法：
Cartographer: 谷歌開源同步定位與製圖庫, 這個庫可以實現2D和3D slam，SLAM 演算法結合了來自多個感測器的資料，比如 LiDAR 鐳射雷達感測器、IMU 慣性測量單元，還有來自多個攝像頭的資料。綜合這些龐雜的資料，得以計算感測器及感測器周圍的環境。
設計目標：低計算資源消耗，實時優化，不追求高精度。這個演算法的目標應用場景：室內用服務機器人（如掃地機器人）、無人機等等計算資源有限、對精度要求不高、且需要實時避障的和尋路的應用。
特點：

1. LIDAR SLAM，並且用了IMU輔助；
2. 有submap，有scan matching，解決了一些基本的問題；
3. 有Loop closure optimization，這個很好，非常好，這是在indoor環境裡必須要有的，
   在visual slam中大幅降低accumulated error的方法，作者給加進來了；
4. 給了一套node selection, branching, and computation of upper bounds的方法.