徐 韜，陳孟元，3*，劉曉曉，韓朋朋

(1.安徽工程大學(xué)電氣工程學(xué)院，安徽 蕪湖 241000；2.高端裝備先進(jìn)感知與智能控制教育部重點(diǎn)實(shí)驗(yàn)室，安徽 蕪湖 241000；3.安徽工程大學(xué)產(chǎn)業(yè)創(chuàng)新技術(shù)研究有限公司，安徽 蕪湖 241000)

同步定位與建圖(Simultaneous Localization and Mapping，SLAM)是實(shí)現(xiàn)移動(dòng)機(jī)器人完全自主導(dǎo)航的關(guān)鍵技術(shù)，傳統(tǒng)SLAM 采用激光雷達(dá)獲取周圍障礙物的信息，然而當(dāng)遇到玻璃環(huán)境時(shí)易發(fā)生激光穿透玻璃導(dǎo)致定位精度較低。 使用視覺傳感器作為移動(dòng)機(jī)器人的視覺SLAM 算法可以較為容易地捕捉環(huán)境中的信息、跟蹤場(chǎng)景中的動(dòng)態(tài)目標(biāo)，因此視覺SLAM(Visual SLAM，VSLAM)得到了廣泛的應(yīng)用[1-3]。

近年來， 較為成熟的SLAM 系統(tǒng)有ORBSLAM2[4]、LSD-SLAM[5]、RGBD SLAM[6]和RTABMap[7]。 但是，它們的大多數(shù)是基于靜態(tài)環(huán)境假設(shè)的，在動(dòng)態(tài)場(chǎng)景下的建圖效果會(huì)大幅下降。 因此，怎樣提高SLAM 系統(tǒng)在動(dòng)態(tài)環(huán)境下的準(zhǔn)確性成為一個(gè)重要課題。 目前針對(duì)動(dòng)態(tài)場(chǎng)景下的SLAM 算法主要有三類:幾何方法、深度學(xué)習(xí)方法以及幾何與深度學(xué)習(xí)結(jié)合的方法。 在幾何方法中，艾青林等[8]提出了一種動(dòng)態(tài)環(huán)境下基于改進(jìn)幾何與運(yùn)動(dòng)約束的機(jī)器人RGBD SLAM 算法，先將特征點(diǎn)分為靜態(tài)、狀態(tài)未知、可疑靜態(tài)、動(dòng)態(tài)和錯(cuò)誤匹配五類，再利用幾何約束對(duì)靜態(tài)特征點(diǎn)進(jìn)行篩選。 Li 等[9]選擇深度邊緣點(diǎn)來尋找對(duì)應(yīng)關(guān)系，并設(shè)計(jì)了一種靜態(tài)加權(quán)方法來降低動(dòng)態(tài)點(diǎn)的影響。 在深度學(xué)習(xí)方法中，通常采用深度學(xué)習(xí)中的目標(biāo)檢測(cè)和語義分割技術(shù)。 Li 等[10]使用語義分割網(wǎng)絡(luò)SegNet[11]對(duì)圖像進(jìn)行分割，然后進(jìn)一步處理以區(qū)分動(dòng)態(tài)對(duì)象。 Liu 等[12]采用不同的語義分割方法來檢測(cè)動(dòng)態(tài)對(duì)象并去除離群點(diǎn)。 在幾何方法和深度學(xué)習(xí)結(jié)合的方法中，由深度學(xué)習(xí)網(wǎng)絡(luò)提供有關(guān)動(dòng)態(tài)對(duì)象的語義先驗(yàn)信息[13]，隨后通過幾何約束進(jìn)一步細(xì)化以過濾動(dòng)態(tài)對(duì)象出現(xiàn)的特征。Wu 等[14]針對(duì)YOLO[15]進(jìn)行了改進(jìn)，提出了Darknet19-YOLOv3 的輕量級(jí)目標(biāo)檢測(cè)網(wǎng)絡(luò)獲取動(dòng)態(tài)物體的語義信息，然后提出一種Depth-RANSAC 算法篩選動(dòng)態(tài)點(diǎn)，以提高系統(tǒng)的實(shí)時(shí)性和精度，但是目標(biāo)檢測(cè)網(wǎng)絡(luò)易發(fā)生動(dòng)態(tài)物體檢測(cè)不完整，產(chǎn)生漏檢測(cè)，導(dǎo)致語義信息缺失，從而難以精確去除動(dòng)態(tài)點(diǎn)，降低了系統(tǒng)定位精度。 Bescos 等[16]提出使用實(shí)例分割網(wǎng)絡(luò)Mask R-CNN[17]精確分割運(yùn)動(dòng)目標(biāo)(人和車等)邊界，并結(jié)合多視角幾何方法檢測(cè)潛在動(dòng)態(tài)物體。 該算法依據(jù)兩幀中同一關(guān)鍵點(diǎn)的變化角度判斷該點(diǎn)是否屬于動(dòng)態(tài)點(diǎn)，但在特征跟蹤過程中關(guān)鍵點(diǎn)變化角度易受到噪聲影響，并且使用的實(shí)例分割網(wǎng)絡(luò)面對(duì)物體外觀缺失情況分割效果較差。 Yu 等[18]提出了DS-SLAM，將語義分割網(wǎng)絡(luò)SegNet 和移動(dòng)一致性檢測(cè)方法相結(jié)合檢測(cè)場(chǎng)景中的動(dòng)態(tài)物體，以過濾出場(chǎng)景中的動(dòng)態(tài)信息。 顯著提高SLAM 系統(tǒng)在高動(dòng)態(tài)環(huán)境下魯棒性和穩(wěn)定性，但該方法在剔除動(dòng)態(tài)特征點(diǎn)后，易發(fā)生因特征點(diǎn)過少而特征跟蹤失敗。

