一種單目視覺里程計/UWB 組合室內(nèi)定位方法

徐愛功，閆可新，高 嵩，宋 帥，隋 心

（1. 遼寧工程技術(shù)大學 測繪與地理科學學院，遼寧 阜新 123000；2. 廣州南方測繪科技股份有限公司 南方高鐵研發(fā)中心，廣州 510000）

0 引言

隨著人們對定位服務(wù)需求的提高，室內(nèi)定位技術(shù)成為研究熱點[1]。在室內(nèi)環(huán)境下，由于信號的遮擋，全球衛(wèi)星導航系統(tǒng)（global navigation satellite system，GNSS）及與其相關(guān)的組合定位系統(tǒng)無法進行有效定位[2]。在此環(huán)境下，視覺傳感器作為無源傳感器，是定位常用的一類傳感器，具有成本低廉、輕巧便攜、安裝簡單、無需先驗信息且數(shù)據(jù)信息豐富等優(yōu)點，因此可以在弱GNSS 信號環(huán)境下進行定位[3]。文獻[4]首先提出了視覺里程計（visual odometry，VO）一詞，指通過連續(xù)的圖像作為輸入估計自身的運動，是同時定位與地圖構(gòu)建（simultaneous localization and mapping，SLAM）技術(shù)的重要組成部分。按照相機的種類，VO 可分為單目VO 和立體VO。相比于立體VO，單目VO計算量小且標定簡單，應(yīng)用更加廣泛[5]。按照實現(xiàn)方法可分為基于特征點法的VO 和直接法的VO[6]。相比于直接法VO，基于特征點法的VO 對光照變化和動態(tài)場景魯棒性強，不易受到光照和噪聲的影響。因此本文選用基于特征點法的VO 進行單目視覺的定位。然而單目VO 算法屬于相對定位方式，本身無法估計絕對尺度，在特征單一的場景下，其定位精度較低，容易跟蹤失敗且誤差隨時間累積。超寬帶（ultra-wide band，UWB）[7-8]沒有場景紋理特征限制且沒有誤差累積問題，在視距（line of sight，LOS）環(huán)境下，能達到厘米級甚至毫米級的測距定位精度。UWB 有多種定位方式，其中基于往返時間（round trip time，RTT）方法，通過對時間的測量間接計算出UWB 基準站與流動站之間的距離，不需要增加其他硬件，沒有時間同步誤差[9]。因此本文選擇RTT 方法進行UWB 的定位。但是UWB 屬于有源定位方法，在非視距（non line of sight，NLOS）環(huán)境下，由于障礙物的遮擋，信號無法直線傳輸，會產(chǎn)生折射、反射或透射現(xiàn)象，使UWB 定位精度和可靠性大幅降低[10-12]。

綜合上述分析，認為在復(fù)雜室內(nèi)環(huán)境下單獨采用單目視覺傳感器或UWB 進行定位很難得到可靠、高精度的定位結(jié)果，因此本文采用二者組合方式進行定位[13]。文獻[14]提出將單目視覺ORB-SLAM算法輸出的位置信息與UWB 解算的定位信息作為量測信息，通過擴展卡爾曼濾波（extended Kalman filter，EKF）進行數(shù)據(jù)融合來實現(xiàn)室內(nèi)定位，但該文沒有對UWB 的NLOS 誤差進行鑒別與剔除。文獻[15]提出了一種基于EKF 的單目視覺與UWB 融合定位的算法，利用UWB 流動站的平面坐標實現(xiàn)了單目視覺尺度因子的實時計算，適用于紋理稀疏、光亮度變化頻繁的室內(nèi)環(huán)境中的行人定位，兩類傳感器數(shù)據(jù)組合方式為松組合。文獻[16]提出了一種通過單目視覺SLAM 增強UWB進行無人機室內(nèi)導航的方法，實現(xiàn)無線定位未覆蓋區(qū)域的自主飛行，在這種方法中，VO 被用來估計高度，然而飛行區(qū)域圖是多次飛行建立的。文獻[17]提出了一種利用慣性傳感器、單目視覺和UWB，通過EKF 進行定位的方法，該方法用UWB 來輔助視覺慣性同步定位和制圖，以獲得改進的無漂移全局6 個自由度位姿估計，雖然結(jié)果精度較高，但使用3 個傳感器增加了成本和融合的復(fù)雜度?；谝陨戏治觯疚慕⒁环N單目VO/UWB 組合的定位方法，將單目VO 的位移增量和UWB 的測距信息作為量測值，利用EKF 算法進行參數(shù)解算。該組合定位方法利用單目VO 推算的距離信息剔除UWB 的NLOS 誤差，利用UWB 恢復(fù)單目VO尺度，在其中一種傳感器定位精度降低或無法定位時仍能保證定位的精度。

