融合SLAM和Marker的AR單目視覺(jué)定位算法

曹志強(qiáng) 豐艷

關(guān)鍵詞：增強(qiáng)現(xiàn)實(shí)；ArUco Marker；ORB_SLAM2；位姿估計(jì)；單目視覺(jué)

0 引言

增強(qiáng)現(xiàn)實(shí)（Augmented Reality，AR） 是將計(jì)算機(jī)生成的虛擬圖像或信息實(shí)時(shí)、動(dòng)態(tài)地疊加到用戶所看到的真實(shí)環(huán)境中，實(shí)現(xiàn)虛擬世界與現(xiàn)實(shí)世界的無(wú)縫結(jié)合，使人們感知到現(xiàn)實(shí)世界之外的信息。近年來(lái)隨著AR技術(shù)的發(fā)展，其在教育、醫(yī)療、娛樂(lè)等領(lǐng)域有了越來(lái)越多的應(yīng)用[1-2]。

為了保證增強(qiáng)現(xiàn)實(shí)的跟蹤穩(wěn)定性，本文針對(duì)計(jì)算機(jī)視覺(jué)的三維注冊(cè)技術(shù)展開(kāi)了研究。基于計(jì)算機(jī)視覺(jué)的注冊(cè)技術(shù)又分為基于Marker的注冊(cè)方式和基于SLAM（Simultaneous Localization and Mapping） 的注冊(cè)方式，二者在定位上各有優(yōu)劣?；贛arker的注冊(cè)方式可以追溯到2000年的ARToolKit[3]，它是第一個(gè)ARSDK，帶領(lǐng)AR從理論走向了現(xiàn)實(shí)，之后涌現(xiàn)出了ARTag[4]、ArUco Marker[5]等一系列標(biāo)記定位技術(shù)。其中ArUco Marker采用標(biāo)記字典來(lái)生成黑白標(biāo)記，有唯一的二進(jìn)制編碼，其檢測(cè)速度快，魯棒性高，在人工標(biāo)記定位中具有一定優(yōu)勢(shì)。但標(biāo)記定位本身存在明顯的缺陷，容易在標(biāo)記被遮擋和相機(jī)視野不好的時(shí)候跟蹤失敗，李旭杰等人[6]通過(guò)將ArUco Marker與ORB特征點(diǎn)相融合進(jìn)行定位跟蹤，雖然對(duì)標(biāo)記定位進(jìn)行了一定的補(bǔ)充，但跟蹤精度不是很高，面對(duì)自然環(huán)境時(shí)仍容易丟失跟蹤。而基于SLAM的注冊(cè)方式不需要任何標(biāo)記即可以進(jìn)行相機(jī)位姿估計(jì)，其中針對(duì)單目相機(jī)涌現(xiàn)了一大批SLAM 算法，有MonoSLAM[7]、LSD-SLAM[8]、ORB_SLAM2[9]等，其中ORB_SLAM2是目前應(yīng)用最廣、研究最多的SLAM算法，其采用ORB特征點(diǎn)進(jìn)行匹配計(jì)算，可以在大部分環(huán)境下使用，而且支持單目、雙目、RGB-D相機(jī)，有很強(qiáng)的泛用性。盡管SLAM可以在自然環(huán)境中實(shí)現(xiàn)跟蹤，但是SLAM系統(tǒng)初始化緩慢、長(zhǎng)時(shí)間跟蹤會(huì)產(chǎn)生累積誤差且消耗資源過(guò)多，嚴(yán)重影響著AR的廣泛應(yīng)用。

針對(duì)以上問(wèn)題，單純基于SLAM或基于Marker的定位，只能在理想條件下達(dá)到較高的定位精度，且系統(tǒng)穩(wěn)定性差，兩種方法都存在固有的弊端。因此，本文將基于SLAM的定位算法與基于ArUco Marker的定位算法相結(jié)合，進(jìn)行融合算法研究，通過(guò)互補(bǔ)短板，使得最終AR系統(tǒng)的跟蹤魯棒性得到顯著提高。

1 算法設(shè)計(jì)

正文內(nèi)容針對(duì)Marker定位和SLAM定位各自存在的缺陷，同時(shí)考慮兩者在定位上的互補(bǔ)性，本文進(jìn)行兩者的融合算法研究，主要圍繞以下三個(gè)方面展開(kāi)：

1） 利用ArUco Marker輔助完成SLAM系統(tǒng)的初始化，有效解決SLAM初始化速度慢的問(wèn)題。

2） 利用ArUco Marker減小SLAM跟蹤過(guò)程中的累積誤差，從而有效解決AR的漂移問(wèn)題。另外，使用Marker進(jìn)行回環(huán)檢測(cè)比ORB特征點(diǎn)更快、更準(zhǔn)確。