綜上所述，現(xiàn)有的SLAM 算法易出現(xiàn)動(dòng)態(tài)物體檢測(cè)不完整以及難以準(zhǔn)確判斷潛在動(dòng)態(tài)物體的運(yùn)動(dòng)狀態(tài)等問題，本文提出一種動(dòng)態(tài)場(chǎng)景下基于注意力機(jī)制與幾何約束的VSLAM 算法，本文目標(biāo)檢測(cè)網(wǎng)絡(luò)通過將坐標(biāo)注意力(Coordinate Attention，CA)機(jī)制[19]嵌入YOLOv5 主干網(wǎng)絡(luò)的殘差單元，并引入自適應(yīng)空間特征融合模塊(Adaptively Spatial Feature Fusion，ASFF)，實(shí)現(xiàn)對(duì)相機(jī)視野中外觀缺失物體的檢測(cè)，并提出一種雙重靜態(tài)點(diǎn)約束方法以解決潛在運(yùn)動(dòng)物體的運(yùn)動(dòng)判斷問題。 通過在公開的TUM 數(shù)據(jù)集進(jìn)行驗(yàn)證，測(cè)試結(jié)果表明本文算法與ORB-SLAM2 算法、DS-SLAM 算法、DynaSLAM 算法相比，在定位精度等方面有較大優(yōu)勢(shì)，表現(xiàn)出了良好的構(gòu)圖能力。

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

圖1 所示為系統(tǒng)整體框架。 本文針對(duì)傳統(tǒng)SLAM算法易出現(xiàn)動(dòng)態(tài)物體檢測(cè)不完整以及難以準(zhǔn)確判斷潛在動(dòng)態(tài)物體的運(yùn)動(dòng)狀態(tài)等問題，提出一種動(dòng)態(tài)場(chǎng)景下基于注意力機(jī)制與幾何約束的VSLAM 算法。 在潛在動(dòng)態(tài)物體檢測(cè)階段，通過本文所提的AFNET(Attention mechanism and adaptively spatial feature Fusion target detection NETwork)目標(biāo)檢測(cè)網(wǎng)絡(luò)對(duì)圖像信息中的潛在動(dòng)態(tài)物體進(jìn)行目標(biāo)檢測(cè)，并提取圖像的ORB 特征點(diǎn)。 在基于雙重靜態(tài)點(diǎn)約束的位姿優(yōu)化階段，通過DBSCAN(Density-Based Spatial Clustering of Applications with Noise)密度聚類算法初步提取潛在動(dòng)態(tài)物體的靜態(tài)點(diǎn)，生成初步靜態(tài)點(diǎn)集合，并使用其進(jìn)行初步位姿估計(jì)，然后使用雙重靜態(tài)點(diǎn)約束方法，進(jìn)一步確定物體真實(shí)運(yùn)動(dòng)狀態(tài)，并剔除動(dòng)態(tài)點(diǎn)，再使用全部的靜態(tài)點(diǎn)集合進(jìn)行二次位姿優(yōu)化，提升系統(tǒng)精度。

圖1 系統(tǒng)整體框架

2 潛在動(dòng)態(tài)物體檢測(cè)

圖2 所示為YOLOv5 目標(biāo)檢測(cè)在物體外觀嚴(yán)重缺失和物體外觀輕微缺失時(shí)發(fā)生的漏檢測(cè)，圓圈為本文所作輔助標(biāo)記。 針對(duì)待檢測(cè)物體在相機(jī)視野中的外觀缺失易導(dǎo)致漏檢測(cè)問題發(fā)生，本文提出一種聚合注意力機(jī)制自適應(yīng)特征融合的目標(biāo)檢測(cè)網(wǎng)絡(luò)AFNET。 AFNET 網(wǎng)絡(luò)結(jié)構(gòu)如圖3 所示。 該網(wǎng)絡(luò)將CA 注意力模塊嵌入YOLOv5 的主干網(wǎng)絡(luò)CSP1_X(X＝1，2，3；代表網(wǎng)絡(luò)中殘差單元個(gè)數(shù))的殘差單元中形成CA 殘差單元，最終構(gòu)造對(duì)外觀缺失區(qū)域特征更加關(guān)注的聚合注意力模塊AttenCSP1_X(X＝1，2，3；代表網(wǎng)絡(luò)中CA 殘差單元個(gè)數(shù))，同時(shí)為更有效地進(jìn)行特征融合，在特征金字塔FPN 的基礎(chǔ)上引入自適應(yīng)空間特征融合ASFF 模塊，構(gòu)造自適應(yīng)空間特征融合金字塔，增強(qiáng)網(wǎng)絡(luò)的特征提取能力，提升對(duì)外觀缺失物體的目標(biāo)檢測(cè)精度。

圖2 YOLOv5 漏檢測(cè)

2.1 聚合注意力模塊

