增強現(xiàn)實與視覺慣導(dǎo)模塊的位姿融合方法

聶雷航，聶 蕓，王國偉

(華北計算技術(shù)研究所，北京 100083)

0 引 言

基于視覺標(biāo)志物的增強現(xiàn)實(AR)系統(tǒng)通過對視覺標(biāo)志物(Marker)的識別能夠?qū)崿F(xiàn)精確的相機定位與跟蹤[1-3]，并將計算機生成的虛擬信息疊加顯示到真實場景中，使得用戶能夠體驗到增強現(xiàn)實的效果[4-5]。其優(yōu)點在于定位精度高，現(xiàn)實中的Marker擁有真實尺度與地理位置信息；缺點在于離開Marker后AR系統(tǒng)就無法工作。

引入視覺慣性里程計(VIO)技術(shù)可以較好地解決這一問題。VIO技術(shù)是同時定位與地圖構(gòu)建(SLAM)技術(shù)[6]的一種視覺里程計(VO)的實現(xiàn)方式，可以使得相機在沒有環(huán)境先驗信息的情況下實時估計自身的運動軌跡[7-9]，其采用的相機+慣性傳感器(IMU)的緊耦合融合定位技術(shù)提高了整個定位系統(tǒng)的魯棒性[10-12]。當(dāng)前VIO技術(shù)在移動增強現(xiàn)實領(lǐng)域已經(jīng)有較為廣泛的應(yīng)用[13-16]，谷歌公司的ARCore、Apple公司的ARKit以及國內(nèi)的easyAR均用到了這一技術(shù)。

當(dāng)前環(huán)境缺乏基于視覺標(biāo)志物的增強現(xiàn)實系統(tǒng)和VIO技術(shù)融合的系統(tǒng)性解決方案。單純基于視覺SLAM或者VIO技術(shù)的AR方案無法確立自身的地理坐標(biāo)，且VIO本身存在著無法避免的累計誤差[17]，需要通過回環(huán)等形式消除，應(yīng)用場景受限。而基于Marker的增強現(xiàn)實系統(tǒng)定位完全依賴Marker，一旦Marker不在相機視野范圍內(nèi)定位就會丟失。

本文由此出發(fā)，研究基于視覺標(biāo)志物的增強現(xiàn)實定位模塊和視覺慣導(dǎo)模塊的位姿融合問題，提出一種可用于多位姿系統(tǒng)的位姿融合方法。位姿融合后，通過VIO技術(shù)，用戶可以在攝像頭離開Marker后依然能夠繼續(xù)體驗增強現(xiàn)實效果，而通過視覺標(biāo)志物的AR跟蹤定位，可以實時校準(zhǔn)VIO的累計誤差，并且利用視覺標(biāo)志物本身可以帶有地理信息的特點，為整個系統(tǒng)提供地理信息坐標(biāo)，與現(xiàn)有業(yè)務(wù)系統(tǒng)進一步結(jié)合。

1 系統(tǒng)概述

基于視覺標(biāo)志物的定位技術(shù)與VIO技術(shù)具有良好的互補性。人們發(fā)現(xiàn)，在2個系統(tǒng)都完成初始化后并對Marker進行跟蹤時，可以額外地加入一個初始化階段。截取2個系統(tǒng)同時輸出的不同坐標(biāo)系下的相同軌跡位姿，通過這2段軌跡計算VIO坐標(biāo)系到AR坐標(biāo)系的最優(yōu)相似變換矩陣，求解ICP問題，將VIO坐標(biāo)系中的點變換到AR系統(tǒng)坐標(biāo)系中，實現(xiàn)位姿的融合。

本文中的VIO采用香港科技大學(xué)的Vins-mono框架[17-18]，通過ROS (Robot Operating System, 機器人操作系統(tǒng))構(gòu)建通信模塊實時采集華為P10發(fā)布的圖像與慣導(dǎo)數(shù)據(jù)信息。在傳統(tǒng)的基于視覺標(biāo)志物的增強現(xiàn)實系統(tǒng)和基于視覺慣性里程計的定位系統(tǒng)的基礎(chǔ)上，通過模塊獨立的方式提出一種松耦合模型：僅將兩者輸出的位姿作為優(yōu)化變量，計算VIO坐標(biāo)系到Marker坐標(biāo)系的最優(yōu)相似變換矩陣，將VIO坐標(biāo)系中的點變換到Marker的定位坐標(biāo)系中。然后通過Marker自身帶有的地理信息將輸出轉(zhuǎn)換為真實的地理信息坐標(biāo)，以VIO的定位跟蹤替代原先的基于Marker的定位跟蹤，在VIO坐標(biāo)系中繪制虛擬對象，使得用戶能夠在整個現(xiàn)實空間中都能體驗到AR效果。整個系統(tǒng)流程如圖1所示。

