2.3.3 SLAM算法

同步定位與地圖構建（Simultaneous Localization and Mapping, SLAM）是指搭載特定傳感器的主體，在沒有環境先驗信息的情況下，運動過程中建立環境的模型，并同時估計自身運動。目前，SLAM主要應用于機器人、無人機、無人駕駛、AR、VR等領域，用途包括傳感器自身定位及路徑規劃、場景理解等功能。

根據傳感器種類和安裝方式的不同，SLAM實現方式主要分為基于激光傳感器的激光SLAM和基于視覺傳感器的視覺SLAM（Visual SLAM,VSLAM）兩大類。其中，激光SLAM比VSLAM起步早，在理論、技術和產品落地上都相對成熟。VSLAM方案目前主要有兩種實現路徑：一種是基于RGB-D（RGB Depth）地圖的深度攝像機，如Kinect；另外一種是基于單目、雙目或魚眼攝像頭。

1. 激光SLAM

激光SLAM技術指搭載激光雷達的主體在運動中估計自身位置與姿態，與此同時建立周邊環境地圖。精確定位依賴精確地建圖，而精確建圖離不開準確定位。激光SLAM所需傳感器一般有激光雷達、慣性測量單元、里程計。更新頻率較高的激光雷達可實現里程計的功能。由于激光雷達具有誤差模型簡單、測距比較準確、點云處理容易、在光照變化明顯的環境中運行穩定等特點，因此激光SLAM理論研究相對成熟，落地產品豐富。由于激光雷達能夠快速地采集到的物體信息（激光點云）的準確距離與角度等豐富的信息，激光SLAM技術成為無人駕駛定位穩定的解決方案。

激光SLAM系統的工作原理是通過對不同時刻兩幀激光點云進行比對與匹配，然后計算激光雷達載體在前后兩個時刻的相對距離和姿態變化，最終完成載體定位。

（1）激光SLAM建模框架

激光SLAM建模框架分為前端掃描匹配、后端優化、閉環檢測、點云地圖構建四個核心模塊，如圖2-23所示。前端掃描匹配，是激光SLAM的關鍵步驟，工作原理是已知前一幀位置與姿態并利用下一幀的位姿變換關系估計當前幀的位姿；激光雷達的掃描頻率決定前端掃描匹配間隔的位姿變換與局部地圖，但是由于不可避免地出現幀間匹配的累計誤差，因此需要后端做長時間增量式掃描匹配后里程計及地圖信息的優化；閉環檢測環節的任務是，通過檢測歷史幀判斷是否曾經到過此地從而減少全局地圖的漂移，進一步減少前端匹配所產生的累計誤差，以便生成全局一致性地圖；激光點云地圖構建的主要任務是，生成和維護全局地圖。

（2）點云匹配模型的建立

搭載激光雷達的無人車在處于運動狀態時會實時構建環境點云數據，地圖點云數據會隨時間與行程的增加而累積，要匹配的激光點云與地圖數據不斷擴大，搜索量隨即增加。在搜索步長不變的情況下，為保證匹配的精度，點云匹配的耗時也必然增加。為了控制匹配時間，激光SLAM的前端擬采用時間前后兩幀點云匹配方法[30]。

該匹配方法的原理：以第一幀載體所在位置為建立全局地圖的坐標系原點，以后所有幀都是基于第一幀與前一幀的點云進行匹配，定義第n次掃描的數據為d_n，第i次匹配過程，首先將d_i作為待匹配數據向d_i-1進行匹配，然后根據d_i-1與d₁之間的變換矩陣，將d_i轉換至d₁所在坐標系下。當前幀點云（x_i,y_i）與上一幀點云（x_i-1，y_i-1）進行配準，獲取旋轉角度為θ_i，平移量設為（Δx_i，Δy_i），旋轉平移后得到的點云為上一幀點云與起始點第一幀點云存在準關系，即角度為，平移量為，匹配后的點云為，變換關系如下：

因此

其中

已經獲得的作為當前幀與原始點集配準的參數。通過此參數將當前幀的點集轉換到全局地圖所處的坐標系下，完成當前時刻的點云匹配。然而，由于時間前后兩幀點云配準的配準會存在一定的誤差，當多幀匹配時，誤差便會累積。假設第i-1次匹配過程中引入誤差，定義第i次匹配時d_i向d_i-1匹配的誤差為，因此通過位姿變換關系轉換到d₁上時所引起的誤差可表示為

因此每一次匹配均有的增量。當激光雷達運動為直線時，由于角度的變化不大，可以認為當前的角度，因此累計誤差為

由于每一次的匹配均有的誤差增量，所以隨著匹配的數量上升，累計誤差隨即增加，綜上為點云匹配的誤差建模的建立過程。

2. VSLAM

VSLAM是將圖像作為主要環境感知信息源的SLAM系統。VSLAM以計算相機位姿為主要目標，通過多視幾何方法構建三維地圖，VSLAM還處于實驗室研究階段，實際應用較少。

VSLAM系統的處理過程一般都是分為2個階段：幀間估計和后端優化。在整個SLAM系統中，幀間估計是根據相鄰兩幀間的傳感器信息獲取該時間間隔內的運動估計，后端優化指對之前幀間估計產生的路徑累積漂移誤差做優化，解決機器檢測到路徑閉環后歷史軌跡的優化問題。與激光SLAM相比，VSLAM對色彩和紋理等信息更敏感，在提高幀間的估計精度和閉環檢測方面有巨大潛力。

一般的VSLAM分為特征點法和直接法。特征點法，首先從每幀圖片中提取分布均勻的角點和周圍的描述子，通過這些角點周圍的描述子的不變性完成幀間匹配，接下來使用對幾何恢復相機姿態并確定地圖坐標，最終根據最小化重投影誤差完成相機位姿和地圖的微調。直接法是根據光度誤差確定相機位姿和地圖的，不用提取角點和描述子。正因為這樣，直接法不能表征一張圖像的全局特征。另外，直接法的閉環檢測面臨的累積漂移的消除問題一直沒有得到很好的解決。

近些年，計算機視覺與深度學習相互結合，促使視覺相關任務的準確率、執行效率及魯棒性等實際表現得到巨大提升，如實例分類[31]、對象檢測[32]、行為識別[33]等領域的表現。VSLAM系統以計算機視覺為基礎，為神經網絡在該領域的應用提供很大的發揮空間。將深度學習與VSLAM結合[34]，有以下優勢：

1）基于深度學習的VSLAM系統有很強的泛化能力，可以在光線復雜的環境中工作。

2）對于動態物體的識別和處理更加有效。

3）采用數據驅動的方式，對模型進行訓練，更符合人類與環境交互的規律。有很大的研究和發展空間。

4）采用神經網絡可以更好地將圖像信息與其他傳感器的數據融合，優化幀間漂移。

5）更高效地提取語義信息，有益于語義SLAM[35]的構建和場景語義的理解。

6）端到端的VSLAM，舍去前端點跟蹤、后端優化求解的一系列過程，直接輸入圖像給出位姿估計。