基于LK 光流與實(shí)例分割的聯(lián)合動(dòng)態(tài)一致性vSLAM 算法

劉 強(qiáng)，袁 杰，匡本發(fā)

（新疆大學(xué) 電氣工程學(xué)院，新疆 烏魯木齊 830017）

0 引言

近年來，機(jī)器人受到廣泛的關(guān)注，投入到越來越多的場景中使用，這對同步定位與建圖（SLAM）技術(shù)提出了新的挑戰(zhàn)[1-3]。SLAM 按照傳感器的不同，可以分為基于激光雷達(dá)的激光SLAM 和基于相機(jī)的視覺SLAM。視覺SLAM 因其傳感器能提供的信息豐富而獲得了較多的關(guān)注，因此眾多的學(xué)者提出了許多優(yōu)秀的SLAM 方案，如：MonoSLAM[4]、PTAM[5]、ORB-SLAM[6]、DTAM[7]和DSO[8]等。

這些算法能在靜態(tài)環(huán)境取得不錯(cuò)的效果，而機(jī)器人的工作環(huán)境趨向于非結(jié)構(gòu)環(huán)境，因此，視覺SLAM 需要能處理場景中的動(dòng)態(tài)物體。

目前，針對動(dòng)態(tài)物體的方法分為三類，分別為基于光流、幾何以及深度學(xué)習(xí)。文獻(xiàn)[9]通過計(jì)算光流的運(yùn)動(dòng)度量的方法來獲取可能的移動(dòng)物體。文獻(xiàn)[10]檢測動(dòng)態(tài)特征采用的是對極幾何與FVB（Flow Vector Bound）約束結(jié)合的方法。隨著深度學(xué)習(xí)的快速發(fā)展，與深度學(xué)習(xí)結(jié)合的方法也迎來了巨大的飛躍。文獻(xiàn)[11-12]通過先驗(yàn)語義信息對動(dòng)態(tài)特征進(jìn)行剔除。文獻(xiàn)[13]提出了基于SegNet[14]的DS-SLAM 算法。文獻(xiàn)[15]提出了基于Mask R-CNN[16]的DynaSLAM 算法。文獻(xiàn)[17]提出了基于目標(biāo)檢測和語義分割的RDTS-SLAM?；谏疃葘W(xué)習(xí)的方法中，在高動(dòng)態(tài)場景中可以有效地剔除動(dòng)態(tài)物體，但其對低動(dòng)態(tài)場景直接利用先驗(yàn)語義信息剔除動(dòng)態(tài)特征反而降低了算法的定位能力。

針對上述問題，本文提出了一種結(jié)合實(shí)例分割與光流的視覺SLAM 算法，能夠?qū)D像中的動(dòng)態(tài)物體與潛在動(dòng)態(tài)物體進(jìn)行運(yùn)動(dòng)狀態(tài)精準(zhǔn)判定，然后再剔除從動(dòng)態(tài)物體處提取的特征點(diǎn)，最后利用靜態(tài)特征點(diǎn)進(jìn)行相機(jī)位姿估計(jì)。

1 算法框架

圖1 為本文算法的系統(tǒng)框架。系統(tǒng)輸入為RGB-D圖像序列，實(shí)例分割線程對RGB 圖像進(jìn)行分割獲取掩碼信息；跟蹤線程收到RGB 圖像后，先對其提取ORB[18]特征，在等待實(shí)例分割掩碼的時(shí)候?qū)ζ溥M(jìn)行稀疏光流處理，然后結(jié)合實(shí)例分割掩碼與先驗(yàn)語義信息對光流信息進(jìn)行處理得到非動(dòng)態(tài)物體、動(dòng)態(tài)物體和潛在動(dòng)態(tài)物體的光流位移增量，再結(jié)合光流信息與實(shí)例分割掩碼進(jìn)行聯(lián)合動(dòng)態(tài)一致性檢查并剔除動(dòng)態(tài)特征，最后利用靜態(tài)特征估計(jì)機(jī)器人位姿；局部建圖線程對地圖點(diǎn)進(jìn)行實(shí)時(shí)更新；回環(huán)檢測線程對相機(jī)軌跡檢測回環(huán)，并進(jìn)行全局優(yōu)化。

