近日，智行者高翔博士帶領(lǐng)的定位團(tuán)隊(duì)撰寫(xiě)的論文《Faster-LIO: Lightweight Tightly Coupled Lidar-inertial Odometry using Parallel Sparse Incremental Voxels》被國(guó)際公認(rèn)的自動(dòng)駕駛領(lǐng)域TOP級(jí)期刊IEEE Robotics and Automation Letters收錄刊登。

該文章主要對(duì)激光雷達(dá)算法進(jìn)行了深入探討，本文提出了一種基于iVox(incremental voxels)的算法，以快速跟蹤旋轉(zhuǎn)的激光雷達(dá)-慣性里程計(jì)(LIO)方法固態(tài)激光雷達(dá)掃描。在該算法中，智行者定位團(tuán)隊(duì)使用iVox作為點(diǎn)云空間數(shù)據(jù)結(jié)構(gòu)，即從傳統(tǒng)的體素修改，支持增量插入和并行近似k-NN查詢(xún)。該算法可以有效的降低點(diǎn)云配準(zhǔn)時(shí)的耗時(shí)，也不會(huì)影響LIO的精度表現(xiàn)。

干貨內(nèi)容如下：

前言

眾所周知SLAM現(xiàn)在越來(lái)越卷了。卷來(lái)卷去，大概有幾種卷的方向：

一是卷精度。然而同樣傳感器做成的數(shù)據(jù)集大體來(lái)說(shuō)不會(huì)有數(shù)量級(jí)上的精度差異，大部分論文都會(huì)說(shuō)在某個(gè)數(shù)據(jù)集上得到了百分之幾的精度提升，然而原因比較玄學(xué)，不好歸因，實(shí)際當(dāng)中也不一定看的出來(lái)。

二是卷魯棒性。魯棒性倒是非常實(shí)在，別人跑丟了的數(shù)據(jù)我跑成功了，魯棒性自然就好。不過(guò)開(kāi)源的數(shù)據(jù)集相比真實(shí)數(shù)據(jù)來(lái)說(shuō)九牛一毛。開(kāi)源數(shù)據(jù)集有十來(lái)個(gè)數(shù)據(jù)就顯得不錯(cuò)了，實(shí)際當(dāng)中往往是幾百幾千的車(chē)輛和機(jī)器人在跑，數(shù)據(jù)集那幾個(gè)包才哪到哪，能體現(xiàn)的魯棒性指標(biāo)很有限。

三是卷效率。效率也是實(shí)打?qū)嵞芸吹降?。別人算100ms，我算50ms，那效率就實(shí)實(shí)在在地快了一倍。別人要工控機(jī)，我用嵌入式;別人占滿(mǎn)CPU，我跑一半的CPU，那整個(gè)系統(tǒng)就更流暢絲滑。而且卷效率沒(méi)那么玄學(xué)，哪哪算的快了都可以找到原因，和數(shù)據(jù)集關(guān)系也不大。這就是我們這次選擇卷效率的一個(gè)理由。

Lidar-inertial odometry (LIO)是SLAM這邊卷的還不那么厲害的方向之一。純Lidar的SLAM已經(jīng)比較穩(wěn)定了，幾個(gè)開(kāi)源方案(cartographer, Loam系列)跑得也挺順，雖然大體上要慢一些。不過(guò)到了LIO，你會(huì)發(fā)現(xiàn)一些神奇的現(xiàn)象：開(kāi)源的LIO大部分只能在自己的數(shù)據(jù)集上跑，換一個(gè)數(shù)據(jù)集就很容易飛或者掛。前期的方案考慮的東西太少，在后出的數(shù)據(jù)集上通常會(huì)出問(wèn)題。近期的方案則相對(duì)要穩(wěn)定一些，但依然沒(méi)有純Lidar方案那么穩(wěn)定，各種指標(biāo)也有一定的提升空間。

