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

1 引言

本文提出的目的：

使用勻速運(yùn)動假設(shè)矯正點(diǎn)云畸變的方法無法適應(yīng)大幅度的方向變化或者劇烈速度變化

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

本文主要貢獻(xiàn)：

提出了一種基于位姿掃描內(nèi)連續(xù)性和掃描間不連續(xù)的彈性激光雷達(dá)里程計(jì)。

本文次要貢獻(xiàn)：

提出了一種基于密集點(diǎn)云的局部地圖，存儲在稀疏體素結(jié)構(gòu)中，以獲得實(shí)時處理速度。

在駕駛和高頻運(yùn)動場景的7個數(shù)據(jù)集上進(jìn)行大規(guī)模實(shí)驗(yàn)，所有實(shí)驗(yàn)都有開源代碼可以復(fù)現(xiàn)。

結(jié)合姿態(tài)圖后端構(gòu)建了完整SLAM的快速回環(huán)檢測方法，為pyLiDAR-SLAM(開源)。

2 算法概述

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

圖中的彩色部分為激光雷達(dá)掃描。點(diǎn)云的顏色對應(yīng)每個點(diǎn)的時間戳(藍(lán)色表示時間戳更遠(yuǎn)的點(diǎn)云，紅色表示時間戳更近的點(diǎn)云)。

通過在每一幀掃描的開始和結(jié)束時刻聯(lián)合優(yōu)化兩個姿勢，并根據(jù)時間戳進(jìn)行插值，使掃描進(jìn)行彈性變形以與地圖(白點(diǎn))對齊，從而創(chuàng)建連續(xù)時間掃描到地圖的里程計(jì)。圖片最下面下面說明軌跡具有掃描內(nèi)姿勢的連續(xù)性和掃描之間的不連續(xù)性。

3 算法框架

3.1 里程計(jì)公式

在激光雷達(dá)的每一幀中，里程計(jì)有兩個位姿用于參數(shù)化插值：

起始位姿

結(jié)束位姿

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

與其他里程計(jì)不同的是，當(dāng)前幀的起始位姿不等于上一幀的結(jié)束位姿，這兩個位姿之間加入了一個鄰近約束，迫使兩個姿態(tài)保持接近，這使里程計(jì)對傳感器的高頻運(yùn)動更加穩(wěn)健。

本文的優(yōu)化函數(shù)如下：

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

是從每一幀雷達(dá)數(shù)據(jù)中提取的一個特征點(diǎn)序號的集合，對于每個i有：

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

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

是雷達(dá)坐標(biāo)系在時間到世界W的變換。通過定義，在和之間進(jìn)行插值估計(jì)。對于旋轉(zhuǎn)插值，使用標(biāo)準(zhǔn)球面線性插值(slerp)。

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

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

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

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

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

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

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

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

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

3.3 回環(huán)檢測和后端

回環(huán)檢測算法在其內(nèi)存中保留了一個由里程計(jì)記錄的最后掃描的窗口。當(dāng)窗口達(dá)到掃描的大小時，這些點(diǎn)被聚合成一個點(diǎn)云，放置在窗口中心的坐標(biāo)框架中。

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

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

每次建立新的高程圖像時，它都會與保存在其內(nèi)存中的高程圖像進(jìn)行匹配。使用RANSAC魯棒擬合兩個特征集之間的二維剛性變換，并使用內(nèi)層數(shù)的閾值來驗(yàn)證對應(yīng)關(guān)系。當(dāng)匹配被驗(yàn)證后，對初始2D變換在高程網(wǎng)格的點(diǎn)云上進(jìn)行ICP細(xì)化，產(chǎn)生精確的6自由度閉環(huán)約束。

后端使用位姿圖，當(dāng)添加新的里程約束時，會向圖中添加新的姿態(tài)，但只有當(dāng)檢測到新的環(huán)路約束時，軌跡才會進(jìn)行全局優(yōu)化，此時閉環(huán)模塊的軌跡也會更新。

4 實(shí)驗(yàn)與結(jié)果

在KITTI, KITTI-raw, KITTI-360, KITTI- carla, ParisLuco, Newer College Dataset (NCD)和NCLT數(shù)據(jù)集上進(jìn)行了實(shí)驗(yàn)。

4.1 里程計(jì)實(shí)驗(yàn)

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

4.2 回環(huán)檢測實(shí)驗(yàn)

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

下圖給出了實(shí)驗(yàn)定性結(jié)果：

下表給出了實(shí)驗(yàn)定量結(jié)果：

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

5 總結(jié)

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

未來的工作將專注于改進(jìn)后端，將所提出的連續(xù)公式擴(kuò)展到掃描匹配之外，并充分利用循環(huán)關(guān)閉程序。