CN117192513A - 一种雪天环境下路侧激光雷达背景过滤方法 - Google Patents

Abstract

本发明涉及激光雷达目标探测技术领域，具体为一种雪天环境下路侧激光雷达背景过滤方法。该方法主要由地面点过滤、雪花点过滤两部分组成；通过地面点过滤、雪花点过滤的逐步实现，完成对降雪天气下激光雷达点云数据的处理，使得获取的点云数据更加清晰的显示道路中所需的目标，缩短目标检测中处理干扰数据的时间，提高获取检测目标信息的准确性，取得更好的检测效果，提高道路交通的安全性及通行效率。

Description

一种雪天环境下路侧激光雷达背景过滤方法

技术领域

本发明涉及激光雷达目标探测技术领域，具体为一种雪天环境下路侧激光雷达背景过滤方法。

背景技术

复杂的降雪天气会在对交通安全产生两个方面的负面影响：影响视线和增加控制汽车的难度。随着交通领域的快速发展，智能网联汽车在雪天条件下的交通安全性要求越来越高。然而，如何准确地获取道路上的交通数据是目前研究的一个难题。这些数据的质量会受到雪天的影响，从而影响交通安全，引发安全事故。因此，研究如何提高雪天下交通数据的准确性至关重要。

激光雷达作为一种高精度传感器设备，可以在360度水平视野(FOV)中检测所有道路使用者和周围环境。与传统的传感器，如摄像头、环路探测器、雷达相比，激光雷达可以昼夜工作，具有更高的目标检测精度。激光雷达的工作原理与雷达相似。它传输和接收激光束，测量激光束的时间差和相位差，确定物体的三维坐标，生成点云数据，对周围环境进行点云成像。

在雪天下，激光雷达发射的激光会被雪花或空气中的任何颗粒吸收、衍射、反向散射或折射，导致点云数据测量不准确，激光雷达的感知能力会受到一定程度的限制。因此，在雪天等天气条件下，如何对获取的路侧激光雷达点云数据进行降噪处理，以得到去除噪声后的点云数据并进行精准的目标识别是亟待解决的技术问题。

发明内容

本发明提出了一种雪天环境下路侧激光雷达背景过滤方法，该方法主要由地面点过滤、雪花点过滤两部分组成，如图1所示；通过地面点过滤、雪花点过滤的逐步实现，完成对降雪天气下激光雷达点云数据的处理，使得获取的点云数据更加清晰的显示道路中所需的目标，缩短目标检测中处理干扰数据的时间，提高获取检测目标信息的准确性，取得更好的检测效果，提高道路交通的安全性及通行效率。

本发明的技术解决方案是首先对于所获取的点云数据进行地面点滤除，再对数据中的雪花点进行处理；地面点滤除主要用于对雪天中由于降雪而造成的凹凸不平的地面进行处理，凹凸不平的地面会使后续的检测工作难度加大，造成较高的误检率，在地面点滤除模块，通过提前处理复杂的地面情况，精确判断出雪堆凸起出的异常，在雪地中构建准确的平面来拟合地面；雪花点过滤主要用于雪天环境中的雪花点滤除处理，雪花点在空气中飘动，容易影响驾驶员的视线，对激光雷达的工作产生影响，在本模块中，对空间点密度进行计算，采用雪花噪声、行人、车辆点的分类框架，计算三种点云空间分布密度，以此滤除雪花噪声，对环境进行高精度分析。

本发明提出的一种雪天环境下路侧激光雷达背景过滤方法，其特征主要包括：

1)地面点过滤

地面点滤除是雪天点云数据滤除的第一步，主要由数据导入单元、地面分析单元、平面拟合单元、地物分离单元组成。由于雪天的环境下，雪花在低温的条件下落在地面不会立即融化。雪量较大的情况下，地面上会形成雪花的堆积，因此会造成地面的凹凸程度变化。数据导入单元用于将路侧激光雷达采集后的原始数据导入计算机，并在计算机中将数据格式变更为计算机计算的数据类型；数据分析单元用于将环境中的点云逐一分析，判断其数据类型属于地面点或是目标点；平面拟合单元用于分析完数据类型后，计算剩余点云与拟合平面的距离与阈值进行比较，确定是否属于拟合平面；地物分离单元用于拟合完成后的平面与目标物体的分离，使后续目标识别工作准确进行。

2)雪花点过滤

雪花点过滤是雪天点云数据滤除的第二步，主要由数据导入单元、降雪分析单元、混合滤波单元、雪物分离单元组成。在雪天环境中，雪花在环境中飘扬，小雪、中雪、大雪不同天气情况会对驾驶员造成不同程度视线干扰，容易造成安全隐患。数据导入单元用于将前一步中处理数据导入雪花点过滤步骤，并进行后续操作；降雪分析单元用于分析非地面点中的空间密度不同的目标，进行空间上目标物体的密度计算，并通过密度阈值判断降雪程度；混合滤波单元用于将环境中的雪花点密度通过降雪程度设定自适应阈值进行滤除；雪物分离单元用于将雪花点与目标点分离，便于目标识别工作准确进行。

