鐘興軍，吳俊琦

（上海交通大學(xué)學(xué)生創(chuàng)新中心，上海 200240）

0 引 言

搭載特定傳感器的主體，如地面移動機(jī)器人、空中無人機(jī)等，在沒有環(huán)境先驗信息的情況下，于運(yùn)動過程中建立環(huán)境模型，同時估計自己的運(yùn)動就是同時定位與建圖（Simultaneous Localization and Mapping, SLAM）[1]。視覺里程計（Visual Odometry, VO）根據(jù)相鄰兩幀間關(guān)聯(lián)特征點的位置來估計兩幀的相對位移，是整個SLAM系統(tǒng)中最關(guān)鍵的部分，然而傳統(tǒng)視覺里程計的計算要求用于計算的三維空間點位置不變。如果圖像內(nèi)的特征點不斷移動，當(dāng)這些點參與位姿計算時將會不斷給系統(tǒng)帶來誤差，最終導(dǎo)致視覺定位失敗。

同時，隨著深度學(xué)習(xí)技術(shù)的發(fā)展，目標(biāo)檢測算法取得了很大的突破。比較流行的目標(biāo)檢測算法可分為兩類：一類是基于Region Proposal 的R-CNN 系算法，它們需要先使用啟發(fā)式方法產(chǎn)生Region Proposal，然后在Region Proposal 上做分類與回歸；另一類是YOLO 這類one-stage 算法，目前具有代表性的是YOLOv4 和YOLOv5。one-stage 算法省略了勾選候選框的步驟，通過建立定位點直接得到檢測結(jié)果，在保證精度的同時，大大提高了檢測速度。第二類算法僅僅使用一個CNN網(wǎng)絡(luò)直接預(yù)測不同目標(biāo)的類別與位置，在保證一定準(zhǔn)確性的同時速度更快，更適合與SLAM 這種注重實時性的算法相結(jié)合。

在SLAM 和深度學(xué)習(xí)在各自領(lǐng)域發(fā)揮巨大作用的同時，許多研究者們也將兩者結(jié)合起來。文獻(xiàn)[2]基于區(qū)域檢測和掩碼的卷積神經(jīng)網(wǎng)絡(luò)技術(shù)，并與圖分割技術(shù)相結(jié)合，實現(xiàn)單幀圖像的語義標(biāo)注。文獻(xiàn)[3]將CNN 的多視角語義預(yù)測融合到地圖中，生成有效的語義3D 地圖。文獻(xiàn)[4]將LSD-SLAM 中的深度估計和圖像匹配都替換成基于CNN 的方法，取得的結(jié)果魯棒性更強(qiáng)。

本文利用深度學(xué)習(xí)的成果解決SLAM 在動態(tài)環(huán)境下的問題，即利用YOLOv5 目標(biāo)檢測[5]優(yōu)化動態(tài)SLAM 系統(tǒng)。首先通過目標(biāo)檢測檢測出場景中的人，并且用矩形框?qū)⑷说奈恢眠x定出來；然后從將要參與位姿估計的像素點中剔除這些分布在人身上可能會發(fā)生移動的像素點，進(jìn)而使用剔除后的像素點進(jìn)行下一步位姿的計算。本文所提出的方案集成于ORB-SLAM2[6]系統(tǒng)，在TUM動態(tài)數(shù)據(jù)集上的定位精度提升了一個量級。

1 系統(tǒng)介紹

從SLAM 與動態(tài)消除、視覺SLAM、集成的ORBSLAM2 方案、YOLO 檢測和YOLO 優(yōu)化的ORB-SLAM2五個方面詳細(xì)介紹本文優(yōu)化的SLAM 視覺定位系統(tǒng)。

1.1 SLAM 與動態(tài)消除

從概率學(xué)的角度，在基于圖的SLAM 框架下估計最優(yōu)狀態(tài)值等價于求解一個最大后驗問題，令X={xt}、分別表示t時刻相機(jī)的位姿和路標(biāo)點位置，SLAM 的最大后驗估計問題[7]即求解最優(yōu)的X*和?*，公式如下：

