采用傳感器融合的車輛前方碰撞預警系統

胡遠志，董泰宏，羅 毅，蒲 浩

(重慶理工大學 汽車零部件先進制造技術教育部重點實驗室，重慶 400054)

0 引言

自動駕駛汽車已經成為當前社會汽車行業發展的主流方向，汽車對主動安全系統的性能要求也因此提升。前碰撞預警系統FCW旨在提前對碰撞做出預警，避免或減輕由于追尾碰撞引起的傷害。設計良好的FCW需要對TTC特征參數進行深入研究。當前比較常見的基于雷達的車間TTC估計算法有一定的控制效果，但無法準確地識別出障礙物類別，同時對于一些邊緣的靜態障礙物會引發虛假警報，容易產生安全隱患。而基于單目視覺的車間TTC估計雖然能夠準確地識別障礙物的類型，但是由于其自身的特點無法準確計算出TTC，容易造成車輛前方碰撞預警系統準確性、及時性差甚至失效的問題。

近年來，攝像頭與激光雷達融合的技術在車輛領域逐漸發展起來[1]，薛培林等[2]提出一種激光雷達與相機特征級融合的城市自主車輛實時目標識別方法，融合識別系統在準確率、實時性方面達到實際行駛要求；胡遠志等[3]提出一種基于4線激光雷達與攝像頭融合的聯合測距的方法，改善了單目視覺目標識別有著巨大優勢，但在目標測距方面存在精度不足且測量過程不穩定的問題；HU等[4]在越野復雜環境中,分析了傳感器融合的優勢,闡述了單一傳感器在范圍、信號特征和檢測工作條件方面存在的局限性；劉鍇[5]使用多激光雷達與相機改善了場景識別的正確率；麥新晨等[6]針對城市道路環境提出了一種基于激光雷達與視覺的車輛檢測與跟蹤方法。受上述案例啟發，本文針對視覺傳感器檢測車間TTC精度低、穩定性差的問題，設計了一種基于激光雷達與攝像頭融合的車間TTC估計算法，結合2種傳感器的優勢，提升了TTC估計的穩定性、降低了估算誤差。

1 車道前方車輛目標檢測

基于激光雷達與單目視覺結合的前方車輛識別方法主要有2個步驟：系統工作前預先對激光雷達和攝像頭進行聯合標定，確定雷達坐標系和攝像頭坐標系間的變換關系[7]；經過視覺識別車輛目標后，通過激光雷達點云聚類確定前方車道的障礙物點云，篩除不在區域內的車輛目標，得到最終位于車道前方的車輛信息。

1.1 傳感器數據配準與同步方法

傳感器數據需要從空間以及時間上配準，空間的配準通過傳感器之間的標定完成，時間配準通過對時間戳的匹配以及幀率同步完成。

空間上激光雷達和攝像機的標定數據包括相機內部參數和坐標系間變換外部參數。相機內部參數主要包括相機焦距、像素中心點、畸變系數等。外部參數包括相機坐標系相對于雷達坐標系的旋轉和平移。

設點在激光雷達雷達坐標系O1中的坐標為(X1,Y1,Z1)，對應在相機坐標系OC中的坐標為P(Xc,Yc,Zc)。

激光雷達點云到相機坐標系的轉換為剛體變換，可通過旋轉與平移表征，公式[8]如下：

(1)

式中：[R|T]為旋轉平移矩陣，表示激光雷達坐標系通過空間旋轉與平移轉換到相機坐標系，構成外參矩陣。

從相機坐標系到圖像坐標系的轉換，是從三維坐標系轉換為二維坐標系的過程，屬于透視投影關系滿足三角形的相似定理：

(2)

從圖像到像素的轉換主要涉及伸縮變換和平移變換：

(3)

綜合式(1)(2)(3),可得到激光雷達坐標系往像素坐標系的轉換公式:

(4)

式中:f為相機焦距，fx、fy分別為相機橫、縱焦距，(u0,v0)為光學中心，這4個參數屬于相機內參，構成內參矩陣,表征相機坐標到像素坐標的變換，內參矩陣可由張正友標定法得到[9]，(x,y)為對應點在圖像坐標系的坐標,(u,v)為點在像素坐標系的坐標。

