基于目標檢測的動態(tài)魯棒視覺SLAM

陳天陽，卜方玲，劉志鴻，程翰琳

(武漢大學(xué) 電子信息學(xué)院，湖北 武漢 430072)

0 引 言

傳統(tǒng)視覺SLAM理論的基本條件之一是假設(shè)環(huán)境中不存在運動物體[1]，這在實驗室場景下容易滿足。然而，動態(tài)環(huán)境普遍出現(xiàn)于自動駕駛等潛在應(yīng)用[2]，隨著相關(guān)需求的擴張，該假設(shè)難以支撐SLAM技術(shù)真正走向應(yīng)用。因此，研究動態(tài)場景下魯棒的視覺SLAM具有重要意義。

近年來動態(tài)視覺SLAM的研究思路可以總結(jié)為運動分割[3]，即將環(huán)境中的運動物體排除在定位與建圖之外。在這一思路之下又有多種具體方法可供選擇：Kundu等[4]利用動態(tài)特征違背對極幾何約束的特性，判定與整體位姿之差超出閾值的點為動態(tài)，但該方法在環(huán)境中動態(tài)點較多時失效；Sabzevari等[5]通過增加輪式編碼器、IMU等設(shè)備以增強相機的外部傳感器約束，但這類方法計算復(fù)雜且應(yīng)用場景受限。除此以外，深度學(xué)習(xí)也被廣泛應(yīng)用于運動分割。這些方法[6,7]大多通過圖像實例分割剔除動態(tài)特征點，在室內(nèi)動態(tài)環(huán)境下取得了很好的效果，但這類方法無法實時運行，也不符合需求。

本文致力于實現(xiàn)高動態(tài)條件下的單目相機實時魯棒自定位，主要貢獻點在于解決了兩個難題。第一，系統(tǒng)運行的實時性與定位的精確性之間的矛盾；第二，當(dāng)前缺乏符合條件的室外高動態(tài)場景作為評估數(shù)據(jù)集。對于第一個問題，現(xiàn)有的視覺SLAM大多使用實例分割算法以實現(xiàn)較精準的圖像構(gòu)建，但無法滿足實時性；本文因為不考慮建圖，所以選擇了對特征點分類較為粗糙但速度更快的目標檢測算法YOLOv4[8]作為語義前端。為避免特征點的粗糙分類可能導(dǎo)致的不利影響，對動態(tài)特征點的判斷策略進行了優(yōu)化。對于第二個問題，當(dāng)前最常用的視覺SLAM數(shù)據(jù)集均有幀率低、動態(tài)目標少的缺點，且錄制環(huán)境與國內(nèi)城市場景有較大不同。為此，本文提供了一個錄制于武漢市的室外數(shù)據(jù)集，動態(tài)目標豐富、環(huán)境多變復(fù)雜，較好地滿足了本文的研究需求。

1 視覺SLAM與視覺里程計

SLAM指搭載傳感器的主體在沒有先驗信息的情況下建立周圍環(huán)境的模型，并于運動過程中估計自身位置。根據(jù)所用感知設(shè)備的不同主要分為視覺和激光SLAM，前者以圖像序列為輸入，后者以激光雷達采集的點云為輸入。經(jīng)典視覺SLAM框架如圖1所示。

圖1 經(jīng)典視覺SLAM框架

圖1中的視覺里程計[9]根據(jù)相鄰圖像之間的視差估計相機運動并計算局部地圖，又被稱為前端；非線性優(yōu)化模塊接受不同時刻視覺里程計輸出的相機位姿和回環(huán)檢測信息并進行聯(lián)合優(yōu)化，得到全局一致的軌跡和地圖，又被稱為后端。本文主要聚焦視覺里程計，一個典型的視覺里程計如圖2所示。

圖2 經(jīng)典視覺里程計

圖2中C0…Cn表示對應(yīng)時刻的相機位姿，Pa,…Pd表示相機觀測到的路標點。在k-1和k時刻，相機各自拍得一幀圖像分別記為Ik-1和Ik，由于相機處于運動狀態(tài)，這兩幀圖像間存在視角差。例如，Ck-1和Ck分別觀測到路標Pa,Pb,Pc和Pc,Pd， 根據(jù)可被兩幀共同觀測的Pc在兩幀中坐標的差異可以計算出兩幀間的位姿變換。記相機兩時刻間的位姿變換為Tk,k-1∈R4×4