作為單獨的定位方案，VIO系統(tǒng)中的IMU能夠測量自身角速度與加速度信息，與視覺輸入融合后能大幅改善傳統(tǒng)視覺SLAM快速移動容易丟失的問題[19]，相較于傳統(tǒng)的視覺SLAM方案能夠大幅度提升定位系統(tǒng)的魯棒性[20-23]。其后，Marker定位模塊與VIO定位模塊同時輸出位姿估計信息，經(jīng)過求解最優(yōu)化后輸出VIO坐標(biāo)系到Marker坐標(biāo)系的相似變換矩陣。

圖1 系統(tǒng)流程圖

雖然理論上融合了IMU的視覺SLAM系統(tǒng)能夠恢復(fù)真實世界的尺度，但實際應(yīng)用與測試時，由于手機端IMU性能較差、標(biāo)定誤差、Marker的邊長與存儲邊長長度不等等問題，估計出來的尺度往往有一定的偏差，導(dǎo)致結(jié)果有較大偏差。所以仍然需要通過基于Marker的定位跟蹤輸出的位姿校正VIO系統(tǒng)的尺度。

2 算法流程

2.1 數(shù)據(jù)采集

本文采用華為P10安卓手機作為數(shù)據(jù)輸入設(shè)備，利用ROS作為中間件，將手機和PC接于同一個局域網(wǎng)內(nèi)，然后手機實時把相機和IMU的數(shù)據(jù)打包成ROS規(guī)定的格式并發(fā)布，PC訂閱這些消息進行進一步處理，通信架構(gòu)采用ROS的topic方式。

首先安卓手機和PC都要作為節(jié)點到ROS的Master(節(jié)點管理器)進行注冊，然后安卓手機作為Publisher會發(fā)布topic，而PC作為Subscriber則訂閱該topic，從而建立起Publisher-Subscriber之間的異步通信。整個通信流程如圖2所示。

圖2 通信流程圖

數(shù)據(jù)采集端發(fā)布2個topic：/camera_image和/imu，數(shù)據(jù)接收端訂閱這2個topic并對采集的數(shù)據(jù)進行進一步處理，這之間的通信是異步的。即無論消息是否被接收和處理，發(fā)布端在發(fā)布一次消息之后就會繼續(xù)執(zhí)行下一次操作；無論消息是誰發(fā)送的，接收端都會不斷地從對應(yīng)的topic里接收消息；兩者獨立工作。經(jīng)過測試，采用這套通信架構(gòu)后本文實驗的圖像采集頻率可以達(dá)到20 fps以上。

采集到的數(shù)據(jù)不能夠直接使用，必須要進行數(shù)據(jù)預(yù)處理。數(shù)據(jù)預(yù)處理有2個步驟，一是對輸入的圖像與IMU數(shù)據(jù)進行處理，二是對AR系統(tǒng)與VIO模塊所輸出的位姿點進行處理。

相機的采集頻率大約20～30 fps，而IMU的幀率大多在100 fps以上，兩者并不同步[2]，因此必須進行預(yù)先的處理以保證輸入同步。本文采用的是輸入數(shù)據(jù)時將數(shù)據(jù)打上時間戳，然后以時間戳最靠近的圖像數(shù)據(jù)和IMU數(shù)據(jù)認(rèn)為是同時輸入的數(shù)據(jù)作為VIO模塊的輸入。

之后，圖像數(shù)據(jù)和圖像以及IMU數(shù)據(jù)會分別輸入AR系統(tǒng)和VIO模塊，然后2個系統(tǒng)將輸出一個個的位姿節(jié)點，許多個位姿節(jié)點將構(gòu)成一段軌跡。本文的目的是截取一段相同時間輸出的AR軌跡和VIO軌跡，然后通過優(yōu)化算法估計VIO模塊輸出的軌跡到AR系統(tǒng)輸出的軌跡的最優(yōu)的相似變換矩陣，從而實現(xiàn)VIO模塊定位替代基于標(biāo)志物的視覺定位與跟蹤的目的。

由于2個定位模塊是以松耦合異步的方式結(jié)合的，所以AR系統(tǒng)計算與輸出位姿的速度要比SLAM快，存在類似于相機和IMU的數(shù)據(jù)輸出不同步問題。因此需要將輸出數(shù)據(jù)也都打上時間戳，然后將時間戳最靠近的一組位姿點對認(rèn)為是同時輸入的一對點對。

最后，得到2組匹配好的3D點：

P={p1,…,pn},P′={p′1,…,p′n}

(1)

