SLAM/VIO學習總結

一、前言

這篇文章作為我的碩士期間學習總結，將從導航定位層面介紹SLAM技術，並給初學者一些學習建議。不會涉及非常深的理論方面的東西，初衷是重點說清楚SLAM方法的套用價值和學習方向，筆者認為這個對初學者更加重要。這篇文章假設讀者已經了解了SLAM的基本概念。

文章中提到的部份參考資料，我整理了一份傳到了網盤上：連結：https:// pan.baidu.com/s/1gWqXp cdvyeID3y7ktXOZQ 密碼：1m34

二、為什麽研究SLAM

最近幾年我們見證了機器人領域的快速發展，大疆的無人機已經很好地實作智能化飛行，谷歌的自動駕駛出行服務已經在局部範圍進行試營運，波士頓動力的Atlas機器人一個漂亮的後空翻甚至讓多數業內人士為止驚嘆，AR/VR技術也在定義新的人機互動方式。這些都離不開穩定可靠的定位導航技術。

2.1從機器人定位導航/狀態估計說起

SLAM的輸出是運動體狀態+環境地圖，所以研究SLAM會有兩個不同的側重點，側重前者可以用於機器人定位導航，側重後者則用於三維重建，這篇文章假設讀者關註的是前者，將SLAM作為機器人定位導航（狀態估計）的一種方法展開下面的內容。

運動體可以抽象為空間中的剛體，它的狀態可以使用六自由度（三軸位置、三軸姿態）進行描述。以無人機為例，對無人機的運動控制是透過內環姿態控制和外環位置控制實作的，內環控制實作了機體在空間中的基本運動，外環位置控制則是實作路徑規劃、航點飛行的基礎。為了對無人機姿態進行控制，需要有姿態量的反饋，這個是透過IMU/AHRS傳感器進行姿態解算實作的；同樣，位置控制需要位置量的反饋，可以實作位置測量和估計的傳感器有GPS、UWB、LiDAR、Camera。GPS和UWB是利用外部訊號實作的定位，LIDAR和Camera則是利用SLAM演算法實作位置估計。同時，控制理論中，速度量可以加快機體響應速度，也是一個非常重要的量，復雜的姿態和位置控制環路裏一般會加入速度（角速度、線速度）控制。所以通常我們也會考慮速度的測量估計。

好吧，扯這麽多其實就是想說，我們做狀態估計的目的就是得出機器人三組狀態量（位置、速度、姿態），為機器人運動控制做狀態反饋。盡管不同的傳感器會有不同的特征，進行數據處理、運動估計的方法也不盡相同，但是目的是一致的，都是為了得到機器人的三組狀態。

所以，如果事有興趣將SLAM套用在機器人領域上，應該從機器人定位導航的層面上認識SLAM的作用，SLAM只是機器人定位導航的一種方法，不同傳感器的不同特性使其適用於不同的場合，為了提高定位導航系統的可靠性穩定性，在實際中往往涉及多傳感器融合。而SLAM方法進行定位導航的優勢在於不需要外部先驗資訊，使用範圍更廣泛。

而且透過SLAM的學習，我們將學到定位導航需要的所有知識技能。下圖是國內一家自動駕駛公司的招聘資訊，我圈出了定位導航的職位要求。數學知識、濾波/最佳化方法、傳感器融合，軟件編程這些做機器人導航定位需要的知識技能，在SLAM的學習中都會涉及到。

2.2 SLAM在機器人領域的套用

前面已經說到，SLAM是機器人定位導航的一種方式，而且如果後端建圖實作的是稠密地圖，即獲取到周圍環境詳細的深度資訊，也是實作避障、路徑規劃的任務的基礎。下圖是港科沈紹劼老師團隊利用VIO實作的無人機導航和單目稠密重建的工作。

