動態場景中的單目視覺里程計

傅 彬，金 棋

（紹興職業技術學院，浙江 紹興 312000）

0 引 言

隨著城市現代化的發展，交通變得愈加擁堵，能源浪費嚴重，因而自動駕駛汽車在人們的生活中就顯得尤為重要。自動駕駛至關重要的任務是自動駕駛汽車對三維環境的感知和定位。自動駕駛汽車感知環境時用到多種傳感器，如激光雷達、毫米波雷達等。攝像頭相比于激光雷達等距離傳感器有價格低廉、重量輕、耗能少等優點，因此基于攝像頭的定位和三維環境感知成為研究的熱點。車載環境是復雜的動態環境，環境中不僅有正在運動的行人、車輛等目標，還有房屋和樹木等靜止的背景，因此，自動駕駛技術需要在三維環境中獲得自身的位置信息、靜態障礙物位置估計以及三維運動目標的位置和角度估計，來保證行車安全。

視覺同步定位與建圖（Simultaneous Localization and Mapping，SLAM）[1]算法能夠實現靜態環境中的定位和地圖構建，但是在動態環境如車載環境中，視覺SLAM精確度會變差。因為視覺SLAM采用靜態特征點計算相機運動，如果不能區分圖像中靜止和運動的特征點，計算得到的相機位姿將會出錯。視覺SLAM中常常采用RANSAC[2]算法剔除異常特征點，如果圖像中僅有少量運動特征點，RANSAC根據大部分符合模型的靜態特征點剔除運動特征點使得估計相機運動較為準確。RANSAC這種方法在低動態場景中比較適用，但是在車載環境中，運動車輛占圖像大部分面積或被前車大量遮擋，將會降低SLAM的魯棒性。現有的multibody SFM[3]和multibody SLAM[4]提出基于運動分割SFM和SLAM用于動態環境中相機定位，但是需要事先得到運動目標和靜止背景的先驗信息或者從運動物體占大面積的圖像中得到幾何約束的缺點，所以不能很好地解決未知的車載環境下視覺SLAM定位問題。

現有的單目視覺的測距方法中，任工昌等人[5]提出一種由變焦鏡頭獲得焦距與像距的雙脈沖數，利用神經網絡計算物距的單目視覺測距。朱丙麗等人[6]將裁剪后的圖像輸入多通道卷積神經網絡，得到多個深度圖候選對象，并將其映射合并為一個深度映射候選對象，最后通過傅里葉反變換生成最終估計深度圖。STEIN等人[7]根據投影幾何關系估計地面上物體深度和垂直物體高度。基于深度神經網絡的單目深度估計雖然能夠得到相對深度但是無法得到絕對深度，而幾何測距方法存在需要事先標定相機安裝外參參數的缺點。

目前，基于深度卷積網絡的目標檢測和分割已經達到了很高的精度，目標檢測算法、語義分割算法及實例分割算法都能達到高檢測精度。雖然二維圖像目標檢測能夠精確地在圖像中檢測目標的位置，但是不能直接得到目標物體的三維信息。因此有人提出單目3D目標檢測算法。現有的單目3D目標檢測算法通過帶有目標3D信息的標簽有監督訓練端到端的神經網絡，直接通過神經網絡輸出目標6自由度姿態和尺寸，但是3D標簽標注獲取困難，會對網絡訓練樣本數量帶來影響，并且這類方法只能得到可能運動的物體，但是不能確定物體目前是否在運動。

本文針對以上不足，提出使用深度學習對圖像實例分割，結合慣性傳感器單元（Inertial Measurement Unit，IMU）預測得到的運動信息在分割結果上分離得到靜止目標、運動目標，利用靜止特征點進行視覺定位和三維重建。

1 相關工作

在動態環境中實現相機定位、靜態目標定位以及動態目標深度和角度估計，需要分割圖像中的運動目標，靜態特征點用于視覺定位，估計運動目標深度和角度。

