基于幾何約束和目標檢測的室內動態SLAM

高逸，王慶，楊高朝，劉鵬飛

(東南大學 儀器科學與工程學院,南京 210096)

0 引言

即時定位與地圖構建(SLAM)指系統在未知的環境中同時估計自身位置和構建周圍地圖的過程[1].它是實現移動機器人、自動駕駛、增強現實和無人機的核心技術[2].近幾年，以相機為傳感器的視覺SLAM受到研究者的重視，產生了一些顯著的成果[3-6].

傳統SLAM 通常假設周圍環境是靜止的，但真實環境的運動物體會給視覺里程計(VO)帶來錯誤的數據關聯，從而降低系統的精度和魯棒性，導致后續建立的地圖也無法使用[7].為了提高SLAM 系統在動態環境的效果，需要估計運動物體的位置，根據原理的不同可分為基于幾何約束和深度學習兩種方法[8].

幾何約束方法利用系統的靜態模型剔除離群的動態點，如SUN 等[9]通過隨機抽樣一致性(RANSAC)方法和相鄰圖片像素可以有效剔除低動態環境中孤立的動態點，但當動態點占據多數時，該方法難以將運動物體完全剔除.張慧娟等[10]利用匹配的特征點估計初始位姿，根據靜態線特征計算相機的真正運動，提高了系統在動態環境的定位精度.

深度學習方法通過神經網絡檢測并剔除運動物體上的動態點，如SHENG 等[11]通過Mask R-CNN(Regional CNN，CNN 最早應用于檢測和分割問題的方法)[12]識別環境中的動態區域，當某個像素及其鄰域都不屬于動態區域時才判定為靜態點，從而有效剔除了環境中的潛在動態點.YU 等[13]通過SegNet[14]語義分割網絡和運動一致性移除環境中的動態點，該方法在數據集上取得了不錯的結果，但卻不能在真實環境中實時運行.針對分割網絡不能滿足SLAM 系統實時性的問題，一些學者利用目標檢測網絡來獲取環境中的語義信息，從而提高了系統運行的效率.YANG 等[15]利用目標檢測網絡YOLOv3[16]移除環境中的運動物體，然后根據位姿進一步判斷特征點的真實狀態.ZHONG 等[17]通過檢測結果和運動概率模型剔除環境中的動態點，從而有效減少了運動物體的影響，但該方法不能剔除人為移動桌椅所導致的定位精度下降的影響.

幾何約束方法在低動態環境下可以取得不錯的效果，但當面對快速運動的物體時易出現錯誤.深度學習方法可以有效處理大多數動態環境問題，但當先驗的靜止物體運動時會保留大量動態點，而且許多基于分割的SLAM 系統通常無法在真實環境中實時運行.

針對上述問題，本文提出一種基于幾何約束和目標檢測的室內動態SLAM 方法.使用目標檢測網絡YOLOv5 獲取語義信息，提出運動物體漏檢的方法；根據先驗知識，提出準確識別動態區域的信息判定方法；結合幾何約束和深度學習方法剔除動態點，利用靜態點估計相機位姿；根據存儲信息構建可閉環的靜態地圖.

1 室內動態SLAM 方法

本文的系統框架如圖1 所示.目標檢測模塊使用YOLOv5 獲取語義信息，根據上一幀的檢測結果添加漏檢的運動物體；信息判定模塊根據檢測結果和先驗知識準確識別動態區域；動態點剔除模塊首先通過深度學習初步估計相機運動，然后利用幾何約束進一步剔除動態點，最終根據保留的靜態點計算真正的位姿；靜態地圖模塊根據存儲信息構建可閉環的靜態地圖.實線框為新增模塊，虛線框為已有模塊.

圖1 系統框架

1.1 輕量級網絡及運動物體漏檢方法

針對基于分割的SLAM 系統不能實時運行的問題，本文采用YOLOv5 網絡來獲取語義信息.與其他目標檢測算法不同，它利用一個端到端的網絡實現更快速的檢測任務.YOLOv5 作為YOLO 系列的最新版本，體積僅為原來的1/9，在V100 GPU 檢測速度達到400 FPS/s，完全滿足系統的實時性要求.

當運動物體移動到圖像邊緣時易出現漏檢，從而導致系統保留大量的動態點.針對上述問題，本文提出運動物體的漏檢方法，如果i時刻的運動物體數量和i1時刻數量不相等，根據下式判斷i時刻運動物體是否出現漏檢：