視覺SLAM 系統(tǒng)運(yùn)行過程中，相機(jī)視野中外觀缺失的物體占據(jù)環(huán)境較大區(qū)域，若無法識(shí)別出其中的潛在動(dòng)態(tài)物體，將對(duì)SLAM 系統(tǒng)的穩(wěn)定性造成干擾。 針對(duì)這一問題，本文在YOLOv5 的基礎(chǔ)上引入CA 注意力模塊。 注意力模塊加入網(wǎng)絡(luò)層的位置與目標(biāo)檢測(cè)網(wǎng)絡(luò)的學(xué)習(xí)和表征能力關(guān)系密切。 若將注意力模塊加入特征金字塔FPN 或檢測(cè)頭部分，不僅難以將冗余信息和關(guān)鍵特征從特征圖中區(qū)分開來，還會(huì)傳遞錯(cuò)誤信息，從而降低網(wǎng)絡(luò)提取特征的能力。因此，本文對(duì)YOLOv5 主干網(wǎng)絡(luò)進(jìn)行改進(jìn)，將CA 注意力模塊嵌入到CSP1_X 的殘差單元中，構(gòu)造CA殘差單元，最終形成聚合注意力模塊。 首先，通過CA 注意力模塊突出外觀缺失區(qū)域特征圖的關(guān)鍵特征，同時(shí)弱化無關(guān)信息，再經(jīng)過殘差網(wǎng)絡(luò)深入提取特征，最后合并輸出以提升目標(biāo)檢測(cè)的精度。 聚合注意力模塊如圖4 所示。

圖4 聚合注意力模塊

CA 殘差單元利用坐標(biāo)注意力機(jī)制對(duì)通道信息與空間位置信息均敏感的特性生成對(duì)外觀缺失區(qū)域更具有判別能力的特征表達(dá)，該模塊不僅增大了感興趣區(qū)域特征通道權(quán)重，還能獲取感興趣區(qū)域空間位置，充分突出外觀缺失區(qū)域有效特征信息，同時(shí)避免了干擾信息影響。 CA 殘差單元具體計(jì)算方式如式(1)所示:

式中:M'為上一個(gè)CA 殘差單元的輸出特征，M"為CA 殘差單元的輸出特征，H(·)表示使用CA 注意力提取特征，f1×1和f3×3分別表示1×1 和3×3 的卷積層，σ(·)表示leakyrelu 激活函數(shù)，Bn為歸一化參數(shù)。

2.2 CA 注意力機(jī)制

現(xiàn)有的SE(Squeeze-and-Excitation)通道注意力機(jī)制[20]對(duì)內(nèi)部通道信息進(jìn)行關(guān)注，但卻忽視了空間位置信息。 而卷積注意力模塊(Convolutional Block Attention Module，CBAM)[21]為了引入空間位置信息，使用了在通道上全局池化的方法，但這種方法卻又只考慮了局部范圍的信息。 因此本文使用CA 注意力機(jī)制，如圖5 所示。 通過在通道注意力中嵌入空間位置信息，生成對(duì)通道信息和位置信息均敏感的特征圖從而使得網(wǎng)絡(luò)更加關(guān)注高維特征信息，提高網(wǎng)絡(luò)對(duì)外觀缺失區(qū)域的注意力。 CA 注意力機(jī)制首先對(duì)輸入的特征圖XC×H×W使用尺寸(H，1)和(1，W)的池化核，分別對(duì)水平和垂直方向的每個(gè)通道進(jìn)行池化，得到一對(duì)大小為C×H×1 和C×1×W的特征圖。 因此，高度為h的第c個(gè)通道和寬度為w的第c個(gè)通道的輸出如下:

圖5 CA 注意力機(jī)制

式中:xc(h，i)和xc(j，w)分別為水平和豎直方向的特征，(h)和(w)分別是編碼后的水平張量和豎直張量，H，W和C分別代表輸入特征圖的長、寬和通道數(shù)。 接著對(duì)這兩個(gè)張量進(jìn)行融合操作并且用1×1卷積函數(shù)實(shí)現(xiàn)降維，如式(4)所示:

式中:[zh，zw]為融合操作，f1×1為1×1 卷積函數(shù)，F(xiàn)∈RC/r×(H+W)為空間信息在水平和豎直方向上進(jìn)行編碼的中間特征映射，δ(·)為h_wish 激活函數(shù)，r是下采樣步長。

接著將F沿著空間維度重新分解為兩個(gè)單獨(dú)的張量Fh和Fw，然后分別使用1×1 卷積函數(shù)將Fh和Fw通道數(shù)變?yōu)榕cX一致，得到如下結(jié)果:

式中:σ(·)為sigmoid 激活函數(shù)，gh和gw分別為水平方向和豎直方向的權(quán)重。 最終CA 注意力模塊的輸出表示為:

2.3 自適應(yīng)空間特征融合金字塔

原YOLOv5 網(wǎng)絡(luò)中的FPN 結(jié)構(gòu)僅將不同特征層調(diào)整為統(tǒng)一尺寸后再進(jìn)行累加，但是不同特征尺度之間不一致易造成融合特征圖噪聲增大從而導(dǎo)致效果變差。 針對(duì)這一問題，本文引入ASFF 模塊構(gòu)造自適應(yīng)空間特征融合金字塔，該算法使網(wǎng)絡(luò)能夠自適應(yīng)地學(xué)習(xí)各個(gè)特征層上每個(gè)位置的權(quán)重，使重要信息的特征在融合時(shí)占據(jù)主導(dǎo)地位，通過在空間上過濾無用信息進(jìn)而抑制反向傳播時(shí)的不一致，改善特征比例不變性。 最終增強(qiáng)目標(biāo)檢測(cè)網(wǎng)絡(luò)對(duì)動(dòng)態(tài)物體特征識(shí)別能力。 ASFF 結(jié)構(gòu)如圖6 所示。 它包括特征縮放和自適應(yīng)融合兩個(gè)部分。