1 單目VO/UWB 組合定位方法

在室內(nèi)定位中，由于平面定位的需求多于高程定位[18]，且高程定位所需基站多，成本高，不適用于實際應(yīng)用，所以本文采用平面定位。

在單目VO/UWB 組合定位方法中，以UWB坐標系作為組合系統(tǒng)的坐標系，將單目VO 量測的位移增量經(jīng)過空間變換轉(zhuǎn)換到UWB 坐標系下。

1.1 單目VO 尺度因子估計

由于單目VO 無法直接測量尺度，在利用開始的前幾幀圖像進行初始化后，只能得到一個相對的尺度。因此，本文利用UWB 在這幾幀運動過程中獲得的絕對位置信息，推算得到的幀間絕對距離值，通過最小二乘算法對單目VO 進行絕對尺度因子的估計。

在組合移動平臺初始運動階段，平臺從靜止狀態(tài)開始以較低的速度勻速向前行進，單目相機和UWB 按照設(shè)置的頻率采集數(shù)據(jù)，單目相機得到一組等時間間隔的圖像并進行初始化，在VO 中處理后得到相機在兩幀之間的相對距離估計值dc，同時UWB 通過得到兩幀之間的絕對距離值dr，則單目VO 的尺度因子s可表示為

上述過程存在UWB 的觀測誤差和單目相機的圖像特征匹配誤差，整體誤差近似服從高斯分布，通過增加觀測數(shù)據(jù)量的方式可以減小誤差的影響。因此，本文用n組觀測數(shù)據(jù)對單目VO 的尺度因子進行估計，分別計算每組數(shù)據(jù)的尺度因子，取平均值作為最終的尺度因子，具體表示為

利用上述方法可以消除一定的隨機誤差，特別在前后兩組估計出的尺度因子相差較大時，UWB的觀測誤差還是會對結(jié)果造成影響。因此，本文利用最小二乘方法進行單目VO 尺度因子的估計，當誤差最小時，認為估計出的尺度因子為正確值。

根據(jù)n組觀測數(shù)據(jù)及式（1）得

通過該方法可以估計出單目VO 的尺度因子，這樣既解決了誤差的影響，又為其恢復(fù)了絕對尺度。在實際應(yīng)用中，如果初始化時間較短，可以采用等權(quán)的方式進行最小二乘解算；如果初始化時間較長，同時考慮單目VO 本身的尺度漂移影響，應(yīng)對不同的觀測數(shù)據(jù)采用不同的權(quán)值，然后利用最小二乘算法進行尺度估計。

1.2 UWB 非視距誤差鑒別

