一種面向智能駕駛的毫米波雷達與激光雷達融合的魯棒感知算法

黨相衛 秦 斐 卜祥璽 梁興東*

①(中國科學院空天信息創新研究院微波成像技術國家級重點實驗室 北京 100190)

②(中國科學院大學 北京 100049)

1 引言

智能駕駛代表著現代汽車技術與產業發展的大趨勢，而環境感知則是汽車智能駕駛的關鍵核心技術，也是學術界長期研究的熱點領域[1]。感知技術主要通過大量車載傳感器為無人駕駛汽車加上了“眼睛”，用以精確識別周邊環境以保證駕駛的安全性與可靠性[2]。雷達傳感器、攝像頭等傳感器技術在智能駕駛中應用較多。

毫米波雷達由于其全天時全天候的特性，能夠穿云透霧，具有較強的環境適用性。同時，毫米波雷達還能夠通過多普勒效應，探測到目標的速度信息。和激光雷達相比，毫米波雷達的分辨率有限，一般可以得到目標的距離、角度、速度等信息，但在高度向上分辨力較差。由于電磁波的傳播特性和物體的非理想散射等，毫米波雷達數據往往受到雜波、噪聲和多徑的影響，雷達數據中存在著虛假目標等。這些都限制了其在智能駕駛感知中的應用。相機和激光雷達傳感器能夠獲取環境豐富的紋理信息及三維結構信息，但在煙霧、雨雪等情況下，感知能力大大下降甚至失效，如圖1所示。因此，基于多傳感器融合的魯棒的感知已成為智能駕駛的研究熱點[3]。

圖1 煙霧環境中相機和激光雷達數據Fig.1 Camera and LiDAR data in a smoky environment

Yamauchi[4]探索了使用超寬帶雷達、激光雷達和立體視覺傳感器實現無人機的全天候導航。Fritsche等人[5]提出了將雷達與激光雷達數據進行二值決策融合的方法，后來，該團隊提出了加權融合的方法[6]，以應對煙霧等惡劣環境。此外，他們還探索了在掃描級和地圖級進行兩者數據融合[7]的可行性。Pritsche等人[8]提出了將激光雷達和雷達傳感器的距離數據融合，并與熱信息融合，實現了對事故現場的危險檢測。Marck等人[9]將來自雷達的距離信息投射到集成在小型頭戴顯示器上的紅外圖像上，以指示消防員的安全空間。Park等人[10]提出了一種利用雷達系統在低能見度環境下進行感知的算法。文獻[11–14]提出利用毫米波雷達與攝像機的信息融合來檢測前方車輛。但是，上述融合方法由于毫米波雷達性能的限制，都是簡單的信息疊加，是一個松耦合，并沒有充分利用各個傳感器的優勢。

本文提出了一種融合毫米波雷達和激光雷達的魯棒的感知算法，以滿足智能駕駛在復雜多變的環境中魯棒感知的需求，保證汽車的安全。該方法充分發揮了兩種傳感器的特性，在數據層進行融合，既保留了激光雷達準確的三維感知能力，同時利用毫米波雷達穿云透霧的優勢，減少煙霧等惡劣環境對感知的影響。本文的主要創新工作如下：(1)提出了新穎的基于特征的兩步配準空間校正方法，實現了空間二維雷達與三維激光傳感器準確的空間同步；(2)改進了毫米波雷達濾波算法，減少了噪聲、多徑等對雷達點云的影響，保證了雷達點云的準確可靠；(3)根據傳感器的測量結果，提出了新的數據融合方法，保證感知的準確及魯棒性；(4)搭建了硬件系統，進行了實際場景測試，驗證了該文算法的有效性，為智能駕駛提供參考。

2 算法概述

本文需要解決的問題可以概括為：使用所搭載的毫米波雷達和激光雷達傳感器，獲取周圍環境信息，然后根據兩種傳感器的測量數據，進行合理的融合，得到最準確的環境表達，以此進行傳感器的運動估計，得到其在環境中的相對位姿，同時建立環境地圖。