式中：Z={zt}和U={ut}分別表示測量和控制輸入。

根據(jù)貝葉斯公式，在不考慮輸入、沒有先驗信息的情況下，以上的最大后驗問題等價于最大似然估計問題，公式如下：

令Δt和Γt分別表示t時刻正確和錯誤的數(shù)據(jù)關(guān)聯(lián)，即有：

式中：r= 1,2,…,。上式的似然估計問題可因式分解為：

式（4）表明，優(yōu)化器容易受錯誤數(shù)據(jù)關(guān)聯(lián)的影響，導(dǎo)致估計的狀態(tài)非最優(yōu)。在動態(tài)環(huán)境下，錯誤的數(shù)據(jù)關(guān)聯(lián)主要由動態(tài)物體引起，對動態(tài)物體進(jìn)行消除能夠保證最優(yōu)的狀態(tài)估計。

1.2 視覺SLAM

在SLAM 估計問題中，如果傳感器主要為相機(jī)，那就稱為視覺SLAM。視覺SLAM 的目標(biāo)是同時恢復(fù)出每幀圖像對應(yīng)的相機(jī)運(yùn)動參數(shù)和場景三維結(jié)構(gòu)，xt包含了相機(jī)的位置和朝向信息，通常表達(dá)為一個3×3 的旋轉(zhuǎn)矩陣Rt和一個三維位置變量[8]pt，Rt和pt將一個世界坐標(biāo)系下的三維點?t變換至xt對應(yīng)的局部坐標(biāo)系：

根據(jù)相機(jī)模型投影至二維圖像，表示為：

式中：fx、fy分別為x、y方向的圖像焦距；cx、cy表示鏡頭光心的位置。

則在高斯測量誤差N(ht(xt,?t),Σt)下最大似然估計問題轉(zhuǎn)化為最小二乘求解：

1.3 集成的ORB-SLAM2 方案

ORB-SLAM2 是基于單目、雙目和RGB-D 相機(jī)的一套完整的SLAM 方案。無論是在室內(nèi)的小型手持設(shè)備，還是到工廠環(huán)境的無人機(jī)和城市里駕駛的汽車，ORB-SLAM2 都能夠在標(biāo)準(zhǔn)的CPU 上進(jìn)行實時工作。

ORB-SLAM2 由三個平行的線程組成，即跟蹤、局部建圖和回環(huán)檢測。跟蹤線程通過尋找局部地圖的特征進(jìn)行匹配，運(yùn)用BA 算法來最小化重投影誤差，對每幀的相機(jī)進(jìn)行跟蹤和定位；局部建圖線程運(yùn)用局部的BA 算法構(gòu)建局部地圖并且優(yōu)化；回環(huán)檢測線程能夠通過執(zhí)行位姿圖的優(yōu)化來更正累計漂移誤差。

1.4 YOLOv5 檢測

YOLOv5 是YOLO 系列中的最新一種，它是Y. Chen在2020年提出的網(wǎng)絡(luò)模型[9]。它基于YOLOv4 并進(jìn)行了一些改進(jìn)，例如在骨干網(wǎng)絡(luò)中添加用于圖像切片操作的焦點結(jié)構(gòu)，使用兩個不同的CSP（Cross Stage Partial Network）模型[10]，并設(shè)置深度的大小和特征圖的寬度。與YOLOv4 相比，YOLOv5 具有更高的可擴(kuò)展性和更快的檢測速度。

為了確保系統(tǒng)的實時性能，選擇YOLOv5 作為本文特征提取網(wǎng)絡(luò)中的可視化SLAM 前端。

YOLOv5 的核心思想就是將整張圖作為網(wǎng)絡(luò)的輸入，直接在輸出層回歸bounding box（邊界框）的位置及其所屬的類別。

