動(dòng)態(tài)環(huán)境下融合激光雷達(dá)和IMU的激光里程計(jì)設(shè)計(jì)

張 濤，張 晨，魏宏宇

(1.東南大學(xué)儀器科學(xué)與工程學(xué)院，南京 210096；2.東南大學(xué)微慣性儀表與先進(jìn)導(dǎo)航技術(shù)教育部重點(diǎn)實(shí)驗(yàn)室，南京 210096)

0 引言

近年來(lái)，世界各地的各個(gè)行業(yè)都在推動(dòng)機(jī)器人的發(fā)展。 智能移動(dòng)機(jī)器人逐漸走入大眾視野，成為人類日常生產(chǎn)、生活中不可或缺的組成部分。在疫情期間，服務(wù)機(jī)器人用于輔助診斷、快速檢測(cè)、精準(zhǔn)測(cè)溫、目標(biāo)識(shí)別，極大地提高了診療的智能化程度，減少醫(yī)務(wù)人員感染風(fēng)險(xiǎn)，提高管控效率。機(jī)器人需要克服許多技術(shù)難點(diǎn)才能成功完成上述復(fù)雜的操作。其中，同時(shí)定位和地圖繪制(Simultaneous Localization and Mapping，SLAM)技術(shù)是機(jī)器人學(xué)中最活躍和最具挑戰(zhàn)性的研究課題之一。激光雷達(dá)SLAM前端根據(jù)前后兩幀的點(diǎn)云進(jìn)行配準(zhǔn)，在估計(jì)變換矩陣時(shí)，動(dòng)態(tài)物體會(huì)引入一定的誤差，最終導(dǎo)致累積誤差不斷增大。盡管通過(guò)引入回環(huán)檢測(cè)節(jié)點(diǎn)，可以消除部分估計(jì)的累積誤差。但是，當(dāng)沒(méi)有回環(huán)檢測(cè)和高精度地圖時(shí)，累積誤差依舊無(wú)法消除。因此，需要通過(guò)去除動(dòng)態(tài)物體對(duì)應(yīng)的點(diǎn)云，使得激光里程計(jì)估計(jì)變換矩陣時(shí)引入的誤差盡可能小，以實(shí)現(xiàn)更準(zhǔn)確的定位和建圖。

D. Giveki等提出了將顏色和紋理特征同Choquet模糊積分相結(jié)合的方法應(yīng)用于背景建模，通過(guò)該建模背景對(duì)現(xiàn)有對(duì)象進(jìn)行分割，以完成動(dòng)態(tài)對(duì)象檢測(cè)。Zou C.等提出了3D全連接條件隨機(jī)場(chǎng)方法，該網(wǎng)絡(luò)對(duì)激光雷達(dá)采集到的連續(xù)兩個(gè)關(guān)鍵幀的點(diǎn)云進(jìn)行處理，得到每個(gè)點(diǎn)作為動(dòng)態(tài)點(diǎn)的概率，從而判斷動(dòng)態(tài)點(diǎn)。Yao W.等提出了一種基于物體特征的目標(biāo)3D分割方法，完成了對(duì)移動(dòng)車輛速度估計(jì)的改進(jìn)，該方法主要應(yīng)用于一些機(jī)載激光雷達(dá)在空中獲取的少量點(diǎn)云。李波等提出了一個(gè)全卷積神經(jīng)網(wǎng)絡(luò)，以激光雷達(dá)點(diǎn)云數(shù)據(jù)為輸入樣本，結(jié)合數(shù)據(jù)增強(qiáng)訓(xùn)練，得到一個(gè)可以檢測(cè)車輛的神經(jīng)網(wǎng)絡(luò)，在神經(jīng)網(wǎng)絡(luò)學(xué)習(xí)的過(guò)程中需要手動(dòng)標(biāo)記學(xué)習(xí)樣本。李響采用傅里葉單像素欠采樣成像對(duì)數(shù)據(jù)進(jìn)行壓縮,提取圖像特征，并在未獲取完整圖像信息的情況下，采用深度學(xué)習(xí)卷積網(wǎng)絡(luò)對(duì)動(dòng)態(tài)物體進(jìn)行檢測(cè)和識(shí)別。文獻(xiàn)[12]的解決方案是先使用前饋神經(jīng)網(wǎng)絡(luò)定位目標(biāo)，計(jì)算其邊界位置，通過(guò)融合方法提取特征，最后用3D卷積網(wǎng)絡(luò)判斷目標(biāo)，并且能夠得到其實(shí)際移動(dòng)速度。

