基于LabVIEW 和單線激光雷達的障礙物識別算法研究

袁 春，曾 凡，李 昊，胡 萌，徐 哲

（1.重慶理工大學 汽車零部件先進制造技術教育部重點實驗室，重慶 400054；2.重慶理工大學 機械檢測技術與裝備教育部工程研究中心，重慶 400054）

激光雷達由于可以獲得極高的角度、距離分辨率和良好的抗干擾能力，被大量應用于車輛智能駕駛中。多線激光雷達能夠獲得障礙物豐富的點云信息，但價格昂貴，給智能車輛普及帶來較大的挑戰。單線激光雷達結構簡單，并且由于價格低廉，在自動駕駛領域開始有所涉及。對于雷達感知算法的整體架構，楊飛等［1］采用Ubuntu下的ROS機器人實時操作系統實現動態障礙物的檢測和追蹤。汪佩［2］采用嵌入式軟硬件系統，基于OpenCv、OpenCL及QT環境進行數據處理。于金霞等［3］采用Windows下的Visual C++編寫的障礙物識別算法。

LabVIEW 是近年來逐步應用的一款圖形化交互式編程軟件［4］，通過虛擬儀器模塊，將軟件和各種硬件工具連接起來。依靠LabVIEW 在數據采集、實時通信、圖形信息顯示方面的強大功能，以及在智能駕駛領域的逐步應用，將LabVIEW 運用于激光雷達環境感知中的障礙物檢測。有關障礙物的單線激光雷達檢測算法，湖南大學莊秀華依據雷達的點云數據掃描特點，采用中值濾波算法處理噪聲數據，使用最近鄰算法歸類障礙物的點云，最近匹配法和卡爾曼濾波進行跟蹤［5］。劉偉［6］選取車輛幾何特征作為目標識別的依據，選用支持向量機進行分類［7］，通過交叉驗證和網格搜索尋找最優參數以提高車輛的識別率。楊成提出一種近鄰域和層次聚類相結合的算法進行聚類，得到各個聚類的質心［8］。通過置信區間的關聯性辨識同一個障礙物的速度和航向。

本文提出一種基于LabVIEW 的單線激光雷達障礙物識別算法。一方面，依靠LabVIEW強大的數據通信和圖形顯示功能，識別算法具有良好的實時性和可顯示性。另一方面，針對DBSCAN［9］算法聚類的缺點，提出一種融合高斯模型，即對雷達掃描范圍分區域自主計算聚類算法的閾值參數的方法，提高對障礙物點云數據的檢測能力和識別算法的執行效率。算法完整流程參考圖1。

以北京佳光科技有限公司生產的L1NO單線離軸激光雷達作為主要傳感器，水平掃描角度為100°，最小測距誤差為2 cm，激光的掃描頻率為5～20 Hz，測距范圍為0.3～30m，采用以太網通訊。

圖1 障礙物識別算法流程框圖

1 障礙物檢測

1.1 雷達數據解析

將雷達報文信息進行解析，在LabVIEW 底層中讀取原雷達數據，雷達原始數據組成見表1。

表1 雷達UDP數據包組成

LabVIEW 解析過程見圖2。基于單線激光雷達的水平視場角和測距范圍，以及路面的實際情況，設定感興趣區域為雷達前方30 m的測量范圍。感興趣區域［10］的設定，一方面需考慮雷達的掃描范圍，另一方面是為了減小數據處理規模，提高算法的執行效率，去掉無關的點云數據。但感興趣區域的設定也不能過小，以防止路面前方車輛信息的丟失，影響車輛的行駛安全。

圖2 數據解析程序框圖

1.2 雷達點云的影響因素

1.2.1 雷達的反射率

激光束掃描到不同的障礙物表面，反射率［11-12］差別較大，表2是經過反射實驗采集的反射率數據。經過試驗，L1NO雷達對于白色車輛的反射率最好，對于黑色車輛，可能會出現車輛輪廓點云失真而導致障礙物信息丟失。

表2 不同物體的反射率

1.2.2 距離因素

雷達離地高度設定為0.7 m，真實環境中，隨著距離的增加，光路的衰減將極大地影響物體的反射率，從而影響測量的準確性。通過雷達實測，在0～30 m的障礙物，隨著障礙物逐漸遠離雷達，得到的雷達點云點數越來越少。對于30 m以外的障礙物，由于受到激光測距范圍的影響，點云數量稀少，不能表征出一個障礙物的外部輪廓，因此設定探測區域為雷達正前方30 m。

1.3 障礙物的聚類