不同傳感器的采樣頻率不同，每個傳感器會按照自己固定的時間基準進行數據采集，從而導致兩者采集的數據來自于不同的時刻，使數據存在時間戳上的不同步，激光雷達的采樣頻率為10幀/s，相機的采樣頻率為30幀/s，無法提升激光雷達的采樣頻率達到與相機一致，因此，以采樣頻率低的激光雷達為準，對相機數據進行采樣，每3幀數據保留1幀，實現相機的采樣頻率為10幀/s，以達到時間戳匹配，實現時間上的數據匹配。

1.2 點云聚類檢測障礙物

通過激光雷達直接獲取的點云數據分布不均勻且量大不便于處理。點云數據是激光反射形成的點云，能夠直接反映目標的外表面信息，但在處理數據中，更多的是處理幾何拓撲信息。因此，建立離散點間的拓撲關系是點云處理的首要步驟。目前廣泛應用在點云數據處理中的是建立空間索引方法，常見空間索引一般是自頂向下逐級的劃分空間。代表性的k-d樹最常應用于點云處理。

1.2.1k-d 樹創建

k-d樹是計算機科學中使用的一種數據結構，用來組織表示k維空間中的點集合，它是一種帶有約束條件的二分查找樹[10]。建立k-d樹最高效的方法是，像快速分類一樣使用分割法，把指定維度的值放在根上，在該維度上較小數值的放在左子樹，較大數值的放在右子樹，然后分別在左右子樹上重復這個過程[11]。

由于只需要考慮當前車道前方的障礙物，為了節省計算資源，可以先過濾掉激光雷達左右2 m外以及后方的多余點云，僅保留當前車道前方的點云。

1.2.2歐式聚類

歐式聚類算法是一種基于歐式距離度量的聚類算法。對于三維點云，兩點之間的歐式距離計算公式為：

(5)

歐式聚類算法的流程：

1)對輸入的點云創建其k-d樹數據結構；

2)對任意一個點Q，遍歷其余點與Q的歐式距離E，將E小于設定閾值的點均放入聚類集合C中；

3)針對每一個還沒有被計算的點循環上述操作，直至所有點都被處理。

最終產生的聚類集合C則為點云聚類所檢測到的障礙物。通過幾何關系篩選出障礙物點云的尾部點云，并通過障礙物尾部點云計算兩車之間的相對距離。

1.3 車道前方車輛識別

2012年，Krizhevsk[12]提出了基于卷積神經網絡的圖像分類。此后大量關于圖像分類、目標檢測的網絡模型都是基于該基礎模型不斷演化產生。目前主流的有Faster R-CNN[13]、SSD[14]、YOLO等。

本文采用YOLOv3作為車輛的檢測算法，使用經過COCO數據集訓練后的權重文件。YOLO全名You Only Look Once[15]，是一種快速和準確的實時對象檢測算法。YOLOv3算法的主干網絡為Darknet-53，使用殘差網絡進行特征提取，使其在加深網絡的情況下又解決了梯度消失的問題，并能輸出3個不同尺度的特征圖，用于檢測不同大小的物體。相比于它的前兩代版本，YOLOv3彌補了小目標檢測精度差的短板，同時保留了速度的優勢。

在目標檢測算法檢測出圖像中的所有車輛之后，將激光雷達聚類檢測出的前方障礙物點云投影到圖像中，僅保留含有一定點云閾值的目標檢測框，以此作為最終的車道前方車輛識別結果。

由YOLOv3算法識別圖像中所有車輛效果如圖2所示，它包括了除車道以外的所有車輛檢測結果。加入了激光雷達篩選系統后的最終檢測效果(圖3)，僅保留了當前車道前方的目標車輛識別框，舍棄了道路邊緣以及旁邊車道的無關車輛的識別框。

圖1 傳感器位置示意圖

圖2 傳感器坐標變化流程框圖

圖3 車道前方車輛識別系統框圖

圖4 篩選前的車道前方車輛識別場景圖

圖5 篩選后的車道前方車輛識別場景圖

2 車輛目標跟蹤

車輛目標可以用一組恒定不變的特征集合來表達。采用基于目標特征的跟蹤方法：在連續幀圖像中提取車輛的特征信息(車尾部特征)，將車輛的特征以旋轉不變性的方法進行描述，在下一幀中通過特征匹配進行車輛的跟蹤。

2.1 特征提取

車輛目標的變化比較穩定，各個尺度的特征突變小，因此采用FAST提取特征[16]。FAST特征提取算法提取速度快、實時性強，算法步驟如下：

