伍子嘉，陳 航，彭 勇，宋 威

（1.江南大學(xué) 物聯(lián)網(wǎng)工程學(xué)院，江蘇 無錫 214122；2.江南大學(xué) 人工智能與計(jì)算機(jī)學(xué)院，江蘇 無錫 214122）

0 概述

同步定位與地圖構(gòu)建（Simutaneous Localization and Mapping，SLAM）是指機(jī)器人在未知環(huán)境中通過自身攜帶的傳感器進(jìn)行環(huán)境感知和周圍環(huán)境的三維重建。機(jī)器人搭載的傳感器常見的有相機(jī)、激光傳感器、慣性傳感器，由于相機(jī)采集的圖像具有更豐富的色彩紋理信息，且相機(jī)價(jià)格低廉、體積小、功耗較低，所以應(yīng)用較廣泛。由相機(jī)作為主要傳感器的系統(tǒng)被稱為視覺SLAM 系統(tǒng)［1］。

現(xiàn)有的視覺SLAM 系統(tǒng)主要分為依靠特征點(diǎn)進(jìn)行匹配并構(gòu)建稀疏地圖的特征點(diǎn)法，及利用最小化光度誤差構(gòu)建稠密地圖的直接法。CAMPOS 等［2］在ARTAL 等［3］的基礎(chǔ)上發(fā)布了基于特征點(diǎn)法的ORBSLAM3 方法，這是特征點(diǎn)法的代表。文獻(xiàn)［4］中的DTAM 系統(tǒng)使用直接法實(shí)現(xiàn)實(shí)時(shí)稠密三維重建，文獻(xiàn)［5］中的LSD-SLAM 系統(tǒng)利用圖像灰度實(shí)現(xiàn)定位和半稠密點(diǎn)云地圖構(gòu)建。直接法的實(shí)現(xiàn)是基于同一像素點(diǎn)的灰度值在各個(gè)圖像中固定不變這一假設(shè)，但由于光照變化、相機(jī)自動(dòng)曝光等因素的影響，這一假設(shè)很難實(shí)現(xiàn)，因此基于直接法的SLAM 系統(tǒng)穩(wěn)定性較差。

將視覺SLAM 系統(tǒng)與深度學(xué)習(xí)技術(shù)相結(jié)合可以有效利用圖像中的語義信息，提高系統(tǒng)的定位建圖精度。文獻(xiàn)［6］的SLAM++系統(tǒng)可以將硬件采集到的點(diǎn)云信息與事先構(gòu)建好的高質(zhì)量3D 模型數(shù)據(jù)庫進(jìn)行對(duì)比，將匹配成功的物體模型插入地圖中，但該系統(tǒng)的缺點(diǎn)是只能構(gòu)建數(shù)據(jù)庫中存在的物體模型。文獻(xiàn)［7］提出將目標(biāo)檢測(cè)網(wǎng)絡(luò)與傳統(tǒng)的單目SLAM相結(jié)合，令目標(biāo)檢測(cè)框結(jié)合先驗(yàn)知識(shí)選出場(chǎng)景中的動(dòng)態(tài)物體，并將邊界框內(nèi)的特征點(diǎn)全部剔除。但由于目標(biāo)檢測(cè)框框選出的范圍大于動(dòng)態(tài)物體的范圍，因此會(huì)造成動(dòng)態(tài)目標(biāo)周圍的靜態(tài)特征點(diǎn)也被誤剔除，導(dǎo)致系統(tǒng)精度下降。DynaSLAM 系統(tǒng)［8］將語義分割網(wǎng)絡(luò)MASK-RCNN［9］與多視圖幾何相結(jié)合，實(shí)現(xiàn)對(duì)動(dòng)態(tài)目標(biāo)的檢測(cè)和剔除。文獻(xiàn)［10］提出DS-SLAM系統(tǒng)，在ORB-SLAM2 的基礎(chǔ)上結(jié)合了SegNet 語義分割網(wǎng)絡(luò)［11］，去除了環(huán)境中運(yùn)動(dòng)的人的影響，同時(shí)可以構(gòu)建語義八叉樹地圖，但是語義分割比較耗時(shí)，無法達(dá)到實(shí)時(shí)性的效果。

針對(duì)現(xiàn)有視覺SLAM 系統(tǒng)在動(dòng)態(tài)環(huán)境下普遍存在精度較低或?qū)崟r(shí)性較差的問題，本文提出一種面向動(dòng)態(tài)環(huán)境下的視覺SLAM 系統(tǒng)，通過融合輕量級(jí)YOLOv5s 網(wǎng)絡(luò)，減小模型的大小。此外，將目標(biāo)檢測(cè)網(wǎng)絡(luò)與光流法相結(jié)合，剔除SLAM 系統(tǒng)前端的動(dòng)態(tài)特征點(diǎn)，提高系統(tǒng)在動(dòng)態(tài)環(huán)境下的定位精度。

1 系統(tǒng)框架

1.1 ORB-SLAM 系統(tǒng)

ORB-SLAM 系統(tǒng)的推出代表著主流特征點(diǎn)法SLAM的高峰，包括ORB-SLAM2與ORB-SLAM3系統(tǒng)。