針對障礙物形狀輪廓的提取，通常采用聚類算法進行處理。與其他聚類算法相比，DBSCAN算法對不同點云數量的簇以及不同形狀的簇，通過對形成一個完整障礙物點云簇的最少點的數量和點間距參數的設置，快速判定該樣本點所對應的外部實體的種類。該算法構成主要包括：

1）核心對象：假定存在某一個數據點為核心對象，則要求在以該點為球心、長度Eps為半徑的球面范圍內，存有最少Min pts個空間點。定義該點為（Eps，Min pts）的核心對象，存在以Eps鄰域為半徑的完整點云簇。

2）密度可達：存在一組數據點集｛Q1，Q2，…，Qn｝，對Qi＜D（0＜i＜n），Qi+1是從Qi出發關于（Eps，Min pts）直接密度可達，直接密度可達則表示對象鏈中的任一個元素Qi密度可達。對于不同大小的實體障礙物，雷達掃描得到的點數也不一樣，并且由于雷達點云的掃描特點，隨著距離的變化，同一個障礙物表面的反射面積也會變化，障礙物點云數量也隨之變化。閾值參數的算法如下：

式（1）表示閾值Eps為點云數據data和最小點數Min pts的函數，式（2）是求取點云data的數量，式（3）是求取距離閾值Eps的大小。參數m表示點簇的數量，n值為2，計算過程引入伽瑪（gamma）函數，k為調節系數。常見雷達掃描車輛外形輪廓的點云形狀分為“I”型、“L”型和其他型，如圖3所示。

參考圖4的高斯分布［13-14］曲線，可以把“I”型看作一個類似的高斯分布，“L”型看作是2個高斯分布的組合，高斯模型采用式（5）表示：r式（4）中模型p表示由多個正態函數g（x|μi，σi）疊加組成；π表示模型權值的向量；σ表示正態分布的方差值矩陣；μ表示統計模型中的期望值向量；i表示數據點所代表的統計正態分布模型的個數。

圖3 雷達掃描點云形狀

圖4 高斯分布曲線

雷達掃描障礙物的動態調節聚類［15］算法的重要參數點數閾值Min pts表達式見式（6），圖5為不同距離下的掃描示意圖。

圖5 不同掃描距離示意圖

式中：d為X方向上二維高斯分布模型點的坐標值（d1、d2、d3表示運動的障礙物不同時刻X方向不同的高斯坐標值），d取值為0＜d≤30，s為Y方向上二維高斯分布點的坐標值（即高斯分布的期望值），s取值為0＜s≤30，θ為相鄰掃描線的夾角，δ為激光雷達掃描寬×長為2 m×4 m車輛外部輪廓的點數經驗值，取值35，γ為調節因子。

2 障礙物跟蹤

障礙物的跟蹤首先需要建立不同時刻障礙物信息的數據關聯，以找到同一個障礙物在不同時間域的點云數據。數據關聯主要依靠前后幀的障礙物點云信息，即障礙物的外部輪廓長寬信息、速度信息、位置信息，通過時間上的連續性，判定是否屬于同一個物體。對于激光雷達傳感器來說，由于雷達掃描受到外部光線干擾、對深色物體掃描失真、路邊花叢樹葉反射等觀測噪聲，以及由于車身抖動造成雷達自身振動帶來的過程噪聲，單幀的檢測結果無法判別出運動對象的準確信息，必須依靠連續多幀的點云數據。同一個運動對象，只有被多次檢測并且能與路面中真實的車輛和行人匹配［16］，才能確定其運動狀態。

本文中的數據關聯算法，采用最近鄰算法的思想，依據距離最近的K個鄰居所歸屬的類來判別新數據集的類別屬性。在目標掃描跟蹤范圍內，若檢測到只有一個觀測點云數據點，則使用該觀測數據對目標進行更新。但是如果在掃描跟蹤范圍內存在多個觀測數據，計算觀測點與目標預測點的距離，并比較距離的大小，通過距離最小的觀測點來更新目標狀態。根據這種近鄰算法思想，對于可能屬于同一個障礙物的點云，聚類成一個塊后，經過一個掃描周期，相鄰2個時刻的障礙物外部矩形框輪廓，必然存在重疊區域，為了簡化這種描述，用矩形框的極坐標的夾角的重疊區域來表示，如圖6所示。

圖6 前后時刻障礙物的運動關聯示意圖

2.1 運動參數

如圖7所示，障礙物沿著直線l運動，由于雷達掃描∠KOH水平視場角為100°，在進入雷達邊界直線OK之前，雷達無法掃描。A點為雷達開始掃描點，受到雷達掃描距離30 m的限制（設定OB=30 m），因此AB段是有效掃描區域。隨著距離越來越遠，障礙物信息開始丟失，能部分顯示車輛輪廓，點云數據不穩定，BC段是掃描失真區域（經過實測，OC約為45 m）。C點之外，障礙物離開雷達視線。圖8是LabVIEW 實時采集的界面，考慮到車輛的外形輪廓，圖8（a）表示障礙物剛進入雷達掃描范圍，圖8（b）表示車身完整輪廓顯示，圖8（c）表示車輛即將離開掃描范圍。