附图说明

图1：一种雪天环境下路侧激光雷达背景过滤方法示意图。

具体实施方式

本发明提出了一种基于路侧激光雷达的雪天点云数据背景过滤方法，该方法主要由地面点滤除、雪花点滤除两部分组成；通过地面点滤除、雪花点滤除对于环境中的相关噪声点进行处理，解决了环境中由于降雪造成的视觉影响，使智能骑车能够通过路侧激光雷达中的原始数据分析判断行驶路线上的雪花障碍物，减少由于不平整的地面或飘扬的雪花造成的误检、漏检问题，缩短判断及处理点云的时间，解决了雪花干扰视线的难题，有效提升点云数据的处理精度与可靠性，提高雪天条件下的交通安全性，提高城市道路交通的通行效率。

本发明所述的一种雪天环境下路侧激光雷达背景过滤方法，其工作的具体流程为：

1)地面点滤除

地面点滤除是雪天点云数据滤除的第一步，主要由数据导入单元、地面分析单元、平面拟合单元、地物分离单元组成。由于雪天的环境下，雪花在低温的条件下落在地面不会立即融化。雪量较大的情况下，地面上会形成雪花的堆积，因此会造成地面的凹凸程度变化。数据导入单元用于将路侧激光雷达采集后的原始数据导入计算机，并在计算机中将数据格式变更为计算机计算的数据类型；数据分析单元用于将环境中的点云逐一分析，判断其数据类型属于地面点或是目标点；平面拟合单元用于分析完数据类型后，计算剩余点云与拟合平面的距离与阈值进行比较，确定是否属于拟合平面；地物分离单元用于拟合完成后的平面与目标物体的分离，使后续目标识别工作精准进行；具体工作步骤如下：

Step1：地面点滤除模块启动数据导入单元，路侧激光雷达主要置于城市道路的路侧区域，以采集道路交通中的多项数据；数据导入单元将路侧激光雷达中的原始.pcap数据分帧以.cav格式存储于计算机中，根据单帧点云数据读取.csv文件，文件编码UTF-8，分别确定单帧点云中不同点云位置，以提取点云坐标Pointcloud_i_X、Pointcloud_i_Y、Pointcloud_i_Z，创建点云对象存储于矩阵P_n中，并将其保存为.pcd格式文件：

其中：P_n为存储第N帧点云的矩阵；Pointcloud_n_X₁、Pointcloud_n_Y₁、Pointcloud_n_Z₁分别为第N帧原始点云数据的X轴坐标、Y轴坐标和Z轴坐标；X轴方向为以激光雷达为坐标原点，以正东方向为正方向，正西方向为负方向；Y轴方向为以激光雷达为坐标原点，以正南方向为正方向，正北方向为负方向；Z轴方向为以激光雷达为坐标原点，以正上方向为正方向，正下方向为负方向；Pointcloud_n_X₁、Pointcloud_n_Y₁、Pointcloud_n_Z₁分别为激光雷达于第N帧数据中第一束激光线束发射后返回的第一个激光点的三维坐标；Pointcloud_n_X_i、Pointcloud_n_Y_i、Pointcloud_n_Z_i第N帧数据中最后一束激光线束发射后返回的第i个激光点的三维坐标；

Step2：地面点滤除在完成数据导入模块后，启动地面分析单元；地面分析单元通过识别路侧激光雷达在道路路侧三维激光成像扫描得到的数据，分析其数据类型，判断数据属于地面点或是目标点；

Step2.1：通过RANSAC算法分析地面点时，对于第N帧数据，随机选取3个点a(Pointcloud_n_X_a,Pointcloud_n_Y_a,Pointcloud_n_Z_a)，

b(Pointcloud_n_X_b,Pointcloud_n_Y_b,Pointcloud_n_Z_b)，

c(Pointcloud_n_X_c,Pointcloud_n_Y_c,Pointcloud_n_Z_c)计算得到初始平面Plan_n_0，计算第i个3D激光点(Pointcloud_n_X_i,Pointcloud_n_Y_i,Pointcloud_n_Z_i)与初始平面Plan_n_0的距离Distance₀_i，如果3D激光点与初始平面Plan_n_0的距离Distance₀_i≤ξ，则判定该3D激光点属于地面点Groudpoints_i，将该3D激光点初步归入地面点集GPS_0:

Plan_n_0:Ax+By+Cz+D＝0

A＝(Y_b-Y_a)*(Z_c-Z_a)-(Z_b-Z_a)*(Y_c-Y_a)

B＝(Z_b-Z_a)*(X_c-X_a)-(X_b-X_a)*(Z_c-Z_a)