由于實(shí)際場(chǎng)景中存在各種形狀和大小的物體，因此本文采用常規(guī)的聚類算法得到的效果不佳，且由于點(diǎn)云數(shù)量過(guò)于龐大，存在復(fù)雜度高、聚類速度慢等特點(diǎn)。為了識(shí)別激光雷達(dá)點(diǎn)云中的動(dòng)靜態(tài)目標(biāo)并提取動(dòng)態(tài)點(diǎn)，本文提出了一種融合激光雷達(dá)和慣性測(cè)量單元(Inertial Measurement Unit，IMU)的動(dòng)態(tài)點(diǎn)云識(shí)別算法。該策略通過(guò)約束激光雷達(dá)掃描范圍角與動(dòng)態(tài)中心，將點(diǎn)云快速分割成多個(gè)簇類，再借助IMU預(yù)估計(jì)的位姿變換信息代替特征點(diǎn)匹配，快速建立點(diǎn)云簇類配準(zhǔn)關(guān)系，從而根據(jù)中心點(diǎn)偏移的程度判斷出簇類是否為動(dòng)態(tài)物體，完成動(dòng)態(tài)點(diǎn)云的識(shí)別與分割任務(wù)。最后，再根據(jù)簇類的對(duì)應(yīng)關(guān)系進(jìn)行約束，以提高傳統(tǒng)特征匹配策略的精度與速度。

1 整體框架

本文的整體方案如圖1所示，系統(tǒng)通過(guò)搭載的激光雷達(dá)掃描得到點(diǎn)云與IMU測(cè)量得到的信息作為輸入，最終輸出系統(tǒng)的位姿信息。整個(gè)系統(tǒng)主要包含3個(gè)模塊：點(diǎn)云預(yù)處理模塊、特征點(diǎn)提取模塊和激光里程計(jì)模塊。點(diǎn)云預(yù)處理模塊首先將地面點(diǎn)去除，再通過(guò)本文提出的動(dòng)態(tài)點(diǎn)識(shí)別去除動(dòng)態(tài)物體與離群散點(diǎn)。特征點(diǎn)提取模塊先將由運(yùn)動(dòng)造成的畸變通過(guò)IMU信息進(jìn)行畸變補(bǔ)償，再結(jié)合簇類信息與點(diǎn)云的粗糙度，提取各自簇類的邊緣特征和平面特征。激光里程計(jì)模塊通過(guò)簇類信息與特征信息進(jìn)行點(diǎn)云配準(zhǔn)，從而完成位姿估計(jì)的過(guò)程，最終輸出位姿。

圖1 方案框架

規(guī)定本文中載體坐標(biāo)系(圖2)的中心位于激光雷達(dá)的坐標(biāo)系原點(diǎn)，在激光雷達(dá)的中心，平面平行于激光雷達(dá)的頂平面，軸指向無(wú)人地面車(Unmanned Ground Vehicle，UGV)的右側(cè)，軸指向UGV的前方，軸垂直于平面向下。

圖2 載體坐標(biāo)系

2 算法流程

2.1 點(diǎn)云預(yù)處理

