標(biāo)題：Point-LIO: Robust High-Bandwidth LiDAR-Inertial Odometry

作者：He et al., HKU MARS Lab

Paper:?https://github.com/hku-mars/Point-LIO/files/10989136/Point-LIO_preprint.pdf

Code:?https://github.com/hku-mars/Point-LIO

來(lái)源：微信公眾號(hào)「3D視覺(jué)工坊」?

1 動(dòng)機(jī)

現(xiàn)有系統(tǒng)都是基于幀的，類(lèi)似于VSLAM系統(tǒng)，頻率固定（例如10Hz), 但是實(shí)際上LiDAR是在不同時(shí)刻進(jìn)行順序采樣，然后積累到一幀上，這不可避免地會(huì)引入運(yùn)動(dòng)畸變，從而影響建圖和里程計(jì)精度。此外，這種低幀率會(huì)增加延時(shí)，限制系統(tǒng)帶寬（里程計(jì)帶寬的定義類(lèi)似動(dòng)態(tài)系統(tǒng)的帶寬，即系統(tǒng)增益降至0.707以下的頻率，表示里程計(jì)在能夠滿意地估計(jì)時(shí)可以運(yùn)動(dòng)多快）。

2 主要貢獻(xiàn)

1) 提出了一種逐點(diǎn)(point-wise) LIO框架，該框架在實(shí)際采樣時(shí)間融合激光雷達(dá)點(diǎn)，而不會(huì)累積到幀中。去除點(diǎn)累積消除了幀內(nèi)運(yùn)動(dòng)失真，并允許以接近點(diǎn)采樣率的高里程計(jì)輸出和建圖更新，這進(jìn)一步使系統(tǒng)能夠跟蹤非?？斓倪\(yùn)動(dòng)。

2) 為了進(jìn)一步提高系統(tǒng)帶寬到超出IMU測(cè)量范圍，用隨機(jī)過(guò)程模型對(duì)IMU測(cè)量進(jìn)行建模。將該模型擴(kuò)展到系統(tǒng)運(yùn)動(dòng)學(xué)中，并將IMU測(cè)量值視為系統(tǒng)輸出。即使IMU飽和，隨機(jī)過(guò)程增強(qiáng)的運(yùn)動(dòng)學(xué)模型也可以平滑估計(jì)系統(tǒng)狀態(tài)，包括角速度和線加速度。

3) 將這兩個(gè)關(guān)鍵技術(shù)集成到一個(gè)完全緊耦合的LIO系統(tǒng)中，稱為Point-LIO。系統(tǒng)使用流形擴(kuò)展卡爾曼濾波器通過(guò)在其各自的采樣時(shí)間融合每個(gè)LiDAR點(diǎn)或IMU數(shù)據(jù)來(lái)更新系統(tǒng)狀態(tài)。通過(guò)利用系統(tǒng)的稀疏性和線性，開(kāi)發(fā)的系統(tǒng)即使在微型飛行器上基于低功耗ARM的計(jì)算機(jī)上也能實(shí)現(xiàn)實(shí)時(shí)狀態(tài)估計(jì)。

4) 開(kāi)發(fā)的系統(tǒng)在由具有非常小FoV的新興固態(tài)LiDAR收集的各種挑戰(zhàn)性的現(xiàn)實(shí)世界數(shù)據(jù)中進(jìn)行了測(cè)試。結(jié)果表明，Point-LIO具有運(yùn)動(dòng)畸變補(bǔ)償能力，具有高里程計(jì)輸出速率 (4-8 kHz) 和高帶寬 (>150Hz) 的能力。該系統(tǒng)還能夠在初始階段后通過(guò)飽和IMU測(cè)量來(lái)估計(jì)極端激進(jìn)運(yùn)動(dòng) (角速度大于75 rad/s) 下的狀態(tài)。此外，對(duì)來(lái)自各種公開(kāi)LiDAR數(shù)據(jù)集的12個(gè)序列的詳盡基準(zhǔn)比較表明，Point-LIO實(shí)現(xiàn)了與其他方法一致可比的準(zhǔn)確性和效率，同時(shí)花費(fèi)更少的計(jì)算資源。最后演示了實(shí)際無(wú)人機(jī)上的實(shí)際應(yīng)用。

3 方法概覽

其設(shè)計(jì)理念主要基于:

1) LiDAR點(diǎn)是在相應(yīng)的時(shí)間順序采樣的，而不是同時(shí)采樣的幀;

2) IMU數(shù)據(jù)是系統(tǒng)的測(cè)量，而不是輸入。

接收到每個(gè)測(cè)量值（即LiDAR點(diǎn)或IMU數(shù)據(jù)），就將其在流形擴(kuò)展卡爾曼濾波器框架中融合。順序采樣的LiDAR點(diǎn)和IMU數(shù)據(jù)都用于在各自的時(shí)間戳更新?tīng)顟B(tài)，從而得到極高頻率(4-8 kHz)的里程計(jì)輸出：

• 對(duì)于接收到的每個(gè)LiDAR點(diǎn)，搜索地圖的對(duì)應(yīng)平面 (為了在允許新注冊(cè)點(diǎn)的同時(shí)實(shí)現(xiàn)快速的平面對(duì)應(yīng)搜索，使用了在FAST-LIO2 中開(kāi)發(fā)的增量k-d樹(shù) (ikd-Tree))。

• 如果點(diǎn)與從地圖中的點(diǎn)擬合的平面匹配，則計(jì)算殘差以使用流形卡爾曼濾波器更新系統(tǒng)狀態(tài)。優(yōu)化的位姿最終將LiDAR點(diǎn)注冊(cè)到全局幀中并合并到地圖，然后進(jìn)行下一個(gè)測(cè)量 (LiDAR點(diǎn)或IMU數(shù)據(jù))。

• 否則，如果點(diǎn)沒(méi)有匹配的平面，則通過(guò)卡爾曼濾波器預(yù)測(cè)的位姿將其直接添加到地圖中。

