室內(nèi)場(chǎng)景下基于曼哈頓約束的多重特征視覺SLAM 方法

孫新柱，龔光強(qiáng)，陳孟元，程 浩，郭行榮

（1.安徽工程大學(xué) 電氣工程學(xué)院，蕪湖 241000；2.高端裝備先進(jìn)感知與智能控制教育部重點(diǎn)實(shí)驗(yàn)室，蕪湖 241000）

同步定位與地圖構(gòu)建（Simultaneous Localization and Mapping,SLAM）是估計(jì)智能機(jī)器人旋轉(zhuǎn)和平移六自由度運(yùn)動(dòng)、同時(shí)構(gòu)建周圍未知環(huán)境地圖的技術(shù)。視覺SLAM（Visual SLAM,V-SLAM）是移動(dòng)機(jī)器人領(lǐng)域研究的熱點(diǎn)和難點(diǎn)。V-SLAM 算法被提出以來，紋理稀疏環(huán)境下的同步定位與建圖一直是研究的熱點(diǎn)[1]。為解決V-SLAM因點(diǎn)特征不足導(dǎo)致定位精度低的問題，除了融合其它傳感器外，場(chǎng)景中的其它特征逐漸被研究者關(guān)注，例如線特征、平面特征和語義特征?，F(xiàn)代建筑的室內(nèi)環(huán)境普遍表現(xiàn)出特殊的規(guī)律性，具有統(tǒng)一的場(chǎng)景結(jié)構(gòu)模式。室內(nèi)的結(jié)構(gòu)化場(chǎng)景表現(xiàn)出具有較多平面、紋理信息不豐富或區(qū)分度不明顯的特點(diǎn)。在VSLAM 技術(shù)中，面是比點(diǎn)和線擁有更多約束的幾何特征，室內(nèi)的地板、墻面和天花板相互垂直或平行，導(dǎo)致平面特征的法線矢量集中在三維空間中的三個(gè)主要正交方向，具有這種結(jié)構(gòu)特點(diǎn)的場(chǎng)景可以被抽象為曼哈頓世界（Manhattan Word,MW）[2]，沿三個(gè)主要正交方向定義的坐標(biāo)系稱為曼哈頓坐標(biāo)系（Manhattan Frame,MF）。對(duì)具有MW 結(jié)構(gòu)特點(diǎn)的環(huán)境，可以將其形式化為MW 假設(shè)模型，目前該方法已應(yīng)用于多種領(lǐng)域，例如相機(jī)校準(zhǔn)、三維場(chǎng)景重建和SLAM 技術(shù)[3]。

目前，室內(nèi)環(huán)境下的定位與導(dǎo)航是V-SLAM 研究的重點(diǎn)，早期基于優(yōu)化的V-SLAM大多僅利用點(diǎn)特征，早在2015 年，ORB（Oriented FAST and Rotated BRIEF）點(diǎn)特征[4]就已經(jīng)被用于單目定位算法。后續(xù)ORBSLAM2[5]增加了對(duì)雙目相機(jī)和RGB-D 相機(jī)的支持，可以適用于更多場(chǎng)景；ORB-SLAM3[6]是將相機(jī)與慣性測(cè)量裝置（Inertial measurement unit,IMU）融合，使精度、魯棒性和實(shí)時(shí)性都有了顯著提高，成為了SLAM領(lǐng)域中廣泛使用的算法之一。但在紋理稀疏的室內(nèi)結(jié)構(gòu)化場(chǎng)景下，僅依賴點(diǎn)特征的SLAM 方法易造成跟蹤丟失；而無需借助其他傳感器，通過加入線特征進(jìn)行跟蹤，這樣可以彌補(bǔ)點(diǎn)特征的退化問題[7]。2019 年，Gomez-Ojeda 等人開源了雙目PL-SLAM[8]，采用快速直線檢測(cè)算法[9]（Line Segment Detector,LSD），融合了點(diǎn)和線特征以提高地圖的準(zhǔn)確性和魯棒性，并且使用點(diǎn)線特征的描述子構(gòu)建詞袋用于閉環(huán)檢測(cè)，建立了包含點(diǎn)和線的地圖，提高了V-SLAM 在點(diǎn)特征稀疏或圖像特征分布不均場(chǎng)景的定位精度。但是該系統(tǒng)簡(jiǎn)單地將點(diǎn)和線特征檢測(cè)結(jié)果相加，會(huì)將線特征匹配的高誤差引入系統(tǒng)中，從而降低數(shù)據(jù)關(guān)聯(lián)的準(zhǔn)確性。