1.1.1 ORB-SLAM3 系統(tǒng)

相比ORB-SLAM2 系統(tǒng)，ORB-SLAM3 系統(tǒng)融合了IMU 傳感器，提升了整個(gè)SLAM 系統(tǒng)的定位及建圖精度。如圖1 所示，ORB-SLAM3 系統(tǒng)具有跟蹤、局部建圖、閉環(huán)檢測(cè)3 個(gè)并行線程。跟蹤線程負(fù)責(zé)ORB 特征點(diǎn)的提取和匹配，通過最小化重投影誤差來估計(jì)2 幀間的相對(duì)位姿［2］。局部建圖線程負(fù)責(zé)對(duì)得到的關(guān)鍵幀進(jìn)行管理，通過集束調(diào)整（Bundle Adjustment，BA）來優(yōu)化局部地圖中所有幀的位姿。閉環(huán)檢測(cè)線程通過關(guān)鍵幀檢測(cè)整個(gè)地圖是否發(fā)生閉合的回環(huán)，并通過位姿圖優(yōu)化不斷矯正累積的漂移誤差。當(dāng)通過詞袋模型檢測(cè)到閉環(huán)后，會(huì)開啟第4線程進(jìn)行全局優(yōu)化。

圖1 ORB-SLAM3 系統(tǒng)線程與結(jié)構(gòu)Fig.1 Threads and structure of ORB-SLAM3 system

1.1.2 改進(jìn)的ORB-SLAM 系統(tǒng)

為提升ORB-SLAM 系統(tǒng)在動(dòng)態(tài)環(huán)境下的精度與魯棒性，本文在該系統(tǒng)原有的3 個(gè)并行線程的基礎(chǔ)上增加了目標(biāo)檢測(cè)模塊與光流模塊作為第4 個(gè)線程。

在傳統(tǒng)的跟蹤線程中如果提取的ORB 特征點(diǎn)一直處于運(yùn)動(dòng)狀態(tài)，那么這些點(diǎn)在特征點(diǎn)匹配計(jì)算位姿的過程中將會(huì)不斷給系統(tǒng)帶來積累誤差，最終導(dǎo)致定位失敗。如圖2 所示，在跟蹤線程中加入目標(biāo)檢測(cè)與光流線程，檢測(cè)并剔除動(dòng)態(tài)目標(biāo)上的動(dòng)態(tài)ORB 特征點(diǎn)，使其不參與相機(jī)位姿計(jì)算，僅留下靜態(tài)特征點(diǎn)參與位姿計(jì)算。

圖2 傳統(tǒng)的跟蹤線程與改進(jìn)的跟蹤線程Fig.2 Traditional trace threads and improved trace threads

1.2 目標(biāo)檢測(cè)網(wǎng)絡(luò)Mobilenetv3-YOLOv5s

在結(jié)合深度學(xué)習(xí)的SLAM 方法中，實(shí)時(shí)性尤為重要。本文首先考慮將基于像素的語義分割方法與SLAM 系統(tǒng)相結(jié)合，但實(shí)驗(yàn)發(fā)現(xiàn)基于像素的語義分割方法難以實(shí)時(shí)運(yùn)行。因此，本文引入基于深度學(xué)習(xí)的目標(biāo)檢測(cè)網(wǎng)絡(luò)來檢測(cè)環(huán)境中潛在的運(yùn)動(dòng)對(duì)象，通過融合輕量級(jí)網(wǎng)絡(luò)進(jìn)一步改進(jìn)，該方法以較高的運(yùn)行速度滿足了SLAM 系統(tǒng)的實(shí)時(shí)性要求。但單獨(dú)的目標(biāo)檢測(cè)網(wǎng)絡(luò)的方形目標(biāo)檢測(cè)框通常會(huì)框選出不屬于動(dòng)態(tài)目標(biāo)上的特征點(diǎn)，本文通過引入光流法可以有效解決該問題。

1.2.1 目標(biāo)檢測(cè)網(wǎng)絡(luò)YOLOv5s

YOLO 是一種基于回歸的目標(biāo)檢測(cè)算法，是目前應(yīng)用最廣泛的目標(biāo)檢測(cè)算法之一。它將目標(biāo)檢測(cè)作為回歸問題求解，直接從一張輸入圖片獲得預(yù)測(cè)物體的邊界框位置及其分類，在保證準(zhǔn)確率的同時(shí)兼顧了實(shí)時(shí)性，在速度與精度上達(dá)到了良好平衡。YOLOv5 目前仍在迭代更新，包含YOLOv5s、YOLOv5m、YOLOv5l、YOLO5x 共4 個(gè)版本。不同版本之間的主要區(qū)別在于深度及特征圖的寬度不同，可以很方便地在其配置文件中通過調(diào)整depth_multiple 與width_multiple 兩個(gè)參數(shù)的大小進(jìn)行配置。

YOLOv5s 網(wǎng)絡(luò)是YOLOv5 系列中深度、特征圖寬度最小的網(wǎng)絡(luò)。其模型參數(shù)量約為7.5M，在V100GPU 上，每張圖片推理速度僅為0.002 s，即幀率為500 frame/s，完全滿足實(shí)時(shí)檢測(cè)的需求。