式中：Rk,k-1∈R3×3是旋轉(zhuǎn)矩陣，tk,k-1∈R3×1是平移向量，則兩幀間相機位姿可以由公式Cn=Cn-1·Tn,n-1相聯(lián)系。如果能得到相機在其整個運動過程中的變換矩陣集合Tn,0={T1,0,T2,1,…,Tn,n-1}， 那么整個時段內(nèi)相機在每一時刻的位姿即可解得。

以圖3為例對幀間相機運動進行數(shù)學(xué)表達。已知I1，I2是相機在兩不同時刻的成像，O1，O2分別為兩時刻的相機光心，可被兩幀共同觀測的路標點P在兩幀中的成像分別為p1，p2， 求相機在兩時刻間的運動R，t。

圖3 兩相鄰幀間的運動求解

在第一幀坐標系下，設(shè)路標點P的空間坐標為P=[X,Y,Z]T， 根據(jù)針孔相機模型，兩像素點坐標為p1，p2， 則在其次坐標系下有

p1=KP,p2=K(RP+t)

(1)

x1=K-1p1,x2=K-1p2

(2)

其中，K為相機內(nèi)參矩陣，x1，x2分別為像素點p1，p2的相機歸一化平面坐標。將式(2)帶入式(1)可得

x2=Rx1+t

(3)

(4)

將式(2)帶入式(4)可得

(5)

將式(4)和式(5)的中間部分分別記為基礎(chǔ)矩陣F和本質(zhì)矩陣E， 可以得到

(6)

至此，求相對運動問題被轉(zhuǎn)換為解方程問題。只要在兩個相鄰幀間找到8對匹配點即可用八點法求解矩陣E； 當(dāng)E已知時，R，t可以直接通過方程解出。

2 動態(tài)魯棒視覺SLAM

2.1 系統(tǒng)結(jié)構(gòu)

本文所構(gòu)建的動態(tài)魯棒視覺SLAM系統(tǒng)結(jié)構(gòu)如圖4所示。首先，圖像采集模塊以30幀每秒(frame per second，F(xiàn)PS)的速率將圖像序列送入系統(tǒng)進行目標檢測和特征點提取：一方面，目標檢測模塊根據(jù)事先訓(xùn)練好的網(wǎng)絡(luò)識別圖像中的指定物體，并得到目標框的像素坐標，另一方面，特征提取模塊計算并存儲圖像的ORB特征；其次，根據(jù)ORB特征點的像素坐標和目標框坐標確定每個ORB特征點的動態(tài)屬性，將落入動、靜態(tài)目標框的特征點分別劃分為動、靜態(tài)特征點，對于同時落入動態(tài)框和靜態(tài)框的特征點需結(jié)合圖像中動態(tài)點的數(shù)量占比確定屬性；最后，將靜態(tài)的ORB特征點送入ORB-SLAM2進行相機位姿的估算，并將計算結(jié)果與真實位姿進行對比，使用開源位姿評估工具evo得到定量的軌跡誤差。

圖4 動態(tài)魯棒視覺SLAM系統(tǒng)框架

2.2 實時目標檢測

圖像序列中的動態(tài)、靜態(tài)目標檢測是通過YOLOv4目標檢測算法實現(xiàn)的。YOLOv4是一階段(one-stage)目標檢測算法，輸入圖像直接返回其類別，具有高精度、低耗時等優(yōu)點。YOLOv4在上一版的基礎(chǔ)上進行了優(yōu)化和改進，選擇了CSPDarkent53作為主干網(wǎng)絡(luò)，相較前一版本的Darkent53取得了更高的分類準確率；設(shè)計了強大而高效的檢測模型，對模型訓(xùn)練的計算需求大大降低，在運算能力較低的設(shè)備上也能很好地運行。最終，YOLOv4在多個數(shù)據(jù)集上相較前一版本獲得了更高的檢測精度和更快的速度，是當(dāng)前最優(yōu)秀的實時目標檢測算法之一。