本文使用上標的大寫字母代表坐標系，定義激光雷達完成一次掃描為一幀數據，k表示時間，Pk表示在時間k得到的一幀激光點云，毫米波雷達類似，Ek表示在時間k得到的一幀毫米波數據。本文定義了兩個三維坐標系：其一為激光雷達坐標系Lg，原點為激光雷達的幾何中心。該坐標系中一個激光點云i,其二為毫米波雷達坐標系Bg，該坐標系的原點為毫米波雷達的幾何中心。該坐標系中一個雷達數據i,

多傳感器信息融合首先要解決時空同步的問題。本文采取軟硬結合的方式，通過硬件電路及軟件的方式，實現兩種傳感器的時間同步，對于空間同步，本文則采取新穎的基于特征的兩步配準法，根據傳感器安裝位置及提取的特征點云匹配，實現兩個傳感器的空間校正。然后對兩種傳感器數據進行預處理，使用機器學習的方法找到煙霧所對應的激光點云，使用改進的三級濾波的方法對雷達點云進行濾波。然后根據提出的融合方法，對兩種數據進行融合，將融合之后的點云進行幀間匹配和全局定位建圖，得到準確魯棒的定位和建圖結果，保證無人駕駛的安全可靠。流程如圖2所示。

3 毫米波雷達融合感知算法

3.1 時空校正

在多傳感器系統中，由于其自身性能的優劣等，各個傳感器觀測到環境中的同一目標獲取的測量數據并不一定同步，需要將各個傳感器在不同時刻不同空間獲取的測量數據轉換到統一的時空坐標下，即進行時空配準。時空配準是實現多傳感器信息融合前的關鍵一步。

本文采取硬同步和軟同步相結合的方法，設計了一款基于ARM (Advanced RISC Machine)的嵌入式硬件系統，提供秒脈沖信號(Pulse Per Second,PPS)以供激光雷達和毫米波雷達使用，實現了兩種傳感器的硬同步。但由于兩個傳感器的開機時間、采樣頻率不盡相同，所以在采集數據時，還會給每種傳感器的數據打上時間標簽，以一幀數據采集起始時刻為基準，根據該時間信息，實現兩種傳感器的時間同步，時間同步示意圖如圖3所示。

多傳感器空間校正一般分為兩類：基于人工設定目標的和無需人為設定目標的標定校正方法[15]，基于目標的標定方法費時費力，本文新提出一種無需人為設定目標的標定方法，如圖4所示，此方法可以隨時隨地對兩種傳感器進行標定。

圖4 空間同步流程圖Fig.4 Space synchronization flow chart

由于激光雷達得到的是三維數據，本文將毫米波雷達轉換到激光雷達坐標系下。首先，定義是毫米波雷達坐標系到激光雷達坐標系的變換矩陣，其中，左上角RB_L是旋轉矩陣，右側tB_L是平移向量，左下角為0向量，右下角為1。

這里采取兩步法實現兩種傳感器的空間校正。在進行校正時，先采取本文提出的三級濾波方法對毫米波點云進行濾波，得到純凈的點云，避免噪聲多徑等因素的干擾。提取毫米波的特征點云，通過式(1)計算毫米波點云的曲率，按照該值進行降序排序，選擇一定數量c最大，并且其強度超過了閾值(由區域內點云強度的均值及標準差確定)的點作為特征點。為了保證所選的特征點云能均勻地分布在整個場景，本文將整個空間均勻劃分為幾塊，在每塊區域中選擇相同數量的特征點。

其中，S為連續的毫米波雷達點云集合，由成，同一幀數據中前后的點云。

然后根據各個傳感器的安裝位置，得到初始的旋轉矩陣Rfir和平移向量tfir，將提取的毫米波雷達特征點云轉換到激光雷達坐標系下，得到成初始校正。

