一種基于目標(biāo)檢測(cè)的動(dòng)態(tài)環(huán)境下視覺定位系統(tǒng)

鐘興軍，吳俊琦

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

0 引 言

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

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

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

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

1 系統(tǒng)介紹

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

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

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

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

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

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

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

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

1.2 視覺SLAM

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

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

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

則在高斯測(cè)量誤差N(ht(xt,?t),Σt)下最大似然估計(jì)問題轉(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)行實(shí)時(shí)工作。

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

1.4 YOLOv5 檢測(cè)

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

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

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

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

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

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

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

2.1 數(shù)據(jù)集和評(píng)價(jià)指標(biāo)

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

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

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

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

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

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

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

表1 ATE 評(píng)估結(jié)果

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

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

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

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

圖3 原始half 軌跡

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

圖5 原始xyz 軌跡

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

3 結(jié) 論

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