圖5 目標檢測技術(shù)路線

2.3 動態(tài)特征點提取與判別

本文使用的特征為ORB-SLAM2視覺前端提取的ORB[10]特征。ORB是一種高效的特征點提取與描述算法，包括oFAST角點和rBRIEF描述子兩部分。oFAST角點改進于FAST算法：對于像素點p，依次比較以其為圓心、3為半徑的圓周上16個像素點與其自身的灰度值，若圓周上有連續(xù)n個像素灰度值比p點大或小，則認為p是FAST角點，n一般取12。oFASTA在此基礎(chǔ)上計算以特征點為圓心、r為半徑的圖像塊質(zhì)心，定義特征點到質(zhì)心的向量為角點方向，以此實現(xiàn)旋轉(zhuǎn)不變性。BRIEF描述子是一個長為n的二進制串b[n]，以當(dāng)前特征點為中心劃定一個鄰域，選擇n對像素點，對于每一對像素點比較其灰度值I，若I(pi)>I(qi)， 則b[i]=1， 否則b[i]=0，n一般取256；選取n對像素的方法有多種，采用一種能讓n對像素間相關(guān)性最小的統(tǒng)計學(xué)習(xí)方法可以使描述子擁有優(yōu)秀的旋轉(zhuǎn)不變性，這就是rBRIEF。ORB特征兼顧效率與速度，能滿足實時性的需求，因此常用于SLAM等實時系統(tǒng)。

動態(tài)點的剔除需要兼顧兩點，一是為了最大限度地降低動態(tài)目標干擾，需要“應(yīng)剔盡剔”；二是需要保證相鄰幀間匹配點的數(shù)量應(yīng)足夠支持SLAM的持續(xù)運行。室外道路環(huán)境中的動態(tài)目標主要為車輛和行人，相機的周圍環(huán)境在大部分時間有如圖6所示的兩種可能情景。

圖6 動、靜態(tài)目標框重疊帶來歧義

情景一，當(dāng)目標處于前方且與自身之間有一定距離時，目標在當(dāng)前視野中近似為矩形，此時可以認為框內(nèi)全部為目標，如圖6中的③、④號框分別為車輛和道路標志，沒有歧義；情景二，當(dāng)目標處于側(cè)方且距離較近時，由于相機的射影變換，目標在視野內(nèi)近似呈梯形，此時框中內(nèi)容大面積為非目標內(nèi)容，如圖6中的①、②號框標注為車輛，但均與內(nèi)容為道路標志的④號框產(chǎn)生重疊；此時重疊部分提取的特征將同時被標注為動態(tài)和靜態(tài)，歧義于是產(chǎn)生。

為了解決這類歧義，本文采取如下方法。首先，由于在大多數(shù)時間內(nèi)目標與相機距離較遠，上述情景一為主流，此時“應(yīng)剔盡剔”為主要需求，可將檢測框向四周分別擴張K個像素以降低漏剔率；其次，當(dāng)特征同時處于兩種檢測框內(nèi)時，計算當(dāng)前圖像內(nèi)動態(tài)特征點的數(shù)量，若其在總特征點中占比超過一定閾值或剩余靜態(tài)特征無法支持SLAM的繼續(xù)運行，則將其歸為靜態(tài)點，反之為動態(tài)點。記閾值為T， 動態(tài)特征點數(shù)量為N1， 特征點總數(shù)為N， 則特征點的屬性判斷見表1。K和T為經(jīng)驗值，本文中分別選取K=10，T=0.3。

表1 動態(tài)特征點判別標準

3 實驗與結(jié)果分析

3.1 數(shù)據(jù)采集

