面向室內(nèi)場(chǎng)景的主被動(dòng)融合視覺(jué)定位系統(tǒng)

謝挺，張曉杰，葉智超，王子豪，王政，張涌，周曉巍，姬曉鵬*

1. 浙江大學(xué)CAD&CG國(guó)家重點(diǎn)實(shí)驗(yàn)室, 杭州 310058; 2. 中國(guó)船舶工業(yè)系統(tǒng)工程研究院, 北京 100094;3. 中訊郵電咨詢?cè)O(shè)計(jì)院有限公司, 北京 100048

0 引 言

高精度的室內(nèi)目標(biāo)定位技術(shù)在虛擬現(xiàn)實(shí)、增強(qiáng)現(xiàn)實(shí)和機(jī)器人導(dǎo)航等領(lǐng)域具有重要的應(yīng)用價(jià)值。按照傳感器信號(hào)來(lái)源，常見(jiàn)的室內(nèi)定位技術(shù)可分為基于Wi-Fi等無(wú)線信號(hào)的定位技術(shù)、基于慣性導(dǎo)航的定位技術(shù)、基于地磁的定位技術(shù)和基于視覺(jué)信息的定位技術(shù)。其中，基于Wi-Fi等無(wú)線信號(hào)的定位技術(shù)對(duì)設(shè)備和場(chǎng)地有較高要求；基于慣性導(dǎo)航的定位技術(shù)只能通過(guò)慣性測(cè)量單元(inertial measurement unit，IMU)獲取相對(duì)位置，無(wú)法獲得絕對(duì)位置；而地磁定位技術(shù)時(shí)間開(kāi)銷較大且精度不高。在室內(nèi)環(huán)境中，由于圖像數(shù)據(jù)存在細(xì)節(jié)豐富、易于獲取以及部署快捷等天然優(yōu)勢(shì)，基于視覺(jué)的室內(nèi)定位方法得到廣泛關(guān)注。室內(nèi)視覺(jué)定位系統(tǒng)可以利用易于獲取的RGB圖像，對(duì)已知目標(biāo)進(jìn)行精確位姿估計(jì)。

按攝像頭部署方式，基于視覺(jué)的室內(nèi)定位方法可分為主動(dòng)式定位(移動(dòng)觀測(cè)視角)和被動(dòng)式定位 (固定觀測(cè)視角)兩種。被動(dòng)式定位方法利用部署在場(chǎng)景中的固定攝像頭，通過(guò)檢測(cè)圖像中的目標(biāo)關(guān)鍵點(diǎn)進(jìn)行模板匹配，從而解算出目標(biāo)的位姿數(shù)據(jù)。這種方案的優(yōu)點(diǎn)是定位結(jié)果比較穩(wěn)定，不易受到光照、模糊圖像的影響，但由于觀測(cè)視角的限制，無(wú)法處理場(chǎng)景中存在的物體遮擋情況。主動(dòng)式定位方法則是利用固定在定位目標(biāo)本身的攝像頭，通過(guò)檢測(cè)場(chǎng)景的特征點(diǎn)，并與事先構(gòu)建的3維場(chǎng)景模型進(jìn)行特征匹配來(lái)得到目標(biāo)的位姿信息。這種方案的缺陷是過(guò)度依賴圖像的紋理特征，對(duì)于紋理豐富、特征明顯的場(chǎng)景可以得到比較準(zhǔn)確的定位結(jié)果；而對(duì)于紋理特征缺失的場(chǎng)景，如墻面等弱紋理區(qū)域，定位結(jié)果非常不穩(wěn)定。

在特定應(yīng)用場(chǎng)景，例如室內(nèi)移動(dòng)機(jī)器人作業(yè)，既可以通過(guò)室內(nèi)固定的監(jiān)控?cái)z像頭進(jìn)行目標(biāo)的被動(dòng)式定位，也可以通過(guò)定位目標(biāo)自身(移動(dòng)機(jī)器人)的移動(dòng)攝像頭進(jìn)行主動(dòng)式定位，這兩類方法的定位結(jié)果有一定的共同性，而采用單一的主動(dòng)式或被動(dòng)式定位方法，都存在場(chǎng)景適應(yīng)能力不足、定位精度受限等問(wèn)題。

針對(duì)這類應(yīng)用場(chǎng)景，本文提出一種主被動(dòng)融合的室內(nèi)場(chǎng)景定位系統(tǒng)。首先，基于單目標(biāo)檢測(cè)深度學(xué)習(xí)框架，提出一種基于平面先驗(yàn)的物體位姿估計(jì)方法，利用室內(nèi)場(chǎng)景中普遍存在的平面約束，對(duì)運(yùn)動(dòng)目標(biāo)進(jìn)行3自由度(degree of freedom，DoF)位姿估計(jì)；其次，提出一個(gè)基于無(wú)損卡爾曼濾波(unscented Kalman filter，UKF)的主被動(dòng)融合定位框架，對(duì)主動(dòng)式和被動(dòng)式定位模塊得到的位姿結(jié)果進(jìn)行融合，提升了室內(nèi)場(chǎng)景下運(yùn)動(dòng)目標(biāo)位姿估計(jì)結(jié)果的穩(wěn)定性和精準(zhǔn)性。為驗(yàn)證提出的主被動(dòng)融合定位系統(tǒng)的性能，在仿真平臺(tái)iGibson和真實(shí)室內(nèi)場(chǎng)景進(jìn)行實(shí)驗(yàn)。實(shí)驗(yàn)結(jié)果表明，本文提出的融合定位方法可以有效提升室內(nèi)場(chǎng)景中移動(dòng)目標(biāo)的定位精度及準(zhǔn)確率。