在自動駕駛裏，一方面則是使用多傳感器融合的方式提高定位系統的可靠性穩定性，這裏面激光/視覺SLAM是其中一個重要的定位來源；另一方面在構建高精地圖時，也會用到SLAM技術，高精地圖被認為是實作自動駕駛的重要技術方案，使用地圖采集車先構建出包含非常詳細的環境細節的地圖，在正常執行的自動駕駛車輛上則利用SLAM重定位技術從高精地圖裏獲取定位資訊。

2.3 SLAM在AR/VR領域的套用

從更廣泛的意義上將， SLAM 技術實作了物體在空間中的定位，因此不僅僅在機器人領域， SLAM 可以套用在任何需要空間定位的場合。

AR(Augmented Reality) 增強現實技術是透過帶有視覺傳感器的器材實作在物理真實場景中添加虛擬資訊，實作現實和虛擬場景的互動。如圖，是Apple在 2017 年 推出的 ARKit在流動通訊器材上實作的AR 效果範例，圖中 ipad 顯示透過後置網絡攝影機拍攝的真實場景和虛擬場景疊加的效果，其中相機是真實的物體，咖啡杯和花瓶則是添加的虛擬物體，透過光線渲染等實作了很好的效果（使用者甚至無法區分出真實物體和虛擬物體）。舉例說明 SLAM 技術在其中發揮的作用，當使用者透過 ipad 向 ipad 拍攝到的場景中添加虛擬資訊，為了實作很好的逼真效果，第一步就是需要固定該虛擬物體相對真實環境中的位置，即 ipad 移動而視角發生變化時，虛擬物體和真實環境的相對位置不應該發生變化，而虛擬物體實際是存在於流動通訊器材上的，換言之，我們需要精確估計出流動通訊器材相對空間的位置變化。 ARKit 是透過VIO實作流動通訊器材在空間中的精確定位。

VR(Virtual Reality) 虛擬現實技術則是透過虛擬現實頭盔投射虛擬資訊，給人身臨其境的感覺，將使用者在實際空間中的移動反映到虛擬空間上可以很大程度地提高互動體驗，透過追蹤使用者佩戴頭盔的位置可以實作這一效果，實作頭盔空間定位的方式可以分為 Outside-in 方案和 Inside-out 方案，前者使用過外部輔助器材實作定位，後者則使用頭盔自身傳感器實作定位，顯然後者的使用場景不受限制是一種更好的定位方式，而 Inside-out 定位目前被廣泛采用的方案就是視覺慣性傳感器融合實作 SLAM的 VINS 演算法。

2.4 VIO的優勢

VIO（visual-inertial odometry）即視覺慣性裏程計，有時也叫視覺慣性系統（VINS，visual-inertial system），是融合相機和IMU數據實作SLAM的演算法，根據融合框架的區別又分為緊耦合和松耦合，松耦合中視覺運動估計和慣導運動估計系統是兩個獨立的模組，將每個模組的輸出結果進行融合，而緊耦合則是使用兩個傳感器的原始數據共同估計一組變量，傳感器雜訊也是相互影響的，緊耦合演算法上比較復雜，但充分利用了傳感器數據，可以實作更好的效果，是目前研究的重點。

單目視覺 SLAM 演算法存在一些本身框架無法克服的缺陷，首先是尺度的問題 ，單目 SLAM 處理的影像幀遺失了環境的深度資訊，即使透過對極約束和三角化恢復了空間路標點的三維資訊，但是這個過程的深度恢復的刻度是任意的，並不是實際的物理尺度，導致的結果就是單目SLAM 估計出的運動軌跡即使形狀吻合但是尺寸大小卻不是實際軌跡尺寸；由於基於視覺特征點進行三角化的精度和幀間位移是有關系的，當相機進行近似旋轉運動的時候，三角化演算法會退化導致特征點跟蹤遺失，同時視覺 SLAM 一般采取第一幀作為世界座標系，這樣估計出的位姿是相對於第一幀影像的位姿，而不是相對於地球水平面 (世界座標系) 的位姿，後者卻是導航中真正需要的位姿，換言之，視覺方法估計的位姿不能和重力方向對齊。

