基于雙目視覺(jué)與IMU的組合導(dǎo)航算法

譚 靜，趙健康，崔 超

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

0 引 言

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

1 相關(guān)研究

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

雖然目前存在很多高精度的VIO算法，但它們都是以提高系統(tǒng)的計(jì)算復(fù)雜度為代價(jià)來(lái)實(shí)現(xiàn)，系統(tǒng)中所采用的慣性器件也大多是價(jià)格昂貴的高精度IMU，不完全適用于搭載低成本、低精度IMU的小型移動(dòng)平臺(tái)，而且高負(fù)荷的運(yùn)算量也使得其無(wú)法在低性能的計(jì)算設(shè)備上使用。此外，現(xiàn)有的基于濾波器的方法，如文獻(xiàn)[9]提出的多速率卡爾曼濾波器保持了IMU的動(dòng)態(tài)特性，但忽略了線性加速度引入的有色噪聲影響，由于采用的是增量式構(gòu)建觀測(cè)量的方法，使得位置估計(jì)含有加速度計(jì)的漂移累計(jì)誤差?；谏鲜龇治?，本文提出一種低計(jì)算成本的雙目視覺(jué)與IMU融合的組合導(dǎo)航技術(shù)。

2 坐標(biāo)系描述

圖1 系統(tǒng)坐標(biāo)系關(guān)系

3 雙目視覺(jué)與IMU組合導(dǎo)航算法

本文所提出的基于誤差狀態(tài)粒子濾波器的雙目視覺(jué)與IMU組合導(dǎo)航算法框架如圖2所示，首先將IMU測(cè)量的加速度與角速度進(jìn)行平滑濾波，過(guò)濾掉測(cè)量數(shù)據(jù)中的野值；在獲得相機(jī)圖像后，構(gòu)建圖像金字塔并完成左右圖像、前后幀圖像特征點(diǎn)的提取與匹配，再根據(jù)剔除誤匹配之后的特征點(diǎn)對(duì)解算相機(jī)的運(yùn)動(dòng)信息，最后采用基于誤差狀態(tài)的粒子濾波器將對(duì)齊到導(dǎo)航坐標(biāo)系的運(yùn)動(dòng)數(shù)據(jù)與IMU測(cè)量數(shù)據(jù)進(jìn)行融合，以實(shí)現(xiàn)載體運(yùn)動(dòng)信息的最優(yōu)估計(jì)。

圖2 雙目視覺(jué)與IMU組合導(dǎo)航算法框架

3.1 IMU測(cè)量模型與預(yù)處理

不考慮地球自轉(zhuǎn)角速度的影響，將重力加速度看作常量，可以得到加速度計(jì)的測(cè)量模型為

(1)

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

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

(2)

(3)

(4)

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

3.2 雙目視覺(jué)運(yùn)動(dòng)信息估計(jì)算法

該雙目視覺(jué)運(yùn)動(dòng)狀態(tài)估計(jì)算法在大多數(shù)情況下已經(jīng)可以實(shí)現(xiàn)較精確的運(yùn)動(dòng)估計(jì)，但此時(shí)估算的位姿信息并未與重力對(duì)齊，無(wú)法直接用于導(dǎo)航，且純雙目的方法仍然對(duì)環(huán)境中的許多因素十分敏感。而IMU能夠幫助載體恢復(fù)運(yùn)動(dòng)信息，有助于提高算法的魯棒性，其敏感于重力加速度的特征，也能夠輔助實(shí)現(xiàn)相機(jī)位姿信息與導(dǎo)航坐標(biāo)系的對(duì)齊，這也是在雙目視覺(jué)運(yùn)動(dòng)估計(jì)算法中引入IMU的原因。

3.3 基于誤差狀態(tài)的雙目視覺(jué)與IMU融合算法

3.3.1 航跡推算

本文所采用融合算法的狀態(tài)量共有16維

(5)

(6)

(7)

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

(8)

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

3.3.2 基于誤差狀態(tài)的粒子濾波融合方法

為了實(shí)現(xiàn)對(duì)載體運(yùn)動(dòng)信息的精確有效估計(jì)，本系統(tǒng)中引入了基于誤差狀態(tài)的粒子濾波融合算法。粒子濾波的核心思想是根據(jù)建議密度函數(shù)選取離散的隨機(jī)采樣點(diǎn)，通過(guò)對(duì)粒子集合求均值獲得濾波器的估計(jì)結(jié)果。但是狀態(tài)量中表示姿態(tài)的四元數(shù)受到模長(zhǎng)為1的額外約束，并且對(duì)加法運(yùn)算不具有封閉性，無(wú)法直接求解隨機(jī)粒子集合。

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

qrandom=q?δq

(9)

(10)

要獲得當(dāng)前時(shí)刻的誤差狀態(tài)，算法中采用了擴(kuò)展卡爾曼濾波實(shí)時(shí)維護(hù)和更新δX，并為求解隨機(jī)誤差四元數(shù)提供合理的建議密度函數(shù)，以減小濾波過(guò)程中退化現(xiàn)象對(duì)估計(jì)結(jié)果的影響。