本文主要貢獻(xiàn)如下：1)提出一個(gè)基于無(wú)損卡爾曼濾波的主被動(dòng)融合室內(nèi)視覺(jué)定位系統(tǒng)框架，可以有效解決弱紋理及遮擋條件下的室內(nèi)移動(dòng)目標(biāo)的位姿估計(jì)問(wèn)題；2)提出一種基于平面先驗(yàn)的物體位姿估計(jì)方法，可有效提升室內(nèi)場(chǎng)景中運(yùn)動(dòng)目標(biāo)的定位精度。

1 相關(guān)工作

視覺(jué)定位技術(shù)根據(jù)定位原理不同，可以分為被動(dòng)式定位技術(shù)和主動(dòng)式定位技術(shù)兩種。

1.1 被動(dòng)式視覺(jué)定位技術(shù)

被動(dòng)式定位的目標(biāo)是根據(jù)固定視角得到的圖像進(jìn)行目標(biāo)的定位，并得到目標(biāo)物體的6-DoF位姿數(shù)據(jù)。傳統(tǒng)的單目標(biāo)定位方法主要通過(guò)模板匹配技術(shù)獲取目標(biāo)的3維點(diǎn)位置信息，如基于圖像的梯度響應(yīng)圖檢測(cè)場(chǎng)景中的3維物體(Hinterstoisser等，2013)，或基于模型邊緣輪廓的形狀描述子估計(jì)目標(biāo)位姿(Zhu等，2014)。但傳統(tǒng)方法對(duì)環(huán)境變化和物體遮擋關(guān)系比較敏感，無(wú)法處理結(jié)構(gòu)復(fù)雜的圖像。

隨著深度學(xué)習(xí)的發(fā)展，卷積神經(jīng)網(wǎng)絡(luò)在復(fù)雜結(jié)構(gòu)和環(huán)境變化的圖像檢測(cè)和識(shí)別領(lǐng)域展示出優(yōu)秀性能，例如PoseNet(Kendall等，2015)使用卷積神經(jīng)網(wǎng)絡(luò)(convolutional neural network，CNN)直接回歸目標(biāo)物體的位姿數(shù)據(jù)，但受限于RGB圖像的深度信息缺乏和龐大的解空間搜索規(guī)模，效果并不魯棒。Sch?nberger等人(2017)對(duì)手工特征和深度學(xué)習(xí)特征的2D—3D匹配性能進(jìn)行評(píng)估，肯定了深度學(xué)習(xí)在圖像匹配定位上的優(yōu)秀效果。PoseCNN(Xiang等，2018)則通過(guò)預(yù)測(cè)2維圖像的深度圖改善3維定位效果。也有一些方法通過(guò)離散化旋轉(zhuǎn)空間將定位問(wèn)題轉(zhuǎn)化為分類問(wèn)題(Sundermeyer等，2018)進(jìn)行處理，從而提升姿態(tài)估計(jì)結(jié)果的精度。謝非等人(2020)基于端到端模型提出準(zhǔn)確高效的機(jī)器人室內(nèi)單目視覺(jué)定位算法。

基于目標(biāo)關(guān)鍵點(diǎn)的定位方法可以得到更加穩(wěn)定的定位結(jié)果，這類方法通過(guò)局部特征點(diǎn)檢測(cè)提取目標(biāo)關(guān)鍵點(diǎn)，然后基于2D—3D點(diǎn)的對(duì)應(yīng)關(guān)系，利用RANSAC(random sample consensus)算法和PnP(perspective-n-point)算法求解得到物體位姿。一些方法基于隨機(jī)森林預(yù)測(cè)3D坐標(biāo)值(Michel等，2017)，并利用幾何約束改進(jìn)生成2D—3D點(diǎn)的對(duì)應(yīng)關(guān)系。DenseFusion(Wang等，2019)則基于RGB-D數(shù)據(jù)，使用網(wǎng)絡(luò)對(duì)RGB圖像特征和3D點(diǎn)云特征進(jìn)行整合，得到RGB-D數(shù)據(jù)的像素密集特征表示，然后通過(guò)投票獲取物體的6-DoF姿態(tài)。PVNet(pixel-wise voting network)(Peng等，2019)通過(guò)對(duì)關(guān)鍵點(diǎn)進(jìn)行投票得到遮擋情況下的局部特征向量，從而緩解圖像中存在的目標(biāo)遮擋和隔斷問(wèn)題。