與先前的目標(biāo)檢測(cè)網(wǎng)絡(luò)相比，YOLOv5 對(duì)整個(gè)網(wǎng)絡(luò)的各個(gè)部分均作出了改進(jìn)，包括在輸入端采用Mosaic 數(shù)據(jù)增強(qiáng)、自適應(yīng)錨框計(jì)算、自適應(yīng)圖片縮放，在Backbone 中引入Focus 結(jié)構(gòu)與CSP 結(jié) 構(gòu)，在Neck 中使用FPN+PAN 的結(jié)構(gòu)，在輸出端中采用GIOU_Loss 作為目標(biāo)檢測(cè)框的損失函數(shù)，同時(shí)引入了NMS 非極大值抑制機(jī)制。

1.2.2 輕量級(jí)網(wǎng)絡(luò)MobileNet-V3

傳統(tǒng)的卷積神經(jīng)網(wǎng)絡(luò)對(duì)內(nèi)存需求大、運(yùn)算量大，導(dǎo)致其無法在移動(dòng)設(shè)備上實(shí)時(shí)運(yùn)行。如VGG16 網(wǎng)絡(luò)的權(quán)重為490M，Resnet-152 層網(wǎng)絡(luò)的權(quán)重有644M。MobileNet 網(wǎng)絡(luò)［13］是專注于移動(dòng)端設(shè)備的輕量級(jí)CNN 網(wǎng)絡(luò)，相比于傳統(tǒng)的卷積神經(jīng)網(wǎng)絡(luò)，在準(zhǔn)確率小幅降低的前提下大幅減少了模型參數(shù)與運(yùn)算量。在Imagenet 數(shù)據(jù)集上相比VGG16 網(wǎng)絡(luò)的準(zhǔn)確率減少了0.9%，但模型參數(shù)只有VGG16 網(wǎng)絡(luò)的1/32。其參數(shù)量大幅減小的原因主要是其引入了深度（Depthwise，DW）卷積、逐點(diǎn)（Pointwise，PW）卷積。如圖3 所示，DW 卷積與PW 卷積相結(jié)合構(gòu)成了深度可分離卷積（Depthwise Separable Convolution，DSC）。圖3 中，DF表示輸入特征矩陣的高和寬；M表示輸入特征矩陣的深度；DK表示卷積核的大??；N為輸出特征矩陣的深度；默認(rèn)卷積步距為1。MobileNet 網(wǎng)絡(luò)中卷積核大小一般為3×3，所以理論上普通卷積計(jì)算量是深度可分離卷積的8～9 倍。

圖3 深度可分離卷積Fig.3 Deep separable convolution

普通卷積的計(jì)算量與深度可分離卷積的計(jì)算量之比如式（1）所示：

MobileNet-V1 被推出 后，Google 公司先 后推出了MobileNet-V2［14］和MobileNet-V3。圖4 所示的MobileNet-V3 網(wǎng)絡(luò)相比前代最大的特點(diǎn)首先是使用了神經(jīng)網(wǎng)絡(luò)架構(gòu)搜索（Neural Architecture Search，NAS）技術(shù)，其次是更新了Block，使用了h-swish 激活函數(shù)，在深層網(wǎng)絡(luò)中，這可以減小計(jì)算量，提高網(wǎng)絡(luò)精度。同時(shí)，引入基于壓縮獎(jiǎng)懲網(wǎng)絡(luò)（Squeeze and Excitation，SE）模塊，注意力機(jī)制能夠在訓(xùn)練過程中在特征圖上自行分配權(quán)重，使網(wǎng)絡(luò)從全局信息出發(fā)選擇性地放大有價(jià)值的特征通道，并能抑制無用的特征通道用以提高網(wǎng)絡(luò)的性能。最后，MobileNet-V3 還減少了第1 個(gè)卷積層的卷積核個(gè)數(shù)，精簡(jiǎn)了Last Stage。

圖4 MobileNet-V3 網(wǎng)絡(luò)的結(jié)構(gòu)Fig.4 Structure of MobileNet-V3 network

1.2.3 輕量級(jí)目標(biāo)檢測(cè)網(wǎng)絡(luò)MobileNetV3-YOLOv5s

為實(shí)現(xiàn)YOLOv5s 網(wǎng)絡(luò)的輕量化，本文采用MobileNetV3-Small 取 代YOLOv5s 的Backbone 特 征提取網(wǎng)絡(luò)，MobileNetV3-Small 網(wǎng)絡(luò)結(jié)構(gòu)如表1所示。

表1 MobileNetV3-Small 網(wǎng)絡(luò)結(jié)構(gòu)Table 1 MobileNetV3-Small network structure

