近日我們團隊參加的 IROS 2021 The HILTI SLAM 挑戰賽獲得了最終的冠軍。本文是對於本次比賽的算法的實驗和思考，版權所有：曠視科技。原文鏈接：https://zhuanlan.zhihu.com/p/420192586

近日，IROS 2021 The HILTI SLAM 挑戰賽公佈了競賽結果，曠視研究院榮獲此次挑戰賽的冠軍。這是繼去年包攬 CVPR 2020 SLAM 挑戰賽全部賽道的冠軍後，曠視研究院再次獲得頂會 SLAM 競賽的冠軍。

2021 HILTI SLAM 挑戰賽是國際頂級會議——智能機器人與系統國際會議（IEEE/RSJ International Conference on Intelligent Robots and Systems，以下簡稱IROS）中的一項競賽，由蘇黎世理工和 HILTI 聯合舉辦。IROS 成立於 1988 年，每年舉辦一次，已發展成為全球規模最大、影響力最強的機器人研究會議之一。

1 賽題介紹

即時定位與地圖構建（Simultaneous Localization and Mapping，SLAM）是機器人導航和執行任務所必需的基本能力之一。雖然近些年來基於幾何的方法和基於學習的 SLAM 方法都取得較大的進展，但在真實世界中部署一套精準且魯棒的SLAM算法，仍然是一個具有挑戰性的問題。在這次挑戰賽中，HILTI 提出了一個新的數據集，包含 12 個序列。每個序列都代表不同階段的施工現場，如地下室、校園、建築工地、辦公室、實驗室、露天停車場等。參賽隊伍可以使用數據集中提供的任何傳感器設計 SLAM 算法。挑戰賽主辦方會根據參賽隊伍軌跡的完整性和精度進行評分，並按照總分的高低進行排名。

2 曠視奪冠算法揭秘

本次比賽吸引了全球20支企業和高校隊伍參賽。最終，我們採用基於雙激光+IMU 的 SLAM 算法作為參賽方案，經過激烈的競爭最後獲得了競賽冠軍。

獲獎證書：

Leader Board：

我們分析了比賽數據集提供的傳感器信息。其中視覺模組為 SevenSense 的五目相機模組，該模組能夠提供豐富的紋理信息，但對光照變化比較敏感，特別是在室外環境。而 3D 激光傳感器在不同光照下均能夠提供魯棒和精確的距離測量，更適合該數據集。另外將這5個相機進行融合工作量較大，短期內無法獲得一個滿意的結果。因此，我們選擇 3D 激光雷達作為主要的傳感器。

數據集中提供了兩個激光雷達，其中 Ouster 具有360度的視場角，能夠提供稠密的測量；而 Livox MID70 的視場角僅有70度，但它的測量範圍長達260米。使用單一的激光雷達容易出現退化的情況，即使利用 IMU 能夠保證短時間內的精度，但整體的定位精度會受到影響。如下圖（左），僅使用 Ouster 激光，在空曠的環境中能夠定位的特徵很少；下圖（右），僅使用 Livox 激光，在特定數據集中視野受限。因此，我們最終選擇了雙激光+IMU的方案。

整體方案如下，這是一個基於濾波的激光慣導里程計：主要分為數據同步、畸變補償、濾波器的預測更新、地圖管理等步驟。

實際數據中，兩個激光並不是完全同步的，時間戳相差 10-20ms。因此第一步是將 Livox 的激光點雲按照每個點的時間戳重新組織（即切割 Livox 的點雲數據），與 Ouster 的時間戳進行對齊。接著在 Ouster 一幀激光的測量時間內，利用 IMU 測量進行狀態更新。狀態更新的數據可以去點雲的運動畸變。然後將 Livox 去畸變的點雲轉換到 Ouster 坐標系下，與 Ouster 的激光點雲進行合併。數據同步步驟尤為關鍵，大大簡化了程序的處理流程。

在濾波器更新階段，採用點面距離構建測量模型進行更新。其中，每個點對應的平面是根據 K 近鄰搜索和 PCA 擬合平面獲得的。在獲得優化的位姿後，將當前幀的點雲轉換到全局坐標系下並插入地圖中。由於數據集的場景並不算太大（最大的場景為 100*100 米），同時為了程序的效率，我們採用一個增量式的 KD 樹維護全局地圖。為了保持全局地圖的稀疏性，只有離地圖中已存在的點較遠的點雲才可以插入。

3 亮點總結

1 雙激光的結合大幅度地提升了程序的魯棒性和精度；
2 濾波器能夠即時估計出IMU的偏置和速度，能夠提升畸變補償的精度，反過來提升定位建圖的精度；
3 由於全局地圖的存在，不需要回環模組。程序得到了簡化。

最後為大家展示一下利用算法構建的地圖：

參考文獻

1. Helmberger, Michael, et al. "The Hilti SLAM Challenge Dataset." arXiv preprint arXiv:2109.11316 (2021).
2. Xu, Wei, et al. "Fast-lio2: Fast direct lidar-inertial odometry." arXiv preprint arXiv:2107.06829 (2021).
3. Zhang, Ji, and Sanjiv Singh. "LOAM: Lidar Odometry and Mapping in Real-time." Robotics: Science and Systems. Vol. 2. No. 9. 2014.
4. Hackel, Timo, Jan D. Wegner, and Konrad Schindler. "Fast semantic segmentation of 3D point clouds with strongly varying density." ISPRS annals of the photogrammetry, remote sensing and spatial information sciences 3 (2016): 177-184.
5. Cai, Yixi, Wei Xu, and Fu Zhang. "ikd-Tree: An Incremental KD Tree for Robotic Applications." arXiv preprint arXiv:2102.10808 (2021).
6. Mourikis, Anastasios I., and Stergios I. Roumeliotis. "A multi-state constraint Kalman filter for vision-aided inertial navigation." Proceedings 2007 IEEE International Conference on Robotics and Automation. IEEE, 2007.