圖1 本文算法系統(tǒng)框圖

1.1 LK 光流

光流是一種描述像素隨時(shí)間在圖像之間運(yùn)動(dòng)的方法，按照計(jì)算像素的多少分為了稀疏光流和稠密光流。稀疏光流中最具代表的是Lucas-Kanada 光流[19]，稠密光流中最具代表的是Horn-Schunck 光流[20]。稀疏光流因計(jì)算像素較少，具有良好的實(shí)時(shí)性。本文算法中僅需要計(jì)算部分像素，故采用LK 光流。

在LK 光流中，假設(shè)同一個(gè)空間點(diǎn)被不同位姿下的相機(jī)觀測到的灰度值固定不變，根據(jù)假設(shè)有：

式中：Ix、Iy、It為該空間點(diǎn)灰度值在x、y、t方向上的偏導(dǎo)；u、v為該空間點(diǎn)對應(yīng)像素點(diǎn)光流在x、y方向上的運(yùn)動(dòng)。

LK 光流存在的另一個(gè)假設(shè)是：某一像素和附近小鄰域Ω內(nèi)像素運(yùn)動(dòng)一致。假設(shè)小鄰域內(nèi)有n個(gè)像素點(diǎn)，可建立n個(gè)約束方程，為：

式（2）為超定線性方程，通過最小二乘對其求解，得到像素點(diǎn)光流運(yùn)動(dòng)信息u、v。利用像素點(diǎn)光流的運(yùn)行信息，可以確定其在下一圖像中出現(xiàn)的位置，即為光流匹配點(diǎn)。為了讓LK 光流具有更強(qiáng)的魯棒性，引入圖像金字塔提取多層次的光流信息。

1.2 YOLOv7 實(shí)例分割

本文中采用YOLOv7[21]實(shí)例分割網(wǎng)絡(luò)來獲取像素級語義分割掩碼。YOLOv7 是一種新的體系結(jié)構(gòu)，并且使用了相應(yīng)的模型縮放方法，該方法提高了參數(shù)的利用率和計(jì)算效率。YOLOv7 在準(zhǔn)確性和速度方面都具有優(yōu)越的性能。

YOLOv7 網(wǎng)絡(luò)模型如圖2 所示，由輸入（Input）、主干網(wǎng)絡(luò)（Backbone）、頭部網(wǎng)絡(luò)（Head）3 個(gè)主要模塊構(gòu)成。輸入模塊的功能是將輸入的圖像調(diào)整為固定的尺寸大小，使其滿足主干網(wǎng)絡(luò)的輸入尺寸要求。CBS卷積層、EELAN 卷積層和MPConv 卷積層組成了主干網(wǎng)絡(luò)。MPConv 卷積層是在CBS 卷積層上加入了最大池化層（Maxpool），構(gòu)成上下兩個(gè)分支，最終利用Concat 操作對兩個(gè)特征提取分支進(jìn)行融合，從而提升了主干網(wǎng)絡(luò)提取特征的能力。

圖2 YOLOv7 網(wǎng)絡(luò)模型