將一幅圖像分成S×S個網(wǎng)格（grid cell），如果某個object 的中心落在這個網(wǎng)格中，則這個網(wǎng)格就負(fù)責(zé)預(yù)測這個object。每個bounding box 要預(yù)測x、y、w、h和confidence 共5 個值，每個網(wǎng)格還要預(yù)測一個類別信息，記為C 類。則S×S網(wǎng)格，每個網(wǎng)格要預(yù)測B個bounding box和C個categories，輸出就是S×S×（5B+C）的一個tensor。

1.5 YOLO 優(yōu)化的ORB-SLAM2

為了達(dá)到剔除分布在人運(yùn)動目標(biāo)上的特征點的目的，本文將YOLOv5 目標(biāo)檢測和ORB-SLAM2 結(jié)合起來。如圖1 所示，在將圖片輸入到跟蹤線程中提取特征點的同時，讓YOLO 檢測其中的人，并得到人的標(biāo)簽信息，利用該標(biāo)簽信息將人的矩形框的位置信息框出來，然后將落在該矩形框內(nèi)的特征點剔除，從而避免了運(yùn)動物體上的特征點參與相機(jī)位姿的計算，大大提高了ORBSLAM2 在動態(tài)場景中的準(zhǔn)確度。

2 實驗結(jié)果

2.1 數(shù)據(jù)集和評價指標(biāo)

為驗證所提方案的有效性，在公共的TUM 數(shù)據(jù)集進(jìn)行評估實驗，實驗使用的4 個高動態(tài)數(shù)據(jù)集[11]分別是fr3/walking/下的static、xyz、halfsphere 和rpy。

絕對軌跡誤差（ATE）根據(jù)位姿的時間戳將真實值和估計值進(jìn)行對齊，然后計算每對位姿之間的差值；相對位姿誤差（RPE）用于計算相同兩個時間戳上的位姿變化量的差，適合用于估計系統(tǒng)的漂移。

2.2 定性目標(biāo)檢測及分割結(jié)果

YOLO 進(jìn)行目標(biāo)檢測時可以檢測多種目標(biāo)，而本次實驗僅限于檢測人，所以將其他標(biāo)簽去掉，檢測結(jié)果如圖2a）所示；當(dāng)人在圖像中所占比例過大的時候，如果在矩形框以外的地方提取特征點會造成特征點數(shù)量過少而不能成功初始化的情況，因此采用深度圖對檢測結(jié)果進(jìn)一步分割，效果如圖2b）所示；剔除動態(tài)特征點后特征點分布如圖2c）所示。

圖2 定性的分割結(jié)果

2.3 定量視覺定位結(jié)果

表1～表3 分別為原始ORB-SLAM2、優(yōu)化后ORBSLAM2 的ATE（平移）和RPE（旋轉(zhuǎn)）的比較結(jié)果，其中RMSE 表示均方根誤差，用來評估系統(tǒng)精度；S.D.表示方差，用來評估系統(tǒng)的穩(wěn)定性。

表1 ATE 評估結(jié)果

表2 平移RPE 評估結(jié)果

表3 旋轉(zhuǎn)RPE 評估結(jié)果

從表1～表3 中可以看出，本文所提出的基于YOLO動態(tài)消除SLAM 方案從精度和穩(wěn)定層面都提升了接近一個量級。

圖3～圖6 所示為數(shù)據(jù)集half 和xyz改進(jìn)前后的軌跡圖，優(yōu)化后絕對誤差更小，估計的軌跡更接近真值。

圖3 原始half 軌跡

圖4 改進(jìn)half 軌跡

圖5 原始xyz 軌跡

圖6 改進(jìn)xyz 軌跡

3 結(jié) 論

本文在傳統(tǒng)ORB-SLAM2 的基礎(chǔ)上，針對其動態(tài)場景下位姿估計不準(zhǔn)確的問題，使用YOLO 目標(biāo)檢測剔除動態(tài)特征點，并在公共數(shù)據(jù)集上進(jìn)行了實驗驗證。結(jié)果表明，改進(jìn)后的系統(tǒng)能夠顯著降低動態(tài)環(huán)境下位姿估計的絕對誤差和相對漂移，從而提高整個系統(tǒng)的準(zhǔn)確性和穩(wěn)定性。