由于地面點(diǎn)與所有語(yǔ)義對(duì)象相連接，因此，首先對(duì)點(diǎn)云進(jìn)行預(yù)處理，將地面點(diǎn)進(jìn)行識(shí)別與去除，本文選擇采用文獻(xiàn)[13]中的方法去除地面點(diǎn)。由于實(shí)際場(chǎng)景中存在各種形狀和大小的物體，且掃描得到的點(diǎn)云數(shù)量龐大，本文根據(jù)這些特點(diǎn)提出了一種融合激光雷達(dá)和IMU的動(dòng)態(tài)點(diǎn)云識(shí)別算法。本文提出的語(yǔ)義分割與點(diǎn)云算法的工作流程描述如下：

第一步：在點(diǎn)云中任意取一點(diǎn)，在該點(diǎn)對(duì)應(yīng)的激光雷達(dá)掃描角周圍的一定范圍內(nèi)(搜索約束角)，搜索與其歐氏距離相近的點(diǎn)。即滿足

(1)

其中，表示兩點(diǎn)之間的歐拉距離閾值。通過(guò)不斷循環(huán)第一步，直到范圍內(nèi)所有的點(diǎn)都遍歷完畢后，將這些滿足條件的點(diǎn)定為一個(gè)簇類。

第二步：在上述簇類中，選取上下左右前后6個(gè)方向上距離最遠(yuǎn)的點(diǎn)，不斷進(jìn)行第一步操作，且已遍歷過(guò)的點(diǎn)不再進(jìn)行二次遍歷，將符合條件的也加入上述簇類，

第三步：不斷重復(fù)第一步，直到?jīng)]有點(diǎn)再滿足式(1)，則停止迭代。如果最終聚類后得到的簇類中的數(shù)字大于給定的數(shù)量閾值，則通過(guò)式(2)計(jì)算出中心點(diǎn)，將該中心點(diǎn)標(biāo)記為該簇類的核心點(diǎn),并為該簇分配一個(gè)新的標(biāo)簽； 如果小于數(shù)量閾值，則將該簇類內(nèi)的所有點(diǎn)統(tǒng)一標(biāo)記為離群點(diǎn)。

(2)

圖3 聚類結(jié)果

(3)

其中，c表示cos函數(shù)；s表示sin函數(shù)。

(4)

2.2 特征提取

(5)

其中，為激光雷達(dá)掃描一周的時(shí)長(zhǎng)。

為了均勻且快速地提取點(diǎn)云中的特征點(diǎn)，此處對(duì)每個(gè)簇類分別計(jì)算各自的粗糙度，從而提取出邊緣特征和平面特征，計(jì)算公式如下

(6)

其中，,表示時(shí)刻，標(biāo)簽為的點(diǎn)云簇；,為,中的一個(gè)點(diǎn)；,為在,中與,不同的其他點(diǎn)。

根據(jù)值對(duì)每個(gè)簇類中的點(diǎn)進(jìn)行排序，選擇較大的幾個(gè)點(diǎn)為邊緣特征點(diǎn)，較小的幾個(gè)點(diǎn)為平面點(diǎn)。最終得到時(shí)刻的邊緣特征集合為，平面特征集合為。

2.3 激光里程計(jì)

激光里程計(jì)的目的是通過(guò)最小化點(diǎn)到邊緣特征與點(diǎn)到平面特征之間的距離，估計(jì)2個(gè)連續(xù)點(diǎn)云之間的變換關(guān)系。為了找到各自的對(duì)應(yīng)關(guān)系，本文中使用的激光里程計(jì)的基本算法與文獻(xiàn)[14]中的一致，但在搜索過(guò)程中，加入了簇類約束，因此在進(jìn)行特征搜索匹配時(shí)，僅對(duì)同一簇類進(jìn)行點(diǎn)云配準(zhǔn)，通過(guò)簇類約束條件排除錯(cuò)誤的對(duì)應(yīng)關(guān)系。根據(jù)2.2節(jié)提到的算法，可以得到連續(xù)2個(gè)時(shí)刻的邊緣特征與平面特征，分別為,+1與,+1。引入簇類約束后，點(diǎn)到線的距離公式變?yōu)?/p>

=

(7)