1)以像素p為中心，半徑為3的圓上，有16個像素點(p1,p2,p3,…,p16)。

2)定義一個閾值，計算p1、p5、p9、p13與中心p的像素差，若都超過閾值則中心點p為候選特征點。

3)計算候選特征點與(p1,p2,p3,…,p16)的像素差，若至少有連續9個超過閾值，則判斷其為特征點。

4)對特征點進行非極大值抑制，選取以特征點p為中心的55鄰域，若鄰域內有多個特征點，則判斷每個特征點各自的像素差總和s，保留s最大的特征點[17]。

(6)

使用FAST特征提取算法檢測的效果如圖6。

圖6 圖像特征提取效果圖

2.2 特征描述與匹配

使用BRISK特征描述對FAST提取的特征點賦予其描述符[18]，僅需要對車道前方車輛檢測框內的特征點進行描述。BRISK特征描述采用自定義的鄰域采樣模式，以特征點pi為中心的40×40像素塊內構建多個同心圓，采樣點等間距分布在圓周上，共60個采樣點[19]。

圖7 BRISK鄰域采樣模式示意圖

采樣完成后依據歐式距離劃分采樣點對集Ω為短距采樣點對集S和長距采樣點對集P。

S={(pi,pj)∈Ω∣pj-pi<δmax}?Ω

(7)

P={(pi,pj)∈Ω∣pj-pi>δmin}?Ω

(8)

閾值δmax=9.75t，δmin=13.67t。

使用長距采樣點對集P來計算特征點的主方向，然后將采樣區域旋轉到主方向，最后在短距采樣點集S中逐一比對與采樣點的像素強度，形成二值化特征描述。

對BRISK描述子得到的二值化特征描述，采用蠻力算法進行前后兩幀之間的特征搜索匹配，若前后兩幀之間的特征匹配大于一定的程度，就認為是同一目標，以此實現車輛目標的跟蹤。

3 傳感器融合計算TTC

TTC(time to collision)是指前后兩車某時刻保持狀態不變時兩車相互碰撞所需的時間。針對乘用車FCW系統，TTC的預警值一般設置為2.7 s，當提前2.5 s給予一個車輛碰撞警告，根據人的反應時間和剎車的距離，可以做到車輛剎停下來。

3.1 基于單目視覺的TTC計算

在前車突然停止或主車減速以避免碰撞時，TTC的計算避免不了兩車之間相對加速度的影響，本文采用一種常值加速度假設的TTC計算方法[20]。根據前車尺寸在圖像中的變化率可以直接估算TTC。

圖8 單目攝像頭幾何投影原理示意圖

設dw為圖像中車寬相對變化率，w為圖像中車輛寬度，有：

(9)

根據相似三角形原理，焦點與車輛尾部W構成的三角形相似于相機成像的三角形，則焦距f、成像寬度w、車輛寬度W、車距d的關系如下：

(10)

結合式(9)(10)可得到車寬相對變化率dw與車距之間的關系：

(11)

則車寬相對于時間的變化率如下：

(12)

考慮到相對加速度，兩車之間的相對距離模型為(v1為第一幀與第二幀之間的兩車相對速度)：

(13)

將相對距離模型式(13)代入式(12):

(14)

由于是常值加速度模型，代入第二幀以及第三幀的數據可得到加速度的表達式：

(15)

在式(14)中，相機的幀率是10幀/s，則dt=0.1，d2t=0.01，當作分子可忽略，于是：

(16)

TTC是d=0時的時間：

(17)

(18)

約去(18)等式兩端的d2：

(19)

根據式(19)，TTC僅與圖像中車寬相對變化率dw存在關系，而dw可直接通過兩幀圖像之間的變化得到，遂可解方程得到TTC。

3.2 基于激光雷達的TTC計算

激光雷達測速可采用幀距離差法，連續兩幀數據均可得到與目標的距離，每幀數據的時間間隔固定不變，即通過兩幀之間的距離變化與時間的對應關系可得到車間相對速度的大小與方向。同樣考慮到相對加速度，采用常值加速度假設的TTC計算方法。

圖9 激光雷達測距原理示意圖

設v1為第一幀到第二幀的相對速度，v2為第二幀到第三幀的相對速度，有：

(20)

(21)

根據三幀之間的數據可求得相對加速度：

(22)

代入常值加速度模型的TTC計算公式：

(23)