近些年，平面特征被廣泛應(yīng)用于SLAM 算法。王雅儀等人[10]通過激光雷達(dá)傳感器直接獲得點(diǎn)云信息，對(duì)原始點(diǎn)云進(jìn)行聚類分割，再從點(diǎn)云中提取面特征。為了將MW 中涉及的結(jié)構(gòu)特征融合到SLAM 中，孫沁璇等人[11]針對(duì)利用平面特征位姿求解的退化問題，提出平面與平面、平面與直線的多特征關(guān)聯(lián)方法，并且使用自適應(yīng)加權(quán)方法保證了平面在位姿計(jì)算的主導(dǎo)作用；2018 年，Kim 等人提出了一種結(jié)合線和平面的視覺里程計(jì)LPVO[12]，將線和平面結(jié)合起來，利用均值偏移算法跟蹤MF 估計(jì)旋轉(zhuǎn)運(yùn)動(dòng)，這使SLAM 系統(tǒng)的位姿解耦估計(jì)成為可能。2019 年，Yang S 等人[13]將面特征加入統(tǒng)一的SLAM 框架進(jìn)行優(yōu)化，聯(lián)合推斷三維對(duì)象和布局平面。2020 年，Li 等人提出了一種用于室內(nèi)場(chǎng)景的Structure-SLAM[14]，利用卷積神經(jīng)網(wǎng)絡(luò)從RGB 圖像中估計(jì)平面法向量，并用當(dāng)前幀的平面特征和線特征計(jì)算出MW 矩陣，從而估算出當(dāng)前姿態(tài)；Structure-SLAM 的優(yōu)點(diǎn)是能夠利用平面特征提高場(chǎng)景重建的精度和魯棒性，同時(shí)能夠?qū)崿F(xiàn)實(shí)時(shí)性能和輕量級(jí)計(jì)算。2021 年，Li 等人的團(tuán)隊(duì)開源了基于RGB-D的Planar-SLAM[15]，提出了一種基于多重特征的解耦細(xì)化方法，通過聯(lián)合平面上的點(diǎn)、線特征和基于曼哈頓約束的位姿優(yōu)化過程來提升定位準(zhǔn)確性。除了通過解耦策略估計(jì)旋轉(zhuǎn)和平移之外，Planar-SLAM 也采用平行或垂直的結(jié)構(gòu)規(guī)律約束姿態(tài)。但解耦方案需要對(duì)每一幀進(jìn)行MW 假設(shè)，因此在不嚴(yán)格符合MW 假設(shè)的室內(nèi)環(huán)境中是非常有限的，導(dǎo)致性能下降甚至跟蹤失敗。為了克服這個(gè)問題，2021 年，Yunus 等人提出基于 RGB-D 融合點(diǎn)、線和面特征的 Manhattan-SLAM[16]，該算法從線和面特征提取MW 矩陣，建立基于MW 假設(shè)的室內(nèi)場(chǎng)景模型，并采用MF 跟蹤和點(diǎn)線特征跟蹤兩種策略，取得了較高的精度和魯棒性；但所用的聚集層次聚類點(diǎn)云分割算法[17]受深度信息噪聲影響較大，會(huì)導(dǎo)致MF 提取出現(xiàn)誤差。

綜上所述，在紋理稀疏場(chǎng)景下，平面特征較點(diǎn)、線特征能表現(xiàn)出更強(qiáng)的穩(wěn)定性。將面特征融入VSLAM，不僅能改善特征退化，還可以通過平面關(guān)系確定MF，利用MF 估計(jì)相機(jī)旋轉(zhuǎn)以減少累積誤差，提高機(jī)器人在室內(nèi)紋理稀疏場(chǎng)景中的定位精度。

1 系統(tǒng)總體框架

針對(duì)傳統(tǒng)RGB-D SLAM 在室內(nèi)紋理稀疏場(chǎng)景中易出現(xiàn)定位精度差的問題，提出一種基于曼哈頓約束的點(diǎn)線面多特征V-SLAM 方法。在特征處理階段，針對(duì)傳統(tǒng)面特征提取算法存在平面漏檢測(cè)和錯(cuò)分割的問題，提出特征增強(qiáng)模塊對(duì)PlaneRecNet 網(wǎng)絡(luò)[18]進(jìn)行改進(jìn)，實(shí)現(xiàn)更準(zhǔn)確的圖像面特征和參數(shù)提取。在位姿跟蹤階段，采用方向引導(dǎo)的方法篩選正交主平面，提高M(jìn)F 估計(jì)的準(zhǔn)確性；針對(duì)單純利用MW 假設(shè)進(jìn)行相機(jī)位姿估計(jì)的應(yīng)用場(chǎng)景受限問題，檢查平面間的正交關(guān)系，分別采用曼哈頓跟蹤和點(diǎn)線特征跟蹤兩種策略。對(duì)于MW 場(chǎng)景，利用MW 假設(shè)結(jié)合球面均值偏移算法獲得旋轉(zhuǎn)矩陣，降低SLAM 過程因旋轉(zhuǎn)累積誤差帶來的軌跡漂移；再固定旋轉(zhuǎn)，構(gòu)建點(diǎn)、線重投影誤差函數(shù)求解三自由度的平移運(yùn)動(dòng)，系統(tǒng)結(jié)構(gòu)框架如圖1所示。