C＝(X_b-X_a)*(Y_c-Y_a)-(Y_b-Y_a)*(X_c-X_a)

D＝-AX_a-BY_a-CZ_a

GPCS_0＝[Groudpoints₁,Groudpoints₂,...,Groudpoints_i]

其中：X_a、Y_a、Z_a、X_b、Y_b、Z_b、X_c、Y_c、Z_c分别为随机三点a点、b点、c点的X轴坐标、Y轴坐标、Z轴坐标；A、B、C、D为初始平面Plan_n_0的相关系数；为平面Plan_n_0的法向量；d(X₀,Y₀,Z₀)为平面Plan_n_0的任意一点；/>为第i点至d点的方向向量；ξ为判定i点属于地面点或目标点的阈值；

Step2.2：如果3D激光点与初始平面Plan_n_0的距离Distance₀_i>ξ，则判定该3D激光点属于目标点Nongroudpoints_i，将该3D激光点初步归入非地面点集{NGPS_0}：

NGPS_0＝[Nongroudpoints₁,Nongroudpoints₂,...,Nongroudpoints_i]

Step2.3：计算第n帧中所有目标点Nongroudpoints_i在点云数据中的比例η，并确保每次迭代通过采样至少一个“外部点”来计算模型的概率P：

P＝1-(1-ηⁿ)^k

其中：n_{Nongroudpoints}为该帧点云数据中目标点数量；n_Groudpoints为该帧点云数据中地面点数量；k为最小迭代次数；

Step3：由于下雪的环境，雪花在低温下下落到地面，不会立即融化。当积雪较多时，地面上形成积雪，导致地面水平发生变化。在地面点滤除模块中，地面分析单元结束后，启动平面拟合单元；平面拟合单元通过计算出点云的法向量，并根据原始算法对点云数据进行初始基点提取。通过遍历最初提取的地面点，计算平均高程，并以计算得到的平均高程作为阈值，得到目标点集，然后从目标点集中最远选择1个点作为种子点，剩余的2个种子点通过最远点采样(FPS)算法进行确定，通过平面模型进行最远拟合。通过遍历点云数据(不包括随机采样的3个点)计算每个点到拟合平面的距离，并将计算出的距离和预设数据与平面模型的最大可接受差值进行比较，确定点云数据类型为地面点或目标点。

Step3.1：求解点云法向量，利用KD-Tree将点云空间划分为不同的区域；根据邻域半径集，得到邻域点集Q_n{(x₁,y₁,z₁),(x₂,y₂,z₂),...,(x_m,y_m,z_m)}；先计算点集的样本均值然后计算点集的协方差矩阵C，最后计算协方差矩阵的特征值和特征向量；最小特征值对应的特征向量是邻域点集的局部法向量：

其中：为x轴，m为第n帧点云数据中点的数量；

Step3.2：由于利用法向量获取的目标点云中仍包含部分地面点，故利用点云高程信息对点云数据进行二次提取。遍历点云数据获取每个点的高程值并将其存放在一个空列表中，求出点云数据的平均高程h，并将h作为第一次滤波的阈值；再次遍历所有点云数据，对比每个点的高程与阈值h的关系，若某点的高程大于h，则判定该点为目标点，若小于h，则判定该点为地面点；

Step3.3：通过点云的平均高程无法将地面点完全滤除，故需要拟合出最优平面模型来滤除更加精确的地面点云。从点云中随机选取1个点作为种子点，利用FPS算法选取剩余2个种子点。假设数据集S＝{p₁,p₂,…,p_L}中共有L个点，在S中随机选取1个点p_i作为第1个种子点；计算剩余n-1个点与p_i点的距离，选择离p_i最远的点p_j作为第2个种子点，将p_i与p_j写入点集S₁＝{p_i,p_j}；选取剩余n-2个点计算其与点集S₁中每个点的距离，将最短距离作为该点到点集的距离，可得到n-2个到点集的距离，选取最远的点p_k作为第3个种子点写入点集S₁，此时p_i＝{p_i,p_j,p_k}；利用点集S₁中的种子点拟合平面模型，通过迭代的方式拟合出最优平面模型，从而精确滤除地面点，获取目标点；

Step4：当平面拟合单元数次循环结束后，启动地物分离单元接收平面拟合单元中点集{S₁,S₂,...,S_n}的所有点，并将其中的所有点都纳入地面点部分，将初始点集{P₁,P₂,...,P_n}中的剩余点纳入目标点部分，并对地面点部分和目标点部分进行3D显示；

Step4.1：如果任意第i个3D激光点属于集合{S₁,S₂,...,S_n}，则标记为Groudpoints_{n_f_i}，其中f为迭代次数，并且Groudpoints_{n_f_i}∈{GDCS_n}，其中{GDCS_n}为第n帧数据的地面点集；

