第4章数据预处理与滤波

第4章：数据预处理与滤波

各位同学，欢迎来到第四章。上一章我们聊了传感器的时间同步和空间对齐，那都是为融合做的准备工作。这一章，我们真正开始动手处理数据本身——说白了，就是给传感器数据“洗个澡”，把脏东西、噪声、异常点都去掉。

我经常跟团队里的新人说：融合算法的上限，很大程度上取决于预处理的下限。你想想看，如果输入的数据都是垃圾，后面再牛的算法也白搭。所以这一章的内容，虽然看起来基础，但绝对是实战中的硬骨头。

4.1 点云滤波：体素滤波与统计滤波

点云数据，尤其是激光雷达的点云，数据量巨大。一帧64线激光雷达的点云，动辄几十万个点。直接处理？算力扛不住。所以第一步，通常是降采样。

4.1.1 体素滤波（Voxel Filter）

体素滤波的原理很简单：把三维空间划分成一个个小立方体（体素），每个体素内只保留一个点（通常是重心点）。这样，点云密度就降下来了。

我个人习惯把体素滤波比作“像素化”。就像你把一张高清照片的分辨率降低，细节没了，但整体轮廓还在。体素滤波也是一样，它保留了点云的宏观几何结构，但会丢失一些微观细节。

核心参数：体素大小（Leaf Size）

体素大小直接决定了滤波后的点云密度。设得太大，点云会变得稀疏，可能丢失关键特征；设得太小，降采样效果不明显。

我在项目中遇到过一个问题：用0.1m的体素滤波，结果把路肩上的小障碍物给滤掉了。后来改成0.05m，才勉强保留住。所以，这个参数需要根据传感器特性和应用场景反复调试。

下面是一个PCL（Point Cloud Library）的实现示例：

// C++ 示例：PCL体素滤波
#include <pcl/filters/voxel_grid.h>

pcl::VoxelGrid<pcl::PointXYZ> vg;
vg.setInputCloud(cloud_input);
vg.setLeafSize(0.1f, 0.1f, 0.1f); // 体素大小 10cm
vg.filter(*cloud_filtered);

嗯，这里要注意：体素滤波后的点云，坐标是体素内所有点的平均值。如果你需要保留原始点的精确位置，那体素滤波就不太合适了。

4.1.2 统计滤波（Statistical Outlier Removal）

体素滤波解决了数据量的问题，但点云里还有噪声——那些孤立的、离群的噪点。统计滤波就是用来干这个的。

它的原理是：对每个点，计算它到K个最近邻点的平均距离。如果这个平均距离超出了全局均值的一定标准差范围，就判定为离群点，直接剔除。

说白了，就是“少数服从多数”。如果一个点周围没什么邻居，那它大概率是噪声。

实战技巧：

我建议先做体素滤波，再做统计滤波。因为体素滤波会改变点的分布，先降采样再滤波，效果更稳定。

另外，统计滤波的K值（邻居数量）和标准差倍数（std_dev_mul）需要配合调整。K值太小，容易误判；K值太大，计算量上去了。我一般从K=50开始试。

// C++ 示例：PCL统计滤波
#include <pcl/filters/statistical_outlier_removal.h>

pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
sor.setInputCloud(cloud_filtered);
sor.setMeanK(50);          // 邻居数量
sor.setStddevMulThresh(1.0); // 标准差倍数
sor.filter(*cloud_clean);

我曾经在一个项目中，统计滤波的参数设得太严（stddev=0.5），结果把远处的车辆点云也给滤掉了。后来改成1.0，才恢复正常。所以，参数一定要根据实际数据来调，别想着一劳永逸。

4.2 图像去畸变

摄像头拍出来的图像，天生就有畸变。尤其是广角镜头，边缘的直线都变成弯的了。这对后续的目标检测、车道线识别影响很大。所以，去畸变是图像预处理的第一步。

畸变主要分两种：径向畸变和切向畸变。径向畸变是镜头形状导致的，表现为“桶形”或“枕形”；切向畸变是镜头和成像平面不平行导致的。

去畸变的原理，就是通过标定得到的相机内参和畸变系数，建立畸变图像到理想图像的映射关系，然后进行重映射。

核心公式（简化版）：

径向畸变：x_distorted = x * (1 + k1 * r^2 + k2 * r^4 + k3 * r^6)

切向畸变：x_distorted = x + [2 * p1 * x * y + p2 * (r^2 + 2 * x^2)]

其中，k1, k2, k3是径向畸变系数，p1, p2是切向畸变系数。这些系数通过相机标定得到。

OpenCV里提供了现成的函数：

// Python 示例：OpenCV图像去畸变
import cv2
import numpy as np

# 相机内参矩阵和畸变系数（来自标定）
camera_matrix = np.array([[fx, 0, cx],
                          [0, fy, cy],
                          [0, 0, 1]])
dist_coeffs = np.array([k1, k2, p1, p2, k3])

# 去畸变
undistorted_img = cv2.undistort(img, camera_matrix, dist_coeffs)