• 對(duì)于每個(gè)IMU測(cè)量，分別對(duì)IMU的每個(gè)通道進(jìn)行飽和度檢查，具有飽和值的通道將不用于狀態(tài)更新。

4 實(shí)驗(yàn)

主要評(píng)估三方面：

• 去除運(yùn)動(dòng)畸變

• 高頻率、高帶寬的里程計(jì)

• 飽和IMU情況下的狀態(tài)估計(jì)

• 4.1 實(shí)現(xiàn)細(xì)節(jié)

使用C++和ROS實(shí)現(xiàn)：

• EKF是基于作者在先前工作中開(kāi)發(fā)的IKFoM工具箱(https://github.com/hku-mars/IKFoM)實(shí)現(xiàn)的。

• 使用FAST-LIO2中開(kāi)發(fā)的增量k-d樹(shù) (ikd-Tree) 作為地圖結(jié)構(gòu)，其默認(rèn)參數(shù)為:

• 局部地圖大小 = 2000 m，

• 空間下采樣分辨率 = 0.25 m，

• ikd-Tree的重平衡閾值為 = 0.6， = 0.5，

• 用于并行重建(在第二線程中)的子樹(shù)大小閾值為 = 1500。

• 盡管系統(tǒng)被設(shè)計(jì)為在每次LiDAR點(diǎn)接收后執(zhí)行狀態(tài)估計(jì)，但實(shí)際上，受LiDAR制造商提供的可用驅(qū)動(dòng)程序的限制，LiDAR點(diǎn)在累積完整掃描后被打包，然后發(fā)送到LIO系統(tǒng)。為了滿足這一實(shí)際限制，Point-LIO根據(jù)它們各自的時(shí)間戳對(duì)接收到的包中包含的所有LiDAR點(diǎn)和IMU數(shù)據(jù)進(jìn)行排序。然后，將排序后的數(shù)據(jù)逐個(gè)逐點(diǎn)處理-LIO。

在所有評(píng)估中，將Point-LIO與最先進(jìn)的基于幀的里程計(jì)FAST-LIO2進(jìn)行了比較：

• FAST-LIO2的所有結(jié)果都是使用開(kāi)源的FAST-LIO2及其默認(rèn)參數(shù)值 (基本上也是上面的參數(shù)值) 得到的

• 由于FAST-LIO2為每次接收到的LiDAR幀執(zhí)行分辨率為0.3的空間降采樣，為了公平比較，也在Point-LIO中執(zhí)行這樣的空間降采樣，然后再進(jìn)行數(shù)據(jù)排序和逐點(diǎn)更新

消融實(shí)驗(yàn)：

• 逐點(diǎn)更新：僅用逐點(diǎn)更新，而沒(méi)有應(yīng)用有色隨機(jī)模型。系統(tǒng)與FAST-LIO2相同，將IMU測(cè)量值建模為系統(tǒng)運(yùn)動(dòng)學(xué)模型的輸入，但不同之處在于掃描中的每個(gè)單獨(dú)的LiDAR點(diǎn)都用于像本文提出的系統(tǒng)那樣順序更新系統(tǒng)，該系統(tǒng)命名為Point-LIO-input。

4.2 實(shí)驗(yàn)平臺(tái)

• 固態(tài)3D激光雷達(dá)Livox Avia

• 1）70.4° (水平) × 77.2° (垂直) 圓形FoV和非常規(guī)的非重復(fù)掃描模式，可產(chǎn)生230,000Hz的點(diǎn)測(cè)量結(jié)果

• 2）內(nèi)置的IMU (BMI088型) 可產(chǎn)生200Hz的IMU數(shù)據(jù)。

• 3）點(diǎn)和IMU數(shù)據(jù)以從10 hz到100Hz可調(diào)的頻率收集。

• 第一人稱視角 (FPV) 相機(jī)

• 五個(gè)用于真值測(cè)量的Vicon標(biāo)記。

• 為了產(chǎn)生不同類(lèi)型的運(yùn)動(dòng)，構(gòu)建了三個(gè)不同的平臺(tái)來(lái)承載傳感器套件，包括

• 1）機(jī)器人車(chē) (圖3 (b))，深圳大疆開(kāi)發(fā)的RoboMaster 2019 AI;

• 2）由步進(jìn)電機(jī)驅(qū)動(dòng)的旋轉(zhuǎn)平臺(tái) (圖3 (c))，Nimotion STM4260A；

• 3）一個(gè)鐘擺（圖3(d)）

• 4.3 運(yùn)動(dòng)畸變處理

使用圖3 (b) 所示的機(jī)器人車(chē)收集了一些序列，測(cè)試了三種不同的場(chǎng)景，即

• 公園：Belcher Bay Park (非結(jié)構(gòu)化場(chǎng)景)

• 廣場(chǎng)：HKU的百年小廣場(chǎng) (半結(jié)構(gòu)化場(chǎng)景)

• 走廊：HKU的Haking Wong大樓的走廊 (結(jié)構(gòu)化場(chǎng)景)

在所有序列中，機(jī)器人車(chē)返回起點(diǎn)，能夠計(jì)算漂移。

LiDAR數(shù)據(jù)的頻率為10Hz。

一個(gè)挑戰(zhàn)是機(jī)器人車(chē)在地面上移動(dòng)時(shí)的強(qiáng)烈振動(dòng)。由于傳感器套件安裝在底盤(pán)上，沒(méi)有任何減震器，振動(dòng)將直接傳遞給傳感器，導(dǎo)致劇烈的抖動(dòng)運(yùn)動(dòng)，如圖4所示的內(nèi)置IMU數(shù)據(jù)所示。

當(dāng)機(jī)器人汽車(chē)在開(kāi)始時(shí)靜止時(shí)，IMU測(cè)量值穩(wěn)定地很小。隨著汽車(chē)開(kāi)始移動(dòng)，IMU的測(cè)量值迅速變化。

4.3.1 建圖結(jié)果

每個(gè)圖里，

• 首先展示最終建圖結(jié)果的全局視圖 (即子圖 (a))，然后關(guān)注包含大平面 (例如墻) 的某些局部區(qū)域 (子圖 (b))。

• 進(jìn)一步通過(guò)墻上點(diǎn)的一致性 (子圖 (c))，比較建圖精度

• 1）FAST-LIO2 (即 (b1)，(c1))

• 2）Point-LIO-input (即 (b2)，(c2))

• 3）Point-LIO (即 (b3)，(c3))。

• 最后，為了顯示幀內(nèi)運(yùn)動(dòng)失真，展示了上面選擇的局部區(qū)域中的一次掃描 (在一個(gè)掃描周期0.1 s內(nèi)的累積) 的點(diǎn) (子圖 (d)) 以及進(jìn)一步放大(子圖 (e)), 其中紅點(diǎn)指的是當(dāng)前掃描中已注冊(cè)的LiDAR點(diǎn)，白點(diǎn)是累積到當(dāng)前掃描的地圖結(jié)果

• 1）FAST-LIO2 (即(d1) 、 (e1))

• 2）Point-LIO-input (即 (d2)，(e2))

• 3）Point-LIO (即 (d3)，(e3))

從圖5，圖6和圖7的子圖 (c) 可以看出，F(xiàn)AST-LIO2 (c1) 的整體圖明顯比Point-LIO-input (c2) 厚，Point-LIO (c3) 產(chǎn)生的墻比Point-LIO-input更薄。這種現(xiàn)象的原因在于如圖5、圖6和圖7的子圖 (e) 所示的每個(gè)單獨(dú)掃描中的幀內(nèi)運(yùn)動(dòng)補(bǔ)償?？梢钥闯?，所選壁周?chē)乃屑t點(diǎn)都應(yīng)該屬于同一平面，但是由于幀內(nèi)運(yùn)動(dòng)失真，它們實(shí)際上會(huì)從壁上散射FAST-LIO2 (e1)。Point-LIO-input和Point-LIO的這種幀內(nèi)失真現(xiàn)象得到了很大緩解 ((e2) 和 (e3))。

