前言

???????? 機器人的“綁架”問題是指在缺少它之前的位置信息情況下，去確定機器人的當前位姿，例如當機器人被安置在一個已經構建好地圖的環境中，但是并不知道它在地圖中的相對位置，或者在移動過程中，由于傳感器的暫時性功能故障或相機的快速移動，都導致機器人先前的位置信息的丟失，就像人質的眼睛被蒙上黑布條，拉上集裝箱被運送到了未知的地方，此時，人質就無法給自己定位了。

??????? "綁架問題"可以分為初始化綁架和追蹤丟失狀態綁架。初始化綁架可以闡述為一種通常狀況初始化問題，可使用蒙特卡洛估計器，即粒子濾波方法，重新分散粒子到三維位形空間里面，被里程信息和隨機擾動不斷更新，初始化粒子聚集到/收斂到可解釋觀察結果的區域。追蹤丟失狀態綁架，即在綁架發生之前，系統已經保存當前狀態，則可以使用除視覺傳感器之外的其他的傳感器作為候補測量設備。

?

IMU-慣性傳感器

???? ?? 慣性傳感器是檢測和測量加速度、傾斜、沖擊、振動、旋轉和多自由度( DoF )運動的傳感器。慣性傳感器是解決導航、定向和運動載體控制的重要部件。????

???? ?? 慣性傳感器包括加速度計（或加速度傳感計）和角速度傳感器（陀螺儀）以及它們的單、雙、三軸組合IMU（慣性測量單元），AHRS（包括磁傳感器的姿態參考系統）。MEMS加速度計是利用傳感質量的慣性力測量的傳感器，通常由標準質量塊（傳感元件）和檢測電路組成。IMU主要由三個MEMS加速度傳感器及三個陀螺和解算電路組成，即九軸傳感器。

????? ? 慣性傳感器能夠測量本體的角速度和線性加速度，可以通過標定+積分的方法輔助其他傳感器得到進一步的測量信息。

??????

IMU與視覺的對比

??????? 使用視覺的方式，不管是稠密的、半稠密的、還是稀疏的，在相機位姿變化過快的時候都必然存在無法匹配的情景，導致先前的位置丟失，簡稱追蹤失敗。

??????? IMU可以測得角速度和加速度，由于物理理論基礎和物理結構本身的原因，測得量都存在一定的誤差。在短時間內能得到較為準確的位姿積分，但長時間積誤差會逐漸累積，造成明顯的漂移。

??????? 相機的位姿轉換是基于世界坐標系的，觀察到的路標也會在世界坐標系中不斷收斂，若不是計算錯誤，基本上不會產生漂移。而IMU是自身坐標系標定的，在轉換到世界坐標系時候一定會產生誤差，時序累積必定產生漂移。兩種方式的優缺點互補，這就引發視覺SLAM的融合思想，既是SensorFusion。

??????

SensorFusion松綁定和緊綁定

??????? 對IMU在視覺融合中所占有的位置和狀態變量的耦合程度，可以在算法中設定不同的觀測和更新方式。

??????? 松綁定：或許可以使用額外的同步時鐘，對IMU進行特定短的時間進行積分，獲取位姿轉換。作為相機轉換的初始位姿或者作為"綁架時間"內的視覺補充。在AR眼睛的SLAM算法中，我們使用了松綁定的方法。主要在策略層面進行改進，IMU的位姿積分作為相機的初始優化和轉換位姿，并在“綁架時間”內進行單獨位姿計算。

??????? 緊綁定：或許可以使用另外的同步時鐘，把IMU的狀態和視覺狀態參數進行融合，同步觀測和更新。基于濾波的方式典型的緊綁定算法有著名的MSCKF算法。

?

MSCKF算法

???????? 如有不適，請移步原文，或者聯系刪除

???????? 參考文章：MSCKF算法流程框架

1. 初始化

???????? 1.1 攝像機參數、噪聲方差（圖像噪聲、IMU噪聲、IMU的bias）、初始的IMU協方差、IMU和攝像機的外參數*、IMU和攝像機的時間偏移量*

???????? 1.2 MSCKF參數：狀態向量里滑動窗口大小的范圍、空間點三角化誤差閾值、是否做零空間矩陣構造和QR分解

???????? 1.3 構造MSCKF狀態向量