本文要談的Faster-LIO是基于FastLIO2開(kāi)發(fā)的。FastLIO2是開(kāi)源LIO中比較優(yōu)秀的一個(gè)，前端用了增量的kdtree(ikd-tree)，后端用了迭代ESKF(IEKF)，流程短，計(jì)算快。Faster-LIO則把ikd-tree替換成了iVox(后文介紹)，順帶優(yōu)化了一些代碼邏輯，實(shí)現(xiàn)了更快的LIO。我們?cè)诘湫偷?2線激光雷達(dá)中可以取得100-200Hz左右的計(jì)算頻率，在固態(tài)雷達(dá)中甚至可以達(dá)到1000-2000Hz，能夠達(dá)到FastLIO2的1.5-2倍左右的速度。當(dāng)然具體數(shù)值和計(jì)算平臺(tái)相關(guān)。讀者也可以用自己的平臺(tái)測(cè)試一下Faster-LIO在你機(jī)器上的表現(xiàn)。

介紹

我們就不聊LIO在什么自動(dòng)駕駛或者無(wú)人機(jī)上的應(yīng)用背景之類(lèi)的話(huà)題了，直接切入主題。

大體來(lái)講，LIO系統(tǒng)的整個(gè)計(jì)算流程是比較固定的：它們從IMU中得到一個(gè)粗略的估計(jì)，然后把雷達(dá)的數(shù)據(jù)與一些歷史數(shù)據(jù)做配準(zhǔn)，最后用某種狀態(tài)估計(jì)算法進(jìn)行濾波或者優(yōu)化。IMU部分的處理差別不大，所以LIO系統(tǒng)的計(jì)算效率主要與點(diǎn)云算法和后端算法相關(guān)，我們大致分三個(gè)方面：

點(diǎn)云最近鄰的數(shù)據(jù)結(jié)構(gòu)。點(diǎn)云配準(zhǔn)的基本問(wèn)題是計(jì)算給定點(diǎn)與歷史點(diǎn)云的最近鄰，通常需要依賴(lài)一些最近鄰的數(shù)據(jù)結(jié)構(gòu)。這些數(shù)據(jù)結(jié)構(gòu)又大體分為樹(shù)類(lèi)的(tree like)和體素類(lèi)(voxel like)的。廣義的，高維的最近鄰問(wèn)題是一個(gè)比較復(fù)雜的問(wèn)題，但LIO里的最近鄰則是低維的、增量式的問(wèn)題。于是，像R*樹(shù)、B* 樹(shù)等靜態(tài)的數(shù)據(jù)結(jié)構(gòu)并不是非常適合LIO。FastLIO2里提出使用增量式的kdtree來(lái)處理最近鄰，我們則認(rèn)為增量的體素更適合LIO系統(tǒng)。

點(diǎn)云殘差的計(jì)算方式。自動(dòng)駕駛里普遍偏向不直接使用點(diǎn)到點(diǎn)的殘差，而是使用點(diǎn)到線或點(diǎn)到面的殘差。點(diǎn)到點(diǎn)的殘差形式雖然簡(jiǎn)單，但雷達(dá)點(diǎn)云和RGBD點(diǎn)云相比，更加稀疏，在車(chē)輛運(yùn)動(dòng)過(guò)程中不見(jiàn)得都能打到同一個(gè)點(diǎn)，而且點(diǎn)云也往往會(huì)被降采樣后再進(jìn)行處理，所以并不太適合在自動(dòng)駕駛中使用。LOAM系列會(huì)計(jì)算點(diǎn)云特征，實(shí)際當(dāng)中特征提取要花的時(shí)間是不可忽略的，甚至是主要計(jì)算部分。

狀態(tài)估計(jì)算法的選型。LIO和VIO中普遍會(huì)使用介于單幀EKF和批量?jī)?yōu)化之間的方案，例如IEKF、MSCKF、Sliding Window Filter等等。這其中又以IEKF算是最簡(jiǎn)單有效的一類(lèi)，既有迭代來(lái)保證精度，又不需要像預(yù)積分系統(tǒng)那樣算一堆雅可比矩陣。

