動態環境中多幀點云融合算法及三維目標檢測算法研究

摘 要：低線束激光雷達掃描的點云數據較為稀疏，導致無人駕駛環境感知系統中三維目標檢測效果欠佳，通過多幀點云配準可實現稀疏點云稠密化，但動態環境中的行人與移動車輛會降低激光雷達的定位精度，也會造成融合幀中運動目標上的點云偏移較大。針對上述問題，提出了一種動態環境中多幀點云融合算法，利用該算法在園區道路實況下進行三維目標檢測，提高了低線束激光雷達的三維目標檢測精度。利用16線和40線激光雷達采集的行駛路況數據進行實驗，結果表明該算法能夠增強稀疏點云密度，改善低成本激光雷達的環境感知能力。

關鍵詞：激光雷達；點云融合；位姿變換；無人駕駛

中圖分類號：TP391.41 文獻標志碼：A

文章編號：1001－3695（2023）03－044－0909－05

doi：10.19734/j.issn.1001-3695.2022.05.0345

Research on multi-frame point cloud fusion algorithm and 3D object

detection algorithm in dynamic environment

Wang Lijia1，Yu Huan2，Liu Shouyin1

（1.College of Physical Science amp; Technology，Central China Normal University，Wuhan 430079，China；2.Wuhan InDriving Technology Co.，Ltd.，Wuhan 430079， China）

Abstract：The point cloud data scanned by the low-beam LiDAR is relatively sparse，resulting in poor 3D object detection in the unmanned environment perception system.Multi-frame point cloud registration can achieve sparse point cloud densification，however，pedestrians and moving vehicles in a dynamic environment will reduce the positioning accuracy of the LiDAR，and will also cause a large offset of the point cloud on the moving object in the fusion frame.Aiming at the above problems，this paper proposed a multi-frame point cloud fusion algorithm in a dynamic environment，and used this algorithm to detect 3D objects in the real situation of park roads，which improved the 3D object detection accuracy of low-beam LiDAR.This paper used the dri-ving road condition data collected by 16-line and 40-line LiDAR to conduct experiments.The results show that the algorithm can enhance the density of sparse point clouds and improve the environmental perception ability of low-cost LiDAR.

Key words：LiDAR；point cloud fusion；pose transformation；autonomous driving

0 引言

隨著計算機視覺、人工智能、自動化等學科的快速發展，自動駕駛汽車［1］應運而生。有研究表明，自動駕駛技術將引領智能交通的變革，為道路擁堵、交通事故、環境污染等問題帶來新的解決方案［2］。自動駕駛系統如圖1所示，包括環境感知、導航定位、決策規劃、運動控制等模塊，其中最基礎的環境感知模塊為上層決策規劃提供依據。障礙物檢測是環境感知的核心任務，也是保障車輛安全行駛的關鍵，目前主流的用于環境感知的車載傳感器有攝像機、激光雷達、毫米波雷達等。其中激光雷達相較于攝像機，可以獲取準確的深度信息，且不受光照影響；相較于毫米波雷達，其分辨率更高，可用于車輛、行人等障礙物的輪廓提取和靜態、動態跟蹤等［3］。因此激光雷達以精度高、不易受光照影響、獲取深度信息精確等特點成為了自動駕駛汽車的重要傳感器。

三維目標檢測與基于圖像的二維目標檢測相比，增加了障礙物的三維尺寸和方向信息。點云是三維視覺的表達方式，激光雷達掃描的點云具有更強的空間捕獲能力，且對自然光不敏感，可以更精確地描述三維世界［4］。因此，基于激光雷達點云的三維目標檢測算法具有重要的應用價值。激光雷達線束越多，對環境感知能力越強，故自動駕駛領域基準數據集大多使用高線束激光雷達或多雷達融合來獲取環境信息，如KITTI［5，6］使用Velodyne-64線激光雷達，nuScenes［7］使用一個LiDAR（激光雷達）和五個RADAR（雷達），Waymo［8］使用五個定制激光雷達等。但高線束的激光雷達在提升檢測性能的同時也增加了高昂的成本，Velodyne生產的64線激光雷達售價約是同系列16線產品價格的10倍左右。為降低成本，可以使用具有較低線束的激光雷達，但必須提高目標檢測算法的性能來保障汽車行駛安全，因此提高低線束激光雷達的目標檢測準確率和精度已成為降低自動駕駛汽車成本、加快其落地量產的重要舉措。