式中：第一幀時兩車距離d0、第二幀時兩車距離d1、第三幀時兩車距離d2可以由點云直接得到，即求出每一幀兩車之間的距離后，就可解方程求得TTC。

3.3 卡爾曼濾波

基于單個傳感器設計的TTC估計受限于傳感器本身的特點，結果的異常值較多，魯棒性差，采用卡爾曼濾波從結果級融合傳感器。

卡爾曼濾波器是一種最優化遞歸處理數據的算法，當系統不存在完美的數學模型、系統的撓動不可控很難建模并且傳感器測量存在誤差時，卡爾曼濾波算法能很好地根據預測數據以及觀測數據得到系統最優的狀態估計[21-22]。傳感器融合計算TTC剛好具備這幾點特性。

(24)

式中:xk為預測值，zk為觀測值，wk-1為過程噪音，vk為測量噪音，兩者近似符合高斯分布，A為傳遞矩陣，H為觀測矩陣。預測部分由先驗估計、先驗誤差協方差矩陣組成，校正部分有卡爾曼增益計算、后驗估計、后驗誤差協方差矩陣更新。

先驗估計值：

(25)

先驗誤差協方差矩陣：

(26)

卡爾曼增益計算：

(27)

后驗估計值：

(28)

后驗誤差協方差矩陣更新：

(29)

卡爾曼濾波算法的核心是運用遞歸算法達到最優狀態估計：利用先驗估計值和現時刻的觀測值更新當前狀態變量的后驗估計。在TTC計算中，激光雷達由于傳感器本身的優勢，與攝像頭相比表現得更加穩定，也更加接近真實值，遂選用激光雷達完成預測步驟，得到先驗估計值，攝像頭則作為觀測傳感器，提供觀測值完成狀態變量后驗估計的更新。

4 驗證與分析

為了驗證本文提出的基于傳感器融合的車間TTC估計算法，通過KITTI數據集中一段勻減速工況進行驗證。圖10中兩車距離初始值為11.3 m，目標車輛速度為0 m/s，本車初始速度為2.7 m/s，制動減速度大小為0.34 m/s2。

圖10 TTC計算實景

在逐漸勻減速靠近前方車輛時，兩車距離變化如圖11。圖12為傳感器融合計算的TTC曲線與單個傳感器計算的TTC曲線相對于實際TTC曲線的關系。

圖11 本車與目標的距離變化曲線

從圖12可以看到，基于單目視覺的TTC估計算法出現異常變化值的概率較高，是由于單目視覺TTC估計是基于兩幀圖像間車輛尺寸的像素變化率求得，而圖像中的車輛像素區域是車輛識別算法分別對每幀圖像數據處理得到的，在每次的計算中不可避免的會產生像素區域的突變問題，造成TTC估計值突變；同時除去異常值，其剩余穩定值整體偏離實際值幅度也較大，是因為攝像頭本身的測距原理是利用相似三角形的特性，受限于攝像頭本身的參數精度問題，單目視覺TTC估計值的精確度較差。

圖12 TTC計算值曲線

與單目視覺相比基于單激光雷達的TTC估計曲線比較穩定，整體偏離實際曲線較小，因為激光雷達的測距精度高，其采集的點云數據穩定，而激光雷達的TTC估計是基于兩幀之間目標距離的變化率求得，因此激光雷達的TTC值誤差小，異常變化值出現概率低。

本文提出的基于傳感器融合的TTC估計算法以激光雷達數據作為預測值，單目視覺數據為觀測值，融合得到的TTC估計值整體準確度表現更加優良，穩定的收斂于實際曲線附近，表明該算法有效提升了TTC估計的穩定性，降低了總體估計誤差。

5 結論

基于自動駕駛汽車常見搭載運用的激光雷達、攝像頭2種傳感器，介紹了激光雷達與攝像頭基于卡爾曼濾波融合測量車間TTC的計算方法，在勻減速工況中進行驗證，證明其能夠改善TTC估計的穩定性與準確性，為FCW系統的參數優化提供了依據。

該方法的不足之處在于：單個傳感器TTC計算的模型為常值加速度假設模型，在勻速行駛以及勻加速度行駛工況下表現良好，但在日常駕駛場景中會出現兩車的相對加速度不斷變化，此時算法的精度將受到影響。針對此問題，提出一種解決思路：加入預測模型估計未來狀態，以提升模型在兩車的相對加速度在不斷變化的實際工況下的適應性。