動態場景下基于語義分割與運動一致性約束的車輛視覺SLAM*

黃圣杰，胡滿江，2，周云水，2，殷周平，秦曉輝，2，邊有鋼，2，賈倩倩

（1.汽車車身先進設計制造國家重點實驗室，湖南大學機械與運載工程學院，長沙 410082；2.湖南大學無錫智能控制研究院，無錫 214115；3.中國汽車工程學會，北京 100000）

前言

同時定位與建圖（simultaneous localization and mapping，SLAM）是指在沒有周圍環境先驗信息的前提下，智能車輛僅依賴攜帶傳感器的信息，完成自身位姿估計與環境地圖構建的過程，它是保障無人駕駛車輛決策、規劃等模塊正常運行的重要前提［1］。其中，視覺SLAM以相機作為主要傳感器，因具備價格低廉、色彩紋理信息豐富、功耗小等優勢，近年來受到了廣泛關注。

傳統視覺SLAM多基于靜態環境假設展開，并借助多視圖幾何原理建立幀間數據的關聯模型，進而計算相鄰幀間相機的位姿以及構建環境地圖。然而，真實場景中的動態對象難以避免，如車輛和行人，動態對象的運動過程將對相機位姿和地圖點的解算結果造成干擾，易導致SLAM系統的位姿漂移和地圖重影問題。為解決上述問題，學者通常在前端視覺里程計之前將圖像中的動態特征濾除，依賴靜態點完成SLAM系統的后續任務，從而提高SLAM在動態環境下的魯棒性與精度。因此，如何精準檢測出圖像中的動態特征成為動態環境下SLAM的關鍵問題。

依據動態特征檢測方法的不同，可將現有視覺SLAM算法分為3大類：基于幾何約束的動態特征檢測方法、基于光流的動態特征檢測方法和基于語義的動態特征檢測方法。在基于幾何約束的檢測方法中，ORB-SLAM系列［2-4］采用了隨機采樣一致性方法（random sample consensus，RANSAC）來剔除離群點（離群點中包含動態特征點），即通過循環隨機抽取特征點子集計算得到單應性矩陣，基于單應性矩陣校驗剩余特征點，超出閾值范圍的特征點則被視為離群點。魏彤等［5］通過相鄰兩幀相機之間的基礎矩陣與極線約束估計出動態特征點，并基于深度與顏色信息進一步確定動態區域的輪廓。Zou等［6］基于三角測量一致性，計算前后幀圖像特征點的重投影誤差，并將誤差超出閾值范圍的特征點判定為動態點。Dai等［7］利用Delaunay三角剖分法建立前后幀特征點的圖模型，隨后移除圖模型中多個關鍵幀之間觀察不一致的約束邊，進而降低動態特征點對地圖點與相機位姿優化過程的影響。基于光流的動態特征檢測方法中，Flowfusion［8］將PWC-Net網絡估計的光流進一步劃分成由相機運動產生的自我流和由動態對象運動產生的3D場景流，從而進行動態分割，經過若干迭代完成場景重建。Derome等［9］首先基于5點法［10］獲取本質矩陣，進而由本質矩陣與上一幀圖像獲取估計圖像，最后借助估計圖像與當前幀圖像完成特征點的光流計算，光流值超出L2距離約束的特征點被判定為動態特征點。近年來受益于深度學習在語義分割和目標檢測領域的快速發展，諸多學者嘗試利用相關算法解決動態場景的定位問題。Runz等［11］借助Mask-RCNN［12］和幾何分割算法獲取場景中的動態對象的掩碼。DS-SLAM［13］結合SegNet［14］語義分割網絡與運動一致性算法，濾除動態特征點，從而得到可靠的相機位姿并構建靜態目標地圖。程騰等［15］融合多層次多模態的特征進行目標檢測。Bescos等［16］結合Mask-RCNN網絡、區域生長算法對動態特征進行識別并濾除。

