喬 智，徐愛功，隋 心，郝雨時

(遼寧工程技術大學 測繪與地理科學學院，遼寧 阜新 123000)

0 引言

基于單目視覺的同時定位和地圖構建(simultaneous localization and mapping，SLAM)技術近年來發(fā)展迅速，從其發(fā)展歷程可以大致分成3個階段，分別為基于擴展卡爾曼濾波(extended Kalman filter，EKF)的SLAM、基于粒子濾波的SLAM和基于圖優(yōu)化的SLAM。最早由文獻[1]提出的基于EKF的SLAM，通過構建SLAM系統(tǒng)的狀態(tài)方程和量測方程來估計相機及地圖點的位置。之后又有研究人員在此基礎上做了許多改進，文獻[2]提出了基于無跡卡爾曼濾波的SLAM方法；文獻[3]提出了基于迭代EKF的SLAM方法。由于EKF-SLAM算法只能處理噪聲為高斯噪聲的情況，文獻[4]提出用粒子濾波器來解決這一問題，其主要思想是用粒子濾波算法對相機和地圖點的位置進行估計，并且通過大量的粒子對位置進行多個假設，因此適用于不同的噪聲情況。近年來基于圖優(yōu)化的SLAM[5]成為研究熱點，其主要思想是把位姿估計作為1個全局優(yōu)化問題，通過圖建模，把所有的觀測數(shù)據(jù)和約束條件一起放到圖中，最終利用這些信息進行全局優(yōu)化處理，獲得相機的位姿和三維地圖點位置。在圖優(yōu)化SLAM中具有里程碑意義的是文獻[6]提出的平行跟蹤和地圖構建算法(parallel tracking and mapping，PTAM)，他們首次把SLAM算法分成2個并行線程，即相機特征跟蹤線程和地圖構建線程，實現(xiàn)在線的實時SLAM。繼PTAM之后，文獻[7-8]在PTAM算法的基礎上，添加了閉環(huán)修正方法進而提出了ORB-SLAM算法。最近有學者將深度學習理論與SLAM相結合，文獻[9]通過卷積神經(jīng)網(wǎng)絡學習圖像數(shù)據(jù)特征，進行前端視覺里程計的最優(yōu)估計。文獻[10]提出一種利用貝葉斯卷積神經(jīng)網(wǎng)絡進行SLAM重定位的方法。與其他的SLAM方法相比，ORB-SLAM具有較高的定位精度和實時性。因此本文使用該算法進行單目視覺的定位。ORB-SLAM算法使用ORB(oriented FAST and rotated BRIEF)[11]作為特征提取和描述的方法，由于ORB算法的特征檢測算子缺乏對尺度變化的適應性，當場景相對單一或運動速度過快的時候，檢測效果會下降，進而導致整個SLAM系統(tǒng)跟蹤失敗。

在室內定位中，超寬帶(ultra wide band，UWB)技術利用功率譜密度極低、脈沖寬度極窄的脈沖信號傳輸數(shù)據(jù)，具有時間分辨率極高、障礙穿透能力強等特點，在視距(line of sight，LOS)環(huán)境下能獲得厘米甚至毫米級的測距和定位精度[12-13]，適用于室內環(huán)境下的高精度定位。UWB定位通常有3種方式：信號到達角(angle of arrival，AOA)，接收信號強度(recevied signal strength，RSS)、信號到達時間(time of arrival，TOA)/信號到達時間差(time difference of arrival，TDOA)。相比較于以上3種方法，基于往返時間(round trip time，RTT)的測距方法可以通過對時間的測量，間接的計算出UWB基準站和流動站之間距離。不需要增加其他硬件，避免了基準站和流動站之間的時間同步。通過該方法進行室內定位，能夠獲得較高的定位精度[14-16]。基于以上特點本文使用RTT算法進行UWB室內定位。

