4、传感器数据处理：LiDAR点云降采样、Camera图像预处理加速、Radar信号滤波优化
传感器数据处理，说白了就是自动驾驶系统的「眼睛」和「耳朵」在干活。我做了这么多年系统优化，发现很多团队把精力都放在模型上，却忽略了传感器数据预处理这个环节。其实这里才是性能瓶颈的重灾区。
你想想看，一辆车上同时跑着激光雷达、摄像头、毫米波雷达，每秒产生的数据量有多大？如果不做优化，计算资源很快就会被吃光。今天我就把这三个传感器数据处理的优化经验，掰开了揉碎了讲给你听。
4.1 LiDAR点云降采样：从百万级到万级的艺术
LiDAR点云数据量巨大，64线激光雷达每秒能产生130万个点。直接处理？不现实。我见过一个项目，点云处理占用了整个系统40%的CPU时间，后来通过降采样优化，降到了8%。
4.1.1 体素滤波（Voxel Filter）
这是最常用的方法。把空间划分成一个个小立方体（体素），每个体素内只保留一个点。说白了就是用空间换时间。
// 体素滤波核心逻辑
struct VoxelGrid {
float leaf_size; // 体素边长
std::unordered_map<int, Point> grid;
void filter(const PointCloud& input, PointCloud& output) {
for (const auto& p : input.points) {
int key = (int)(p.x / leaf_size) * 1000000 +
(int)(p.y / leaf_size) * 1000 +
(int)(p.z / leaf_size);
// 只保留每个体素内的第一个点
if (grid.find(key) == grid.end()) {
grid[key] = p;
}
}
// 输出降采样后的点云
for (const auto& [key, point] : grid) {
output.push_back(point);
}
}
};
我的经验：体素大小选0.1m比较合适。太小了降采样效果不明显，太大了会丢失障碍物细节。我在高速场景用过0.2m，效果也还行，但城区场景就不行了。
4.1.2 随机降采样
简单粗暴，随机扔掉一部分点。优点是速度快，缺点是有可能把关键点也扔了。
我个人习惯在以下场景用随机降采样：
- 地面点云（反正地面点都差不多）
- 远距离点云（超过50米外的点，精度本来就不高）
- 预处理阶段的快速预览
4.1.3 基于距离的自适应降采样
这个技巧是我在一个项目中摸索出来的。近处的点保留多一些，远处的点少保留一些。为什么？因为近处的障碍物需要高精度检测，远处的点本来就不准，保留太多反而浪费。
| 距离范围 | 保留比例 | 体素大小 |
|---|---|---|
| 0-10m | 100% | 不降采样 |
| 10-30m | 50% | 0.05m |
| 30-50m | 20% | 0.1m |
| 50m以上 | 5% | 0.2m |
注意：自适应降采样虽然效果好，但实现起来要注意边界处理。我曾经在10m和30m的边界处出现过点云密度突变，导致障碍物检测出现断裂。后来加了平滑过渡才解决。
4.2 Camera图像预处理加速：别让CPU成为瓶颈
摄像头数据量更大。一个1080p的摄像头，30fps，每秒要处理62MB数据。如果做畸变校正、颜色空间转换、缩放，CPU很快就扛不住了。
4.2.1 使用NEON/SIMD指令集加速
ARM架构的芯片都支持NEON指令集。说白了就是一条指令同时处理多个数据。我做过测试，用NEON优化后的图像缩放，速度提升了4倍。
// NEON加速的RGB转灰度
void rgb_to_gray_neon(const uint8_t* rgb, uint8_t* gray, int width, int height) {
int total = width * height;
// 使用NEON指令并行处理8个像素
for (int i = 0; i < total; i += 8) {
uint8x8x3_t rgb_vec = vld3_u8(rgb + i * 3);
// 加权计算灰度值
uint16x8_t r = vmull_u8(rgb_vec.val[0], vdup_n_u8(77));
uint16x8_t g = vmlal_u8(r, rgb_vec.val[1], vdup_n_u8(150));
uint16x8_t b = vmlal_u8(g, rgb_vec.val[2], vdup_n_u8(29));
uint8x8_t gray_vec = vshrn_n_u16(b, 8);
vst1_u8(gray + i, gray_vec);
}
}
4.2.2 图像预处理流水线
我建议把图像预处理做成流水线。别等一帧处理完再处理下一帧，那样太慢了。用双缓冲或者三缓冲，让CPU和DMA同时工作。
- Stage 1: DMA从摄像头取数据到内存
- Stage 2: CPU做畸变校正（这个必须CPU做）
- Stage 3: GPU/NEON做颜色空间转换和缩放
- Stage 4: 直接送到推理引擎
避坑指南：我曾经把畸变校正和颜色转换放在同一个线程里做，结果发现CPU利用率只有60%。后来拆成两个线程，利用率到了95%，吞吐量翻了一倍。记住，流水线的关键是让每个环节都忙起来。
4.2.3 感兴趣区域（ROI）裁剪
不是整张图像都需要处理。比如车道线检测，只需要下半部分图像就行。行人检测，重点关注图像中间区域。提前裁剪，能省下不少计算量。
我习惯在配置文件中定义ROI参数，这样不同场景可以灵活切换：
// ROI配置示例
struct CameraROI {
int x_offset; // 水平偏移
int y_offset; // 垂直偏移
int width; // ROI宽度
int height; // ROI高度
float scale; // 缩放比例
};
4.3 Radar信号滤波优化：去伪存真
毫米波雷达的数据量不大，但噪声多。虚假目标、多径反射、地面杂波，各种干扰。滤波做不好，后面的融合模块就遭殃了。
4.3.1 卡尔曼滤波的参数调优
卡尔曼滤波是雷达信号处理的标配。但参数怎么调？很多人直接用默认值，结果效果很差。
我分享一个经验：
- 过程噪声Q矩阵：不要设太大。我一般设0.01-0.1，太大了滤波器会跟着噪声跑
- 测量噪声R矩阵：根据雷达的精度来。比如速度测量精度0.1m/s，R就设0.01
- 初始P矩阵：设大一点没关系，滤波器会自己收敛
4.3.2 多普勒速度一致性检查
这是个很实用的技巧。雷达能测出目标的径向速度，如果这个速度和位置变化不一致，那多半是虚假目标。
// 多普勒一致性检查
bool check_doppler_consistency(const RadarTarget& target, float dt) {
// 根据位置变化计算速度
float v_pos = (target.range - target.prev_range) / dt;
// 雷达直接测量的多普勒速度
float v_doppler = target.doppler_velocity;
// 两者差异超过阈值，认为是虚假目标
float diff = fabs(v_pos - v_doppler);
return diff < 2.0f; // 阈值2m/s
}
注意：这个阈值不能设太死。我遇到过隧道场景，多径反射导致速度测量偏差很大，但目标其实是真实的。后来我加了场景判断，隧道里放宽阈值。
4.3.3 静态杂波滤除
路边的护栏、路牌、静止的车辆，这些都会产生雷达回波。如果不滤除，融合模块会以为周围全是障碍物。
我的做法是：
- 建立静态杂波地图（离线采集）
- 在线运行时，把雷达检测到的静态目标跟地图比对
- 匹配上的直接滤除
- 没匹配上的，再判断是不是新出现的静态障碍物
4.4 多传感器数据同步的坑
最后说一个容易忽略的问题：数据同步。LiDAR、Camera、Radar的采样频率不一样，时间戳也不一样。如果不做同步，融合出来的结果就是错的。
我建议用以下策略：
- 以Camera的时间为基准（30fps，最稳定）
- LiDAR数据做线性插值，对齐到Camera时间
- Radar数据用最近邻匹配，因为Radar帧率低（10-20fps）
我的经验：时间同步的精度要求是1ms以内。我曾经因为系统时钟漂移，导致LiDAR和Camera的数据差了5ms，结果障碍物位置偏差了0.3米。后来用了PTP（精确时间协议）才解决。
传感器数据处理这块，说白了就是跟数据量和噪声做斗争。降采样要讲究策略，预处理要善用硬件加速，滤波要结合实际场景。把这些做好了，后面的感知模块才能跑得又快又准。