如表1 所示，MobileNetV3-Small 的網(wǎng)絡(luò)結(jié)構(gòu)中有多個(gè)含有卷積層的塊。其中：conv2d 表示正常的卷積層；bneck 即MobileBlock，表示卷積層，BN 層和激活函數(shù)的組合；NBN 表示不使用BN 層；SE 表示是否使用注意力模塊；NL 表示非線性激活函數(shù)的類型；HS 表示h-swish 激活函數(shù)；RE 表示ReLU 激活函數(shù)；S表示步長(zhǎng)；K參數(shù)表示這一層是模型中的第幾個(gè)Block，相對(duì)輸入的縮放是多少，“—”表示該層并沒有被使用到，所以并無參數(shù)。

將MobileNetV3-Small 取 代YOLOv5s 的Backbone特征提取網(wǎng)絡(luò)，并將各層所需特征圖調(diào)整對(duì)齊，YOLOv5s 的Neck 所需的3 種不同尺度的特征圖分別從3-P3/8、8-P4/16、11-P5/32 處引出，如 圖5所示。

圖5 改進(jìn)的YOLOv5s BackboneFig.5 Backbone of improved YOLOv5s

1.3 ORB 特征點(diǎn)與光流法

1.3.1 ORB 特征點(diǎn)提取

常見的特征點(diǎn)有SIFT［15］、SURF［16］、ORB［17］等，這些特征點(diǎn)有的很精確，有的在光照變化下仍具有相似的表達(dá)，但實(shí)用的SLAM 系統(tǒng)主要考慮實(shí)時(shí)性問題。在同一幅圖像中同時(shí)提取1 000 個(gè)特征點(diǎn)，SIFT 約花費(fèi)5 228.7ms，SURF 約花費(fèi)217.3 ms，ORB約花費(fèi)15.3 ms［18］。由此可以看出，ORB 特征點(diǎn)在速度方面提升明顯，對(duì)于實(shí)時(shí)性要求很高的SLAM 系統(tǒng)來說是一個(gè)很好的選擇。

ORB-SLAM 系統(tǒng)在提取特征點(diǎn)時(shí)，建立了一個(gè)8 層的圖像金字塔來實(shí)現(xiàn)尺度不變性，每層之間的比例為1.2 倍，在金字塔的每一層上檢測(cè)特征點(diǎn)，將每一層的ORB 特征點(diǎn)均檢測(cè)完畢后，會(huì)將所有特征點(diǎn)按比例投影至最底層。

1.3.2 光流法

光流法是一種描述像素在不同圖像幀之間運(yùn)動(dòng)狀態(tài)的方法。在動(dòng)態(tài)目標(biāo)上，同一個(gè)像素會(huì)在圖像中運(yùn)動(dòng)，通過光流法可以追蹤固定像素的運(yùn)動(dòng)過程。其中，描述整幅圖像中部分像素運(yùn)動(dòng)狀態(tài)的稱為稀疏光流，描述所有像素運(yùn)動(dòng)狀態(tài)的稱為稠密光流。稠密光流以Hom-Schunck 光流為代表。稀疏光流以Lucas-Kanade［19］光流為主，也稱為L(zhǎng)K 光流。本文為減少計(jì)算量，只計(jì)算SLAM 系統(tǒng)的視覺里程計(jì)提取到的ORB 特征點(diǎn)的光流場(chǎng)，所以采用了LK 光流。

LK 光流的成立基于3 種假設(shè)：圖像中的像素亮度（灰度）在連續(xù)幀間不會(huì)發(fā)生變化；幀間的時(shí)間間隔相對(duì)較短；相鄰像素具有相似的運(yùn)動(dòng)。如圖6 所示，圖像的灰度可以視為時(shí)間的函數(shù)：在t時(shí)刻，位于圖像中（x，y）處的ORB 特征點(diǎn)的灰度可以寫為I（x，y，t）。

圖6 LK 光流Fig.6 LK optical flow

根據(jù)光流法的灰度不變假設(shè)：同一個(gè)特征點(diǎn)的灰度值在各個(gè)圖像中是固定不變的。對(duì)于t時(shí)刻位于（x，y）處的特征點(diǎn)，在t+dt時(shí)刻將移動(dòng)至（x+dx，y+dy）處。

根據(jù)灰度不變假設(shè)可得：

對(duì)式（2）等號(hào)左側(cè)進(jìn)行泰勒展開，保留一階項(xiàng)，得到式（3）：

根據(jù)灰度不變假設(shè)，特征點(diǎn)在t與t+dt時(shí)刻的灰度值相等，可得：

兩邊同時(shí)除以dt可得：

式（5）中dx/dt為特征點(diǎn)在X軸上的運(yùn)動(dòng)速度，而dy/dt為在Y軸上的速度，分別記為u、v。?I/?x為圖像在該點(diǎn)處X軸方向的梯度，?I/?y則是在Y軸方向的梯度，分別記為Ix、Iy。把特征點(diǎn)灰度對(duì)時(shí)間的變化量記為It，將其寫成矩陣形式，如式（6）所示：

僅憑式（6）無法計(jì)算出特征點(diǎn)的速度u、v，必須引入額外的約束。在LK 光流中，以特征點(diǎn)為中心假設(shè)一個(gè)6×6 的窗口，內(nèi)部36 個(gè)像素具有相同的運(yùn)動(dòng)。此方程是1 個(gè)超定方程，常用最小二乘法求解，如果一次迭代效果不佳，可以多迭代幾次即可求解u、v。