UWB屬于有源定位方法，雖然定位精度比較高，但當信號受到干擾時，會產(chǎn)生較大的誤差。單目視覺ORB-SLAM屬于無源定位方法，當環(huán)境比較單一或運動速度比較快時容易出現(xiàn)跟蹤失敗導致系統(tǒng)崩潰，并且該方法屬于相對定位方式，定位會隨著時間增加而發(fā)散。根據(jù)上述分析可以認為在復雜的室內環(huán)境下僅僅依靠某1種定位方法，很難取得魯棒性好、精度高的定位結果，因此，本文將2種定位方法進行組合，單目視覺ORB-SLAM可降低UWB非視距(non line of sight，NLOS)誤差的影響，UWB的加入可以提升定位的可靠性，保證定位的精度，利用組合定位方法最終實現(xiàn)高精度室內定位。

1 單目視覺和UWB定位算法原理

1.1 單目視覺定位算法原理

ORB-SLAM是1種采用圖優(yōu)化理論的單目視覺同步定位與建圖算法，在小型和大型的室內室外環(huán)境下實時的得到精確的相機位置和姿態(tài)。文獻[8]完整地介紹了該算法，ORB-SLAM算法流程概述如圖1所示。本文對它的3個并行線程做了簡單的介紹。

1)特征跟蹤線程：工業(yè)相機獲得視頻幀，對影像進行預處理，將影像轉化為灰度圖。對灰度圖進行特征提取，采用的是FAST算子檢測特征點，再給檢測到的特征點加個方向信息，構成OFAST，然后用BRIEF作為特征描述符。使用詞袋模型[17]對影像的特征點進行分類。經(jīng)過分類后的特征點在前后幀影像中進行快速匹配，進而求出前后幀影像間的相對位置關系，最終得到當前相機的位姿信息。然后用光流法[18]來跟蹤其位置。如果跟蹤成功，則進行本地地圖的構建；如果跟蹤失敗，則等待著重定位或閉環(huán)檢測。

2)構建局部地圖線程：存儲跟蹤線程中獲得的每1幀影像的三維地圖點和關鍵幀到數(shù)據(jù)庫，對關鍵幀進行特征匹配，如果匹配成功了，就采用光束平差法(bundle adjustment，BA)來優(yōu)化估計三維地圖點位置和關鍵幀位姿。從而構建環(huán)境的初始化地圖。當相機運動到新的環(huán)境時，通過特征跟蹤線程產(chǎn)生新的關鍵幀并添加到地圖中，在關鍵幀數(shù)據(jù)庫中新舊關鍵幀會建立關聯(lián)并通過重投影得到新環(huán)境下的三維地圖點，從而構建新環(huán)境的局部地圖。

3)閉環(huán)檢測與修正線程：把局部構圖線程增加的三維地圖點和關鍵幀作為閉環(huán)檢測候選幀，在建立好的地圖中對候選幀進行搜索并匹配，對相機進行重定位或閉環(huán)檢測。為了減少ORB-SLAM算法中的前端誤差積累，對整個算法進行有效的閉環(huán)修正。首先在本征圖上利用通用圖優(yōu)化框架(general graph optimization，G2o)優(yōu)化關鍵幀的位姿，將累積誤差沿著圖分散開；其次根據(jù)優(yōu)化后的關鍵幀位姿矯正三維地圖點云；最后更新地圖點。

1.2 UWB定位算法原理

RTT測距是通過流動站到基準站之間信號傳播的往返時間來確定二者間的距離。則RTT測距的模型可表示為

(1)

式中：ts和Xr為UWB流動站發(fā)出信號時的時間和位置；tr和Xb為UWB基準站接收信號時的時間和位置；tD為UWB流動站與基準站之間的標準時間偏差；‖·‖2為2范數(shù)；c為光速。那么，通過RTT測距得到的流動站和基準站之間的距離可表示為

d=‖Xr-Xb‖2=rRTT-rD

(2)

式中：rRTT為真實的流動站和基準站之間的距離；rD=ctD/2為標準時間偏差造成的測距誤差。為了保證組合導航的精度，本文對實驗過程中使用的UWB進行標準時間偏差標定，具體方法見文獻[19]。

在實際UWB平面定位中，由于流動站和基準站之間的真實距離無法準確獲得，所以本文用下式表示UWB基準站和流動站之間的真實距離為