低線束激光雷達掃描的點云數據較為稀疏，造成三維目標檢測效果欠佳，因此許多研究者通過增大其點云密度來提升目標檢測精度。比如張佳鵬［9］通過提取點云的曲率特征進行點云配準，將歷史幀點云投影至當前幀，再以配準后的密集點云投影至柵格地圖，最后根據柵格內每個點的高度來判別柵格內是否為障礙物點云；蘇天楚等人［10］通過激光雷達里程計獲取坐標變換矩陣，從而實現多幀疊加點云，再基于歐氏聚類完成障礙物檢測。但是在實際的駕駛環境中存在移動的行人和行駛的車輛，這些動態目標的點云數據會降低激光雷達定位精度［11］，也會導致融合幀中運動目標的點云偏移較大［12］。上述文獻方法在利用激光里程計計算位姿階段沒有考慮動態目標點云引起的定位偏差問題，在點云融合階段沒有考慮動態目標點云偏移問題，在目標檢測階段僅使用傳統的基于規則的障礙物檢測方法，無法給出目標精確的三維框信息。

此外，其他的多幀點云融合方法大多借助GPS、IMU（慣性測量單元）等外部運動測量傳感器來獲取載體運動信息。例如文獻［11］直接通過計算IMU數據來獲取幀間位姿變換矩陣，避免了動態目標點云引起的定位誤差；文獻［12］在此基礎上進一步考慮了利用IMU直接投影存在動態目標上的點云偏移問題。上述文獻方法不僅增加了成本，而且其融合效果極大地依賴外部傳感器的性能。因此，本文考慮不使用額外傳感器，僅基于激光雷達進行多幀點云融合，以稀疏點云稠密化來增強點云表征能力，從而改善其三維目標檢測效果。但在動態環境中進行多幀點云配準，行人與移動車輛等動態目標點云會降低激光雷達的定位精度，文獻［13］同樣借助IMU來濾除動態目標點云從而提高激光里程計的定位精度，而且動態目標點云在融合幀中存在較大偏移問題。

針對上述問題，本文提出一種動態環境中多幀點云融合算法，并結合基于深度學習的三維目標檢測模型PointRCNN［14］完成園區道路實況中的障礙物檢測任務。該算法通過幀幀配準與Ceres優化來計算幀間位姿變換矩陣，并借助動態目標檢測解決動態目標點云引起的定位偏差與融合幀動態目標點云偏移問題，最后使用融合幀點云進行三維目標檢測，提高低線束激光雷達的檢測精度。

1 整體結構設計

本文的整體算法框圖如圖2所示。低線束激光雷達以10 Hz的頻率輸出點云幀，表示為（N，3+C），其中N是點的個數，3是每個點的三維坐標［x，y，z］，C是傳感器捕捉到的其他信息，如點云的強度、目標的材質信息等。在每三幀點云數據中，使用前兩幀作為歷史幀，通過多幀點云融合算法將歷史幀點云數據融合進當前幀，豐富當前幀點云信息，得到融合幀，再將融合幀點云數據作為三維目標檢測算法的輸入，獲取環境中障礙物的類別與三維框信息。