圖1 系統(tǒng)結(jié)構(gòu)框圖Fig.1 System architecture block diagram

2 平面處理

傳統(tǒng)平面特征提取采用圖像中的幾何形狀，將特征聚類分組以檢測(cè)平面，或從3D 點(diǎn)云中提取平面[19]。但是在鏡像表面和非平面中很難檢測(cè)其幾何性質(zhì)，在SLAM 中實(shí)際效果不佳。鑒于卷積神經(jīng)網(wǎng)絡(luò)在目標(biāo)檢測(cè)和語義分割領(lǐng)域的成功應(yīng)用，本文采用改進(jìn)的PlaneRecNet 網(wǎng)絡(luò)來提高結(jié)構(gòu)化場(chǎng)景下平面特征提取的精度。PlaneRecNet 是一種具有單階段實(shí)例分割分支的多任務(wù)卷積神經(jīng)網(wǎng)絡(luò)，具有平面優(yōu)先注意的深度解碼器和用于分段平面區(qū)域估計(jì)與重建的共享主干，通過引入深度梯度分割損失函數(shù)和平面表面法線損失函數(shù)來加強(qiáng)分段平面分割。特征金字塔網(wǎng)絡(luò)[20]（Feature Pyramid Network,FPN）作為大多數(shù)物體檢測(cè)器的基本組成部分，可以有效提取多尺度特征。FPN 在進(jìn)行特征融合之前，不同層次的特征會(huì)獨(dú)立進(jìn)行1×1 卷積，將特征通道維數(shù)減少至最低常數(shù)（256），而骨干網(wǎng)絡(luò)在每個(gè)時(shí)間段提取的原始特征往往包含比這個(gè)常數(shù)更豐富的信息（如512、1024、2048）。這種橫向連接直接將通道維數(shù)降低了幾倍，損失了部分特征圖的語義信息。由于高層特征包含更多通道，因此通道減少帶來的信息損失在高層特征中最為嚴(yán)重。針對(duì)PlaneRecNet 在紋理稀疏且相似的環(huán)境中因信息丟失造成的平面漏檢測(cè)和錯(cuò)分割問題，提出了特征增強(qiáng)模塊（Feature Enhancement Module,FEM），如圖2 所示。

圖2 特征增強(qiáng)模塊Fig.2 Feature enhancement module

FEM 利用具有更大感受野的C5層上下文信息彌補(bǔ)特征金字塔頂層橫向連接的信息丟失，有助于在紋理稀疏場(chǎng)景提取更豐富的特征。FEM 的關(guān)鍵思想是融合大量局部和全局上下文信息以生成更具辨別力的特征。原始圖像在ResNet101 骨干網(wǎng)絡(luò)中通過一系列下采樣得到的特征圖有{C2,C3,C4,C5}。首先，F(xiàn)EM 對(duì)來自ResNet101 最高層C5的信息行執(zhí)行三個(gè)不同尺度的池化與卷積操作：1）在C5上應(yīng)用3×3 卷積來提取局部信息；2）輸入特征通過3×3 最大池化下采樣到w×h，并經(jīng)過1×1 卷積層以擴(kuò)展通道維度；3）對(duì)C5進(jìn)行全局平均池化以獲取通道關(guān)聯(lián)信息。通過擴(kuò)展三個(gè)尺度的特征表示，有效擴(kuò)大了C5的感受野，并細(xì)化了表示能力。然后，將三個(gè)具有不同比例的上下文特征Fi(1≤i≤ 3)通過雙線性插值上采樣到C5的尺度，以進(jìn)行后續(xù)concat 融合得到。最后，經(jīng)過卷積并進(jìn)行逐元素求和得到Mi，Mi與融合進(jìn)行張量對(duì)齊得到，再將得到的添加到FPN。

其中，σ為sigmoid 函數(shù)；w1和w2是卷積層參數(shù)。

3 基于曼哈頓約束的位姿解耦估計(jì)

ORB 算法和LSD 算法是計(jì)算機(jī)視覺領(lǐng)域中常用的特征提取算法，分別用于對(duì)場(chǎng)景中的點(diǎn)特征和線特征進(jìn)行提取。使用LBD（Line Band Descriptor）描述符將線特征參數(shù)化表示。在提取圖像幀的二維特征后，點(diǎn)特征二維坐標(biāo)用p表示，得到歸一化線函數(shù)l記為：

