摘要

精確狀態估計是自主機器人的基礎問題。為了實現局部精確且全局無漂移的狀態估計，通常將具有互補性質的多傳感器融合到一起。局部傳感器（相機、IMU、激光雷達等）在小區域內提供了精確的位姿，而全局傳感器（GPS、磁力計、氣壓計等）在大尺度環境中提供了有噪聲但是全局無漂移的定位。在本文中，我們提出一種傳感器融合框架將局部狀態與全局傳感器融合，這實現了局部精確且全局無漂移的位姿估計。由現有的VO/VIO方法產生的局部估計與全局傳感器在位姿圖優化中融合。在圖優化中，局部估計與全局坐標對齊。同時，消除了累積漂移。我們在公開數據集和真實世界實驗中評估本文系統的性能。實驗結果再次與其它最新的算法進行比較。我們強調，本文系統是一種通用的框架，它能夠在統一的位姿圖優化中輕易地融合各種全局傳感器。我們的實現已經開源。

介紹

在過去幾十年中，自主機器人已經成為主流的研究課題。我們已經看到，在自動駕駛、檢查、搜索和救援等各種應用中，對機器人的需求越來越迫切。自主任務的基礎技術之一為定位。機器人需要精確的6自由度（DoF）位姿進行導航和控制。很多傳感器已經被用于局部位姿估計。雷達和激光雷達廣泛應用于受限的室內環境，而相機和IMU在室內和室外環境中都適用。

有很多令人印象深刻的算法用于局部位姿估計，例如基于視覺的方法和基于視覺-慣性的方法。這些算法在局部區域內實現了增量式且精確的狀態估計。然而，存在一些缺陷限制了這些算法在實際中的使用。 局部位姿估計算法的第一個缺陷為，它們在局部坐標系內產生位姿估計（相對于起始點），而不是全局坐標。當我們即使從同一環境中的不同位置開始，我們也可能獲得不同的估計結果。因此，如果沒有固定的全局坐標，它們對復用是不友好的。第二個缺陷為，由于缺少全局測量數據，在長期運行過程中，局部估計容易累積漂移。

盡管已經提出了一些基于視覺的回環方法來消除漂移，但是它們仍然無法處理帶有大量數據的大尺度環境。 與局部傳感器相比，GPS、氣壓計、磁力計等全局傳感器在大尺度環境中的全局定位方面具有優勢。它們提供了相對于固定地球坐標系的全局測量，這是沒有漂移的。然而，這些測量信息通常是非平滑且帶有噪聲的，因此無法直接用于精確控制和導航。以GPS為例，它能夠測量以米為單位的近似位置，但是該測量在低頻率下是不連續的。此外，它只能測量三維位置，而無法測量三維旋轉。

因此，僅有全局傳感器是不足以實時地進行6自由度狀態估計的。 由于局部傳感器（相機、IMU和激光雷達）在局部精確性方面實現了令人印象深刻的性能且全局傳感器（GPS、磁力計和氣壓計）是無漂移的，因此聰明的方式是將它們融合到一起以實現局部精確且全局無漂移的6自由度位姿估計。

為了提高魯棒性，我們想要融合盡可能多的傳感器。因此，需要一種支持多傳感器的通用框架。盡管傳統的基于EKF方法能夠逐漸地將局部估計融合到全局坐標系中，但是需要有關不同坐標系之間變換的精確初始值，以保證結果收斂。此外，EKF方法還對時間同步敏感。任何延遲傳入的測量數據將導致麻煩，因為無法在濾波過程中反向傳遞狀態。為此，我們使用一種基于優化的方法來解決該問題，它本質上適用于多傳感器融合。

在本文中，我們提出一種優化框架來融合局部估計與全局傳感器測量。局部估計來自現有的最新VO/VIO工作。全局傳感器被作為位姿圖中的通用因子。局部傳感器和全局傳感器因子被累加到一起以構建優化問題。本文系統實現了局部精確且全局無漂移的狀態估計。我們強調本文貢獻如下： 1）一種通用的框架，將各種全局傳感器與局部估計融合，從而實現局部精確且全局無漂移的定位； 2）所提出系統在公開數據集和真實實驗中評估； 3）代碼對社區開源。

相關工作

最近，狀態估計的多傳感器融合方法已經成為一種主流趨勢，以便提高精度和魯棒性。根據系統中使用的傳感器類型，研究工作能夠被分類為局部定位和全局定位。 對于局部定位，相機、IMU、激光雷達和RGB-D傳感器通常用于小規模環境中進行6自由度狀態估計。