Step4.2：如果任意第n帧点云数据中的第i个3D激光点属于集合则标记为Nongroudpoints_{n_f_i}，其中f为迭代次数，并且Nongroudpoints_{n_f_i}∈NGDS_n，其中NGDS_n为第n帧数据的目标点集；

Step4.5：当初始点集{P₁,P₂,...,P_n}中的所有点经过判断后，分别标记为Groudpoints_{n_f_i}、Nongroudpoints_{n_f_i}，并分帧存储在点集{GDCS_n,NGDS_n}中，通过open3d中的point_cloud_viewer进行逐帧可视化显示；

其中：open3d为点云数据处理的一个开源库，支持快速开发和处理3D点数据；point_cloud_viewer为open3d库中的显示3D点云的函数，point_cloud_viewer支持对点云数据试图进行移动缩放、平移等操作；

2)雪花点滤除

雪花点过滤是雪天点云数据滤除的第二步，主要由数据导入单元、降雪分析单元、混合滤波单元、雪物分离单元组成。在雪天环境中，雪花在环境中飘扬，小雪、中雪、大雪不同天气情况会对驾驶员造成不同程度视线干扰，容易造成安全隐患。数据导入单元用于将前一步中处理数据导入雪花点过滤步骤，并进行后续操作；统计滤波单元用于分析非地面点中的空间密度不同的目标，进行空间上目标物体的密度计算，并通过密度阈值判断降雪程度；半径滤波单元用于将环境中的雪花点密度通过降雪程度设定自适应阈值进行滤除；雪物分离单元用于将雪花点与目标点分离；具体工作步骤如下：

Step1：雪花点过滤模块启动数据导入单元，数据导入单元分帧导入地面点滤除单元处理后的地面点集{NGDS_n}，并创建点云对象，将其保存为.pcd格式文件；

Step2：数据导入单元将地面点集{NGDS_n}传入数据分析单元，首先，根据环境中所有点的空间分布特性，判断点类型为雪花点或目标点；再根据判定后雪花点数据，计算环境中雪花密度，判断降雪程度，将降雪程度分为小雪(Light)、中雪(Moderate)、大雪(Heavy)三等；

Step2.1：在道路环境中，距离地面3m以上的环境中很少存在目标物体，但在雪天中，该空间分布的点云数据雪花点占比高，因此通过高程滤波，选取3m为高程阈值，计算该空间内雪花点密度ρ_{n_snow}：

其中：n_{snowpointclouds_n}表示当前帧雪花点的数量；n_{Nongroudpoints_n}表示当前帧非雪花点的数量；

Step2.2：如果地面点集{NGDS_n}中第i个Nongroudpoints_{n_i}点的高程z_{n_i}≥3，则判断该点属于雪花点集{SPCS_n}：

SPCS_n＝[snowpointcloud₁,snowpointcloud₂,...,snowpointcloud_i]

其中：snowpointcloud_i表示第n帧数据中的第i个雪花点；

Step2.3：如果点集{NGDS_n}中第i个Nongroudpoints_{n_i}点的高程z_{n_i}<3，则判断该点属于非雪花点集{NSPCS_n}：

NSPCS_n

＝[Nonsnowpointcloud₁,Nonsnowpointcloud₂,...,Nonsnowpointcloud_i]

其中：Nonsnowpointcloud_i表示第n帧数据中的第i个非雪花点；

Step2.4：当获取雪花点集{SPCS_n}后，计算当前帧ρ_{n_snow}，如果0<ρ_{n_snow}≤ξ₁，则判断当前帧降雪程度为小雪(Light)，如果ξ₁<ρ_{n_snow}≤ξ₂，则判断当前帧降雪程度为中雪(Moderate)，如果ρ_{n_snow}>ξ₂，则判断当前帧降雪程度为大雪(Heavy)；

其中：ξ₁为小雪和中雪的强度阈值，ξ₂为中雪和大雪的强度阈值；

Step3：数据分析单元判断降雪程度后，将不同帧降雪程度分别传入混合滤波单元，混合滤波单元通过自适应统计滤波、自适应半径滤波对该帧点云进行雪花降噪处理；

Step3.1：基于点云特征，对于输入点云中的每一个点Nongroudpoint_i，其领域内各点的平均距离：

假设服从高斯分布，可进一步求得点云空间中的N个点对应的均值μ和标准差σ为：

其中：k为设置的最小邻近点的搜索数量，d_i为在k构建的邻域内Nongroudpoint_i到各点的距离；

