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 點雲匹配，並更新整合位姿，以此強化匹配穩定性。而關鍵幀的選擇是基於距離移動距離大於門檻值加入，並維持窗口大小，刪除較久遠的關鍵幀。
| 圖 1、Visual-NDT 架構圖 |
- 建立離線光達點雲地圖：
LOAM 為基於光達搭建在 ROS 平台下的 SLAM 系统。特徵點提取的方式是根據平滑度來計算；使用相鄰點雲特徵點匹配，取得兩次掃描之間的位姿轉換。而 A-LOAM 是改良自 LOAM 的一項開源演算法，它使用 Eigen 和 Ceres Solver 提供的函數來簡化代碼結構。本研究使用 A-LOAM 演算法作光達的定位及製圖，以成果地圖作為實驗的先驗光達點雲圖 ( PCtarget = PCmap )；軌跡作為實驗成果的精度基準。 - Tnago RGB-D SLAM 資料處理：
Tango 裝置提供多種 API 可以用於定位與製圖。其中 Tango 於 ROS 系統傳送的 tf topic，其取樣頻率為 206Hz，此 API 可以取代相機的視覺里程，進行 SLAM 的前端計算，同時也提供每一幀由深度感測器解算的點雲。因此，利用智慧行動裝置獨立運算的特點，本研究以 Tango 的 API 計算相機位姿與點雲。 - NDT 點雲匹配優化：
本研究採取以相機的點雲與光達點雲圖進行 NDT 匹配，匹配演算法使用基於多線程優化的 ndt_omp，根據實驗該演算法效率優於傳統的 PCL NDT，更容易實現即時點雲匹配，且相較於 GPU 優化的 NDT 演算法更適合部屬在沒有 GPU 的系統中。先以視覺 SLAM 計算的相機位姿轉換 ( Tcamera ) 作為初始匹配估計，進行局部的匹配。
Tguess = Tcurr * Tcamera
再利用滑動窗口的方式將當前幀的點雲與關鍵幀的點雲拼接在一起，並將拼接後的點雲與光達點雲進行匹配 ( Tndt )，之後基於移動距離選擇當前幀是否為關鍵幀。
PCsource = PCcurr + PCKey
Tndt = NDT( PCsource, PCtarget, Tguess )
| 圖 2、點雲匹配 Banckmark |
五、實驗結果
此實驗以 Turtlebot 作為移動載台，載台上裝載的智慧行動裝置選擇使用 Lenovo Phab2 pro；光達使用 Velodyne VLP-16。，實驗場域為成功大學整合式定位技術發展實驗室，該場地包含連續轉彎以及直線走廊，實驗的移動距離約為 22 公尺。以 0.05 公尺作為關鍵幀篩選標準，滑動窗口的窗口大小為 10 幀，作為此次實驗的參數。先將光達的點雲用 A-LOAM 建立點雲地圖，再將此地圖匯入本研究的演算法進行計算，最後比較 LiDAR-SLAM、V-SLAM 和本研究的算法 (V-NDT) 的軌跡精度。
| 圖 3、移動平台配置圖 | 圖 4、實驗場域光達製圖成果 |
誤差結果顯示，V-NDT 軌跡 RMSE = 0.390 公尺，相較原先 V-SLAM 的軌跡 RMSE = 1.174 公尺提升了 65% 的準確度。從軌跡成果來看，V-NDT 在轉彎處的誤差較大，這是由於 V-SLAM 的尺度估計不準確，影響點雲的品質，導致匹配成果不佳。
| 圖 5、軌跡誤差分析圖 |
六、結論
本研究提出利用手機等級的感測器，基於 RGB-D SLAM 的相機位姿作為初始匹配估計，在先驗的光達地圖中進行 NDT 三維點雲匹配定位，並以滑動窗口的方式解決相機視角不足的問題，強化定位的穩定性。透過真實世界的數據驗證，以三維點雲匹配的方式極大的減少視覺 SLAM 的累積誤差，證明整合光達與相機的優勢為有效的低成本定位整合方案。而本研究未來可以整合擴展卡漫濾波器，並針對相機點雲的尺度問題進行修正。
