Try   HackMD

基於智慧行動裝置的視覺 SLAM 與光達點雲匹配之定位整合研究

林晉暘1, 吳庭鈞2, 江凱偉3

一、摘要

自動駕駛汽車在商業化的過程中,許多車廠試圖避免使用光達等高成本感測器,轉而使用相機作為感測器,因此整合相機與高精地圖成為一項熱門的替代方案。本研究提出以手機等級相機感測器整合光達的即時整合定位方案,基於 RGB-D SLAM 的相機位姿作為初始匹配估計,在先驗的光達地圖中進行多線程優化的 ndt_omp 點雲匹配,並以滑動窗口的方式解決相機視角不足的問題,強化定位的穩定性。實驗證明,此方案可以大幅減少視覺 SLAM 的誤差漂移,相較原先視覺 SLAM 提升 65% 的準確度。

二、研究動機

隨著自動駕駛汽車的蓬勃發展,高精度的車輛即時定位備受關注。雖然 GNSS 可以在全球範圍內進行準確的定位,但由於城市中的多路徑效應與訊號脫落,以及室內無法接收訊號,將導致定位成果不佳,無法符合自動駕駛對定位精度與穩定性的要求,因此整合其他感測器 (Sensor Fusion) 的研究受到重視。高精地圖(HD Map)提供理想的點雲和語意地圖,利用此地圖可以相對容易的實現光達定位,並且融合 INS/GNSS 和輪速記等定位方法後,可以滿足自駕車的定位精度與穩定性需求。然而,光達雖然提供高精度的距離量測,但其價格十分昂貴,導致許多車廠試圖避免使用高成本的傳感器,轉而使用成本低且應用性廣的相機。智慧行動裝置作為最普遍的相機感測器,同時具有獨立運算能力並搭載多樣的感測器。而Google 於 2017 年發表的 Tango 智慧行動裝置,配合搭載的 RGB-D 相機,提供智慧行動裝置對周遭空間與動態的感知能力。因此,本研究提出以 Google 的 Tango 智慧行動裝置作為感測器的低成本相機-光達即時整合定位方案,通過 RGB-D SLAM 的相機位姿作為初始匹配估計,在先驗的光達地圖中進行多線程優化的 ndt_omp 點雲匹配,並以滑動窗口的方式解決相機視角不足的問題,強化定位的穩定性。成果顯示有效地減少視覺 SLAM 的累積誤差,並達到更高的精度。

三、文獻回顧

機器人領域中,在給定的地圖中今行感測器的定位一直是非常熱門的技術。由於光達精確的距離量測,同時對光照變化的穩定特性,使其在地圖定位有不錯的效果,擴展其應用領域。然而,光達感測器的高成本很大程度上阻礙其廣泛部署。而低成本的相機同樣可以實現地圖定位,借助視覺特徵匹配的閉環檢測 (Loop Closuring),同樣可以在建立好的地圖下進行定位,並有效的抑制累積誤差。不過其在尺度估計的準確性仍不及光達,導致效果不竟理想。因此,越來越多單位嘗試整合兩個感測器的特性,藉由第三方提供準確的先驗光達地圖,融合以提高視覺系統的定位精度,達到考量成本與精度的平衡方案。

在光達地圖中的相機定位主要處理 2D 和 3D 之間的對應關係,而目前相機在光達地圖中的定位方案,分為兩種類別。第一種方法是在 2D 空間中進行匹配,此方法通常用於單目相機,為了處理沒有相關視覺特徵的光達地圖,通常使用點雲外觀或強度合成圖像,再透過特徵匹配或光度誤差進行定位。這些定位方法都試圖將 2D-3D 匹配問題轉化為 2D-2D 匹配問題。然而,外觀和視覺特徵對光照變化和光照條件很敏感,這使得長期相機定位的對應關係不穩定。第二種方法是透過額外感測器 (IMU、深度相機) 獲得尺度較為精確的密集點雲,再透過 3D 空間的點雲匹配 (ICP、NDT) 進行定位。與僅基於視覺的方法相比,這些通過 3D 配準的定位方法獲得了較可行的定位結果。

因此,本研究提出基於手機等級 RGB-D 相機的低成本相機即時定位方案,由手機運算進行 RGB-D SLAM 並計算每幀的點雲,以相機位姿作為匹配估計,與光達地圖進行 NDT 點雲匹配。為了彌補相機不如光達的視野範圍廣,利用移動距離選定關鍵幀,再透過滑動窗口進行點雲拼接,強化匹配穩定性。此外,本研究提出的方法藉由多線程的 ndt_omp,使系統再不需要 GPU 的支持下達到即時的幀率。

四、研究方法

本研究提出的方法通過手機運算能力先進行 RGB-D SLAM,利用相機位姿與生成之密集點雲建立一套後端即時優化系統。演算法的架構分為離線階段與在線階段,先在離現階段基於光達 A-LOAM 建立點雲地圖。接著在線階段,利用手機 RGB-D SLAM 即時計算相機位姿與當前幀的點雲,輸入計算的相機位姿作為匹配估計。由於相機點雲不如光達點雲的視角(FOV)廣,因此在進行 NDT 匹配時,容易產生匹配錯誤,所以本研究提出利用滑動窗口拼接當前幀與關鍵幀點雲,將拼接點雲與光達地圖進行 NDT 點雲匹配,並更新整合位姿,以此強化匹配穩定性。而關鍵幀的選擇是基於距離移動距離大於門檻值加入,並維持窗口大小,刪除較久遠的關鍵幀。