所以這樣一看，能進(jìn)一步挖掘的地方主要是LIO的近鄰結(jié)構(gòu)。Kd樹(shù)類(lèi)結(jié)構(gòu)的優(yōu)勢(shì)在于，可以嚴(yán)格地查詢(xún)K近鄰，不會(huì)多也不會(huì)少;也可以以范圍或盒子形式來(lái)查詢(xún)最近鄰(range search/box search)。查詢(xún)過(guò)程中也以設(shè)置附加條件比如最大距離，實(shí)現(xiàn)快速的近似最近鄰查找(Aproximate Nearest Neighbor, ANN)。然而傳統(tǒng)的Kd樹(shù)是不帶增量結(jié)構(gòu)的。像ikd-tree這種帶增量加點(diǎn)的結(jié)構(gòu)，雖然不用完全重新構(gòu)建，但也需要花額外的時(shí)間去維護(hù)這個(gè)樹(shù)的結(jié)構(gòu)。我們不禁要問(wèn)：LIO里真的需要嚴(yán)格的K近鄰搜索嗎?能不能放寬一點(diǎn)，使用更簡(jiǎn)單的結(jié)構(gòu)?

實(shí)際上點(diǎn)云配準(zhǔn)往往不需要嚴(yán)格的K近鄰。如果K近鄰找到了一個(gè)很遠(yuǎn)的點(diǎn)，拿這個(gè)點(diǎn)過(guò)來(lái)做點(diǎn)面殘差也是不合理的，這部分計(jì)算就是無(wú)效的。我們不妨讓近鄰結(jié)構(gòu)本身就具有這種查找范圍限制，而kd樹(shù)即使帶了范圍限制，也需要一個(gè)節(jié)點(diǎn)一個(gè)節(jié)點(diǎn)地來(lái)遍歷，這種遍歷顯然也是會(huì)耗時(shí)的。于是我們來(lái)考慮一種基于體素的近鄰結(jié)構(gòu)?？紤]到點(diǎn)云的稀疏性，我們希望體素也能夠以稀疏的形式存儲(chǔ)。體素具備一些天然的優(yōu)點(diǎn)：一是天然具有K近鄰時(shí)的范圍限制;二是增量構(gòu)建的時(shí)候不需要額外的操作，刪除的時(shí)候也很方便;三是最近鄰的范圍也可預(yù)先定義，想多搜點(diǎn)就多搜點(diǎn)，想快點(diǎn)就少搜點(diǎn)，豐儉由人;四是很容易并行化或者GPU化。

于是FasterLIO使用了一種基于稀疏體素的近鄰結(jié)構(gòu)iVox(incremental voxels)。我們會(huì)發(fā)現(xiàn)這種結(jié)構(gòu)用來(lái)做LIO更加合適，可以有效的降低點(diǎn)云配準(zhǔn)時(shí)的耗時(shí)，也不會(huì)影響LIO的精度表現(xiàn)。我們使用兩個(gè)版本的iVox：一種是線性的，一種是基于空間填充曲線的(偽希爾伯特空間曲線，pseudo-Hilbert curve, PHC)，下面來(lái)說(shuō)明其原理。

iVox

iVox由空間中稀疏分布的體素組成。每個(gè)體素內(nèi)部可以存在多個(gè)點(diǎn)，體素自身的網(wǎng)格坐標(biāo)由空間哈希函數(shù)映射到哈希鍵值上，再組成哈希表。

哈希函數(shù)可以取一些典型形式。由于實(shí)際存儲(chǔ)的是三維點(diǎn)，我們使用簡(jiǎn)單的空間哈希即可：

其中p為三維點(diǎn)，v為體素網(wǎng)絡(luò)，id是它的哈希鍵值，xor表示異或。

iVox內(nèi)部點(diǎn)的存儲(chǔ)方式稱(chēng)為它的底層結(jié)構(gòu)。最簡(jiǎn)單的底層結(jié)構(gòu)是線性的，我們稱(chēng)為線性的iVox;如果要存的點(diǎn)很多，我們利用空間填充曲線來(lái)存，稱(chēng)為iVox-PHC。當(dāng)然在LIO算法流程上，我們會(huì)避免在同一個(gè)體素中大量插入點(diǎn)而影響計(jì)算效率，所以?xún)煞N算法實(shí)際用起來(lái)差異不大。