單目視覺(jué)定位的一個(gè)缺陷是由于視角限制，位姿估計(jì)精度對(duì)拍攝距離和遮擋比較敏感。針對(duì)這一問(wèn)題，劉昶等人(2012)基于共面二點(diǎn)一線特征進(jìn)行視覺(jué)定位，MLOD(multi-view labelling object detector)(Deng和Czarnecki，2019)用多視圖目標(biāo)檢測(cè)方法進(jìn)行目標(biāo)定位，取得了出色效果。

1.2 主動(dòng)式視覺(jué)定位技術(shù)

主動(dòng)式視覺(jué)定位方法指的是通過(guò)自身攜帶的移動(dòng)攝像頭拍攝環(huán)境圖像來(lái)定位自身的方法，依據(jù)拍攝的當(dāng)前觀測(cè)圖像，查詢數(shù)據(jù)庫(kù)中存在的圖像位置信息，進(jìn)行圖像匹配來(lái)完成定位。經(jīng)典的主動(dòng)式定位方法包含場(chǎng)景建圖和圖像檢索兩個(gè)階段。首先通過(guò)3維重建方法獲取場(chǎng)景的3維點(diǎn)云模型，然后將輸入的查詢圖像與點(diǎn)云模型中的3維點(diǎn)建立對(duì)應(yīng)關(guān)系，使用RANSAC算法解算出查詢圖像的位置。

近年利用圖像檢索算法(Torii等，2015)從場(chǎng)景圖像數(shù)據(jù)庫(kù)中檢索近似圖像，得到較準(zhǔn)確的初始位姿成為主動(dòng)式視覺(jué)定位的主流方法。InLoc(indoor visual localization)(Taira等，2018)利用稠密的圖像匹配和視角一致性來(lái)提升室內(nèi)定位結(jié)果精度，但其算法依賴于全局匹配，計(jì)算代價(jià)較高，系統(tǒng)實(shí)時(shí)性較差。HFNet(hierarchical feature network)(Sarlin等，2019)則提出將圖像檢索和局部特征提取融合到統(tǒng)一的網(wǎng)絡(luò)框架中，以提升姿態(tài)估計(jì)算法的計(jì)算效率。

主動(dòng)式定位方法通常依賴于圖像的強(qiáng)紋理特征，對(duì)紋理結(jié)構(gòu)豐富的區(qū)域，特征點(diǎn)匹配成功率較高，得到的定位結(jié)果也比較準(zhǔn)確，但在一些弱紋理或特征不顯著的區(qū)域上的定位效果往往較差。針對(duì)這一問(wèn)題，可以通過(guò)加入基于軌跡的濾波算法(Sattler等，2017)或利用其他輔助信息來(lái)改善定位結(jié)果。有一些方法(DeTone等，2018)通過(guò)增強(qiáng)特征點(diǎn)檢測(cè)的性能來(lái)提升定位精度。場(chǎng)景無(wú)關(guān)相機(jī)定位方法DSM(dense scene matching)(Tang等，2021)和KF-Net(Kalman filtering network)(Zhou等，2020)主要使用稠密場(chǎng)景匹配，在圖像和場(chǎng)景間構(gòu)造cost volume，通過(guò)CNN網(wǎng)絡(luò)來(lái)估計(jì)稠密坐標(biāo)。

1.3 數(shù)據(jù)融合定位技術(shù)

多傳感器數(shù)據(jù)融合技術(shù)在導(dǎo)航中的應(yīng)用比較廣泛，通常結(jié)合卡爾曼濾波算法、圖優(yōu)化算法來(lái)實(shí)現(xiàn)多傳感器輸入下的高精度定位?？柭鼮V波假定誤差滿足線性高斯分布，但實(shí)際系統(tǒng)并不滿足線性近似假設(shè)。擴(kuò)展卡爾曼濾波(extended Kalman filter，EKF)將非線性模型在狀態(tài)估計(jì)值附近做泰勒級(jí)數(shù)展開(kāi)，采用局部線性化方法獲得狀態(tài)估計(jì)。無(wú)損卡爾曼濾波基于無(wú)損變換(unscented transform，UT)對(duì)后驗(yàn)概率密度進(jìn)行近似估計(jì)，可以有效解決非線性系統(tǒng)引起的濾波發(fā)散問(wèn)題。

VINSMono(Qin等，2018)基于單目視覺(jué)里程計(jì)和擴(kuò)展卡爾曼濾波，提出了一套視覺(jué)信息和視覺(jué)慣性單元(inertial measurement unit，IMU)融合的松耦合框架，具有完善和魯棒的初始化及閉環(huán)檢測(cè)過(guò)程，以適應(yīng)室內(nèi)外環(huán)境。Li等人(2017)基于GPS(global positioning system)和圖像融合，實(shí)現(xiàn)了對(duì)智能車輛的高精度定位。Xue等人(2017)基于相機(jī)、激光雷達(dá)和地理信息系統(tǒng)(geographic information system，GIS)的融合定位方法，實(shí)現(xiàn)了無(wú)人車的障礙物感知和高精度自主定位。楊承凱等人(2009)探討了多傳感器融合中的卡爾曼濾波。