其中，P為AR坐標(biāo)系中的點，P′為VIO坐標(biāo)系中的點。

2.2 計算尺度(縮放比s)

計算尺度即計算VIO輸出軌跡與真實軌跡的縮放比，較為簡單。本文采取的方法是：截取同一時間的2段系統(tǒng)輸出的軌跡，排除明顯的壞點后，計算2段軌跡的質(zhì)心p與p′：

(2)

然后計算每個點到質(zhì)心的平均距離，其比值就是縮放比s，即：

(3)

2.3 最優(yōu)化求解相似變換

設(shè)有2組配對好的3D點，且同時知道這2組點之間的縮放比s。要找到一組變換R、t，使得pi=sRp′+t。這個問題可以構(gòu)建成一個3D-3D的位姿估計問題，即使用迭代最近點(ICP)求解，2個點集中的點滿足關(guān)系：

pi=sRp′i+t+ei

(4)

其中，R是一個3×3的旋轉(zhuǎn)矩陣，t是一個3×1的平移向量，而ei則是一個誤差向量，故而第i對點的誤差為：

ei=pi-(sRp′i+t)

(5)

本文的目的是要求得對于2組點集之間最優(yōu)的R和t，因此需要構(gòu)建最小二乘問題，求使誤差的平方和最小的R和t:

(6)

直接對式(6)求導(dǎo)過于復(fù)雜，故而先進行化簡，已知質(zhì)心p與p′，則式(6)可變?yōu)椋?/p>

2(pi-p-sR(p′i-p′))T(p-sRp′-t)

(7)

可以看出，式(7)的最終求和項的第3項中的(pi-p-sR(p′i-p′))求和后等于0，而第2項只和質(zhì)心相關(guān)，因此只要獲得了R，令第2項為0就可以得到t。因此定義每個點的去質(zhì)心坐標(biāo)：

qi=pi-p,q′i=p′i-p′

(8)

計算：

(9)

得：

(10)

由于s是常數(shù)，RTR=I，所以可以看出式(10)第1項與第2項都與R無關(guān)，實質(zhì)決定誤差的只有第3項，所以優(yōu)化的目標(biāo)函數(shù)變成了：

(11)

然后，通過SVD分解求解上述目標(biāo)函數(shù)中的最優(yōu)的R。

令：

(12)

其中，H為一個3×3的矩陣。對H進行SVD分解：

H=UΛVT

(13)

其中，Λ為由奇異值組成的對角矩陣，U和V為正交矩陣。當(dāng)H滿秩時，可得：

R=VTU

(14)

求得R后，則：

t=p-sRp′

(15)

由此，便求出了使得位置坐標(biāo)誤差最小的，SLAM坐標(biāo)系中的點到AR坐標(biāo)系的轉(zhuǎn)換關(guān)系R和t。

2.4 使用最小二乘進一步優(yōu)化旋轉(zhuǎn)角

在2.3節(jié)中，通過求解ICP問題求出了VIO坐標(biāo)系中的點轉(zhuǎn)換到AR坐標(biāo)系中的最優(yōu)的R和t。在某些情況下，有必要進一步單獨對SLAM輸出的歐拉角進行優(yōu)化，從3個角度(x,y,z)分別對3個歐拉角進行優(yōu)化，以保證SLAM輸出的歐拉角最大程度地貼合實際。

顯然，VIO輸出的姿態(tài)角和AR輸出的姿態(tài)角滿足線性關(guān)系，所以這個問題可以簡單構(gòu)建成一個線性回歸問題，使用最小二乘求解。

設(shè)AR的歐拉角Ax,Ay,Az與SLAM的歐拉角Vx,Vy,Vz滿足某種線性關(guān)系，以x軸為例，令：

Axi=aVxi+b+ei

(16)

其中，Axi為AR系統(tǒng)輸出的x軸歐拉角，Vxi為SLAM系統(tǒng)輸出的x軸歐拉角，ei為誤差，則求使得誤差平方和最小的a和b：

(17)

式(17)能夠比較方便地求導(dǎo)，分別對a和b求偏導(dǎo)，然后使偏導(dǎo)數(shù)等于0，得：

(18)

(19)

由此，便得到了使得旋轉(zhuǎn)角誤差最小的系數(shù)。

2.5 將位姿輸出轉(zhuǎn)換為真實的地理坐標(biāo)

為使該融合定位系統(tǒng)能與地理信息系統(tǒng)(GIS)相結(jié)合，需要將輸出位姿轉(zhuǎn)換為經(jīng)緯度坐標(biāo)+海拔+旋轉(zhuǎn)角的形式。在該系統(tǒng)中，已知Marker經(jīng)緯度坐標(biāo)，面向正南方，且位于三維空間定位的原點(0，0，0)，規(guī)定x軸正方向為東，y軸正方向為北。融合定位系統(tǒng)能夠?qū)崟r輸出相機的位置坐標(biāo)(X,Y,Z)，單位為m。則可以實時計算出相機的經(jīng)緯度坐標(biāo)：