視覺SLAM常用數(shù)據(jù)集為TUM[11]、EuRoC[12]和KITTI[13]，其中TUM和EuRoC錄制于室內(nèi)，KITTI雖是室外數(shù)據(jù)集，但一方面幀率較低，另一方面其圖像中動態(tài)目標較少，主要為歐洲小鎮(zhèn)與公路街景，且錄制環(huán)境與國內(nèi)城市場景具有較大不同，無法滿足評估需求。為此，本文搭建了一個視覺SLAM數(shù)據(jù)采集平臺，在武漢市內(nèi)錄制了多段圖像序列作為本文的數(shù)據(jù)集。該數(shù)據(jù)集有多個圖像序列，包含普通公路、隧道、大橋、城市快速路、停車場、校園等多個場景，動態(tài)目標豐富、環(huán)境多變復(fù)雜，較好地滿足了本文的研究需求。數(shù)據(jù)采集平臺包括車輛、RGB相機、GNSS-IMU定位設(shè)備、存儲器、便攜電腦等，所有傳感器已事先校正并準確安裝，車輛平臺與各傳感器相對位置及坐標系設(shè)定如圖7所示，設(shè)備清單見表2。

圖7 傳感器設(shè)定

表2 數(shù)據(jù)采集平臺設(shè)備清單

車輛運行期間，ZED2相機同步錄制30 FPS，1280*720像素雙目RGB圖像和100 Hz六軸IMU數(shù)據(jù)，同時GNSS-IMU設(shè)備以10 Hz速率記錄有IMU輔助的RTK差分GNSS數(shù)據(jù)作為相機位姿的真實值，即groundtruth，所有信息經(jīng)由計算機處理后存入移動硬盤。本文所有實驗使用的圖像均由ZED2左鏡頭采集。本文所采集的數(shù)據(jù)已部分制作為數(shù)據(jù)集，全部處理完成后將面向社區(qū)開源。

3.2 車輛識別

3.3 動態(tài)魯棒視覺SLAM自定位

3.3.1 動態(tài)魯棒視覺SLAM降低誤差的定性分析

根據(jù)視覺里程計的實現(xiàn)原理，視野中的動態(tài)目標對相

表3 YOLOv4與YOLOv4-tiny訓(xùn)練結(jié)果

機自定位產(chǎn)生干擾的主要原因是相對運動讓相機產(chǎn)生了錯誤的認知。例如，當(dāng)相機靜止時，前方目標的前進、倒退、向左移動、向右移動有可能讓相機分別產(chǎn)生自身正在倒退、前進、向右移動和向左移動的誤判。實驗中的一些局部結(jié)果也很好印證了這個事實，下面結(jié)合實驗結(jié)果對3類場景進行分析。

場景1：相機與前方運動物體較長時間保持近似同速同向運行，使相機對自身運動速度的估計遠低于事實，甚至認為自己接近靜止。圖8中從左上到右下的4張圖按時間順序拍攝于某行車場景，可以從視野中左右兩側(cè)較遠處的大樓看出相機已實際產(chǎn)生了較大位移，而相機正前方近處的車輛卻基本上與相機保持同樣的距離，并且在視野中面積占比較高，導(dǎo)致動態(tài)特征點占比也較高，進而產(chǎn)生如圖9所示的結(jié)果(圖9、圖11、圖13橫縱坐標分別為經(jīng)緯度)。

圖8 場景1：相機靜止誤判

圖9 算法改進前后場景1中相機自定位效果對比

圖9中的線條表示相機真實運動軌跡，星號*和圓圈o分別表示用原ORB-SLAM2和本文所采用的增加了動態(tài)特征去除的改進方法計算得到的相機位置，分別記為orb和orb-sta(orb-static的縮寫，意為靜態(tài))，所有結(jié)果投影到以經(jīng)緯度為坐標的真實世界坐標系中。可以看出，在圖9矩形框處，由原方法計算得到的相機位置在較短區(qū)間內(nèi)大量聚集，說明原方法計算的相機速度較低、接近靜止，而改進方法可以正確判斷出相機在以一定的速度移動；最終結(jié)果是原方法計算得到的相機位移遠低于實際值、改進方法的結(jié)果更接近真實位移。

場景2：相機靜止或前行時前方運動物體加速前進，使相機對自身運動速度的估計低于實際值，甚至產(chǎn)生倒退的誤判。這種情況多出現(xiàn)于交通信號燈前的起步階段，此時相機與前車間距較小，動態(tài)特征點占比較大，容易產(chǎn)生誤判。如圖10所示，從右側(cè)建筑物可以判斷相機正在前向行駛，但是前方的3輛汽車正在遠離，于是產(chǎn)生如圖11所示的結(jié)果。

圖10 場景2：相機倒退誤判

