基于雙目視覺與IMU的組合導航算法

譚 靜，趙健康，崔 超

(上海交通大學 電子信息與電氣工程學院，上海 200240)

0 引 言

精確的導航數據是保證機器人穩定運行的核心。現有慣性導航技術的累積誤差限制了該方法的精度和應用。INS/GPS組合導航技術[1,2]在室內等GPS失效場景中又無法提供可靠的導航信息。區域無線導航技術[3,4]又受限于繁瑣的基站布置工作無法用于大型場景。視覺慣性里程計通過融合視覺信息與IMU數據，可以在GPS失效時提供精確的導航數據，主動的信息攝入方式又避免了繁瑣的基站布置。但現有方法計算復雜，對主控設備性能要求高。基于此，本文提出一種基于誤差狀態粒子濾波器(ESPF)的雙目相機與IMU融合的組合導航方法。將雙目視覺和IMU解耦為兩個獨立的模塊，針對傳感器觀測周期不一致問題，采用歐拉積分對無觀測時的系統狀態進行航跡推算。通過把狀態量映射到李代數空間，將無法直接獲得四元數隨機粒子分布的過程轉化為向量空間的隨機粒子求解問題，以融合相機解算的位姿信息與IMU測量數據實現小型無人平臺運動信息的實時估計。為了在保證更好的跟蹤載體運動狀態的同時，減小航跡推算階段的累積誤差，提高狀態信息的輸出頻率，在濾波算法中實時估計了IMU的漂移。方法中將相機與IMU解耦為獨立模塊的算法結構降低了計算的復雜程度，同時更方便進行傳感器的故障檢測與隔離，有利于提高算法的魯棒性。實驗結果表明，本文提出的算法具有良好的實時性與魯棒性，能夠滿足低成本小型無人平臺導航的需求。

1 相關研究

以濾波方法為主的MSCKF[5]和基于非線性優化理論的VINS[6]被認為是目前主流的兩種融合方法的代表。Mur-Artal R等提出的ORB_SLAM2算法[7,8]將相機與IMU聯合初始化，結合g2o實現局部的BA優化，利用ORB提供的描述子在大范圍運動時實現回環檢測和重定位，但三線程的系統結構加重了CPU的負擔。MSCKF中提出一種多狀態約束的雙目視覺慣性里程計，以類似滑動窗口的形式，在觀察到同一特征點的多個位姿之間建立約束，再進行濾波更新，但以高維度的特征點為狀態量增加了計算的復雜度。文獻[9]根據IMU與相機的更新頻率，設計多速率卡爾曼濾波器實現狀態估計，充分利用了IMU數據，但其沒有考慮線性加速度引入的有色噪聲的影響，使用相機計算的位姿增量與濾波器結果構建觀測量的方法，也使得加速度計的漂移在位置估計中不斷累積。沈紹劼等提出的VINS[6]，使用IMU預積分構建緊耦合框架，將邊緣化的先驗信息、IMU測量殘差以及視覺重投影誤差進行聯合優化，實現了載體的位姿估計。文獻[10]提出的基于非線性優化的OKVIS框架，通過最小化地標重投影誤差和IMU誤差項尋求最優的相機位姿和地標位姿。

雖然目前存在很多高精度的VIO算法，但它們都是以提高系統的計算復雜度為代價來實現，系統中所采用的慣性器件也大多是價格昂貴的高精度IMU，不完全適用于搭載低成本、低精度IMU的小型移動平臺，而且高負荷的運算量也使得其無法在低性能的計算設備上使用。此外，現有的基于濾波器的方法，如文獻[9]提出的多速率卡爾曼濾波器保持了IMU的動態特性，但忽略了線性加速度引入的有色噪聲影響，由于采用的是增量式構建觀測量的方法，使得位置估計含有加速度計的漂移累計誤差。基于上述分析，本文提出一種低計算成本的雙目視覺與IMU融合的組合導航技術。

2 坐標系描述

圖1 系統坐標系關系

3 雙目視覺與IMU組合導航算法

本文所提出的基于誤差狀態粒子濾波器的雙目視覺與IMU組合導航算法框架如圖2所示，首先將IMU測量的加速度與角速度進行平滑濾波，過濾掉測量數據中的野值；在獲得相機圖像后，構建圖像金字塔并完成左右圖像、前后幀圖像特征點的提取與匹配，再根據剔除誤匹配之后的特征點對解算相機的運動信息，最后采用基于誤差狀態的粒子濾波器將對齊到導航坐標系的運動數據與IMU測量數據進行融合，以實現載體運動信息的最優估計。

圖2 雙目視覺與IMU組合導航算法框架