E-ELAN 卷積層是高效層聚合網(wǎng)絡(luò)，可以在保持原始梯度路徑不變的狀態(tài)下，提升網(wǎng)絡(luò)的學(xué)習(xí)能力，同時(shí)也可以引導(dǎo)計(jì)算塊學(xué)習(xí)更多樣化的特征。頭部網(wǎng)絡(luò)先通過采用SPP 金字塔結(jié)構(gòu)，提高頭部網(wǎng)絡(luò)的多尺寸輸入適應(yīng)能力；再使用聚合特征金字塔網(wǎng)絡(luò)結(jié)構(gòu)，使得底層信息可以自底層向上傳遞到高層，融合了不同層次的特征；最后生成實(shí)例分割掩碼。YOLOv7 的損失函數(shù)由定位損失、置信度損失和分類損失三部分構(gòu)成，前者采用CIoU 損失，后兩者采用BCELoss 二值交叉熵?fù)p失。

1.3 聯(lián)合動(dòng)態(tài)一致性檢查

實(shí)例分割線程為跟蹤線程提供了實(shí)例分割掩碼，通過掩碼信息，將出現(xiàn)在相機(jī)視野中的物體分為3 大類，分別為：靜態(tài)物體、動(dòng)態(tài)物體和潛在動(dòng)態(tài)物體。其中潛在動(dòng)態(tài)物體主要是指椅子和書等較大可能處于被運(yùn)動(dòng)狀態(tài)的物體。首先計(jì)算得出光流運(yùn)動(dòng)信息中的非動(dòng)態(tài)物體（潛在動(dòng)態(tài)物體與靜態(tài)物體總稱）的平均位移增量S0，再計(jì)算每個(gè)潛在動(dòng)態(tài)物體以及動(dòng)態(tài)物體的位移增量，分別為、。

利用實(shí)例分割掩碼信息對3 類物體初始運(yùn)動(dòng)概率賦初值：

式中：P1、P2、P3分別為靜態(tài)物體、潛在動(dòng)態(tài)物體和動(dòng)態(tài)物體的初始運(yùn)動(dòng)概率。再計(jì)算出Smax與Smin作為位移增量判定上下界：

本文算法中有一個(gè)基本假設(shè)：靜態(tài)物體的狀態(tài)一直為靜止，故其聯(lián)合運(yùn)動(dòng)概率為：

結(jié)合動(dòng)態(tài)物體和潛在動(dòng)態(tài)物體光流位移增量，按照公式（7）、公式（8）計(jì)算聯(lián)合運(yùn)動(dòng)概率：

最后將＞0.75 的潛在動(dòng)態(tài)物體作為運(yùn)動(dòng)狀態(tài)進(jìn)行剔除，將＜0.75 的動(dòng)態(tài)物體作為靜止?fàn)顟B(tài)進(jìn)行保留。將保留的特征用于相機(jī)運(yùn)動(dòng)估計(jì)。

本文算法不是直接剔除先驗(yàn)動(dòng)態(tài)物體上提取的特征，而是通過上述公式計(jì)算先驗(yàn)語義信息與LK 光流信息的聯(lián)合運(yùn)動(dòng)概率，結(jié)合了語義信息與光流信息后，物體的運(yùn)動(dòng)判定更加準(zhǔn)確。特別是，當(dāng)RGB 圖像中先驗(yàn)動(dòng)態(tài)物體占據(jù)較大部分比例時(shí)，語義SLAM 若直接剔除未運(yùn)動(dòng)的人時(shí)，會導(dǎo)致估計(jì)的相機(jī)軌跡與真實(shí)軌跡之間的誤差過大。

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

本節(jié)將從定位準(zhǔn)確度和真實(shí)場景兩個(gè)方面對本文算法進(jìn)行驗(yàn)證。此外，還將本文提出的算法與其他先進(jìn)的SLAM 算法在TUM[22]中RGB-D 數(shù)據(jù)集上進(jìn)行實(shí)驗(yàn)對比。由于實(shí)驗(yàn)需要，將選取TUM 中fr3 數(shù)據(jù)集中的4 個(gè)高動(dòng)態(tài)場景和4 個(gè)低動(dòng)態(tài)場景。絕對軌跡誤差（Absolute Trajectory Error,ATE）是相機(jī)位姿真實(shí)值與估計(jì)值的差值，本文將選擇ATE 作為定位準(zhǔn)確度評價(jià)指標(biāo)。