圖2中“融合幀點云”通過多幀點云融合算法得到，其算法流程如圖3所示，主要包括位姿變換矩陣計算與動態目標平移兩部分。算法首先通過預處理來濾除動態目標點云，再根據環境中的靜態目標點云提取特征進行點云配準，從而降低動態目標點云引起的定位偏差；計算出歷史幀到當前幀的位姿變換矩陣（R，T）后，將歷史幀點云數據轉換至當前幀點云坐標系下，得到初步融合點云幀；接下來將預處理后的動態目標點云簇進行歐氏聚類［15］，通過動態目標平移來解決初步融合點云幀中動態目標點云偏移問題，最終得到經過精確配準的致密點云數據，便于后續的檢測算法。

2 算法設計與實現

2.1 動態目標預處理

預處理主要是為了濾除行駛環境中的動態目標點云，因為在根據前后幀點云配準來估計位姿變換矩陣時，動態目標點云會導致累積誤差，從而降低定位精度。為快速提取出點云幀中的動態點，將三維點云數據投影至二維柵格圖，根據柵格內點云屬性劃分出障礙物點云簇，再由候選目標點云簇特點找到動態目標點云簇，完成動態目標點云的快速識別與濾除。

設點云集合為P，其中某一點Pi的坐標為（xi，yi，zi），在激光雷達坐標系的XOY平面中劃分柵格，柵格尺寸G為0.5 m，整個柵格圖在x、y坐標軸上的偏移量為（gridx，gridy），則激光雷達坐標系中點Pi到二維柵格圖的投影轉換公式為

柵格圖中每個柵格存儲原始點云的全部信息，如三維坐標與反射率等。通過將柵格內點云與最低點的高度差與閾值進行比較，可以劃分出地面點云與非地面點云，濾除地面點云后，對每個柵格判斷其屬性來劃分障礙物與非障礙物點云柵格。具體做法是，遍歷柵格圖中每個柵格里的點，計算柵格內點與點之間的最大高度差H_max和柵格中點的數量point_num，如果柵格內H_max大于閾值T1的同時point_num大于閾值T2，那么認為此柵格為障礙物柵格。這種考慮高度差和點云數量的判定方法可以進一步消除殘余地面點影響，降低環境噪聲帶來的干擾，在保留點云簇原始幾何特征的同時也降低了計算存儲量。

障礙物柵格中包含靜態與動態目標，環境中的動態目標主要考慮行人和車輛，根據行人、車輛點云簇的特點從候選目標柵格中確定其對應的柵格。上述候選目標點云簇表示為［l，w，h，0；cluster{i}］，主要包括點云簇的長、寬、高尺寸信息與點云簇中每個點的位置坐標（x，y，z）和強度信息。本文根據車輛、行人點云簇的輪廓信息、幾何信息和材質信息來區分出車輛點云簇和行人點云簇。文中提取了點云簇的寬高比、點云強度信息的均值和方差作為特征向量，從候選目標點云簇中識別車輛與行人目標點云。

2.2 位姿變換矩陣計算

從歷史幀到當前幀的位姿變換矩陣的計算主要包括載體運動畸變去除、點云特征提取和點云配準三部分。因為激光雷達傳感器位于載體汽車上，隨著汽車的運動，激光雷達所發射的激光線束掃描完成一圈后，其起點與終點會發生偏移，產生點云畸變，為精確實現點云位姿變換，需要先計算畸變因子，消除因載體運動產生的點云畸變。假設激光雷達點云所在的三維空間坐標系為L，第k幀點云表述為pLk，該幀點云掃描的起始與終止時刻分別為tk、tk+1，對于幀內的某一點i，其對應的畸變因子fi的計算公式為

2.3 動態目標平移

根據位姿變換矩陣將歷史幀點云投影至當前幀完成點云初步融合，對于歷史幀中的動態目標，其在從歷史幀到當前幀的時間間隔內存在運動偏移情況，參考文獻［12］完成動態目標點云的提取與平移，與文獻［12］不同，本文無須限制障礙物的長寬，可直接利用預處理獲得動態目標點云簇。首先將預處理階段得到的動態目標點云簇進行歐氏聚類［15］，并對聚類出的點云簇計算出最小外接矩形框，矩形框的中心為動態目標點云簇的中心。