基于深度神經網絡的圖像目標檢測和分割算法，能夠精準識別圖像中物體并用框或者像素級掩模標記目標在圖像中的位置。深度卷積網絡能夠檢測和分割車載環境下拍攝得到的目標，可以從預測標簽中用人類先驗知識判斷目標是否會運動，比如樹木和建筑物標簽的物體是不會運動的，但是根據標簽無法得知車輛、行人等可能會運動的目標是否在運動。在固定攝像頭的情況下，JAIN等人[8]提出幀間差分法，該方法運算速度快，但一般不能完全檢測出運動物體的所有像素點并且常常在檢測到的運動物體內部出現“空洞”現象。針對幀間差分法的不足，人們提出基于統計的背景建模，WREN等人[9]提出單高斯模型的背景建模，STAUFFER等人[10]提出高斯混合模型背景建模方法，能夠更好地適應復雜場景。基于非概率的背景建模方法中，LONG等人[11]提出基于樣本一致性（SACON）的背景建模方法，BARNICH等人[12]提出ViBe算法，不需要假定任何的概率模型，計算速度快。靜止的攝像機在監控環境下應用廣泛，但是在車載環境中，攝像機在運動，圖像中物體的運動來源于物體獨立運動和相機運動的疊加，因此固定攝像頭運動目標分割的方法在運動攝像頭下大部分都失效。在運動攝像頭情況下，基于動態物體先驗知識的運動分割方法中，WANG等人[13]將運動物體上的SURF描述子構成一個集合，在SURF特征點檢測時通過與集合中的描述比較來確定運動物體。雖然基于運動物體先驗知識方法對退化運動比較有效，但是需要事先得到運動目標和靜止背景的信息。基于幾何約束的方法中，JUNG等人[14]采用隨機投票的方式選出滿足同一對極幾何約束的特征點。基于幾何約束的方法雖然不需要先驗知識，通過跟蹤以前幀中靜止特征點計算運動，但是在運動物體占大面積的圖像中靜止特征點將會跟蹤失敗，因此在運動物體占大面積的圖像中將失效。

目前，單目視覺SLAM根據特征點是否需要匹配可以分為特征點法和直接法兩類。PTAM算法提出并實現了跟蹤與建圖并行處理，是第一個后端非線性優化的算法。ORB-SLAM算法繼承了PTAM算法多線程的優點，采用ORB特征并使用三個線程（tracking，local mapping和loop closing）完 成SLAM。基于直接法的SLAM被提出用于克服基于特征點優化的SLAM在特征點提取匹配上比較耗時、特征點比較稀疏等缺點。ENGEL等人[15]提出的LSD_SLAM描述了像素梯度與直接法的關系以及像素梯度與極線方向在半稠密重建中的角度關系。直接法容易受到光照、曝光時間等影響，導致算法魯棒性降低。ENGEl等人[16]提出了光度標定，認為對相機的曝光時間、暗角、伽馬響應等參數進行標定后，能夠提高魯棒性。但在實際車載環境中，實際行車路線和數據集采集過程中采集車的行車路線有著不同之處，在實際行車過程中很少形成閉環，如果閉關檢測錯誤，將引入很大的誤差。

本文通過深度神經網絡實例分割得到目標類型的先驗知識，區分出靜止的背景和可能會運動的目標；采用IMU信息估計相機運動，在可能會運動的物體中分類正在運動的目標和靜止目標，利用靜止特征點同時定位和地圖重建，并且估計道路平面方程，得到可行駛區域。

2 理論基礎

2.1 符號與定義

在本文中，定義(·)w為世界坐標系，z軸方向與重力對齊。為采樣第kth幀圖像時攝像機坐標系，為采樣第kth幀圖像時的IMU坐標系，如圖1所示。

圖1 坐標系

本文使用旋轉矩陣R表示旋轉，用下標表示旋轉方向，如Rcw表示世界坐標系到攝像機坐標系的旋轉。位移用P來表示，Pcw表示在世界坐標系下攝像機的位置。重力用gw=[0 0g]T表示，g=-9.81 m·s-2為重力加速度大小。用Spc表示攝像機坐標系下三維點坐標，Spw表示世界坐標系下三維點坐標。表示第kth幀相機運動狀態，。本文用表示測量值。記李代數到李群的指數映射和李群到李代數的對數映射：

式 中：?∈R3，?∧為反對稱矩陣運算,。

2.2 IMU運動狀態

慣性傳感器單元（Inertial Measurement Unit，IMU）包含三軸加速度計和三軸陀螺儀，分別測量得到IMU坐標系下加速度和三維角速度。

此處忽略地球旋轉的影響，認為地球為慣性參考系。

根據IMU運動學模型，假設在?t時間內和是常數且IMU采樣與攝像機采樣同步，由于IMU采樣速率比攝像機快，因此需要計算兩幀之間IMU采樣值的計算。第k幀和第k+1幀之間IMU采樣j-i次，IMU運動方程如下：

2.3 攝像機投影模型

投影三維點到圖像平面，首先要將世界坐標系中的點轉換到攝像機坐標系中，如式（8）所示。