符合MW 假設(shè)要求場(chǎng)景中同時(shí)具有三個(gè)相互正交的平面，但實(shí)際上兩個(gè)正交的平面也可滿足MW 假設(shè)。如果平面角差滿足正交閾值（至少86°），則認(rèn)為由兩個(gè)平面確定的主導(dǎo)方向是正交的，第三個(gè)方向可以通過二者的向量積得到，任何三個(gè)相互正交的主導(dǎo)方向都可構(gòu)成MF。本文算法使用兩種不同的策略估計(jì)相機(jī)位姿：對(duì)于非MW 場(chǎng)景，直接跟蹤點(diǎn)線特征估計(jì)六自由度的相機(jī)位姿；對(duì)于MW 場(chǎng)景，首先比較跟蹤平面結(jié)構(gòu)的正交性進(jìn)行三自由度旋轉(zhuǎn)估計(jì)，然后構(gòu)建點(diǎn)、線重投影誤差函數(shù)求解三自由度平移運(yùn)動(dòng)。

3.1 方向引導(dǎo)的MF 估計(jì)

曼哈頓場(chǎng)景平面法向量主要集中在三個(gè)正交主方向上，但每幀被探測(cè)的平面一般位置和面積都不一樣，除了正交優(yōu)勢(shì)面外，還有很多小平面和非正交面區(qū)域。這些小平面和非正交面區(qū)域也可能存在平行或垂直關(guān)系，這會(huì)導(dǎo)致MF 估計(jì)的結(jié)果出現(xiàn)誤差。因此，如何在眾多具有一致法線方向的平面中選取合適的平面，成為確定最優(yōu)MF 的關(guān)鍵問題。在三維歐式空間R3中，平面法向量n在單位高斯球面上具有一定的規(guī)律性，高斯球是以相機(jī)光學(xué)中心為球心的虛擬單位球體，是在相機(jī)固有參數(shù)已知的情況下表示直線、平面法向量等幾何元素的一種簡(jiǎn)便方法，如圖3 所示。

圖3 平面法向量在高斯球中的分布Fig.3 Distribution of plane normal vectors in a Gaussian sphere

圖4 切平面上的均值偏移Fig.4 Mean shift in the tangent plane

圖5 各坐標(biāo)之間的轉(zhuǎn)換Fig.5 Conversion between coordinates

對(duì)于滿足MW 假設(shè)的室內(nèi)場(chǎng)景，當(dāng)前幀的平面集合可以表示為H={h1,h2…h(huán)i}，其中hi表示第i個(gè)平面（i2≥ ）。任意兩個(gè)平面hj和hk之間的關(guān)系可以用它們之間的夾角θ表示，θ越接近90°，hj和hk越接近垂直。曼哈頓場(chǎng)景的大部分平面都相互垂直或平行，H中各平面的面積和方向關(guān)系可以表示為：

其中，S(hj)為H中提取平面的面積；F⊥(hk)可以表示為除hk之外H中其它平面在垂直于hk的平面上的投影面積之和。H中與hk近似垂直的平面數(shù)量越多，則面積越大，F(xiàn)⊥(hk)越大，反之F⊥(hk)值越小。本文算法結(jié)合場(chǎng)景中提取平面之間的方向關(guān)系和面積選取確定MF 的優(yōu)勢(shì)平面，采用具有最大F⊥(hk)值的平面來確定主導(dǎo)方向，如式(4)所示。計(jì)算H中所有平面的F⊥(hk)值，取F⊥(hk)最大的兩個(gè)相互正交的平面作為主導(dǎo)平面，求二者向量積得到第三個(gè)主導(dǎo)方向。

3.2 跟蹤曼哈頓幀

跟蹤曼哈頓幀的核心是比較和跟蹤平面結(jié)構(gòu)的正交性。使用{Rcw,Tcw}表示六自由度的相機(jī)位姿，其中Rcw?SO(3)和Tcw?R3分別表示從世界坐標(biāo)系到相機(jī)坐標(biāo)系的旋轉(zhuǎn)和平移。MF 相對(duì)于相機(jī)坐標(biāo)系的旋轉(zhuǎn)矩陣表示為Rcm=[rxryrz]?SO(3)，其中rx、ry、rz分別表示MF 的x、y、z軸對(duì)應(yīng)的方向向量。將當(dāng)前相機(jī)坐標(biāo)系的平面法向量n變換為在MF 的表示：