(20)

sin (lat1)sin (lat2))

(21)

由此，可以將相機的位姿坐標(biāo)映射到地理坐標(biāo)系中，以經(jīng)緯度+海拔+旋轉(zhuǎn)角的形式替換原先的位姿坐標(biāo)，實現(xiàn)融合定位與地理信息系統(tǒng)(GIS)的有效結(jié)合。

3 實驗與分析

3.1 仿真可視化實驗

原始數(shù)據(jù)由移動端AR雙引擎提供，使用該引擎可以同時獲取基于Marker的AR系統(tǒng)輸出的位姿點和VIO輸出的位姿點。該實驗設(shè)置以經(jīng)過預(yù)處理后的300個點為一組實驗對象，這樣可以保證完成一次初始化的時間在10 s以內(nèi)，以Unity引擎作為可視化工具呈現(xiàn)算法效果。

以一組實驗數(shù)據(jù)為例，圖3高亮部分為未經(jīng)過處理的VIO模塊輸出的位姿軌跡，圖4為Marker定位模塊輸出的位姿軌跡，兩者處于2個完全不同的坐標(biāo)系下，沒有任何的重合部分。

圖3 未處理的VIO模塊輸出的軌跡

經(jīng)過融合算法后，VIO的軌跡成功地轉(zhuǎn)換到了Marker定位模塊的坐標(biāo)系中，如圖5所示，高亮部分為經(jīng)過轉(zhuǎn)換后的VIO軌跡。從圖5可以看出，經(jīng)過轉(zhuǎn)換后，雖然仍然有不重合的點，但Marker坐標(biāo)系下的軌跡(圖4高亮部分)與VIO坐標(biāo)系下的軌跡(圖5高亮部分)形狀已基本相同，點和點之間可以一一對應(yīng)，2段軌跡之間只存在一個最小化的平均誤差。

圖5 經(jīng)過轉(zhuǎn)換后的VIO模塊的軌跡

3.2 誤差分析

采集10組實驗對象，排除掉明顯的壞點，首先計算Marker的軌跡相對于VIO軌跡的縮放比，即真實尺度s。結(jié)果如表1所示。

表1 尺度比

可以看出，Marker的軌跡與VIO的軌跡的比值大約在1.3～1.4之間，即基于Vins-mono框架的VIO定位模塊的尺度與真實尺度的比值大約在0.7～0.8之間。因此采用這種方法恢復(fù)VIO模塊的真實尺度十分重要，不可直接將VIO的尺度作為真實尺度，這會造成后續(xù)的計算出現(xiàn)較大偏差。

比較經(jīng)過變換后的VIO模塊輸出的位姿相對于Marker定位模塊的誤差，位姿點的位置平均誤差結(jié)果如表2所示。

表2 位置坐標(biāo)平均誤差

由于使用ICP方法構(gòu)建最小二乘求得了使得位置坐標(biāo)誤差平方和最小的R和t，故而上述的位置坐標(biāo)的誤差可以看作是最小誤差。誤差產(chǎn)生的主要原因是VIO定位和Marker定位輸出的位姿本身帶有的誤差所致。

位姿點的角度(歐拉角)平均誤差如表3所示。

表3 歐拉角平均誤差

經(jīng)過轉(zhuǎn)換后，VIO定位模塊輸出的位姿與Marker定位系統(tǒng)輸出的位姿相互之間的誤差在5%以內(nèi)。造成這部分誤差的主要原因有：VIO定位算法本身的誤差、手機的計算性能、相機采集過程中的硬件誤差等。

4 結(jié)束語

本文以安卓手機作為數(shù)據(jù)采集端，實現(xiàn)了實時的相機與IMU的數(shù)據(jù)采集；然后以采集數(shù)據(jù)作為實時數(shù)據(jù)源，在保證采樣充分的條件下，能夠在10 s以內(nèi)完成初始化，準(zhǔn)確地將VIO坐標(biāo)系下的位姿轉(zhuǎn)換到Marker坐標(biāo)系下；實現(xiàn)了基于視覺標(biāo)志物的增強現(xiàn)實系統(tǒng)與視覺慣導(dǎo)定位模塊的位姿融合。該融合方法不僅可以適用于增強現(xiàn)實與視覺SLAM領(lǐng)域，也可以進一步推廣適用于任何多位姿系統(tǒng)的位姿融合。