過去幾十年中令人印象深刻的方法包括基于視覺的方法、基于激光雷達的方法、基于RGB-D的方法和基于事件相機的方法。還有一些多傳感器融合方法，例如視覺-慣性融合和視覺-激光雷達融合。在這些工作中，工作A multi-state constraint Kalman filter for vision-aided inertial navigation、High-precision, consistent EKF-based visualinertial odometry和Robust visual inertial odometry using a direct ekf-based approach是基于濾波的方法，而工作Keyframe-based visual-inertial odometry using nonlinear optimization、Visual-inertial monocular slam with map reuse和Vins-mono: A robust and versatilemonocular visual-inertial state estimator是基于優化的方法。在基于優化的框架中，很多視覺測量數據和慣性測量數據被保留在一個捆集中。與觀測到的測量值相關的狀態被一起優化。

基于優化的方法相比于基于EKF的方法的一個優勢在于，狀態可以迭代線性化以提高精度。基于濾波的方法和基于優化的方法都可以實現高度精確的狀態估計。由于缺少全局測量，累積漂移隨著時間是不可避免的。 對于全局定位，系統集成了全局傳感器（GPS、磁力計、氣壓計等）。全局傳感器測量相對于地球坐標系的絕對量，它們與起點無關。全局測量通常具有噪聲且頻率較低，所以它們不能夠單獨使用。所以，全局傳感器通常與局部傳感器融合，以實現精確且全局的定位。工作A robust and modular multi-sensor fusion approach applied to mav navigation提出一種基于EKF的算法，將視覺測量與慣性和GPS測量相融合，以獲得無漂移的估計。工作Multi-sensor fusion for robust autonomous flight in indoor and outdoor environments with a rotorcraft MAV使用無跡卡爾曼濾波（UKF）算法來融合視覺、激光雷達和GPS測量，它是EKF的擴展，其沒有解析的雅可比。基于濾波的方法對時間同步很敏感。由于狀態無法在濾波過程中反向傳遞，所以任何延遲傳入的測量都會引起麻煩。因此，需要特殊的排序機制來確保來自多個傳感器的所有測量數據都是有序的。

與基于濾波的方法相比，基于優化的方法在這方面具有優勢。因為大的捆集作為一個天然的緩沖區，它可以等待并且存儲很長一段時間的測量值。工作Gomsf: Graph-optimization based multi-sensor fusion for robust uav pose estimation使用一種基于優化的框架來融合局部視覺慣性里程計（VIO）與GPS測量，這比工作A robust and modular multi-sensor fusion approach applied to mav navigation中提出的方法產生了更精確的結果。在該方法中，局部坐標和全局坐標之間的變換被經常優化。很少有研究工作融合超過三種類型的傳感器。在本文中，我們提出一種更通用的基于優化的全局定位框架，它可以支持多個全局傳感器。每個傳感器作為一個通用的因子，其可以很容易地加入到優化問題中。

系統概述

根據測量數據所在的參考系，我們將傳感器分類為局部類型和全局類型。

1）局部傳感器：相機、激光雷達、IMU（加速度計和陀螺儀）等。這類傳感器不是全局參考的，因此通常需要一個參考坐標系。通常，機器人的首個位姿被設置為原點，以便啟動傳感器。機器人的位姿估計從起點開始增量式地累積。因此，累積漂移將隨著距離起點距離的增加而增大。

2）全局傳感器：GPS、磁力計、氣壓計等。這類傳感器是全局參考的。它總是在一個固定的全局坐標系（例如地球坐標系）下工作。參考系的原點固定，并且事先是已知的。它們的測量數據是全局但是帶有噪聲的。其誤差與行駛距離無關。對于GPS，它測量相對于地球的絕對經度、維度和高度。精度、維度和高度可以轉換為x、y和z坐標。對于磁力計，它測量磁場方向和強度，這可以確定方向。對于氣壓計，它測量氣壓，這可以轉換成高度。 本文框架如圖2所示。局部傳感器（相機和IMU）被用于局部估計。采用現有的VO/VIO方法來產生局部位姿。局部結果和全局傳感器輸入到全局位姿圖中。它們被轉化為統一的因子來構建優化問題。全局估計器生成局部精確且全局的6自由度位姿結果。

方法

A.局部位姿估計

對于局部位姿估計，我們采用現有的視覺里程計（VO）/視覺-慣性里程計（VIO）算法。有很多令人印象深刻的VO/VIO算法，例如工作Orb-slam: a versatile and accurate monocular slam system、A multi-state constraint Kalman filter for vision-aided inertial navigation和Vins-mono: A robust and versatile monocular visual-inertial state estimator。它們中任何一個可以在本框架中用于局部位姿估計，只要它能產生6自由度位姿。這部分不是本文的主要貢獻。出于完整性考慮，本文簡要介紹我們先前的VIO算法，其在我們的開源實現中使用。

VIO估計滑動窗口內若干IMU幀的位姿和特征的深度。狀態量定義為：

其中，第個IMU狀態由IMU中心相對于局部參考系的位置、速度和姿態組成。我們使用四元數來表示姿態。首個IMU位姿被設置為參考幀。和分別為加速度計偏置和陀螺儀偏置。當特征在相機幀中首次被觀測到時，使用它們的逆深度進行參數化。估計問題被轉換成一個非線性最小二乘問題：

