综合强度信息的激光雷达去拖尾算法解析和源码实现

1. 内容本文主要介绍基于几何特征与信号强度的去拖尾算法和程序实现。2. 激光雷达的常见误差类型2.1 拖尾Trailing拖尾是指当激光束照射到高反射率物体如反光条、玻璃、镜子、路面标志等时在真实目标后方沿激光扫描方向出现一连串虚假点云的现象。这些虚假点通常呈“拖尾”状延伸严重时可达数米会误判为障碍物或引起建图畸变。2.2 高反High Reflectivity高反物体如反光柱、金属表面会使回波信号强度饱和造成测距精度下降甚至产生错误的距离值。高反点本身可能被探测为比实际更近或更远的距离并在其后方伴随拖尾噪声。2.3 混合像素Mixed Pixel当激光束落在两个不同距离的物体边缘时接收器可能同时接收到两部分回波导致测距值在两个真实距离之间漂移产生边缘模糊或孤立点。2.4 多路径反射Multipath激光在多个表面之间反射后再返回导致测距值远大于真实距离常出现在玻璃、镜面、水体等场景。3. 拖尾现象的物理原理拖尾现象的核心成因是激光脉冲在强反射表面的多次反射与接收器饱和。以典型的脉冲飞行时间ToF激光雷达为例信号饱和当激光束照射到高反射率物体如反光条时回波信号强度远超接收器线性响应范围导致信号波形截断削顶测距电路无法准确提取脉冲前沿从而输出一个偏离真实距离的值通常偏近。多次反射强反射表面的部分光能量会在雷达外壳、透镜或其他光学元件间发生内部反射形成多个延迟的回波。这些延迟回波被接收器捕获后可能被误判为后方物体的回波从而在原始点云中产生“拖尾”点。光斑扩散强反射点会使得激光光斑在探测器中扩散影响相邻像素的测量导致在扫描方向上出现连续的错误点。拖尾点通常具有以下特征位于真实高反点的后方沿扫描方向强度值较高但仍可能低于饱和阈值距离变化缓慢呈线性衰减趋势与周围点的角度关系异常几何一致性差。4. 去拖尾的基本原理去拖尾的核心思想是识别拖尾区域的特征将其与真实障碍物区分开并予以滤除。常用方法包括4.1 强度跳变检测拖尾总是从高反点边缘开始因此通过检测信号强度的阶跃变化由高到低或由低到高可以定位高反物体的边界。随后将边界后的一段连续区域视为潜在拖尾区域进行滤除。4.2 几何角度分析拖尾点与雷达、真实高反点之间构成特殊的几何关系。通过计算相邻两点与雷达的夹角∠OP₁P₂可以识别出拖尾点拖尾点通常会使该夹角趋近于 0° 或 180°即三点共线而真实障碍物之间的夹角则接近 90°。利用这一特性可以精确筛选拖尾点。4.3 距离跳变一致性拖尾点的距离变化通常比较平缓而真实物体的边缘存在明显跳变。通过滑动窗口比较相隔一定距离的两点若距离差超过阈值且满足几何条件即可判定为拖尾。4.4 时间连续性拖尾点在不同扫描周期中通常具有相似的位置而真实细小物体则可能移动。利用帧间信息可以剔除新出现的孤立点保留稳定目标。4.5 聚类恢复为避免误删真实细小物体需对识别出的拖尾区域进行保护若区域内存在连续多个点且距离变化在物理允许范围内如满足聚类条件则将其恢复为有效点。5. 去拖尾算法的工程实现本节基于实际工程代码trailing_filter.cpp和algorithmAPI.cpp展示去拖尾算法的具体实现。5.1 源码解析/** * brief 综合滤波函数基于强度跳变识别高反区域并清除拖尾再通过距离跳变剔除孤立噪声 * 最后通过聚类恢复细小物体。 * * param data 输入输出的点云数据距离和强度 * param resolution 角分辨率单位0.0001度如 1700 表示 0.17° * param cluster_num 聚类窗口大小连续多少个点可构成一个有效聚类 * param broad_filter_num 拖尾清除宽度从强度跳变点开始清除的连续点数 * * 该函数处理流程 * 1. 强度跳变检测识别高反射率物体的边界将边界后/前的区域视为潜在拖尾区域并清零。 * 2. 距离跳变滤波比较相隔一定距离的两个点若距离跳变异常且满足几何条件则将其间的点清零。 * 3. 聚类恢复对于连续若干点如果它们之间的距离变化小于理论最小聚类距离则将其恢复为有效点 * 以保护细小物体不被误删。 */ void TrailingFilter::Filter(ScanData data, uint16_t resolution, uint16_t cluster_num, uint16_t broad_filter_num) { // 前置检查点云数量必须足够 if (data.ranges_data.size() 2 || data.intensities_data.size() 2) { return; } std::size_t point_num data.ranges_data.size(); // 总点数 // 备份原始数据用于后续恢复有效点 ScanData temp_data; for (std::size_t i 0; i point_num; i) { temp_data.ranges_data.push_back(data.ranges_data[i]); temp_data.intensities_data.push_back(data.intensities_data[i]); } // 滤波缓冲区用于标记每个点是否处于“高敏感区域”如反光条后方拖尾区域 std::vectoruint16_t filter_buf(point_num, 0); const uint16_t filter_value 20; // 正常区域滤波系数百分比 const uint16_t filter_value_high 2000; // 高敏感区域滤波系数放大系数 const uint16_t loop2 broad_filter_num; // 直接清零的宽度拖尾清除宽度 const uint16_t loop1 100; // 提高滤波阈值的宽度更温和的拖尾抑制区域 // 初始化所有点的滤波系数为正常值 for (std::size_t i 0; i point_num; i) { filter_buf[i] filter_value; } // 距离跳变阈值单位毫米 uint16_t len_value 10; // 强度跳变阈值用于判断高反区域此处设为4095表示几乎不会触发需根据实际调整 uint16_t rssi_threshold 4095; // 参与距离跳变比较的点间隔比较 i 和 iparticipation_points 两点 uint8_t participation_points 4; // 1. 强度跳变检测与拖尾清除 // 遍历所有相邻点对检测强度从高到低或从低到高的跳变识别反光条边界 for (std::size_t i 0; i point_num - 1; i) { // 1.1 高 - 低跳变反光条结束位置后方可能产生拖尾 if (data.intensities_data[i] rssi_threshold data.intensities_data[i1] rssi_threshold) { // 计算拖尾区域的范围从跳变点开始向后延伸 loop1 个点提高滤波系数 std::size_t start_index i; std::size_t stop_index; if (i loop1 point_num - 1) { stop_index i loop1; } else { stop_index point_num - 1; } // 将拖尾区域内的滤波系数设置为高敏感值更易触发距离跳变滤波 for (std::size_t j start_index; j stop_index; j) { filter_buf[j] filter_value_high; // 如果该点距离小于等于跳变点距离且大于10mm则恢复为正常系数认为可能是真实物体 if (data.ranges_data[j] data.ranges_data[i] data.ranges_data[j] 10) { filter_buf[j] filter_value; } } // 直接清零在跳变点后方 broad_filter_num 个点强制清除激进拖尾消除 std::size_t start_index2 i; std::size_t stop_index2; if (i loop2 point_num - 1) { stop_index2 i loop2; } else { stop_index2 point_num - 1; } for (std::size_t j start_index2; j stop_index2; j) { data.ranges_data[j] 0; // 距离置零 data.intensities_data[j] 0; // 强度置零 temp_data.ranges_data[j] 0; // 备份同步置零避免后续恢复 temp_data.intensities_data[j] 0; } } // 1.2 低 - 高跳变反光条开始位置前方可能产生拖尾 else if (data.intensities_data[i] rssi_threshold data.intensities_data[i1] rssi_threshold) { // 计算拖尾区域的范围从跳变点向前延伸 loop1 个点提高滤波系数 std::size_t start_index; std::size_t stop_index i; if (i loop1) { start_index i - loop1; } else { start_index 0; } for (std::size_t j start_index; j stop_index; j) { filter_buf[j] filter_value_high; if (data.ranges_data[j] data.ranges_data[i1] data.ranges_data[j] 10) { filter_buf[j] filter_value; } } // 直接清零在跳变点前方 broad_filter_num 个点强制清除 std::size_t start_index2; std::size_t stop_index2 i; if (i loop2) { start_index2 i - loop2; } else { start_index2 0; } for (std::size_t j start_index2; j stop_index2; j) { data.ranges_data[j] 0; data.intensities_data[j] 0; temp_data.ranges_data[j] 0; temp_data.intensities_data[j] 0; } } } // 2. 距离跳变滤波 // 比较相距 participation_points 的两个点若距离跳变过大且满足加权条件则清除中间的点 for (std::size_t i 0; i point_num - participation_points - 2; i) { uint16_t temp_filter_value filter_buf[i]; // 当前点的滤波系数已根据强度跳变区域调整 // 2.1 正向跳变后点距离 ≥ 前点距离 len_value if (temp_data.ranges_data[i] 8000) { // 仅处理有效测距范围8m以内 if (temp_data.ranges_data[i] 10 temp_data.ranges_data[iparticipation_points] temp_data.ranges_data[i] len_value) { // 计算加权差值若大于等于前点距离则认为中间点为拖尾噪声 if (temp_filter_value * (temp_data.ranges_data[iparticipation_points] - temp_data.ranges_data[i]) temp_data.ranges_data[i]) { for (int j 0; j participation_points; j) { data.ranges_data[ij1] 0; // 清除中间点 data.intensities_data[ij1] 0; } } } // 2.2 反向跳变前点距离 ≥ 后点距离 len_value else if (temp_data.ranges_data[iparticipation_points] 10 temp_data.ranges_data[i] temp_data.ranges_data[iparticipation_points] len_value) { if (temp_filter_value * (temp_data.ranges_data[i] - temp_data.ranges_data[iparticipation_points]) temp_data.ranges_data[iparticipation_points]) { for (int j 0; j participation_points; j) { data.ranges_data[ij] 0; // 清除中间点 data.intensities_data[ij] 0; } } } } } // 3. 细小物体保护聚类恢复 // 对于连续 cluster_num 个点如果它们之间的距离变化小于理论最小聚类距离 // 则认为是真实细小物体从备份数据中恢复。 for (std::size_t i 0; i point_num - cluster_num 1; i) { // 计算理论最小聚类距离当前点距离 × 2π × 角分辨率弧度即相邻两点在圆周上的弧长 // 注意resolution 单位为 0.0001°需转换为度再转弧度 float min_cluster_distance (float)temp_data.ranges_data[i] * 2 * M_PI * resolution / (360.0f * 10000); bool is_Cluster true; // 检查窗口内相邻点之间的距离差是否都小于理论值 for (int j 0; j cluster_num - 1; j) { if (abs(temp_data.ranges_data[ij] - temp_data.ranges_data[ij1]) min_cluster_distance) { is_Cluster false; break; } } // 同时确保窗口内所有点的距离都大于10mm避免恢复零点 for (int j 0; j cluster_num; j) { if (temp_data.ranges_data[ij] 10) { is_Cluster false; break; } } // 如果满足聚类条件则从备份中恢复这些点的距离和强度 if (is_Cluster) { for (int j 0; j cluster_num; j) { data.ranges_data[ij] temp_data.ranges_data[ij]; data.intensities_data[ij] temp_data.intensities_data[ij]; } } } // 注意函数结尾没有 return因为 data 是引用传递直接修改了原始数据。 }6 实际效果对比滤波前滤波后