4. 激光雷达数据处理：点云数据格式、噪声滤除与降采样
激光雷达扫出来的原始数据，说白了就是一堆点。每个点带着距离、角度、强度等信息。但你要是直接把这一堆点扔给导航算法，那基本跑不动——数据量太大，噪声太多，还有一堆没用的遮挡点。
我刚开始做机器人导航那会儿，就吃过这个亏。雷达一转，几万个点涌进来，程序直接卡死。后来才明白，数据处理不是可选项，是必选项。这一章，我就把点云数据怎么读、怎么滤、怎么降采样，掰开揉碎了讲清楚。
核心要点：原始点云 → 格式解析 → 噪声滤除 → 降采样 → 干净点云。每一步都影响后续建图和定位的精度。
4.1 点云数据格式：sensor_msgs/LaserScan
ROS2 里激光雷达的标准消息类型是 sensor_msgs/LaserScan。你想想看，一个旋转的激光雷达，每转一圈就发出一系列激光束，每条光束返回一个距离值。这些数据怎么组织？
我直接给你看结构：
# sensor_msgs/LaserScan 消息结构
std_msgs/Header header # 时间戳和坐标系
uint32 seq # 序列号
time stamp # 时间戳
string frame_id # 坐标系名称，如 "laser_frame"
float32 angle_min # 起始角度（弧度）
float32 angle_max # 终止角度（弧度）
float32 angle_increment # 角度分辨率（弧度/步）
float32 time_increment # 每步时间间隔（秒）
float32 scan_time # 整圈扫描时间（秒）
float32 range_min # 最小有效距离（米）
float32 range_max # 最大有效距离（米）
float32[] ranges # 距离数据数组
float32[] intensities # 强度数据数组（可选）这里有几个关键点，我重点说一下：
- angle_min 和 angle_max：决定了雷达的视野范围。比如 0 到 6.28 就是 360 度全覆盖。
- angle_increment：角度步长。如果雷达是 0.5 度分辨率，那这个值就是 0.0087 弧度。
- ranges 数组：长度 = (angle_max - angle_min) / angle_increment。每个元素对应一个角度上的距离值。
- intensities：反射强度。不同材质反射率不同，可以用来区分地面和障碍物。
我的经验：拿到 LaserScan 数据后，第一件事不是处理，而是检查 frame_id。我遇到过好几次，雷达数据坐标系设错了，导致建图全歪。养成习惯：先打印 header.frame_id 确认坐标系。
4.2 噪声滤除：离群点与遮挡点
原始点云里什么妖魔鬼怪都有。灰尘、飞虫、玻璃反射、边缘跳变……这些噪声不滤掉，建图质量直接崩。
4.2.1 离群点滤除
离群点就是那些明显偏离正常范围的点。比如你站在 5 米外，突然冒出一个 50 米的点，那肯定是噪声。
常用的方法有两种：
| 方法 | 原理 | 适用场景 |
|---|---|---|
| 半径滤波 | 以每个点为中心，统计半径内的邻居数量。少于阈值则剔除。 | 稀疏离群点，比如飞虫、灰尘 |
| 统计滤波 | 计算每个点到邻居的平均距离，超过均值+若干标准差则剔除。 | 均匀分布的噪声，比如雷达自身抖动 |
我个人习惯用半径滤波，参数好调。举个例子：
// C++ 示例：PCL 半径滤波
pcl::RadiusOutlierRemoval<pcl::PointXYZ> radius_filter;
radius_filter.setInputCloud(cloud);
radius_filter.setRadiusSearch(0.1); // 搜索半径 0.1 米
radius_filter.setMinNeighborsInRadius(3); // 至少 3 个邻居
radius_filter.filter(*cloud_filtered);避坑指南：我曾经把半径设得太小（0.02 米），结果把墙上的点都滤掉了，因为墙上的点间距本来就大。后来我根据雷达分辨率来算：半径 = 角度分辨率 × 距离 × 2，这样比较合理。
4.2.2 遮挡点处理
遮挡点是什么？就是激光打到物体边缘时，一部分光束打到前景，一部分打到背景，返回一个中间值。这种点位置不准，必须处理。
我常用的方法是相邻点跳变检测：
- 遍历 ranges 数组，计算相邻点的距离差。
- 如果差值超过阈值（比如 0.3 米），则标记为遮挡点。
- 剔除这些点，或者用插值填补。
// Python 示例：遮挡点检测
def remove_occlusion_points(ranges, threshold=0.3):
clean_ranges = ranges.copy()
for i in range(1, len(ranges) - 1):
diff = abs(ranges[i] - ranges[i-1])
if diff > threshold:
clean_ranges[i] = 0.0 # 标记为无效
return clean_ranges你想想看，如果两个相邻角度上的距离突然从 2 米跳到 10 米，那中间肯定有遮挡。这种点留着，建图时就会出现“穿墙”现象。
4.3 数据降采样
高分辨率雷达一秒钟能扫几十万个点。导航算法根本吃不下。降采样就是在保持几何特征的前提下，减少点的数量。
4.3.1 体素滤波（Voxel Grid Filter）
这是最常用的方法。把空间划分成一个个小立方体（体素），每个体素内只保留一个点（通常是重心）。
体素大小是关键参数：
| 体素大小 | 效果 | 适用场景 |
|---|---|---|
| 0.01 米 | 几乎不降采样，保留细节 | 高精度建图 |
| 0.05 米 | 适中降采样，保留主要特征 | 实时导航 |
| 0.1 米 | 大幅降采样，丢失细节 | 快速定位 |
// C++ 示例：PCL 体素滤波
pcl::VoxelGrid<pcl::PointXYZ> voxel_filter;
voxel_filter.setInputCloud(cloud);
voxel_filter.setLeafSize(0.05f, 0.05f, 0.05f); // 5cm 体素
voxel_filter.filter(*cloud_downsampled);我的经验：体素大小不要一刀切。室内场景用 0.03-0.05 米，室外开阔场景可以放宽到 0.1 米。我做过一个项目，在走廊里用 0.1 米体素，结果门框特征全丢了，定位直接飘。后来改成 0.03 米才稳住。
4.3.2 均匀采样
另一种方法，每隔 N 个点取一个。简单粗暴，但可能丢失重要特征。
// Python 示例：均匀采样
def uniform_sampling(ranges, step=5):
return ranges[::step]均匀采样适合点云密度均匀的场景。如果雷达近处点密、远处点稀，那均匀采样会导致远处特征丢失更严重。
4.3.3 两种方法对比
| 方法 | 优点 | 缺点 |
|---|---|---|
| 体素滤波 | 保持几何特征，密度均匀 | 计算量稍大 |
| 均匀采样 | 速度快，实现简单 | 可能丢失特征，密度不均 |
我个人更推荐体素滤波。虽然计算量稍微大一点，但效果稳定。你想想看，导航算法对点云质量很敏感，多花几毫秒处理，换来更可靠的定位，值。
4.4 实战：完整的数据处理管线
好了，理论讲完了。我直接给你一个完整的处理流程，你在 ROS2 里可以直接用：
# 伪代码：激光雷达数据处理管线
def process_laser_scan(raw_scan):
# 1. 格式解析
ranges = raw_scan.ranges
angle_min = raw_scan.angle_min
angle_increment = raw_scan.angle_increment
# 2. 遮挡点滤除
ranges = remove_occlusion_points(ranges, threshold=0.3)
# 3. 离群点滤除（半径滤波）
cloud = ranges_to_pointcloud(ranges, angle_min, angle_increment)
cloud = radius_filter(cloud, radius=0.1, min_neighbors=3)
# 4. 体素滤波降采样
cloud = voxel_filter(cloud, leaf_size=0.05)
return cloud关键参数速查表：
- 遮挡点阈值：0.2 - 0.5 米（根据场景调整）
- 半径滤波半径：0.05 - 0.15 米
- 半径滤波邻居数：2 - 5 个
- 体素大小：0.03 - 0.1 米
嗯，这一章的内容就这些。数据处理看起来琐碎，但每一步都影响最终效果。我见过太多人跳过这一步，直接拿原始点云建图，结果地图全是噪点，定位也跑偏。记住：好数据是好结果的前提。