圖6 ASFF 結(jié)構(gòu)

F1、F2、和F3為主干網(wǎng)絡(luò)后的三個(gè)特征層，F(xiàn)1和F2經(jīng)過特征縮放后分別生成與F3尺寸相同的特征層F1→3和F2→3。

AFNET 網(wǎng)絡(luò)參數(shù)設(shè)置如表1 所示。 如輸入列的[-1，6]表示為來自于上一層和第6 層合并輸出，層編號(hào)從0 開始，張量信息代表該模塊的輸入通道數(shù)、輸出通道數(shù)、卷積核大小、步長等參數(shù)信息。

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

3 基于雙重靜態(tài)點(diǎn)約束的位姿優(yōu)化

目標(biāo)檢測(cè)網(wǎng)絡(luò)僅能識(shí)別出潛在動(dòng)態(tài)物體，但是對(duì)潛在動(dòng)態(tài)物體的運(yùn)動(dòng)狀態(tài)缺少有效判斷。 由于動(dòng)態(tài)物體上也可能存在靜態(tài)特征點(diǎn)，若直接剔除檢測(cè)框中的所有特征點(diǎn)會(huì)造成很多靜態(tài)特征點(diǎn)被剔除，影響姿態(tài)估計(jì)的準(zhǔn)確性。 因此，本文首先使用DBSCAN 密度聚類對(duì)檢測(cè)框內(nèi)的前景點(diǎn)和背景點(diǎn)進(jìn)行區(qū)分，篩選初步靜態(tài)點(diǎn)集合進(jìn)行初步位姿估計(jì)。 然后提出一種雙重靜態(tài)點(diǎn)約束的方法，進(jìn)一步準(zhǔn)確判斷特征點(diǎn)的真實(shí)運(yùn)動(dòng)狀態(tài)，從而得到更加精確的靜態(tài)點(diǎn)集合，并進(jìn)行二次位姿優(yōu)化，提升系統(tǒng)的精度。

3.1 DBSCAN 密度聚類

與語義分割方法相比，目標(biāo)檢測(cè)網(wǎng)絡(luò)的實(shí)時(shí)性更高，但不能提供準(zhǔn)確的分割掩碼從而導(dǎo)致分類為動(dòng)態(tài)物體的檢測(cè)框中靜態(tài)背景眾多，靜態(tài)特征點(diǎn)的誤刪除將減少相機(jī)位姿優(yōu)化的約束，降低相機(jī)位姿估計(jì)的準(zhǔn)確性。 行人作為前景的非剛體，其深度具有很好的連續(xù)性，并且通常與背景深度有很大的斷層。 因此，本文引入DBSCAN 密度聚類算法來區(qū)分分類為人的檢測(cè)框的前景點(diǎn)和背景點(diǎn)。 本文采取自適應(yīng)的方法確定DBSCAN 密度聚類算法的鄰域半徑eps 和鄰域樣本數(shù)的閾值minPts，聚類完成后，將簇群C＝{C1，C2，…，Ck}中平均值最低的樣本作為檢測(cè)框的前景點(diǎn)。然后，將背景點(diǎn)內(nèi)的特征點(diǎn)設(shè)置為初步靜態(tài)點(diǎn)并用集合Uk表示，用于后續(xù)的位姿估計(jì)。

圖7 所示為DBSCAN 密度聚類結(jié)果，圖7(a)為未使用DBSCAN 密度聚類，檢測(cè)框內(nèi)的特征點(diǎn)全部被標(biāo)記為動(dòng)態(tài)點(diǎn)并剔除，不參與位姿計(jì)算；而圖7(b)為經(jīng)過DBSCAN 密度聚類后的特征點(diǎn)分類結(jié)果；由圖可知，2 個(gè)檢測(cè)框內(nèi)背景點(diǎn)內(nèi)的特征點(diǎn)都被正確地歸類為靜態(tài)點(diǎn)，參與位姿計(jì)算過程，因此，采用密度聚類區(qū)分靜態(tài)點(diǎn)能在較大程度上提高后續(xù)位姿估計(jì)的精度。

圖7 DBSCAN 密度聚類結(jié)果

3.2 初步位姿估計(jì)

對(duì)檢測(cè)框內(nèi)的靜態(tài)點(diǎn)和動(dòng)態(tài)點(diǎn)進(jìn)行區(qū)分之后，使用當(dāng)前幀F(xiàn)k中的初步靜態(tài)點(diǎn)集合Uk進(jìn)行初步位姿估計(jì)，Uk在三維空間中的集合為Ck。 設(shè)空間中點(diǎn)的三維坐標(biāo)為ci＝[Xi，Yi，Zi]T，其對(duì)應(yīng)的投影像素坐標(biāo)為ui＝[ui，vi]T，且ui∈Uk，ci∈Ck，則有:

式中:ξ∈se(3)是相機(jī)位姿R，t的李代數(shù)形式，di為對(duì)應(yīng)靜態(tài)點(diǎn)深度，e為誤差項(xiàng)，K為相機(jī)內(nèi)參矩陣。式(10)可寫成以下矩陣形式:

最后，通過將誤差求和構(gòu)造最小二乘問題并使其最小化，即可得出最小化重投影誤差:

