轉向避撞工況下裝載激光雷達車輛的障礙物跟蹤

趙治國，王 鵬，陳曉蓉，梁凱沖

（同濟大學汽車學院，上海201804）

前言

隨著人工智能和5G技術的不斷發展，自動駕駛將是車輛未來發展的主要趨勢［1-2］。自動駕駛車輛集環境感知、決策規劃和運動控制等多功能于一體［3］，其中環境感知是實現自動駕駛的關鍵功能，道路邊界和障礙物等信息的感知為自動駕駛車輛提供可行駛區域，并幫助自動駕駛車輛躲避道路行人和車輛［4］。激光雷達憑借測距精度高、受環境影響小、實時性好等優點，在環境感知領域中受到廣泛應用［5］。

自動駕駛車輛感知到前方道路的障礙物信息后，需進行轉向避撞。由于車載激光雷達與車輛之間為剛性連接，其坐標系將隨車輛轉向而旋轉，導致相鄰時刻檢測的同一障礙物位置相距過大，增加了障礙物信息關聯、運動狀態分析和后續避撞路徑規劃的難度。因此，實時獲取車輛行駛的航向角，將有利于對障礙物的跟蹤。

障礙物的檢測和跟蹤是車輛感知的重要環節，為提高檢測和跟蹤的效果，國內外做了大量研究。Maclachlan等［6］采用距離特征對動態車輛相關數據進行關聯，并通過卡爾曼濾波對車輛運動狀態進行估計。鄒斌等［7］采用可變閾值的密度聚類方法對障礙物點云聚類，提高了障礙物檢測的準確性，并通過多假設跟蹤模型和卡爾曼濾波實現了對動態障礙物的跟蹤。Leonard等［8］對檢測的障礙物用最近鄰方法對前后幀的數據關聯，然后對多幀速度進行濾波，實現障礙物的跟蹤。王濤等［9］利用加權多特征數據關聯算法集合卡爾曼濾波器實現了對動態障礙物的跟蹤。上述方法未考慮車輛轉向對障礙物跟蹤的影響，當雷達坐標系隨車輛轉向而旋轉時，前后幀同一障礙物的坐標發生大幅變化，易造成漏跟和誤跟。

為實時獲取車輛行駛的航向角，從而對雷達坐標系變換，需要精確提取道路邊界點云，在這方面已有不少研究。劉梓等［10］采用了基于線性鑒別分析分類思想的算法，有效檢測出道路邊界，該算法兼具可靠性和實時性，但沒有考慮到有障礙物的情況，抗干擾能力不足。朱學葵等［11］采用隨機抽樣一致性算法對道路邊界點云進行直線擬合，但該算法在有多條邊界線時存在閾值設定困難、迭代次數較多的缺點。Sun等［12］為準確提取不規則或被障礙物部分遮擋的道路邊界，利用車輛的預測軌跡搜索道路邊界候選點，有效提取出道路邊界，但該方法在車輛轉向避撞工況下并不適用。Li等［13］采用主、次窗口方法搜索道路邊界點，提高了邊界擬合精度，但搜索時沿著車輛行駛方向，在車輛轉向避撞時容易失效。

綜上所述，現有的道路邊界提取和障礙物跟蹤方法較少考慮到車輛轉向避撞工況，針對這一問題，本文中基于激光雷達坐標系變換，提出了一種車輛轉向避撞工況下道路邊界提取和目標障礙物跟蹤的方法。首先對原始點云進行地面點云分割和道路邊緣檢測等預處理，并采用改進的K-means聚類方法提取道路邊界；而后根據道路邊界利用麻雀搜索算法獲取車輛航向角，并對雷達坐標系進行變換；最后利用關聯算法和粒子濾波器，實現轉向避撞工況下目標障礙物的跟蹤。

1 道路邊界提取

道路邊界提取不僅是獲取車輛航向角的基礎，且可以縮小目標障礙物的搜索范圍，提高障礙物檢測和跟蹤的準確率［14-15］。

1.1 點云預處理

本文中使用的三維激光雷達為速騰聚創的RSLiDAR-32，該雷達水平角分辨率為0.2°，豎直方向角度范圍是-25°~+15°，共有32線激光束，工作頻率為10 Hz。每幀數據約有5萬個點，為提高處理效率，需對原始點云進行預處理。

點云的預處理主要包括地面點云分割和道路邊界檢測。地面點云約占總數量的30%，其中包含了道路邊界點。因此在地面點云中檢測道路邊界，既可提高檢測速度，又可避免諸如車輛、行人、樹木等障礙物的干擾。有研究采用平面擬合法，較好分割出地面點云［16］，并在地面點云中，根據邊界點的高度差值和距離差值，實現道路邊界的檢測［17］。