最后，根據最近點迭代算法，計算轉換后的雷達特征點云與所有激光點云之間的旋轉矩陣Rsec及平移向量tsec。

根據以上計算結果，可以得到最終的旋轉矩陣RB_L及平移向量tB_L，從而得到變換矩陣TB_L，完成兩種傳感器的空間校正。

通過利用激光雷達和毫米波雷達傳感器之間的安裝位置及所感知的環境信息進行傳感器校正，不需要設計特殊的標定目標，能夠快速地實現兩種傳感器的空間校正。本文的標定方法具有自動和無目標的特性，可以隨時隨地對激光雷達和毫米波雷達傳感器進行標定，根據測試可知，相對于使用全部點云進行匹配，本文方法能夠節省3倍左右的計算時間。通過本文提出的兩步法得到的校正結果如圖5所示，白色的點云為毫米波點云，彩色的為激光點云，未校正之前兩者差別較大，通過兩步配準后，可以實現精確的空間同步。

圖5 多傳感器空間校正結果Fig.5 Multi-sensor spatial calibration results

3.2 數據預處理

本文采取的毫米波雷達為調頻連續波體制，具體參數如表1所示。將雷達回波數據進行脈沖壓縮，并結合轉臺信息，可以得到環境掃描圖像，然后根據目標檢測算法[16]，能夠得到類似激光點云的毫米波點云。但是，毫米波雷達常常會受到噪聲、雜波及多徑的影響，從而產生許多虛假目標等，如圖6(a)所示，為后續的數據融合帶來不便。因此，本文提出了三級濾波的方法，對雷達點云進行處理，得到純凈的點云。

表1 毫米波雷達系統參數Tab.1 Millimeter wave radar system parameters

由于毫米波數據中包含著目標的距離、方位、速度和反射強度等信息，所以第1步根據強度信息進行初級濾波。首先統計所有點的強度的均值及標準差，確定強度閾值(均值減去n倍的標準差)，將點云強度低于一定閾值的點剔除掉，完成初級濾波。同時，由于噪聲、雜波等因素產生的點云都比較孤立且分散，所以利用點云之間的位置關系再次進行濾波。根據點云的位置，搜索距離該點云最近的一定數量的點云，計算該點與臨近點云之間的平均距離，統計所有平均距離的均值及標準差，確定距離閾值(均值加減n倍的標準差)，如果平均距離大于距離閾值，則認為這些點是比較孤立的點進行去除。最后，設置濾波器的搜索半徑，在設定半徑范圍內進行點云搜索，統計其內部的點云數量，若點云數量低于一定的閾值，則進行濾除，具體流程如圖7所示。

圖7 毫米波雷達點云三級濾波流程圖Fig.7 Three-stage filtering flow chart of millimeter wave radar point cloud

通過上述三級濾波，能夠將由噪聲、雜波及多徑等因素產生的離群點、孤立點及虛假目標等剔除掉，從而保留環境中真實存在的并且具有較強反射特性比較穩定的點云，如圖6所示。

圖6 毫米波雷達點云濾波結果Fig.6 Millimeter wave radar point cloud filtering results

由于激光雷達很難穿透煙霧等小顆粒，煙霧的存在會遮擋環境中其他物體，影響智能駕駛的感知，同時激光雷達會把煙霧當作障礙物，建立到環境地圖中，從而影響了后續的導航，需要進行剔除。但是，激光點云只包含位置和強度信息，很難通過單一的強度或者位置信息將環境中的煙霧識別出來，所以本文借鑒機器學習的方法，采取支持向量機(Support Vector Machines,SVM)[17]，對我們的數據進行訓練，在更高維度找出煙霧與其他物體的不同特征，從而實現煙霧點云的識別。