透過引入 IMU 資訊可以很好地解決上述問題，首先透過將 IMU 估計的位姿序列和相機估計的位姿序列對齊可以估計出相機軌跡的真實尺度，而且 IMU 可以很好地預測出影像幀的位姿以及上一時刻特征點在下幀影像的位置，提高特征跟蹤演算法匹配速度和應對快速旋轉的演算法魯棒性，最後 IMU 中加速度計提供的重力向量可以將估計的位置轉為實際導航需要的世界座標系中。同時，智能電話等流動終端對 MEMS 器件和網絡攝影機的大量需求大大降低了兩種傳感器的價格成本；硬件實作上， MEMS 器件也可以直接嵌入到網絡攝影機電路板上。綜合以上，融合 IMU 和視覺資訊的 VINS 演算法可以很大程度地提高單目 SLAM 演算法效能，是一種低成本高效能的導航方案，在機器人、AR/VR 領域得到了很大的關註。

從另外一個角度理解視覺慣性融合的意義，我們一般采用運動方程式和測量方程式描述機器人的運動過程，具體到視覺慣性緊耦合運動估計問題，這裏的第k 時刻的狀態 x_k=[{R_k,p_k,v_k,b^g_k,b^a_k}], u_k 為IMU的運動測量輸入， f(x_{k-1},u) 實作根據上一時刻估計的狀態和k-1到k 過程中IMU的運動測量預測的當前時刻狀態 \tilde{x_k} ，由於測量雜訊的存在，預測狀態 \tilde{x_k} 和當前真實狀態 x_k 的差異由雜訊項 w_k 表示。測量方程式中， P_k 為k時刻可以觀測到的所有3D路標點集，h在這裏對應相機投影模型， h(P,x_k) 為k 時刻對路標點集 P_k 的觀測，觀測值為 p_{k} ，觀測雜訊為 v_{k} ，即k時刻3D路標點集 P_k 在相機影像上的投影點集為 p_{k} 。

整個機器人的運動過程就是運動方程式根據上一時刻狀態和當前時刻運動輸入 u_k 預測當前狀態，利用傳感器的測量方程式作為矯正的過程，在純視覺SLAM中，因為缺少運動輸入，預測方程式提供資訊不足，我們是直接采用測量方程式構建重投影誤差進行運動最佳化估計，而引入IMU數據之後，用IMU作為運動輸入 u_k 可以對狀態進行很好的預測，VIO就是利用了運動預測和測量方程式構建聯合估計。具體來說，在假設雜訊服從高斯分布，對非線性方程式進行一階近似的條件下，從最大後驗估計的角度就可以推匯出我們在VIO論文中看到的聯合最佳化目標函數：

三、學習資源

3.1 書籍

入門書籍推薦高博的【SLAM十四講】，這本書不僅涵蓋了SLAM和狀態估計相關的基本理論知識，而且高博給出了每個理論模組的編程實作，第一遍建議可以先不細推公式，把書瀏覽一遍了解主要內容，第二遍就可以動手跟著書一點點擼程式碼了。之後就可以學習目前主流的開源框架，在對SLAM有較深的了解和實踐之後，建議讀【state estimation for robotics】和【Multi View Geometry】,前者是介紹狀態估計理論非常完備的一本書，舉例來說，在非線性性非高斯章節中，作者從機器人的運動和測量方程式出發，根據馬爾科夫假設和貝葉斯公式，將目前主流的濾波（EKF，UKF，PF）方法統一在貝葉斯估計的框架下。後者涵蓋了SLAM視覺幾何的所有理論知識。

對三維旋轉的描述計算需要李群李代數的知識，高博的書和【state estimation for robotics】都有詳細講解，簡單來說，使用旋轉矩陣表示的旋轉方便向量的座標計算，但直接對其最佳化是有約束最佳化問題（9個參數3個自由度），解決思路就是在三維旋轉群的正切空間上對用李代數表示的旋轉誤差量進行無約束最佳化，對正切空間的理解可以參考【Lie groups, Lie algebras, projective geometry and optimization