點云預處理結果如圖1所示。圖中紅色軸為X軸，綠色軸為Y軸，藍色軸為Z軸。紅色標記點為檢測的道路邊界候選點?？梢钥闯?，上述方法能夠從原始點云中分割出地面點云，并能有效檢測出道路的邊界點。

1.2 邊界候選點聚類

根據幾何結構特征提取的直行道路邊界點在理想情況下應近似分布于一條直線，然而由于實際路面存在凸起和凹陷、部分激光掃描線被車輛和行人遮擋等問題，提取的邊界候選點存在干擾和不連續部分，如圖1（c）所示。為解決上述問題，本文中提出了一種改進的K?means聚類方法，實現道路邊界點的聚類。

K?means聚類具有算法簡單、收斂速度快等優點［18］。它有兩個重要參數：聚類數目K，聚類中心cj。K?means算法將所有數據點分配至相似度最高的聚類中心，計算各類數據的均值作為新的聚類中心，不斷迭代直至聚類中心不變，從而實現數據點的聚類，其目標是使準則F最小：

式中：nj表示類j中數據點的總數；xi表示類j中的數據點。

K?means通常基于歐氏距離作相似度指標，即兩點之間距離越近，其相似度越高，其公式為

式中：（xo，yo）為聚類中心坐標；（xi，yi）為數據點坐標。

采用式（2）對邊界候選點進行聚類分析，僅能將相距較近的邊界點聚類，對分布于同一直線的邊界點的聚類效果不好。為解決這一問題，本文中使用點到直線的距離dl代替do，如式（4）所示，聚類中心則用各類中數據點擬合的直線（kl，bl）代替數據點坐標的平均值（xo，yo）。由于邊界點的分布與坐標系Y軸近乎平行，為避免擬合直線斜率kl過大，直線方程采用式（3）表示：

式中kl、bl分別為擬合直線的斜率和截距。

聚類中心采用最小二乘法擬合出匹配各類數據點分布的直線方程，使數據點到擬合直線的誤差平方和最小。

K?means算法對初始聚類中心非常敏感，易陷入局部最優，導致聚類穩定性不高。針對這一問題，初始聚類中心的確定步驟如下：

（1）基于式（5）對邊界候選點擬合，得到初始直線方程（k，b）；

（2）直線斜率k不變，代入邊界候選點坐標（xi，yi），求出最小截距bmin和最大截距bmax；

（3）根據聚類數目等分bmin和bmax，得到初始聚類中心（ki，bi）。

由圖1（c）可知，上節中提取的邊界候選點中大部分點為道路邊界點，存在少部分的路面和路沿上的干擾點，根據這一特點，對式（4）的距離公式引入修正系數，避免因初始聚類中心（ki，bi）選取不當，導致聚類陷入局部最優，而未能正確提取道路邊界點。修正公式為

式中：ni為聚類簇內的點數量；N為邊界候選點總數量。

聚類數目K初始設定為3，選擇聚類后包含點數量最多的類作為道路邊界，類中心（kl，bl）則為擬合的道路邊界線。當道路邊界提取效果較差時，將增加聚類數目K重新聚類。由于雷達采集頻率為10 Hz，在0.1 s的時間間隔內車輛航向角變化較小，道路邊界線的斜率k變化不會過大。因此選取邊界線相比前一幀的斜率變化Δk和邊界點到擬合邊界線的平均距離，即聚類的緊密性（compactness）CP指標作為評價標準。當擬合邊界線的Δk和CP值，任一超過設定的閾值，將增加聚類數目K對邊界候選點重新聚類。左右道路邊界的提取流程如圖2所示。

圖2 邊界提取流程圖

道路邊界候選點的聚類效果如圖3所示。其中圖3（a）為未修正的結果，圖中紅色點是對左、右邊界候選點分別聚類提取的結果，由于初始聚類中心的選取不當，部分邊界點（黑色點）被劃分至其他類中，造成左邊界提取錯誤；圖3（b）為修正后的結果，黑色點為聚類后的左邊界點，避免了局部最優，較好地將道路邊界點聚類為一類。

圖3 邊界點聚類結果

2 車輛航向角獲取

道路邊界點在XY平面內的投影與Y軸之間的夾角，即為當前車輛的航向角。為提高航向角求解的精確性和穩定性，采用麻雀搜索算法（sparrow search algorithm，SSA）進行求解。SSA是一種新型群體智能優化算法，種群通過不斷更新當前搜索位置，以達到全局最優解，相比于粒子群等其他智能優化算法具有搜索精度高、收斂速度快、穩定性好、魯棒性強等特點［19］。

2.1 麻雀搜索算法原理

