參考博文：
https://blog.csdn.net/qq_41839222/article/details/87860819
https://blog.csdn.net/weixin_41843971/article/details/86537228
摘要
精確的狀態估計是自主機器人的一個基本問題。爲了實現局部精確和全局無漂移狀態估計，通常將具有互補特性的多個傳感器融合在一起。**局部傳感器（攝像機、imu、lidar等）在小範圍內提供精確的姿態，而全局傳感器（gps、磁強計、氣壓計等）**在大範圍環境中提供噪聲大但無漂移的定位。本文提出了一種融合局部狀態和全局傳感器的傳感器融合框架，實現了局部精確、全局無漂移的姿態估計。由現有的VO/VIO方法產生的局部估計在姿態圖優化中與全局傳感器融合。在圖優化中，局部估計被對齊成全局座標。同時，消除了累積漂移。我們在公共數據集和實際實驗中評估了系統的性能。將結果與其他最新算法進行了比較。我們強調，我們的系統是一個通用的框架，可以很容易地融合各種全局傳感器在一個統一的圖優化中。我們的實現是開源的。
I 背景
自主移動機器人(自動駕駛、檢查、搜索和救援) 基本技術：定位，精確的6自由度姿態來導航和控制
可用於局部姿態估計的sensor：
- 雷達和激光雷達廣泛應用於室內受限環境
- 攝像機和慣性測量單元則適用於室內和室外環境。
局部位姿估計算法缺點：
- 沒有全局座標的局部幀（相對於起始點）中產生位姿估計。即使在同一個環境中，我們從不同的角度出發，也可能得到不同的估計。因此，如果沒有固定的全局座標，它們不利於重用
- 由於缺乏全局測量，從長遠來看，局部估計容易累積漂移。雖然有人提出了一些基於視覺的環路閉合方法來消除漂移，但這些方法無法處理大量數據的大規模環境。
全局傳感器優點：
- 提供關於固定地座標系的全局測量，固定地座標系是無漂移的。
全局傳感器缺點： - 量通常是不平滑和噪聲的，不能直接用於精確控制和導航。以GPS爲例，它可以測量米的大致位置，但測量是不連續的低速率。而且，它只測量三維位置而不測量三維方向。因此，只有全局傳感器不足以進行實時的六自由度狀態估計。
由於局部傳感器（相機、imu和lidar）在局部精度方面取得了令人印象深刻的性能，而全局傳感器（GPS、磁強計和氣壓計）是無漂移的，因此將它們融合在一起實現局部精確和全局無漂移的6-dof位姿估計是一種聰明的方法。
爲了提高魯棒性，我們希望儘可能多地融合傳感器。因此，需要一個支持多個傳感器的通用框架。
傳統的基於EKF的方法：
- 雖然可以將局部估計逐步融合到全局幀中，但爲了保證收斂性，需要對不同幀之間的變換進行準確的初始猜測。
- EKF方法對時間同步也很敏感。由於狀態不能在過濾過程中傳播回來，任何遲來的測量都會引起麻煩。
本文提出了一個局部估計與全局傳感器測量融合的優化框架。局部估計採用的是最先進的VIO方法，然後將全局傳感器作爲姿態圖中的一般因子，將局部因子和全局因子綜合起來構建優化問題。實現局部精確並且全局無漂的狀態估計。
本文的貢獻如下：
- 在公共數據集和實際實驗上對擬議系統進行評估。
- 一個通用框架，用於將各種全局傳感器與局部估計融合，從而實現局部精確和全局無漂移定位。
- 社區開源代碼
II 之前的相關工作
A 局部定位（攝像機、imu、lidar和RGB-D傳感器）
基於視覺的方法： PTAM，SVO，LSD-SLAM，ORB-SLAM，DSO
基於雷達的方法：LOAM
視覺慣性融合：MSCKF，VINS-mono，Ice-ba
視覺激光雷達融合：V-Loam
B 全局感知定位（gps、磁強計、氣壓計等）
全局測量通常是帶有噪聲且低頻，因此不能單獨使用。
通常將全局傳感器與局部傳感器融合，實現精確的全局感知定位。例如：
- 基於濾波的方法
基於EKF方法的GPS+視覺+慣性測量
基於UKF方法的視覺+激光+GPS測量
基於濾波的方法對時間同步非常敏感，需要特殊的排序機制確保來自多個傳感器的所有測量都是有序的。 - 基於優化的方法
基於優化的框架的VIO+GPS
很少有研究工作融合傳感器超過三種類型。本文提出的全局定位框架可以支持多個全局傳感器。每個傳感器都是一個通用的因子，可以很容易地加入到優化問題中。
III 系統概述
A 局部傳感器
B 全局傳感器
GPS、磁強計、氣壓計等，這種傳感器具有全局參考值。它總是在固定的全局框架下工作，例如地球座標系。參考座標系的原點是預先確定和已知的。它們的測量是全局參考的，但有噪聲。誤差與行駛距離無關。對於GPS，它測量相對於地球的絕對經度、緯度和高度。經度、緯度和高度可以轉換爲X、Y和Z座標。對於磁強計，它測量磁場的方向和強度，從而確定磁場的方向。對於氣壓計，它測量氣壓，氣壓可以轉換成高度。
我們的框架結構如圖2所示。局部傳感器（攝像機和imu）用於局部估計。採用現有的VO/VIO方法產生局部姿態。局部結果和全局傳感器被輸入到全局姿態圖中。將它們轉化爲統一的因子來構造優化問題。全局估計器產生局部精確和全局感知的 6-DoF pose結果。
IV 理論
D位姿圖優化
一旦構建了圖，優化它就等於找到儘可能匹配所有邊的節點配置。ceres解算器[22]用於求解這一非線性問題，它以迭代的方式利用高斯牛頓和levenberg-marquadt方法。我們在低頻（1hz）下運行位姿圖優化。在每次優化後，我們得到局部幀到全局幀的轉換。因此，我們可以通過這種變換來變換後續的高速率局部姿態（vo/vio，200hz），從而實現實時的高速率全局姿態。
由於姿態圖是非常稀疏的，計算複雜度隨姿態的數目線性增加。爲了得到精確的全局無漂移位姿估計，我們可以爲位姿圖優化保留一個巨大的窗口。當計算複雜度超過實時能力時，我們拋出舊的姿態和度量，並將窗口保持在有限的大小。
V 實驗
上圖是實驗系統
和MSF(EKF方法融合了視覺，慣性，GPS)相比
從上圖可以看出改進的方法幾乎沒有漂移。
VI 總結
本文提出了一種基於優化的局部估計與全局傳感器融合的框架。本地估計來自以前的vo/vio工作。在位姿圖優化中，全局傳感器被視爲通用因子。該系統實現了局部精確的和全局無漂移姿態估計。我們在公共數據集和實際實驗中展示了我們的系統令人印象深刻的性能。