SVM是一種有監督的學習，其目的是尋找一個超平面來對樣本進行分割，SVM的基本想法是求解能夠正確劃分訓練數據集并且幾何間隔最大的分離超平面。假設訓練集其類別yi 20,1g，SV M通過學習得到分離超平面wTx+b=0。這樣的超平面有無窮多個，但是幾何間隔最大的分離超平面卻是唯一的，因此可以轉換為最優問題進行求解

對于煙霧點云的識別，本文采取徑向基核函數將激光點云映射到更高維度，通過間隔最大化，將煙霧點云與其他物體分隔開。本文采集了不同場景不同能見度情況下的煙霧環境的激光點云，由于場景是已知可控的，手工進行打標簽，煙霧點云標簽為–1，即labj=-1，其他為1，并且將地面點云進行剔除，然后選擇這些數據中的80%進行訓練，選擇剩下20%進行測試，訓練集的正確率為99.95%，測試集的正確識別率為96.87%。一幀激光數據的測試結果如圖8所示，紅色為識別到的煙霧點云。

圖8 激光雷達點云煙霧識別結果Fig.8 LiDAR point cloud smoke recognition results

3.3 數據融合

由于毫米波雷達具有多普勒效應，能夠識別環境中的動態目標，而動目標的存在影響智能駕駛的定位。根據我們之前提出的動目標去除方法[18,19]，可以得到動態和靜態的點云數據，將靜態點云進行融合，用于定位，而動態數據則用于避障。

由于傳感器的性能不同，不同傳感器具有不同的視場范圍，本文所使用激光雷達屬于商業產品，視場范圍是確定的，而毫米波雷達則可以根據我們的需求進行調整，所以，根據毫米波雷達的性能及激光雷達的參數，確定兩種傳感器的融合范圍η=[rmin,rmax,θmin,θmax]，其中r為距離向的限制，θ為角度向的限制。利用兩種傳感器的優勢，融合激光雷達和毫米波雷達數據，形成融合數據SFusion。

融合后的數據SFusion應該包括毫米波雷達點云激光雷達點云毫米波雷達和激光雷達融合后點云XF。如果在融合范圍內，毫米波雷達點i和激光點云j之 間的距離小于閾值dF，則認為它們代表了環境中的同一物體。因此可以根據式(6)進行融合。

其中，σradar和σLiDAR是兩種傳感器的測量誤差。

如果激光雷達點和毫米波雷達的相對應點在融合范圍η內，但彼此之間的距離大于dF，則可能是激光雷達受到了環境中煙霧的影響，這時候需要用毫米波雷達點云替代激光雷達的點。由于我們對激光點云進行了識別，這時檢查激光點云的標簽是否為–1，如果是，這表明是由于煙霧的影響產生的點云，則用毫米波數據進行替換。如果發現不是煙霧點云，由于激光雷達的測量相對更準確一些，則認毫米波數據為虛假目標，進行剔除，選擇相信激光雷達的測量。如果兩種傳感器的測量在融合范圍η之外，那么只有測量范圍較大的傳感器才能對融合有貢獻，所以選擇有測量數據的結果進行融合。所有情況總結為

4 實驗結果與分析

為了驗證本文算法的有效性，使用自己搭建的輪式機器人系統進行了不同場景下的驗證。首先，傳感器保持不動，在傳感器前方模擬煙霧場景，采集激光雷達和毫米波雷達的數據，進行環境感知，結果如圖9所示。環境中的煙霧會被當作障礙物建立在點云地圖中，不利于機器人的自主導航，同時由于煙霧的遮擋，場景中的幾輛汽車無法被探測到(紅色矩形標注位置)，所以煙霧環境下，激光雷達的感知能力會大大下降。然而，毫米波雷達能夠穿云透霧，可以探測到煙霧后面的車輛。使用本文的融合方法，將兩者數據進行融合，最終得到的環境地圖如圖9(c)所示?？梢钥闯?，本文的算法能夠有效地將煙霧進行去除，同時把隱藏在煙霧后面的物體探測出來，有利于機器人在煙霧環境中的感知及導航。