視覺SLAM目前已在智能車輛領域得到應用，但現有方法仍存在如下問題：（1）基于幾何約束、光流法的動態特征檢測方法大多受限于靜態點占比更大的隱含假設，當圖像中動態點占比更大時，依賴幀間數據建立的幾何約束、光流向量難以有效區分動靜態點；（2）大多基于語義的動態特征檢測方法實時性較低，為了保證分割精度，大多研究者采用了諸如Mask-RCNN的大型分割網絡，加上其他諸如背景修復的任務，系統每幀處理時長常高達幾百ms，難以保證實時性要求；（3）基于語義的動態檢測方法大多還存在過度去除特征點的風險，此類方法在獲得所有潛在動態對象后（如，人、車），往往將位于潛在對象輪廓區域內的特征點全部刪除，這樣導致剔除的特征點過多，在紋理信息較少的場景，系統易跟蹤失敗。

為解決上述問題，本文提出一種動態場景下結合語義分割與幾何約束的SLAM方法，該方法不基于靜態點數量占優的假設，可在動態環境下準確分割出實際運動目標，且具備實時性與魯棒性。本文主要創新點在于：（1）提出一種融合輕量級語義分割網絡Fast-SCNN［17］和ORB-SLAM3結合的SLAM算法架構，實現了動態定位場景下的潛在動態對象的實時分割；（2）利用運動約束和卡方檢驗進一步篩選實際動態特征，避免了分割網絡對特征點的過度分割，提高系統在欠紋理環境下SLAM系統的魯棒性；（3）通過慕尼黑工業大學公開數據集與實車試驗對該方法進行驗證，證明該類方法的有效性。

1 系統架構

系統在ORB-SLAM3原有框架的基礎上，增加語義分割模塊與運動一致性約束模塊，總系統架構見圖1。

圖像數據進入系統后，分別沿藍色箭頭進入Fast-SCNN網絡的語義分割線程以及沿黑色箭頭進入前端里程計的跟蹤線程。分割線程中，Fast-SCNN網絡對圖像進行分割以獲得潛在動態區域掩碼，同時跟蹤線程基于ORB特征提取圖像幀中的特征點，隨后借助動態區域掩碼去除所有潛在動態特征點，將剩余特征點輸入位姿估計模塊，獲取相機位姿的初步估計。然后，結合初步估計的相機位姿、運動一致性約束與卡方檢驗篩選出真實動態特征點，將潛在動態特征點中的剩余靜態特征點輸入位姿估計模塊，以完成相機位姿的進一步優化，提高SLAM精度并降低圖像中特征點較少時系統的跟蹤失敗率。最后，依次執行圖中剩余模塊中的步驟，構建完整的SLAM系統。

2 語義分割與運動一致性約束

通過語義分割剔除所有潛在動態特征點以獲取相機位姿的初步估計，再結合運動約束完成對靜態特征點的重添加，以進一步優化相機位姿估計。該部分從原理上避免了對靜態點數目占優這一假設的依賴，同時降低了因動態點誤判而造成的前端里程計跟蹤失敗的風險。

2.1 基于Fast-SCNN的語義分割

Fast-SCNN網絡參考了文獻［18］和文獻［19］中提出的分支結構，以及文獻［20］和文獻［21］中提出的跨越連接層（skip connections）。如圖1所示，該網絡主要由下采樣、全局特征提取、特征融合和分類器4個模塊組成，其中，下采樣模塊在提取圖像初級特征的同時降低圖像分辨率以提高網絡運行速度；全局特征提取模塊用于提取圖像全局語義信息；特征融合模塊用于融合圖像初高級特征以保證網絡更為魯棒的特征提取能力；分類器則輸出最后的像素預測類別。Fast-SCNN網絡的輸入為m×n×3維度的彩色圖像，輸出為m×n×c維度的概率矩陣，其中c為分割的類別數量，在c維度上尋找概率最大值，即可獲得表征每個像素類別的m×n維矩陣，本文中，按照潛在動態目標與非潛在動態目標，將像素分為兩類標簽。