3.3 雙重靜態(tài)點(diǎn)約束

若直接使用初步靜態(tài)點(diǎn)集合解算位姿，則會(huì)存在一些在當(dāng)前場(chǎng)景處于靜止?fàn)顟B(tài)的對(duì)象被錯(cuò)誤地歸類為動(dòng)態(tài)特征的問題(如坐在椅子上不動(dòng)的人)，為了進(jìn)一步判斷場(chǎng)景中特征點(diǎn)的真實(shí)運(yùn)動(dòng)狀態(tài)，如其在當(dāng)前場(chǎng)景中處于靜止?fàn)顟B(tài)，則可將其確定為靜態(tài)特征點(diǎn)，從而增加參與位姿計(jì)算的特征點(diǎn)個(gè)數(shù)。 針對(duì)以上問題，本文提出一種雙重靜態(tài)點(diǎn)約束的方法進(jìn)一步獲得特征點(diǎn)的真實(shí)運(yùn)動(dòng)狀態(tài)，并對(duì)位姿進(jìn)行二次優(yōu)化。

圖8(a)所示為幾何約束示意圖，O1和O2分別為前一幀I1和當(dāng)前幀I2相機(jī)光學(xué)中心，P為空間中的某一點(diǎn)，其在I1、I2上的投影分別為P1、P2，P1、P2的歸一化像素坐標(biāo)分別為X1＝[u1，v1，1]、X2＝[u2，v2，1]，F(xiàn)為基礎(chǔ)矩陣，應(yīng)該滿足:

圖8 幾何約束示意圖

若F矩陣的計(jì)算足夠準(zhǔn)確，則P2在極線l2上，滿足式(13)約束條件。 設(shè)極線方程為Ax+By+C＝0，P2到極線的距離為D，則此時(shí)滿足:

然而在實(shí)際情況中，有以下三種情況不能滿足以上約束:①由于特征提取和F矩陣估計(jì)的不確定性，P2可能不會(huì)準(zhǔn)確地落在極線l2上，不能滿足極線約束。 ②空間點(diǎn)發(fā)生運(yùn)動(dòng)，如圖8(b)所示，相機(jī)由前一幀移動(dòng)至當(dāng)前幀位置時(shí)，P點(diǎn)運(yùn)動(dòng)到P'，P'在當(dāng)前幀I2的投影P'2不在極線l2上，不能滿足以上約束。 ③當(dāng)特征點(diǎn)沿著相機(jī)光軸方向運(yùn)動(dòng)時(shí)，如圖8(c)所示，空間點(diǎn)P點(diǎn)運(yùn)動(dòng)到P'，此時(shí)P'在當(dāng)前幀I2的投影P'2仍然在極線l2上，滿足極線約束，但實(shí)際P點(diǎn)是動(dòng)態(tài)點(diǎn)。 綜上所述，只使用極線幾何約束來確定動(dòng)態(tài)點(diǎn)并不嚴(yán)謹(jǐn)，具有一定的局限性。 因此本文提出一種雙重靜態(tài)點(diǎn)約束方法準(zhǔn)確確定動(dòng)態(tài)點(diǎn)，為位姿估計(jì)篩選出良好的靜態(tài)點(diǎn)。

定義空間中3D 點(diǎn)S在關(guān)鍵幀K上的實(shí)際投影點(diǎn)為S1，在當(dāng)前幀C中的投影點(diǎn)為S2(x1，y1)，由當(dāng)前幀C中與S1匹配得到的點(diǎn)為S'2(x2，y2)，雙重靜態(tài)點(diǎn)約束方法如式(15)所示:

式中:T表示靜態(tài)點(diǎn)得分，D為S2到極線的距離，距離越接近0，說明投影點(diǎn)S2越有可能在當(dāng)前幀的極線l2上，則該點(diǎn)是動(dòng)態(tài)點(diǎn)的可能性越?。沪為S2與S'2的歐氏距離，Δz為P點(diǎn)在當(dāng)前幀的投影深度z1與實(shí)際深度z2的差值。λ和μ為權(quán)重，當(dāng)距離D大于設(shè)定閾值τD時(shí)，取λ＝1，μ＝0，表示不需計(jì)算位置差與深度差便可確定動(dòng)態(tài)點(diǎn)。 否則取λ＝0，μ＝1，若Δd大于閾值τd或Δz大于閾值τz時(shí)，則認(rèn)為S點(diǎn)為動(dòng)態(tài)點(diǎn)。 最后計(jì)算臨界點(diǎn)T的得分并設(shè)定臨界閾值τT，若T＞τT，則確定S點(diǎn)為動(dòng)態(tài)點(diǎn)。

3.4 二次位姿優(yōu)化

為了提升位姿估計(jì)的精度，使用雙重靜態(tài)點(diǎn)約束方法確定靜態(tài)點(diǎn)后，仍然使用最小化重投影誤差對(duì)位姿進(jìn)行二次優(yōu)化，初始值設(shè)置為初步位姿估計(jì)值，參與優(yōu)化的特征點(diǎn)為初步靜態(tài)點(diǎn)集合Uk以及使用雙重靜態(tài)點(diǎn)約束方法篩選出的靜態(tài)點(diǎn)。

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

本文仿真實(shí)驗(yàn)平臺(tái)電腦配置為:Ubuntu18.04 版本Linux 操作系統(tǒng)，Intel(R)Core(TM)i5-11400F＠2.60 GHz，顯卡為RTX3060，16G 內(nèi)存。 實(shí)驗(yàn)所用的數(shù)據(jù)集均來自公開的TUM RGB-D 數(shù)據(jù)集。