FAST-LIO2使用基于IMU測(cè)量的向后傳播來(lái)將掃描的所有點(diǎn)投影到掃描結(jié)束的位姿。此過(guò)程很容易受到IMU測(cè)量噪聲，偏差估計(jì)誤差和有限的IMU采樣率的干擾。具體來(lái)說(shuō)，由傳感器振動(dòng)引起的加速度和角速度即使在IMU的一個(gè)采樣間隔內(nèi)也以高速率變化，導(dǎo)致大的IMU傳播誤差，因?yàn)榧僭O(shè)角速度和加速度在一個(gè)采樣間隔期間是恒定的。此外，低幀率 (即10Hz) 還需要長(zhǎng)時(shí)間 (即100 ms) 的IMU傳播，這累積了位姿誤差并導(dǎo)致大的幀內(nèi)失真，如子圖 (b1) 和 (c1) 所示。

相反，Point-LIO在其真實(shí)采樣時(shí)間處融合LiDAR點(diǎn)，而沒(méi)有任何點(diǎn)累積，這從根本上消除了運(yùn)動(dòng)失真，如Point-LIO-input的子圖 (c2) 和Point-LIO的子圖 (c3) 所示。

此外，當(dāng)將兩個(gè)逐點(diǎn)更新方案Point-LIO-input (c2) 與Point-LIO (c3) 進(jìn)行比較時(shí)，Point-LIO的性能稍好。這是因?yàn)镻oint-LIO-input仍然使用IMU測(cè)量來(lái)傳播狀態(tài) (盡管僅用于一個(gè)LiDAR點(diǎn)間隔)，因此仍然受IMU測(cè)量噪聲和偏差估計(jì)誤差影響。相反，Point-LIO使用IMU測(cè)量的濾波但不使用原始數(shù)據(jù)來(lái)傳播狀態(tài)，這稍微減少了 (e3) 中觀察到的運(yùn)動(dòng)失真。

4.3.2 漂移結(jié)果

由于機(jī)器人車(chē)的不完善操作，開(kāi)始和結(jié)束位置之間的距離不完全為零，但小于10cm。

可以看出：

• 對(duì)于Square和Corridor序列，F(xiàn)AST-LIO2，Point-LIO-input和Point-LIO具有可比的漂移。

• 對(duì)于Park序列，FAST-LIO2無(wú)法返回起點(diǎn)，這是因?yàn)楣珗@是一個(gè)非結(jié)構(gòu)化的環(huán)境，這使得運(yùn)動(dòng)失真對(duì)測(cè)距精度的影響更加明顯。

4.4 高頻率、高帶寬里程計(jì)輸出

通過(guò)向步進(jìn)電機(jī)提供快速變化的速度命令，在使用旋轉(zhuǎn)平臺(tái) (圖3 (c)) 收集的室內(nèi)數(shù)據(jù)集 (表示為 “Odo”) 上測(cè)試FAST-LIO2、Point-LIO-input和Point-LIO的里程計(jì)輸出頻率。在本實(shí)驗(yàn)中，LiDAR包的速率為100Hz。

即使以100Hz的LiDAR速率收集數(shù)據(jù)集，通過(guò)將一個(gè)幀分成多個(gè)幀 (但低于200Hz的IMU速率)，F(xiàn)AST-LIO2框架也自然可擴(kuò)展到更高的狀態(tài)更新頻率。因此，將一個(gè)LiDAR幀分為兩個(gè)運(yùn)行FAST-LIO2。

圖8顯示了里程計(jì)輸出頻率的分布。FAST-LIO2的輸出頻率是200Hz，和幀率一樣。Point-LIO-input和Point-LIO的頻率在4kHz和8kHz之間。