當UWB 定位出現(xiàn)NLOS 誤差時，RTT 測距值會發(fā)生較大變化，如果不對NLOS 誤差進行處理，會大幅降低單目VO/UWB 組合定位系統(tǒng)的精度及穩(wěn)定性。單目VO 通過對圖像序列進行預(yù)處理、初始化、位姿估計、跟蹤局部地圖等操作，得到相機每個關(guān)鍵幀的位姿。當在運動過程中，相機速度過快或在特征單一、光照不均勻、景深變化明顯的環(huán)境下定位時，可能會發(fā)生跟蹤失敗的現(xiàn)象，此時單目VO 的尺度因子會發(fā)生大幅變化，根據(jù)前后歷元的位移增量可以很容易對其鑒別。在跟蹤正常情況下，單目VO 的位移增量是相對準確的，因此本文借助單目VO 的位移增量，對UWB 的RTT 測距值進行NLOS 誤差鑒別。

根據(jù)單目VO 的坐標增量以及組合系統(tǒng)上一歷元的坐標，計算當前歷元組合系統(tǒng)的坐標，進而推算出組合系統(tǒng)與UWB 基準站間的距離，即

1.3 單目VO/UWB 組合定位算法

由于單目VO/UWB 組合是非線性系統(tǒng)，本文采用處理非線性系統(tǒng)的EKF 算法進行數(shù)據(jù)融合。

1.3.1 狀態(tài)方程

單目VO/UWB 組合的狀態(tài)方程為

圖1 單目VO/UWB 組合定位流程

2 實驗與分析

2.1 實驗說明

在本次室內(nèi)實驗中，組合移動平臺使用的相機是德國巴斯勒（Basler）公司生產(chǎn)的Aca2500-20gc型號工業(yè)單目相機，曝光時間設(shè)置為20 ms，光圈大小設(shè)置為F2.8，圖片分辨率設(shè)置為648×512。組合移動平臺中使用的 UWB 是美國泰姆·多曼（Time Domain）公司生產(chǎn)的PulsON440 測距模塊，其帶寬為3.1～4.8 GHz。實驗場景為教學樓內(nèi)某會議室，如圖2 所示。

圖2 室內(nèi)實驗場景

實驗前首先將相機和UWB 固定在移動平臺上，通過標定外參數(shù)完成空間坐標系統(tǒng)統(tǒng)一，然后利用計算機對二者添加時間標簽，進行時間同步。實驗過程中設(shè)置相機和 UWB 的采樣頻率同為2 Hz，相機通過吉格·埃（GigE）接口、UWB 通過USB 接口同時向計算機傳輸數(shù)據(jù)。將徠卡（Leica）棱鏡通過基座固定在移動平臺上，組合定位實驗平臺如圖3 所示。

圖3 單目相機/UWB 組合定位實驗平臺

實驗前將5 個UWB 基站安裝在三腳架上，分布于室內(nèi)各個角落，利用全站儀自動跟蹤棱鏡確定其真實坐標，利用7 個移動電源為1 個單目相機和6 個UWB 設(shè)備供電。設(shè)計好行進軌跡，實驗時移動平臺按照線路順時針由靜止開始勻速前進，利用全站儀自動跟蹤觀測移動平臺的位置作為參考軌跡，本次實驗軌跡總長度為31 m，實驗時長為5 min，實驗設(shè)計示意圖如圖4 所示。

圖4 實驗設(shè)計示意

2.2 實驗結(jié)果與分析

對實驗過程中單目VO 解算的結(jié)果與參考軌跡進行對比，結(jié)果如圖5 所示。

圖5 單目VO 解算軌跡與參考軌跡對比

從圖5 可以看出，整體軌跡與真實軌跡較為接近，然而由于在轉(zhuǎn)彎處相機景深和光照強度變化較大，導致單目VO 定位出現(xiàn)較大誤差；由于單目VO 定位存在累積誤差，導致定位誤差逐漸增大，且最終無法回到起點，最大定位誤差為35.6 cm。

對實驗過程UWB 解算的結(jié)果與參考軌跡進行對比，結(jié)果如圖6 所示。

圖6 UWB 解算軌跡與參考軌跡對比