將歷史幀與當前幀點云的聚類結果進行最近鄰搜索，設置最大搜索距離閾值為0.3 m，在閾值內搜索到的進行匹配，將歷史幀中對應的聚類目標平移至當前幀中目標所在的位置，使得動態目標矩形框的中心重合。通過上述動態目標平移降低動態目標點云偏移問題，得到更精確的融合點云幀。

2.4 三維目標檢測算法

基于點云的3D目標檢測任務，輸入是點云（N，3+C），輸出是場景中目標的3D包圍框（3D bounding box，3D Bbox）。3D Bbox是在真實三維世界中包圍目標物體的最小長方體，其主要包含物體中心位置距離自車的原點距離坐標（cx，cy，cz），物體的長、寬、高（dx，dy，dz）以及物體在鳥瞰圖上的朝向（heading），即物體的移動方向，自動駕駛場景下是車頭的方向。因此自動駕駛中的三維目標檢測也是一個目標物體7自由度預測問題。

基于深度學習的點云目標檢測算法根據特征提取方式分為基于點point-based、基于體素voxel-based、基于鳥瞰圖BEV-based等；根據檢測方式分為基于錨定anchor-based與無錨定anchor-free；根據階段劃分可以分為一階段one-stage和兩階段two-stage。三維目標檢測算法流程如圖4所示。

本文選擇基于點的、無錨定兩階段檢測網絡PointRCNN［14］用于識別環境中車輛并定位車輛的三維框。PointRCNN網絡結構如圖5所示，第一階段是從原始點云中自底向上生成3D提案，即從每一個前景點中生成三維候選框；第二階段進行三維框細化，通過點云區域池化來回歸目標準確的3D包圍框和分類置信度。

3 實驗結果與分析

本文利用16線和40線激光雷達分別采集實際行駛路況的點云數據進行實驗，測試本文的多幀點云融合算法在不同線束激光雷達點云數據上的融合效果，驗證算法的魯棒性；同時進行三維目標檢測實驗，驗證點云稠密化對目標檢測算法性能的提升，證明本文算法能夠增強稀疏點云密度，在較低成本的激光雷達上實現較好的環境感知效果。

3.1 傳感器介紹

本文使用速騰聚創RS-LiDAR-16型號的16線與禾賽科技Pandar40P型號的40線激光雷達分別采集環境信息，傳感器為車頂對心安裝。選用NVIDIA Jetson TX2嵌入式計算機作為主控板實時接收環境信息，軟件系統為Ubuntu 16.04和ROS（robot operation system），采集場景為兩個高校和兩個科技園區。RoboSense-16與Pandar40P 激光雷達性能參數如表1、2所示。

3.2 實驗平臺

本文的實驗基于Ubuntu服務器和PyTorch深度學習框架，實驗平臺具體配置介紹如表3所示。

3.3 多幀點云融合結果

本文基于16線和40線激光雷達點云數據進行了融合實驗，通過處理前后的效果對比圖，可視化展示了激光雷達點云的稠密化效果。

圖6為RoboSense-16的融合效果對比圖，中間黑色圓圈是搭載激光雷達傳感器采集車所在的位置，點云從采集車發散出去，距離激光雷達越遠，點云數量越少，越稀疏。其中，（a）是16線激光雷達的單幀點云數據；（b）是三幀融合后的點云數據。可以看出，歷史幀點云可以較好地增強當前幀的點云信息。

激光雷達40線的點云融合效果對比如圖7所示，其中自車的前進方向為圖中由左至右，整個行駛場景點云圖展示出遠距離、大范圍的點云融合效果。

圖8為動態目標“行人”的點云融合效果對比圖，是Pandar40P激光雷達采集的點云數據。由圖可知，算法通過歷史幀中點云信息豐富了當前幀點云，較好地解決了動態目標點云偏移問題，增強了運動目標的點云密度，便于后續檢測。

