融合SLAM和Marker的AR單目視覺定位算法

曹志強 豐艷

關鍵詞：增強現實；ArUco Marker；ORB_SLAM2；位姿估計；單目視覺

0 引言

增強現實（Augmented Reality，AR） 是將計算機生成的虛擬圖像或信息實時、動態地疊加到用戶所看到的真實環境中，實現虛擬世界與現實世界的無縫結合，使人們感知到現實世界之外的信息。近年來隨著AR技術的發展，其在教育、醫療、娛樂等領域有了越來越多的應用[1-2]。

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

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

1 算法設計

正文內容針對Marker定位和SLAM定位各自存在的缺陷，同時考慮兩者在定位上的互補性，本文進行兩者的融合算法研究，主要圍繞以下三個方面展開：

1） 利用ArUco Marker輔助完成SLAM系統的初始化，有效解決SLAM初始化速度慢的問題。

2） 利用ArUco Marker減小SLAM跟蹤過程中的累積誤差，從而有效解決AR的漂移問題。另外，使用Marker進行回環檢測比ORB特征點更快、更準確。

3） 利用SLAM定位解決單純基于Marker定位時對于標記的過度依賴問題。

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

1.1 利用ArUco Marker 進行快速初始化

本文在ORB_SLAM2 的初始化環節利用ArUcoMarker完成快速初始化。

首先，選擇包含Marker的兩幀畫面作為初始幀I1和匹配幀I2，兩幀分別以ArUco Marker坐標系為世界坐標系得到對應的相機位姿T1、T2，SLAM初始化過程中需要進行地圖點初始化，已知相機位姿的情況下使用對極幾何原理進行特征點匹配和地圖點創建。

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

使用上述方法進行所有特征點的匹配計算，在匹配過程中，會出現一些誤匹配的點，通過限制搜索半徑剔除誤匹配的點，保留符合的地圖點，將所有符合的地圖點存入地圖中完成系統初始化。

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

當系統識別到ArUco Marker圖像時，利用其計算相鄰兩幀間的相機位姿變換，并以此為基準判斷SLAM中相機位姿的漂移程度。如果漂移未達到優化閾值，則跳過，否則根據此時由ArUco Marker計算出的位姿信息修正SLAM系統的相機位姿信息，從而減小累積誤差。

一方面，在跟蹤和局部建圖階段，不時地進行誤差計算，在檢測到ArUco Marker后，將當前幀作為關鍵幀記錄，并同時記錄當前標記估計位姿和SLAM估計位姿。之后每當檢測到ArUco Marker就同時記錄兩者的位姿信息，通過比較兩者之間的變化程度來判斷SLAM的漂移程度。

設當前檢測到ArUco Marker的畫面幀作為當前關鍵幀Ki，上一次檢測到標記的關鍵幀記為Km，由于標記受到遮擋和相角度傾斜容易丟失跟蹤，因此創建的關鍵幀數量會比SLAM少，使用ORB特征匹配計算的上一個關鍵幀記為KS，使用當前關鍵幀Ki 下估計的位姿對于ORB位姿進行優化。計算Ki 與KS 之間的相似變換矩陣，相似變換矩陣中包含了尺度漂移和位姿漂移產生的累計誤差，在將KS 的位姿進行優化之后，繼續往前一個特征點關鍵幀進行優化，直到優化到Km停止，此時Ki 與Km 之間的關鍵幀位姿全部得到了優化。

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

1.3 利用SLAM 解決Marker 定位中對標記的過度依賴問題

在單純基于標記的定位系統中，ArUco Marker是提供真實空間三維信息的唯一線索。因此，一旦ArUco Marker被部分遮擋或因相機拍攝角度不理想等原因無法正常識別標記時，會導致三維注冊失敗，造成AR融合畫面中虛擬物體突然消失。

本文融合定位算法中，利用SLAM定位系統構建的地圖會記錄下ArUco Marker的物理空間位置。當基于標記的定位失敗時，利用SLAM跟蹤系統檢測到的最佳平面進行虛擬物體的疊加。從而解決單純基于標記的定位系統中對于ArUco Marker的過度依賴問題。

2 實驗與評估

本文將ArUco Marker 定位與ORB_ SLAM2 定位相融合，并對整體框架進行了優化。為檢測算法有效性，本文在系統初始化、誤差累積和最終AR效果等方面進行了對比實驗。

2.1 實驗軟硬件平臺

實驗平臺配備Inter i7 CPU、RTX 3060 GPU 和16G內存，搭載Ubuntu16系統，在Kdevelop上開發，使用Cmake編譯運行。

2.2 系統初始化時間對比實驗

實際應用中的AR系統，是以攝像頭隨機移動過程中實時捕獲的視頻圖像為依據，完成系統的初始化。為了將本文融合算法與單純SLAM算法的初始化時間進行對比，需要前后兩次啟動程序，進行初始化計時。但是兩次啟動過程中，因無法保證攝像頭移動軌跡完全一致，導致前后兩次捕獲的視頻畫面不完全相同，而視頻內容會直接影響初始化時間。

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

2.3 SLAM 累積誤差實驗與評估

本文實驗數據通過USB攝像頭拍攝真實環境獲得，如圖3所示，因無法獲得準確的真實軌跡，故采用ORB_SLAM3算法[10]計算的相機軌跡作為參考軌跡，并使用SLAM精度評定工具—EVO進行實驗評估。

運行實驗數據得到軌跡對比圖，如圖4所示，圖中，虛線為標準參照線，綠色實線表示ORB_SLAM2估計的相機軌跡，藍色實線表示本文算法估計的相機軌跡。從圖中可以明顯看出，使用本文算法估計的相機軌跡線更貼近標準參照線，定位精度更好。

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

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

為了從各個角度驗證本文算法的有效性，除了進行累積誤差值的對比實驗與評估外，還在AR系統中進行了累計誤差造成虛擬物體漂移現象的觀察實驗。圖7表示ORB_SLAM2算法下由于累積誤差導致虛擬物體發生了漂移現象，圖8表示在本文算法下虛擬物體疊加正常。通過兩圖中虛擬物體的位置看，使用本文的融合定位算法，明顯改善了因SLAM累積誤差造成的虛擬物體漂移現象。

2.4 環境特征點稀少情況下的跟蹤實驗

本文融合定位算法解決了ORB_ SLAM2算法在特征點較少的環境下無法進行跟蹤的情況，實驗結果如圖9所示，由于人工標記對單一環境起到了重要的信息補充，使得ORB_SLAM2能夠進行正常的跟蹤和地圖構建。

2.5 本文算法與單純基于Marker 跟蹤算法的AR 效果對比實驗

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

2.6 本文算法對于提升AR 系統魯棒性的實驗

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

3 結束語

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

本文工作盡管為AR定位跟蹤提供了一種有效方法，但仍存在一些不足，例如ORB_SLAM2無法有效識別動態目標。后續工作將考慮結合語義SLAM區別和剔除動態物體，從而加強SLAM的跟蹤穩定性。