3.1 IMU測量模型與預處理

不考慮地球自轉角速度的影響，將重力加速度看作常量，可以得到加速度計的測量模型為

(1)

其中，α、β、γ代表載體繞x、y、z三軸旋轉的角度，Ra、Rβ、Rγ代表旋轉矩陣，旋轉順序定義為z-y-x，g為重力加速度，Vx、Vy、Vz為載體的線性速度。

通常在小型移動平臺上多使用基于MEMS技術的傳感器，這種傳感器具有低成本、低功率的特點，但也伴隨著相對高水平的測量噪聲和偏差，忽略非線性誤差源(比如比例因子、正交誤差)，陀螺儀與加速度計的輸出結果可以表示為

(2)

(3)

(4)

其中，D為正整數，為濾波器滑動窗口大小，y(k)為k時刻的濾波輸出。經過上述濾波器處理后，可以濾除傳感器測量數據中的高頻噪聲。

3.2 雙目視覺運動信息估計算法

該雙目視覺運動狀態估計算法在大多數情況下已經可以實現較精確的運動估計，但此時估算的位姿信息并未與重力對齊，無法直接用于導航，且純雙目的方法仍然對環境中的許多因素十分敏感。而IMU能夠幫助載體恢復運動信息，有助于提高算法的魯棒性，其敏感于重力加速度的特征，也能夠輔助實現相機位姿信息與導航坐標系的對齊，這也是在雙目視覺運動估計算法中引入IMU的原因。

3.3 基于誤差狀態的雙目視覺與IMU融合算法

3.3.1 航跡推算

本文所采用融合算法的狀態量共有16維

(5)

(6)

(7)

本系統所采用的算法將陀螺儀和加速度計的測量偏差視為常值漂移，則bω、bf的微分為0。最終基于慣性傳感器的航跡推算方程為

(8)

其中，dt為IMU的更新間隔，Xk|k-1代表由k-1時刻獲得的航跡推算結果，Xk-1為上一時刻的載體狀態信息。

3.3.2 基于誤差狀態的粒子濾波融合方法

為了實現對載體運動信息的精確有效估計，本系統中引入了基于誤差狀態的粒子濾波融合算法。粒子濾波的核心思想是根據建議密度函數選取離散的隨機采樣點，通過對粒子集合求均值獲得濾波器的估計結果。但是狀態量中表示姿態的四元數受到模長為1的額外約束，并且對加法運算不具有封閉性，無法直接求解隨機粒子集合。

通常，由航跡推算得到的四元數q與采樣四元數qrandom之間的誤差可以視為小量，則可以表示為

qrandom=q?δq

(9)

(10)

要獲得當前時刻的誤差狀態，算法中采用了擴展卡爾曼濾波實時維護和更新δX，并為求解隨機誤差四元數提供合理的建議密度函數，以減小濾波過程中退化現象對估計結果的影響。

在估計誤差四元數的過程中，誤差狀態的微分方程可以定義為

(11)

其中

(12)

當相機的測量數據更新后，通過融合相機數據可以對維護的隨機粒子的誤差狀態進行校正，由于相機計算的位姿信息并未與導航坐標系對齊，因此在算法中利用加速度計敏感于重力的特性，在系統運行開始階段，將載體靜置一段時間，隨后取多個加速度計測量數據的平均值，求得相機位姿數據與導航坐標系的對齊矩陣，完成相機測量數據與IMU數據的對齊。測量經過上述轉化后代表當前時刻載體的位姿信息，則誤差狀態的測量轉移矩陣H簡化為單位矩陣。

經過更新獲得隨機粒子的誤差狀態和建議密度分布函數后，根據一般向量空間的隨機變量計算方法，求解得到隨機誤差狀態帶入式(9)即可獲得狀態量的隨機后代粒子。最后整個IMU與雙目視覺融合算法流程如下：

(1)采用歐拉積分推算載體運動信息；

(5)根據建立密度分布函數產生隨機的誤差狀態δXk～q(δXk-1,Zk)=N(δXk,Pk)，并根據式(9)計算后代隨機粒子xk，針對每一個粒子重復上述步驟；

4 實驗結果

為了驗證本文中方法的可行性及有效性，本節會詳細闡述將所推薦的方法運行于EuRoC數據集[11]的實驗結果，并從精度、運行性能和實時性等方面進行分析。

EuRoC是基于小型無人機平臺所采集的視覺慣性里程計數據集，其中包含以20 Hz頻率更新的752×480分辨率的雙目圖像(MT9V034)以及200 Hz更新率的陀螺儀和角速度計測量數據(ADIS16448)；位置和姿態的真值分別由Leica MS50和Viconc采集得到，精度約1 mm。數據集中提供的11個測試情景，包含從良好視覺條件下的慢速飛行到運動模糊或者光線條件較差的快速飛行等多個場景，能夠更全面的對算法進行評估。