Step3.2：根据均值μ和标准差σ，分别设置统计滤波最小邻近点的搜索数量k₁、k₂、k₃和标准差系数V_{σ_1}、V_{σ_2}、V_{σ_3}，根据k₁、k₂、k₃和V_{σ_1}、V_{σ_2}、V_{σ_3}计算阈值范围(μ-σ·V_{σ_1},μ+σ·V_{σ_1})、(μ-σ·V_{σ_2},μ+σ·V_{σ_2})、(μ-σ·V_{σ_3},μ+σ·V_{σ_3})，滤除部分噪点，将滤除的雪花点存入雪花点集{SPCS_statistic_n_1}、{SPCS_statistic_n_2}、{SPCS_statistic_n_3}，并将滤除后剩余的目标点存入目标点集{NSPCS_statistic_n_1}、{NSPCS_statistic_n_2}、{NSPCS_statistic_n_3}；

其中：k₁、k₂、k₃分别为小雪、中雪、大雪情况下统计滤波最小邻近点的搜索数量；V_{σ_1}、V_{σ_2}、V_{σ_3}分别为小雪、中雪、大雪情况下统计滤波标准差系数；{SPCS_statistic_n_1}、{SPCS_statistic_n_2}、{SPCS_statistic_n_3}分别为小雪、中雪、大雪情况下自适应统计滤波后的雪花点集；{NSPCS_statistic_n_1}、{NSPCS_statistic_n_2}、{NSPCS_statistic_n_3}分别为小雪、中雪、大雪情况下自适应统计滤波后的目标点集；

Step3.3：将目标点集{NSPCS_statistic_n_1}、{NSPCS_statistic_n_2}、{NSPCS_statistic_n_3}分别传入自适应半径滤波算法，对于不同帧点云的不同降雪程度，针对该帧数据中的每一个点Nongroudpoint_i，设置自适应的滤波半径r₁、r₂、r₃，构建球形搜索范围，若范围内点的数量m小于设定的最少临近点数量M，则判定该Nongroudpoint_i为雪花噪声点，存储为新的点集{SPCS_radius_n_1}、{SPCS_radius_n_2}、{SPCS_radius_n_3}，予以滤除，反之予以保留，并存入新的点集{NSPCS_radius_n_1}、{NSPCS_radius_n_2}、{NSPCS_radius_n_3}；

其中：{SPCS_radius_n_1}、{SPCS_radius_n_2}、{SPCS_radius_n_3}为小雪、中雪、大雪情况下自适应半径滤波后的雪花点集；{NSPCS_radius_n_1}、{NSPCS_radius_n_2}、{NSPCS_radius_n_3}为小雪、中雪、大雪情况下自适应半径滤波后的非雪点集；

Step4：当混合滤波单元完成后，计算机将{SPCS_statistic_n_1}、{SPCS_statistic_n_2}、{SPCS_statistic_n_3}与{SPCS_radius_n_1}、{SPCS_radius_n_2}、{SPCS_radius_n_3}进行合并，并存储为新的滤波后点集{SPCS_afterfilter_n_1}、{SPCS_afterfilter_n_2}、{SPCS_afterfilter_n_3}，与{NSPCS_radius_n_1}、{NSPCS_radius_n_2}、{NSPCS_radius_n_3}通过point_cloud_viewer进行逐帧可视化显示。

Claims (1)

1.一种雪天环境下路侧激光雷达背景过滤方法，其特征主要包括：

1)地面点滤除

地面点滤除是雪天点云数据滤除的第一步，主要由数据导入单元、地面分析单元、平面拟合单元、地物分离单元组成，由于雪天的环境下，雪花在低温的条件下落在地面不会立即融化；雪量较大的情况下，地面上会形成雪花的堆积，因此会造成地面的凹凸程度变化；数据导入单元用于将路侧激光雷达采集后的原始数据导入计算机，并在计算机中将数据格式变更为计算机计算的数据类型；数据分析单元用于将环境中的点云逐一分析，判断其数据类型属于地面点或是目标点；平面拟合单元用于分析完数据类型后，计算剩余点云与拟合平面的距离与阈值进行比较，确定是否属于拟合平面；地物分离单元用于拟合完成后的平面与目标物体的分离，使后续目标识别工作精准进行；具体工作步骤如下：

Step1：地面点滤除模块启动数据导入单元，路侧激光雷达主要置于城市道路的路侧区域，以采集道路交通中的多项数据；数据导入单元将路侧激光雷达中的原始.pcap数据分帧以.cav格式存储于计算机中，根据单帧点云数据读取.csv文件，文件编码UTF-8，分别确定单帧点云中不同点云位置，以提取点云坐标Pointcloud_i_X、Pointcloud_i_Y、Pointcloud_i_Z，创建点云对象存储于矩阵P_n中，并将其保存为.pcd格式文件：

