車載激光雷達的應用算法

杜海林，張杭鋮，王齊超

（長安大學 汽車學院，陜西 西安 710064）

在國家交通強國戰略和新基建政策推動下，智能網聯汽車產業已進入加速發展的新階段。在未來萬物互聯的體系中，只有實現“車-路-云”三方數據互聯互通，形成車路感知數據融合與決策控制才有可能實現最高級別的全場景自動駕駛。而激光雷達作為目前技術成熟度和檢測效果較為均衡的傳感器之一，是智能網聯汽車實現車路協同感知不可或缺的傳感器。

激光雷達系統是以收發激光束偵察目標物體的位移、速率等特征量的雷達系統[1]。

目前，車載激光雷達上的應用算法主要包括目標跟蹤和識別、即時定位與地圖構建（Simultaneous Localization And Mapping, SLAM）和點云分割[2]。

1 目標跟蹤與識別算法

激光雷達的目標跟蹤是點云數據處理的最關鍵的一部分。目標跟蹤的主要目的是獲取障礙物的距離、速度以及運動軌跡等信息。通過目標跟蹤算法對障礙物運動狀態做出估計和預測，我們便可以實時地評估障礙物和無人駕駛車輛的安全等級，作出相應的決策。因此，目標跟蹤算法的優劣直接影響到無人駕駛車輛的安全等級。

目標跟蹤算法需要處理的信息有很多，除了對目標位置，速度的估計之外，有時候目標的運動狀態也需要考慮。所以為了評估激光雷達目標跟蹤算法的性能，算法的速度、準確度以及跟蹤穩定性是重要的指標。

目標跟蹤可以分為以下幾種任務：單目標追蹤、多目標追蹤、姿態追蹤、在線追蹤、離線追蹤。雖然目標跟蹤的前景非常廣闊，但是也存在一些問題限制了其發展。形態變化，追蹤過程中目標的姿態發生變化時，會導致其特征以及外觀模型變化，容易使得目標追蹤失敗[3]。尺度變化，當目標尺度縮小時，容易包含多的背景色；而當目標尺度增大時，又容易導致跟蹤框不能包括整個物體。激光雷達點云隨距離增加變得稀疏，環境感知算法對障礙物檢測性能降低，遠處目標不能有效檢測[4]。遮擋與消失，在跟蹤目標時，如果遇到障礙物或者目標突然消失，容易造成跟蹤失敗。圖像模糊，目標的快速移動，外界環境如：光照、雨霧等因素會造成分辨率降低，從而使得跟蹤失敗。

隨著無人駕駛技術的日益發展，目標跟蹤算法理論也日益成熟，在目標跟蹤算法中主要的有加權最小二乘濾波法、CSK濾波跟蹤法、DCS濾波法以及卡爾曼濾波法。其中使用最為廣泛的就是卡爾曼濾波法。然而在車輛行駛過程中由于激光雷達的特性，在每一幀中屬于同一個障礙物的點云數量會產生差異，這就導致了聚類出的同一障礙物的大小發生變化，使得卡爾曼濾波對目標進行跟蹤預測時會產生較大的誤差。而且誤差可能會隨著時間的推進逐步增大，導致目標跟蹤丟失[5]。李帥印等[6]結合了卡爾曼濾波算法和改進匈牙利算法，提出了一種基于多層激光雷達的目標檢測與跟蹤算法，以適應無人駕駛中目標增多或減少的情況。王海等[7]提出了一種基于激光雷達的3D實時車輛跟蹤方法，使用全局最近鄰（Global Nearest Neighbor, GNN）算法作為關聯算法，并通過雙波門設計，提升了算法精度與速度。ALIREZA A等[8]在激光雷達數據的背景下，提供了一個基于體素和平面的三維感知體系，可以進行在大城市環境中的地面模型和障礙物測量。由于對周圍環境感知的需求提升，可以看出多目標跟蹤算法將會是未來的主流方向之一，同時應用GNN也能幫助我們解決多目標跟蹤中存在的一些問題。也可以考慮攝像頭與激光雷達融合的跟蹤算法。雖然多算法與多傳感器的耦合能提高整個系統的精度，但同時伴隨著的也有整個系統復雜性和成本的提高，這也是以后研究需要考慮的問題。