本文所用的實(shí)驗(yàn)平臺為聯(lián)想Y9000P 筆記本電腦，CPU 為11th Gen Intel?CoreTMi7-11800H，RAM 為雙通道16 GB，GPU 為RTX 3060 Laptop，顯存6 GB。運(yùn)行系統(tǒng)為Ubuntu 18.04，實(shí)例分割環(huán)境為CUDA 11.3，PyTorch 1.8，Python 3.8。

2.1 定位準(zhǔn)確度

為了驗(yàn)證本文算法對運(yùn)動(dòng)物體處提取的特征點(diǎn)剔除后對定位性能的提升，將本文算法的ATE 與ORBSLAM2 進(jìn)行對比，如表1 所示。從表中數(shù)據(jù)可以看出，本文算法在4 個(gè)低動(dòng)態(tài)數(shù)據(jù)集中相對提升率最高可達(dá)52.04%；在4 個(gè)高動(dòng)態(tài)數(shù)據(jù)集中提升率最高達(dá)到了98.11%，最低達(dá)到了93.35%；在數(shù)據(jù)集s_x 中，本文算法的ATE 較ORB-SLAM2 的ATE 要大，經(jīng)分析在s_x 數(shù)據(jù)集中，剔除運(yùn)動(dòng)的特征點(diǎn)后的特征點(diǎn)數(shù)量較少，導(dǎo)致改進(jìn)后的定位精度反而比ORB-SLAM2 要低。

表1 本文算法與ORB-SLAM2 算法的絕對軌跡誤差對比

將本文算法的ATE 與僅使用YOLOv7 刪除先驗(yàn)動(dòng)態(tài)物體處提取的特征點(diǎn)的ATE 相對比，如表2 所示。從表2 中數(shù)據(jù)可以得出，LK 光流與YOLOv7 實(shí)例分割結(jié)合的方法與僅使用YOLOv7 實(shí)例分割的方法相對比，在4 個(gè)低動(dòng)態(tài)數(shù)據(jù)集中，LK 光流與YOLOv7 實(shí)例分割結(jié)合的方法的相對提升率均為正值；在4 個(gè)高動(dòng)態(tài)數(shù)據(jù)集中，兩個(gè)為正值兩個(gè)為負(fù)值；相對提升率最小為-49.7%，經(jīng)分析，w_r 數(shù)據(jù)集中因相機(jī)旋轉(zhuǎn)導(dǎo)致光流精度受限，因此最終的定位精度反而較僅使用YOLOv7 實(shí)例分割的方法小。

表2 本文算法與僅使用YOLOv7 剔除動(dòng)態(tài)特征方法的絕對軌跡誤差對比

結(jié)合表1 和表2 數(shù)據(jù)不難得出，本文算法在低動(dòng)態(tài)數(shù)據(jù)集中的定位效果要比僅利用YOLOv7 實(shí)例分割剔除動(dòng)態(tài)特征的定位效果好，在高動(dòng)態(tài)數(shù)據(jù)集中的定位效果也有不弱于僅利用YOLOv7實(shí)例分割剔除動(dòng)態(tài)特征的方法。圖3為ORB-SLAM2、YOLOv7與LK光流+YOLOv7估計(jì)的相機(jī)運(yùn)動(dòng)軌跡對比圖。圖中實(shí)線為相機(jī)的真實(shí)軌跡，點(diǎn)線為算法估計(jì)的軌跡，從圖中可以看出，本文算法在s_h、s_r、w_h 數(shù)據(jù)集中估計(jì)的相機(jī)軌跡更加貼合真實(shí)估計(jì)，估計(jì)軌跡與真實(shí)軌跡的誤差更小，直觀地體現(xiàn)了本文算法的先進(jìn)性。

圖3 本文算法與ORB-SLAM2 的跟蹤軌跡對比