K近鄰的查找

iVox里的K近鄰相對(duì)簡(jiǎn)單。我們首先定義一個(gè)體素的近鄰范圍，典型的有0，6，18，26這幾種。實(shí)際當(dāng)中主要用18和26.

在查找K近鄰時(shí)，先計(jì)算被查找的點(diǎn)落在哪個(gè)體素中，然后看預(yù)定義的范圍內(nèi)是否存在有效的iVox。如果有，就把這些iVox內(nèi)部的點(diǎn)也考慮進(jìn)來(lái)。我們?cè)诿總€(gè)iVox內(nèi)部查找至多K個(gè)最近鄰，再進(jìn)行合并即可。線性的iVox只須遍歷內(nèi)部點(diǎn)，然后進(jìn)行部分排序，再進(jìn)行匯總。PHC的iVox則可以根據(jù)空間填充曲線上的索引值來(lái)查找最近鄰。

PHC是一種建立高維數(shù)據(jù)與低維數(shù)據(jù)映射的方法。離散的PHC可以看成把空間分成許多小格子，然后給每個(gè)格子找一個(gè)id的過(guò)程。于是，在查找某個(gè)點(diǎn)的最近鄰時(shí)，可以先看它在這個(gè)iVox里的曲線上ID，然后在這個(gè)ID周邊找到若干個(gè)近鄰，再返回其結(jié)果即可。在點(diǎn)數(shù)較多時(shí)，PHC的查找會(huì)比線性查找的復(fù)雜度更低一些。

增量地圖更新

iVox的增量地圖更新比kd樹(shù)簡(jiǎn)單很多。簡(jiǎn)而言之，算出增加點(diǎn)對(duì)應(yīng)的體素網(wǎng)格，直接往里添加即可。如果是PHC的，則還需要算一下PHC的曲線位置。除此之外沒(méi)有其他操作了。

在FastLIO2中，系統(tǒng)會(huì)刪除一部分歷史點(diǎn)云，讓局部地圖跟隨車(chē)輛前進(jìn)。在iVox里，由于遍歷整個(gè)iVox局部地圖是比較慢的，我們讓這個(gè)過(guò)程變?yōu)楸粍?dòng)刪除，而不是主動(dòng)地在每幀計(jì)算后進(jìn)行刪除。于是我們給局部地圖添加一個(gè)LRU緩存(least recently used)，在進(jìn)行近鄰搜索時(shí)，記錄哪些體素是最近使用過(guò)的，那么不怎么使用的體素自然被移動(dòng)到隊(duì)尾。我們會(huì)設(shè)置一個(gè)局部地圖的容量，超過(guò)最大容量時(shí)，就刪除那些很久未使用的體素。刪除操作是針對(duì)整個(gè)體素的，內(nèi)部的點(diǎn)會(huì)被全部刪除。這種局部地圖緩存策略也會(huì)讓局部地圖跟隨車(chē)輛運(yùn)動(dòng)，而實(shí)際操作的地方更少。

iVox的具體參數(shù)和復(fù)雜度等細(xì)節(jié)見(jiàn)論文和代碼，這里不再描述。

實(shí)驗(yàn)

實(shí)驗(yàn)部分主要包含仿真實(shí)驗(yàn)和數(shù)據(jù)集實(shí)驗(yàn)。

仿真實(shí)驗(yàn)

仿真實(shí)驗(yàn)是在一個(gè)隨機(jī)生成的點(diǎn)云里進(jìn)行K近鄰查找以及新增地圖點(diǎn)的實(shí)驗(yàn)。我們對(duì)比了Kdtree flann, ikd-tree, nanoflann R-tree, faiss-IVF, nmslib幾個(gè)庫(kù)。耗時(shí)與地圖點(diǎn)數(shù)的關(guān)系圖如下：