2 即時定位與地圖構建

SLAM 即機器人在移動的過程中根據自身的位置進行自我定位，從而實現機器人的自我導航和定位[9]。建圖這一步驟也可利用激光雷達掃描周圍環境完成，目前激光雷達主要分為單軌制與多線這兩個主要方面，根據這兩大類激光雷達（Light Detection and Ranging, LIDAR）提供了不同的計算方式。首先，單軌制激光雷達主要使用在平面體育運動場景中，而多線激光雷達則使用在三維空間的體育運動場景。當面對復雜多變的體育運動自然環境時，多傳感器融合的SLAM則成為了當前發展趨勢。視野特點會帶來精確的體育里程數計和數據信息量巨大的地圖信息內容，而激光雷達則為視野特點帶來更精確的體育深度信號。因此，SLAM 計算的魯棒特征和實時化能力有待進一步提高。在提升SLAM算法魯棒性技術領域方面，還必須充分考慮里程計的標準、激光雷達的外參與時戳標準、激光雷達運動畸變的消除等數據處理步驟，但同樣也受制于退化環境、全局位置、動態環境位置等技術問題。

2.1 Gmapping

Gmapping是一種通過粒子濾波的2D激光雷達 SLAM，可以用來形成二維柵格圖，它融合里程計數據，但沒有回環檢測。它的優點是在小環境中，運算容量小，速度也比較快。但不足之處是每種粒子都包含著一張版圖，根本無法處理大環境（存儲和運算量很大）；一旦運營里程錯誤或者標定參數錯誤，在較長回廊的環境中會將版圖重建歪。使用粒子濾波架構的激光 SLAM，融合了里程計和激光數據信息，每種粒子都可以裝載一張版圖[10]，建立的長場景版圖所需要的運算時間相對較少，而且準確性也較好。正因為這是一種基于粒子濾波的新技術，所以存在的問題還有粒子耗盡的問題。GRISETTI G等[11]在論文中提供了一個對有選擇性進行重采樣操作的辦法，這有效地克服了粒子耗盡的問題。

2.2 Hector_slam

通過優化算法實現幀內匹配的激光SLAM算法，由于不要求里程計數據，所以代碼量較小。Hector slam是完全基于掃描匹配的，掃描匹配就是使用當前幀與已有地圖數據構建誤差函數，并用高斯牛頓法得到最優解和偏差量。Hector slam可以通過迭代優化的算法求得匹配的最佳距離，為了防止陷入局部極值錯誤，還可以使用更高解析度的地圖匹配。但因為完全依賴于掃描匹配，所以需要雷達設備的位置準確度很高、雷達測速效率高、視野范圍寬，或者掃描效率很高（或移動速度慢）。在噪音大、特征點少的情況下就比較容易失效。文中[12]提出的方法加入了慣性測量單元（Inertial Measurement Unit, IMU），使用擴展卡爾曼濾波（Extended Kalman Filter, EKF）估計整體的六自由度（Degree Of Freedom, DOF）位姿，并通過橫擺角和俯仰角將激光掃描圖像數據投影到XY平面位置，因而支持激光雷達數據有一些程度的偏斜，例如：手持或機器人運動在并非很水平的地板上。KOHLBRECHER S 等[12]則通過使用對地圖梯度的快速近似和高分辨率網格，從而達到了在各種挑戰性場合中的準確位置和地圖測繪水平。

2.3 Karto