將本文算法的ATE 與其他優(yōu)秀的語義SLAM 相對比，如DS-SLAM、DynaSLAM，結(jié)果如表3 所示。

表3 本文算法與經(jīng)典SLAM 算法的絕對軌跡誤差對比 m

在4 個(gè)低動(dòng)態(tài)數(shù)據(jù)集中，本文算法有5 個(gè)最優(yōu)，且ORB-SLAM2 有2 個(gè)最優(yōu)，其他優(yōu)秀算法僅1 個(gè)最優(yōu)；在4 個(gè)高動(dòng)態(tài)數(shù)據(jù)集中，本文算法有4 個(gè)最優(yōu)。本文算法在低動(dòng)態(tài)數(shù)據(jù)集中的表現(xiàn)優(yōu)于其他算法，在高動(dòng)態(tài)數(shù)據(jù)集中也有不弱于其他算法的表現(xiàn)，充分驗(yàn)證了本文算法的優(yōu)越性。

2.2 真實(shí)場景

為了評估本文算法在真實(shí)場景中的性能，使用Kinect v2 深度相機(jī)對實(shí)驗(yàn)室的日常場景進(jìn)行圖像信息提取，并輸入本文算法，對算法運(yùn)行中的特征提取、實(shí)例分割、光流運(yùn)行信息、動(dòng)態(tài)特征剔除進(jìn)行可視化展示，如圖4 所示。從圖4 第一列圖像可以看出，當(dāng)人坐在椅子上處于低動(dòng)態(tài)時(shí)，本文算法認(rèn)定其為靜止?fàn)顟B(tài)，不剔除其上特征點(diǎn)；圖中第二列，當(dāng)人從座位中起身時(shí)被認(rèn)定為動(dòng)態(tài)狀態(tài)，剔除其上特征點(diǎn)；圖中第三列和第四列，人處于行走時(shí)，本文算法認(rèn)定人處于運(yùn)動(dòng)狀態(tài)，剔除其上特征點(diǎn)。綜合圖4可以得出，本文算法可以精準(zhǔn)地對運(yùn)動(dòng)特征進(jìn)行剔除。

圖4 真實(shí)場景驗(yàn)證

3 結(jié)語

本文將物體分為3 類：靜態(tài)物體、動(dòng)態(tài)物體和潛在動(dòng)態(tài)物體，通過YOLOv7 實(shí)例分割掩碼與先驗(yàn)信息，對其運(yùn)動(dòng)概率賦予初值，然后將YOLOv7 實(shí)例分割與光流運(yùn)動(dòng)信息相結(jié)合，分別計(jì)算每一個(gè)動(dòng)態(tài)物體和潛在動(dòng)態(tài)物體光流位移增量，根據(jù)位移增量判定其是否運(yùn)動(dòng)，最終對運(yùn)動(dòng)物體上提取的特征點(diǎn)進(jìn)行剔除。本文算法能很好地判定物體的運(yùn)動(dòng)狀態(tài)，不像其他語義SLAM，直接對先驗(yàn)動(dòng)態(tài)物體進(jìn)行剔除，忽略了余留的靜態(tài)特征太少，導(dǎo)致算法定位精度降低。在TUM RGB-D 數(shù)據(jù)集中對本文算法進(jìn)行驗(yàn)證，實(shí)驗(yàn)結(jié)果表明：本文算法在低動(dòng)態(tài)數(shù)據(jù)集中定位效果優(yōu)于其他算法，在高動(dòng)態(tài)數(shù)據(jù)集中也處于一流的水平。本文算法在真實(shí)場景中的運(yùn)行過程展示了對于動(dòng)態(tài)物體的運(yùn)動(dòng)狀態(tài)判別精準(zhǔn)，能有效地幫助提高算法的定位能力。在未來的工作中，將探索以物體作為地標(biāo)的語義SLAM。