3） 利用SLAM定位解決單純基于Marker定位時(shí)對(duì)于標(biāo)記的過(guò)度依賴問(wèn)題。

融合定位算法框架如圖1所示。

1.1 利用ArUco Marker 進(jìn)行快速初始化

本文在ORB_SLAM2 的初始化環(huán)節(jié)利用ArUcoMarker完成快速初始化。

首先，選擇包含Marker的兩幀畫(huà)面作為初始幀I1和匹配幀I2，兩幀分別以ArUco Marker坐標(biāo)系為世界坐標(biāo)系得到對(duì)應(yīng)的相機(jī)位姿T1、T2，SLAM初始化過(guò)程中需要進(jìn)行地圖點(diǎn)初始化，已知相機(jī)位姿的情況下使用對(duì)極幾何原理進(jìn)行特征點(diǎn)匹配和地圖點(diǎn)創(chuàng)建。

如圖2所示，在空間中尋找一點(diǎn)，記為點(diǎn)P，其在初始幀I1 和匹配幀I2 上得到一對(duì)匹配點(diǎn)P1 和P2，根據(jù)針孔成像原理得到點(diǎn)P 在相機(jī)坐標(biāo)系O1 和相機(jī)坐標(biāo)系O2下的坐標(biāo)信息：

使用上述方法進(jìn)行所有特征點(diǎn)的匹配計(jì)算，在匹配過(guò)程中，會(huì)出現(xiàn)一些誤匹配的點(diǎn)，通過(guò)限制搜索半徑剔除誤匹配的點(diǎn)，保留符合的地圖點(diǎn)，將所有符合的地圖點(diǎn)存入地圖中完成系統(tǒng)初始化。

1.2 利用Marker 減小SLAM 的累積誤差

當(dāng)系統(tǒng)識(shí)別到ArUco Marker圖像時(shí)，利用其計(jì)算相鄰兩幀間的相機(jī)位姿變換，并以此為基準(zhǔn)判斷SLAM中相機(jī)位姿的漂移程度。如果漂移未達(dá)到優(yōu)化閾值，則跳過(guò)，否則根據(jù)此時(shí)由ArUco Marker計(jì)算出的位姿信息修正SLAM系統(tǒng)的相機(jī)位姿信息，從而減小累積誤差。

一方面，在跟蹤和局部建圖階段，不時(shí)地進(jìn)行誤差計(jì)算，在檢測(cè)到ArUco Marker后，將當(dāng)前幀作為關(guān)鍵幀記錄，并同時(shí)記錄當(dāng)前標(biāo)記估計(jì)位姿和SLAM估計(jì)位姿。之后每當(dāng)檢測(cè)到ArUco Marker就同時(shí)記錄兩者的位姿信息，通過(guò)比較兩者之間的變化程度來(lái)判斷SLAM的漂移程度。

設(shè)當(dāng)前檢測(cè)到ArUco Marker的畫(huà)面幀作為當(dāng)前關(guān)鍵幀Ki，上一次檢測(cè)到標(biāo)記的關(guān)鍵幀記為Km，由于標(biāo)記受到遮擋和相角度傾斜容易丟失跟蹤，因此創(chuàng)建的關(guān)鍵幀數(shù)量會(huì)比SLAM少，使用ORB特征匹配計(jì)算的上一個(gè)關(guān)鍵幀記為KS，使用當(dāng)前關(guān)鍵幀Ki 下估計(jì)的位姿對(duì)于ORB位姿進(jìn)行優(yōu)化。計(jì)算Ki 與KS 之間的相似變換矩陣，相似變換矩陣中包含了尺度漂移和位姿漂移產(chǎn)生的累計(jì)誤差，在將KS 的位姿進(jìn)行優(yōu)化之后，繼續(xù)往前一個(gè)特征點(diǎn)關(guān)鍵幀進(jìn)行優(yōu)化，直到優(yōu)化到Km停止，此時(shí)Ki 與Km 之間的關(guān)鍵幀位姿全部得到了優(yōu)化。

另一方面，在回環(huán)檢測(cè)階段，本文利用ArUcoMarker對(duì)ORB_SLAM2的閉環(huán)檢測(cè)環(huán)節(jié)進(jìn)行優(yōu)化，提高檢測(cè)效率。由于ArUco Marker內(nèi)置編碼信息，相比于詞袋檢索來(lái)說(shuō)更加容易，且標(biāo)記不易受到光照影響，檢測(cè)效率和穩(wěn)定性更好，所以優(yōu)先在鄰近關(guān)鍵幀隊(duì)列中進(jìn)行基于標(biāo)記的回環(huán)檢測(cè)。遍歷鄰近關(guān)鍵幀隊(duì)列根據(jù)標(biāo)記ID號(hào)進(jìn)行篩選，將含有相同標(biāo)記ID號(hào)的關(guān)鍵幀作為回環(huán)候選幀。然后對(duì)關(guān)鍵幀和回環(huán)候選幀進(jìn)行特征匹配，并求解兩幀之間的Sim3變換，對(duì)于滿足特征匹配數(shù)目的候選回環(huán)幀，基于共視關(guān)系，更新各個(gè)關(guān)鍵幀之間的連接關(guān)系，并根據(jù)優(yōu)化后的位姿進(jìn)行地圖點(diǎn)更新，消除累積誤差產(chǎn)生的不良地圖點(diǎn)，從而提高跟蹤精度。