其中，label為對(duì)應(yīng)簇類的標(biāo)簽；∈+1；,∈。

同樣，點(diǎn)到面的距離公式變?yōu)?/p>

=

(8)

其中，∈+1;,,∈。

通過(guò)結(jié)合IMU的預(yù)估計(jì)姿態(tài)變換矩陣式(3)與式(7)～式(8)，可以得到一個(gè)非線性優(yōu)化函數(shù)

(9)

激光里程計(jì)最后的目標(biāo)是將式(9)通過(guò)迭代使得盡可能地趨向于0。

(10)

3 實(shí)驗(yàn)驗(yàn)證

本實(shí)驗(yàn)使用的UGV如圖4所示，采用激光雷達(dá)采集點(diǎn)云，同時(shí)為了便于驗(yàn)證，使用相機(jī)采集對(duì)應(yīng)時(shí)間的圖像信息。激光雷達(dá)選擇使用Velodyne-VLP16，該設(shè)備以10幀/s的速度旋轉(zhuǎn)，每個(gè)周期捕獲大約30k點(diǎn)。相機(jī)選用ZED2，以10幀/s和720p的分辨率獲取圖像，同時(shí)該相機(jī)內(nèi)部自帶IMU，可輸出400Hz的IMU信息。處理器為英偉達(dá)的Jetson AGX Xavier，最高能提供32TOPs的運(yùn)算能力。在本節(jié)中，首先使用配備激光雷達(dá)的UGV在多種場(chǎng)景下采集數(shù)據(jù)，并使用本文提出的算法驗(yàn)證其可行性，最后給出一個(gè)直觀的分割結(jié)果。然后，將該算法應(yīng)用于UGV和KITTI數(shù)據(jù)集，并與其他算法進(jìn)行對(duì)比，以驗(yàn)證去除點(diǎn)云中動(dòng)態(tài)物體對(duì)應(yīng)的相關(guān)點(diǎn)后對(duì)激光雷達(dá)SLAM定位精度和效率的影響。整個(gè)系統(tǒng)在Ubuntu 18.04 (Linux)中使用ROS執(zhí)行，并借助ROS中Time_Synchronizer模塊對(duì)3個(gè)傳感器輸入的信息進(jìn)行時(shí)間戳對(duì)齊。實(shí)驗(yàn)中涉及的參數(shù)如表1所示。

圖4 無(wú)人地面車

表1 實(shí)驗(yàn)參數(shù)

3.1 動(dòng)態(tài)點(diǎn)檢測(cè)試驗(yàn)

實(shí)驗(yàn)地點(diǎn)選在走廊。 實(shí)驗(yàn)分為以下四種情況。

情況一：當(dāng)激光雷達(dá)靜止時(shí)，檢測(cè)到單個(gè)動(dòng)態(tài)物體并移除；

情況二：當(dāng)激光雷達(dá)靜止時(shí)，檢測(cè)到多個(gè)動(dòng)態(tài)物體并移除；

情況三：當(dāng)激光雷達(dá)處于動(dòng)態(tài)狀態(tài)時(shí)，檢測(cè)并去除走廊拐角處的動(dòng)態(tài)物體；

情況四：當(dāng)激光雷達(dá)處于動(dòng)態(tài)狀態(tài)時(shí)，檢測(cè)直線走廊并去除動(dòng)態(tài)物體。

如圖5所示，從四種不同的情況中選擇間隔為1s的兩幀進(jìn)行處理。 在實(shí)驗(yàn)環(huán)境中，移動(dòng)的物體主要由人來(lái)扮演。通過(guò)使用第2節(jié)提出的算法進(jìn)行動(dòng)態(tài)點(diǎn)去除，四種不同情況的結(jié)果如圖6所示。為了觀察方便，圖6顯示了處理后的點(diǎn)云的俯視圖， 其中藍(lán)色六邊形代表激光雷達(dá)所在位置，聚類得到的動(dòng)態(tài)物體用青色虛線標(biāo)記。

(a) 情況一