其中，和分別表示慣性和視覺殘差。先驗項包含關于過去邊緣化狀態的信息。表示魯棒huber核。VIO在局部坐標系中實現了精確的實時6自由度位姿估計。

B.全局位姿圖結構

全局位姿圖結構的圖示如圖3所示。



每個位姿包含世界坐標系中位置和姿態，其作為位姿圖中一個節點。節點的密度由最低頻率的傳感器確定。兩個連續節點之間的邊是一個局部約束，它來自于局部估計（VO/VIO）。該邊約束了一個節點到另一個節點的相對位姿。其它邊為全局約束，它來自全局傳感器。 位姿圖優化的本質是一個最大似然估計（MLE）問題。MLE由機器人位姿在一段時間上的聯合概率分布組成。變量為所有節點全局位姿，，其中，。和為全局坐標系下位置和姿態。在所有測量概率是獨立的假設之下，問題通常被推導為： 其中，為測量值集合，其包括局部測量（VO/VIO）和全局測量（GPS、磁力計、氣壓計等）。我們假設測量的不確定性為具有均值和協方差的高斯分布，即。因此，上述方程推導為： Mahalanobis范數為。接著，狀態估計轉化為非線性最小二乘問題，這也被稱為捆集調整（BA）。

C.傳感器因子

1）局部因子：由于局部估計（VO/VIO）在小范圍區域內是精確的，所以我們利用兩幀之間的相對位姿。考慮兩個連續幀和幀，局部因子推導為： 其中，和為來自VO/VIO的局部幀在時刻和的位姿。為四元數誤差狀態上的減法操作。第一行表示兩個位姿間相對位置誤差，第二行表示兩個位姿間相對旋轉誤差。如果VO/VIO算法產生位姿的協方差矩陣，我們使用它作為局部測量的協方差。否則，我們對所有的局部測量使用統一的協方差。

2）GPS因子：GPS的原始測量值為經度、緯度和海拔高度，其不是x、y和z軸坐標。通常，我們可以將經度、緯度和海拔高度轉換為地心地固（ECEF）坐標，局部東北天（ENU）坐標和局部北東地（NED）坐標。這里，我們取ENU坐標為例。通過設置首個GPS測量為原點，我們獲得ENU世界坐標系中的GPS測量數據，。GPS因子推導為： GPS測量直接約束每個節點的位置。當接收到測量數據時，協方差由衛星的數量確定。當接收到越多衛星信號，協方差就越小。

3）磁力計因子：磁力計可以測量磁場強度的矢量。這個矢量的方向可以幫助確定世界坐標系中的姿態。我們假定磁力計是經過離線標定的，沒有偏移或者偏置。首先，我們查表以獲得ENU坐標系中局部區域的磁場強度。我們假設磁場強度在這個區域內是恒定的。我們的測量數據表示為。如果我們將傳感器與ENU坐標對齊，則的姿態應該與匹配。受此啟發，因子推導為： 其中，為機器人中心到磁力計中心的變換，這是已知且經過離線標定的。由于磁場容易受到環境影響，我們僅使用沒有長度的歸一化向量。長度被用于確定協方差。如果測量的長度與相差很大，我們就設置一個大的協方差。否則，我們使用一個小的協方差。

4）氣壓計因子：氣壓計測量一個地區的氣壓。我們假設一段時間內某一海拔高度上的氣壓是恒定的。因此，氣壓可以線性地轉換為高度。與GPS相同，我們設置首個測量為原始高度。接著，我們獲得高度的測量值。直觀而言，該因子為高度估計的殘差，其寫為： 由于該測量具有噪聲，因此我們計算短時間內若干測量的方差，并且在代價函數中使用它。

5）其它全局因子：盡管我們僅詳細說明GPS因子、磁力計因子和氣壓計因子，但是我們的系統不限于這些全局傳感器。本文系統也可以使用其它全局傳感器，甚至一些人工傳感器（例如運動捕捉系統、WiFi和藍牙指紋）。關鍵是將這些測量建模為全局坐標系下的殘差因子。

D.位姿圖優化

一旦構建了位姿圖，優化它等價于尋找盡可能匹配所有邊的節點配置。Ceres求解器用于求解該非線性問題，它以迭代的方式利用Gaussian-Newton和Levenberg -Marquadt方法。 我們以低頻率（1Hz）運行位姿圖優化。在每次優化后，我們獲得局部坐標系到全局坐標系的變換。因此，我們可以通過這一變換來轉換隨后的高頻局部位姿（VO/VIO，200Hz），以獲取實時高頻的全局位姿。由于位姿圖是相當稀疏的，所以計算復雜度隨位姿數量線性增加。我們可以維持一個巨大的窗口進行位姿圖優化，以獲取精確且全局無漂移的位姿估計。當計算復雜度超過實時能力時，我們丟棄舊的位姿和測量數據，并且將窗口規模維持在有限的尺寸。

審核編輯：劉清