圖優化 SLAM，包括回環，一般使用對矩陣的稀疏化加以解決，在大規模條件下建圖時具有優越性。KARTO是使用了scan-matching，回環檢測和圖優化的SLAM方法，并通過稀疏姿態調整（Sparse Pose Adjustment, SPA）實現優化。KONOLIGE K等[10]提供了一個建立并解決線性子問題的最有效方式，即SPA優化法。該方法與其他直接方法相比較，有更優越的收斂速度和精度，這一方法在真實地圖和大規模數據集上展示了其有效性。

2.4 Cartographer

Cartographer是谷歌開源的激光 SLAM 技術框架，其優點是首先引入了 submap，即 scan to submap matching，將自動更新到的每幀數據都和最近的submap對應，并放在最佳地址上。一旦不再有自動更新的當前scan或者自動更新到最近的當前 submap時，就封存了這個 submap，然后再去建立最新的submap。第二點是加入了回環測試與優化。使用最近 submap和當前 scan開始回環測試，一旦當前 scan和最近開始建立的 submap在長度上已有一定接近，則開始回環測試。在監測到的回環過后使用 ceres加以調節，并改變submap間的相對位置。為完成回環分析，通常使用分支定界法。WOLFGANG H等[13]提出并實驗驗證了一個二維SLAM系統，該系統將子圖像掃描匹配、閉合回路監測與圖形優化相結合。使用基于網格的局部SLAM方法創建單獨的子地圖軌跡。

2.5 LOAM

要在一個算法當中實現同時定位和建圖，這個難度是比較大的。LOAM 是一種面向多線激光雷達的改進SLAM技術。由于激光雷達獲得的點是在不同時刻，再加上雷達一直處在運動的狀態中，這會使得點云圖發生失真。因此，在 LOAM上將同時定位與建圖可以區分為兩種算法，第一種算法追求更高頻段但保真度相對較低，因此，可用來預測激光雷達的速度。而另一個算法則減少頻率，從而增加了精度，用于點云的精細配準[14]。LOAM的提出在一定程度上解決了點云的運動估計和建圖，當然LOAM也有需要改進的地方，比如沒有開發閉合回路修復運動估計中的漂移量，這一問題在后面的LeGO-LOAM得到了解決。

2.6 LeGO-LOAM

LeGO-LOAM相比如LOAM，其主要的提升在于輕量化和地面優化[15]。LeGO-LOAM 主要包括四個方面，分別是地面分割、特征點提取、雷達里程計和雷達建圖。地面分割主要是將非地面點和地面點進行區分，處理大量的散亂點，使得整個點云圖變得干凈。特征點提取中使用了和LOAM中基本相同的方法，區分邊緣點和平面點。雷達里程計模塊，對六自由度模型進行了兩次列文伯格-馬夸特法（Levenberg Marquardt, LM）算法優化求解。在雷達建圖中，要得到相應的全局地圖特征點云，將使用基于傳感器視域和基于圖像優化的方式。在地圖優化的方法中，通常采用回環檢查的方式，并加入了 gtsam優化，以得出最后的全局結果地圖[15]。

2.7 LOAM_livox

這是大疆在2019年發表的基于小FOV Lidar的LOAM算法。該算法繼承了標準LOAM算法的基本結構和技術，但有幾個關鍵的新貢獻[16]：1）特征點篩選方法的主要進步。省略了視角邊界上有明顯畸變的地方；去除反射強度偏大或過小的點；去除輻射角度與所在平面偏差或過小的點；2）迭代姿勢優化；3）算法可以并行運行。該算法雖然在建圖和定位方面實現了高水平的精度，但順序掃描匹配本質上是漂移的。因此，未來的相關優化可能會集中在環路閉合和滑動窗口。LIN J R等[17]提出了一種環路閉合方法用于校正激光雷達里程計和建圖中的長期漂移。提出的方法計算了關鍵幀的二維直方圖，并利用對二維直方圖的歸一化互相關作為當前關鍵幀與貼紙中關鍵幀間的相似性度。經過測試，這一環路閉合可以顯著降低長期漂移誤差。

2.8 Lio-mapping