式中：x為i1 時刻的運動物體；y為i時刻的運動物體；y0為i時刻的所有運動物體的數量；Di1,x為i1 時刻運動物體的位置；Di,yj0＜y＜y0為i時刻所有運動物體的位置；dx為i1時刻運動物體和i時刻所有運動物體的最小距離；Xi1,x為i1 時刻運動物體的位移.

1.2 基于先驗知識的信息判定方法

信息判定方法根據檢測結果和先驗知識準確識別動態區域.室內物體根據先驗知識分為三類：運動物體、相對物體和靜止物體，如表1 所示.運動物體是具有主動運動狀態的物體，如人和寵物.相對物體是具有被動運動狀態的物體，如被人移動的椅子.靜止物體是具有絕對靜止狀態的物體，如不可移動的柜子和冰箱，拿起的書籍和杯子等具有被動運動狀態的小物體在幾何約束中當作離群值剔除，也視為靜止物體.

表1 室內物體

傳統方法僅剔除人所在的動態區域，在面對極端情況時易出現如下問題：

1) 當運動物體占據大部分區域時，提取的靜態特征點數量過少導致視覺里程計跟蹤失敗；

2) 當人移動椅子時，系統誤將其當作靜止物體，從而保留大量的動態特征點.

針對上述問題，本文提出準確識別動態區域的信息判定方法：

1) 當運動物體和靜止物體重疊時，兩者重疊的區域視為靜態，避免極端情況下靜態特征點數量不足的情況；

2) 當運動物體和相對物體重疊時，當滿足下式時，剔除相對物體的潛在動態點

式中：S重合表示運動物體和相對物體的重合面積；S相對表示相對物體的面積.

1.3 動態點剔除原理

動態點剔除原理如表2 所示.根據深度學習結果初步估計相機位姿，此時系統中仍存在少量動態點，需要結合幾何約束進一步剔除動態點，從而計算相機真正的運動.

表2 動態點剔除

極線約束原理如圖2 所示.空間點P在相機1 和相機2 的像素點分別為p1和p2，光心O1和光心O2的連線與像平面的交點分別為極點e1和極點e2，投影點和極點的連線分別為極線l1和極線l2.

圖2 極線約束

根據針孔相機模型，像素點p1和p2滿足：

式中：s1表示空間點P在相機1 的深度；s2表示空間點P在相機2 的深度；R和t表示相機1 轉換到相機2 的旋轉和平移.

像素點坐標轉換為歸一化坐標：

式中：K表示相機內參；x1表示空間點P在相機1 的歸一化坐標；x2表示空間點P在相機2 的歸一化坐標.

經過一系列變換得到極線約束：

式中：F表示相機1 轉換到相機2 的基礎矩陣.

計算像素點到極線的距離，若距離大于閾值 ε，剔除像素點的匹配關系：

式中：l2表示像素點p1在相機2 的極線；a、b、c分別表示極線l2的三維坐標；d表示像素點p2到極線l2的距離.

根據靜態特征點構建重投影誤差函數，計算相機的真實位姿為

式中：T表示相機的位姿；u表示像素坐標.

1.4 構建靜態地圖

傳統方法構建的地圖僅利用空間的幾何信息，不能有效剔除環境中的運動物體，導致地圖中出現運動物體的重影.本文方法根據目標檢測獲取的語義信息構建不包含運動物體的靜態地圖，當檢測到閉環時根據存儲的信息重新構建地圖，提高了系統在動態環境下的地圖效果，具體如表3 所示.

表3 靜態地圖

2 實驗與分析

為了驗證本文方法的效果，在TUM 數據集[18]和室內真實環境進行實驗.TUM 數據集包含兩種動態序列：在walking 高動態序列中，兩個人圍繞辦公桌來回走動并不時移動一些物體；在sitting 低動態序列中，兩個人坐在辦公桌旁一邊交談一邊打手勢.室內真實環境通過已標定的D435i 深度相機采集宿舍生活場景：一個人坐在椅子上，隨后起身，一邊交談一邊四處移動.

2.1 TUM 數據集

2.1.1 動態點剔除

本文方法和ORB-SLAM2 的動態點剔除結果，如圖3 所示.圖3(a)中，ORB-SLAM2 保留了人和移動椅子上的動態點，從而產生了大量的誤匹配，降低了系統的精度和魯棒性.圖3(b)中，本文方法利用幾何約束和深度學習有效剔除了行走的人和移動椅子上的動態點，僅使用靜態特征點估計位姿，從而實現了更好地定位效果.