相比于粒子群算法，SSA通過將麻雀種群分為發現者、加入者和偵察者，并根據個體的不同身份制定不同的位置更新規則，提高了收斂速度［19］。

假設搜索過程中共有n只麻雀，第i只麻雀的位置如下：

式中d為搜索維度。

發現者位置更新方式如下：

式中：itermax為最大迭代次數；t為當前迭代次數；α為（0，1］間的隨機數；Q為服從標準正態分布的隨機數；L為大小1×d、元素均為1的矩陣；R2∈［0，1］和ST∈［0.5，1］分別表示預警值和安全值。R2＜ST時，種群安全，發現者可廣泛搜索；否則種群發現捕食者，迅速向安全區移動。

加入者位置更新方式如下：

偵察者位置更新方式如下：

2.2 建立優化指標

采用SSA求解車輛航向角θ，因此種群搜索維度為1，由于激光雷達坐標系經過前一幀求得的航向角變換，Y軸近乎平行于道路延伸方向，所以初始化種群位置為0。隨機選取左右邊界各5個數據點，建立種群適應值函數為

式中：xi為道路邊界點的橫坐標；xˉ為道路邊界點橫坐標的平均值。

SSA通過個體位置所代表的含義，即車輛航向角，對提取的道路邊界點進行坐標變換，根據式（12）求出該個體的適應值，以適應值最小作為優化目標，求解出當前車輛航向角的最優解，具體流程如下：

（1）初始化種群位置，即車輛航向角θ=0°；

（2）基于個體位置所代表的航向角對道路邊界點坐標變換；

（3）根據式（9）~式（11）更新種群中發現者、加入者和偵察者的位置信息；

（4）根據式（12），更新種群適應值f；

（5）迭代次數達至最大或適應值f小于閾值，則輸出航向角θ，否則轉到步驟（2）。

根據SSA求解的車輛航向角θ，對地面點云坐標變換后的結果如圖4所示?？梢钥闯觯す饫走_坐標系經過變換后，其Y軸與道路延伸方向基本平行，消除了車輛轉向對障礙物跟蹤的影響，為下節的障礙物跟蹤奠定基礎。

圖4 地面點坐標變換

3 障礙物跟蹤

行駛車輛關注的目標障礙物多為道路區域的車輛和行人，車與車、車與行人之間的距離一般較大，之前的研究中已通過DBSCAN聚類，較好地實現了障礙物的檢測［20］。

為實現障礙物的跟蹤，需將前后幀檢測的障礙物信息關聯，常用的方法是聯合概率關聯算法（joint probabilistic data association，JPDA）。JPDA算法選取總概率最大的事件匹配，其關鍵在于計算當前時刻障礙物（觀測點）與前一時刻障礙物的預測位置（目標點）的聯合概率矩陣，即

式中：i為觀測點；j為目標點；ωij為觀測點與目標點的關聯概率。

關聯概率的計算通常采用歐式距離，即觀測點與目標點距離越近，則關聯概率越大。然而歐氏距離的使用忽略了車輛在道路區域的行駛特性，在道路區域內車輛幾乎為縱向行駛，很少出現橫向行駛，且橫向車速也相對較低。由于本文已基于車輛航向角對激光雷達坐標系變換，避免了前后幀檢測的障礙物位置因坐標系旋轉而大幅變化，因此可通過分別計算觀測點與目標點的橫、縱向距離，并增加橫向距離的權重，來提高關聯成功的概率，即

式中：ki為各項偏差的加權系數；Px和Py分別為觀測點與目標點間的橫向與縱向距離。

本文取k1=0.75，k2=0.25，增加橫向距離的權重，當車輛和行人等障礙物在前后幀的橫向距離越近，越有可能為同一障礙物。

由于實際中存在測量噪聲和丟幀的現象，直接對前后幀檢測的障礙物進行關聯，容易發生關聯錯誤。因此，采用粒子濾波器對目標進行估計和更新，提高下一時刻預測位置的準確性。

粒子濾波算法的基本思想是在先驗概率分布中隨機采樣獲得一組粒子，在跟蹤過程中根據獲得的量測值實時調整粒子的權重和位置，并利用粒子的狀態信息近似表示系統的后驗概率分布，由此估計非線性系統的狀態。粒子濾波算法如表1所示。

表1 粒子濾波器算法

根據坐標變換后的各目標相對本車的距離和角度信息，利用上述粒子濾波算法可獲得各目標的縱向位置、縱向車速、橫向位置和橫向車速等運動狀態。由于已對激光雷達坐標系變換，對目標障礙物下一時刻的位置預測將更加準確。

4 實車試驗