for 3D Geometry, Engineering and Computer Vision】。

推公式的時候發現矩陣白學了？寶寶不哭，這有一本【the Matrix CookBook】送給你。

3.2 VIO 開源框架

VIO目前實作比較好的有vinsmono,okvis,MSCKF。前兩個是基於非線性最佳化的方案而且框架比較相似，後者是基於濾波最佳化的方案，也是Google Tango上使用的方法，MSCKF目前並沒有開源，不過賓夕法尼亞的Kumar實驗室18年有一個相似的工作，目前已經開源。此外還有ROVIO。值得註意的是，雖然在純視覺SLAM中，學界已經公認基於非線性最佳化方法的SLAM方法效果要好於濾波的方法，但在VIO中，非線性最佳化和濾波方法目前還沒有很明顯的優劣之分。我的理解是結合相機和IMU兩種傳感器的資訊，提供了對當前狀態更多的觀測，使得演算法對歷史觀測之間約束資訊的依賴降低，這也是為什麽okvis、vinsmono采用滑動視窗法（global bundle adjustment和filter 的折中）也能取得很好效果的原因。

vinsmono的程式碼和論文比較一致，是目前大家學習參考比較多的開源框架。

VIO中，目前多采用流形空間上預積分的方法對IMU數據進行預處理，核心思路是在兩幀之間計算IMU的幀間運動增量，在叠代最佳化時直接使用運動增量，提高計算效率。這部份參考論文【On-Manifold Preintegration for Real-Time
Visual-Inertial Odometry】，整篇論文推導的思路是先推匯出流形空間上的運動狀態的表達公式，然後為了能夠整合到最大後驗估計的最佳化框架裏，分離出運動運算式中的雜訊項，使其近似滿足高斯分布，之後推導了在bias變化時的運動估計值的更新方法。

前面有提到VIO用到了以IMU作為控制輸入的運動方程式，Joan Sola 寫的【Quaternion kinematics for the error-state Kalman filter】對以IMU測量值為運動輸入的運動方程式的誤差遞進形式進行了詳細推導，如果在vinsmono和okvis的運動遞進方程式的雅各比矩陣推導遇到困難，或者對四元數方法進行運動描述不了解，可以仔細閱讀下這篇文章。文章對座標系的locally和globally的解釋也讓人印象深刻。

VIO的初始化是系統工作非常關鍵的部份，這部份可以參考vinsmono以及ORB作者寫的的VIO文章【Visual-Inertial Monocular SLAM with Map Reuse】。兩篇文章思路比較相似，先是透過單目運動估計的方法獲取多幀影像的位姿，然後以此為運動參考估計其他參數，整個過程和相機IMU標定比較相似。初始化主要完成三部份工作：1. 為非線性估計系統提供一個運動初值 2. 估計尺度、IMUbias 3. 估計重力向量在視覺座標系下的投影向量，以此將視覺座標系對齊到世界座標系下。

vinsmono的程式碼中共變異數矩陣遞進公式采用的是中值積分的方法，推導思路和Joan Sola文章中的一致，具體推導可以參考https://www. zhihu.com/question/6438 1223/answer/255818747 ，此外有篇網誌也對vinsmono的整體框架做了整理總結https:// blog.csdn.net/u01287187 2/article/details/78128087 。