(3)

(4)

Y=AX+V

(5)

式中：

2 單目視覺/UWB組合定位算法

單目視覺ORB-SLAM/UWB組合定位方法屬于非線性系統(tǒng)，所以本文使用處理非線性問題的EKF算法?；贓KF的單目視覺ORB-SLAM/UWB組合定位模型的狀態(tài)方程和量測方程構建如下。

在單目視覺ORB-SLAM/UWB組合方法中，以UWB定位過程中所使用的獨立坐標系作為全局坐標系，將單目視覺ORB-SLAM解算出的位置信息經(jīng)過空間變換，轉換到UWB坐標系下。將UWB和單目視覺ORB-SLAM的數(shù)據(jù)進行融合，獲得組合系統(tǒng)的位置和速度更新，其模型為

(6)

Xk=FXk-1+Wk-1

(7)

(8)

利用經(jīng)過空間變換的單目視覺ORB-SLAM的位置信息與UWB解算出的位置信息的差值作為組合系統(tǒng)的量測信息。則組合系統(tǒng)的量測方程為

Yk=HXk+Vk

(9)

(10)

式中：I為單位矩陣；Rk為觀測噪聲協(xié)方差矩陣。根據(jù)狀態(tài)方程和量測方程，通過EKF進行狀態(tài)更新和量測更新得到組合系統(tǒng)的定位信息。

3 實驗與分析

3.1 單目視覺/UWB平臺搭建

移動定位平臺使用的相機是Basler公司的Aca2500-20gc型號單目相機，鏡頭為FL-BC1218A-VG定焦鏡頭，焦距為12.5 mm，分辨率為1 296個像素乘1 024個像素。設置采樣頻率為1 Hz。使用的UWB是Time Domain的PulsON410通信與測距模塊，其帶寬為3.1～5.3 GHz，數(shù)據(jù)采樣頻率為1 Hz。在移動定位平臺上，固定2種傳感器，并在實驗前對2個傳感器的定位中心進行統(tǒng)一，即保證2個中心在同一條鉛垂線上。對UWB和相機采集的數(shù)據(jù)都加上時間標簽，保證這個時間標簽和電腦的系統(tǒng)時間保持統(tǒng)一，在筆記本電腦上同時采集2個傳感器的數(shù)據(jù)。具體平臺如圖2所示。

3.2 室內定位實驗與分析

本文在教學樓中分別進行正常光照下小場地定位實驗和弱光照下大場地定位實驗。小場地定位實驗，在實驗室中進行，實驗室的長寬分別為12和6 m，實驗時間選擇在中午，光照充足。利用4個UWB模塊、移動定位平臺、萊卡TS09全站儀和聯(lián)想筆記本電腦進行組合定位實驗。4個UWB模塊分別作為基站布置在實驗室的4角，已預先通過全站儀獲取固定軌跡各拐點坐標和4個UWB基準站坐標，本文實驗場景的詳細布設如圖3所示，矩形ABCD為固定軌跡的平面圖。移動定位平臺以接近勻速度沿著已知的固定軌跡運動，實驗時間為3 min。分別比較單一傳感器和組合系統(tǒng)的定位精度，實驗結果如圖4～圖6所示。

對實驗過程中采集到的UWB的數(shù)據(jù)進行解算，解算結果與固定軌跡進行對比，對比結果如圖4所示?？梢钥闯鲈谡Ｇ闆r下UWB能達到厘米級的定位精度，但當場景中出現(xiàn)墻或人遮擋等非視距情況時，定位精度出現(xiàn)較大的誤差，最大誤差能達到20 cm。

對實驗過程中單目視覺ORB-SLAM算法解算的位置信息與固定軌跡進行對比，對比結果如圖5所示。由于在轉彎處相機場景變化比較大，導致單目視覺ORB-SLAM跟蹤失敗。所以，定位軌跡缺失左邊部分。因為單目視覺ORB-SLAM算法本身在定位上存在累積誤差，所以在軌跡的后半段發(fā)生漂移。其中最大誤差為10 cm。