4.1 算法精度

此外在本次實驗中，將所推薦的算法運行于EuRoC數據集，并與Vins Fusion[12]和OKVIS進行了比較。其中Vins Fusion是在2019年提出的一種通用的多傳感器里程計估計框架，OKVIS是基于滑動窗口的視覺里程計算法。

本實驗采用文獻[13]所推薦的工具對各算法的性能進行評估，該工具是基于Python語言開發，能夠全面地分析各算法精度，最終提供所測試算法估計精度的定量分析結果與常用的誤差評價結果。表1是各算法在EuRoC數據集上運行結果的均方根誤差(RMSE)。本文的算法在Vicon Room場景中具有較好的定位精度，其中在V1-01和V2-02中的精度最高。經分析，主要是Machine Hall數據集中的環境條件、飛行路徑都更加復雜；過快的飛行速度和較長的飛行距離也是MH-03和MH-04估計精度下降的原因之一；此外，本文的算法無法在V1-03、V2-03以及MH-05上實現載體的信息估計，除了飛行速度過快之外，還有光照條件太差或者存在運動模糊的情況導致，從而導致兩幀圖像之間的共識區域太小，無法追蹤特征點。

表1 各算法估計結果均方根誤差(RMSE)

圖3表明了在V1-01和V2-02數據集上各算法估計的軌跡信息的效果圖，其中點線為軌跡真值，實線是本文算法的實驗軌跡估計結果，點劃線和虛線分別為OKVIS和Vins Fusion的估計結果。從圖3可以看出V2-02相比于V1-01飛行軌跡更復雜，飛行路徑也更長。而從圖4(a)中可以明顯看出隨著飛行軌跡增加，估計軌跡和真值之間的誤差也不斷增加，這主要是由于本文中的算法沒有加入閉環檢測和后端優化，在長時間運行情況下，累計誤差不斷增加，無法得到校正的緣故。但是從圖4(b)可以看出，盡管估計軌跡的絕對誤差在不斷增大，但是其相對誤差始終保持在4%以內。

圖3 V1-01和V2-02中各算法軌跡估計結果

圖4 V2-02中各算法在軌跡不同長度誤差結果

4.2 運行性能

本文所推薦的算法主要是應用于無法攜帶高性能計算設備的小型無人平臺上，因此除了對估計精度進行分析，算法的運行性能也是需要考量的重要元素之一。在實驗中分別將各算法在數據上運行了6次，并統計了相關算法的CPU占用率。從圖5中可以看出，OKVIS有很高的CPU占用率，經分析，認為主要是由于在兩幀圖像匹配的過程中計算了BRISK描述子導致的。而本文所采用的相機與IMU解耦的融合算法相比于另外兩個算法具有更低的CPU占用率，相比于Vins Fusion降低了約29.7%，相比OKVIS降低了約57.2%，表明本文所用算法在運行性能上具有一定的優勢。

圖5 各算法在EuRoC數據集運行的CPU占用率

4.3 實時性

本節對所提出的算法的運行時間進行了測試，以說明所推薦的算法能夠在線實時估計載體的運動信息。在實驗過程中，將所推薦的算法與對比算法分別運行于EuRoC數據集上6次，其中實驗平臺為基于Ubuntu16.04的機器人操作系統，CPU型號為AMD Ryzen1600，記錄程序每獲取到一次數據之后程序的開始時間和運行時間，統計后總結于表2中。

表2 各算法平均運行時間

從表2中可以看出，本文所推薦的方法與Vins Fusion和OKVIS都能夠實時估計載體的運動信息，但是本文所推薦的算法在運行時間上的表現更好，相較于Vins Fusion減少了約42.4%，相比OKVIS減少了約15%，能夠保證穩定實時輸出對載體的運動信息估計結果。及時追蹤和估計載體的運動信息，利于平臺根據載體數進行有效的控制，也使得該算法在低成本、低性能設備中的推廣更具有競爭力。

5 結束語

本文提出了基于誤差狀態粒子濾波器的雙目視覺與IMU融合算法實現了載體運動信息的實時估計。根據部分數據集失敗的原因分析，未來考慮在相機位姿估算時引入IMU信息以進一步提高算法在高速運動場景下的魯棒性。實驗結果表明，與現有的方法相比，本文的算法在保證必要的導航精度和實時性的前提下，具有更高的計算效率和更快的運行速度，使其在無法攜帶高性能計算設備的小型無人平臺部署中更具競爭力。