圖7 雷達坐標系下掃描示意圖

圖8 LabVIEW 實時采集的界面

常用運動模型分為勻速運動、勻加速運動、變速運動和統計模型，由于模型的復雜多變難以得到定性的實驗結果，本文中試驗場景包含靜止的試驗車掃描運動的目標車和運動的試驗車掃描從左后方超車的目標車。

為了簡化模型，在數據關聯成功之后，提取當前時刻障礙物外接矩形框的幾何點，取其坐標作為該障礙物的位置信息。連續兩幀的同一個障礙物的位置除以掃描時間，可以直接求出速度。

式中：xt、yt為t時刻的X、Y方向的坐標值；xt+1、yt+1為（t+1）時刻的X、Y方向的坐標值；T為掃描周期。

2.2 軌跡跟蹤

在實際情況下，很難得到運動對象的真實狀態值。雷達在探測目標時，根據反射波能夠得到目標的距離，但雷達探測時存在周圍環境的不確定干擾，需要在包含噪聲數據的觀測信息數據集中，識別出車輛的狀態參數。因為很難獲取所需運動對象的真實狀態值，只能根據觀測信號估計或者預測這些狀態變量，工程實踐中，通常選取卡爾曼濾波器［17］作為降低噪聲干擾的處理方法。

雷達空間中運動物體的運動模型可以表示為：

式中：X（k）為4階的狀態參數矩陣，包含位移的X方向和Y方向參量，速度的X方向和Y方向參量，F是狀態矩陣，W（k）和Q分別表示過程噪聲矩陣和過程噪聲值。

系統的觀測方程：

式中：Z（k+1）為雷達觀測量；H 為觀測矩陣；V（k）為觀測誤差矩陣。

目標信息的下一步預測：

一步預測協方差陣：

求濾波增益矩陣：

狀態更新：

協方差更新：

通常，狀態矩陣F表示為

觀測矩陣H：

過程噪聲Q：

觀測噪聲R：

協方差矩陣P：

式中：In為n階單位矩陣；P為協方差矩陣，表示誤差的偏差，與X（k）狀態矩陣相對應，同為4階矩陣；K為卡爾曼增益；X＾為卡爾曼估計量；Q為過程噪聲方差；R為觀測噪聲方差；dt為雷達掃描周為100 ms。通過雷達采集的大量離線數據實測，Q和R取值參照式（17）和式（18）。本文中，對上述公式進行了修正，在式（9）中，原公式中的觀測值Z（k）用雷達關聯后的障礙物的信息data（k）代替，式（9）實際情況下不應用于濾波程序。式（13）中的Z（k+1）用data（k+1））替換，即表明雷達的觀測數據，包含了雷達的觀測噪聲方差R，實時更新導入卡爾曼濾波程序中。經過多次迭代后，濾波器的增益矩陣和協方差矩陣不斷更新，以適應障礙物連續多幀的航跡［18］。

3 試驗驗證

3.1 試驗實車平臺

圖9為重慶理工大學機械工程中心搭建的第二代物流電動車平臺。該物流小車主要用于解決社區和校園道路最后1 km的快遞物件的配送問題。該實驗車的傳感器搭載方案包括L1NO單線激光雷達（圖10）、1套RGPark 982 GNSS衛星導航系統（圖11）、1臺具備4核2.7 GHz中央處理器和8 G運行內存的工控機。表3為RGPark 982 GNSS衛星導航系統的定位精度。

圖9 物流電動車平臺

圖10 L1NO單線激光雷達

圖11 RGPark 982 GNSS衛星導航系統

表3 RGPark 982 GNSS衛星導航系統定位精度

3.2 試驗設置

使用LabVIEW+DLL軟件平臺，動態鏈接庫DLL是一個多程序可以共享的數據模塊［19-20］，擴展名為dll，內部對代碼進行封裝。其與可執行文件（EXE）較為接近，區別在于DLL雖含有執行代碼卻不能單獨執行，必須用Windows應用程序調用。實驗道路選在靠近校園操場的一段路，如圖12所示。

圖12 實驗現場

L1NO雷達掃描時間100 ms，算法處理時間150 ms。圖13為雷達自帶軟件掃描原始場景和LabVIEW 軟件下的掃描結果。

圖13 雷達掃描障礙物