你想想看，如果不去畸变，一个在图像边缘的行人，他的位置可能偏差好几个像素。对于需要精确测距的融合算法来说，这误差就太大了。

注意：

去畸变后，图像边缘会被裁剪掉一部分。如果你需要保留完整的视场角，可以考虑使用“保留所有像素”的映射方式，但那样会引入一些无效区域。

另外，去畸变会改变像素的坐标，所以如果你有图像上的标注信息（比如2D检测框），记得也要做相应的坐标变换。

4.3 雷达CFAR检测

毫米波雷达的数据，不像激光雷达那样直观。它返回的是目标点（点迹），但里面夹杂着大量的虚警——比如地面反射、雨雪杂波。CFAR（Constant False Alarm Rate，恒虚警率）检测，就是用来从噪声中提取真实目标的。

CFAR的核心思想是：对每个待检测单元，计算它周围背景噪声的功率水平，然后设定一个动态阈值。如果待检测单元的功率超过这个阈值，就判定为目标。

常见的CFAR算法有CA-CFAR（单元平均）、OS-CFAR（有序统计）等。CA-CFAR计算简单，但在多目标场景下性能下降；OS-CFAR抗干扰能力强，但计算量大。

CA-CFAR原理：

1. 在待检测单元周围，选取保护单元（避免目标自干扰）和参考单元。

2. 计算参考单元的平均功率。

3. 阈值 = 平均功率 * (N * (P_fa^(-1/N) - 1))，其中N是参考单元数量，P_fa是期望的虚警率。

4. 如果待检测单元功率 > 阈值，则判为目标。

我在项目中遇到过一个问题：在隧道场景下，雷达回波特别强，CA-CFAR的阈值被拉得很高，导致很多真实目标被漏检。后来换成了OS-CFAR，情况才好转。

// Python 示例：CA-CFAR简化实现
import numpy as np

def ca_cfar(signal, guard_cells, ref_cells, pfa):
    N = 2 * ref_cells
    alpha = N * (pfa ** (-1/N) - 1)
    threshold = np.zeros_like(signal)
    
    for i in range(ref_cells + guard_cells, len(signal) - ref_cells - guard_cells):
        # 计算参考单元平均功率
        left = signal[i - ref_cells - guard_cells : i - guard_cells]
        right = signal[i + guard_cells + 1 : i + guard_cells + ref_cells + 1]
        noise_power = np.mean(np.concatenate([left, right]))
        threshold[i] = alpha * noise_power
    
    return threshold

嗯，这里要注意：CFAR的参数（保护单元、参考单元、虚警率）需要根据雷达的特性和应用场景来调。保护单元太小，目标会自干扰；参考单元太少，噪声估计不准。

4.4 信号平滑

传感器数据，尤其是时序数据，往往带有高频噪声。比如，IMU的角速度、加速度，或者雷达的目标跟踪结果。信号平滑，就是把这些噪声滤掉，让数据更“顺滑”。

常用的平滑方法有：

移动平均滤波（Moving Average）：简单有效，但会引入延迟。
指数加权移动平均（EWMA）：对近期数据更敏感，延迟更小。
卡尔曼滤波（Kalman Filter）：最优线性滤波器，但需要建立状态模型。
中值滤波（Median Filter）：对脉冲噪声（野点）特别有效。

我个人习惯，在融合算法的前端，先用一个简单的移动平均滤波做初步平滑，再用卡尔曼滤波做状态估计。这样既能快速响应，又能保证稳定性。

实战建议：

移动平均的窗口大小很关键。窗口太大，信号延迟高，实时性差；窗口太小，平滑效果不明显。

我一般从窗口大小=5开始试，然后根据实际数据的噪声水平和系统延迟要求来调整。

// Python 示例：移动平均滤波
import numpy as np

def moving_average(data, window_size):
    return np.convolve(data, np.ones(window_size)/window_size, mode='valid')

你想想看，如果雷达的目标距离数据不做平滑，直接送给融合算法，那融合出来的轨迹会一跳一跳的，根本没法用。所以，信号平滑虽然看起来简单，但它是保证系统稳定性的基石。

注意：

平滑滤波会引入相位延迟。对于实时性要求高的系统（比如AEB自动紧急制动），延迟必须控制在毫秒级。这时候，可以考虑使用零相位滤波（如filtfilt），但那是离线处理用的。

另外，平滑滤波会抑制高频信息。如果信号中有需要保留的快速变化（比如急刹车时的加速度突变），那就要谨慎选择滤波器的截止频率。

小结

这一章的内容，说白了就是给传感器数据“做清洁”。点云滤波去掉冗余和噪声，图像去畸变纠正几何失真，雷达CFAR检测提取真实目标，信号平滑让数据更稳定。

这些预处理步骤，看似不起眼，但往往是决定融合算法成败的关键。我在实际项目中见过太多因为预处理没做好，导致整个系统崩溃的例子。所以，同学们，别小看这些基础操作，多花点时间在数据预处理上，绝对值得。

下一章，我们将进入融合算法的核心——目标关联与状态估计。到时候，你会发现，预处理做得越好，后面的工作就越轻松。

📚 课程目录