4.1 潛在動(dòng)態(tài)物體檢測(cè)實(shí)驗(yàn)

圖9 所示為本文所提AFNET 與YOLOv5 的熱力圖效果對(duì)比，由圖可知，YOLOv5 未使用聚合注意力模塊，難以對(duì)外觀缺失行人形成有效聚焦。 本文所提聚合注意力模塊，對(duì)目標(biāo)物體有所側(cè)重，改善了對(duì)目標(biāo)區(qū)域的關(guān)注程度，提升了對(duì)目標(biāo)物體的聚焦能力。

圖9 熱力圖對(duì)比實(shí)驗(yàn)

在動(dòng)態(tài)場(chǎng)景中，物體外觀缺失會(huì)造成目標(biāo)檢測(cè)網(wǎng)絡(luò)發(fā)生漏檢測(cè)現(xiàn)象。 圖10 所示為本文算法和其他各種算法在物體外觀嚴(yán)重缺失和物體外觀輕微缺失狀態(tài)下的檢測(cè)結(jié)果對(duì)比。 由于YOLOv4 與YOLOv5 缺少對(duì)外觀缺失目標(biāo)區(qū)域特征增強(qiáng)網(wǎng)絡(luò)層，導(dǎo)致在物體外觀缺失情況下易造成目標(biāo)檢測(cè)失效。 與前兩種算法相比，AFNET 設(shè)計(jì)了聚合注意力模塊且進(jìn)行了空間特征自適應(yīng)融合，使AFNET 專注外觀缺失區(qū)域信息，同時(shí)抑制無用信息，從而提升對(duì)外觀缺失物體的目標(biāo)檢測(cè)效果。

圖10 潛在動(dòng)態(tài)物體檢測(cè)對(duì)比

表2 為AFNET 與不同算法性能對(duì)比表，采用全類平均精度mAP(交并比＝0.5:0.95)、平均精度AP50(交并比＝0.5)、AP75(交并比＝0.75)，COCO 數(shù)據(jù)集定義的小中大目標(biāo)對(duì)應(yīng)的mAP:APS(目標(biāo)面積＜322像素)、APM(322像素＜目標(biāo)面積＜962像素)、APL(目標(biāo)面積＞962像素)，作為模型精度的評(píng)價(jià)標(biāo)準(zhǔn)。 由表中數(shù)據(jù)可知，AFNET 算法mAP 與YOLOv4和YOLOv5 算法相比提高了12.2%、5.5%。

表2 AFNET 與不同算法的性能對(duì)比

4.2 動(dòng)態(tài)點(diǎn)剔除實(shí)驗(yàn)

圖11 所示為特征點(diǎn)提取消融實(shí)驗(yàn)圖，矩形方框?yàn)楸疚妮o助框。 由圖可知，雖然ORB-SLAM2 算法使用了BA 優(yōu)化算法，可將少量外點(diǎn)剔除，但在高動(dòng)態(tài)場(chǎng)景下，ORB-SLAM2 算法明顯無法繼續(xù)剔除動(dòng)態(tài)點(diǎn)。 圖11(b)中僅采用DBSCAN 算法將人身上的特征點(diǎn)進(jìn)行聚類并標(biāo)記為動(dòng)態(tài)點(diǎn)，但難以區(qū)分物體的運(yùn)動(dòng)狀態(tài)，如框中靜態(tài)物體上的特征點(diǎn)被標(biāo)記為動(dòng)態(tài)點(diǎn)。 圖11(c)僅采用雙重靜態(tài)點(diǎn)約束方法可以將物體的特征點(diǎn)正確歸類為靜態(tài)點(diǎn)和動(dòng)態(tài)點(diǎn)，但僅使用該算法實(shí)時(shí)性不足。 本文算法結(jié)合語義信息，先使用DBSCAN 密度聚類區(qū)分動(dòng)態(tài)點(diǎn)和靜態(tài)點(diǎn)，然后采用雙重靜態(tài)點(diǎn)約束進(jìn)一步確定特征點(diǎn)的真實(shí)運(yùn)動(dòng)狀態(tài)。 因此本文算法能對(duì)潛在動(dòng)態(tài)物體進(jìn)行精準(zhǔn)判斷并剔除動(dòng)態(tài)物體，且將矩形框內(nèi)在當(dāng)前場(chǎng)景為靜態(tài)的特征點(diǎn)正確保留下來，不僅增加了參與位姿計(jì)算的特征點(diǎn)個(gè)數(shù)，還避免了由于大量使用雙重靜態(tài)點(diǎn)約束導(dǎo)致系統(tǒng)實(shí)時(shí)性不足的問題。

圖11 特征點(diǎn)提取消融實(shí)驗(yàn)

表3 所示為本文算法僅采用DBSCAN、本文算法僅采用雙重靜態(tài)點(diǎn)約束和本文算法在TUM 序列的絕對(duì)軌跡誤差(ATE)和時(shí)間對(duì)比。 由表中數(shù)據(jù)可知，由于DBSCAN 不能判斷物體的運(yùn)動(dòng)狀態(tài)，所以ATE 較大，但該算法實(shí)時(shí)性較好，單幀平均處理時(shí)間17.04 ms。 僅采用雙重靜態(tài)點(diǎn)約束去除動(dòng)態(tài)點(diǎn)ATE 較小，但是實(shí)時(shí)性不足，單幀處理時(shí)間大于60 ms。 本文算法由于先使用BDSCAN 初步區(qū)分動(dòng)態(tài)點(diǎn)，再使用雙重靜態(tài)點(diǎn)約束進(jìn)一步確定特征點(diǎn)的真實(shí)運(yùn)動(dòng)狀態(tài)，不僅ATE 最小，實(shí)時(shí)性也較好。