從圖6 可以看出，在視距良好的情況下，UWB的定位精度能夠達到厘米級，解算軌跡與參考軌跡無較大偏差。在有2 個柱體或人為遮擋的時候，非視距情況的出現(xiàn)導致定位結(jié)果存在較大誤差，最大定位誤差為29.5 cm。

將本文提出的單目VO/UWB 組合算法解算的軌跡與參考軌跡進行對比，結(jié)果如圖7 所示。

圖7 單目VO/UWB 組合解算軌跡與參考軌跡對比

從圖7 可以看出，在有障礙物遮擋時，由于利用單目 VO 的位移增量鑒別并剔除了 UWB 的NLOS 誤差，軌跡貼近參考軌跡；由于UWB 測距信息的加入，減小了單目VO 的誤差累積。最大定位誤差為11.4 cm。相比較于單一傳感器，可以看出單目VO/UWB 組合定位方法解算的結(jié)果良好，有效地減弱了UWB 的非視距誤差對定位結(jié)果的影響，減小了單目VO 的軌跡漂移。組合方法的定位軌跡更加接近參考軌跡，可靠性強。

單目VO 定位、UWB 定位以及單目VO/UWB組合定位方法在x和y方向上的誤差如圖8 所示。

圖8 單目VO，UWB 和單目VO/UWB 組合三者誤差

分析圖8 可知，誤差在各個方向上的趨勢具有一致性。當出現(xiàn)非視距情況時，UWB 定位出現(xiàn)較大誤差，誤差的幅度大于單目VO 定位方法和單目VO/UWB 組合定位方法；在視距良好時，單目VO定位誤差的幅度大于 UWB 定位方法和單目VO/UWB 組合方法。在整個實驗過程中，單目VO/UWB 組合定位誤差的幅度均小于單目VO 定位方法和UWB 定位方法。

表1 給出了在室內(nèi)場景下，單目VO、UWB 以及單目VO/UWB 組合這3 種定位方法的結(jié)果在x和y方向上的誤差統(tǒng)計。表1 中：RMSE 為均方根誤差；MAX 為誤差最大值。

表1 三種定位方法結(jié)果誤差統(tǒng)計 單位：m

從表1 可以看出：1）單目VO 的定位精度相對較差，x方向的平均誤差為10.3 cm，y方向的平均誤差為6.6 cm，x方向最大誤差為33.9 cm，y方向最大誤差為13.8 cm；2）UWB 的定位精度相對較高，x方向的平均誤差為4.1 cm，y方向的平均誤差為4.7 cm，但易受到非視距的影響，x方向最大誤差為29.3 cm，y方向最大誤差為9.9 cm；3）單目VO/UWB 組合定位方法x方向的平均誤差為3.6 cm，y方向的平均誤差為3.5 cm，x方向最大誤差為9.5 cm，y方向最大誤差為8.4 cm，定位精度能穩(wěn)定維持在8 cm 左右，相較于單一傳感器魯棒性更好，定位精度更高，可靠性更強。

本次實驗結(jié)果表明，本文所設(shè)計的組合定位方法是切實可行的，該方法可以有效解決單目VO 定位中的誤差累積問題，提高了VO 算法的穩(wěn)定性，并解決了UWB 的NLOS 誤差問題，提高了UWB定位結(jié)果的魯棒性，組合定位精度可達亞分米級。

3 結(jié)束語

針對室內(nèi)環(huán)境下單目VO 定位缺少絕對尺度且存在誤差累積、UWB 定位受NLOS 誤差影響的等問題，本文提出了一種基于 EKF 的單目VO/UWB 組合的室內(nèi)定位方法，將單目VO 的位移增量和UWB 的測距信息作為量測值，通過EKF算法進行數(shù)據(jù)融合并進行了室內(nèi)實測實驗。實驗結(jié)果表明，單目VO/UWB 組合方法減小了單目VO的誤差累積，消除了UWB 的NLOS 影響，相較于單一傳感器提高了整體定位的精度及魯棒性。