為衡量語義分割網絡的預測值與真值之間的差異程度以正確優化網絡參數，本文采用交叉熵損失函數（cross entropy loss function）作為Fast-SCNN的優化目標函數，其數學表達如下：

式中：x為分割網絡的輸出；class為對應真值；c為像素類別數。

2.2 運動一致性約束與卡方檢驗

如圖2所示，C1、C2為相機運動過程中t1、t2兩個時刻的光心位置。p點為潛在動態對象上的一個特征點，若p為靜態點，設其在兩個成像平面上的像素坐標分別為xp1、xp2，且xp1、xp2分別位于極線l1、l2上；若p為動態點，且在t2時刻運動到了q位置，則其在C2圖像幀中的像素坐標為xq2。

極線l2可表示為

令K為相機內參矩陣，F為C1圖像幀到C2圖像幀的基礎矩陣，則極線l2滿足：

xp2到極線l2的距離平方為

式中d2服從自由度為1的卡方分布，取95%置信度，其拒絕域為3.84。由于ORB-SLAM3中為了實現尺度不變性，采用了縮放因子為1.2的圖像金字塔，故位于第n層圖像金字塔的特征點像素坐標方差為1.22n，則動態點xp2到極線l2的距離平方d2應滿足式（5），由此進一步確定所有潛在動態特征的動靜態特性。

點p在C1、C2相機坐標系下的坐標為Pc1、Pc2，其滿足：

式中：T c1_w、T c2_w分別為世界坐標系到C1、C2相機坐標系的轉換矩陣。

點q在C2相機坐標系下坐標為Q c2。P c2、Q c2滿足自由度為3的卡方分布，取95%置信度，其拒絕域為7.81，則動態點P c2滿足：

3 試驗與結果分析

本文中采用MHP-v2［22］數據集對Fast-SCNN語義分割網絡進行訓練，MHP-v2采集了真實世界場景中不同視角、不同位姿、不同遮擋情況和不同場景下的行人數據，其共包含25 403張圖片，訓練時訓練集、驗證集、測試集分別包含15 403張、5 000張、5 000張圖片。Fast-SCNN訓練過程在服務器上進行，其CPU為英特爾至強E5-2683，內存為64GB，同時搭載4張顯存為12G的TITANX Pascal顯卡。

SLAM效果利用標準數據集與實車采集數據進行評估。標準數據集采用慕尼黑大學提供的Tum數據集，具體使用其中的walking_static、walking_xyz、sitting_xyz、walking_halfsphere序列。序列中，sitting、walking分別代表數據集中的人坐和行走的不同狀態，static、xyz、halfsphere分別表示相機靜止、沿XYZ軸運動、沿半球運動的狀態。

實車數據集利用圖3（a）所示的試驗小車采集，其上搭載了ZED-2i雙目攝像頭以及華測-410組合慣導。數據集中包含一段長度為402.3 m、頻率為15 Hz、分辨率為720×1280的雙目數據，以及組合慣導在RTK信號下輸出的厘米級真實軌跡。數據集拍攝場景見圖3（b），場景中各目標在相機視野內時而靜止時而運動且隨機占據不同比例的相機視野，以評估算法在實際動態場景下的運行效果。

分割網絡和SLAM試驗的運行均在本地筆記本上，其CPU為英特爾i5-8300H，內存為8GB，顯卡為GTX 1050ti，顯存為4GB。

3.1 Fast-SCNN網絡訓練及性能評估

Fast-SCNN網絡使用pytorch-1.7深度學習平臺搭建，訓練時，采用Adam［23］優化器，學習率依據式（8）poly函數進行變換。

式中：lr為新的學習率；base_lr為基準學習率，設置為0.045；epoch為當前整個數據集的訓練輪次；num_epoch為整個數據集總訓練輪次，設置為200；power為指數衰減因子，設置為0.9。

數據增強部分，對輸入圖像進行比例為0.5～2的隨機縮放、50%概率的水平翻轉、50%概率的高斯模糊以及隨機裁剪，每張顯卡上每個訓練批次輸入80張圖片。

