聶雷航，聶 蕓，王國(guó)偉

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

0 引 言

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

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

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

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

1 系統(tǒng)概述

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

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

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

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

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

2 算法流程

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

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

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

圖2 通信流程圖

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

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

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

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

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

最后，得到2組匹配好的3D點(diǎn)：

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

(1)

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

2.2 計(jì)算尺度(縮放比s)

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

(2)

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

(3)

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

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

pi=sRp′i+t+ei

(4)

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

ei=pi-(sRp′i+t)

(5)

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

(6)

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

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

(7)

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

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

(8)

計(jì)算：

(9)

得：

(10)

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

(11)

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

令：

(12)

其中，H為一個(gè)3×3的矩陣。對(duì)H進(jìn)行SVD分解：

H=UΛVT

(13)

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

R=VTU

(14)

求得R后，則：

t=p-sRp′

(15)

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

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

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

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

設(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)，分別對(duì)a和b求偏導(dǎo)，然后使偏導(dǎo)數(shù)等于0，得：

(18)

(19)

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

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

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

(20)

sin (lat1)sin (lat2))

(21)

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

3 實(shí)驗(yàn)與分析

3.1 仿真可視化實(shí)驗(yàn)

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

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

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

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

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

3.2 誤差分析

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

表1 尺度比

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

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

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

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

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

表3 歐拉角平均誤差

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

4 結(jié)束語

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