3、传感器数据处理：激光雷达、摄像头、IMU、里程计的数据融合与预处理
各位同学，咱们今天聊点实在的。
做机器人路径规划，传感器数据就是你的眼睛和耳朵。但说实话，这些传感器没一个省心的。激光雷达怕雨雾，摄像头怕光线暗，IMU 飘得厉害，里程计打滑是家常便饭。我刚开始做项目那会儿，就吃过这个亏——传感器数据没处理好，路径规划直接跑飞，差点把样机撞墙上。
所以这一节，咱们把激光雷达、摄像头、IMU、里程计这四样东西的数据怎么融合、怎么预处理，掰开了揉碎了讲清楚。
3.1 激光雷达数据预处理
激光雷达给的是点云，一帧少则几万点，多则几十万点。直接拿去做路径规划？不现实。你得先做三件事：去噪、降采样、地面滤除。
3.1.1 去噪
点云里的噪声主要来自两个地方：一是传感器本身的测量误差，二是环境中的干扰（比如玻璃反射、雨滴）。
我常用的方法是统计滤波。说白了，就是计算每个点周围 k 个邻居的平均距离，如果这个距离超过某个阈值，就认为是离群点，直接干掉。
// 伪代码示例：统计滤波
for each point p in cloud:
distances = []
for each neighbor q in kdtree.nearest(p, k):
distances.append(dist(p, q))
mean_dist = avg(distances)
if mean_dist > threshold:
remove(p)
嗯，这里要注意：k 值一般取 20-50，阈值取 1-2 倍标准差。我调过很多次，这个范围比较稳。
3.1.2 降采样
点云太密了，计算量扛不住。用体素滤波，把空间切成小立方体，每个立方体里只保留一个重心点。体素大小我一般设 0.1m，室内够用，室外可以放宽到 0.2m。
3.1.3 地面滤除
路径规划只关心障碍物，地面是没用的。用RANSAC 平面拟合，把地面点云分离出来。我在项目中遇到过一个问题：如果地面有坡度，RANSAC 会误把斜坡当障碍物。后来我加了高度阈值限制，只保留高于地面 0.1m 以上的点，问题就解决了。
3.2 摄像头数据处理
摄像头给的是图像，信息量大，但噪声也多。我主要做三件事：畸变校正、图像增强、语义分割。
3.2.1 畸变校正
摄像头镜头有畸变，尤其是广角镜头。不做校正，你看到的障碍物位置就是歪的。用张正友标定法，先标定内参和畸变系数，然后实时校正。
// OpenCV 示例
cv::Mat cameraMatrix, distCoeffs;
cv::initUndistortRectifyMap(cameraMatrix, distCoeffs, cv::Mat(),
cameraMatrix, imageSize, CV_32FC1, map1, map2);
cv::remap(rawImage, undistortedImage, map1, map2, cv::INTER_LINEAR);
你想想看，如果这一步没做，后面路径规划算出来的避障点全是偏的，那还怎么玩？
3.2.2 图像增强
光线不好怎么办？我习惯用直方图均衡化，把图像对比度拉起来。如果是夜间，还得配合红外补光。有一次我在仓库里测试，光线暗得不行，摄像头几乎全黑。后来加了自适应直方图均衡化（CLAHE），才勉强能看清货架轮廓。
3.2.3 语义分割
现在深度学习成熟了，我一般用轻量级网络（比如 MobileNet-SSD）做实时语义分割。把行人、车辆、障碍物标出来。但注意：深度学习模型有延迟，我建议只做辅助，不要完全依赖。路径规划的核心还是激光雷达的几何信息。
3.3 IMU 与里程计数据融合
IMU 测加速度和角速度，里程计测轮子转了多少圈。但这两个东西都有硬伤：IMU 积分会漂移，里程计会打滑。所以必须融合。
3.3.1 IMU 预处理
IMU 原始数据有零偏和噪声。我一般先做零偏校准：让机器人静止 10 秒，取平均值作为零偏。然后做低通滤波，把高频噪声滤掉。
// 简单的一阶低通滤波
float alpha = 0.8; // 滤波系数
filtered_acc = alpha * raw_acc + (1 - alpha) * prev_filtered_acc;
为什么用 0.8？我试过 0.9 太滞后，0.5 太敏感。0.8 是个折中，适合大多数场景。
3.3.2 里程计预处理
里程计数据要小心打滑。我习惯加一个置信度判断：如果 IMU 检测到加速度突变（比如急加速或急刹车），就降低里程计的权重。说白了，就是「信 IMU 还是信轮子」的问题。
3.3.3 融合：扩展卡尔曼滤波（EKF）
这是最经典的方法。IMU 做预测，里程计做观测更新。我直接给个简化版流程：
- 预测步骤： 用 IMU 的角速度更新姿态，用加速度更新速度。
- 观测步骤： 用里程计的位置和速度修正预测结果。
- 更新协方差： 控制融合的权重。
我在项目中遇到过一个问题：EKF 的噪声协方差矩阵调不好，融合结果反而比单独用里程计还差。后来我用了自适应 EKF，根据实时残差动态调整噪声参数，效果好了很多。
3.4 多传感器时间同步
最后说一个容易被忽略的问题：时间同步。
激光雷达 10Hz，摄像头 30Hz，IMU 100Hz，里程计 50Hz。如果时间没对齐，你融合出来的数据就是「鸡同鸭讲」。我见过有人把 0.1 秒前的激光数据和 0.1 秒后的摄像头数据硬凑在一起，结果障碍物位置差了半米。
我的做法是：以激光雷达的时间戳为基准，其他传感器的数据通过插值对齐到同一个时间点。IMU 和里程计频率高，用线性插值就行。摄像头频率低，用最近邻插值。
// 时间同步伪代码
for each lidar_scan:
target_time = lidar_scan.timestamp
imu_data = interpolate(imu_buffer, target_time)
odom_data = interpolate(odom_buffer, target_time)
camera_data = nearest(camera_buffer, target_time)
fused_data = fuse(lidar_scan, imu_data, odom_data, camera_data)
嗯，这里要注意：插值会引入误差，但总比时间错乱好。我一般要求时间同步误差控制在 10ms 以内，超过这个值就丢弃这一帧数据。
3.5 小结
传感器数据处理，说白了就是「去噪、对齐、融合」六个字。激光雷达去噪降采样，摄像头校正增强，IMU 和里程计互补融合，最后时间同步。每一步都有坑，但踩过了就是经验。
我建议你在实际项目中，先单独调试每个传感器，确保数据质量过关，再做融合。别一上来就搞复杂算法，基础不牢，地动山摇。
下一节咱们聊路径规划的核心算法，到时候这些预处理好的数据就是你的「弹药」了。