試驗采用像素精度（pixel accuracy，PA）、平均交并比（mean intersection over union，MIOU）和運行時間3個評價指標在MHP-v2驗證集上對分割網絡的訓練結果進行評估。像素精度與平均交并比的計算公式分別如式（9）和式（10）所示。

式中：pij為類別為i、預測為j的像素；pji為類別為j、預測為i的像素；k為總類別數。

Fast-SCNN網絡訓練結果如表1所示，其像素精度與平均交并比均超過90%，每張圖片的處理時間約14.5 ms，滿足動態對象分割的精度和時延要求，其部分分割測試結果如圖4所示。

表1 Fast-SCNN訓練結果

3.2 在Tum與實車數據集上的試驗

本文利用絕對位姿誤差（absolute pose error，APE）計算每一時刻算法輸出的相機位姿與真值之間的絕對誤差，同時，提供了誤差平均值（mean）、中值（median）、均方根誤差（root mean squared error，RMSE）、標準偏差（standard deviation，STD），以全面客觀地反映算法性能。

圖5展示了在walking_halfsphere序列上分別使用ORB-SLAM3算法和本文算法的運行結果。對比圖5（a）和圖5（b）可以看出，ORB-SLAM3在該動態環境下難以準確恢復相機位姿，其估計的相機軌跡與真實軌跡偏離較大，而基于本文算法剔除動態特點后，SLAM得到的相機軌跡和真實軌跡基本重合。觀察圖5（c）和圖5（f）可以看出，本文算法在各坐標軸與各方向上均有效減少了動態特征點對原有SLAM系統的干擾，保證SLAM在動態環境的穩定運行。同時由圖5（d）和圖5（e）可以看出，對比ORBSLAM3，本文算法的絕對位姿誤差峰值小、波動平穩，且在各類統計量上均有一個數量級上的減小。

表2提供了在各個數據集上本文算法與ORBSLAM3關于絕對位姿誤差的具體數值對比，表中的提升率采用式（11）計算。可以看出，在Tum數據的walking動態序列和實車混合場景數據中，本文算法在平均值（mean）、中值（median）、均方根誤差（RMSE）、標準偏差（STD）這些統計量上的平均提升率均在80%以上；在Tum sitting_xyz序列中，由于人坐在椅子上交談并未隨意行走，整個場景處于低動態狀態，因此兩種算法的精度都較高，但本文算法能在保留人身上靜止部分特征點（如不動的軀干）的同時剔除其運動部分的特征點（如手），因而也提升了相應的試驗精度；在實車復雜場景中，本文算法在各項指標上也均有大幅提升。試驗結果充分說明了本文算法在動態場景下對于SLAM算法精度與魯棒性的顯著提升作用。

式中：ur為提升率；Δ1為ORB-SLAM3上的相關誤差項；Δ2為本文算法的相關誤差項。

4 結論

針對動態場景，本文中提出了一種結合輕量級Fast-SCNN語義分割網絡與運動一致性約束的SLAM方法，該方法首先通過分割網絡獲取潛在動態區域掩碼并進行特征點去除，從而獲取相機的初步位姿估計結果，隨后基于運動一致性約束與卡方檢驗，完成動態特征點的進一步篩選，以進一步優化相機位姿估計結果。本文中訓練的語義分割網絡像素精度與平均交并比均在90%以上，分割耗時約14.5 ms，滿足SLAM系統的實時分割要求。同時，通過將本文算法與ORB-SLAM3系統融合，并在Tum動態場景公共數據集和實車動態場景數據集上試驗，本文提出的算法在各項指標的平均提升均超過了80%，顯著提升了ORB-SLAM3在動態場景的運行精度與魯棒性，有效拓寬了現有視覺SLAM的應用領域，有助于保證智能車輛運行安全。

未來將結合TensorRT工具與模型剪枝，進一步提高算法的運行效率。