徐愛(ài)功，閆可新，高 嵩，宋 帥，隋 心

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

0 引言

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

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

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

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

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

1.1 單目VO 尺度因子估計(jì)

由于單目VO 無(wú)法直接測(cè)量尺度，在利用開(kāi)始的前幾幀圖像進(jìn)行初始化后，只能得到一個(gè)相對(duì)的尺度。因此，本文利用UWB 在這幾幀運(yùn)動(dòng)過(guò)程中獲得的絕對(duì)位置信息，推算得到的幀間絕對(duì)距離值，通過(guò)最小二乘算法對(duì)單目VO 進(jìn)行絕對(duì)尺度因子的估計(jì)。

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

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

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

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

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

1.2 UWB 非視距誤差鑒別

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

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

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

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

1.3.1 狀態(tài)方程

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

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

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

2.1 實(shí)驗(yàn)說(shuō)明

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

圖2 室內(nèi)實(shí)驗(yàn)場(chǎng)景

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

圖3 單目相機(jī)/UWB 組合定位實(shí)驗(yàn)平臺(tái)

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

圖4 實(shí)驗(yàn)設(shè)計(jì)示意

2.2 實(shí)驗(yàn)結(jié)果與分析

對(duì)實(shí)驗(yàn)過(guò)程中單目VO 解算的結(jié)果與參考軌跡進(jìn)行對(duì)比，結(jié)果如圖5 所示。

圖5 單目VO 解算軌跡與參考軌跡對(duì)比

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

對(duì)實(shí)驗(yàn)過(guò)程UWB 解算的結(jié)果與參考軌跡進(jìn)行對(duì)比，結(jié)果如圖6 所示。

圖6 UWB 解算軌跡與參考軌跡對(duì)比

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

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

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

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

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

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

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

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

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

從表1 可以看出：1）單目VO 的定位精度相對(duì)較差，x方向的平均誤差為10.3 cm，y方向的平均誤差為6.6 cm，x方向最大誤差為33.9 cm，y方向最大誤差為13.8 cm；2）UWB 的定位精度相對(duì)較高，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 左右，相較于單一傳感器魯棒性更好，定位精度更高，可靠性更強(qiáng)。

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

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

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