基于ESKF 與圖優(yōu)化的輕量級(jí)多傳感器融合SLAM 系統(tǒng)

李文豪

（廣東工業(yè)大學(xué)，廣東 廣州510006）

1 概述

狀態(tài)估計(jì)、定位和建圖是移動(dòng)機(jī)器人的關(guān)鍵技術(shù)，是反饋控制、避障和規(guī)劃等任務(wù)的先決條件。目前主流的是基于視覺以及激光的建圖定位方案，并且投入大量的研究在如何確保系統(tǒng)的穩(wěn)定實(shí)時(shí)運(yùn)行中?；谝曈X方法通常使用單目或立體相機(jī)對(duì)連續(xù)圖像中的特征進(jìn)行分析以確定攝像機(jī)的運(yùn)動(dòng)。盡管基于視覺的方法是可行的，但它們對(duì)初始化、光照以及環(huán)境的紋理比較敏感，這使它們不可靠，但是基于視覺的方案特別適合位置識(shí)別。另一方面，激光雷達(dá)對(duì)環(huán)境不敏感，光照變化基本不會(huì)影響激光雷達(dá)。尤其是遠(yuǎn)程高分辨率三維激光雷達(dá)的應(yīng)用，例如Velodyne VLS-128 和Outster OS1-128 激光雷達(dá),使得激光更適合直接捕捉三維空間中的環(huán)境因此，本文的研究重點(diǎn)是基于激光雷達(dá)的狀態(tài)估計(jì)和SLAM方法。我們提出的方法有如下幾個(gè)特點(diǎn)：

1.1 結(jié)合點(diǎn)線、點(diǎn)面ICP 以及ndt 算法實(shí)現(xiàn)激光里程計(jì)的匹配，并采用了一種高頻幀間匹配與低頻Map 優(yōu)化的策略。

1.2 采用一種以機(jī)器人坐標(biāo)為中心的ESKF 對(duì)IMU 進(jìn)行融合，并實(shí)現(xiàn)對(duì)先驗(yàn)觀測的融合，可更好適配室內(nèi)平面環(huán)境。

1.3 實(shí)現(xiàn)一種簡單直接的去除激光匹配過程中動(dòng)態(tài)物體影響的方法。

2 相關(guān)工作

對(duì)于激光SLAM任務(wù)，核心是根據(jù)傳感器兩幀的數(shù)據(jù)推斷其運(yùn)動(dòng)，其最經(jīng)典的方法即ICP 算法[1],ICP 算法衍生出了多種變體，如IMLS-ICP、NICP、GICP[2]等,近年，Zhang 提出了一種基于特征提取的ICP 變種方法[3]，它首先進(jìn)行特征提取減少匹配點(diǎn)云量并實(shí)現(xiàn)點(diǎn)云的配準(zhǔn)。由于機(jī)械式激光的旋轉(zhuǎn)機(jī)制，當(dāng)激光傳感器運(yùn)動(dòng)時(shí)，產(chǎn)生的點(diǎn)云會(huì)產(chǎn)生較大的畸變，因此單獨(dú)使用激光雷達(dá)姿態(tài)估計(jì)效果不理想，必須要另外的傳感器提供信息去進(jìn)行補(bǔ)償，此外，單獨(dú)的激光傳感器無法適應(yīng)劇烈的運(yùn)動(dòng)以及退化環(huán)境，多傳感器融合提供了解決上述問題可靠的方法，多傳感器融合通常分為松耦合與緊耦合，松耦合通常使用EKF，例如[4-6]采用EKF 去融合IMU、GPS、Lidar 等傳感器，緊耦合由于可以獲得更好的精度，所以獲得了更大的關(guān)注，[7]實(shí)現(xiàn)了一種基于滑動(dòng)窗口優(yōu)化的緊耦合系統(tǒng)，[8]實(shí)現(xiàn)了一種基于IEKF 的緊耦合激光慣導(dǎo)里程計(jì)。

3 系統(tǒng)框架

系統(tǒng)主要由數(shù)據(jù)處理、激光慣導(dǎo)里程計(jì)、后端優(yōu)化、回環(huán)檢測等幾個(gè)模塊構(gòu)成。系統(tǒng)采用ROS 作為通信框架，ROS 是一種適用于機(jī)器人的開源系統(tǒng)，可方便實(shí)現(xiàn)進(jìn)程間的通信，以及進(jìn)程的管理，并有豐富的數(shù)據(jù)分析工具如可視化工具RVIZ 等。

4 數(shù)據(jù)處理

數(shù)據(jù)處理的主要完成濾波與畸變?nèi)コ嫉募す饫走_(dá)點(diǎn)云數(shù)據(jù)包含大量的噪聲，并且過于稠密，因此需要通過濾波去處理，濾波方法如下：首先采用距離濾波提取出ROI 區(qū)域范圍內(nèi)的點(diǎn)云，然后采用基于角度的快速聚類算法[9]對(duì)點(diǎn)云進(jìn)行聚類，剔除掉聚類后數(shù)量少于一定閾值的點(diǎn)，只保留一定體積的點(diǎn)云進(jìn)行匹配，聚類濾波并且過濾掉地面后的效果如圖1 所示。

