本文主要是提出了一種新的實時激光雷達里程測量方法，稱為CT-ICP(連續時間ICP)，通過新穎的環路檢測程序完成完整的SLAM。該方法的核心是在掃描匹配中引入連續性和掃描間不連續的組合。它允許在配準過程中掃描的彈性變形，以提高精度，并增加了對不連續的高頻運動的魯棒性。在七個數據集上進行了測試:KITTI、KITTI-raw、KITTI-360、KITTICARLA、ParisLuco、Newer College和NCLT，分別用于駕駛和高頻運動場景。

1 引言

本文提出的目的：

使用勻速運動假設矯正點云畸變的方法無法適應大幅度的方向變化或者劇烈速度變化

即使目前有工作考慮了連續時間內的運動，但是也無法適應高頻的劇烈運動情況，或者以精度損失為代價。

本文主要貢獻：

提出了一種基于位姿掃描內連續性和掃描間不連續的彈性激光雷達里程計。

本文次要貢獻：

提出了一種基于密集點云的局部地圖，存儲在稀疏體素結構中，以獲得實時處理速度。

在駕駛和高頻運動場景的7個數據集上進行大規模實驗，所有實驗都有開源代碼可以復現。

結合姿態圖后端構建了完整SLAM的快速回環檢測方法，為pyLiDAR-SLAM(開源)。

2 算法概述

算法主要流程如下圖所示：

圖中的彩色部分為激光雷達掃描。點云的顏色對應每個點的時間戳(藍色表示時間戳更遠的點云，紅色表示時間戳更近的點云)。

通過在每一幀掃描的開始和結束時刻聯合優化兩個姿勢，并根據時間戳進行插值，使掃描進行彈性變形以與地圖(白點)對齊，從而創建連續時間掃描到地圖的里程計。圖片最下面下面說明軌跡具有掃描內姿勢的連續性和掃描之間的不連續性。

3 算法框架

3.1 里程計公式

在激光雷達的每一幀中，里程計有兩個位姿用于參數化插值：

起始位姿

結束位姿

對于在每一幀掃描的第一個時間戳和最后一個時間戳之間的時間∈[,]捕獲的每個傳感器的測量，通過在掃描的起始和結束兩個姿態之間插值來估計傳感器的姿態。

與其他里程計不同的是，當前幀的起始位姿不等于上一幀的結束位姿，這兩個位姿之間加入了一個鄰近約束，迫使兩個姿態保持接近，這使里程計對傳感器的高頻運動更加穩健。

本文的優化函數如下：

其中，優化變量,為掃描到地圖的連續時間ICP(其實就是插值和ICP的過程)。

是從每一幀雷達數據中提取的一個特征點序號的集合，對于每個i有：

是樣本點與其在地圖中最近的鄰居之間點到平面距離的殘差；

為世界坐標系中表示的點，為在局部地圖中鄰域的法線，為傳感器測量值(在LiDAR坐標系中)；

是雷達坐標系在時間到世界W的變換。通過定義，在和之間進行插值估計。對于旋轉插值，使用標準球面線性插值(slerp)。

還引入了有利于平面鄰域的權重:，是鄰域的平面度，其中是鄰域協方差特征值的平方根。

優化函數還引入了兩個約束(位置一致性約束)和(等速約束)，其權重分別為和，定義如下:

迫使傳感器的開始和結束位置保持一致(限制不連續性)，而限制過快的加速。

CT-ICP執行迭代，直到滿足參數步長范數的閾值(通常為平移0.1 cm和旋轉0.01°)或達到多次迭代(5次以確保實時性)。

3.2 局部地圖和魯棒的配置文件

作為局部地圖，世界坐標系(W)中的點存儲在體素的稀疏數據結構中，以便比kd-trees(常數時間訪問而不是對數訪問)更快地進行鄰域訪問。地圖的體素大小控制著鄰域搜索的半徑，以及存儲點云的詳細程度。

每個體素最多存儲20個點，這樣兩個點之間的距離就不會超過10厘米，以限制由于沿著掃描線的測量密度而造成的冗余。一旦一個體素被填滿，就不會再插入點了。

為了構建點的鄰域(用于計算ni和ai)，從當前點的27個相鄰體素中選擇地圖中k=20個最近鄰。在對當前掃描n運行CT-ICP后，這些點被添加到局部地圖中。

使用這些類型地圖的里程計對錯誤配準高度敏感，并且無法從錯誤掃描插入的地圖污染中恢復，為了處理這個問題，引入了一個魯棒的配置文件，可以檢測硬情況(快速方向變化)和注冊失敗(位置不一致或大量新關鍵點落在空體素中)，并嘗試使用一組更保守的參數(最明顯的是大量采樣關鍵點和更大的鄰域搜索)對當前掃描進行新的注冊;對于重要的方向修改(≥5?)，不會在地圖中插入新的掃描，因為這有更高的不對齊概率。

3.3 回環檢測和后端

回環檢測算法在其內存中保留了一個由里程計記錄的最后掃描的窗口。當窗口達到掃描的大小時，這些點被聚合成一個點云，放置在窗口中心的坐標框架中。

然后將該地圖的每個點插入到二維高程網格中，使每個像素點保持最大高程。從這個二維網格中，通過在和之間剪切每個像素的z坐標來獲得高程圖像。

然后提取旋轉不變的2D特征，并將其與高程網格一起保存在內存中。除了最后一次之外的所有掃描都將從窗口中刪除。

每次建立新的高程圖像時，它都會與保存在其內存中的高程圖像進行匹配。使用RANSAC魯棒擬合兩個特征集之間的二維剛性變換，并使用內層數的閾值來驗證對應關系。當匹配被驗證后，對初始2D變換在高程網格的點云上進行ICP細化，產生精確的6自由度閉環約束。

后端使用位姿圖，當添加新的里程約束時，會向圖中添加新的姿態，但只有當檢測到新的環路約束時，軌跡才會進行全局優化，此時閉環模塊的軌跡也會更新。

4 實驗與結果

在KITTI, KITTI-raw, KITTI-360, KITTI- carla, ParisLuco, Newer College Dataset (NCD)和NCLT數據集上進行了實驗。

4.1 里程計實驗

使用KITTI相對平移誤差(RTE)進行評估。并且在提供的數據集上與其他三種LiDAR里程計進行比較:MULLS、IMLSSLAM和pyLIDAR-SLAM F2M。實驗結果如下：

4.2 回環檢測實驗

為了定量評價其質量，RTE不太適合，并且在環路關閉后趨于惡化。為了證明全局軌跡改進，使用標準的絕對軌跡誤差(ATE)來進行評估。然而，為了將質量評估與初始姿態的方向誤差分離開，在計算ATE之前首先估計地面真值與估計軌跡之間的最佳剛性變換。這里也推薦「3D視覺工坊」新課程《三維點云處理：算法與實戰匯總》。

下圖給出了實驗定性結果：

下表給出了實驗定量結果：

最后給出了NCLT數據集(左上)、KITTI-CARLA(右上)、Newer College數據集(左下)和ParisLuco(右下)的聚合點云使用CT-ICP獲得的地圖的質量。

5 總結

本文提出了一種新的實時激光雷達里程計方法，該方法在七個不同數據集(從駕駛到高頻運動場景)上超越了目前的技術水平。該方法的核心是匹配CT-ICP的連續掃描，它在優化過程中彈性地扭曲新的掃描，以補償采集過程中的運動。

未來的工作將專注于改進后端，將所提出的連續公式擴展到掃描匹配之外，并充分利用循環關閉程序。