點(diǎn)到點(diǎn)ICP做對(duì)了也能實(shí)現(xiàn)簡(jiǎn)單準(zhǔn)確且魯棒的激光里程計(jì)

1 動(dòng)機(jī)與貢獻(xiàn)

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

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

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

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

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

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

不依賴IMU或輪速計(jì)的高效運(yùn)動(dòng)補(bǔ)償方法

2 方法

激光里程計(jì)主要步驟：

運(yùn)動(dòng)估計(jì)和運(yùn)動(dòng)補(bǔ)償（畸變矯正）

幀下采樣

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

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

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

2.1 運(yùn)動(dòng)預(yù)測(cè)和幀畸變矯正

不使用IMU或輪速計(jì)，用恒速模型進(jìn)行運(yùn)動(dòng)補(bǔ)償，主要有兩個(gè)原因：

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

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

用前兩幀的相對(duì)位姿預(yù)測(cè)當(dāng)前幀與前一幀的相對(duì)位姿，t-1到t-2的相對(duì)位姿為：



速度和角速度：



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



2.2 點(diǎn)云下采樣

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

2.3 自適應(yīng)閾值的scan到local map對(duì)應(yīng)關(guān)系估計(jì)

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

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



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



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



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



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



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

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

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



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



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



第j步迭代完后更新點(diǎn)云



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



式（5）中運(yùn)動(dòng)預(yù)測(cè)和ICP糾正之間的相對(duì)位姿為：



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

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

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



r_max依賴于傳感器種類。

3 實(shí)驗(yàn)

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



運(yùn)動(dòng)補(bǔ)償：



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

審核編輯：劉清