相機(jī)在曼哈頓場(chǎng)景中運(yùn)動(dòng)，相當(dāng)于交替地將當(dāng)前幀檢測(cè)到的平面法向量投影到上一幀MF 的x、y和z軸對(duì)應(yīng)的高斯球切平面上，用表示與MF 的x軸平行的平面相關(guān)法向量。

然后，使用具有高斯內(nèi)核的均值偏移算法計(jì)算法向量在切平面上的均值偏移。均值偏移是一種通過迭代運(yùn)算找到目標(biāo)位置實(shí)現(xiàn)目標(biāo)區(qū)域跟蹤的統(tǒng)計(jì)迭代算法，如式(7)所示。

其中，c是內(nèi)核的寬度，根據(jù)法向量在切平面分散的稀疏程度來確定。

最后，用黎曼指數(shù)映射進(jìn)行歸一化，將均值偏移從切平面變換回高斯球面上，得到更新后的法向量sx，并將sx相對(duì)于相機(jī)坐標(biāo)系進(jìn)行轉(zhuǎn)換，獲得相機(jī)坐標(biāo)系中下一幀的方向向量，如式(8)所示。

其中，[?]代表向量的歸一化。

得到旋轉(zhuǎn)矩陣之后就可以固定旋轉(zhuǎn)，構(gòu)建點(diǎn)、線重投影誤差函數(shù)求解三自由度的平移運(yùn)動(dòng)，滿足SLAM 系統(tǒng)在低紋理場(chǎng)景中穩(wěn)定運(yùn)行所需特征。平移運(yùn)動(dòng)估計(jì)的核心是將上一幀的三維點(diǎn)線重新投影到當(dāng)前幀，定義相機(jī)測(cè)量的重投影誤差函數(shù)如式(10)所示。

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

本文實(shí)驗(yàn)平臺(tái)配置為：CPU 為Inter Core i5-12400F，6 核12 線程，主頻3.2 GHz，內(nèi)存16 GB；GPU 為RTX3060 顯卡，顯存12 GB；算法運(yùn)行軟件平臺(tái)為Ubuntu20.04。為了驗(yàn)證本文算法有效性，面特征提取網(wǎng)絡(luò)和定位精度測(cè)試在合成數(shù)據(jù)集ICL-NUIM 與真實(shí)世界數(shù)據(jù)集TUM RGB-D 上進(jìn)行。ICL-NUIM 是合成數(shù)據(jù)集，可模擬在人工環(huán)境（客廳和辦公室）捕獲的RGB 和深度圖像數(shù)據(jù)中的真實(shí)傳感器噪聲，包含缺少紋理的墻壁和天花板。TUM RGB-D 數(shù)據(jù)集是由慕尼黑工業(yè)大學(xué)制作的VO/V-SLAM 評(píng)估數(shù)據(jù)集，在室內(nèi)環(huán)境中通過微軟Kinect RGB-D 相機(jī)采集的圖像組成，用運(yùn)動(dòng)捕捉系統(tǒng)收集相機(jī)的真實(shí)運(yùn)動(dòng)軌跡。

4.1 平面分割實(shí)驗(yàn)分析

在室內(nèi)結(jié)構(gòu)化場(chǎng)景中驗(yàn)證本文所提的改進(jìn)算法平面分割效果，為滿足本實(shí)驗(yàn)所需條件，選取ICL-NUIM和TUM 數(shù)據(jù)集的室內(nèi)場(chǎng)景序列進(jìn)行測(cè)試。PlaneRCNN、PlaneRecNet 網(wǎng)絡(luò)改進(jìn)前和改進(jìn)后的分割效果對(duì)比如圖6 所示。PlaneRCNN 算法基于兩階段分割方法和ROI 內(nèi)的局部掩模分割，對(duì)小平面表現(xiàn)出更高的檢測(cè)能力，但也會(huì)檢測(cè)出很多假性小平面，如圖6(b)所示。圖6(f)中的門框被錯(cuò)誤識(shí)別為平面（箭頭所指），并且在海報(bào)上分割出了小平面。

圖6 平面分割對(duì)比圖Fig.6 Comparison of planar divisions

