一种面向智能驾驶的毫米波雷达与激光雷达融合的鲁棒感知算法

党相卫 秦 斐 卜祥玺 梁兴东*

①(中国科学院空天信息创新研究院微波成像技术国家级重点实验室 北京 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。可以发现，多传感器融合能够有效提高机器人的定位精度，定位误差减少了至少50%，建立更精准的环境地图，更有利于实现智能驾驶。

5 结束语

总而言之，智能驾驶汽车系统在研发与应用阶段，毫米波雷达是一种重要设备，在汽车自动驾驶中，由于外界环境的变化较大，往往会面临未知的不利因素，通过将毫米波雷达和激光雷达进行有效的融合，大幅度提高了无人驾驶汽车在感知技术的性能，保障了无人驾驶的安全。通过本文提出的两步配准方法，可以实现两种传感器精准的空间同步。通过改进的三级滤波法，减少了各种噪声杂波等对雷达数据的影响。最终通过本文提出的数据融合方法，可以将两种传感器的数据进行精确融合，既去除了烟雾对激光雷达的影响，同时还能够保留环境精准的感知结果。最后通过各种实验验证了本文方法的有效性，将两种传感器的数据进行融合，能够有效地提高汽车的定位和建图精度，保证了智能驾驶感知的鲁棒性。