結(jié)合視覺(jué)和激光雷達(dá)、慣性測(cè)量單元等其他物理傳感器的定位方法可以得到比較高的定位精度，但對(duì)于定位裝置的要求也非常高，通常需要裝配價(jià)格昂貴的傳感器設(shè)備。因此，本文希望能夠利用低成本的RGB攝像頭，提出一套基于視覺(jué)的室內(nèi)主被動(dòng)融合定位系統(tǒng)以實(shí)現(xiàn)便捷、精準(zhǔn)的室內(nèi)目標(biāo)定位。

本文提供了一個(gè)在特定應(yīng)用場(chǎng)景(被動(dòng)式定位技術(shù)與主動(dòng)式定位技術(shù)的定位目標(biāo)相同)可實(shí)現(xiàn)的系統(tǒng)性視覺(jué)定位解決思路，結(jié)合了被動(dòng)式定位和主動(dòng)式定位兩種方式的優(yōu)點(diǎn)，當(dāng)遇到視角盲區(qū)和強(qiáng)遮擋情況時(shí)，會(huì)更多地傾向于主動(dòng)式定位的結(jié)果，而當(dāng)遇到圖像弱紋理情況時(shí)，更多地傾向于被動(dòng)式定位的結(jié)果。仿真實(shí)驗(yàn)和實(shí)物實(shí)驗(yàn)結(jié)果都印證了本文的思路和猜想。

2 本文方法

提出的主被動(dòng)融合室內(nèi)視覺(jué)定位系統(tǒng)主要包含3個(gè)模塊，即被動(dòng)式定位模塊、主動(dòng)式定位模塊和主被動(dòng)融合模塊，如圖1所示。其中，被動(dòng)式定位模塊的輸入為室內(nèi)固定視角攝像頭得到的RGB圖像，輸出為該圖像包含目標(biāo)的位姿數(shù)據(jù)；主動(dòng)式定位模塊的輸入為待定位目標(biāo)視角拍攝的RGB圖像，輸出為該目標(biāo)在3維場(chǎng)景中的位姿信息；主被動(dòng)融合模塊負(fù)責(zé)融合被動(dòng)式定位和主動(dòng)式定位的定位結(jié)果，然后輸出最終的位姿數(shù)據(jù)。

圖1 主被動(dòng)融合系統(tǒng)的整體框架

在場(chǎng)景定位中，本文定義全局坐標(biāo)系為目標(biāo)運(yùn)動(dòng)的水平面，這樣直觀且便于測(cè)量誤差，而被動(dòng)式定位和主動(dòng)式定位結(jié)果都會(huì)轉(zhuǎn)換到這一坐標(biāo)系上。

2.1 被動(dòng)式定位模塊

被動(dòng)式定位模塊流程主要包括關(guān)鍵點(diǎn)檢測(cè)與篩選、位姿估計(jì)與優(yōu)化兩個(gè)步驟，如圖2所示。

圖2 被動(dòng)式方法定位流程圖

2.1.1 關(guān)鍵點(diǎn)檢測(cè)與篩選

本文使用卷積神經(jīng)網(wǎng)絡(luò)獲取待定位目標(biāo)的關(guān)鍵點(diǎn)信息。對(duì)固定視角輸入的RGB圖像，首先使用CenterNet(Zhou等，2019)提出的基于中心點(diǎn)的目標(biāo)檢測(cè)方法，構(gòu)建網(wǎng)絡(luò)模型，從圖像中檢測(cè)出目標(biāo)的中心點(diǎn)和尺度信息，得到目標(biāo)的2維邊框，從原始圖像中裁剪出來(lái)用于定位。然后，針對(duì)檢測(cè)到的目標(biāo)區(qū)域，基于預(yù)先定義的目標(biāo)關(guān)鍵點(diǎn)信息，預(yù)測(cè)圖像中對(duì)應(yīng)于目標(biāo)關(guān)鍵點(diǎn)分布的多幅熱力圖，每幅熱力圖都代表一個(gè)關(guān)鍵點(diǎn)位置的估計(jì)結(jié)果。在關(guān)鍵點(diǎn)檢測(cè)過(guò)程中，采用網(wǎng)絡(luò)輸出的關(guān)鍵點(diǎn)熱力圖計(jì)算每個(gè)關(guān)鍵點(diǎn)的像素坐標(biāo)及對(duì)應(yīng)的置信度，并使用關(guān)鍵點(diǎn)置信度預(yù)先過(guò)濾掉一些錯(cuò)誤的關(guān)鍵點(diǎn)，如圖3所示。

圖3 關(guān)鍵點(diǎn)檢測(cè)與篩選示意圖