表3 TUM 數(shù)據(jù)集下三種算法的結(jié)果對(duì)比

4.3 SLAM 系統(tǒng)評(píng)估

本文選取TUM 數(shù)據(jù)集中含有動(dòng)態(tài)場(chǎng)景的fr3/w/half、fr3/w/static 和fr3/w/xyz 序列驗(yàn)證本文算法的有效性。 圖12 所示為ORB-SLAM2、DS-SLAM、DynaSLAM 和本文算法在以上三種序列下生成的軌跡圖。 圖中g(shù)round truth 為相機(jī)真實(shí)軌跡，estimated為相機(jī)估計(jì)軌跡、difference 為軌跡誤差。 由圖可知，由于ORB-SLAM2 算法無法識(shí)別場(chǎng)景中的動(dòng)態(tài)物體，所以建圖效果較差。 DS-SLAM 和DynaSLAM分別通過實(shí)例分割對(duì)動(dòng)態(tài)點(diǎn)進(jìn)行剔除，減小了動(dòng)態(tài)物體對(duì)位姿軌跡的影響。 但無法在物體外觀缺失環(huán)境下準(zhǔn)確識(shí)別潛在動(dòng)態(tài)物體且難以對(duì)潛在動(dòng)態(tài)物體運(yùn)動(dòng)狀態(tài)進(jìn)行判斷，影響系統(tǒng)的定位精度。 本文算法由于融入了AFNET 目標(biāo)檢測(cè)算法，能夠識(shí)別場(chǎng)景中外觀缺失的潛在動(dòng)態(tài)物體，且采用基于雙重靜態(tài)點(diǎn)約束的位姿優(yōu)化進(jìn)一步確定特征點(diǎn)的真實(shí)運(yùn)動(dòng)狀態(tài)，并進(jìn)行二次位姿優(yōu)化，進(jìn)一步提升位姿估計(jì)的精度，減少動(dòng)態(tài)物體對(duì)建圖的影響，生成的軌跡圖更接近真實(shí)軌跡。 因此本文算法軌跡誤差最小，展現(xiàn)出良好的構(gòu)圖能力。

圖12 4 種不同算法軌跡對(duì)比

本文采用標(biāo)準(zhǔn)差(Standard Deviation，SD)和均方根誤差(Root Mean Square Error，RMSE)作為算法的評(píng)價(jià)標(biāo)準(zhǔn)。 其中均方根誤差(RMSE)表示估計(jì)值和真實(shí)值之間的偏差，誤差值越小代表算法所估計(jì)的軌跡越接近真實(shí)值。 而標(biāo)準(zhǔn)差(SD)表示算法所估計(jì)出來軌跡與真實(shí)軌跡的離散程度。 表4~表6分別為絕對(duì)軌跡誤差，相對(duì)平移誤差與相對(duì)旋轉(zhuǎn)誤差。 由表可知，本文算法由于融合AFNET 目標(biāo)檢測(cè)網(wǎng)絡(luò)提高對(duì)外觀缺失物體檢測(cè)精度，并采用雙重靜態(tài)點(diǎn)約束消除了動(dòng)態(tài)物體的影響。 本文算法絕對(duì)軌跡誤差與ORB-SAM2 和DS-SLAM 相比在fr3/w/rpy和fr3/w/static 分別減少 93. 08%、 18. 52%， 與DynaSLAM 算法相比除了fr3/w/rpy 序列增加5.4%外， 在 fr3/w/half 和 fr3/w/xyz 序列分別減少20.28%、10.98%，這是由于fr3/w/rpy 序列相機(jī)不斷進(jìn)行旋轉(zhuǎn)運(yùn)動(dòng)，使得場(chǎng)景中不僅有動(dòng)態(tài)物體，還有自身旋轉(zhuǎn)限制了算法的位姿估計(jì)精度。 此外，這四種算法的靜態(tài)序列fr2/xyz 表現(xiàn)十分接近。

表4 絕對(duì)軌跡誤差

表5 相對(duì)平移誤差

表6 相對(duì)旋轉(zhuǎn)誤差

表7 為ORB-SLAM2、DS-SLAM、DynaSLAM 和本文算法對(duì)單幀圖片處理時(shí)間的對(duì)比結(jié)果。 由表可知，ORB-SLAM2 單幀處理總時(shí)間最短。 DynaSLAM由于使用Mask-RCNN 實(shí)例分割網(wǎng)絡(luò)導(dǎo)致檢測(cè)時(shí)間較長，單幀總時(shí)間大于300ms，DS-SLAM 使用SegNet語義分割處理圖片實(shí)時(shí)性較好，但是單幀處理時(shí)間也在65ms 以上，本文算法由于使用AFNET 目標(biāo)檢測(cè)網(wǎng)絡(luò)大大減少了語義線程檢測(cè)時(shí)間，單幀檢測(cè)時(shí)間僅需15.21 ms，同時(shí)雖然雙重靜態(tài)點(diǎn)約束由于增加了深度和位置判斷單幀處理時(shí)間與運(yùn)動(dòng)一致性判斷相比略微增加，但DBSCAN 每幀處理時(shí)間僅需要1.83 ms，因此本文算法單幀總時(shí)間僅需47.19 ms，即每秒可跟蹤21 幀，能夠滿足視覺SLAM 的實(shí)時(shí)性要求。