大尺寸平面的高質(zhì)量分割可以顯著提高室內(nèi)V-SLAM 定位精度，PlaneRecNet 網(wǎng)絡(luò)主要針對(duì)大平面區(qū)域進(jìn)行分割提取，因此本文在PlaneRecNet 基礎(chǔ)上進(jìn)行改進(jìn)，但紋理相似和目標(biāo)區(qū)域的語義信息丟失會(huì)導(dǎo)致平面檢測(cè)精度降低。PlaneRecNet 在ICL-NUIM 數(shù)據(jù)集office1 序列上存在嚴(yán)重錯(cuò)分割的情況，辦公桌正面和部分地面被識(shí)別為同一平面，而地面沒有被識(shí)別，如圖 6(c)所示。PlaneRecNet 在 TUM 數(shù)據(jù)集fre3/s_t_near 序列場(chǎng)景上存在錯(cuò)分割的情況，墻面海報(bào)本身是同一平面，但在海報(bào)下方多分割出了一個(gè)平面（箭頭所指黃色區(qū)域），同時(shí)還出現(xiàn)了過度分割，如圖6(g)所示。通過PlaneRecNet 改進(jìn)前后的分割效果對(duì)比可知，本文算法通過減少FPN 高層圖像語義信息過度損失，改善了紋理稀疏的結(jié)構(gòu)化場(chǎng)景中PlaneRecNet網(wǎng)絡(luò)的分割效果，如圖6(d)和圖6(h)所示。

本文使用具有不同交并比（Intersection over Union,IoU）的平均精準(zhǔn)率（Average Precision,AP）作為衡量指標(biāo)，PlaneRCNN、PlaneRecNet 和PlaneRecNet+FEM三種算法的AP、AP50（IoU>50%）、AP75（IoU>75%）對(duì)比結(jié)果如表 1 所示。由表 1 數(shù)據(jù)可知，PlaneRecNet+FEM 的平均精準(zhǔn)率比PlaneRCNN 提高了11.89%，與改進(jìn)前的PlaneRecNet 相比提高了5.47%。

表1 三種算法AP 值對(duì)比Tab.1 Comparison of AP values for the three algorithms

4.2 合成數(shù)據(jù)集定位結(jié)果與分析

為驗(yàn)證算法的整體效果，使用ICL-NUIM 數(shù)據(jù)集進(jìn)行評(píng)估。為比較ORB-SLAM3、Planar-SLAM、Manhattan-SLAM 與本文算法（圖、表中用Our 表示）的誤差大小，本文使用絕對(duì)軌跡和相對(duì)旋轉(zhuǎn)的均方根誤差（Root Mean Squared Error,RMSE）來衡量定位精度。絕對(duì)軌跡誤差（Absolute Trajectory Error,ATE）適合評(píng)估SLAM 算法估計(jì)軌跡與相機(jī)真實(shí)運(yùn)動(dòng)軌跡的整體一致性，而相對(duì)旋轉(zhuǎn)誤差更適合評(píng)估軌跡漂移程度，因此通過比較各算法的絕對(duì)軌跡誤差和相對(duì)旋轉(zhuǎn)誤差進(jìn)一步驗(yàn)證。

不同算法在ICL-NUIM 數(shù)據(jù)集部分序列軌跡對(duì)比結(jié)果如圖7 所示：其中黑色線條代表序列的真實(shí)軌跡，藍(lán)色線條為算法估計(jì)的軌跡，紅色線條是偏移誤差。從圖7(a)-(d)可以看出，lr-kt1 序列由于點(diǎn)特征缺失加上陽臺(tái)光照影響導(dǎo)致ORB-SLAM3 與Planar-SLAM 誤差較大；圖7(e)-(h)中of-kt0 序列由于紋理稀疏加之彎道角域過大，對(duì)SLAM 系統(tǒng)提出了較大挑戰(zhàn)，Planar-SLAM 僅利用結(jié)構(gòu)規(guī)律性估計(jì)直線和平面上的旋轉(zhuǎn)運(yùn)動(dòng)，面對(duì)室內(nèi)場(chǎng)景存在局限性，導(dǎo)致精度較差。綜合圖7 可以看出，本文算法的相機(jī)運(yùn)動(dòng)軌跡與序列真實(shí)軌跡吻合度較高。

四種算法在ICL-NUIM 數(shù)據(jù)集所有序列上運(yùn)行的絕對(duì)軌跡誤差和相對(duì)旋轉(zhuǎn)誤差RMSE 結(jié)果，如表2 所示。ICL-NUIM 數(shù)據(jù)集包含大量缺少紋理的地板、天花板和墻壁，能提取到的點(diǎn)特征稀少，導(dǎo)致基于點(diǎn)特征的 ORB-SLAM3 精度較差；Planar-SLAM 和Manhattan-SLAM 均是將位姿估計(jì)解耦，在符合MW假設(shè)的情況下聯(lián)合線、面特征估計(jì)旋轉(zhuǎn)運(yùn)動(dòng)，其中Manhattan-SLAM 為了應(yīng)對(duì)室內(nèi)場(chǎng)景復(fù)雜性，加入了MW 場(chǎng)景和非MW 場(chǎng)景判斷策略，但二者的面特征提取方法都是傳統(tǒng)聚集層次聚類算法，并且主平面未經(jīng)篩選，導(dǎo)致誤差較大；本文采用深度學(xué)習(xí)的方法，在MW 場(chǎng)景中改善了面特征提取的數(shù)量和質(zhì)量，通過方向引導(dǎo)的MF 估計(jì)策略選取場(chǎng)景優(yōu)勢(shì)平面來估計(jì)最優(yōu)MF，從而提高了曼哈頓幀跟蹤效果。在ICL-NUIM 數(shù)據(jù)集的測(cè)試結(jié)果如表 2 所示，本文算法相對(duì)于Manhattan-SLAM、Planar-SLAM 和ORB-SLAM3 的絕對(duì)軌跡誤差 RMSE 分別減少了10.82%、20.46%和34.77%，相對(duì)旋轉(zhuǎn)誤差RMSE 分別減少了10.63%、15.11%和55.76%。