為了能夠進(jìn)行帶寬分析，以上實(shí)驗(yàn)以最高頻率300Hz記錄來(lái)自Vicon跟蹤器的真值測(cè)量重新進(jìn)行。將系統(tǒng)輸出 (根據(jù)里程計(jì)估計(jì)的偏航角) 除以系統(tǒng)輸入 (由Vicon系統(tǒng)測(cè)量的真值偏航角)，我們得到在不同的輸入頻率下FAST-LIO2、Point-LIO-input和Point-LIO的幅度響應(yīng) (dB)，如圖9所示。

可以看出，當(dāng)輸入頻率接近100Hz時(shí)，F(xiàn)AST-LIO2的幅度響應(yīng)開(kāi)始下降，這表明帶寬為100Hz (見(jiàn)表2)。根據(jù)Nyquist-Shannon采樣定理，當(dāng)輸出頻率為200Hz時(shí)，100Hz也是可達(dá)到的最高帶寬。相反，Point-LIO-input和Point-LIO都具有大于150Hz的帶寬，這超出了Vicon系統(tǒng)的測(cè)量能力。

4.5 飽和IMU測(cè)量下的極激進(jìn)運(yùn)動(dòng)

盡管到目前為止，Point-LIO-input和Point-LIO具有可比的性能，但本節(jié)展示了Point-LIO即使超出IMU測(cè)量范圍，也能夠跟蹤極其激進(jìn)的運(yùn)動(dòng)。

在實(shí)驗(yàn)中產(chǎn)生兩種類(lèi)型的運(yùn)動(dòng)：

• 旋轉(zhuǎn)運(yùn)動(dòng) (表示為 “Satu-1”)

• 在空間中盤(pán)旋 (表示為 “Satu-2”)。

由于較高的旋轉(zhuǎn)速率或較大的離心力，兩個(gè)實(shí)驗(yàn)在初始階段后都會(huì)遭受IMU飽和。據(jù)我們所知，以前的SLAM系統(tǒng)無(wú)法應(yīng)對(duì)這種激進(jìn)的運(yùn)動(dòng)或飽和的IMU測(cè)量。

4.5.1 旋轉(zhuǎn)運(yùn)動(dòng)

使用放置在雜亂的實(shí)驗(yàn)室環(huán)境中的旋轉(zhuǎn)平臺(tái)進(jìn)行 (見(jiàn)圖10 (a1)-(a4))。

在實(shí)驗(yàn)過(guò)程中，傳感器套件由步進(jìn)電機(jī)在步進(jìn)角速度命令下旋轉(zhuǎn)，該步進(jìn)角速度命令從零逐步增加到峰值，然后在結(jié)束時(shí)減小到零。所得的峰值角速度為75 rad/s (yaw)，遠(yuǎn)遠(yuǎn)超過(guò)IMU測(cè)量范圍，即35 rad/s。高角速度還會(huì)導(dǎo)致大約80m/s2 的峰值加速度，也遠(yuǎn)遠(yuǎn)超出IMU測(cè)量范圍，即大約30m/s2 。

圖10 (c1) 和 (c2) 中所示的板載FPV圖像給出了旋轉(zhuǎn)過(guò)程的圖示。Point-LIO的建圖結(jié)果如圖10 (b1) 所示，它顯示了建圖結(jié)果與環(huán)境基本一致，估計(jì)的結(jié)束位置與開(kāi)始位置非常吻合，如圖10 (b2) 所示。

將估計(jì)的運(yùn)動(dòng)學(xué)狀態(tài) (包括歐拉角旋轉(zhuǎn)和位置) 與圖11 (a) 中的GT進(jìn)行比較，其中x軸被分成三個(gè)部分以放大時(shí)間段84-85 s。yaw的連續(xù)快速變化是由于步進(jìn)電機(jī)驅(qū)動(dòng)的連續(xù)旋轉(zhuǎn)，位置上的正弦形波動(dòng)是由Vicon標(biāo)記和步進(jìn)電機(jī)軸之間的偏移引起的。可以看出：

• 估計(jì)的yaw角可以在整個(gè)過(guò)程中緊密跟蹤實(shí)際yaw角

• 總體旋轉(zhuǎn)和平移誤差 (以RMSE計(jì)) 分別為4.60°和0.233m

• 1）平移的RMSE略大主要是由y方向引起的，從一開(kāi)始，該方向的約束就不足?？紤]到實(shí)驗(yàn)中的極端運(yùn)動(dòng)，這種平移誤差是可以接受的。

Point-LIO的另一個(gè)好處是能夠估計(jì)IMU飽和時(shí)的角速度和加速度 (即系統(tǒng)的狀態(tài)，因此可以通過(guò)卡爾曼濾波器進(jìn)行估計(jì))。圖11 (b) 展示了估計(jì)值與IMU測(cè)量值的關(guān)系。可以看出，在50-106 s的時(shí)間段內(nèi)，IMU飽和 (陀螺儀的z軸和加速度計(jì)的y軸)，而Point-LIO仍然可以給出合理的估計(jì)。在該區(qū)域之外，盡管對(duì)一些高頻分量進(jìn)行了濾波，但Point-LIO的估計(jì)與IMU測(cè)量結(jié)果非常吻合。

通過(guò)以不同的電動(dòng)機(jī)初始角速度啟動(dòng)Point-LIO來(lái)進(jìn)一步挑戰(zhàn)它。如圖12中的地圖結(jié)果和表3中的旋轉(zhuǎn)RMSE所示，當(dāng)初始角速度低于IMU飽和值，即35 rad/s，Point-LIO能夠通過(guò)構(gòu)建合理的地圖和狀態(tài)估計(jì)來(lái)生存。與上述傳感器從靜止位姿開(kāi)始的情況相比，狀態(tài)估計(jì)的質(zhì)量略有下降。這種性能下降是合理的，因?yàn)榭焖俚某跏冀撬俣葘?dǎo)致Point-LIO在一開(kāi)始就構(gòu)建有偏差的地圖，這進(jìn)一步誤導(dǎo)了隨后的狀態(tài)估計(jì)。這也導(dǎo)致旋轉(zhuǎn)RMSE隨初始角速度的增加而增加，如表3所示。當(dāng)初始角速度超出IMU測(cè)量范圍時(shí)，Point-LIO由于太大的初始狀態(tài)估計(jì)而失敗 (例如，初始角速度估計(jì)設(shè)置為零，而實(shí)際大于35 rad/s)。