在估計(jì)誤差四元數(shù)的過(guò)程中，誤差狀態(tài)的微分方程可以定義為

(11)

其中

(12)

當(dāng)相機(jī)的測(cè)量數(shù)據(jù)更新后，通過(guò)融合相機(jī)數(shù)據(jù)可以對(duì)維護(hù)的隨機(jī)粒子的誤差狀態(tài)進(jìn)行校正，由于相機(jī)計(jì)算的位姿信息并未與導(dǎo)航坐標(biāo)系對(duì)齊，因此在算法中利用加速度計(jì)敏感于重力的特性，在系統(tǒng)運(yùn)行開(kāi)始階段，將載體靜置一段時(shí)間，隨后取多個(gè)加速度計(jì)測(cè)量數(shù)據(jù)的平均值，求得相機(jī)位姿數(shù)據(jù)與導(dǎo)航坐標(biāo)系的對(duì)齊矩陣，完成相機(jī)測(cè)量數(shù)據(jù)與IMU數(shù)據(jù)的對(duì)齊。測(cè)量經(jīng)過(guò)上述轉(zhuǎn)化后代表當(dāng)前時(shí)刻載體的位姿信息，則誤差狀態(tài)的測(cè)量轉(zhuǎn)移矩陣H簡(jiǎn)化為單位矩陣。

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

(1)采用歐拉積分推算載體運(yùn)動(dòng)信息；

(5)根據(jù)建立密度分布函數(shù)產(chǎn)生隨機(jī)的誤差狀態(tài)δXk～q(δXk-1,Zk)=N(δXk,Pk)，并根據(jù)式(9)計(jì)算后代隨機(jī)粒子xk，針對(duì)每一個(gè)粒子重復(fù)上述步驟；

4 實(shí)驗(yàn)結(jié)果

為了驗(yàn)證本文中方法的可行性及有效性，本節(jié)會(huì)詳細(xì)闡述將所推薦的方法運(yùn)行于EuRoC數(shù)據(jù)集[11]的實(shí)驗(yàn)結(jié)果，并從精度、運(yùn)行性能和實(shí)時(shí)性等方面進(jìn)行分析。

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

4.1 算法精度

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

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

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

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

圖3 V1-01和V2-02中各算法軌跡估計(jì)結(jié)果

圖4 V2-02中各算法在軌跡不同長(zhǎng)度誤差結(jié)果

4.2 運(yùn)行性能

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

圖5 各算法在EuRoC數(shù)據(jù)集運(yùn)行的CPU占用率

4.3 實(shí)時(shí)性

本節(jié)對(duì)所提出的算法的運(yùn)行時(shí)間進(jìn)行了測(cè)試，以說(shuō)明所推薦的算法能夠在線實(shí)時(shí)估計(jì)載體的運(yùn)動(dòng)信息。在實(shí)驗(yàn)過(guò)程中，將所推薦的算法與對(duì)比算法分別運(yùn)行于EuRoC數(shù)據(jù)集上6次，其中實(shí)驗(yàn)平臺(tái)為基于Ubuntu16.04的機(jī)器人操作系統(tǒng)，CPU型號(hào)為AMD Ryzen1600，記錄程序每獲取到一次數(shù)據(jù)之后程序的開(kāi)始時(shí)間和運(yùn)行時(shí)間，統(tǒng)計(jì)后總結(jié)于表2中。

表2 各算法平均運(yùn)行時(shí)間

從表2中可以看出，本文所推薦的方法與Vins Fusion和OKVIS都能夠?qū)崟r(shí)估計(jì)載體的運(yùn)動(dòng)信息，但是本文所推薦的算法在運(yùn)行時(shí)間上的表現(xiàn)更好，相較于Vins Fusion減少了約42.4%，相比OKVIS減少了約15%，能夠保證穩(wěn)定實(shí)時(shí)輸出對(duì)載體的運(yùn)動(dòng)信息估計(jì)結(jié)果。及時(shí)追蹤和估計(jì)載體的運(yùn)動(dòng)信息，利于平臺(tái)根據(jù)載體數(shù)進(jìn)行有效的控制，也使得該算法在低成本、低性能設(shè)備中的推廣更具有競(jìng)爭(zhēng)力。

5 結(jié)束語(yǔ)

本文提出了基于誤差狀態(tài)粒子濾波器的雙目視覺(jué)與IMU融合算法實(shí)現(xiàn)了載體運(yùn)動(dòng)信息的實(shí)時(shí)估計(jì)。根據(jù)部分?jǐn)?shù)據(jù)集失敗的原因分析，未來(lái)考慮在相機(jī)位姿估算時(shí)引入IMU信息以進(jìn)一步提高算法在高速運(yùn)動(dòng)場(chǎng)景下的魯棒性。實(shí)驗(yàn)結(jié)果表明，與現(xiàn)有的方法相比，本文的算法在保證必要的導(dǎo)航精度和實(shí)時(shí)性的前提下，具有更高的計(jì)算效率和更快的運(yùn)行速度，使其在無(wú)法攜帶高性能計(jì)算設(shè)備的小型無(wú)人平臺(tái)部署中更具競(jìng)爭(zhēng)力。