4.3 真實(shí)世界數(shù)據(jù)集定位結(jié)果與分析

本文算法面向室內(nèi)環(huán)境，因此需在TUM RGB-D數(shù)據(jù)集上進(jìn)行評(píng)估。在圖8 中，對(duì)本文算法和其他三種算法估計(jì)的相機(jī)運(yùn)動(dòng)軌跡進(jìn)行可視化，并與相機(jī)真實(shí)運(yùn)動(dòng)軌跡進(jìn)行對(duì)比。對(duì)于具有豐富紋理的fr2/desk序列，本文算法性能與其他算法基本相當(dāng)，如圖8(a)所示，Planar-SLAM 雖然可以追蹤整個(gè)序列，但其估計(jì)出的相機(jī)軌跡誤差較大。在fr3/l_cabinet 等弱紋理序列中，ORB-SLAM3 初始階段表現(xiàn)良好，但隨著場(chǎng)景紋理變少，定位精度逐漸下降，在fr3/s_t_near 等序列上點(diǎn)特征提取失敗，算法失效，無法跟蹤完整序列，如圖8(b)和圖8(c)所示。

圖8 部分TUM 序列估計(jì)的軌跡對(duì)比Fig.8 Comparison of trajectories estimated for partial TUM sequences

四種算法在TUM RGB-D 數(shù)據(jù)集fr2/desk 序列和部分紋理稀疏室內(nèi)場(chǎng)景序列測(cè)試的絕對(duì)軌跡誤差和相對(duì)旋轉(zhuǎn)誤差RMSE 結(jié)果，如表3 所示。ORB-SLAM3在所選取的大部分序列中誤差較大，在fr3/s_nt_far 等挑戰(zhàn)性較高的序列上出現(xiàn)了跟蹤丟失的現(xiàn)象，無法獲得完整運(yùn)行軌跡。在紋理豐富且不滿足MW 假設(shè)的fr2/desk 序列上，Planar-SLAM 僅依靠MW 假設(shè)進(jìn)行位姿估計(jì)，導(dǎo)致旋轉(zhuǎn)誤差較大，定位精度反而下降；本文采用曼哈頓跟蹤與點(diǎn)線特征跟蹤兩種策略，避免了算法對(duì)曼哈頓場(chǎng)景的過度依賴，使得定位精度高于Planar-SLAM，略低于ORB-SLAM3。fr3/cabinet 和fr3/l_cabinet 序列的結(jié)果表明，基于點(diǎn)特征的方法難以有效減少旋轉(zhuǎn)估計(jì)帶來的累計(jì)漂移，基于MW 假設(shè)的旋轉(zhuǎn)估計(jì)可以有效減少旋轉(zhuǎn)誤差；而本文算法的相對(duì)旋轉(zhuǎn)誤差普遍低于其他三種算法。本文算法相對(duì)于Manhattan-SLAM、Planar-SLAM 和ORB-SLAM3 的絕對(duì)軌跡誤差RMSE 分別減少了15.92%、31.12%和43.23%，相對(duì)旋轉(zhuǎn)誤差RMSE 分別減少了16.44%、29.53%和32.89%。

表3 各算法在TUM RGB-D 數(shù)據(jù)集軌跡誤差對(duì)比Tab.3 Comparison of trajectory errors of various algorithms with the TUM RGB-D dataset

4.4 真實(shí)場(chǎng)景實(shí)驗(yàn)