Image Not Showing Possible Reasons
  • The image file may be corrupted
  • The server hosting the image is unavailable
  • The image path is incorrect
  • The image format is not supported
Learn More →
圖 1、Visual-NDT 架構圖
  1. 建立離線光達點雲地圖:
    LOAM 為基於光達搭建在 ROS 平台下的 SLAM 系统。特徵點提取的方式是根據平滑度來計算;使用相鄰點雲特徵點匹配,取得兩次掃描之間的位姿轉換。而 A-LOAM 是改良自 LOAM 的一項開源演算法,它使用 Eigen 和 Ceres Solver 提供的函數來簡化代碼結構。本研究使用 A-LOAM 演算法作光達的定位及製圖,以成果地圖作為實驗的先驗光達點雲圖 ( PCtarget = PCmap );軌跡作為實驗成果的精度基準。
  2. Tnago RGB-D SLAM 資料處理:
    Tango 裝置提供多種 API 可以用於定位與製圖。其中 Tango 於 ROS 系統傳送的 tf topic,其取樣頻率為 206Hz,此 API 可以取代相機的視覺里程,進行 SLAM 的前端計算,同時也提供每一幀由深度感測器解算的點雲。因此,利用智慧行動裝置獨立運算的特點,本研究以 Tango 的 API 計算相機位姿與點雲。
  3. NDT 點雲匹配優化:
    本研究採取以相機的點雲與光達點雲圖進行 NDT 匹配,匹配演算法使用基於多線程優化的 ndt_omp,根據實驗該演算法效率優於傳統的 PCL NDT,更容易實現即時點雲匹配,且相較於 GPU 優化的 NDT 演算法更適合部屬在沒有 GPU 的系統中。先以視覺 SLAM 計算的相機位姿轉換 ( Tcamera ) 作為初始匹配估計,進行局部的匹配。
    Tguess = Tcurr * Tcamera
    再利用滑動窗口的方式將當前幀的點雲與關鍵幀的點雲拼接在一起,並將拼接後的點雲與光達點雲進行匹配 ( Tndt ),之後基於移動距離選擇當前幀是否為關鍵幀。
    PCsource = PCcurr + PCKey
    Tndt = NDT( PCsource, PCtarget, Tguess )
Image Not Showing Possible Reasons
  • The image file may be corrupted
  • The server hosting the image is unavailable
  • The image path is incorrect
  • The image format is not supported
Learn More →
圖 2、點雲匹配 Banckmark

五、實驗結果

此實驗以 Turtlebot 作為移動載台,載台上裝載的智慧行動裝置選擇使用 Lenovo Phab2 pro;光達使用 Velodyne VLP-16。,實驗場域為成功大學整合式定位技術發展實驗室,該場地包含連續轉彎以及直線走廊,實驗的移動距離約為 22 公尺。以 0.05 公尺作為關鍵幀篩選標準,滑動窗口的窗口大小為 10 幀,作為此次實驗的參數。先將光達的點雲用 A-LOAM 建立點雲地圖,再將此地圖匯入本研究的演算法進行計算,最後比較 LiDAR-SLAM、V-SLAM 和本研究的算法 (V-NDT) 的軌跡精度。

Image Not Showing Possible Reasons
  • The image file may be corrupted
  • The server hosting the image is unavailable
  • The image path is incorrect
  • The image format is not supported
Learn More →
Image Not Showing Possible Reasons
  • The image file may be corrupted
  • The server hosting the image is unavailable
  • The image path is incorrect
  • The image format is not supported
Learn More →
圖 3、移動平台配置圖 圖 4、實驗場域光達製圖成果

誤差結果顯示,V-NDT 軌跡 RMSE = 0.390 公尺,相較原先 V-SLAM 的軌跡 RMSE = 1.174 公尺提升了 65% 的準確度。從軌跡成果來看,V-NDT 在轉彎處的誤差較大,這是由於 V-SLAM 的尺度估計不準確,影響點雲的品質,導致匹配成果不佳。

Image Not Showing Possible Reasons
  • The image file may be corrupted
  • The server hosting the image is unavailable
  • The image path is incorrect
  • The image format is not supported
Learn More →
Image Not Showing Possible Reasons
  • The image file may be corrupted
  • The server hosting the image is unavailable
  • The image path is incorrect
  • The image format is not supported
Learn More →
Image Not Showing Possible Reasons
  • The image file may be corrupted
  • The server hosting the image is unavailable
  • The image path is incorrect
  • The image format is not supported
Learn More →
Image Not Showing Possible Reasons
  • The image file may be corrupted
  • The server hosting the image is unavailable
  • The image path is incorrect
  • The image format is not supported
Learn More →
圖 5、軌跡誤差分析圖

六、結論

本研究提出利用手機等級的感測器,基於 RGB-D SLAM 的相機位姿作為初始匹配估計,在先驗的光達地圖中進行 NDT 三維點雲匹配定位,並以滑動窗口的方式解決相機視角不足的問題,強化定位的穩定性。透過真實世界的數據驗證,以三維點雲匹配的方式極大的減少視覺 SLAM 的累積誤差,證明整合光達與相機的優勢為有效的低成本定位整合方案。而本研究未來可以整合擴展卡漫濾波器,並針對相機點雲的尺度問題進行修正。