1 動機與貢獻

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

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

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

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

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

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

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

2 方法

激光里程計主要步驟：

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

幀下采樣

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

用魯棒的point-to-point ICP進行scan與local map的配準(zhǔn)

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

2.1 運動預(yù)測和幀畸變矯正

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

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

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

用前兩幀的相對位姿預(yù)測當(dāng)前幀與前一幀的相對位姿，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 自適應(yīng)閾值的scan到local map對應(yīng)關(guān)系估計

用大小為v的voxel存儲局部地圖，每個voxel最多存N_max個點，scan配準(zhǔn)到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è)定的。

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



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



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



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



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



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

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

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



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



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



第j步迭代完后更新點云



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



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



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

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

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



r_max依賴于傳感器種類。

3 實驗

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



運動補償：



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

審核編輯：劉清