為了在真實(shí)環(huán)境中全面驗(yàn)證本文方法，使用固定在地面機(jī)器人上的RealSense D435i 攝像頭采集了實(shí)驗(yàn)場(chǎng)景數(shù)據(jù)集并且進(jìn)行算法測(cè)試。場(chǎng)景一為含有較多小平面和非正交面，且符合MW 假設(shè)的室內(nèi)實(shí)驗(yàn)室，規(guī)模為7.8 m×7.2 m，如圖10(a)所示。在場(chǎng)景一的平面布局圖中，A 和B 位置分別指示機(jī)器人的開始和終止，途經(jīng)1、2 和3 位置的折線路徑用于衡量算法運(yùn)行軌跡的漂移。場(chǎng)景二為紋理稀疏的走廊和樓梯間，用走廊到樓梯間的藍(lán)色路徑衡量算法運(yùn)行軌跡，走廊部分長8 m，樓梯間部分長4 m，如圖10(b)所示。

圖10 真實(shí)場(chǎng)景設(shè)置Fig.10 Real scene settings

在場(chǎng)景一中，ORB-SLAM3 由于特征稀疏和彎道角域過大出現(xiàn)了跟蹤丟失的現(xiàn)象，導(dǎo)致軌跡不完整。本文算法與Manhattan-SLAM 的定位軌跡對(duì)比，如圖11(a)所示：Manhattan-SLAM 的MF 受到小平面和非正交面的干擾，導(dǎo)致軌跡漂移較大；而本文算法利用方向引導(dǎo)的方法進(jìn)行主平面篩選，減少了MF 估計(jì)的誤差，利用曼哈頓約束實(shí)現(xiàn)了更精確的旋轉(zhuǎn)估計(jì)。在場(chǎng)景二中，ORB-SLAM3 由于點(diǎn)特征過少導(dǎo)致跟蹤丟失。本文算法與Manhattan-SLAM 的定位軌跡對(duì)比，如圖11(b)所示：由于稀疏且相似的紋理會(huì)影響面特征提取的準(zhǔn)確性，導(dǎo)致Manhattan-SLAM 系統(tǒng)定位精度下降；而本文算法因?yàn)楦倪M(jìn)了面特征的提取方式，一定程度上避免了面特征提取的錯(cuò)誤，使算法在室內(nèi)弱紋理場(chǎng)景有更高的定位精度。真實(shí)場(chǎng)景實(shí)驗(yàn)的絕對(duì)軌跡誤差RMSE 對(duì)比如表4 所示：本文算法在場(chǎng)景一中誤差比Manhattan-SLAM 降低了26.58%，在場(chǎng)景二中誤差比Manhattan-SLAM 降低了43.22%。

表4 真實(shí)場(chǎng)景絕對(duì)軌跡誤差RMSE 對(duì)比（單位：米）Tab.4 Comparison of absolute trajectory error RMSE in real scenes (Unit: m)

圖11 真實(shí)場(chǎng)景軌跡對(duì)比圖Fig.11 Trajectory comparison of real scene

5 總結(jié)

本文針對(duì)傳統(tǒng)V-SLAM在紋理稀疏的室內(nèi)環(huán)境下出現(xiàn)的定位精度不高甚至失效的問題，提出了一種魯棒的V-SLAM 系統(tǒng)，該系統(tǒng)充分利用室內(nèi)場(chǎng)景的面特征結(jié)構(gòu)規(guī)律實(shí)現(xiàn)了更為精確的旋轉(zhuǎn)估計(jì)。針對(duì)傳統(tǒng)面特征提取算法存在的平面漏檢測(cè)和錯(cuò)分割的問題，使用特征增強(qiáng)模塊改進(jìn)了PlaneRecNet 網(wǎng)絡(luò)，使面特征及參數(shù)提取更精確。針對(duì)單純利用曼哈頓假設(shè)的應(yīng)用場(chǎng)景受限以及MF 估計(jì)不準(zhǔn)確的問題，通過方向引導(dǎo)的方法進(jìn)行主導(dǎo)平面篩選，減少了小平面和非平面噪聲對(duì)MF 估計(jì)的影響，分別采用曼哈頓跟蹤與點(diǎn)線特征跟蹤兩種策略以降低對(duì)曼哈頓場(chǎng)景的依賴。公開數(shù)據(jù)集上的實(shí)驗(yàn)結(jié)果表明，本文算法的平均絕對(duì)軌跡誤差比Manhattan-SLAM 減少了13.37%，相較于ORBSLAM3 減少了39%。然而，深度學(xué)習(xí)網(wǎng)絡(luò)使算法整體實(shí)時(shí)性受到限制，面特征的提取占用了大部分時(shí)間。在接下來的研究中，需要進(jìn)一步優(yōu)化算法，提高系統(tǒng)時(shí)效性。曼哈頓約束目前的限制在于要求當(dāng)前幀有兩個(gè)及以上正交的主方向，后續(xù)可以利用線特征的方向或加入IMU 信息作為主方向來構(gòu)建MW 假設(shè)，并可能將其擴(kuò)展到室外環(huán)境。