本文采用針孔攝像機模型，攝像機內參矩陣K事先標定得到式（9）。

3 系統框架

本文實現的視覺定位、三維重建與運動目標定位系統，總共由運動分割模塊和視覺慣性里程計模塊兩個部分組成，如圖2所示。

圖2 系統框圖

場景中有靜止的背景如房子、道路，交通燈等，也有可能會運動的物體，如車輛和行人。運動分割模塊的作用是將運動物體和靜止物體分離出來，分離得到靜止的背景、靜止的物體以及運動的物體，從而將圖像中的特征點分為運動物體上的特征點和靜止物體上的特征點。運動分割模塊為視覺慣性里程計提供靜止的特征點，幫助視覺慣性里程計更具有魯棒性。深度神經網絡對圖像進行實例分割，得到物體的分割掩模，沒有被分割的部分為道路、房子等背景，被分割出來的物體為交通牌、行人和車輛等目標，通過先驗知識可以判斷物體能不能運動，但是實例分割不能判斷當前幀物體是不是在運動。本文利用IMU估計運動和對極幾何約束區分當前時刻正在運動的物體和靜止物體。靜止物體和背景上的特征點作為視覺慣性里程計的輸入。

基于特征點的視覺里程計估計相機運動、特征點三維點位置以及IMU偏置。視覺里程計接收靜止特征點作為輸入，減少RANSAC迭代次數。每來一幀新圖像，先用IMU預測相機運動Sck，用于運動分割。優化后得到的相機運動輸出作為運動目標深度估計模塊的輸入。

4 運動分割

車載環境下，攝像機采集的圖像有著三維環境場景大、攝像機運動速度快、圖像中有大塊快速運動物體等特點。視覺SLAM算法通過多幅圖像中靜止的特征點根據多視角幾何關系完成同時定位與三維重建的任務，但是兩幀圖像中的特征點可能是誤匹配，也可能是運動物體，視覺SLAM常常采用RANSAC算法排除這些異常值。

RANSAC算法是一種從帶有異常值的數據中估計模型參數的算法。視覺SLAM利用RANSAC算法從圖像匹配好的特征點對中估計相機旋轉R和位移t，其中異常值為誤匹配的特征點對和位于運動物體上的特征點。RANSAC算法從觀測的特征點對中隨機采樣最小點對集合，通過最小點對集合計算模型假設，通過其他點對驗證模型假設，反復采樣N次之后，選擇最符合其他點對的模型假設作為最佳模型。采樣次數N是RANSAC算法成功與否的非常重要的參數，如式（10）所示：

式中：s為隨機采樣最小點對集合的大小，ε為異常值占所有數據的比例，p為置信度。在置信度為p的情況下，N次采樣中，至少有一次采樣得到的最小點對集合中的點對全部為正常值。以八點法為例，s=8，異常值比例ε=60%，要達到p=99%的置信度，需要迭代至少7 025次。當靜止特征點在圖像中占主要部分時，RANSAC算法能發揮用處，但是在車載環境中，攝像機前有大量動態物體或者被運動物體遮擋大部分畫面時，RANSAC算法將會失效，因此，找出圖像中正在運動的物體顯得尤為重要。

4.1 基于幾何約束的運動目標分割

圖像上二維點的運動是二維點對應的三維點在空間中的獨立運動和相機運動造成的。因此，想要找出獨立運動的物體，需要排除相機運動的干擾。對極幾何約束是兩個視角之間的投影幾何關系，如圖3所示，對于靜止的三維點P在兩幀的投影x1、x1′滿足x1′TFx1=0。其中F矩陣為基本矩陣（fundamental matrix），，對極線l2=Fx1。對于非退化的運動，若P點運動到P′點，P′在第二幀上的投影x2′將不滿足對極幾何約束。

圖3 對極幾何約束

本文采用基于卷積神經網絡的物體分割方法，用Mask R-CNN對圖像中的物體進行實例分割，確定圖像中的物體，如圖4所示。通過Mask R-CNN輸出的物體標簽，將物體分為不會運動的物體（如交通燈）和會運動的物體（如汽車），但是還不能確定這輛汽車是停靠在路邊的汽車還是開動的汽車。

圖4 語義分割和運動分割的對比圖

在不知道兩幀運動之前，視覺SLAM算法采用八點法或五點法，用RANSAC的方法從特征點集中計算基本矩陣F并分解得到相機運動，同時分辨出異常值。但是這在高動態環境中是不適用的，在不知道相機運動的情況下無法得到基本矩陣F，在沒有基本矩陣F的情況下不能得到正常值計算相機運動。本文利用慣性傳感器信息，在視覺慣性里程計優化之后用IMU測量得到的加速度和角速度估計相機運動，得到下一幀圖像的旋轉R和位移t，用于計算基本矩陣F。

