張 濤，張 晨，魏宏宇

(1.東南大學儀器科學與工程學院，南京 210096；2.東南大學微慣性儀表與先進導航技術教育部重點實驗室，南京 210096)

0 引言

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

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

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

1 整體框架

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

圖1 方案框架

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

圖2 載體坐標系

2 算法流程

2.1 點云預處理

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

第一步：在點云中任意取一點，在該點對應的激光雷達掃描角周圍的一定范圍內(搜索約束角)，搜索與其歐氏距離相近的點。即滿足

(1)

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

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

第三步：不斷重復第一步，直到沒有點再滿足式(1)，則停止迭代。如果最終聚類后得到的簇類中的數字大于給定的數量閾值，則通過式(2)計算出中心點，將該中心點標記為該簇類的核心點,并為該簇分配一個新的標簽； 如果小于數量閾值，則將該簇類內的所有點統一標記為離群點。

(2)

圖3 聚類結果

(3)

其中，c表示cos函數；s表示sin函數。

(4)

2.2 特征提取

(5)

其中，為激光雷達掃描一周的時長。

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

(6)

其中，,表示時刻，標簽為的點云簇；,為,中的一個點；,為在,中與,不同的其他點。

根據值對每個簇類中的點進行排序，選擇較大的幾個點為邊緣特征點，較小的幾個點為平面點。最終得到時刻的邊緣特征集合為，平面特征集合為。

2.3 激光里程計

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

=

(7)

其中，label為對應簇類的標簽；∈+1；,∈。

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

=

(8)

其中，∈+1;,,∈。

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

(9)

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

(10)

3 實驗驗證

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

圖4 無人地面車

表1 實驗參數

3.1 動態(tài)點檢測試驗

實驗地點選在走廊。 實驗分為以下四種情況。

情況一：當激光雷達靜止時，檢測到單個動態(tài)物體并移除；

情況二：當激光雷達靜止時，檢測到多個動態(tài)物體并移除；

情況三：當激光雷達處于動態(tài)狀態(tài)時，檢測并去除走廊拐角處的動態(tài)物體；

情況四：當激光雷達處于動態(tài)狀態(tài)時，檢測直線走廊并去除動態(tài)物體。

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

(a) 情況一

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

(a) 情況一

(a) 情況一

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

表2 分割實驗仿真結果

3.2 定位實驗

這部分主要使用裝載了激光雷達的UGV和KITTI數據集開展實驗。通過對比實驗結果，可以驗證本文提出的動態(tài)點去除策略對定位精度的影響。選擇開源解決方案LEGO_LOAM與本文的方案進行實驗比較。

這部分實驗分為以下四種情況：

情況一：UGV靜止；

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

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

情況四：將兩種解決方案應用于 KITTI 數據集。

情況一：由于UGV處于靜止狀態(tài)，實際坐標始終為[0,0,0]。但是，動態(tài)物體的存在也會影響UGV本身的位姿估計。實驗結果如圖8所示，其中藍線表示使用LEGO_LOAM估計得到的實時位置，綠線表示使用本文方案估計得到的實時位置。從圖8可以看出，即使UGV處于靜止狀態(tài)，由于周圍物體的移動，在估計位置時也會存在偏差。通過去除動態(tài)對象對應的點云，可以減少動態(tài)對象引入的誤差。具體誤差值如表3所示。由表3可知，由動態(tài)誤差引入的誤差的標準偏差降低了約57.7%，最大偏差降低了約49.2%。

圖8 情況一的定位結果

表3 情況一的定位結果偏差

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

圖9 情況二的路徑

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

圖10 情況三的路徑

情況四：為了驗證大范圍復雜場景下全路徑定位精度的提升，將算法應用到KITTI數據集下。最終兩種解決方案的結果如圖11和表4所示。在整個路徑中，最大偏差減少了約31.7%，標準偏差減少了約16.5%。

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

表4 情況四的定位結果偏差

4 結論

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

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

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

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

4)當連續(xù)兩幀中檢測到的物體在另外一幀中沒有出現時，本文提出的方案會將其認為是動態(tài)物體進行去除，能夠提高點云配準的精度。