在關(guān)鍵點(diǎn)檢測(cè)網(wǎng)絡(luò)模型訓(xùn)練中，通過(guò)預(yù)先定義的目標(biāo)關(guān)鍵點(diǎn)3維位置，由其真實(shí)位姿值進(jìn)行投影，得到關(guān)鍵點(diǎn)在圖像上的像素坐標(biāo)。

2.1.2 位姿估計(jì)與優(yōu)化

對(duì)于室內(nèi)場(chǎng)景中的運(yùn)動(dòng)目標(biāo)，例如室內(nèi)服務(wù)機(jī)器人，通常沿水平面進(jìn)行移動(dòng)，高度信息不會(huì)發(fā)生劇烈變化，即待定位目標(biāo)處于高度已知的水平平面內(nèi)。設(shè)物體系z(mì)軸與世界系z(mì)軸平行，則該物體在3維場(chǎng)景中的運(yùn)動(dòng)可以簡(jiǎn)化為3-DoF的位姿估計(jì)問(wèn)題。利用這樣的平面假設(shè)，可以縮小位姿估計(jì)的求解空間，提高其估計(jì)的穩(wěn)定性和準(zhǔn)確性。

假設(shè)攝像頭固定，可以通過(guò)場(chǎng)景中設(shè)置的已知標(biāo)記點(diǎn)得到固定攝像頭在全局坐標(biāo)系中的位置和姿態(tài)。在此基礎(chǔ)上，可以將通過(guò)單目物體定位得到的物體相對(duì)于相機(jī)坐標(biāo)系的位姿轉(zhuǎn)換到全局坐標(biāo)系中。圖像中每個(gè)2維像素點(diǎn)對(duì)應(yīng)的全局坐標(biāo)系3維坐標(biāo)值是固定的，可以根據(jù)目標(biāo)的2維框中心位置來(lái)估計(jì)定位目標(biāo)的位姿初值(x0,y0)。

給定一組關(guān)鍵點(diǎn)的2D—3D對(duì)應(yīng)關(guān)系，通過(guò)最小化目標(biāo)函數(shù)可以得到物體在3維平面上的坐標(biāo)(x0,y0)，目標(biāo)函數(shù)為

(1)

為避免式(1)的優(yōu)化過(guò)程陷入局部最優(yōu)，本文對(duì)(x0,y0)和目標(biāo)對(duì)象的初始朝向角θinit進(jìn)行初始化，以2維候選框的中心坐標(biāo)(x0,y0)代表目標(biāo)的中心坐標(biāo)，通過(guò)相機(jī)成像模型，并假設(shè)Z=0。因此，可以根據(jù)投影方程計(jì)算出目標(biāo)中心在世界坐標(biāo)系位置的初始值(Xinit,Yinit)，具體為

(2)

式中，K∈R3×4為相機(jī)內(nèi)參矩陣，T∈R4×4為世界坐標(biāo)系相對(duì)于相機(jī)坐標(biāo)系的位姿，Zinit=0。

接下來(lái)，計(jì)算目標(biāo)對(duì)象的初始朝向角θinit。

朝向角初始估計(jì)值θinit的取值范圍為[0°,360°)，將解空間以k°為步長(zhǎng)進(jìn)行采樣，可計(jì)算出360/k組重投影誤差，從中選取使重投影誤差(如式(1)所示)最小的θinit作為初始朝向角。

位姿優(yōu)化過(guò)程以位姿參數(shù)為待求解參數(shù)，以初始位姿為初始值，通過(guò)最小化重投影誤差得到目標(biāo)的最終位姿數(shù)據(jù)。

2.2 主動(dòng)式定位模塊

主動(dòng)式方法定位流程包含3維場(chǎng)景恢復(fù)和場(chǎng)景特征點(diǎn)定位兩個(gè)階段，如圖4所示。

圖4 主動(dòng)式方法定位流程圖

主動(dòng)式定位以場(chǎng)景地圖為先驗(yàn)，類似于SLAM(simultaneous localization and mapping)系統(tǒng)中的重定位部分。在定位過(guò)程中，用采集的圖像數(shù)據(jù)(定位目標(biāo)攜帶的移動(dòng)攝像頭拍攝)匹配場(chǎng)景地圖，獲得的是相對(duì)于場(chǎng)景地圖的位姿，而場(chǎng)景地圖是預(yù)先對(duì)齊到全局坐標(biāo)系的，因此主動(dòng)式定位能獲取定位目標(biāo)在全局坐標(biāo)系中的位姿。

2.2.1 3維場(chǎng)景恢復(fù)

3維重建的場(chǎng)景恢復(fù)過(guò)程主要基于運(yùn)動(dòng)恢復(fù)結(jié)構(gòu)(structure from motion，SfM)技術(shù)。首先通過(guò)特征提取和特征匹配算法得到場(chǎng)景不同視角下圖像之間的關(guān)聯(lián)，初始選擇兩個(gè)基線距離較大且匹配較多的圖像對(duì)進(jìn)行重建，通過(guò)初始重建的3維點(diǎn)對(duì)其他圖像幀進(jìn)行定位。對(duì)新增的注冊(cè)幀重新三角化出更多的3維點(diǎn)，然后不斷迭代直到恢復(fù)出所有的圖像位姿和點(diǎn)云表示的3維場(chǎng)景模型。

