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

本文提出的目的：

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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