其中：P_n为存储第N帧点云的矩阵；Pointcloud_n_X₁、Pointcloud_n_Y₁、Pointcloud_n_Z₁分别为第N帧原始点云数据的X轴坐标、Y轴坐标和Z轴坐标；X轴方向为以激光雷达为坐标原点，以正东方向为正方向，正西方向为负方向；Y轴方向为以激光雷达为坐标原点，以正南方向为正方向，正北方向为负方向；Z轴方向为以激光雷达为坐标原点，以正上方向为正方向，正下方向为负方向；Pointcloud_n_X₁、Pointcloud_n_Y₁、Pointcloud_n_Z₁分别为激光雷达于第N帧数据中第一束激光线束发射后返回的第一个激光点的三维坐标；Pointcloud_n_X_i、Pointcloud_n_Y_i、Pointcloud_n_Z_i第N帧数据中最后一束激光线束发射后返回的第i个激光点的三维坐标；

Step2：地面点滤除在完成数据导入模块后，启动地面分析单元；地面分析单元通过识别路侧激光雷达在道路路侧三维激光成像扫描得到的数据，分析其数据类型，判断数据属于地面点或是目标点；

Step2.1：通过RANSAC算法分析地面点时，对于第N帧数据，随机选取3个点a(Pointcloud_n_X_a,Pointcloud_n_Y_a,Pointcloud_n_Z_a)，

b(Pointcloud_n_X_b,Pointcloud_n_Y_b,Pointcloud_n_Z_b)，

c(Pointcloud_n_X_c,Pointcloud_n_Y_c,Pointcloud_n_Z_c)计算得到初始平面Plan_n_0，计算第i个3D激光点(Pointcloud_n_X_i,Pointcloud_n_Y_i,Pointcloud_n_Z_i)与初始平面Plan_n_0的距离Distance₀_i，如果3D激光点与初始平面Plan_n_0的距离Distance₀_i≤ξ，则判定该3D激光点属于地面点Groudpoints_i，将该3D激光点初步归入地面点集GPS_0:

Plan_n_0:Ax+By+Cz+D＝0

A＝(Y_b-Y_a)*(Z_c-Z_a)-(Z_b-Z_a)*(Y_c-Y_a)

B＝(Z_b-Z_a)*(X_c-X_a)-(X_b-X_a)*(Z_c-Z_a)

C＝(X_b-X_a)*(Y_c-Y_a)-(Y_b-Y_a)*(X_c-X_a)

D＝-AX_a-BY_a-CZ_a

GPCS_0＝[Groudpoints₁,Groudpoints₂,...,Groudpoints_i]

其中：X_a、Y_a、Z_a、X_b、Y_b、Z_b、X_c、Y_c、Z_c分别为随机三点a点、b点、c点的X轴坐标、Y轴坐标、Z轴坐标；A、B、C、D为初始平面Plan_n_0的相关系数；为平面Plan_n_0的法向量；d(X₀,Y₀,Z₀)为平面Plan_n_0的任意一点；/>为第i点至d点的方向向量；ξ为判定i点属于地面点或目标点的阈值；

Step2.2：如果3D激光点与初始平面Plan_n_0的距离Distance₀_i>ξ，则判定该3D激光点属于目标点Nongroudpoints_i，将该3D激光点初步归入非地面点集{NGPS_0}：

NGPS_0＝[Nongroudpoints₁,Nongroudpoints₂,...,Nongroudpoints_i]

Step2.3：计算第n帧中所有目标点Nongroudpoints_i在点云数据中的比例η，并确保每次迭代通过采样至少一个“外部点”来计算模型的概率P：

P＝1-(1-ηⁿ)^k

其中：n_{Nongroudpoints}为该帧点云数据中目标点数量；n_Groudpoints为该帧点云数据中地面点数量；k为最小迭代次数；

Step3：由于下雪的环境，雪花在低温下下落到地面，不会立即融化；当积雪较多时，地面上形成积雪，导致地面水平发生变化；在地面点滤除模块中，地面分析单元结束后，启动平面拟合单元；平面拟合单元通过计算出点云的法向量，并根据原始算法对点云数据进行初始基点提取；通过遍历最初提取的地面点，计算平均高程，并以计算得到的平均高程作为阈值，得到目标点集，然后从目标点集中最远选择1个点作为种子点，剩余的2个种子点通过最远点采样(FPS)算法进行确定，通过平面模型进行最远拟合；通过遍历点云数据(不包括随机采样的3个点)计算每个点到拟合平面的距离，并将计算出的距离和预设数据与平面模型的最大可接受差值进行比较，确定点云数据类型为地面点或目标点；

Step3.1：求解点云法向量，利用KD-Tree将点云空间划分为不同的区域；根据邻域半径集，得到邻域点集Q_n{(x₁,y₁,z₁),(x₂,y₂,z₂),...,(x_m,y_m,z_m)}；先计算点集的样本均值然后计算点集的协方差矩阵C，最后计算协方差矩阵的特征值和特征向量；最小特征值对应的特征向量是邻域点集的局部法向量：

其中：为x轴，m为第n帧点云数据中点的数量；