表7 單幀處理時(shí)間對(duì)比

4.4 真實(shí)場(chǎng)景測(cè)試

在真實(shí)場(chǎng)景中對(duì)本文算法的有效性進(jìn)行驗(yàn)證，實(shí)驗(yàn)平臺(tái)為Husky 輪式移動(dòng)機(jī)器人，其硬件配置為:CPU i7-10875H 處理器，內(nèi)存8G，GPU 為GTX1080，操作系統(tǒng)為Ubuntu18.04。 圖13(a)、圖13(b)、圖13(c)所示分別為機(jī)器人硬件外觀、真實(shí)場(chǎng)景實(shí)驗(yàn)環(huán)境和真實(shí)場(chǎng)景平面布局圖。 表8 為Husky 輪式機(jī)器人的主要參數(shù)設(shè)置。

表8 主要參數(shù)設(shè)置

圖13 實(shí)驗(yàn)平臺(tái)及真實(shí)實(shí)驗(yàn)環(huán)境場(chǎng)景

4.4.1 潛在動(dòng)態(tài)物體檢測(cè)實(shí)驗(yàn)

圖14 所示為移動(dòng)機(jī)器人在運(yùn)行過程中獲取的兩幀圖像，其中圖14(a)為物體外觀嚴(yán)重缺失下的目標(biāo)檢測(cè)效果，圖14(b)為物體外觀輕微缺失下的目標(biāo)檢測(cè)效果。 由圖可知，本文算法AFNET 在物體外觀嚴(yán)重和輕微缺失情況下對(duì)動(dòng)態(tài)物體檢測(cè)準(zhǔn)確，驗(yàn)證了本文算法對(duì)潛在動(dòng)態(tài)物體檢測(cè)的有效性。

圖14 目標(biāo)檢測(cè)效果圖

4.4.2 動(dòng)態(tài)點(diǎn)剔除實(shí)驗(yàn)

圖15 所示為動(dòng)態(tài)點(diǎn)剔除效果圖。 其中圖15(a)為物體外觀嚴(yán)重缺失下的動(dòng)態(tài)點(diǎn)剔除效果，圖15(b)為物體外觀輕微缺失下的動(dòng)態(tài)點(diǎn)剔除效果。 由圖可知，本文算法通過對(duì)潛在動(dòng)態(tài)物體進(jìn)行檢測(cè)，然后對(duì)動(dòng)態(tài)點(diǎn)進(jìn)行有效剔除，增加系統(tǒng)的魯棒性。

圖15 動(dòng)態(tài)點(diǎn)剔除效果

4.4.3 軌跡地圖構(gòu)建

圖16 為本文算法和ORB-SLAM2 算法在真實(shí)場(chǎng)景中的軌跡對(duì)比圖。 由圖可知，在動(dòng)態(tài)場(chǎng)景中，本文算法的軌跡圖與ORB-SLAM2 算法相比更接近于真實(shí)軌跡。 本文算法由于加入AFNET 目標(biāo)檢測(cè)算法檢測(cè)場(chǎng)景中的潛在動(dòng)態(tài)物體，利用基于雙重靜態(tài)點(diǎn)約束的位姿優(yōu)化方法精確剔除動(dòng)態(tài)點(diǎn)，并進(jìn)行二次位姿優(yōu)化。 因此本文算法在動(dòng)態(tài)場(chǎng)景下魯棒性較好。

圖16 兩種算法軌跡對(duì)比

5 結(jié)論

為提高移動(dòng)機(jī)器人在動(dòng)態(tài)場(chǎng)景中的定位精度，本文提出一種動(dòng)態(tài)場(chǎng)景下基于注意力機(jī)制與幾何約束的VSLAM 算法，該算法具有以下優(yōu)點(diǎn)。 ①針對(duì)YOLOv5 目標(biāo)檢測(cè)網(wǎng)絡(luò)易出現(xiàn)動(dòng)態(tài)物體檢測(cè)不完整的問題，提出一種聚合注意力自適應(yīng)特征融合的目標(biāo)檢測(cè)網(wǎng)絡(luò)AFNET，提高對(duì)物體的檢測(cè)能力，減少漏檢測(cè)。 ②提出一種雙重靜態(tài)點(diǎn)約束的方法解決潛在動(dòng)態(tài)點(diǎn)的剔除問題，為位姿估計(jì)提供高質(zhì)量的靜態(tài)點(diǎn)，提升了系統(tǒng)的精度。 在公開的TUM 數(shù)據(jù)集和真實(shí)場(chǎng)景下對(duì)本文算法進(jìn)行了豐富的對(duì)比實(shí)驗(yàn)，結(jié)果表明本文算法與ORB-SLAM2、DS-SLAM 和DynaSLAM 相比在定位精度方面具有較大優(yōu)勢(shì)，并體現(xiàn)出了良好的構(gòu)圖能力。 下一步將在本文基礎(chǔ)上，融合慣性測(cè)量單元(IMU)數(shù)據(jù)，為低紋理環(huán)境下的相機(jī)位姿求解添加約束項(xiàng)，進(jìn)一步提升算法精度和魯棒性。