將圖像金字塔中所有的特征點(diǎn)向下迭代進(jìn)行光流計(jì)算，每個(gè)特征點(diǎn)的光流信息（u，v）將會(huì)被記錄到ORB-SLAM 系統(tǒng)內(nèi)部記錄特征點(diǎn)信息的結(jié)構(gòu)體中，并用于下一步動(dòng)態(tài)ORB 特征點(diǎn)的剔除。

1.4 運(yùn)動(dòng)目標(biāo)檢測(cè)與動(dòng)態(tài)ORB 特征點(diǎn)的剔除

之所以要將目標(biāo)檢測(cè)網(wǎng)絡(luò)與光流法相結(jié)合，一方面是因?yàn)閮H從目標(biāo)檢測(cè)的結(jié)果來看，只能基于先驗(yàn)知識(shí)間接判斷物體是否運(yùn)動(dòng)。對(duì)于靜止的汽車、貼在墻上的人像畫等此類目標(biāo)，上面的特征點(diǎn)均會(huì)被識(shí)別成動(dòng)態(tài)特征點(diǎn)。另一方面是因?yàn)槟繕?biāo)檢測(cè)網(wǎng)絡(luò)檢測(cè)出來的區(qū)域是一個(gè)規(guī)則的方形區(qū)域，而實(shí)際生活中的動(dòng)態(tài)目標(biāo)常常是一個(gè)邊界不規(guī)則物體。如果粗暴地直接將目標(biāo)檢測(cè)框中圈出的目標(biāo)區(qū)域內(nèi)特征點(diǎn)全部去除的話，會(huì)刪除一部分靜態(tài)特征點(diǎn)，導(dǎo)致SLAM 系統(tǒng)匹配出的特征點(diǎn)數(shù)量大幅減少，影響定位精度。當(dāng)動(dòng)態(tài)目標(biāo)占據(jù)較大畫幅時(shí)甚至?xí)?dǎo)致初始化失敗或者跟蹤失敗。

MobileNetV3-YOLOv5s目標(biāo)檢測(cè)網(wǎng)絡(luò)對(duì)圖像進(jìn)行推理后生成的預(yù)測(cè)框由位置參數(shù)x、y、w、h以及置信度和分類結(jié)果6 個(gè)參數(shù)組成。x、y分別為預(yù)測(cè)框的中心與原圖像的相對(duì)值，w和h表示預(yù)測(cè)框的長(zhǎng)和寬與原圖像的相對(duì)值。置信度代表該預(yù)測(cè)框內(nèi)包含某物體的可信度以及預(yù)測(cè)框位置的準(zhǔn)確度。分類結(jié)果則根據(jù)訓(xùn)練時(shí)使用的數(shù)據(jù)集所包含的物體類別決定。

如圖7 所示，以預(yù)測(cè)框A為例，本文首先將其位置參數(shù)x、y、w、h轉(zhuǎn)換為預(yù)測(cè)框左上方和右下方頂點(diǎn)在原圖像下的坐標(biāo)，分別設(shè)為（XA1，YA1），（XA2，YA2）（在原圖像中，左上方頂點(diǎn)為坐標(biāo)原點(diǎn)，向右設(shè)為X軸正方向，向下設(shè)為Y軸正方向）。

圖7 特征點(diǎn)分布示意圖Fig.7 Distribution schematic diagram of feature points

將目標(biāo)檢測(cè)網(wǎng)絡(luò)輸出的參數(shù)轉(zhuǎn)換為目標(biāo)檢測(cè)框在原圖像下坐標(biāo)的公式，如式（8）所示：

其中：l為原圖像的寬；d為原圖像的高。

預(yù)測(cè)框內(nèi)所包含的ORB 特征點(diǎn)均是基于先驗(yàn)知識(shí)判斷的待定動(dòng)態(tài)特征點(diǎn)，預(yù)測(cè)框之外均為靜態(tài)特征點(diǎn)。首先假設(shè)所有的特征點(diǎn)集合為P=｛p1，p2，…，pn｝，待定動(dòng)態(tài)特征點(diǎn)集合R=｛r1，r2，…，rn｝，靜態(tài)特征點(diǎn)集合為O=｛o1，o2，…，on｝，P=R∪O。集合R內(nèi)所有特征點(diǎn)都將參與動(dòng)態(tài)特征點(diǎn)的篩選，每個(gè)特征點(diǎn)的坐標(biāo)信息（x，y）由ORB-SLAM 系統(tǒng)前端計(jì)算所得，在X與Y軸方向上的速度信息（u，v）由光流法計(jì)算所得。由于在實(shí)際應(yīng)用場(chǎng)景中，不僅圖像中的動(dòng)態(tài)目標(biāo)運(yùn)動(dòng)會(huì)產(chǎn)生光流信息，畫面的背景（靜態(tài)特征點(diǎn)）也會(huì)隨著相機(jī)本身的運(yùn)動(dòng)產(chǎn)生光流信息，所以需要先依照式（9）計(jì)算集合O內(nèi)所有靜態(tài)特征點(diǎn)的平均運(yùn)動(dòng)速度，即背景運(yùn)動(dòng)速度。