使用IMU預積分和LOAM融合進行雷達里程測量和建圖的方法[18]。它包括里程計的狀態優化和帶旋轉約束的細化。此種方法不同于僅使用激光雷達，也優于松耦合方法。緊耦合激光雷達IMU融合方法顯示了具有快速更新率的魯棒姿態估計結果。

3 點云分割算法

點云分割法是指通過空間，幾何和紋理等特性對點云進行合理分割，從而使一個合理劃分內的點云都具有相同的特性，點云的合理劃分通常是多種應用的前提[19]，例如：物體識別和分類，地圖構建等。而地面分割往往是一些三維點云處理算法的預先步驟，地面點的數量往往是比較多的，因此在自動駕駛車輛中，正確的區分地面點和非地面點就顯得尤為重要[19]。地面分割的算法主要分為六類[20]。

3.1 基于高程的方法

在點云的統計中，道路點云相對于其他地方的點云存在著幾個明顯的特點：1）道路點屬于低地方點，且高度遠小于附近的任何地物，如建筑、植物等。使用該特性提供的點云濾波，可區別出路面點和其余非地面點；2）道路較周邊的任何地物更平緩，并統計各點的方法向量，通過設置適當的坡度閾值在地面點中抽取出候選道路點；3）在幾何形狀上道路呈長條帶狀分布且彼此連通。在這種方式中，三維點云通常被投射在二維網格上，以減小網格中各個單元所包含的級別變量特征值，而其中特征值通常是根據網格內的測量數值統計而得到的，如：最大平均寬度、最大平均高度和寬度方差，以及最大平均相對高度和高度方差[21-22]。所以，分割通常是利用比較特征值和設定的閾值來實現的。基于高程的分割方法存在的一個不足之處在于，它不能很好地區分橋梁和帶有坡度的地面，容易將這兩種識別為非地面點。基于高程的方法其步驟如圖1所示。

圖1 基于高程的分割算法

3.2 基于地面建模的方法

三維點云的處理在許多時候都是作為一種整體或沿縱向的部分加以處理，但在文中[22]提供了一個對地面建模的分割方式。這一方式并不適用前述的處理方式，而只是通過將點云按角度分割，然后再獨立地將二維線擬合在這些部分。而區分地面點與非地面點，主要是通過比較段內從各節點到線路的記錄。當然這一方法也難免會遇到地面分割的最大難題，那就是帶坡度的地面。論文中[22]給出的方案，使用了高斯過程（Gaussian Process, GP）等概率方式來得到更有效的擬合，從而處理帶有坡度的地面。不過，正因為以這種方式單獨處理每個部分，所以并無法保持分割結果沿角度走向的一致連續性。王肖等[23]針對于智能車輛環境感知實時性要求，研究一種基于3D全景式激光雷達的點云快速分割方法。采用帶地面狀態變化判斷的分塊直線擬合算法來去除背景地面數據，生成前景點云圖像并利用機器視覺算法進行聚類分割。

3.3 基于馬爾科夫的方法

道路現場劃分，在高速公路環境條件認識中是個十分關鍵的工作環節。關于路面場景分割方式對有所不同的場合下或許會形成誤劃分的提問，文章[24]使用基于粒子群優化算法（Particle Swarm Optimization, PSO）的改良馬爾科夫隨機場方式對路面情景實現了切割，先利用 Gabor對路面情景做出預處理，接著再利用改良馬爾科夫隨機場方式中的勢函數統計圖像能量，最后一步再利用PSO方式對圖像處理的能力實現了優化，從而得出路面場景分割結論。

3.4 基于相鄰點關系的方法