實驗場景1中，實驗車輛靜止，運動的目標車輛以12 km/h的平均時速左側駛過，單線激光雷達掃描并采集到60幀的車輛信息。實驗場景2，20 km/h平均時速的目標車輛對前方行駛的實驗車輛超車后右轉，實驗車輛上的雷達感知系統采集85幀的目標車輛信息。受制于單線激光雷達100°的水平視場角和雷達的實時數據刷新率，對于寬×長為2 m×4 m的車輛的檢測，目標車輛位于雷達前方5～30 m的識別效果最佳。

圖14是場景2實驗時算法捕捉雷達掃描目標相鄰幀的LabVIEW 圖形。

在場景2超車實驗中，籃球場前側的石墻被誤判為車輛，如圖12箭頭所指（1）處。隨著掃描幀數的增加，車輛的長寬外形尺寸上的穩定性以及速度的連續性，被數據關聯和濾波算法判定為其他的障礙物，因而取消了對不屬于同一個移動障礙物的跟蹤。此算法能實時獲取移動車輛的位置和速度，并且在LabVIEW 的VI程序前面板中動態顯示。

圖14 場景2實驗LabVIEW 實時圖形

3.3 實驗結果分析

卡爾曼濾波算法依據時間維度進行濾波，程序迭代是進行目標狀態的“預測—修正”。通過實驗結果可知，剛開始前幾幀濾波值跳動角度，濾波效果不理想。隨著迭代次數的增加，卡爾曼濾波算法根據連續多幀的觀測數據，不斷調整自身的過程噪聲誤差Q、觀測噪聲R、誤差協方差P。線性卡爾曼濾波能夠有效減少位移和速度的跳變，使運行運動狀態更加平穩。穩定跟蹤后的濾波值受到觀測狀態的影響，但是總體上與實際狀態相符。在雷達出現掃描失真時，通過對障礙物位置以及速度參數的滾動更新，提供最新的狀態信息給決策模塊，以適應復雜多變的道路環境。當連續5幀障礙物信息失真或者完全丟失，則終止該軌跡。圖15、16表示場景1的位移和速度濾波結果。

圖中散點表示雷達掃描的障礙物位移和速度信息，實線是濾波結果。通過分析，實驗車輛以12 km/h的平均速度行駛，雷達掃描采集的觀測數據波動較大，通過卡爾曼濾波，減小了環境噪聲點的干擾，當前時刻所預測的位移和速度信息更接近下一時刻的觀測信息。圖17、18表示場景2位移和速度濾波結果。

圖15 場景1位移濾波軌跡

圖16 場景1速度濾波曲線

圖17 場景2位移濾波軌跡

圖18 場景2速度濾波曲線

場景2中，速度波動較大，這是因為雷達在運動的試驗車上，受到自身支架輕微震動的影響，導致聚類點云位置波動。相比較場景1，場景2中的實時位移和速度波動較大，但通過卡爾曼算法對多個相鄰時刻障礙物參數信息的滾動優化，觀測值與預測值之間的誤差變小，濾波之后的位移和速度減小了干擾信息，保證輸出參數的平穩。通過在校園路段多次試驗，本算法對于車輛前方30 m的障礙物的聚類的準確率為99%，跟蹤的有效率為90%，少數未能準確匹配的幾幀參數，主要原因是單線激光雷達在室外的工作環境下，外部強光光線的干擾以及雷達對車輛特殊顏色（黑色）的反射率不高。

4 結論

對校園環境下低速行駛的車輛的檢測跟蹤問題進行研究，提出了在單線激光雷達點云數據上檢測移動障礙物算法。在Simulink中完成點云聚類，多幀點云信息的數據關聯，對同一個運動目標觀測出實時軌跡和速度。通過加載相關插件的編譯環境后，編譯成動態鏈接庫DLL，放入LabVIEW底層框架下予以調用。考慮到激光雷達的觀測噪聲和過程噪聲，對于出現障礙物信息丟失以及航跡跳動較大的情況，濾波程序能夠給出修正后的估計值，提供給決策系統，保證航跡的穩定和行車的安全。實驗表明，本算法能夠以150 ms的刷新頻率，對車輛前方30 m的運動的障礙物實時有效地識別和跟蹤，滿足低速行駛條件下物流電動車對道路環境感知的需求。

在此工作的基礎上，下一步研究的重點是提高識別的正確率和多場景應用：

1）采用多個單線激光雷達，提高水平掃描視場角，降低漏檢率；

2）考慮增加相機，通過相機這種視覺傳感器采集的深度信息，并融合其他傳感器采集的信息，強化對道路路面車輛信息的檢測能力；

3）增加對行人的掃描識別，提高對車輛前方障礙物的感知能力。