為驗證本文提出的車輛轉向避撞工況下道路邊界提取和目標障礙物的檢測跟蹤算法，開展了實車道路試驗。試驗車輛如圖5所示，車頂部中間安裝32線激光雷達以及GPS/IMU，其中GPS/IMU提供試驗車的航向角，采集頻率為100 Hz，以驗證本文基于SSA獲取航向角的準確性。如圖6所示，試驗場景選取了一條單向車道的校園道路，其中道路左邊界為灌木叢，右邊界為路沿。

圖5 改裝試驗車

圖6 試驗道路場景

試驗車在校園道路中先進行向左的轉向行駛，而后減速停車，之后再加速向右轉向，重新回到道路中心，模擬車輛的轉向避撞場景。在試驗的校園道路上共有4位試驗人員，隨機走動，模擬周圍的障礙物。試驗采集的激光雷達數據共有690幀，GPS/IMU數據共有7 000幀。

圖7 為激光雷達數據處理結果。由圖7（c）可知，基于改進的K?means方法有效去除了路沿上的干擾點；由圖7（d）可以看出，坐標系經過變換后，其Y軸與道路邊界平行。

圖7 點云處理結果

圖8 為改進的K?means聚類算法和SSA的求解結果。由圖8（a）可知，由SSA求解的車輛航向角與GPS/IMU測得的基本一致，在前15 s車輛航向角為負，進行向左的轉向行駛，在15~35 s車輛減速停車，航向角不變，在35 s后車輛航向角向正方向修正，回到道路中心。由圖8（b）和圖8（c）可知，由于道路左邊界為灌木叢，右邊界為路沿，左邊界聚類后的邊界斜率以及CP值相比于右邊界明顯更加穩定。本文設定的斜率變化閾值為0.02，CP閾值為0.5。結合圖8（d）可以看出，增加聚類數目后能夠確保所提取的左右邊界線斜率絕對值在0.02以內，CP值在0.3以內，有效提高了邊界提取的穩定性。由圖8（d）可知，左邊界提取更加穩定，聚類數目始終為3，右邊界偶爾需要增加聚類數目，最多為6。

圖8 改進的K?means和SSA結果

圖9 為改進的K?means聚類算法和文獻［11］中使用的Ransac算法結果對比。由圖可知，在干擾點數量較多時，改進的K?means算法相比于Ransac算法求解的相鄰幀邊界線斜率的變化更小，穩定性更好。

圖9 K-means與Ransac的對比結果

圖10 為PSO和SSA不同時刻的對比結果。本文設定的適應度閾值為0.000 3，由圖可知，SSA收斂速度更快，僅需迭代約200次，而PSO需迭代約900次，因此SSA的實時性和穩定性更好。

圖10 SSA與PSO的對比結果

圖11 為障礙物檢測和跟蹤算法的結果。目標1-4是試驗人員，目標5是中途經過的騎自行車人員。由圖11（a）和圖11（b）可知，粒子濾波器計算的目標物相對速度更加平滑，有效去除了噪聲，使預測點的位置更準確，具有良好的跟蹤性能。圖11（c）表明，提出的關聯算法能夠保證不同目標物的正確關聯。

圖11 粒子濾波器跟蹤結果

圖12 為激光雷達檢測的目標物信息。在第440幀時出現目標5。在第484幀時由于目標1的遮擋，丟失目標5。在第487幀時目標5重新出現。在目標1和5距離相近且目標5連續丟失兩幀時，提出的跟蹤算法仍能實現目標1和5的正確跟蹤。

圖12 目標關聯結果

在實時性上，以上激光雷達的點云處理程序在Intel i7處理器的計算機上運行。運行時間如圖13所示，每幀從點云數據輸入到各障礙物信息輸出，所提算法的計算時間一般為50~75 ms，平均運行時間約為61 ms，最大耗時86 ms，小于激光雷達采樣周期100 ms，能夠滿足實時要求。

圖13 運行時間

5 結論

針對車輛轉向避撞工況，提出了一種基于激光雷達的目標障礙物跟蹤方法。采用改進的K?means聚類方法提高了道路邊界擬合的準確性，并通過增加聚類數目保證了邊界線的擬合效果，使相鄰幀擬合的邊界線斜率變化小于0.02，邊界點至擬合邊界線的平均距離小于0.3，相比于Ransac算法更加穩定；利用SSA獲取的車輛航向角對激光雷達坐標系進行變換，避免了車輛轉向對道路邊界提取、障礙物關聯和運動狀態判斷的干擾；再通過JPDA關聯算法和粒子濾波器實現了有丟幀情況的障礙物跟蹤。實車試驗驗證了提出的算法在轉向避撞工況下具有良好的道路邊界提取效果和目標障礙物的跟蹤效果。

文中研究僅對結構化的直道進行了討論，后續將進一步研究算法在非結構化道路和彎道的檢測效果，提高算法的適用性。