本发明公开了一种多雷达数据融合的扬尘场景下障碍物检测方法与系统，其包括：步骤S1，获取多雷达数据；步骤S2，将多雷达数据中的激光雷达点云转换为矩阵化后的点云深度图；步骤S3，计算点云深度图上点云坡度特征，并根据连通域搜索获得非地面特征点云聚类，将单个连通域内特征点云数量超过阈值的点云聚类作为障碍物点云聚类，计算非地面特征点云聚类离地高度、连通度和表面离散度；步骤S4，将多雷达数据中的毫米波雷达障碍物检测数据的坐标转换到激光雷达坐标系下，并根据激光雷达目标与毫米波雷达目标之间的距离和/或交并比，计算单个激光雷达的关联特征值；步骤S5，计算点云聚类为灰尘点云聚类的概率，并获得非扬尘障碍物的点云聚类位置。
1.一种多雷达数据融合的障碍物检测方法，其特征在于，包括：
步骤S2，将所述多雷达数据中的激光雷达点云转换为矩阵化后的点云深度图；
步骤S3，计算所述点云深度图上点云坡度特征，并根据连通域搜索获得非地面特征点云聚类，将单个所述连通域内特征点云数量超过阈值的点云聚类作为障碍物点云聚类，计算所述非地面特征点云聚类离地高度、连通度和表面离散度；
步骤S4，将所述多雷达数据中的毫米波雷达障碍物检测数据的坐标转换到激光雷达坐标系下，并根据激光雷达目标与毫米波雷达目标之间的距离和/或交并比，计算单个激光雷达的关联特征值；
步骤S5，计算点云聚类为灰尘点云聚类的概率，并获得非扬尘障碍物的点云聚类位置；
步骤S4中根据激光雷达目标与毫米波雷达目标之间的距离和/或交并比，计算单个激光雷达的关联特征值的方法包括：步骤S41，判断激光雷达目标与毫米波雷达目标是否关联，若是，则进入步骤S42；
步骤S42，采用式(16)或式(17)计算单个激光雷达的关联特征值：
式中，single_relevancy、obj_relevancy均表示单个激光雷达的关联特征值，rectA表示激光雷达的目标矩形框，rectB表示毫米波雷达的目标矩形框，∩表示交集，∪表示并集，rectC表示能够将rectA、rectB包围的最小矩形框，numobj_rele为与当前激光雷达目标成功关联的毫米波雷达目标数量，single_relevancyn表示第n个激光雷达的关联特征值，max_single_relevancy表示当前激光雷达的最大关联特征值。
2.如权利要求1所述的多雷达数据融合的障碍物检测方法，其特征在于，所述步骤S3中的所述点云坡度特征α通过所述点云深度图中上、下相邻两障碍物特征点p1、p2形成的直线与激光雷达坐标系xy平面之间的角度获得，如式(3)所示：式中，(x1,y1,z1)、(x2,y2,z2)分别为p1、p2点在激光雷达坐标系中的坐标，l1，2为p1、p2在所述xy平面上的距离。
3.如权利要求1所述的多雷达数据融合的障碍物检测方法，其特征在于，所述步骤S3中连通域搜索包括对所述点云深度图的上下方向和左右方向的搜索，将p1、p2之间欧式空间几何距离阈值l2thresh的计算公式设置为式(5)，将p1、p2之间的连通域聚类判断条件设置为式(6)和式(7)：式中，k为距离阈值系数，β为p1、p2分别与激光雷达的几何中心连线形成的夹角，max_d为p1、p2中与激光雷达的几何中心距离较远的点与激光雷达原点之间的距离，b为预设的固定偏置系数，(row1,col1)、(row2,col2)分别为p1、p2在所述点云深度图中的坐标。
4.如权利要求1所述的多雷达数据融合的障碍物检测方法，其特征在于，步骤S41具体包括：步骤S411，判断激光雷达目标与毫米波雷达目标是否产生重叠，若是，则可直接认为两者成功关联，若两目标未发生重叠，则进入步骤S412；
步骤S412，判断毫米波雷达目标与激光雷达目标之间的距离是否小于关联距离阈值connect_distance_thresh，若是，则认为该毫米波雷达目标与该激光雷达目标成功关联。
5.如权利要求1‑4中任一项所述的多雷达数据融合的障碍物检测方法，其特征在于，所述步骤S5中，利用下式(19)计算该点云聚类属于扬尘点云聚类的概率dust_probability：dust_probability＝k1×obj_height+k2×obj_ratio+k3×obj_discrete+k4×obj_relevancy，#(19)
式中，k1、k2、k3、k4分别为不同特征的缩放系数，obj_height、obj_ratio、obj_discrete为通过所述步骤S3获得的所述非地面特征点云聚类离地高度、连通度和表面离散度，obj_relevancy为单个激光雷达的关联特征值。
6.一种多雷达数据融合的障碍物检测系统，其特征在于，包括：
点云深度图转换单元，其用于将所述多雷达数据中的激光雷达点云转换为矩阵化后的点云深度图；
点云特征计算单元，其用于计算所述点云深度图上点云坡度特征，并根据连通域搜索获得非地面特征点云聚类，将单个所述连通域内特征点云数量超过阈值的点云聚类作为障碍物点云聚类，计算所述非地面特征点云聚类离地高度、连通度和表面离散度；
关联特征计算单元，其用于将所述多雷达数据中的毫米波雷达障碍物检测数据的坐标转换到激光雷达坐标系下，并根据激光雷达目标与毫米波雷达目标之间的距离和/或交并比，计算单个激光雷达的关联特征值；
概率计算单元，其用于计算点云聚类为灰尘点云聚类的概率，并获得非扬尘障碍物的点云聚类位置；
关联特征计算单元中根据激光雷达目标与毫米波雷达目标之间的距离和/或交并比，计算单个激光雷达的关联特征值的方法包括：判断激光雷达目标与毫米波雷达目标是否关联，若是，则采用式(16)或式(17)计算单个激光雷达的关联特征值：式中，single_relevancy、obj_relevancy均表示单个激光雷达的关联特征值，rectA表示激光雷达的目标矩形框，rectB表示毫米波雷达的目标矩形框，∩表示交集，∪表示并集，rectC表示能够将rectA、rectB包围的最小矩形框，numobj_rele为与当前激光雷达目标成功关联的毫米波雷达目标数量，single_relevancyn表示第n个激光雷达的关联特征值，max_single_relevancy表示当前激光雷达的最大关联特征值。
7.如权利要求6所述的多雷达数据融合的障碍物检测系统，其特征在于，所述点云特征计算单元中的所述点云坡度特征α通过所述点云深度图中上、下相邻两障碍物特征点p1、p2形成的直线与激光雷达坐标系xy平面之间的角度获得，如式(3)所示：式中，(x1,y1,z1)、(x2,y2,z2)分别为p1、p2点在激光雷达坐标系中的坐标，l1，2为p1、p2在所述xy平面上的距离。
8.如权利要求6所述的多雷达数据融合的障碍物检测系统，其特征在于，所述点云特征计算单元中连通域搜索包括对所述点云深度图的上下方向和左右方向的搜索，将p1、p2之间欧式空间几何距离阈值l2thresh的计算公式设置为式(5)，将p1、p2之间的连通域聚类判断条件设置为式(6)和式(7)：式中，k为距离阈值系数，β为p1、p2分别与激光雷达的几何中心连线形成的夹角，max_d为p1、p2中与激光雷达的几何中心距离较远的点与激光雷达原点之间的距离，b为预设的固定偏置系数，(row1,col1)、(row2,col2)分别为p1、p2在所述点云深度图中的坐标。
9.如权利要求6‑8中任一项所述的多雷达数据融合的障碍物检测系统，其特征在于，所述概率计算单元中，利用下式(19)计算该点云聚类属于扬尘点云聚类的概率dust_probability：dust_probability＝k1×obj_height+k2×obj_ratio+k3×obj_discrete+k4×obj_relevancy，#(19)
式中，k1、k2、k3、k4分别为不同特征的缩放系数，obj_height、obj_ratio、obj_discrete为通过点云特征计算单元获得的所述非地面特征点云聚类离地高度、连通度和表面离散度，obj_relevancy为单个激光雷达的关联特征值。
多雷达数据融合的扬尘场景下障碍物检测方法与系统
技术领域
[0001] 本发明涉及雷达数据融合技术领域，特别是关于一种多雷达数据融合的扬尘场景下障碍物检测方法与系统。
背景技术
[0002] 在矿山运输场景中，矿用自卸车具有载重大、稳定性好等特点，因此矿用自卸车成为了矿山中的重要运输装备。在以往的生产场景中矿用自卸车往往由具有丰富驾驶经验的卡车司机来驾驶，但由于工作环境恶劣、位置偏远、工作要求高等原因，矿用自卸车司机招聘愈发困难，因此通过实现矿用自卸车的自动驾驶来减少矿山运输对人工的依赖程度，对
于促进未来矿山生产、运输的发展具有重要意义。而实现道路中障碍物的精准、稳定检测对于实现矿用自卸车自动驾驶具有重要意义，精准、稳定的障碍物检测能够让矿用自卸车提
前规划安全行驶路径，避免与障碍物发生碰撞而发生安全事故。
[0003] 然而，目前矿山环境下的障碍物检测还面临着诸多挑战，例如非结构化的道路环境、扬尘引起的传感器噪声、夜间行驶环境亮度偏低等问题。非结构化的道路路面崎岖不
平，难以直接提取道路地面信息，对障碍物提取产生了阻碍；扬尘易使传感器产生噪声数
据，错误提取扬尘特征数据，使障碍物检测算法产生误检；夜间亮度偏低则易使摄像头图像质量偏低，难以提取出有效特征数据，从而使图像障碍物检测算法产生漏检。因此矿山环境下的障碍物检测系统正逐步向着多传感器融合的方向推进，通过融合多个传感器的数据，
解决单一传感器在面临具有挑战的矿山场景时可能出现的误检、漏检问题，实现矿山环境
下障碍物的精准、稳定的检测。
发明内容
[0004] 本发明的目的在于提供一种多雷达数据融合的扬尘场景下障碍物检测方法与系统，其能够精准、稳定检测扬尘场景下障碍物目标。
[0005] 为实现上述目的，本发明提供一种多雷达数据融合的障碍物检测方法，其包括：
[0006] 步骤S1，获取多雷达数据；
[0007] 步骤S2，将所述多雷达数据中的激光雷达点云转换为矩阵化后的点云深度图；
[0008] 步骤S3，计算所述点云深度图上点云坡度特征，并根据连通域搜索获得非地面特征点云聚类，将单个所述连通域内特征点云数量超过阈值的点云聚类作为障碍物点云聚
类，计算所述非地面特征点云聚类离地高度、连通度和表面离散度；
[0009] 步骤S4，将所述多雷达数据中的毫米波雷达障碍物检测数据的坐标转换到激光雷达坐标系下，并根据激光雷达目标与毫米波雷达目标之间的距离和/或交并比，计算单个激光雷达的关联特征值；
[0010] 步骤S5，计算点云聚类为灰尘点云聚类的概率，并获得非扬尘障碍物的点云聚类位置。
[0011] 进一步地，所述步骤S3中的所述点云坡度特征α通过所述点云深度图中上、下相邻两障碍物特征点p1、p2形成的直线与激光雷达坐标系xy平面之间的角度获得，如式(3)所示：
[0012]
[0013] 式中，(x1，y1，z1)、(x2，y2，z2)分别为p1、p2点在激光雷达坐标系中的坐标，l1，2为p1、p2在所述xy平面上的距离。
[0014] 进一步地，所述步骤S3中连通域搜索包括对所述点云深度图的上下方向和左右方向的搜索，将p1、p2之间欧式空间几何距离阈值l2thresh的计算公式设置为式(5)，将p1、p2之间的连通域聚类判断条件设置为式(6)和式(7)：
[0015]
[0016]
[0017]
[0018] 式中，k为距离阈值系数，β为p1、p2分别与激光雷达的几何中心连线形成的夹角，max_d为p1、p2中与激光雷达的几何中心距离较远的点与激光雷达原点之间的距离，b为预设的固定偏置系数，(row1，col1)、(row2，col2)分别为p1、p2在所述点云深度图中的坐标。
[0019] 进一步地，步骤S4中根据激光雷达目标与毫米波雷达目标之间的距离和/或交并比，计算单个激光雷达的关联特征值的方法包括：
[0020] 步骤S41，判断激光雷达目标与毫米波雷达目标是否关联，若是，则进入步骤S42；
[0021] 步骤S42，采用式(16)或式(17)计算单个激光雷达的关联特征值：
[0022]
[0023]
[0024]
[0025] 式中，single_relevancy、obj_relevancy均表示单个激光雷达的关联特征值，rectA表示激光雷达的目标矩形框，rectB表示毫米波雷达的目标矩形框，∩表示交集，∪表示并集，rectc表示能够将rectA、rectB包围的最小矩形框，numobj_rele为与当前激光雷达目标成功关联的毫米波雷达目标数量，single_relevancyn表示第n个激光雷达的关联特征值，
max_single_relevancy表示当前激光雷达的最大关联特征值。
[0026] 进一步地，步骤S41具体包括：
[0027] 步骤S411，判断激光雷达目标与毫米波雷达目标是否产生重叠，若是，则可直接认为两者成功关联，若两目标未发生重叠，则进入步骤S412；
[0028] 步骤S412，判断毫米波雷达目标与激光雷达目标之间的距离是否小于关联距离阈值connect_distance_thresh，若是，则认为该毫米波雷达目标与该激光雷达目标成功关
联。
[0029] 进一步地，所述步骤S5中，利用下式(19)计算该点云聚类属于扬尘点云聚类的概率dust_probability：
[0030] dust_probability＝k1×obj_height+k2×obj_ratio+k3×obj_discrete+k4×obj_relevancy， #(19)
[0031] 式中，k1、k2、k3、k4分别为不同特征的缩放系数，obj_height、obj_ratio、obj_discrete为通过所述步骤S3获得的所述非地面特征点云聚类离地高度、连通度和表面离散度，obj_relevancy为单个激光雷达的关联特征值。
[0032] 本发明还提供一种多雷达数据融合的障碍物检测系统，其包括：
[0033] 雷达数据获取单元，其用于获取多雷达数据；
[0034] 点云深度图转换单元，其用于将所述多雷达数据中的激光雷达点云转换为矩阵化后的点云深度图；
[0035] 点云特征计算单元，其用于计算所述点云深度图上点云坡度特征，并根据连通域搜索获得非地面特征点云聚类，将单个所述连通域内特征点云数量超过阈值的点云聚类作
为障碍物点云聚类，计算所述非地面特征点云聚类离地高度、连通度和表面离散度；
[0036] 关联特征计算单元，其用于将所述多雷达数据中的毫米波雷达障碍物检测数据的坐标转换到激光雷达坐标系下，并根据激光雷达目标与毫米波雷达目标之间的距离和/或
交并比，计算单个激光雷达的关联特征值；
[0037] 概率计算单元，其用于计算点云聚类为灰尘点云聚类的概率，并获得非扬尘障碍物的点云聚类位置。
[0038] 进一步地，所述点云特征计算单元中的所述点云坡度特征α通过所述点云深度图中上、下相邻两障碍物特征点p1、p2形成的直线与激光雷达坐标系xy平面之间的角度获得，如式(3)所示：
[0039]
[0040] 式中，(x1，y1，z1)、(x2，y2，z2)分别为p1、p2点在激光雷达坐标系中的坐标，l1，2为p1、p2在所述xy平面上的距离。
[0041] 进一步地，所述点云特征计算单元中连通域搜索包括对所述点云深度图的上下方向和左右方向的搜索，将p1、p2之间欧式空间几何距离阈值l2thresh的计算公式设置为式(5)，将p1、p2之间的连通域聚类判断条件设置为式(6)和式(7)：
[0042]
[0043]
[0044]
[0045] 式中，k为距离阈值系数，β为p1、p2分别与激光雷达的几何中心连线形成的夹角，max_d为p1、p2中与激光雷达的几何中心距离较远的点与激光雷达原点之间的距离，b为预设的固定偏置系数，(row1，col1)、(row2，col2)分别为p1、p2在所述点云深度图中的坐标。
[0046] 进一步地，所述概率计算单元中，利用下式(19)计算该点云聚类属于扬尘点云聚类的概率dust_probability：
[0047] dust_probability＝k1×obj_height+k2×obj_ratio+k3×obj_discrete+k4×obj_relevancy， #(19)
[0048] 式中，k1、k2、k3、k4分别为不同特征的缩放系数，obj_height、obj_ratio、obj_discrete为通过所述步骤S3获得的所述非地面特征点云聚类离地高度、连通度和表面离散度，obj_relevancy为单个激光雷达的关联特征值。
[0049] 本发明由于采取以上技术方案，其具有以下优点：
[0050] 1、本发明不依赖于深度学习，因此不需要额外标注数据以及额外增加计算资源，部署与应用难度较低；
[0051] 2、传感器为激光雷达与毫米波雷达，不需要提供额外的光线照明，夜间也可正常工作，满足矿山夜间运输需求；
[0052] 3、通过多传感器数据融合有效减少扬尘、雨雪等产生的误检，在矿山扬尘场景下障碍物检测功能依然具有较高的稳定性与准确性。
附图说明
[0053] 图1为本发明实施例提供的传感器安装示意图。
[0054] 图2为本发明实施例提供的障碍物检测方法流程图。
[0055] 图3为本发明实施例提供的点云坡度计算示意图。
[0056] 图4为本发明实施例提供的欧式空间几何距离阈值设置示意图。
[0057] 图5为本发明实施例提供的目标关联判断示意图。
具体实施方式
[0058] 在附图中，使用相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面结合附图对本发明的实施例进行详细说明。
[0059] 在本发明的描述中，术语“中心”、“纵向”、“横向”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明保护范围的限制。
[0060] 在不冲突的情况下，本发明各实施例及各实施方式中的技术特征可以相互组合，并不局限于该技术特征所在的实施例或实施方式中。
[0061] 下面结合附图以及具体实施例对本发明做进一步的说明，需要指出的是，下面仅以一种最优化的技术方案对本发明的技术方案以及设计原理进行详细阐述，但本发明的保
护范围并不仅限于此。
[0062] 本文涉及下列术语，为便于理解，对其含义说明如下。本领域技术人员应当理解，下列术语也可能有其它名称，但在不脱离其含义的情形下，其它任何名称都应当被认为与本文所列术语一致。
[0063] 传统基于单一传感器的障碍物检测方法在矿山环境下往往具有严重的问题，例如，摄像头在弱光照环境下曝光不足，成像效果差，因此基于摄像头的障碍物检测方法在夜间检测效果差，漏检率高，难以满足矿山夜间的弱光照运输环境使用需求；激光雷达波长较短，易受扬尘、雨、雪等悬浮物影响产生噪声数据，因此基于激光雷达的障碍物检测方法在矿山扬尘环境下极易产生误检；毫米波雷达数据纬度低、信息量少，单一基于毫米波雷达难以实现稳定的障碍物检测。因此本发明意图通过融合激光雷达与毫米波雷达数据，结合激
光雷达数据丰富、毫米波雷达不受扬尘影响与两者均不受光照影响的特性，实现矿山运输
场景下障碍物的精准、稳定检测。
[0064] 本发明实施例提供的多雷达数据融合的障碍物检测方法包括：
[0065] 步骤S1，获取多雷达数据。
[0066] 多雷达数据包括由激光雷达获取的激光雷达点云与由毫米波雷达获取的毫米波雷达障碍物检测数据。
[0067] 本实施例中，激光雷达和毫米波雷达的安装方式如图1所示，箭头所指方向为车辆行驶正方向，激光雷达(Lidar)与毫米波雷达(Radar)均安装于车辆前部，其中激光雷达安
装靠上，毫米波雷达安装靠下，二者在正方向角度差较小，二者的检测区域重合度较高，例如：毫米波雷达安装位置高于地面30cm且小于1m，激光雷达则安装于毫米波雷达上方1m至
2m的范围内，这样有利于传感器信息的融合。
[0068] 步骤S2，将所述多雷达数据中的激光雷达点云转换为矩阵化后的点云深度图。
[0069] 本实施例中，使用如下式(1)和式(2)所示的投影模型将所述多雷达数据中的激光雷达点云转换为矩阵化后的点云深度图：
[0070]
[0071]
[0072] 式中， 分别为点云投影后在点云深度图上所处位置横、纵坐标，θ0、φ0分别为点云矩阵化横、纵起始角度，(x、y、z)为激光点在以激光雷达扫描中心及方向确定的激光雷达坐标系下的三维空间坐标，Δθ、Δφ分别为点云矩阵化横、纵角度分辨率，该矩阵化分辨率可以通过参考激光雷达扫描角度分辨率确定。
[0073] 步骤S3，计算所述点云深度图上点云坡度特征，并根据连通域搜索获得非地面特征点云聚类，将单个所述连通域内特征点云数量超过点云数量阈值point_numobj_thresh的非地面点云聚类作为障碍物点云聚类，计算所述非地面特征点云聚类离地高度、连通度和表
面离散度。
[0074] 其中，点云数量阈值point_numobj_thresh根据所要检测的最小障碍物大小设置得到。
[0075] 在一个实施例中，如图3所示，步骤S3中的所述点云坡度特征α通过所述点云深度图中上、下相邻两点p1、p2形成的直线与激光雷达坐标系xy平面之间的角度获得，如式(3)所示：
[0076]
[0077] 式中，(x1，y1，z1)、(x2，y2，z2)分别为p1、p2点在激光雷达坐标系中的坐标，l1，2为p1、p2在所述xy平面上的距离，如式(4)所示。
[0078]
[0079] 于是，障碍物点云聚类中的障碍物特征点云的获取方法包括：根据所在道路路面的最大坡度角和实际调试经验可以设定阈值α0，如果大于该阈值α0即可认为该点是障碍物特征点。
[0080] 在一个实施例中，步骤S3中连通域搜索包括对所述点云深度图的上下方向和左右方向的搜索，将p1、p2之间欧式空间几何距离阈值l2thresh的计算公式设置为式(5)，将p1、p2之间的连通域聚类判断条件设置为式(6)和式(7)：
[0081]
[0082]
[0083]
[0084] 式中，k为距离阈值系数，通常根据实际调试范围为1‑2之间，β为p1、p2分别与与激光雷达的几何中心连线形成的夹角，max_d为p1、p2中与激光雷达的几何中心距离较远的点与激光雷达的几何中心之间的距离，b为预设的固定偏置系数，偏置系数通常设置为略大于激光雷达测量精度，(row1，col1)、(row2，col2)分别为p1、p2在所述点云深度图中的坐标。
[0085] 在一个实施例中，步骤S3中点云聚类离地高度为求点云聚类最低点到地面平面距离，在本方法中结合随机采样一致性算法与最小二乘法进行地面平面拟合。随机采样一致
性算法首先每次取三个点确定一个平面方程，然后找出所有与该平面距离小于阈值的点作
为该次模型内点，根据所需要的的模型精度设置迭代次数num_iter，迭代采样并确定模型
num_iter次，最终选择模型内点数量最多的所对应的平面方程作为随机采样一致性模型。
[0086] 设最小二乘法待求解地面平面方程x、y、z三轴系数分别为A、B、C，则在激光雷达坐标系下最小二乘法所求得的地面平面方程可以表示为Ax+By+Cz+1＝0，因此点与模型之间的误差可以表示为f(x，y，z)＝Ax+By+Cz+1，点云与模型误差的平方和可以表示为
(xi，yi，zi)表示第i个点在激光雷达坐标系中的坐标。
[0087] 根据导数的性质可以将求解误差平方和S最小的问题转化为求解如下方程(8)：
[0088]
[0089] 将最终随机采样一致性模型内点集代入上式中得到如下公式(9)：
[0090]
[0091] 将式(9)化简，可以得到如下表达式(10)：
[0092]
[0093] 然后找到点云聚类中z轴值最小的点，代入公式(11)即可得到点云聚类离地高度：
[0094]
[0095] 步骤S3中特征点云连通度obj_ratio为点云聚类附近非聚类特征点数目point_numno_obj与障碍物本身特征点数目point_numobj之比，如下式(12)所示，表示当前点云聚类附近无法被正确连通搜索的特征点云比例，用于表征点云聚类附近噪音点云比例。
[0096] 使用点云深度图搜索和欧式聚类结合的方式对点云聚类附近的点云进行搜索，以点云聚类在点云深度图上横纵坐标的最大值以及最小值为范围获得位于该矩形区域内的
点云，然后使用欧式聚类获得与当前聚类点云处于同一聚类的点云，最后去除其中已经被
包含在其他点云聚类的点云，最终剩余的则是点云聚类附近非聚类特征点云。则点云聚类
特征点云连通度计算公式为：
[0097]
[0098] 式中，point_numno_obj为点云聚类附近非聚类特征点数目，point_numobj为障碍物本身特征点数目。
[0099] 步骤S3中表面离散度用于表征点云聚类表面连续、平滑程度。点云聚类单个点表面离散度point_discrete的计算公式如式(13)所示：
[0100] point_discrete＝huber(|2×xmid‑xleft‑xright|)， #(13)
[0101]
[0102] 式中，xleft、xmid、xright为在点云深度图上相邻的三个点在x轴方向上的值。则点云聚类表面离散度obj_discrete可以表示为下式(15)：
[0103]
[0104] 步骤S4，将所述多雷达数据中的毫米波雷达障碍物检测数据的坐标转换到激光雷达坐标系下，并根据距离和/或交并比(英文全称为Intersection overUnion)，计算单个激光雷达的关联特征值。
[0105] 步骤S4中关联特征计算包括两部分，分别是目标级关联判断以及关联特征值计算。目标级关联判断使用如图所示的激光雷达目标与毫米波雷达目标的一对多关联算法，
关联策略包含重叠判断及距离判断两种。若激光雷达目标与毫米波雷达目标产生重叠，则
可直接认为两者成功关联，若两目标未发生重叠，则设置关联距离阈值为connect_
distance_thresh，若毫米波雷达目标与激光雷达目标之间的距离小于阈值connect_
distance_thresh，则认为该毫米波雷达目标与该激光雷达目标成功关联，该关联距离阈值通常设置为所要检测的最小障碍物大小的两到三倍。
[0106] 对关联成功的激光雷达与毫米波雷达，根据激光雷达目标与毫米波雷达目标之间的交并比，如下式(16)所示，计算单个激光雷达的关联特征值single_relevancy：
[0107]
[0108] 式中，rectA表示激光雷达的目标矩形框，rectB表示毫米波雷达的目标矩形框，∩表示交集，∪表示并集，rectC表示能够将rectA、rectB包围的最小矩形框。
[0109] 在一个实施例中，激光雷达目标可能有多个毫米波雷达目标与其关联，因此单个激光雷达的关联特征值obj_relevancy可以统一表示为下式(17)：
[0110]
[0111]
[0112] 式中，numobj_rele为与当前激光雷达目标成功关联的毫米波雷达目标数量，single_relevancyn表示第n个激光雷达的关联特征值。对于无毫米波雷达目标与之成功关联的激光雷达目标，其关联特征值为0。
[0113] 步骤S5，计算点云聚类为灰尘点云聚类的概率，并获得非扬尘障碍物的点云聚类位置。
[0114] 在一个实施中，利用下式(19)计算该点云聚类属于扬尘点云聚类的概率dust_probability：
[0115] dust_probability＝k1×obj_height+k2×obj_ratio+k3×obj_discrete+k4×obj_relevancy， #(19)
[0116] 式中，k1、k2、k3、k4分别为不同特征的缩放系数，根据不同特征在扬尘判断中的重要程度设置不同特征的缩放系数，并将最终概率值缩放到1附近，obj_height、obj_ratio、obj_discrete为通过所述步骤S3获得的所述非地面特征点云聚类离地高度、连通度和表面离散度，obj_relevancy为单个激光雷达的关联特征值。
[0117] 需要说明的是，也可以将式(19)中的一次函数替换成二次函数或三次函数，实现特征的统一。
[0118] 本发明实施例还提供一种多雷达数据融合的障碍物检测系统，其包括雷达数据获取单元、点云深度图转换单元、点云特征计算单元、关联特征计算单元和概率计算单元，其中：
[0119] 雷达数据获取单元用于获取多雷达数据。
[0120] 点云深度图转换单元用于将所述多雷达数据中的激光雷达点云转换为矩阵化后的点云深度图。
[0121] 点云特征计算单元用于计算所述点云深度图上点云坡度特征，并根据连通域搜索获得非地面特征点云聚类，将单个所述连通域内特征点云数量超过阈值的点云聚类作为障
碍物点云聚类，计算所述非地面特征点云聚类离地高度、连通度和表面离散度。
[0122] 关联特征计算单元用于将所述多雷达数据中的毫米波雷达障碍物检测数据的坐标转换到激光雷达坐标系下，并根据距离和/或交并比，计算单个激光雷达的关联特征值。
[0123] 概率计算单元用于计算点云聚类为灰尘点云聚类的概率，并获得非扬尘障碍物的点云聚类位置。
[0124] 所述点云特征计算单元中的所述点云坡度特征通过所述点云深度图中上、下相邻两点p1、p2形成的直线与激光雷达坐标系xy平面之间的角度α获得，如式(3)所示：
[0125]
[0126] 式中，(x1，y1，z1)、(x2，y2，z2)分别为p1、p2点在激光雷达坐标系中的坐标，l1，2为p1、p2在所述xy平面上的距离。
[0127] 所述点云特征计算单元中连通域搜索包括对所述点云深度图的纵向和横向两个方向的搜索，将p1、p2之间欧式空间几何距离阈值l2thresh的计算公式设置为式(5)，将两障碍物特征点之间的连通域聚类判断条件设置为式(6)和式(7)：
[0128]
[0129]
[0130]
[0131] 式中，k为距离阈值系数，β为p1、p2分别与激光雷达原点连线形成的夹角，max_d为p1、p2中与激光雷达距离较远的点与激光雷达原点之间的距离，b为预设的固定偏置系数，(row1，col1)、(row2，col2)分别为p1、p2在所述点云深度图中的坐标。
[0132] 所述概率计算单元中，利用下式(19)计算该点云聚类属于扬尘点云聚类的概率dust_probability：
[0133] dust_probability＝k1×obj_height+k2×obj_ratio+k3×obj_discrete+k4×obj_relevancy， #(19)
[0134] 式中，k1、k2、k3、k4分别为不同特征的缩放系数，obj_height、obj_ratio、obj_discrete为通过所述步骤S3获得的所述非地面特征点云聚类离地高度、连通度和表面离散度，obj_relevancy为单个激光雷达的关联特征值。
[0135] 步骤S7中将dust_probability小于1的点云聚类作为真实障碍物点云聚类进行输出。
[0136] 现有发明或依赖单一传感器不够稳定，或无法有效应对扬尘问题，或无法工作于夜间环境，因此本发明融合激光雷达与毫米波雷达的不同特性，提出了一种基于一种多雷
达数据融合的扬尘场景下的障碍物检测方法与系统，通过特征计算以及传感器回波特性融
合实现了扬尘场景下的障碍物稳定、精确检测，且在夜间也能正常工作，对于实现矿山场景下的无人驾驶具有重要意义，对于城市环境下雨雪、扬尘场景的自动驾驶也具有重要意义。
[0137] 本发明除了适用于矿用自卸车，还可以适用于如卡车、挖掘机等其他运载装备或工程车辆。应用场景除了矿山外，也可应用于如城市、乡村等其他环境下，同样具有应对扬尘、雨雪等场景的功能。
[0138] 最后需要指出的是：以上实施例仅用以说明本发明的技术方案，而非对其限制。本领域的普通技术人员应当理解：可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；这些修改或者替换，并不使相应技术方案的本质脱离本
发明各实施例技术方案的精神和范围。