在完成場(chǎng)景的3維重建后，將場(chǎng)景地圖對(duì)齊到全局坐標(biāo)系。選取一些固定的標(biāo)記點(diǎn)采集對(duì)應(yīng)位置的圖像，通過(guò)直接測(cè)量的方式獲取標(biāo)記點(diǎn)對(duì)應(yīng)的全局坐標(biāo)系3維坐標(biāo)，然后將對(duì)應(yīng)圖像通過(guò)特征提取、與場(chǎng)景地圖數(shù)據(jù)的特征匹配以及PnP算法得到在場(chǎng)景地圖坐標(biāo)系中的3維坐標(biāo)，使用Umeyama算法(Umeyama，1991)計(jì)算得到兩個(gè)坐標(biāo)系之間的變換矩陣。

2.2.2 場(chǎng)景特征點(diǎn)定位

對(duì)輸入的待定位圖像，首先對(duì)其進(jìn)行局部特征提取，并與已恢復(fù)場(chǎng)景地圖中的局部特征進(jìn)行匹配，得到待定位圖像中2維局部特征位置與場(chǎng)景模型中3維結(jié)構(gòu)之間的幾何映射關(guān)系。然后在圖像2D特征點(diǎn)和場(chǎng)景中3D特征點(diǎn)匹配基礎(chǔ)上，通過(guò)求解PnP問(wèn)題，得到該圖像對(duì)應(yīng)目標(biāo)對(duì)象的6-DoF位姿信息。

2.3 主被動(dòng)融合定位模塊

(3)

系統(tǒng)的狀態(tài)方程和觀測(cè)方程可以表示為

(4)

(5)

式中，Xk為第k時(shí)刻目標(biāo)的狀態(tài)變量，包括位置、速度和加速度等，Uk為系統(tǒng)噪聲，Qk為系統(tǒng)噪聲分布的協(xié)方差，Wk為觀測(cè)噪聲，Rk為觀測(cè)噪聲分布的協(xié)方差，觀測(cè)向量Zk為第k時(shí)刻主/被動(dòng)式定位方法的預(yù)測(cè)定位結(jié)果(xk,yk)。

使用無(wú)損卡爾曼濾波算法將被動(dòng)式預(yù)測(cè)定位結(jié)果與主動(dòng)式預(yù)測(cè)定位結(jié)果進(jìn)行非線性融合，根據(jù)式(3)和式(4)，實(shí)際系統(tǒng)的狀態(tài)方程可以表示為

Xk+1=f(Xk,Uk)=f(Xk)+Uk

(6)

根據(jù)式(5)，觀測(cè)方程可以表示為

Zk=h(Xk,Wk)=HXk+Wk

(7)

式中，初始噪聲分布情況根據(jù)訓(xùn)練數(shù)據(jù)集中的樣本采樣估計(jì)、統(tǒng)計(jì)預(yù)測(cè)值和真實(shí)值的誤差分布情況得到，包括被動(dòng)式定位系統(tǒng)噪聲Qp、觀測(cè)噪聲Rp和主動(dòng)式定位系統(tǒng)噪聲Qa、觀測(cè)噪聲Ra，即

主被動(dòng)定位的結(jié)果按時(shí)間整理成觀測(cè)值隊(duì)列，按順序加入到融合定位的濾波系統(tǒng)中，分別按照各自的噪聲分布更新協(xié)方差矩陣和狀態(tài)向量。

具體融合定位實(shí)現(xiàn)過(guò)程如下：

1)求初始狀態(tài)均值和方差矩陣

(8)

2)通過(guò)UT變換，獲取狀態(tài)量采樣點(diǎn)和對(duì)應(yīng)權(quán)值，各采樣點(diǎn)可以表示為

(9)

各采樣點(diǎn)對(duì)應(yīng)的權(quán)值可以表示為

(10)

(11)

式中，γi,k為采樣點(diǎn)集通過(guò)觀測(cè)函數(shù)得到的觀測(cè)點(diǎn)集。

4)計(jì)算卡爾曼增益Kk+1，更新協(xié)方差PX,k+1和狀態(tài)向量Xk+1，得到狀態(tài)轉(zhuǎn)移方程

(12)

將式中狀態(tài)向量Xk+1包含的坐標(biāo)值作為k+1時(shí)刻主被動(dòng)融合定位的位置坐標(biāo)。至此，完成了基于無(wú)損卡爾曼濾波的主被動(dòng)融合定位方法。