可以看到iVox在K近鄰查找和新增的耗時(shí)都是很少的，但它們隨地圖點(diǎn)數(shù)的增長(zhǎng)會(huì)更快，畢竟單個(gè)iVox里的點(diǎn)會(huì)變多。順便說(shuō)一句，PCL Kdtree的查詢(xún)速度(這里寫(xiě)的flann)也是杠杠的，只是沒(méi)有增量接口。

我們也比較了iVox的K近鄰質(zhì)量問(wèn)題。大部分時(shí)候iVox的K近鄰不是嚴(yán)格的，因?yàn)樘烊坏挠蟹秶拗?。我們把K近鄰的結(jié)果與暴力搜索的結(jié)果進(jìn)行比對(duì)，可以得到它的召回率(recall)。各算法的召回率與時(shí)間曲線如下：

如果要求iVox有很高的召回，那就不得不設(shè)置很大的體素尺寸或者很大的搜索近鄰，這時(shí)候iVox的查找時(shí)間會(huì)增長(zhǎng)很快。不過(guò)LIO系統(tǒng)在70%左右的召回率下就能很好地工作，所以實(shí)際也沒(méi)啥大不了的。

數(shù)據(jù)集實(shí)驗(yàn)

數(shù)據(jù)集實(shí)驗(yàn)主要比較整個(gè)LIO系統(tǒng)的耗時(shí)和計(jì)算精度。由于Faster-LIO框架與FastLIO2基本相同，我們時(shí)間上對(duì)標(biāo)的也主要是FastLIO2，其他系統(tǒng)主要是用來(lái)做個(gè)參考。32線雷達(dá)的詳細(xì)步驟算法耗時(shí)如下圖所示：

可見(jiàn)主要的耗時(shí)在IEKF+ICP的迭代過(guò)程。使用iVox替代iKd-tree時(shí)，我們縮短的也主要是這個(gè)過(guò)程。UTBM數(shù)據(jù)集要明顯一些，我們可以把差不多20ms的IEKF迭代降到5-8ms左右，整個(gè)流程可以從30ms左右降到10ms左右，實(shí)現(xiàn)明顯的效率提升。

我們也繪制了時(shí)間軸上的耗時(shí)曲線，如下：

兩個(gè)版本的LIO的運(yùn)算效率不會(huì)隨著時(shí)間有明顯變化，而FasterLIO要明顯用時(shí)更低一些。對(duì)LIO-SAM、LiLi-OM的一些計(jì)算用時(shí)指標(biāo)可以參考論文的表格。

在精度方面，考慮到LIO默認(rèn)不帶回環(huán)檢測(cè)，所以我們主要評(píng)價(jià)每百米的漂移誤差指標(biāo)(百分比形式)，見(jiàn)下表。

實(shí)際上，只要不漂，LIO精度都是差不多的，天下哪有改改算法就提升精度的事情(逃)。

iVox也可以被集成到其他LO或LIO里，但是大部分方案里，最近鄰并不是主要的計(jì)算瓶頸，gtsam/ceres什么的耗時(shí)相比最近鄰那可太多了。我們也嘗試了把iVox集成到Lego-LOAM里，發(fā)現(xiàn)主要只是省了增量地圖構(gòu)建那部分時(shí)間，優(yōu)化方面沒(méi)什么變化(點(diǎn)少)。所以iVox與FastLIO倒是相性更好一些。

結(jié)論與聲明

本文提出了一種更快速的LIO方法，使用iVox作為最近鄰方案，在同等精度指標(biāo)下可以明顯提升LIO計(jì)算速度。

我們也歡迎讀者自己測(cè)試Faster-LIO的計(jì)算性能。祝大家科研愉快。

同時(shí)，智行者還提供了開(kāi)源代碼以更方便社區(qū)使用。

原文標(biāo)題：頂刊收錄!智行者提出全新基于ivox激光雷達(dá)算法

文章出處：【微信公眾號(hào)：智行者科技】歡迎添加關(guān)注!文章轉(zhuǎn)載請(qǐng)注明出處。

審核編輯：湯梓紅