1.3 利用SLAM 解決Marker 定位中對(duì)標(biāo)記的過(guò)度依賴問(wèn)題

在單純基于標(biāo)記的定位系統(tǒng)中，ArUco Marker是提供真實(shí)空間三維信息的唯一線索。因此，一旦ArUco Marker被部分遮擋或因相機(jī)拍攝角度不理想等原因無(wú)法正常識(shí)別標(biāo)記時(shí)，會(huì)導(dǎo)致三維注冊(cè)失敗，造成AR融合畫(huà)面中虛擬物體突然消失。

本文融合定位算法中，利用SLAM定位系統(tǒng)構(gòu)建的地圖會(huì)記錄下ArUco Marker的物理空間位置。當(dāng)基于標(biāo)記的定位失敗時(shí)，利用SLAM跟蹤系統(tǒng)檢測(cè)到的最佳平面進(jìn)行虛擬物體的疊加。從而解決單純基于標(biāo)記的定位系統(tǒng)中對(duì)于ArUco Marker的過(guò)度依賴問(wèn)題。

2 實(shí)驗(yàn)與評(píng)估

本文將ArUco Marker 定位與ORB_ SLAM2 定位相融合，并對(duì)整體框架進(jìn)行了優(yōu)化。為檢測(cè)算法有效性，本文在系統(tǒng)初始化、誤差累積和最終AR效果等方面進(jìn)行了對(duì)比實(shí)驗(yàn)。

2.1 實(shí)驗(yàn)軟硬件平臺(tái)

實(shí)驗(yàn)平臺(tái)配備Inter i7 CPU、RTX 3060 GPU 和16G內(nèi)存，搭載Ubuntu16系統(tǒng)，在Kdevelop上開(kāi)發(fā)，使用Cmake編譯運(yùn)行。

2.2 系統(tǒng)初始化時(shí)間對(duì)比實(shí)驗(yàn)

實(shí)際應(yīng)用中的AR系統(tǒng)，是以攝像頭隨機(jī)移動(dòng)過(guò)程中實(shí)時(shí)捕獲的視頻圖像為依據(jù)，完成系統(tǒng)的初始化。為了將本文融合算法與單純SLAM算法的初始化時(shí)間進(jìn)行對(duì)比，需要前后兩次啟動(dòng)程序，進(jìn)行初始化計(jì)時(shí)。但是兩次啟動(dòng)過(guò)程中，因無(wú)法保證攝像頭移動(dòng)軌跡完全一致，導(dǎo)致前后兩次捕獲的視頻畫(huà)面不完全相同，而視頻內(nèi)容會(huì)直接影響初始化時(shí)間。

因此，為了保證對(duì)比實(shí)驗(yàn)的客觀性和有效性，本文首先離線錄制一段場(chǎng)景視頻，然后用兩種算法分別對(duì)該視頻進(jìn)行初始化時(shí)間計(jì)時(shí)。實(shí)驗(yàn)結(jié)果如表1所示，顯然使用本文算法的初始化速度更快。

2.3 SLAM 累積誤差實(shí)驗(yàn)與評(píng)估

本文實(shí)驗(yàn)數(shù)據(jù)通過(guò)USB攝像頭拍攝真實(shí)環(huán)境獲得，如圖3所示，因無(wú)法獲得準(zhǔn)確的真實(shí)軌跡，故采用ORB_SLAM3算法[10]計(jì)算的相機(jī)軌跡作為參考軌跡，并使用SLAM精度評(píng)定工具—EVO進(jìn)行實(shí)驗(yàn)評(píng)估。

運(yùn)行實(shí)驗(yàn)數(shù)據(jù)得到軌跡對(duì)比圖，如圖4所示，圖中，虛線為標(biāo)準(zhǔn)參照線，綠色實(shí)線表示ORB_SLAM2估計(jì)的相機(jī)軌跡，藍(lán)色實(shí)線表示本文算法估計(jì)的相機(jī)軌跡。從圖中可以明顯看出，使用本文算法估計(jì)的相機(jī)軌跡線更貼近標(biāo)準(zhǔn)參照線，定位精度更好。