最后，可以在去除圖6中的動(dòng)態(tài)點(diǎn)后得到點(diǎn)云的頂視圖。從圖7中可以看出，在上述四種情況下，本文提出的算法成功去除了環(huán)境中的動(dòng)態(tài)物體。在這四種情況中，還可以觀察到部分情況下由于人體離激光雷達(dá)太近，以至于擋住了身后的整面墻。

(a) 情況一

(a) 情況一

因此，部分墻體在俯視圖中產(chǎn)生了斷層。由于部分墻只在兩個(gè)時(shí)刻中的一個(gè)時(shí)刻被檢測(cè)到，在該算法的處理中，墻的這部分同樣被當(dāng)作動(dòng)態(tài)點(diǎn)被去除。 在后續(xù)的點(diǎn)云配準(zhǔn)過(guò)程中，如果一幀中有一些點(diǎn)在另一幀中沒(méi)有被檢測(cè)到，那么在配準(zhǔn)過(guò)程中強(qiáng)行尋找不對(duì)應(yīng)的點(diǎn)進(jìn)行配準(zhǔn)會(huì)引入相當(dāng)大的誤差。 因此，將這部分靜態(tài)點(diǎn)識(shí)別為動(dòng)態(tài)點(diǎn)并去除它們,也有助于提高姿態(tài)估計(jì)的準(zhǔn)確性。 表2顯示了處理前后的點(diǎn)云數(shù)量，以及在CPU i7-4720上運(yùn)行的平均處理時(shí)間。

表2 分割實(shí)驗(yàn)仿真結(jié)果

3.2 定位實(shí)驗(yàn)

這部分主要使用裝載了激光雷達(dá)的UGV和KITTI數(shù)據(jù)集開(kāi)展實(shí)驗(yàn)。通過(guò)對(duì)比實(shí)驗(yàn)結(jié)果，可以驗(yàn)證本文提出的動(dòng)態(tài)點(diǎn)去除策略對(duì)定位精度的影響。選擇開(kāi)源解決方案LEGO_LOAM與本文的方案進(jìn)行實(shí)驗(yàn)比較。

這部分實(shí)驗(yàn)分為以下四種情況：

情況一：UGV靜止；

情況二：UGV運(yùn)動(dòng)，路徑中不存在環(huán)路；

情況三：UGV運(yùn)動(dòng)，路徑中存在環(huán)路；

情況四：將兩種解決方案應(yīng)用于 KITTI 數(shù)據(jù)集。

情況一：由于UGV處于靜止?fàn)顟B(tài)，實(shí)際坐標(biāo)始終為[0,0,0]。但是，動(dòng)態(tài)物體的存在也會(huì)影響UGV本身的位姿估計(jì)。實(shí)驗(yàn)結(jié)果如圖8所示，其中藍(lán)線表示使用LEGO_LOAM估計(jì)得到的實(shí)時(shí)位置，綠線表示使用本文方案估計(jì)得到的實(shí)時(shí)位置。從圖8可以看出，即使UGV處于靜止?fàn)顟B(tài)，由于周圍物體的移動(dòng)，在估計(jì)位置時(shí)也會(huì)存在偏差。通過(guò)去除動(dòng)態(tài)對(duì)象對(duì)應(yīng)的點(diǎn)云，可以減少動(dòng)態(tài)對(duì)象引入的誤差。具體誤差值如表3所示。由表3可知，由動(dòng)態(tài)誤差引入的誤差的標(biāo)準(zhǔn)偏差降低了約57.7%，最大偏差降低了約49.2%。

圖8 情況一的定位結(jié)果

表3 情況一的定位結(jié)果偏差

情況二：該情況下，由于實(shí)時(shí)真值難以獲取，因此本文僅比較終點(diǎn)的估計(jì)位置和實(shí)際位置，以觀察最后的定位誤差。分別在LEGO_LOAM和本文方案下運(yùn)動(dòng)UGV，獲得估計(jì)位置的路徑如圖9所示。UGV 實(shí)際行駛了18.035m，且路徑呈直線。通過(guò)算法估計(jì)得到，LEGO_LOAM的終點(diǎn)位置偏差為0.5494m，本文方案的終點(diǎn)位置偏差為0.4237m，誤差降低了約23%。

