摘要

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

介紹

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

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

盡管已經(jīng)提出了一些基于視覺的回環(huán)方法來消除漂移，但是它們?nèi)匀粺o法處理帶有大量數(shù)據(jù)的大尺度環(huán)境。 與局部傳感器相比，GPS、氣壓計、磁力計等全局傳感器在大尺度環(huán)境中的全局定位方面具有優(yōu)勢。它們提供了相對于固定地球坐標系的全局測量，這是沒有漂移的。然而，這些測量信息通常是非平滑且?guī)в性肼暤模虼藷o法直接用于精確控制和導(dǎo)航。以GPS為例，它能夠測量以米為單位的近似位置，但是該測量在低頻率下是不連續(xù)的。此外，它只能測量三維位置，而無法測量三維旋轉(zhuǎn)。

因此，僅有全局傳感器是不足以實時地進行6自由度狀態(tài)估計的。 由于局部傳感器（相機、IMU和激光雷達）在局部精確性方面實現(xiàn)了令人印象深刻的性能且全局傳感器（GPS、磁力計和氣壓計）是無漂移的，因此聰明的方式是將它們?nèi)诤系揭黄鹨詫崿F(xiàn)局部精確且全局無漂移的6自由度位姿估計。

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

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

相關(guān)工作

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

過去幾十年中令人印象深刻的方法包括基于視覺的方法、基于激光雷達的方法、基于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是基于優(yōu)化的方法。在基于優(yōu)化的框架中，很多視覺測量數(shù)據(jù)和慣性測量數(shù)據(jù)被保留在一個捆集中。與觀測到的測量值相關(guān)的狀態(tài)被一起優(yōu)化。

基于優(yōu)化的方法相比于基于EKF的方法的一個優(yōu)勢在于，狀態(tài)可以迭代線性化以提高精度?；跒V波的方法和基于優(yōu)化的方法都可以實現(xiàn)高度精確的狀態(tài)估計。由于缺少全局測量，累積漂移隨著時間是不可避免的。 對于全局定位，系統(tǒng)集成了全局傳感器（GPS、磁力計、氣壓計等）。全局傳感器測量相對于地球坐標系的絕對量，它們與起點無關(guān)。全局測量通常具有噪聲且頻率較低，所以它們不能夠單獨使用。所以，全局傳感器通常與局部傳感器融合，以實現(xiàn)精確且全局的定位。工作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的擴展，其沒有解析的雅可比?；跒V波的方法對時間同步很敏感。由于狀態(tài)無法在濾波過程中反向傳遞，所以任何延遲傳入的測量都會引起麻煩。因此，需要特殊的排序機制來確保來自多個傳感器的所有測量數(shù)據(jù)都是有序的。

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

系統(tǒng)概述

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

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

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

方法

A.局部位姿估計

對于局部位姿估計，我們采用現(xiàn)有的視覺里程計（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。它們中任何一個可以在本框架中用于局部位姿估計，只要它能產(chǎn)生6自由度位姿。這部分不是本文的主要貢獻。出于完整性考慮，本文簡要介紹我們先前的VIO算法，其在我們的開源實現(xiàn)中使用。

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

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

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

B.全局位姿圖結(jié)構(gòu)

全局位姿圖結(jié)構(gòu)的圖示如圖3所示。



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

C.傳感器因子

1）局部因子：由于局部估計（VO/VIO）在小范圍區(qū)域內(nèi)是精確的，所以我們利用兩幀之間的相對位姿?？紤]兩個連續(xù)幀和幀，局部因子推導(dǎo)為： 其中，和為來自VO/VIO的局部幀在時刻和的位姿。為四元數(shù)誤差狀態(tài)上的減法操作。第一行表示兩個位姿間相對位置誤差，第二行表示兩個位姿間相對旋轉(zhuǎn)誤差。如果VO/VIO算法產(chǎn)生位姿的協(xié)方差矩陣，我們使用它作為局部測量的協(xié)方差。否則，我們對所有的局部測量使用統(tǒng)一的協(xié)方差。

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

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

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

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

D.位姿圖優(yōu)化

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

審核編輯：劉清