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

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

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

前言

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

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

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

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

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

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

介紹

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

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

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

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

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

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

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

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

iVox

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

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

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

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

K近鄰的查找

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

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

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

增量地圖更新

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

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

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

實驗

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

仿真實驗

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

可以看到iVox在K近鄰查找和新增的耗時都是很少的，但它們隨地圖點數(shù)的增長會更快，畢竟單個iVox里的點會變多。順便說一句，PCL Kdtree的查詢速度(這里寫的flann)也是杠杠的，只是沒有增量接口。

我們也比較了iVox的K近鄰質(zhì)量問題。大部分時候iVox的K近鄰不是嚴格的，因為天然的有范圍限制。我們把K近鄰的結(jié)果與暴力搜索的結(jié)果進行比對，可以得到它的召回率(recall)。各算法的召回率與時間曲線如下：

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

數(shù)據(jù)集實驗

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

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

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

兩個版本的LIO的運算效率不會隨著時間有明顯變化，而FasterLIO要明顯用時更低一些。對LIO-SAM、LiLi-OM的一些計算用時指標可以參考論文的表格。

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

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

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

結(jié)論與聲明

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

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

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

原文標題：頂刊收錄!智行者提出全新基于ivox激光雷達算法

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

審核編輯：湯梓紅