其中：U和V分別為在X與Y軸方向上的平均運(yùn)動(dòng)速度。

利用背景運(yùn)動(dòng)速度可以對(duì)集合R內(nèi)所有待定動(dòng)態(tài)特征點(diǎn)進(jìn)行篩選，如式（10）所示。

其中：l為判斷集合內(nèi)特征點(diǎn)是否為動(dòng)態(tài)特征點(diǎn)的自適應(yīng)閾值，如果大于該閾值則判定為動(dòng)態(tài)特征點(diǎn)，否則判定為靜態(tài)特征點(diǎn)。動(dòng)態(tài)特征點(diǎn)在計(jì)算位姿時(shí)將會(huì)全部被刪除，不參與重投影計(jì)算位姿。

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

本文采用慕尼黑工業(yè)大學(xué)提供的RGB-D數(shù)據(jù)集系列中5個(gè)數(shù)據(jù)集進(jìn)行測(cè)試，分別為sitting_xyz、walking_static、walking_xyz、walking_halfsphere、walking_rpy。該系列分為高動(dòng)態(tài)場(chǎng)景和低動(dòng)態(tài)場(chǎng)景。在高動(dòng)態(tài)場(chǎng)景中，人在場(chǎng)景中會(huì)持續(xù)行走；在低動(dòng)態(tài)場(chǎng)景中，人坐在椅子上，沒有明顯的動(dòng)作。數(shù)據(jù)集名稱中的static、xyz、halfsphere、rpy分別代表4種不同的相機(jī)運(yùn)動(dòng)方式：相機(jī)基本靜止；相機(jī)沿X、Y、Z軸移動(dòng)；相機(jī)在直徑為1 m 的半球表面移動(dòng)以及相機(jī)在翻滾、俯仰和偏航軸上旋轉(zhuǎn)。

數(shù)據(jù)集中的真實(shí)軌跡由一個(gè)運(yùn)動(dòng)捕捉系統(tǒng)獲得，該系統(tǒng)包括多臺(tái)高速攝像機(jī)與慣性測(cè)量系統(tǒng)，能夠?qū)崟r(shí)獲取相機(jī)位置與姿態(tài)的數(shù)據(jù)。因此該數(shù)據(jù)集被大多數(shù)視覺SLAM 研究人員采用，作為評(píng)估視覺SLAM 系統(tǒng)的標(biāo)準(zhǔn)數(shù)據(jù)集之一。

為加快深度學(xué)習(xí)模型的收斂速度，在本實(shí)驗(yàn)中，目標(biāo)檢測(cè)網(wǎng)絡(luò)的訓(xùn)練在一臺(tái)服務(wù)器上運(yùn)行，其CPU為AMD Ryzen 9 3900X，內(nèi)存為128 GB，GPU 為RTX3090，顯存為24 GB。其他所有實(shí)驗(yàn)均運(yùn)行在1臺(tái)PC 上，CPU 為AMD Ryzen 7 5800H，內(nèi)存為16 GB，GPU 為RTX 3060 Laptop，顯存為6 GB。系統(tǒng)環(huán)境為Ubuntu 18.04，采用CUDA 11.1，深度學(xué)習(xí)框架PyTorch 1.9.0 裝載在Anaconda 的虛擬環(huán)境中。目標(biāo)檢測(cè)實(shí)驗(yàn)運(yùn)行軟件為Pycharm。目標(biāo)檢測(cè)網(wǎng)絡(luò)采用Python3.6 編寫，SLAM 部分采用C++編寫。

2.1 光流法實(shí)驗(yàn)

由圖8（a）可知，因?yàn)橄鄼C(jī)本身的運(yùn)動(dòng)導(dǎo)致背景畫面的運(yùn)動(dòng)，使傳統(tǒng)的光流法在并沒有運(yùn)動(dòng)目標(biāo)的情況下也追蹤到了大量的光流信息。由圖8（b）、圖8（c）可知，背景運(yùn)動(dòng)與人物運(yùn)動(dòng)的光流方向不一致，利用該特性，可以有效提取出運(yùn)動(dòng)目標(biāo)的特征點(diǎn)。圖8（d）為經(jīng)過算法篩選后的預(yù)期效果，可以觀察到全部光流信息均來自于運(yùn)動(dòng)的人。

圖8 光流法檢測(cè)動(dòng)態(tài)特征點(diǎn)示例Fig.8 Examples of dynamic feature point detection by optical flow method

2.2 動(dòng)態(tài)目標(biāo)檢測(cè)實(shí)驗(yàn)