點云分割，形象地可以解釋為找出與自己不相同的點。所以一種很自然的思路就是鄰近搜索的分割方式。通過對比某一個點與其相鄰點的屬性，從而進行點云的劃分。在相鄰點關系的劃分方式中，如何確定鄰近點是一項非常重要的工作。在一些掃描的點云中，鄰近點是確認的，可是對于有些雜亂無章或者經過某些濾波后的點，怎么樣確認其臨近點就顯得尤為重要。這種確認可以通過“樹”來完成，目前主流的樹有 KDtree和Octree。確認了臨近點之后，下一工作就是計算目標點與臨近點的關系，這種關系可以使用歐幾里得算法和區域生長算法。歐幾里得計算通常采用了臨近一個點之間的間距為判別準則，而區域生長計算則使用了法線、曲率、顏色等信息，來綜合地判別點云是否應該被聚成一類。基于鄰近點的分類方法，其運算速度快，能更好地適用于實時處理。此類方法的不足之處在于其不能很好地處理低矮點，會有很大的分割誤差。另外本方法的閾值需要人為進行反復調試，故一般用于點云的粗分割。

3.5 基于深度學習的方法

CHARLES等提出了一個新的直接消耗節點云的神經網絡，這一新技術較好地尊重了輸入節點的置換不變性。網名被命名為PointNet，并為從大數據分類、零件分離和場景語義計算中的應用創造了統一的體系結構。PointNet方法非常有效，并且達到了令人印象深刻的結果，不過采用深度訓練的方式還是有著使用了大量的標記方法進行訓練的缺陷。而且，測試這種方法也相當麻煩。

3.6 基于平面擬合的方法

平面擬合法是指使用原始的幾何形體模式（平面、圓柱體、圓柱體）作為先驗知識進行空間劃分，把帶有相似表達式的點云數據都歸于同一個區域中，這也被稱之為基于模型的空間分割計算。隨機抽樣一致性方法（RANSAC）[25]，它能夠在一個具有“局外點”特征的觀測數據中，通過采用迭代方法預測數學模型的參數。假設大地是平直的，因此，可以用平面來描述大地。平面模型的基本數學公式是ax+by+cz+d=0[26]，可以通過不停地變化四個函數，找到在這個模型中一定階段內可以擬合更多的點的函數。在找到了這組函數之后，這個可以被模型擬合的點就是在平面的最后一個點。平面被擬合后，接下來就是通過激光雷達的距離來分出地面點與非地面點。在自主汽車所遇到的實際情景中，由于地球表面并不永遠都是平坦的，所以點云并非直接地將一個平面擬合到三維點云，而是首先沿汽車行進方向劃定范圍。接著從距離汽車最近的部門出發，在各個部門中，通過四分位距（Inter Quartile Range,IQR）的方法篩選出大部分離群點，之后再用剩余點估計帶有RANSAC的平面。當通過檢測高程與方向差來確定連續平面的穩定性以后，可以利用把截面中各個節點與結果平面間的長度與給定閾值加以對比來實現分割[22]。平面擬合的分割方式簡單，以幾何方法為基本，計算簡單處理快，對噪聲和奇異點都不敏感性。限制是指無法處理大量或復雜情景下的數據。

4 結論

高效的應用算法，對汽車激光雷達充分實現探測性能至關重要，所以汽車業界中更多的人都相信智能駕駛車輛，最終將變成一種“軟件產品”。

為了有效準確了解周邊環境，追蹤和辨識障礙物、實現智能駕駛車輛定位系統和行車路線規劃，保障其安全、有效地運行，車載激光雷達的信息處理將符合實時、穩定、精確的特點。

目前車內激光雷達應用算法都存在不同程度的局限：第一，算法安全性與實時性互相抑制，兩者無法共同實現；其次，算法多是面向某一特殊場合而開發，因此，無法提高可移植性和穩定性。

場景的復雜性與多樣化導致算法的發展異彩紛呈，并出現了全方位、多角度的多元融合趨勢。

各種計算方式的合理選擇應用將滿足各種情景下的智慧行車問題，同時點云分割算法是實現目標追蹤與識別的重要基石，通過目標追蹤與識別將完成對車輛及周邊障礙物運動狀況與幾何特性的綜合評估，而SLAM則將完成對車輛的精確定位和可行駛路線規劃。