本文算法融合效果與迭代最近點算法（iterative closest point，ICP）融合效果的對比如圖9所示。為便于觀察融合效果，使用Pandar40P激光雷達采集的點云數據。由圖可知，針對動態目標“車輛”，本文算法相較于迭代最近點算法，可以較好地降低動態目標點云的偏移誤差，改善動態目標融合效果。

由實驗結果可知，本文提出的多幀點云融合算法可以適應不同線束激光雷達點云數據，能夠降低動態目標點云引起的定位偏差與融合幀點云偏移問題，對于動態環境中的點云有較好的融合效果。

接下來進行算法耗時性分析，本文采集16線點云數據共1 050幀，融合算法共耗時76.65 s，平均每幀耗時0.073 s，激光雷達以10 Hz輸出點云，本文設計的算法能夠滿足實時性要求。

3.4 三維目標檢測結果

為驗證點云融合算法對后續三維目標檢測算法的影響，本文分別在單幀點云和融合幀點云中使用PointRCNN算法進行三維目標檢測，對比其檢測結果中目標識別的準確率與三維框的定位精度。

圖10為RoboSense-16的三維目標檢測效果對比。圖中綠色三維框為“車輛”，每個三維框右上角的數值為判別其是該類目標的置信度，三維框中含有×標記的一面為車頭方向。由對比圖可知，融合幀點云可以識別出更遠處的“車輛”，提高16線激光雷達的目標檢測效果。

基于Pandar40P的三維目標檢測效果對比如圖11所示。其中藍色三維框為“行人”，可以看出，相較于融合幀，單幀點云難以識別出遠距離、有遮擋、小目標的障礙物。

上述是分別針對不同線束的激光雷達點云進行三維目標檢測效果對比，圖12是對比不同融合算法的檢測效果。由圖12可知，基于ICP的點云融合算法在靜態環境中效果較好，但在動態環境中，動態目標點云會引起定位誤差，且位姿變換后動態目標點云會產生偏移，影響三維目標檢測的檢測精度。圖12中，（a）顯示不能檢測到遠處的目標，（b）則能很好地檢測出遠處的目標。

由實驗結果對比可知，融合幀可以較好地提升低線束激光雷達單幀點云的三維目標檢測精度，即本文算法能夠增強稀疏點云密度，改善低成本激光雷達的環境感知效果。

4 結束語

本文從降低成本、加快技術落地、提高算法檢測性能的角度出發，使用低線束激光雷達進行環境感知，為改善稀疏點云的三維目標檢測效果，提出一種動態環境中多幀點云融合算法，通過識別動態目標來降低點云幀的定位誤差與融合幀的點云偏移，讓歷史幀點云更精確地融合進當前幀，最后使用融合幀點云進行三維目標檢測。實車實驗證明，該方法能夠適用于不同線束激光雷達點云數據，改善低成本激光雷達的環境感知能力。

參考文獻：

［1］張茜.無人駕駛系統的設計與實現［J］.電子技術與軟件工程，2019（18）：103-104.（Zhang Qian.Design and implementation of unmanned driving system［J］.Electronic Technology amp; Software Engineering，2019（18）：103-104.）

［2］劉少山，唐潔，吳雙，等.第一本自動駕駛技術書［M］.北京：電子工業出版社，2017.（Liu Shaoshan，Tang Jie，Wu Shuang，et al.The first handbook of autonomous driving technology［M］.Beijing：Publishing House of Electronics Industry，2017.）

［3］陳篤廉.無人駕駛客車的發展現狀及展望［J］.機電技術，2017（3）：109-112.（Chen Dulian.Development status and prospect of driverless buses［J］.Mechanical amp; Electrical Technology，2017（3）：109-112.）

［4］Wu Yutian，Wang Yueyu，Zhang Shuwei，et al.Deep 3D object detection networks using LiDAR data：a review［J］.IEEE Sensors Journal，2021，21（2）：1152-1171.