目標(biāo)檢測(cè)網(wǎng)絡(luò)采用VOC-2007 數(shù)據(jù)集訓(xùn)練，該數(shù)據(jù)集包含人、公交車、汽車、自行車、椅子在內(nèi)的20 種不同物體，本次訓(xùn)練中訓(xùn)練集包含7 867 張圖片，將MobileNetV3-YOLOv5s 網(wǎng)絡(luò)訓(xùn)練了1 000 個(gè)epoch，batch_size 設(shè)置為128，學(xué)習(xí)率為0.003 2，余弦退火超參數(shù)為0.12，學(xué)習(xí)率動(dòng)量為0.843，權(quán)重衰減系數(shù)為0.000 36。訓(xùn)練過程中利用可視化工具Tensorboard 得到的損失函數(shù)變化如圖9 所示，可以觀察到預(yù)測(cè)框、類別、置信度的損失均在穩(wěn)步下降，為了達(dá)到較好的訓(xùn)練效果，隨后又將batch size 減小并進(jìn)行了訓(xùn)練，直至模型收斂。

圖9 訓(xùn)練過程中損失函數(shù)可視化Fig.9 Visualization of loss function during training

利用MobileNetV3-YOLOv5s 網(wǎng)絡(luò)對(duì)圖片進(jìn)行檢測(cè)，檢測(cè)效果示例如圖10 所示。預(yù)設(shè)動(dòng)態(tài)場(chǎng)景中運(yùn)動(dòng)物體均為人，所以將目標(biāo)檢測(cè)網(wǎng)絡(luò)設(shè)置為只檢測(cè)人，檢測(cè)效果如圖11 所示，可以觀察到在圖11（b）中人物僅露出雙腿的情況下，目標(biāo)檢測(cè)網(wǎng)絡(luò)也能將其準(zhǔn)確框選出來。

圖10 MobileNetV3-YOLOv5s 檢測(cè)效果示例Fig.10 Detection effect examples of MobileNetv3-YOLOv5s

圖11 運(yùn)動(dòng)中的人的檢測(cè)效果示例Fig.11 Detection effect examples of people in motion

對(duì)walking_rpy 數(shù)據(jù)集中的910 張圖片進(jìn)行檢測(cè)推理，不同目標(biāo)檢測(cè)網(wǎng)絡(luò)在GPU 與CPU 上的模型參數(shù)量、算力要求、推理時(shí)間、幀率表現(xiàn)如表2 與表3 所示，所有測(cè)試均進(jìn)行5 次，最終結(jié)果取平均值?？梢杂^察到，利用MobileNetv3-Small 網(wǎng)絡(luò)取代YOLOv5s的Backbone 特征提取網(wǎng)絡(luò)使模型參數(shù)量減少了51.88%，算力要求減少了63.42%，在CPU 上推理速度提升了53.06%。

表2 在GPU 上的實(shí)驗(yàn)結(jié)果Table 2 Experiment results on GPU

表3 在CPU 上的實(shí)驗(yàn)結(jié)果Table 3 Experiment results on CPU

2.3 位姿估計(jì)誤差分析實(shí)驗(yàn)

位姿估計(jì)誤差分析實(shí)驗(yàn)采用evo工具將ORB-SLAM系統(tǒng)所估計(jì)的相機(jī)位姿CameraTrajectory.txt 與數(shù)據(jù)集所給的真實(shí)位姿groundtruth.txt 進(jìn)行測(cè)試對(duì)比。測(cè)試指標(biāo)為絕對(duì)軌跡誤差（Absolute Trajectory Error，ATE），ATE 指標(biāo)直接計(jì)算相機(jī)位姿的真實(shí)值與估計(jì)值之差，可以非常直觀地反映算法精度和軌跡的全局一致性。

實(shí)驗(yàn)中計(jì)算了均方根誤差（Root Mean Squared Error，RMSE）和標(biāo)準(zhǔn)偏差（Standard Deviation，STD）。其中均方根誤差用于描述觀測(cè)值與真實(shí)值的偏差，易受到較大或偶發(fā)錯(cuò)誤的影響，所以能更好地反應(yīng)系統(tǒng)的魯棒性。標(biāo)準(zhǔn)差評(píng)價(jià)估計(jì)軌跡相較于真實(shí)軌跡的離散程度，能夠反映系統(tǒng)的穩(wěn)定性。

實(shí)驗(yàn)將ORB-SLAM3 系統(tǒng)與本文系統(tǒng)在高動(dòng)態(tài)場(chǎng)景中的4 個(gè)數(shù)據(jù)集以及sitting_xyz 數(shù)據(jù)集上進(jìn)行定量比較，計(jì)算時(shí)在旋轉(zhuǎn)、平移與尺度縮放這3 個(gè)尺度上對(duì)齊，結(jié)果如表4 所示，表中每組實(shí)驗(yàn)均進(jìn)行了2 次，最終結(jié)果取平均值。

表4 ORB-SLAM3 與本文系統(tǒng)的絕對(duì)軌跡誤差對(duì)比Table 4 Comparison of absolute trajectory error between ORB-SLAM3 and system in this paper

令ORB-SLAM3 的誤差為m，本文系統(tǒng)的誤差為n，則相對(duì)提升率R的計(jì)算公式為：

