1 動機與貢獻

現有很多LiDAR里程計都依賴于某種形式的ICP估計幀間位姿，例如CT-ICP, LOAM等。現有的系統設計需要對機器人的運動(CT-ICP)和環境的結構(LeGO-LOAM)有一些特定的假設。而且幾乎沒有系統能夠不需要調參（例如特征提取、面特征擬合、法向量估計、畸變矯正）就能用于不同的場景、不同的LiDAR、不同的運動模式、以及不同種類的機器人（例如地面和空中機器人）。

與現有很多工作增加里程計的復雜度相反，本文通過去除大部分部件并專注于核心元素，回顧1992年最初提出的ICP方法，研究阻礙其泛化性能的根本原因，得到了一個簡單且非常高效的系統，并且可以使用不同的LiDAR傳感器在各種環境條件下運行（無人車、無人機、兩輪車Segway、手持固態LiDAR）。

提出的里程計估計方法基于point-to-point ICP，結合了自適應閾值進行對應匹配、魯棒核、簡單但廣泛適用的運動補償方法和點云下采樣策略。 和現有很多SLAM系統不同，本文的系統不用精巧的特征提取，學習方法，也不用回環檢測。

整個系統參數較少，在大多數情況下甚至不需要調整到特定的LiDAR傳感器。 不需要集成IMU信息，只需要從各種3D LiDAR傳感器獲得的3D點云數據，因此能夠滿足廣泛的不同應用和操作條件。 系統運行速度比所有數據集中的傳感器幀率都快，并且是為現實場景而設計的。 KISS-ICP (keep it small and simple):

與SOTA里程計系統相當

同一套參數可以用于不同的機器人、不同的環境和運動模式

不依賴IMU或輪速計的高效運動補償方法

2 方法

激光里程計主要步驟：

運動估計和運動補償（畸變矯正）

幀下采樣

使用自適應閾值估計scan到local map的對應關系，對可能的數據關聯做限制，濾除可能的異常值

用魯棒的point-to-point ICP進行scan與local map的配準

將下采樣的scan更新到local map中

2.1 運動預測和幀畸變矯正

不使用IMU或輪速計，用恒速模型進行運動補償，主要有兩個原因：

應用廣泛，不需要其他傳感器，也就不需要和其他傳感器做時間同步

對于獲得LO的初始值和畸變矯正足夠了，因為通常LiDAR頻率在10Hz到20Hz（50ms~100ms），大多數情況下，加速度或者減速度在短時間內與恒速模型的差距相對較小

用前兩幀的相對位姿預測當前幀與前一幀的相對位姿，t-1到t-2的相對位姿為：



速度和角速度：



畸變矯正（投影到掃描開始）：



2.2 點云下采樣

local map的voxel 大小是v，對于scan，先用alpha * v (0 < alpha <= v)的voxel大小進行下采樣, 然后 用beta * v (1.0 <= beta <= 2.0)的voxel大小進行下采樣，兩次下采樣的想法源于CT-ICP。 大多數體素下采樣方法保留voxel的中心點，不一定位于原點云上，本文實驗發現保留原點云的點效果要好一點，因此在實現中，保留第一個插入voxel里的點。

2.3 自適應閾值的scan到local map對應關系估計

用大小為v的voxel存儲局部地圖，每個voxel最多存N_max個點，scan配準到local map后，用估計的位姿將上述第一次下采樣的點云加入到local map中，如果voxel超過了傳感器最大距離r_max, 就從local map去除。 一般ICP方法都用某一最大距離范圍（例如1m或2m）內的最近鄰做數據關聯，這個閾值的確定需要考慮初始位姿誤差、動態物體種類和數量、以及傳感器噪聲等，通常是根據經驗設定的。

基于恒速運動預測模型，可以估計運動估計的初值與ICP糾正的偏差大小，但這個是不能提前知道的。直觀地，可以觀察機器人在該偏差上的大小的加速度，如果機器人沒有加速，該偏差的大小會很小，接近0，ICP基本不需要做糾正。 將該信息集成到數據關聯中，估計兩幀之間關聯點的距離：



旋轉部分的偏差對應在掃描最大距離上偏差的大小。



式（5）給出了點偏差的上界（根據三角不等式）：



為了計算t時刻的閾值，假設式（5）的值服從高斯分布，根據已有的軌跡，只考慮當偏差大于delta_min（恒速運動和真實運動相差較大）時，得到標準差：



只考慮偏差大于delta_min避免了由于機器人靜止或者勻速運動很長時間使得標準差估計得太小。實驗中將delta_min設為0.1m。最后估計的閾值設為



用于數據關聯的最近鄰搜索時的距離閾值。

2.4 通過魯棒優化配準

先通過預測的相對位姿和上一幀的里程計位姿把點云轉到全局坐標系（也可以是局部子圖坐標系）



每次迭代時，點到點殘差做ICP配準的優化問題為：



rho是Geman-McClure魯棒核函數，一種具有很強的外點剔除能力的M估計器:



第j步迭代完后更新點云



然后重復迭代數據關聯、優化，直到滿足收斂條件。 最后里程計的位姿為：



式（5）中運動預測和ICP糾正之間的相對位姿為：



通常ICP會設定一個最大迭代次數，或者加上迭代結果與最小變化量的比較，本文認為限制這些約束可能使ICP不一定能收斂到好的結果，而且可能會累積漂移。

因此，本文的終止條件為迭代結果小于gamma，而限制迭代次數。 最后用ICP糾正后的位姿將第一次下采樣的幀加入到local map。

算法的參數設定：



r_max依賴于傳感器種類。

3 實驗

公開數據集對比：



運動補償：



自適應數據關聯閾值：

審核編輯：劉清