圖3 本文方法和ORB-SLAM2 的動態點剔除

2.1.2 定位精度

系統在室內動態環境的定位精度使用絕對軌跡誤差(ATE)作為評價指標.為了降低不確定性的影響，最終結果取15 次運行的中間值.

本文方法和ORB-SLAM2 的定位精度，如表4所示.本文方法在高動態序列的定位精度比ORBSLAM2 提升了一個數量級，在低動態序列的提升則相對較小.原因在于：在高動態序列中人和椅子發生了明顯移動，本文通過深度學習和幾何約束有效剔除了動態點，而ORB-SLAM2 誤將其視為靜態區域從而產生大量的誤匹配.在低動態序列中人基本處于靜坐狀態，只有局部肢體發生運動，ORB-SLAM2 通過剔除少量離群點實現了準確定位，本文方法改進效果有限.

表4 本文方法和ORB-SLAM2 的定位精度

本文方法和ORB-SLAM2 在fr3/walking_xyz下的運行軌跡，分別如圖4～5 所示.圖4 中，ORB-SLAM2受運動物體影響導致估計軌跡和真實軌跡產生了較大的偏差.圖5 中，本文方法通過剔除動態點，估計軌跡和真實軌跡基本重合.

圖4 ORB-SLAM2 的運行軌跡

圖5 本文方法的運行軌跡

本文方法和DS-SLAM 的性能，如表5 所示.本文方法的運行時間遠小于面向動態環境的語義視覺SLAM(DS-SLAM)，原因在于：DS-SLAM 對每一幅圖片進行像素級別地處理，而本文方法通過YOLOv5縮短了獲取語義信息的時間，提高了系統運行的效率.本文和DS-SLAM 的定位精度互有優劣，但在walking_xyz和waling_rpy 中卻明顯優于DS-SLAM，原因在于：DS-SLAM 僅將人所在的區域視為動態而未考慮移動椅子產生的大量動態點，同時SegNet 分割網絡對運動物體的局部輪廓分割效果較差.本文通過深度學習剔除了人和移動椅子上的大部分動態點，利用幾何約束剔除了移動書籍和端起杯子的少量動態點，從而實現了高精度的定位結果.

表5 本文方法和DS-SLAM 的性能

2.1.3 靜態地圖

本文方法和ORB-SLAM2 在fr3/walking_xyz序列下的靜態地圖結果，如圖6 所示.圖6(a)中，ORBSLAM2 構建的靜態地圖因含有許多重影從而不能正常使用.圖6(b)中，本文方法根據相機位姿和語義信息剔除了環境中的運動物體，從而準確構建了周圍環境的靜態地圖.

圖6 本文方法和ORB-SLAM2 的靜態地圖

2.2 室內真實環境

2.2.1 動態點剔除

室內真實環境的動態點剔除結果，如圖7 所示.圖7(a)中，ORB-SLAM2 未能剔除人身上的大量動態點，從而降低了系統的定位精度，導致構建的靜態地圖也出現錯誤.圖7(b)中，本文方法有效剔除了人身上的動態點，根據保留的靜態特征點準確估計相機位姿.

圖7 室內真實環境的動態點剔除

2.2.2 靜態地圖

室內真實環境的靜態地圖結果，如圖8 所示.圖8(a)中，ORB-SLAM2 構建的地圖不僅含有人的重影，錯誤的定位結果也導致地圖的其他結構出現錯誤.圖8(b)中，本文方法構建的靜態地圖不僅有效剔除了環境中的運動物體，而且完整再現了室內的空間結構.

圖8 室內真實環境的靜態地圖

3 結束語

針對室內動態環境下定位精度低和地圖效果差的問題，提出一種基于幾何約束和目標檢測的室內動態SLAM 方法.使用YOLOv5 獲取語義信息，提出運動物體的漏檢方法；根據先驗知識，提出準確識別動態區域的信息判定方法；結合幾何約束和深度學習剔除動態點，利用靜態點估計相機位姿；根據存儲信息構建可閉環的靜態地圖.在TUM 數據集和室內真實環境進行實驗，有效提高了室內動態SLAM 的定位精度和地圖效果，但仍然存在一些缺陷，當環境特征缺失和相機運動過快時會出現跟蹤失敗的問題，后續可以通過添加線面特征和慣性測量單元(IMU)來提高系統的魯棒性.