［5］Geiger A，Lenz P，Urtasun R.Are we ready for autonomous driving？The KITTI vision benchmark suite［C］//Proc of IEEE Conference on Computer Vision and Pattern Recognition.Washington DC：IEEE Computer Society，2012：3354-3361.

［6］Geiger A，Lenz P，Stiller C，et al.Vision meets robotics：the KITTI dataset［J］.International Journal of Robotics Research，2013，32（11）：1231-1237.

［7］Caesar H，Bankiti V，Lang A H，et al.nuScenes：a multimodal dataset for autonomous driving［C］//Proc of IEEE/CVF Conference on Computer Vision and Pattern Recognition.Piscataway，NJ：IEEE Press，2020：11621-11631.

［8］Sun Pei，Kretzschmar H，Dotiwalla X，et al.Scalability in perception for autonomous driving：Waymo open dataset［C］//Proc of IEEE/CVF Conference on Computer Vision and Pattern Recognition.Piscataway，NJ：IEEE Press，2020：2446－2454.

［9］張佳鵬.基于時空融合的激光雷達障礙物檢測與運動目標跟蹤［D］.杭州：浙江大學，2019.（Zhang Jiapeng.Obstacle detection and moving object tracking based on spatial-temporal fusion of LiDAR［D］.Hangzhou：Zhejiang University，2019.）

［10］蘇天楚，潘博，倪俊.基于低線束雷達的低矮障礙物優化檢測［C］//2018中國汽車工程學會年會論文集.北京：機械工業出版社，2018：177-182.（Su Tianchu，Pan Bo，Ni Jun.Optimal detection of low obstacles based on low beam radar［C］//Proc of Society of Automotive Engineers-China Congress.Beijing：China Machine Press，2018：177-182.）

［11］張艷國，李擎.基于慣性測量單元的激光雷達點云融合方法［J］.系統仿真學報，2018，30（11）：4334－4339.（Zhang Yanguo，Li Qing.Multi-frame fusion method for point cloud of LiDAR based on IMU［J］.Journal of System Simulation，2018，30（11）：4334－4339.）

［12］劉輝席，陳文浩，劉守印.基于IMU和動態目標檢測的多幀點云融合算法［J］.計算機應用研究，2021，38（7）：2179－2182.（Liu Huixi，Chen Wenhao，Liu Shouyin.Multi-frame point cloud fusion algorithm based on IMU and dynamic target detection［J］.Application Research of Computers，2021，38（7）：2179－2182.）

［13］張濤，張晨，魏宏宇.動態環境下融合激光雷達和IMU的激光里程計設計［J］.導航定位與授時，2022，9（3）：70-78.（Zhang Tao，Zhang Chen，Wei Hongyu.Design of LiDAR Odemeter integrating LiDAR and IMU in dynamic environment［J］.Navigation Positioning and Timing，2022，9（3）：70-78.）

［14］Shi Shaoshuai，Wang Xiaogang，Li Hongsheng.PointRCNN：3D object proposal generation and detection from point cloud［C］//Proc of IEEE/CVF Conference on Computer Vision and Pattern Recognition.Piscataway，NJ：IEEE Press，2019：770-779.

［15］陳向陽，楊洋，向云飛.歐氏聚類算法支持下的點云數據分割［J］.測繪通報，2017（11）：27－31，36.（Chen Xiangyang，Yang Yang，Xiang Yunfei.Measurement of point cloud data segmentation based on Euclidean clustering algorithm［J］.Bulletin of Surveying and Mapping，2017（11）：27－31，36.）

收稿日期：2022－05－15；修回日期：2022－07－12

作者簡介：王理嘉（1993－），女，河南鹿邑人，碩士研究生，主要研究方向為自動駕駛、三維目標檢測；于歡（1991－），男（土家族），湖南慈利人，博士研究生，主要研究方向為多源感知與定位在自動駕駛里面的應用；劉守印（1964－），男（通信作者），河南周口人，教授，博士，主要研究方向為無線通信、物聯網、機器學習（syliu@mail.ccnu.edu.cn）．