Step3.2：由于利用法向量获取的目标点云中仍包含部分地面点，故利用点云高程信息对点云数据进行二次提取；遍历点云数据获取每个点的高程值并将其存放在一个空列表中，求出点云数据的平均高程h，并将h作为第一次滤波的阈值；再次遍历所有点云数据，对比每个点的高程与阈值h的关系，若某点的高程大于h，则判定该点为目标点，若小于h，则判定该点为地面点；

Step3.3：通过点云的平均高程无法将地面点完全滤除，故需要拟合出最优平面模型来滤除更加精确的地面点云；从点云中随机选取1个点作为种子点，利用FPS算法选取剩余2个种子点；假设数据集S＝{p₁,p₂,…,p_L}中共有L个点，在S中随机选取1个点p_i作为第1个种子点；计算剩余n-1个点与p_i点的距离，选择离p_i最远的点p_j作为第2个种子点，将p_i与p_j写入点集S₁＝{p_i,p_j}；选取剩余n-2个点计算其与点集S₁中每个点的距离，将最短距离作为该点到点集的距离，可得到n-2个到点集的距离，选取最远的点p_k作为第3个种子点写入点集S₁，此时p_i＝{p_i,p_j,p_k}；利用点集S₁中的种子点拟合平面模型，通过迭代的方式拟合出最优平面模型，从而精确滤除地面点，获取目标点；

Step4：当平面拟合单元数次循环结束后，启动地物分离单元接收平面拟合单元中点集{S₁,S₂,...,S_n}的所有点，并将其中的所有点都纳入地面点部分，将初始点集{P₁,P₂,...,P_n}中的剩余点纳入目标点部分，并对地面点部分和目标点部分进行3D显示；

Step4.1：如果任意第i个3D激光点属于集合{S₁,S₂,...,S_n}，则标记为Groudpoints_{n_f_i}，其中f为迭代次数，并且Groudpoints_{n_f_i}∈{GDCS_n}，其中{GDCS_n}为第n帧数据的地面点集；

Step4.2：如果任意第n帧点云数据中的第i个3D激光点属于集合则标记为Nongroudpoints_{n_f_i}，其中f为迭代次数，并且Nongroudpoints_{n_f_i}∈NGDS_n，其中NGDS_n为第n帧数据的目标点集；

Step4.5：当初始点集{P₁,P₂,...,P_n}中的所有点经过判断后，分别标记为Groudpoints_{n_f_i}、Nongroudpoints_{n_f_i}，并分帧存储在点集{GDCS_n,NGDS_n}中，通过open3d中的point_cloud_viewer进行逐帧可视化显示；

其中：open3d为点云数据处理的一个开源库，支持快速开发和处理3D点数据；point_cloud_viewer为open3d库中的显示3D点云的函数，point_cloud_viewer支持对点云数据试图进行移动缩放、平移等操作；

2)雪花点滤除

雪花点过滤是雪天点云数据滤除的第二步，主要由数据导入单元、降雪分析单元、混合滤波单元、雪物分离单元组成；在雪天环境中，雪花在环境中飘扬，小雪、中雪、大雪不同天气情况会对驾驶员造成不同程度视线干扰，容易造成安全隐患；数据导入单元用于将前一步中处理数据导入雪花点过滤步骤，并进行后续操作；统计滤波单元用于分析非地面点中的空间密度不同的目标，进行空间上目标物体的密度计算，并通过密度阈值判断降雪程度；半径滤波单元用于将环境中的雪花点密度通过降雪程度设定自适应阈值进行滤除；雪物分离单元用于将雪花点与目标点分离；具体工作步骤如下：

Step1：雪花点过滤模块启动数据导入单元，数据导入单元分帧导入地面点滤除单元处理后的地面点集{NGDS_n}，并创建点云对象，将其保存为.pcd格式文件；

Step2：数据导入单元将地面点集{NGDS_n}传入数据分析单元，首先，根据环境中所有点的空间分布特性，判断点类型为雪花点或目标点；再根据判定后雪花点数据，计算环境中雪花密度，判断降雪程度，将降雪程度分为小雪(Light)、中雪(Moderate)、大雪(Heavy)三等；

Step2.1：在道路环境中，距离地面3m以上的环境中很少存在目标物体，但在雪天中，该空间分布的点云数据雪花点占比高，因此通过高程滤波，选取3m为高程阈值，计算该空间内雪花点密度ρ_{n_snow}：

其中：n_{snowpointclouds_n}表示当前帧雪花点的数量；n_{Nongroudpoints_n}表示当前帧非雪花点的数量；

Step2.2：如果地面点集{NGDS_n}中第i个Nongroudpoints_{n_i}点的高程z_{n_i}≥3，则判断该点属于雪花点集{SPCS_n}：

SPCS_n＝[snowpointcloud₁,snowpointcloud₂,...,snowpointcloud_i]

其中：snowpointcloud_i表示第n帧数据中的第i个雪花点；