圖1 濾波提取后的點(diǎn)云

然后基于曲率提取特征點(diǎn)，特征點(diǎn)有兩類平面點(diǎn)與曲面點(diǎn)，具體的實(shí)現(xiàn)方法參考LOAM[3]。經(jīng)過優(yōu)化，濾波以及特征提取的的任務(wù)可以在5ms 以內(nèi)的時(shí)間內(nèi)完成。

5 激光慣導(dǎo)里程計(jì)

激光慣導(dǎo)里程計(jì)輸入IMU 數(shù)據(jù)以及提取的特征點(diǎn)數(shù)據(jù)，輸出系統(tǒng)運(yùn)動(dòng)的狀態(tài)信息。出于輕量化以及運(yùn)行效率考慮，我們采用高頻的scan-scan 匹配結(jié)合低頻的scan-map 優(yōu)化的前端思路，可以最大限度的平衡快速性以及精確性，同時(shí)在scan-scan匹配的階段，我們采用eskf 融合IMU 與激光的估計(jì)結(jié)果，同樣為了追求快速性以及高效率，我們采用松耦合的策略，激光慣導(dǎo)里程計(jì)模塊的結(jié)構(gòu)如圖2。

圖2 激光慣導(dǎo)里程計(jì)模塊

激光慣導(dǎo)里程計(jì)主要由兩個(gè)模塊構(gòu)成，LIO 模塊- 融合scan-scan 的匹配結(jié)果與IMU 的運(yùn)動(dòng)預(yù)測結(jié)果，校正模塊則負(fù)責(zé)對(duì)其結(jié)果進(jìn)行優(yōu)化。Scan-scan 匹配通過最小化點(diǎn)線、點(diǎn)面的殘差從而求解最優(yōu)變換T，即求解一個(gè)非線性最小二乘問題。點(diǎn)線，點(diǎn)面的殘差構(gòu)造如下：

上式可以采用高斯牛頓法迭代求解，首先求解出，如下式所示：

6 圖優(yōu)化融合

圖優(yōu)化信息融合主要負(fù)責(zé)融合激光慣導(dǎo)里程計(jì)、GNSS 信息、回環(huán)檢測信息，結(jié)構(gòu)如圖3 所示，我們采用g2o 實(shí)現(xiàn)，執(zhí)行圖優(yōu)化的策略如下：

(1)具備回環(huán)信息時(shí)，執(zhí)行全局圖優(yōu)化。

(2)由于激光里程計(jì)的飄逸很慢，沒有必要一直添加GNSS 因子，當(dāng)激光里程計(jì)輸出的狀態(tài)協(xié)方差過大，且GNSS 協(xié)方差足夠小時(shí)，添加GNSS 約束因子，并取前m 個(gè)關(guān)鍵幀執(zhí)行局部圖優(yōu)化。

(3)當(dāng)機(jī)器人出于平面場景下運(yùn)動(dòng)時(shí)，可以添加先驗(yàn)全局平面約束，由于LIO 模塊融合了對(duì)于平面信息的觀測，因此全局平面約束可以每間隔n 個(gè)關(guān)鍵幀添加一次。

圖3 后端優(yōu)化框架

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

為了測試系統(tǒng)的實(shí)際性能，搭建了載人車輛并在校園環(huán)境下以及開源數(shù)據(jù)集下進(jìn)行了測試，實(shí)驗(yàn)車輛如圖4 所示。

圖4 實(shí)驗(yàn)自動(dòng)駕駛車輛

我們使用velodyne VLP16 激光雷達(dá)、MTI-300S 慣導(dǎo)以及差分式GPS 作為實(shí)驗(yàn)傳感器，工控機(jī)的配置為i7-8550u, 8g 內(nèi)存。在校園環(huán)境進(jìn)行了測試，并將最終輸出的點(diǎn)云地圖與實(shí)際的衛(wèi)星地圖進(jìn)行比較，可以看到點(diǎn)云地圖可以與衛(wèi)星地圖很好的進(jìn)行匹配，這說明該系統(tǒng)估計(jì)的位姿與GPS 真值之間具有很小的誤差，另外，由于高頻scan-scan 的匹配與低頻scan-map 的匹配是通過兩個(gè)獨(dú)立的進(jìn)程完成的，系統(tǒng)狀態(tài)輸出頻率是由scan-scan 的頻率決定的，而scan-scan 的匹配由于具有較少的約束，因此可以在很短的時(shí)間完成，經(jīng)過實(shí)際的測試，對(duì)于16 線的激光雷達(dá)，scan-scan 的匹配可以在10ms 左右的時(shí)間完成，完全達(dá)到實(shí)時(shí)的需求。

圖5 實(shí)驗(yàn)自動(dòng)駕駛車輛

Kitti 數(shù)據(jù)集下測試的結(jié)果如下，其中綠色軌跡為經(jīng)過后端優(yōu)化后的結(jié)果，紅色軌跡為激光里程計(jì)結(jié)果。通過EVO 工具對(duì)最終輸出的結(jié)果與GNSS 真值進(jìn)行比較具有極小的誤差。

圖6 后端優(yōu)化與前端的對(duì)比

圖7 GNSS 融合后evo 評(píng)估結(jié)果