最后，作為對(duì)比，在同一數(shù)據(jù)集上運(yùn)行FAST-LIO2和Point-LIO-input，旋轉(zhuǎn)和位置的誤差比較如圖13所示?？梢钥闯?

• FAST-LIO2，Point-LIO-input和Point-LIO的估計(jì)在IMU正常工作的前50 s期間具有可比的旋轉(zhuǎn)誤差。

• 從IMU開(kāi)始飽和的50s開(kāi)始，F(xiàn)AST-LIO2和Point-LIO-input的旋轉(zhuǎn)估計(jì)開(kāi)始立即發(fā)散，并且位置的估計(jì)也開(kāi)始漂移然后發(fā)散。

總之，F(xiàn)AST-LIO2以及Point-LIO-input在飽和IMU測(cè)量下無(wú)法工作，而如果沒(méi)有從一開(kāi)始就飽和，Point-LIO可以很好地生存。

4.5.2 盤(pán)旋運(yùn)動(dòng)

該實(shí)驗(yàn)是在相同的實(shí)驗(yàn)室環(huán)境中使用擺錘進(jìn)行的 (參見(jiàn)圖14 (d))。在此實(shí)驗(yàn)中，傳感器套件被綁在繩索的一端，該繩索在垂直平面中擺動(dòng)成盤(pán)旋軌跡 (請(qǐng)參見(jiàn)圖14 (b))。

此運(yùn)動(dòng)導(dǎo)致圓底部的加速度高達(dá)40m/s2，超過(guò)了IMU測(cè)量范圍30m/s2。圖14 (e) 中所示的FPV圖像給出了運(yùn)動(dòng)的視覺(jué)圖示。

定性結(jié)果：

圖14 (c1) 和 (c2) 顯示了Point-LIO的建圖結(jié)果。由于LiDAR面向前方，具有70.4° × 77.2° 圓形FoV，因此僅繪制了實(shí)驗(yàn)室的一側(cè)。估計(jì)的軌跡如圖14 (a) 所示，與圖14 (b) 所示的實(shí)際傳感器路徑高度吻合。

定量結(jié)果：

將估計(jì)的歐拉角旋轉(zhuǎn)和位置與圖15 (a) 中由Vicon測(cè)量的真值進(jìn)行比較，其中歐拉角和位置的估計(jì)在12個(gè)連續(xù)圓都成功了。平均旋轉(zhuǎn)誤差和平移誤差的RMSE分別為4.42°和0.0990 m。

圖15 (b) 顯示了通過(guò)Point-LIO與IMU測(cè)量值的估計(jì)角速度和加速度?？梢钥闯觯?/p>

• 當(dāng)這些動(dòng)態(tài)狀態(tài)在IMU測(cè)量范圍內(nèi)時(shí)，估計(jì)值與IMU測(cè)量非常吻合

• 即使IMU飽和，Point-LIO也可以給出合理的加速度估計(jì)

圖16進(jìn)一步示出了Point-LIO、Point-LIO-input和基于幀的里程計(jì)FAST-LIO2之間的誤差比較。與上一個(gè)實(shí)驗(yàn)類(lèi)似，由于能夠應(yīng)對(duì)IMU飽和，Point-LIO的估計(jì)誤差始終低于其他兩個(gè)。

4.6 實(shí)時(shí)性能

在圖17中示出了用于處理LiDAR點(diǎn)的一次掃描的Point-LIO-output的每個(gè)步驟的平均時(shí)間成本，其在基于intel i7的微型UAV機(jī)載計(jì)算機(jī)、具有1.8 GHz四核Intel i7-8550U CPU和8 GB RAM的DJI Manifold2-C7上進(jìn)行測(cè)試。

建圖包括搜索最近的點(diǎn)并將點(diǎn)添加到地圖中，這占用了最大的時(shí)間消耗。即使在每個(gè)LiDAR點(diǎn)處更新系統(tǒng)狀態(tài)，EKF濾波的時(shí)間包括狀態(tài)傳播和更新，對(duì)于10Hz序列小于10ms，對(duì)于100Hz序列小于1 ms。在表4中的FAST-LIO2、Point-LIO-input和Point-LIO-output之間比較一次掃描的平均總時(shí)間消耗。表示LIO在這些序列中失敗。

可以看出，Point-LIO (Point-LIO-input或Point-LIO)，具有與FAST-LIO2相當(dāng)?shù)臅r(shí)間消耗，且都實(shí)現(xiàn)了實(shí)時(shí)性能，即，對(duì)于10Hz序列在100ms內(nèi)，對(duì)于100Hz序列在10ms內(nèi)。最后，所有序列上每秒處理的平均點(diǎn)數(shù) (包括具有和不具有平面對(duì)應(yīng)關(guān)系的點(diǎn)數(shù))是33,710，并且每個(gè)點(diǎn)的平均處理時(shí)間為9 us。

5 基準(zhǔn)結(jié)果

在各種公開(kāi)數(shù)據(jù)集序列上對(duì)Point-LIO進(jìn)行測(cè)試，這些序列具有更平緩的運(yùn)動(dòng)，沒(méi)有IMU飽和，并將其與其他最先進(jìn)的LiDAR慣性里程計(jì)方法進(jìn)行了比較，包括FAST-LIO2，LILI-OM，LIO-SAM 和LINS。