數(shù)據(jù)融合的過(guò)程中，觀測(cè)值來(lái)自于被動(dòng)式定位和主動(dòng)式定位的定位結(jié)果，共用同一個(gè)狀態(tài)向量進(jìn)行更新，但是被動(dòng)式定位和主動(dòng)式定位有各自的協(xié)方差矩陣。狀態(tài)向量更新時(shí)，先判斷觀測(cè)數(shù)據(jù)來(lái)源是被動(dòng)式定位還是主動(dòng)式定位，選擇協(xié)方差矩陣，然后對(duì)主被動(dòng)定位方法得到的初步定位結(jié)果采用閾值篩選的方法去除偏差較大的觀測(cè)值，得到有效觀測(cè)。

對(duì)于被動(dòng)式定位結(jié)果，基于重投影誤差和關(guān)鍵點(diǎn)置信度進(jìn)行評(píng)價(jià)和篩選，誤差函數(shù)為

(13)

對(duì)于主動(dòng)式定位結(jié)果，基于檢測(cè)到的特征點(diǎn)數(shù)量和匹配點(diǎn)數(shù)量比例進(jìn)行評(píng)價(jià)和篩選，誤差函數(shù)為

(14)

式中，nsift為圖像中檢測(cè)到的SIFT(scale-invariant feature transform)特征點(diǎn)數(shù)量，ninliers為匹配到場(chǎng)景3維點(diǎn)的圖像特征點(diǎn)數(shù)量。

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

3.1 實(shí)驗(yàn)平臺(tái)與數(shù)據(jù)

為驗(yàn)證提出的主被動(dòng)融合方法的有效性，本文分別在仿真平臺(tái)和真實(shí)場(chǎng)景進(jìn)行實(shí)驗(yàn)。仿真數(shù)據(jù)和真實(shí)環(huán)境獲取的數(shù)據(jù)示例如圖5所示。

圖5 仿真數(shù)據(jù)和真實(shí)數(shù)據(jù)示例

本文在室內(nèi)實(shí)際場(chǎng)景中利用Turtlebot3和Realsense采集圖像數(shù)據(jù)進(jìn)行實(shí)物實(shí)驗(yàn)，共生成10組數(shù)據(jù)，其中5組樣本作為訓(xùn)練數(shù)據(jù)，用于被動(dòng)式定位的模型訓(xùn)練以及主動(dòng)式定位的場(chǎng)景3維重建，其余5組用于模型測(cè)試。其中，物體位姿定位的真實(shí)值(ground truth)根據(jù)在地面標(biāo)記的記號(hào)點(diǎn)利用直尺直接測(cè)量得到。

3.2 評(píng)價(jià)標(biāo)準(zhǔn)

在算法評(píng)估方面，本文使用平均定位精度和定位準(zhǔn)確率作為量化指標(biāo)。

平均定位精度(average accuracy)指測(cè)試集中每個(gè)樣本的定位結(jié)果到真實(shí)值(ground truth)的歐氏距離均值。具體為

(15)

定位準(zhǔn)確率指能達(dá)到指定定位精度的正確樣本占所有樣本的比例。實(shí)驗(yàn)設(shè)置了小于15 cm、小于10 cm和小于5 cm三段范圍的準(zhǔn)確率指標(biāo)來(lái)定量評(píng)估不同算法的定位精度。

3.3 網(wǎng)絡(luò)參數(shù)設(shè)置

被動(dòng)式定位方法中目標(biāo)檢測(cè)的網(wǎng)絡(luò)框架采用Resnet-18，關(guān)鍵點(diǎn)檢測(cè)網(wǎng)絡(luò)框架采用Dla-34。模型訓(xùn)練輸入的圖像分辨率為512 × 512像素，批大小(batch size)設(shè)置為8，學(xué)習(xí)率設(shè)置為0.000 1。

3.4 仿真實(shí)驗(yàn)結(jié)果

在仿真實(shí)驗(yàn)平臺(tái)上的可視化結(jié)果如圖6所示，其中藍(lán)色軌跡為iGibson記錄真實(shí)值，紅色軌跡為不同定位方法輸出預(yù)測(cè)值，黃色區(qū)域?yàn)閳?chǎng)景中固定視角的攝像頭。

圖6 仿真環(huán)境被動(dòng)式/主動(dòng)式/融合定位方法可視化結(jié)果

從定位軌跡結(jié)果可以看到，被動(dòng)式定位方法的特點(diǎn)是在固定攝像頭靠近中心的視野區(qū)域定位精度較高，而對(duì)于偏離視野中心距離的區(qū)域以及存在較嚴(yán)重遮擋的區(qū)域，其定位精度會(huì)嚴(yán)重下降。主動(dòng)式定位方法對(duì)于紋理貧乏、特征不明晰的圖像定位效果較差，其軌跡吻合度相對(duì)于被動(dòng)式定位方法精度要差，但不容易產(chǎn)生較大估計(jì)誤差，具備比較好的穩(wěn)定性。經(jīng)過(guò)UKF融合后的結(jié)果能準(zhǔn)確恢復(fù)機(jī)器人的運(yùn)動(dòng)軌跡，對(duì)于主/被動(dòng)存在定位誤差部分進(jìn)行了有效修正，且其軌跡的平滑程度和魯棒性都得到了明顯提升。