vonsmono程式碼中的融合部份是非常值得學習參考的，但是程式碼中的視覺處理部份多是直接使用OPENCV的函數，而且程式碼風格是C++/C 混合的。所以推薦看下ORBSLAM的程式碼，首先編程實作非常規範，對編程學習有很大的參考價值，而且整個程式碼對opencv的依賴較低，視覺部份的特征提取，視覺運動估計，都是作者自己編程實作的，對理解視覺幾何的實作有很好的幫助，泡泡有篇公開課對ORBSLAM的程式碼進行了梳理(https:// pan.baidu.com/s/1c1QOoH M 密碼: xfjd)。網上相關的網誌也很多。

3.3 相機-IMU標定

相機IMU標定的目的是獲取兩個傳感器座標系之間的空間關系和數據延遲，是VIO系統工作的前提工作。相機-IMU標定可以看成狀態估計的逆過程，標定是透過標定板獲取每個時刻的精確運動狀態，計算出模型參數（座標系間旋轉位移、時間延遲、IMUbias），而運動估計則是在已知兩個傳感器座標系間的模型參數，估計每個時刻的運動狀態。

目前有現成的標定庫可以使用(/kalibr/wiki/Camera-IMU-calibration )，我寫了一個使用方法總結，在資源連結裏。

此外，相機-IMU標定需要事先知道相機和IMU的內參；做SLAM中我們也經常需要對新的相機進行內參標定。推薦ROS內建相機標定工具，可以實作線上標定。使用方法我寫了一個文件，在資源連結裏。IMU內參即陀螺儀加速度計的雜訊參數，Kalibr 也給了一些說明。

3.3 數據集

電腦視覺發展如此迅速，我覺得有兩點是有促進作用的。其一是整個領域開源的思想，使得其他人不需要重復造輪子或者對一方面的工作可以很快的上手；另一點就是網上公開的數據集，這些采直實際物理環境的數據集一方面給了大家進行演算法對比的標準，另一方面可以在缺少實驗條件的情況下快速驗證演算法。

飛行數據集，目前使用比較多的是EUROC數據集，給出了幾種場景下飛行平台采集到的雙目影像，IMU數據以及VICON提供的運動基準值。

無人車的數據集，最流行的當數KITTI

定位誤差有ATE和RPE兩種評測指標，TUM給了評測估計值和基準值誤差Python指令碼，而且對兩種誤差的解釋，TUM的評測指令碼是假設估計軌跡和真實軌跡都是實際物理尺度的。ORB作者覆寫了一個自動縮放尺度的指令碼，可以用來評測單目SLAM運動估計效果。

3.4 工具軟件

C++是做SLAM的主流程式語言，需要重點掌握，也是很多公司非常重視的技能，C++學習可以參考北大的C++公開課，配合著網站給出的作業一起搞。Pyhton的基本語法比較簡單，網上介紹的資料比較多，Python中的Matplotlib可以很方便的替代Matlab 進行數據分析的事情，這裏有一個簡單的Matplotlib學習課程。Linux 下的C++、Python開發環境推薦Clion和PyCahrm，兩款軟件是同一個公司出的，學生可以利用教育郵箱在官網上申請免費使用。

Cmake是目前主流的C++工程構建方法，可以參考【Cmake Practice】。

在進行軟件版本更新之間，是不是經常需要進行備份？有時候新寫的程式碼崩潰又需要返回到之前的版本。Git 可以高效的完成這件事情（而且能做的不止這些），這種高效的開發工具你值得擁有。

同時，需要對Linux系統有一些宏觀了解，並掌握基本的命令，學習資源推薦一門公開課，講解的非常清晰。

ROS的資料推薦官網的wiki，網上也有很多介紹的網誌。

時常進行知識整理對我們學習會有很好的促進作用，常用的雲筆記本有為知筆記，有道筆記等，使用markdown語法可以很方便的寫出一篇排版比較清晰的筆記。筆者用的是為知筆記，支持Markdown和LaTeX語法，更重要的是可以在Linux上使用（不過最近版本更新，Linux版本一直登陸出錯，囧）。

求職

泡泡上有一篇大神寫的求職指南，參考閱讀SLAM求職經驗貼上， SLAM求職經驗貼下。

關於職業規劃，推薦兩本書，瑞-達利歐 的【原則】和布賴恩的【遠見】，筆者讀過之後感覺獲益良多。

最後，感謝泡泡機器人（微信公共號ID:paopaorobot_slam）、高博、還有很多請教過問題的前輩。