2.讀取IMU數據，估計新的MSCKF狀態變量和對應的協方差矩陣

3.圖像數據處理

????????? 3.1 MSCKF狀態向量中增加當前幀的攝像機位姿；若位姿數大于滑動窗口大小的范圍，去除狀態變量中最早的視圖對應的攝像機位姿

????????? 3.2提取圖像特征并匹配，去除外點。

????????? 3.3 處理所有提取的特征。判斷當前特征是否是之前視圖中已經觀察到的特征

????????? 是：

?????????? ? ? ?? 3.3.1 如果當前幀還可以觀測到該特征，則加入該特征的track列表

????????????????? 3.3.2 如果當前幀觀測不到該特征(Out_of_View)，將該特征的track加入到featureTracksToResidualize，用于更新MSCKF的狀態變量

???????? 否：

?????????? ? ? ?? 3.3.4 給該特征分配新的featureID，并加入到當前視圖可觀測特征的集合

??????????? 3.4 循環遍歷featureTracksToResidualize中的track，用于更新MSCKF的狀態變量

?????????????????? 3.4.1 計算每個track對應的三維空間點坐標(利用第一幅視圖和最后一幅視圖計算兩視圖三角化，使用逆深度參數化和高斯牛頓優化求解)，若三角化誤差小于設置的閾值，則加入map集合

?????????????????? 3.4.2 計算視覺觀測(即圖像特征)的估計殘差，并計算圖像特征的雅克比矩陣

?????????????????? 3.4.3 計算圖像特征雅克比矩陣的左零空間矩陣和QR分解，構造新的雅克比矩陣

??????????? 3.5 計算新的MSCKF狀態向量的協方差矩陣

????????????????? 3.5.1 計算Kalman增益

????????????????? 3.5.2 狀態矯正

????????????????? 3.5.3 計算新的協方差矩陣

??????????? 3.6 狀態變量管理

???????? ? ??????? 3.6.1 查找所有無feature track可見的視圖集合deleteIdx

?????????????????? 3.6.2 將deleteIdx中的視圖對應的MSCKF中的狀態去除掉

?????????????????? 3.6.3 繪制運動軌跡

簡單分析補充：

1. IMU讀取數據時盡量接近當前攝像機圖像獲取的時間（額外的同步時鐘），最好先離線標定IMU的bias（一般是這樣的，工程化時應該使用相同批次標定的IMU）

2. 離線標定好攝像機內參數，必要的話把畸變因子也標定出來（內參矯正是必須的）。算法運行時，利用攝像機內參數將圖像特征歸一化到攝像機坐標系。（PNP）

3. 圖像特征選擇ORB之類的(提取和匹配速度快，且具有旋轉尺度不變性)，特征匹配時使用快速魯棒的方法去除外點（RANSAC）

4. 若當前視角下場景紋理不豐富，提取的特征較少，則調用另一個函數，只利用IMU的讀數做EKF更新（綁架問題備選方案-只使用IMU作為追蹤方案）

5. 根據攝像機能觀察到的track個數，攝像機之間的base長度、角度，確定關鍵幀，并構造攝像機視圖的graph，根據攝像機之間的關系，利用g2o等工具優化攝像機軌跡。（平差方法）。

6. 可以利用并行線程，實時構建地圖，并構造DBOW，用于回環檢測和重定位（場景識別—模式識別）。

?

后記：

?????? 在整個場景重建中，不同尺度的SLAM有不同的意義。SLAM在局部的改進可以使用多傳感器融合的思路，在更為寬廣的范圍內，語義SLAM變得重要，SLAM本身又成為一個小比例的問題。

?

1. VIO算法總結：視覺慣性里程綜述。有代碼、有圖表、邏輯清晰。MSCKF、VINS、

2. VIO開源框架：VIO目前實現比較好的有vinsmono,okvis,MSCKF。前兩個是基于非線性優化的方案而且框架比較相似，后者是基于濾波優化的方案，也是Google Tango上使用的方法，MSCKF目前并沒有開源，不過賓夕法尼亞的Kumar實驗室18年有一個相似的工作，目前已經開源。此外還有ROVIO。VINS-Mono分析。

?

總結

以上是生活随笔為你收集整理的三维重建6：绑架问题/SensorFusion/IMU+CV-小尺度SLAM的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。