本文采用圖像上特征點到對極線的距離來評價3D點運動與否，如式（11）所示。

式中：Fx1(i)表示Fx1向量第i個元素。圖像經過Mask R-CNN實例分割的多個剛體，對于同一物體上的特征點滿足相同基本矩陣F，因此采用RANSAC算法去除同一個物體上錯誤的特征點匹配。第k個物體上的特征點到對極線的平均距離如式（12）所示。

若同一個物體上特征點到對極線的平均距離EK>threshold則判定物體為獨立運動物體。判決閾值threshold的選取如圖5所示，選取運動目標與靜止目標最開始相交的點作為判決閾值。

圖5 對極幾何約束誤差直方圖

4.2 攝像機運動估計和地面建模

由于車載視頻攝像機運動較大，直接法容易陷入局部最優解，并且直接法容易受到室外光照變化的影響，因此本文選擇較為魯棒的基于特征點的視覺SLAM。由于應用場景不同，與室內場景和AR應用不同，在車輛行駛過程中很少有閉合回路的情況出現，在高速公路上更不可能閉合回路，因此回環檢測線程對于車載環境來說是沒有必要的。本文去除回環檢測線程，保留前端跟蹤線程和后端優化線程。

視覺里程計前端跟蹤模塊用于計算兩幀之間的相機運動，包括旋轉和位移。前端跟蹤模塊將地圖中的三維點投影到當前幀并與當前幀特征點匹配，通過優化當前幀j上所有匹配上的特征點的重投影誤差得到相機運動。

后端優化模塊的作用是減少誤差累積。后端優化是對連續n個關鍵幀狀態和h個關鍵幀能觀測到的三維點進行優化。當有新的一幀被確認為關鍵幀時，開啟后端local BA優化地圖中n個關鍵幀能夠觀測到的三維點和相機狀態。

式中：f為待優化幀的集合，g為待優化三維點的集合。

假設圖像中下方1/3、中間1/5的位置是地面，且地面是一個平面。視覺SLAM根據靜態背景的特征點已經估計出兩幀之間相機相對位姿{R,p}。圖像中位于地面上的特征點在三維中處于同一個平面，根據平面單應性質，第i幀同一個平面特征點映射到第j幀xj′=Hxi，其中H為單應矩陣，{n,d}為平面方程。第j幀與第i幀匹配特征點得到觀測量xk。為了得到路面方程，以重投影誤差建立目標函數，優化目標函數得到地平面方程{n,d}。

式中：eHk=xj′-xj。估計得到路面的平面方程y=nTx+d。距離地面10 cm以上的三維點被認定為靜態障 礙物。

4.3 基于運動分割的單目視覺定位

本文構建的系統在KITTI數據集[17]上測試。KITTI是無人駕駛數據庫，包含長達將近50 km不同場景下的行車圖像以及激光雷達和IMU信息。這個歷程及序列中，相機旋轉和位移速度較快，還包含部分運動目標，對于單目視覺具有一定挑戰性。序列00-10提供每幀相機位置和姿態，通過相似變換讓每幀軌跡與真實軌跡對齊，測量絕對路徑誤差。所有實驗在Intel Core i7-4810MQ（2.8 GHz，4 cores）的CPU和8 GB內存的筆記本電腦上運行，可以達到實時的要求。

表1顯示了每幀估計的軌跡均方根誤差（Root Mean Square Error，RMSE）以及真實地圖的大小。本文分別比較了不做任何處理的ORB-SLAM算法和運動分割的ORB-SLAM算法的軌跡RMSE。由表1可知，有運動分割比沒有運動分割的ORBSLAM算法在序列02-06、08-10上的RMSE好1.91%～23.75%，序列00、07分別變差6.78%、13.2%。運動分割有助于車載環境下提高單目SLAM的魯棒性和精確度。圖6為顯示相機運動軌跡對比圖。

圖6 相機運動軌跡

表1 KITTI視覺里程計測試

5 結 語

本文提出的深度神經網絡實例分割結合IMU可以有效實現運動對象、靜止對象及靜止背景的分離，克服圖像中運動物體占圖像大部分面積時無法運動分割的缺點。在KITTI車載數據庫上的實驗表明，本文提出的方法可以精確地估計自身運動，重建靜態三維場景。