1 動機與貢獻

現(xiàn)有很多LiDAR里程計都依賴于某種形式的ICP估計幀間位姿，例如CT-ICP, LOAM等。現(xiàn)有的系統(tǒng)設(shè)計需要對機器人的運動(CT-ICP)和環(huán)境的結(jié)構(gòu)(LeGO-LOAM)有一些特定的假設(shè)。而且?guī)缀鯖]有系統(tǒng)能夠不需要調(diào)參（例如特征提取、面特征擬合、法向量估計、畸變矯正）就能用于不同的場景、不同的LiDAR、不同的運動模式、以及不同種類的機器人（例如地面和空中機器人）。

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

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

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

與SOTA里程計系統(tǒng)相當

同一套參數(shù)可以用于不同的機器人、不同的環(huán)境和運動模式

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

2 方法

激光里程計主要步驟：

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

幀下采樣

使用自適應閾值估計scan到local map的對應關(guān)系，對可能的數(shù)據(jù)關(guān)聯(lián)做限制，濾除可能的異常值

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

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

2.1 運動預測和幀畸變矯正

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

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

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

用前兩幀的相對位姿預測當前幀與前一幀的相對位姿，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。 大多數(shù)體素下采樣方法保留voxel的中心點，不一定位于原點云上，本文實驗發(fā)現(xiàn)保留原點云的點效果要好一點，因此在實現(xiàn)中，保留第一個插入voxel里的點。

2.3 自適應閾值的scan到local map對應關(guān)系估計

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

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



旋轉(zhuǎn)部分的偏差對應在掃描最大距離上偏差的大小。



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



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



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



用于數(shù)據(jù)關(guān)聯(lián)的最近鄰搜索時的距離閾值。

2.4 通過魯棒優(yōu)化配準

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



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



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



第j步迭代完后更新點云



然后重復迭代數(shù)據(jù)關(guān)聯(lián)、優(yōu)化，直到滿足收斂條件。 最后里程計的位姿為：



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



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

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

算法的參數(shù)設(shè)定：



r_max依賴于傳感器種類。

3 實驗

公開數(shù)據(jù)集對比：



運動補償：



自適應數(shù)據(jù)關(guān)聯(lián)閾值：

審核編輯：劉清