用于基準(zhǔn)比較的計(jì)算平臺(tái)是與FAST-LIO2中使用的相同的輕型無(wú)人機(jī)機(jī)載計(jì)算機(jī)，它是具有1.8 GHz四核英特爾i7-8550U CPU和8GB RAM的DJI Manifold2-C7，因此FAST-LIO2，LILI-OM，LIO-SAM，LINS可以直接從FAST-LIO2論文中獲得。

Point-LIO使用與FAST-LIO2相同的建圖結(jié)構(gòu)，為了公平比較，將Point-LIO的建圖參數(shù)設(shè)置為與FAST-LIO2的默認(rèn)值相同，即：

• 局部地圖大小L = 1000 m

• 在1:4 時(shí)間下采樣(每四個(gè)LiDAR點(diǎn)取一個(gè)) 之后，LiDAR原始點(diǎn)直接輸入狀態(tài)估計(jì)

• 空間下采樣分辨率l= 0.5m

• ikd-Tree的重新平衡閾值為 ,Nmax=1500

• 對(duì)于Point-LIO的EKF部分，將卡爾曼濾波器的LiDAR測(cè)量噪聲設(shè)置為 (見(jiàn) (18))

這些參數(shù)值對(duì)于所有序列都保持相同。

在FAST-LIO2中使用的相同12個(gè)序列上評(píng)估本文的方法，這些序列來(lái)自4個(gè)不同的公共數(shù)據(jù)集，即 “l(fā)ili”（來(lái)自LILI-OM），“utbm”，“ulhk”， “l(fā)iosam”（LIO-SAM中）。其中，“l(fā)ili” 使用固態(tài)3D激光雷達(dá)Livox Horizon，而其他三個(gè)數(shù)據(jù)集使用旋轉(zhuǎn)的激光雷達(dá)，即，用于 “utbm” 和 “ulhk” 的Velodyne HDL-32E激光雷達(dá)，以及用于 “l(fā)iosam” 的VLP-16激光雷達(dá)。這些激光雷達(dá)有不同的掃描模式。有關(guān)數(shù)據(jù)集和所選序列的更多詳細(xì)信息，參見(jiàn)FAST-LIO2論文。

5.1 準(zhǔn)確率評(píng)估

和FAST-LIO2一樣，采用ATE（平均平移距離）的RMSE（對(duì)于有好GT軌跡的序列）和端到端誤差（對(duì)于起點(diǎn)終點(diǎn)在同一位置的序列）進(jìn)行準(zhǔn)確率評(píng)估。

5.1.1 RMSE基準(zhǔn)

Point-LIO在5個(gè)序列中的4個(gè)達(dá)到了最好的RMSE，在utbm_9序列上最顯著，在liosam_1上略比FAST-LIO2和LIO-SAM差。總體來(lái)說(shuō)，Point-LIO和其他方法準(zhǔn)確率相當(dāng)（大部分情況要更好）。

5.1.2 漂移基準(zhǔn)

總體趨勢(shì)與RMSE基準(zhǔn)結(jié)果相似，即Point-LIO在7個(gè)序列中的5個(gè)序列中實(shí)現(xiàn)了最低的漂移。

• 序列l(wèi)ili_8的結(jié)果比LILI-OM和FAST-LIO2差，這是因?yàn)長(zhǎng)ILI-OM對(duì)他們自己的 “l(fā)ili” 序列調(diào)整了參數(shù)，而Point-LIO的參數(shù)在所有序列中保持相同。

• 由于lili_8具有比其他兩個(gè) “l(fā)ili” 序列長(zhǎng)得多的軌跡，由不適當(dāng)?shù)膮?shù)引起的漂移將沿著后續(xù)過(guò)程累積，并導(dǎo)致比FAST-LIO2差10米以上的漂移。

• Point-LIO在序列ulhk_6上顯示出比FAST-LIO2稍大的RMSE，盡管差距非常小。

從以上基準(zhǔn)結(jié)果可以看出，Point-LIO在大多數(shù)序列中實(shí)現(xiàn)了更高的精度，而對(duì)于其余序列，和最佳方法的差距并不顯著?？紤]到所有數(shù)據(jù)集和序列的各種類(lèi)型的LiDAR、環(huán)境和移動(dòng)平臺(tái)，這有效地顯示了Point-LIO在實(shí)際數(shù)據(jù)上的準(zhǔn)確性和魯棒性。

5.2 處理時(shí)間評(píng)估

Point-LIO和FAST-LIO2都將里程計(jì)和建圖集成在一起，其中在里程計(jì)更新的每個(gè)步驟中立即更新地圖。因此，總時(shí)間 (表7中的 “總計(jì)”) 計(jì)算了里程計(jì)中發(fā)生的所有可能過(guò)程，包括點(diǎn)到地圖匹配，狀態(tài)估計(jì)和建圖。

另外，LILI-OM，LIO-SAM和LINS都基于里程計(jì) (包括特征提取和粗位姿估計(jì)) 和建圖 (例如LILI-OM中的后端融合，LIO-SAM中的增量平滑和建圖，和在LINS中進(jìn)行的地圖細(xì)化)，其每個(gè)LiDAR掃描的平均處理時(shí)間在對(duì)計(jì)算時(shí)間進(jìn)行排序時(shí)由這兩個(gè)部分 (“Odo.” 和 “Map.”) 求和。

可以看出，與其他方法相比，Point-LIO和FAST-LIO2實(shí)現(xiàn)了最少的計(jì)算時(shí)間。與FAST-LIO2相比，Point-LIO在12個(gè)序列中的7個(gè)上花費(fèi)的時(shí)間更少。這兩種方法的平均計(jì)算時(shí)間非常接近。

需要注意，基于幀的FAST-LIO2使用4個(gè)線程來(lái)并行化最近鄰搜索，而Point-LIO是逐點(diǎn)的，必須串行執(zhí)行這樣的操作。盡管如此，Point-LIO平均計(jì)算時(shí)間仍然相當(dāng)，這表明計(jì)算資源的使用較少，可以保留給其他模塊 (例如規(guī)劃、控制)。計(jì)算效率歸功于系統(tǒng)的稀疏性以及不需要迭代卡爾曼濾波。

