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::vector<uint16_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 和 i+participation_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[i+1] < 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[i+1] >= 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[i+1] && 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[i+participation_points] >= temp_data.ranges_data[i] + len_value) {
// 计算加权差值，若大于等于前点距离，则认为中间点为拖尾噪声
if (temp_filter_value * (temp_data.ranges_data[i+participation_points] - temp_data.ranges_data[i])
>= temp_data.ranges_data[i]) {
for (int j = 0; j < participation_points; j++) {
data.ranges_data[i+j+1] = 0; // 清除中间点
data.intensities_data[i+j+1] = 0;
}
}
}
// 2.2 反向跳变：前点距离 ≥ 后点距离 + len_value
else if (temp_data.ranges_data[i+participation_points] > 10 &&
temp_data.ranges_data[i] >= temp_data.ranges_data[i+participation_points] + len_value) {
if (temp_filter_value * (temp_data.ranges_data[i] - temp_data.ranges_data[i+participation_points])
>= temp_data.ranges_data[i+participation_points]) {
for (int j = 0; j < participation_points; j++) {
data.ranges_data[i+j] = 0; // 清除中间点
data.intensities_data[i+j] = 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[i+j] - temp_data.ranges_data[i+j+1]) > min_cluster_distance) {
is_Cluster = false;
break;
}
}
// 同时确保窗口内所有点的距离都大于10mm（避免恢复零点）
for (int j = 0; j < cluster_num; j++) {
if (temp_data.ranges_data[i+j] <= 10) {
is_Cluster = false;
break;
}
}
// 如果满足聚类条件，则从备份中恢复这些点的距离和强度
if (is_Cluster) {
for (int j = 0; j < cluster_num; j++) {
data.ranges_data[i+j] = temp_data.ranges_data[i+j];
data.intensities_data[i+j] = temp_data.intensities_data[i+j];
}
}
}
// 注意：函数结尾没有 return，因为 data 是引用传递，直接修改了原始数据。
}6 实际效果对比
滤波前
滤波后