圖11 算法改進前后場景2中相機自定位效果對比

根據(jù)圖11可以看出，原方法在車輛起步階段對相機的運動產(chǎn)生了向左后方偏移的誤判，導(dǎo)致后續(xù)整段數(shù)據(jù)明顯偏離真實值，產(chǎn)生了極大的誤差；而改進后的方法避免了起步時的誤判，整體誤差明顯更小。

場景3：運動物體從相機的某一側(cè)邊緣以與其它目標不同的速度進出視野，使相機產(chǎn)生旋轉(zhuǎn)的誤判。這種情況多出現(xiàn)于超車與被超車時。圖12中，前方左側(cè)車輛逐漸被相機從右側(cè)超越，從車輛上提取的特征點以與其它特征點相比更大的速度持續(xù)向相機左側(cè)偏移，這使原算法認為相機正在右轉(zhuǎn)，產(chǎn)生如圖13所示的結(jié)果。

圖12 場景3：相機旋轉(zhuǎn)誤判

圖13 算法改進前后場景3中相機自定位效果對比

根據(jù)圖13可以看出，原方法在相機超車時產(chǎn)生了多次右轉(zhuǎn)誤判，且由此帶來的誤差是可以累積的。這導(dǎo)致計算得出的相機軌跡與真實值相比產(chǎn)生極大誤差，而改進后的方法顯著降低了這種誤差。

3.3.2 動態(tài)魯棒視覺SLAM降低誤差的定量分析

3.3.1節(jié)中定性分析了ORB-SLAM2算法在相機自定位中產(chǎn)生誤判的原因、本文的改進點以及改進之后的效果，本節(jié)將在采集的多個圖像序列中對改進前后的方法進行全面評估，并給出定量結(jié)果。實驗所用的評估工具為開源工具evo，將實驗用到的7個圖像序列按序編號為01-07，其詳細信息見表4。表中的序列01-03對應(yīng)3.3.1節(jié)中的3段數(shù)據(jù)。

在上述7段圖像序列上分別運行原SLAM和改進后的動態(tài)魯棒視覺SLAM，得到的關(guān)鍵幀位姿結(jié)果分別記為orb和orb-sta，用開源軌跡評估模塊evo分別比較兩者與對應(yīng)的groundtruth之間的誤差，采用絕對位姿誤差(absolute pose error，APE)和相對位姿誤差(relative pose error，RPE)兩個指標，得到表5所示的結(jié)果。

表5 算法改進前后在7段數(shù)據(jù)集上的定位誤差對比

表5顯示，在APE指標下，改進后的orb-sta在全部7段圖像序列上的表現(xiàn)均優(yōu)于改進前，定位誤差大幅降低；在RPE指標下，orb-sta在其中5段上取得了更好的結(jié)果。綜合以上定性、定量分析可以得出結(jié)論，相較于原SLAM系統(tǒng)，本文所提出的改進方法可以有效避免多種運動目標帶來的干擾，在城市道路的復(fù)雜動態(tài)環(huán)境中可以顯著降低定位誤差，取得更準確的結(jié)果。

4 結(jié)束語

本文針對傳統(tǒng)視覺SLAM算法在動態(tài)場景中存在的問題，采用YOLOv4目標檢測模塊改善視覺里程計，優(yōu)化動態(tài)特征點剔除策略，在ORB-SLAM2基礎(chǔ)上構(gòu)建了動態(tài)魯棒的相機自定位系統(tǒng)。該系統(tǒng)可以在動態(tài)環(huán)境中自行識別并標注運動目標，將對自身定位造成不利影響的動態(tài)特征點剔除以降低誤差。同時，為全面準確評估該方法的有效性，本文構(gòu)建了一個武漢城市道路數(shù)據(jù)集，該數(shù)據(jù)集有多個圖像序列，包含普通公路、隧道、大橋、城市快速路、停車場、校園等多個環(huán)境復(fù)雜的高動態(tài)場景。實驗結(jié)果表明，該方法可以有效抑制動態(tài)目標給相機自定位帶來的多種不利影響和誤判，與改進前方法相比誤差顯著降低，有效提升了相機在動態(tài)環(huán)境中的定位精度。