表1給出了不同算法在仿真數(shù)據(jù)集上的定位精度指標(biāo)。從定位精度評(píng)估結(jié)果可以看到，基于UKF的主被動(dòng)融合定位方法在平均定位精度和準(zhǔn)確率上有明顯提升。與SSD-6D的方法相比，被動(dòng)式定位方法的平均定位精度和準(zhǔn)確率都要高出一些，主要優(yōu)勢(shì)來(lái)自平面場(chǎng)景的3自由度優(yōu)化。與基于EKF的融合定位方法相比，平均定位精度基本一致，但10 cm/5 cm的準(zhǔn)確率有明顯提升，特別是對(duì)遮擋比較嚴(yán)重的區(qū)域有更好的定位效果。

表1 仿真數(shù)據(jù)集定位結(jié)果

仿真實(shí)驗(yàn)結(jié)果表明，本文提出的主被動(dòng)融合定位方法可以在仿真場(chǎng)景中實(shí)現(xiàn)cm級(jí)的精準(zhǔn)定位，平均定位精度為2.01 cm，誤差在10 cm內(nèi)的準(zhǔn)確率可以達(dá)到99.0%以上。

3.5 真實(shí)場(chǎng)景實(shí)驗(yàn)結(jié)果

為進(jìn)一步驗(yàn)證本文方法的可用性，在真實(shí)室內(nèi)場(chǎng)景的實(shí)物數(shù)據(jù)集上對(duì)不同算法的定位精度進(jìn)行測(cè)試，結(jié)果如表2所示。從定位結(jié)果可以看出，基于UKF的主被動(dòng)融合定位方法在平均定位精度和準(zhǔn)確率上均取得了最優(yōu)結(jié)果。圖8給出了實(shí)際場(chǎng)景的定位準(zhǔn)確率曲線，可以看到，在定位準(zhǔn)確率上，基于UKF的方法相比主被動(dòng)定位方法和EKF融合方法都有一定提升。

表2 實(shí)物數(shù)據(jù)集定位結(jié)果

真實(shí)場(chǎng)景的實(shí)驗(yàn)結(jié)果表明，主被動(dòng)融合視覺(jué)定位系統(tǒng)能有效降低固定視角下被動(dòng)式定位算法由于視角局限性、物體遮擋等外界干擾，同時(shí)可以有效克服單幀的定位算法穩(wěn)定性不足、隨機(jī)誤差大的缺陷。本文提出的主被動(dòng)融合定位方法同樣能夠在真實(shí)室內(nèi)環(huán)境中實(shí)現(xiàn)cm級(jí)的目標(biāo)精準(zhǔn)定位，誤差在10 cm內(nèi)和15 cm內(nèi)的準(zhǔn)確率分別為93.1%和97.1%。

4 結(jié) 論

本文針對(duì)室內(nèi)移動(dòng)機(jī)器人作業(yè)等特定應(yīng)用場(chǎng)景，提出了一個(gè)基于無(wú)損卡爾曼濾波的主被動(dòng)融合室內(nèi)視覺(jué)定位系統(tǒng)框架，與現(xiàn)有的視覺(jué)定位算法相比，能夠以較低設(shè)備成本實(shí)現(xiàn)室內(nèi)場(chǎng)景中高精度的目標(biāo)定位結(jié)果，并在遮擋、目標(biāo)丟失等復(fù)雜環(huán)境因素干擾下展示出魯棒的定位性能，實(shí)現(xiàn)室內(nèi)場(chǎng)景的純視覺(jué)cm級(jí)精準(zhǔn)定位。在仿真和實(shí)物環(huán)境下都進(jìn)行了測(cè)試和驗(yàn)證，實(shí)驗(yàn)結(jié)果表明，定位系統(tǒng)具備很高的定位準(zhǔn)確率和魯棒性，能夠用于多種場(chǎng)景。

但是，本文的研究工作還存在不少可以改進(jìn)的地方。一方面，單目視覺(jué)定位方法存在對(duì)物體先驗(yàn)?zāi)Ｐ偷囊蕾囆?，在無(wú)先驗(yàn)條件下進(jìn)行定位是一個(gè)難題，而基于機(jī)器學(xué)習(xí)的定位方法需要大量的標(biāo)注數(shù)據(jù)，如何對(duì)原始數(shù)據(jù)進(jìn)行快速智能標(biāo)注也是一個(gè)需要解決的問(wèn)題；另一方面，從數(shù)據(jù)融合算法的角度看，本文方法主要是在數(shù)據(jù)級(jí)融合和特征級(jí)融合上展開(kāi)，在更高一層的目標(biāo)級(jí)融合研究上還存在一些欠缺。

在未來(lái)工作中，將繼續(xù)深入研究如何與視覺(jué)里程計(jì)等SLAM方法進(jìn)行更深層次的數(shù)據(jù)融合，實(shí)現(xiàn)不同場(chǎng)景的模型遷移，將數(shù)據(jù)融合的系統(tǒng)定位算法應(yīng)用于更加廣闊的場(chǎng)景。