總之，Point-LIO具有與FAST-LIO2相當(dāng)?shù)臏?zhǔn)確性和計(jì)算效率，且花費(fèi)更少的計(jì)算資源。同時(shí)，Point-LIO比當(dāng)前最先進(jìn)的LIO算法 (即LILI-OM，LIO-SAM，LINS) 明顯更快，實(shí)現(xiàn)了相當(dāng)或更好的準(zhǔn)確率。

6 應(yīng)用

將Point-LIO應(yīng)用于兩個(gè)無(wú)人機(jī) (UAV) 的狀態(tài)估計(jì):

• 圖18 (a) 中所示的競(jìng)賽四旋翼無(wú)人機(jī)上，其推力重量比高達(dá)5.4。高推力重量比使其能進(jìn)行極其敏捷的運(yùn)動(dòng)。

• 另一個(gè)UAV位于稱為自旋轉(zhuǎn)UAV的敏捷、單驅(qū)動(dòng)飛機(jī)上，如圖18 (b) 所示。螺旋槳葉片通過(guò)兩個(gè)被動(dòng)鉸鏈連接到電機(jī)軸，通過(guò)調(diào)節(jié)螺旋槳旋轉(zhuǎn)速度的瞬時(shí)加速度和減速度，能夠產(chǎn)生穩(wěn)定UAV姿態(tài)所需的roll和pitch力矩。由于電機(jī)產(chǎn)生的未補(bǔ)償力矩，無(wú)人機(jī)將產(chǎn)生高速率的連續(xù)yaw旋轉(zhuǎn)。

6.1 競(jìng)賽無(wú)人機(jī)

如圖18 (a) 所示，競(jìng)賽無(wú)人機(jī)安裝有Livox Avia LiDAR和FPV相機(jī)。LiDAR FoV與FPV相機(jī)的FoV對(duì)齊，基于該相機(jī)，專(zhuān)業(yè)人員手動(dòng)控制無(wú)人機(jī)以執(zhí)行極其敏捷的飛行操作。飛行是在植被、池塘和建筑物的農(nóng)田上方進(jìn)行的 (見(jiàn)圖19 (b)，(c))。在飛行過(guò)程中執(zhí)行了幾種激進(jìn)的動(dòng)作，包括極快的滾動(dòng)翻轉(zhuǎn) (見(jiàn)圖19 (e1)-(e3))，俯沖和橫向加速。在翻轉(zhuǎn)期間，角速度達(dá)到59.37 rad/s，超過(guò)IMU測(cè)量范圍35 rad/s。

進(jìn)行了兩次飛行都成功了。由于空間限制，僅展示一次飛行的結(jié)果。建圖結(jié)果如圖19 (d1)-(d3) 所示，可以看出所構(gòu)造的地圖與地面上易于區(qū)分的精細(xì)結(jié)構(gòu) (例如樹(shù)木和建筑物) 一致。

歐拉角旋轉(zhuǎn)、位置和速度的估計(jì)如圖20 (a) 所示，角速度與IMU測(cè)量值的估計(jì)如圖20 (b) 所示?？梢钥吹?，即使在極端激進(jìn)的運(yùn)動(dòng)下，也能夠估計(jì)無(wú)人機(jī)的狀態(tài)。一個(gè)軸上的最大速度達(dá)到14.63 m/s，最大加速度達(dá)到30 m/s，角速度達(dá)到59 rad/s。此外，LiDAR偶爾會(huì)面對(duì)天空，導(dǎo)致沒(méi)有LiDAR測(cè)量，但是狀態(tài)仍然能穩(wěn)定地被估計(jì)。

6.2 自轉(zhuǎn)無(wú)人機(jī)

Livox Avia LiDAR位于UAV前方，當(dāng)UAV經(jīng)歷連續(xù)yaw旋轉(zhuǎn)時(shí)，F(xiàn)oV會(huì)快速變化。IMU是LiDAR內(nèi)置的，測(cè)量范圍為17.5 rad/s，而無(wú)人機(jī)的平均yaw角速度約為25 rad/s。無(wú)人機(jī)還配備了基于ARM的低功耗計(jì)算機(jī)Khadas VIM3 Pro，該計(jì)算機(jī)具有2.2 GHz四核Cortex-A73 CPU和4 GB RAM。機(jī)載計(jì)算機(jī)運(yùn)行Point-LIO實(shí)時(shí)估計(jì)無(wú)人機(jī)狀態(tài)，估計(jì)的狀態(tài)輸入給飛行控制器Pixhawk 4 Mini，以執(zhí)行實(shí)時(shí)控制任務(wù)。

使用這種無(wú)人機(jī)進(jìn)行了兩個(gè)實(shí)驗(yàn)，一個(gè)是香港大學(xué)Haking Wong樓外的室外實(shí)驗(yàn)，如圖21 (a1) 和 (a2) 所示，另一個(gè)是在圖21(b1)和(b2)所示的雜亂無(wú)章的實(shí)驗(yàn)室進(jìn)行的室內(nèi)實(shí)驗(yàn)。

兩個(gè)實(shí)驗(yàn)的實(shí)時(shí)建圖結(jié)果如圖21所示， (c) 為室外，(d) 為室內(nèi)。兩個(gè)環(huán)境都能成功建圖，且沒(méi)有明顯的重影。

圖22 (a) 和 (b) 分別進(jìn)一步示出了運(yùn)動(dòng)學(xué)狀態(tài) (即旋轉(zhuǎn)，位置和速度)，角速度和加速度的估計(jì)。在53-55s的時(shí)間間隔內(nèi)放大了這些圖，以更好地顯示估計(jì)結(jié)果。

可以看出：

• 盡管IMU在中間飽和以及快速旋轉(zhuǎn)引起的快速FoV變化，仍可以產(chǎn)生與IMU測(cè)量一致的穩(wěn)定狀態(tài)估計(jì)。