Step2.3：如果点集{NGDS_n}中第i个Nongroudpoints_{n_i}点的高程z_{n_i}<3，则判断该点属于非雪花点集{NSPCS_n}：

NSPCS_n

＝[Nonsnowpointcloud₁,Nonsnowpointcloud₂,...,Nonsnowpointcloud_i]

其中：Nonsnowpointcloud_i表示第n帧数据中的第i个非雪花点；

Step2.4：当获取雪花点集{SPCS_n}后，计算当前帧ρ_{n_snow}，如果0<ρ_{n_snow}≤ξ₁，则判断当前帧降雪程度为小雪(Light)，如果ξ₁<ρ_{n_snow}≤ξ₂，则判断当前帧降雪程度为中雪(Moderate)，如果ρ_{n_snow}>ξ₂，则判断当前帧降雪程度为大雪(Heavy)；

其中：ξ₁为小雪和中雪的强度阈值，ξ₂为中雪和大雪的强度阈值；

Step3：数据分析单元判断降雪程度后，将不同帧降雪程度分别传入混合滤波单元，混合滤波单元通过自适应统计滤波、自适应半径滤波对该帧点云进行雪花降噪处理；

Step3.1：基于点云特征，对于输入点云中的每一个点Nongroudpoint_i，其领域内各点的平均距离：

假设服从高斯分布，可进一步求得点云空间中的N个点对应的均值μ和标准差σ为：

其中：k为设置的最小邻近点的搜索数量，d_i为在k构建的邻域内Nongroudpoint_i到各点的距离；

Step3.2：根据均值μ和标准差σ，分别设置统计滤波最小邻近点的搜索数量k₁、k₂、k₃和标准差系数V_{σ_1}、V_{σ_2}、V_{σ_3}，根据k₁、k₂、k₃和V_{σ_1}、V_{σ_2}、V_{σ_3}计算阈值范围(μ-σ·V_{σ_1},μ+σ·V_{σ_1})、(μ-σ·V_{σ_2},μ+σ·V_{σ_2})、(μ-σ·V_{σ_3},μ+σ·V_{σ_3})，滤除部分噪点，将滤除的雪花点存入雪花点集{SPCS_statistic_n_1}、{SPCS_statistic_n_2}、{SPCS_statistic_n_3}，并将滤除后剩余的目标点存入目标点集{NSPCS_statistic_n_1}、{NSPCS_statistic_n_2}、{NSPCS_statistic_n_3}；

其中：k₁、k₂、k₃分别为小雪、中雪、大雪情况下统计滤波最小邻近点的搜索数量；V_{σ_1}、V_{σ_2}、V_{σ_3}分别为小雪、中雪、大雪情况下统计滤波标准差系数；{SPCS_statistic_n_1}、{SPCS_statistic_n_2}、{SPCS_statistic_n_3}分别为小雪、中雪、大雪情况下自适应统计滤波后的雪花点集；{NSPCS_statistic_n_1}、{NSPCS_statistic_n_2}、{NSPCS_statistic_n_3}分别为小雪、中雪、大雪情况下自适应统计滤波后的目标点集；

Step3.3：将目标点集{NSPCS_statistic_n_1}、{NSPCS_statistic_n_2}、{NSPCS_statistic_n_3}分别传入自适应半径滤波算法，对于不同帧点云的不同降雪程度，针对该帧数据中的每一个点Nongroudpoint_i，设置自适应的滤波半径r₁、r₂、r₃，构建球形搜索范围，若范围内点的数量m小于设定的最少临近点数量M，则判定该Nongroudpoint_i为雪花噪声点，存储为新的点集{SPCS_radius_n_1}、{SPCS_radius_n_2}、{SPCS_radius_n_3}，予以滤除，反之予以保留，并存入新的点集{NSPCS_radius_n_1}、{NSPCS_radius_n_2}、{NSPCS_radius_n_3}；

其中：{SPCS_radius_n_1}、{SPCS_radius_n_2}、{SPCS_radius_n_3}为小雪、中雪、大雪情况下自适应半径滤波后的雪花点集；{NSPCS_radius_n_1}、{NSPCS_radius_n_2}、{NSPCS_radius_n_3}为小雪、中雪、大雪情况下自适应半径滤波后的非雪点集；

Step4：当混合滤波单元完成后，计算机将{SPCS_statistic_n_1}、{SPCS_statistic_n_2}、{SPCS_statistic_n_3}与{SPCS_radius_n_1}、{SPCS_radius_n_2}、{SPCS_radius_n_3}进行合并，并存储为新的滤波后点集{SPCS_afterfilter_n_1}、{SPCS_afterfilter_n_2}、{SPCS_afterfilter_n_3}，与{NSPCS_radius_n_1}、{NSPCS_radius_n_2}、{NSPCS_radius_n_3}通过point_cloud_viewer进行逐帧可视化显示。