圖9 情況二的路徑

情況三：這種情況下，UGV從一個(gè)點(diǎn)出發(fā)，再次回到起點(diǎn)，從而形成一個(gè)閉環(huán)。 LEGO_LOAM的最終偏差值為0.1736m，本文方案的最終偏差值為0.1040m。 如圖10所示，2個(gè)最終位置偏差都不大，這是由于LEGO-LOAM中使用了檢測(cè)節(jié)點(diǎn)，因此可以有效地消除環(huán)路中的累積誤差，即使行駛的路徑更遠(yuǎn)，也可以得到比情況二更準(zhǔn)確的定位結(jié)果。

圖10 情況三的路徑

情況四：為了驗(yàn)證大范圍復(fù)雜場(chǎng)景下全路徑定位精度的提升，將算法應(yīng)用到KITTI數(shù)據(jù)集下。最終兩種解決方案的結(jié)果如圖11和表4所示。在整個(gè)路徑中，最大偏差減少了約31.7%，標(biāo)準(zhǔn)偏差減少了約16.5%。

圖11 情況四的路徑及位置誤差

表4 情況四的定位結(jié)果偏差

4 結(jié)論

本文針對(duì)動(dòng)態(tài)物體造成激光里程計(jì)定位產(chǎn)生偏差的問(wèn)題，提出了一種融合激光雷達(dá)和IMU的動(dòng)態(tài)點(diǎn)云識(shí)別算法，并且結(jié)合該過(guò)程改進(jìn)了傳統(tǒng)特征匹配策略，組成了動(dòng)態(tài)環(huán)境下融合激光雷達(dá)和IMU的激光里程計(jì)。算法分析與實(shí)驗(yàn)結(jié)果表明：

1)動(dòng)態(tài)物體的存在對(duì)系統(tǒng)自身位姿估計(jì)確實(shí)產(chǎn)生了一定的影響，而本文提出的動(dòng)態(tài)點(diǎn)云識(shí)別算法,通過(guò)約束范圍角與動(dòng)態(tài)中心點(diǎn),將點(diǎn)云快速分割成多個(gè)簇類，再借助IMU預(yù)估計(jì)的變換矩陣，快速建立點(diǎn)云簇類配準(zhǔn)關(guān)系，從而去除動(dòng)態(tài)點(diǎn)，能夠有效地去除環(huán)境中的單個(gè)和多個(gè)動(dòng)態(tài)物體。

2)當(dāng)動(dòng)態(tài)物體和靜態(tài)物體在三維空間中重合時(shí)，很容易將兩者聚類為一個(gè)整體，在后續(xù)過(guò)程中則會(huì)將它們作為一個(gè)完整的動(dòng)態(tài)物體進(jìn)行點(diǎn)云去除。然而，在實(shí)際情況中，通常只有在動(dòng)態(tài)對(duì)象與靜態(tài)對(duì)象碰撞時(shí)才會(huì)發(fā)生，因此可以單獨(dú)處理。

3)當(dāng)存在回環(huán)的情況下，能夠通過(guò)增加回環(huán)檢測(cè)節(jié)點(diǎn)減小動(dòng)態(tài)物體引入的累積誤差，但無(wú)法徹底消除，而通過(guò)去除動(dòng)態(tài)點(diǎn)可以較大地提高激光里程計(jì)定位的準(zhǔn)確度。

4)當(dāng)連續(xù)兩幀中檢測(cè)到的物體在另外一幀中沒(méi)有出現(xiàn)時(shí)，本文提出的方案會(huì)將其認(rèn)為是動(dòng)態(tài)物體進(jìn)行去除，能夠提高點(diǎn)云配準(zhǔn)的精度。