• 每個(gè)LiDAR包的平均處理時(shí)間為14.63 ms，而LiDAR包的速率為50Hz，確保了實(shí)時(shí)性能。實(shí)驗(yàn)也證實(shí)了這一點(diǎn)，其中控制器能夠通過(guò)實(shí)時(shí)狀態(tài)反饋執(zhí)行穩(wěn)定的受控飛行。

7 總結(jié)

Point-LIO是一個(gè)魯棒的高帶寬LIO框架：

• 基于一種新穎的逐點(diǎn)更新方案，在每個(gè)點(diǎn)的真實(shí)采樣時(shí)間更新系統(tǒng)狀態(tài)，而無(wú)需將點(diǎn)累積到幀中。消除點(diǎn)累積消除了長(zhǎng)期存在的幀內(nèi)運(yùn)動(dòng)失真，并允許以接近點(diǎn)采樣率 (4-8 kHz) 的高里程計(jì)輸出，這進(jìn)一步使系統(tǒng)能夠跟蹤非?？斓倪\(yùn)動(dòng)

• 為了進(jìn)一步將系統(tǒng)帶寬提高到IMU測(cè)量范圍之外，將有色隨機(jī)過(guò)程擴(kuò)展到運(yùn)動(dòng)學(xué)模型中，將IMU測(cè)量作為系統(tǒng)輸出

所開(kāi)發(fā)系統(tǒng)的帶寬、魯棒性、精度和計(jì)算效率已在具有極其劇烈運(yùn)動(dòng)的現(xiàn)實(shí)世界的實(shí)驗(yàn)，以及具有多種LiDAR類(lèi)型、環(huán)境和運(yùn)動(dòng)模式的公共數(shù)據(jù)集上進(jìn)行了詳盡的測(cè)試。在所有測(cè)試中，Point-LIO都實(shí)現(xiàn)了與其他最先進(jìn)的LIO算法相當(dāng)?shù)挠?jì)算效率和里程計(jì)精度，同時(shí)顯著提高了系統(tǒng)帶寬。

作為里程計(jì)，Point-LIO可以用于各種自主任務(wù)，例如軌跡規(guī)劃、控制和感知，尤其是在涉及非常快的自運(yùn)動(dòng)的情況下 (例如，在存在劇烈振動(dòng)和高角速度或線速度的情況下) 或需要高速里程計(jì)輸出和建圖 (例如，用于高速反饋控制和感知)。

將來(lái)可以在沒(méi)有慣性測(cè)量的LiDAR系統(tǒng)，以及動(dòng)態(tài)物體檢測(cè)系統(tǒng)中探索逐點(diǎn)策略。

本文僅做學(xué)術(shù)分享，如有侵權(quán)，請(qǐng)聯(lián)系刪文。

3D視覺(jué)工坊精品課程官網(wǎng)：3dcver.com

1.面向自動(dòng)駕駛領(lǐng)域的3D點(diǎn)云目標(biāo)檢測(cè)全棧學(xué)習(xí)路線！(單模態(tài)+多模態(tài)/數(shù)據(jù)+代碼)

2.徹底搞透視覺(jué)三維重建：原理剖析、代碼講解、及優(yōu)化改進(jìn)

3.國(guó)內(nèi)首個(gè)面向工業(yè)級(jí)實(shí)戰(zhàn)的點(diǎn)云處理課程

4.激光-視覺(jué)-IMU-GPS融合SLAM算法梳理和代碼講解

5.徹底搞懂視覺(jué)-慣性SLAM：基于VINS-Fusion正式開(kāi)課啦

6.徹底搞懂基于LOAM框架的3D激光SLAM: 源碼剖析到算法優(yōu)化

7.徹底剖析室內(nèi)、室外激光SLAM關(guān)鍵算法原理、代碼和實(shí)戰(zhàn)(cartographer+LOAM +LIO-SAM)

8.從零搭建一套結(jié)構(gòu)光3D重建系統(tǒng)[理論+源碼+實(shí)踐]

9.單目深度估計(jì)方法：算法梳理與代碼實(shí)現(xiàn)

10.自動(dòng)駕駛中的深度學(xué)習(xí)模型部署實(shí)戰(zhàn)

11.相機(jī)模型與標(biāo)定(單目+雙目+魚(yú)眼）

12.重磅！四旋翼飛行器：算法與實(shí)戰(zhàn)

13.ROS2從入門(mén)到精通：理論與實(shí)戰(zhàn)

14.國(guó)內(nèi)首個(gè)3D缺陷檢測(cè)教程：理論、源碼與實(shí)戰(zhàn)

15.基于Open3D的點(diǎn)云處理入門(mén)與實(shí)戰(zhàn)教程

16.透徹理解視覺(jué)ORB-SLAM3：理論基礎(chǔ)+代碼解析+算法改進(jìn)

17.不斷更新中......

重磅！粉絲學(xué)習(xí)交流群已成立

交流群主要有3D視覺(jué)、CV&深度學(xué)習(xí)、SLAM、三維重建、點(diǎn)云后處理、自動(dòng)駕駛、多傳感器融合、CV入門(mén)、三維測(cè)量、VR/AR、3D人臉識(shí)別、醫(yī)療影像、缺陷檢測(cè)、行人重識(shí)別、目標(biāo)跟蹤、視覺(jué)產(chǎn)品落地、視覺(jué)競(jìng)賽、車(chē)牌識(shí)別、硬件選型、ORB-SLAM系列源碼交流、深度估計(jì)、TOF、求職交流等方向。

添加小助理微信（dddvisiona）,一定要備注：研究方向+學(xué)校/公司+昵稱，例如：”3D視覺(jué) + 上海交大 + 靜靜“。請(qǐng)按照格式備注，可快速通過(guò)且邀請(qǐng)進(jìn)群。? ? ? ? ??