接下來(lái)運(yùn)用EVO工具對(duì)數(shù)據(jù)完成進(jìn)一步對(duì)比與分析。絕對(duì)軌跡誤差（Absolute Trajectory Error，ATE） 為相機(jī)位姿真實(shí)值與估計(jì)值的差值，用來(lái)體現(xiàn)SLAM 系統(tǒng)的定位精度，當(dāng)絕對(duì)位姿誤差越小時(shí)，算法定位精度越高，表2為絕對(duì)軌跡誤差的部分輸出結(jié)果，從最大值、最小值和均方根誤差的結(jié)果得出，本文算法可以獲得更好的精度，定位更加精準(zhǔn)。詳細(xì)的誤差分析如圖5所示，其中縱軸為絕對(duì)軌跡誤差，橫軸為時(shí)間，可以看到本文算法相比于ORB_ SLAM2算法在定位精度上得到了大幅提升。

相對(duì)位姿誤差（Relative Pose Error，RPE） 表示兩個(gè)相同時(shí)間戳上相機(jī)位姿變化的差異，可以表現(xiàn)SLAM系統(tǒng)的漂移程度。表3為相對(duì)位姿誤差的部分輸出結(jié)果，從最大值、最小值和均方根誤差的結(jié)果得出，本文算法漂移程度更小，跟蹤更加穩(wěn)定，詳細(xì)的誤差分析如圖6所示，其中縱軸為相對(duì)位姿誤差，橫軸為時(shí)間，可以看到本文算法相比于ORB_SLAM2算法，系統(tǒng)的漂移程度大大減小。

為了從各個(gè)角度驗(yàn)證本文算法的有效性，除了進(jìn)行累積誤差值的對(duì)比實(shí)驗(yàn)與評(píng)估外，還在AR系統(tǒng)中進(jìn)行了累計(jì)誤差造成虛擬物體漂移現(xiàn)象的觀察實(shí)驗(yàn)。圖7表示ORB_SLAM2算法下由于累積誤差導(dǎo)致虛擬物體發(fā)生了漂移現(xiàn)象，圖8表示在本文算法下虛擬物體疊加正常。通過(guò)兩圖中虛擬物體的位置看，使用本文的融合定位算法，明顯改善了因SLAM累積誤差造成的虛擬物體漂移現(xiàn)象。

2.4 環(huán)境特征點(diǎn)稀少情況下的跟蹤實(shí)驗(yàn)

本文融合定位算法解決了ORB_ SLAM2算法在特征點(diǎn)較少的環(huán)境下無(wú)法進(jìn)行跟蹤的情況，實(shí)驗(yàn)結(jié)果如圖9所示，由于人工標(biāo)記對(duì)單一環(huán)境起到了重要的信息補(bǔ)充，使得ORB_SLAM2能夠進(jìn)行正常的跟蹤和地圖構(gòu)建。

2.5 本文算法與單純基于Marker 跟蹤算法的AR 效果對(duì)比實(shí)驗(yàn)

圖10和圖11分別展示了當(dāng)Marker被部分遮擋和攝像頭拍攝角度不理想兩種情況下，基于Marker定位和本文融合定位算法實(shí)現(xiàn)的AR效果對(duì)比圖。很顯然，利用本文算法，解決了單純使用Marker跟蹤定位存在的弊端。

2.6 本文算法對(duì)于提升AR 系統(tǒng)魯棒性的實(shí)驗(yàn)

本文算法中，由于Marker參與了SLAM算法的點(diǎn)云地圖生成，使得兩個(gè)跟蹤系統(tǒng)的數(shù)據(jù)同步。因此在不影響相機(jī)位姿的情況下，甚至可以去掉Marker，將虛擬物體直接疊加在真實(shí)環(huán)境中。實(shí)驗(yàn)效果如圖12 所示，顯然，去掉Marker后虛實(shí)場(chǎng)景的融合效果幾乎不受影響。

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

元宇宙概念的興起，將推動(dòng)增強(qiáng)現(xiàn)實(shí)技術(shù)更快速的發(fā)展。本文提出融合ORB_ SLAM2和Marker的定位算法，利用Marker定位不僅加速了ORB_SLAM2的初始化，提高了整個(gè)系統(tǒng)的啟動(dòng)速度，而且減小了累積誤差，改善了AR漂移現(xiàn)象，同時(shí)解決了SLAM對(duì)弱紋理環(huán)境跟蹤不利的弊端；利用ORB_SLAM2解決了Marker定位算法中由于跟蹤失敗造成虛擬物體消失的問(wèn)題。

本文工作盡管為AR定位跟蹤提供了一種有效方法，但仍存在一些不足，例如ORB_SLAM2無(wú)法有效識(shí)別動(dòng)態(tài)目標(biāo)。后續(xù)工作將考慮結(jié)合語(yǔ)義SLAM區(qū)別和剔除動(dòng)態(tài)物體，從而加強(qiáng)SLAM的跟蹤穩(wěn)定性。