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

鐘興軍，吳俊琦

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

0 引 言

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

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

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

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

1 系統(tǒng)介紹

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

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

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

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

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

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

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

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

1.2 視覺SLAM

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

根據相機模型投影至二維圖像，表示為：

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

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

1.3 集成的ORB-SLAM2 方案

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

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

1.4 YOLOv5 檢測

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

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

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

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

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

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

2 實驗結果

2.1 數據集和評價指標

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

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

2.2 定性目標檢測及分割結果

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

圖2 定性的分割結果

2.3 定量視覺定位結果

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

表1 ATE 評估結果

表2 平移RPE 評估結果

表3 旋轉RPE 評估結果

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

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

圖3 原始half 軌跡

圖4 改進half 軌跡

圖5 原始xyz 軌跡

圖6 改進xyz 軌跡

3 結 論

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