圖9 煙霧環境下實驗結果Fig.9 Experimental results in smoky environment

為了進一步驗證我們提出的融合方法對感知性能的提升，我們進行了另一組實驗。在實驗場景中，采用煙霧發生器模擬了煙霧場景，車輛按照預先設定的路線前進，同時采集兩種傳感器的數據，分別使用兩種傳感器基于開源的LOAM (Lidar Odometry And Mapping)[20]進行定位和建圖，結果如圖10所示。從結果可以看出，煙霧的存在會影響激光雷達的幀間匹配，使得建立的環境地圖比較模糊，環境中同一個墻會有多個重影(粉色橢圓標注)。對于毫米波雷達來說，由于其分辨率有限，且只能得到場景空間二維數據，并且受到各種干擾，只能得到粗略的二維點云圖。

為了提高機器人定位精度，對兩種傳感器數據進行融合，得到的結果如圖11所示。紅色的點云表示毫米波雷達的測量，紫色的點云表示激光雷達的測量，綠色的表示兩者加權融合的結果，從中可以看出，毫米波雷達能夠很好地將煙霧點云去除，并將受煙霧影響的區域進行補償，驗證了我們融合方法的有效性。

使用融合之后的點云采取相同的算法進行機器人的定位和建圖，結果如圖12和圖13所示。與圖10對比可以發現，將兩種傳感器的數據進行融合之后進行定位和建圖效果更好。首先，融合之后建立的地圖中的重影消失了，更加接近真實場景。其次，為了定量化評估建圖結果本文選擇了環境中固定不變的建筑物的結構(兩墻之間的距離，即圖10中線段A所示)來評估。使用激光測距儀得到的真實距離為62.50 m，圖10(b)中的線段A長度為63.78 m，而使用融合之后數據得到的結果，即圖12(b)中線段B的長度為62.32 m。所以，融合之后建立的環境地圖更加準確。

圖10 使用激光雷達和毫米波雷達進行感知建圖結果Fig.10 The mapping results based on LiDAR and millimeter-wave radar

圖11 煙霧環境中多傳感器數據融合結果Fig.11 Multi-sensor data fusion results in a smog environment

圖12 傳感器數據融合之后建圖結果Fig.12 Mapping results after sensor data fusion

圖13 使用不同傳感器數據定位結果Fig.13 The localization results using different sensor data

為了定量化地分析結果，本文使用絕對位置的均方根誤差(Root Mean Square Error,RMSE)，來比較使用不同數據時得到的定位精度。

其中，n是估計的次數，是第i次估計的位置結果，是其對應的真實位置。使用該評價標準計算得到：僅使用激光雷達進行定位的誤差為0.27 m，只使用毫米波雷達進行定位的誤差為0.32 m，將兩種傳感器進行融合之后定位的誤差為0.13 m?？梢园l現，多傳感器融合能夠有效提高機器人的定位精度，定位誤差減少了至少50%，建立更精準的環境地圖，更有利于實現智能駕駛。

5 結束語

總而言之，智能駕駛汽車系統在研發與應用階段，毫米波雷達是一種重要設備，在汽車自動駕駛中，由于外界環境的變化較大，往往會面臨未知的不利因素，通過將毫米波雷達和激光雷達進行有效的融合，大幅度提高了無人駕駛汽車在感知技術的性能，保障了無人駕駛的安全。通過本文提出的兩步配準方法，可以實現兩種傳感器精準的空間同步。通過改進的三級濾波法，減少了各種噪聲雜波等對雷達數據的影響。最終通過本文提出的數據融合方法，可以將兩種傳感器的數據進行精確融合，既去除了煙霧對激光雷達的影響，同時還能夠保留環境精準的感知結果。最后通過各種實驗驗證了本文方法的有效性，將兩種傳感器的數據進行融合，能夠有效地提高汽車的定位和建圖精度，保證了智能駕駛感知的魯棒性。