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

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

0 引 言

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

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

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

1 視覺SLAM與視覺里程計

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

圖1 經典視覺SLAM框架

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

圖2 經典視覺里程計

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

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

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

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

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

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

(1)

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

(2)

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

x2=Rx1+t

(3)

(4)

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

(5)

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

(6)

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

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

2.1 系統(tǒng)結構

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

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

2.2 實時目標檢測

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

圖5 目標檢測技術路線

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

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

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

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

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

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

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

3 實驗與結果分析

3.1 數據采集

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

圖7 傳感器設定

表2 數據采集平臺設備清單

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

3.2 車輛識別

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

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

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

表3 YOLOv4與YOLOv4-tiny訓練結果

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

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

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

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

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

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

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

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

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

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

圖12 場景3：相機旋轉誤判

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

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

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

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

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

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

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

4 結束語

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