由表4 可知，與ORB-SLAM3 系統(tǒng)相比，本文系統(tǒng)在5 個(gè)數(shù)據(jù)集下RMSE 的平均提升率達(dá)80.16%。對(duì)于在walking_static 與walking_rpy 這2 個(gè)數(shù)據(jù)集下的提升效果不如其他3 個(gè)數(shù)據(jù)集這一問題，通過觀察目標(biāo)檢測(cè)網(wǎng)絡(luò)處理后的圖片與系統(tǒng)運(yùn)行時(shí)的實(shí)際狀態(tài)發(fā)現(xiàn)，在walking_static 數(shù)據(jù)集中，有相當(dāng)一部分圖像幀中人物占據(jù)畫幅面積太大，導(dǎo)致檢測(cè)到的靜態(tài)特征點(diǎn)較少。而在walking_rpy 數(shù)據(jù)集中由于圖像大幅旋轉(zhuǎn)且畫面模糊，部分人像未能被目標(biāo)檢測(cè)網(wǎng)絡(luò)框選出來，最終導(dǎo)致了估計(jì)精度下降。

圖12、圖13 所示分別為ORB-SLAM3 系統(tǒng)與本文系統(tǒng)在walking_halfsphere、walking_xyz 兩個(gè)數(shù)據(jù)集下的估計(jì)軌跡與誤差分布圖（彩色效果見《計(jì)算機(jī)工程》官網(wǎng)HTML 版）。

圖12 ORB-SLAM3 系統(tǒng)估計(jì)的軌跡與誤差分布Fig.12 Trajectory and error distribution estimated by ORB-SLAM3 system

圖13 本文系統(tǒng)估計(jì)的軌跡與誤差分布Fig.13 Trajectory and error distribution estimated by improved system

圖12 和圖13 每組圖像中的子圖（a）、圖（c）分別為在walking_halfsphere 數(shù)據(jù)集下的計(jì)算結(jié)果，每組圖像中的子圖（b）、圖（d）分別為在walking_xyz 數(shù)據(jù)集下的計(jì)算結(jié)果，從圖中可以較明顯地觀察到本文系統(tǒng)所估算出的位姿和真實(shí)軌跡十分接近，相比ORB-SLAM3系統(tǒng)，各類誤差值均有一個(gè)數(shù)量級(jí)的減小。

將本文所設(shè)計(jì)的視覺SLAM 系統(tǒng)與其他動(dòng)態(tài)場(chǎng)景下的SLAM 系統(tǒng)進(jìn)行對(duì)比，包括基于語義分割網(wǎng)絡(luò)的DynaSLAM 和DS-SLAM 系統(tǒng)、通過優(yōu)化關(guān)鍵幀之間的位姿圖計(jì)算最小化光度和深度誤差來獲得約束條件的DVO-SLAM［20］、基于光流法的動(dòng)態(tài)SLAM 方 法OFD-SLAM［21］以及MR-SLAM［22］，對(duì) 比結(jié)果如表5 所示，其中“—”表示原論文中并未對(duì)這一數(shù)據(jù)集進(jìn)行測(cè)試，加粗部分代表最佳結(jié)果，次佳結(jié)果下方用下劃線示意，從表中可以發(fā)現(xiàn)最佳與次佳結(jié)果主要來自于DynaSLAM與本文系統(tǒng)。Mask-RCNN 在Nvidia Tesla M40 GPU 上處理每張圖片需要195 ms，相比使用MASK-RCNN 的Dyna SLAM 系統(tǒng)，本文系統(tǒng)實(shí)時(shí)性更高，且ATE 指標(biāo)相差非常小，可以認(rèn)為性能相當(dāng)。

表5 不同系統(tǒng)的絕對(duì)軌跡誤差對(duì)比Table 5 Comparison of absolute trajectory error between different system

3 結(jié)束語

本文提出一種面向動(dòng)態(tài)環(huán)境的視覺SLAM 系統(tǒng)，使用輕量級(jí)網(wǎng)絡(luò)MobileNetV3 作為目標(biāo)檢測(cè)網(wǎng)絡(luò)YOLOv5s 的主干網(wǎng)絡(luò)，以減少模型參數(shù)量和算力需求，提升在CPU 上的推理速度。通過將目標(biāo)檢測(cè)網(wǎng)絡(luò)與光流法相結(jié)合用于ORB-SLAM 系統(tǒng)的前端，剔除動(dòng)態(tài)特征點(diǎn)。在TUM 數(shù)據(jù)集上的實(shí)驗(yàn)結(jié)果表明，相比ORB-SLAM3 系統(tǒng)，本文系統(tǒng)位姿估計(jì)精度提升了80.16%，相比DS-SLAM、DVOSLAM、DynaSLAM 等典型的動(dòng)態(tài)SLAM 系統(tǒng)，本文系統(tǒng)在實(shí)時(shí)性與精度上均有部分提升。下一步將對(duì)網(wǎng)絡(luò)模型進(jìn)行剪枝蒸餾，將網(wǎng)絡(luò)移植到性能更差的移動(dòng)端設(shè)備上，同時(shí)加強(qiáng)對(duì)模型的訓(xùn)練，有效利用目標(biāo)檢測(cè)提取出的語義信息進(jìn)行稀疏語義地圖的構(gòu)建，從而優(yōu)化系統(tǒng)，使其能適用于更高層次的機(jī)器人定位工作。