采用本文提出的單目視覺ORB-SLAM/UWB組合定位方法對實驗數(shù)據(jù)進行解算，解算結果與固定軌跡對比，結果如圖6所示。可以看出單目視覺ORB-SLAM/UWB組合定位方法解算的結果良好，未出現(xiàn)顯著的跳變與誤差累積，解決了單目視覺ORB-SLAM由于跟蹤失敗導致的無法定位的問題。從軌跡上看組合方法的最大定位誤差是7 cm。但相比較于單一的UWB定位結果，誤差有了明顯的縮小。

表1給出了小場地下單目視覺ORB-SLAM、UWB以及單目視覺ORB-SLAM/UWB組合這3種定位方法在x和y方向上的誤差統(tǒng)計。其中STD表示標準差，MAX表示最大值?？梢钥闯鯱WB的定位精度相對較高，但是穩(wěn)定性不好，易受到非視距的影響；單目視覺ORB-SLAM系統(tǒng)的穩(wěn)定性不好，魯棒性不強，易受到環(huán)境因素影響。單目視覺ORB-SLAM/UWB組合定位方法的精度能穩(wěn)定維持在6 cm左右，相比較于單一傳感器魯棒性更好，定位精度更高，定位結果比較接近固定軌跡，可靠性強。

表1 3種定位方法結果誤差統(tǒng)計 m

大場地定位實驗，在報告廳中進行，報告廳的長寬分別為18和14 m，實驗時間選擇在傍晚，光照相對較弱。具體實驗環(huán)境如圖7所示，實驗布置和小場地實驗一樣。通過全站儀獲取固定軌跡坐標，移動定位平臺以接近勻速度沿著固定軌跡運動，實驗時間為8 min。

分別比較單一傳感器和組合系統(tǒng)的定位精度，實驗結果如圖8所示。圖中實線表示組合定位軌跡，點虛線表示單目視覺ORB-SLAM定位軌跡，虛線表示固定軌跡，點表示UWB定位軌跡。橢圓形區(qū)域是由于室內柱子遮擋而產(chǎn)生的NLOS誤差，可以看出當場景中出現(xiàn)遮擋等非視距情況時，UWB的定位精度出現(xiàn)較大跳變和誤差，最大誤差能達到2 m。因為ORB特征提取算法具有一定的光照不變性，所以在弱光照下單目視覺ORB-SLAM仍可以進行定位。但由于算法在定位過程中的誤差累計，在軌跡的后半段發(fā)生漂移，其中最大誤差為11 cm。相比較于單一傳感器，可以看出單目視覺ORB-SLAM/UWB組合定位方法解算的結果良好，有效克服了顯著的跳變與誤差累積，組合方法的定位軌跡更加接近固定軌跡，可靠性強。其最大定位誤差是8 cm。

表2給出了大場地下單目視覺ORB-SLAM、UWB以及單目視覺ORB-SLAM/UWB組合這3種定位方法在x和y方向上的誤差統(tǒng)計?？梢钥闯鲇捎贜LOS的影響UWB的定位精度相對較差。單目視覺ORB-SLAM系統(tǒng)可以在弱光照下進行定位，但穩(wěn)定性不好，易發(fā)生漂移。單目視覺ORB-SLAM/UWB組合定位方法的精度能穩(wěn)定維持在7 cm左右，有效減弱了UWB非視距的影響，相比較于單一傳感器魯棒性更好，定位精度更高。

表2 3種定位方法結果誤差統(tǒng)計 m

4 結束語

針對在室內環(huán)境下單目視覺ORB-SLAM，易發(fā)生跟蹤失敗導致定位失效的情況，本文提出了1種單目視覺ORB-SLAM/UWB組合的室內定位方法，并使用搭建的移動定位平臺在室內環(huán)境下采集影像和UWB測距數(shù)據(jù)，對本文提出的方法進行實驗驗證。實驗結果表明所設計的組合定位方法是切實可行的，利用該方法可以在ORB-SLAM跟蹤失敗的時候繼續(xù)定位，提高了SLAM算法的穩(wěn)定性。并且減少了NLOS造成的UWB定位誤差，定位精度可達亞分米級。