CT-ICP：實時彈性激光雷達(dá)里程計與回環(huán)檢測

本文提出的目的：

1. 使用勻速運動假設(shè)矯正點云畸變的方法無法適應(yīng)大幅度的方向變化或者劇烈速度變化
2. 即使目前有工作考慮了連續(xù)時間內(nèi)的運動，但是也無法適應(yīng)高頻的劇烈運動情況，或者以精度損失為代價。

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

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

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

• 提出了一種基于密集點云的局部地圖，存儲在稀疏體素結(jié)構(gòu)中，以獲得實時處理速度。
• 在駕駛和高頻運動場景的7個數(shù)據(jù)集上進(jìn)行大規(guī)模實驗，所有實驗都有開源代碼可以復(fù)現(xiàn)。
• 結(jié)合姿態(tài)圖后端構(gòu)建了完整SLAM的快速回環(huán)檢測方法，為pyLiDAR-SLAM(開源)。

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

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

通過在每一幀掃描的開始和結(jié)束時刻聯(lián)合優(yōu)化兩個姿勢，并根據(jù)時間戳進(jìn)行插值，使掃描進(jìn)行彈性變形以與地圖(白點)對齊，從而創(chuàng)建連續(xù)時間掃描到地圖的里程計。圖片最下面下面說明軌跡具有掃描內(nèi)姿勢的連續(xù)性和掃描之間的不連續(xù)性。這里也推薦「3D視覺工坊」新課程《三維點云處理：算法與實戰(zhàn)匯總》。

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

• 起始位姿
• 結(jié)束位姿

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

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

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

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

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

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

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

• 是雷達(dá)坐標(biāo)系在時間到世界W的變換。通過定義，在和之間進(jìn)行插值估計。對于旋轉(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次以確保實時性)。

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

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

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

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

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

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

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

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

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

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

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

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

下圖給出了實驗定性結(jié)果：

下表給出了實驗定量結(jié)果：

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

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

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