基于優(yōu)化的多傳感器全局位姿估計(jì)框架

摘要

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

介紹

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

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

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

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

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

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

相關(guān)工作

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

過去幾十年中令人印象深刻的方法包括基于視覺的方法、基于激光雷達(dá)的方法、基于RGB-D的方法和基于事件相機(jī)的方法。還有一些多傳感器融合方法，例如視覺-慣性融合和視覺-激光雷達(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)化的框架中，很多視覺測(cè)量數(shù)據(jù)和慣性測(cè)量數(shù)據(jù)被保留在一個(gè)捆集中。與觀測(cè)到的測(cè)量值相關(guān)的狀態(tài)被一起優(yōu)化。

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

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

系統(tǒng)概述

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

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

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

方法

A.局部位姿估計(jì)

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

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

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

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

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

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



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

C.傳感器因子

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

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

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

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

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

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

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

審核編輯：劉清