第1章：传感器与数据预处理——激光雷达、深度相机、IMU的数据特点与滤波
大家好，我是你们的老朋友。做机器人这么多年，我踩过最大的坑，就是「传感器数据还没洗干净，就直接扔给算法吃」。结果呢？定位飘、建图糊、路径规划撞墙。今天这一章，咱们就把激光雷达、深度相机、IMU这三个传感器的「脾气」摸透，再聊聊怎么给它们「洗澡」——也就是滤波。
1.1 激光雷达：点云里的「硬汉」
激光雷达，说白了就是个「激光尺子」在疯狂旋转。它发射激光束，打到物体上反射回来，通过时间差算出距离。我最早接触的是16线雷达，那时候觉得「哇，好酷」，后来才发现，这玩意儿的数据坑也不少。
1.1.1 数据特点
- 稀疏性：16线雷达一圈也就30万个点，跟相机动辄几百万像素比，简直「寒酸」。但好处是计算量小。
- 距离精度高：毫米级误差，这是它的看家本领。我在做AGV（自动导引车）时，全靠它来识别货架腿。
- 受环境影响大：下雨天、大雾天，点云里全是「噪点」。有一次我在室外测试，一场小雨下来，点云直接变成了「雪花屏」。
- 视角有限：水平360度，垂直视角一般只有30度左右。所以天花板上的东西，它基本看不见。
1.1.2 常见滤波方法
嗯，这里要注意，滤波不是「乱杀」，而是「精准打击」。我一般按这个顺序来：
- 直通滤波：先砍掉太远（比如>50米）和太近（比如<0.3米）的点。太近的点往往是雷达自身反射，太远的点噪声太大。
- 体素滤波：把空间切成小格子，每个格子只保留一个点。这样点云密度就均匀了。我习惯用0.05米的格子，既保留细节又降采样。
- 统计滤波：计算每个点周围邻居的平均距离，如果偏离太大，就干掉。这招对付「离群点」特别管用。
重要提醒：滤波顺序不能乱！先直通再体素，最后统计。否则体素滤波会把离群点「平均」掉，统计滤波就失效了。
给你们看一段我常用的PCL代码：
// 直通滤波
pcl::PassThrough<pcl::PointXYZ> pass;
pass.setInputCloud(cloud);
pass.setFilterFieldName("z");
pass.setFilterLimits(0.3, 50.0);
pass.filter(*cloud_filtered);
// 体素滤波
pcl::VoxelGrid<pcl::PointXYZ> voxel;
voxel.setInputCloud(cloud_filtered);
voxel.setLeafSize(0.05f, 0.05f, 0.05f);
voxel.filter(*cloud_voxel);
// 统计滤波
pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
sor.setInputCloud(cloud_voxel);
sor.setMeanK(50);
sor.setStddevMulThresh(1.0);
sor.filter(*cloud_clean);个人经验：setMeanK的值别太大，50就够。太大反而会把边缘特征点给「温柔地」删掉。我曾经因为这个，让机器人在墙角处定位失败，折腾了两天。
1.2 深度相机：RGB-D的「双刃剑」
深度相机，比如Kinect、RealSense，能同时给你彩色图和深度图。听起来很完美？其实不然。
1.2.1 数据特点
- 稠密性：每个像素都有深度值，信息量巨大。但这也意味着计算量爆炸。
- 精度随距离下降：1米以内精度不错，超过3米就开始「飘」了。我试过用RealSense D435测5米外的墙，误差能到10厘米。
- 对光照敏感：强光下深度值全是NaN（无效值），弱光下又全是噪声。室内用还行，室外基本「歇菜」。
- 边缘效应：物体边缘的深度值经常「跳变」，因为红外光打到边缘会散射。
1.2.2 常见滤波方法
深度图的滤波，我建议分两步走：
- 深度值有效性检查：先把NaN和0值过滤掉。0值通常是传感器没测到，直接扔掉。
- 双边滤波：既能平滑噪声，又能保留边缘。比高斯滤波强多了。高斯滤波会把边缘也糊掉，你想想看，那墙角还怎么识别？
避坑指南：我曾经直接用OpenCV的高斯滤波处理深度图，结果机器人在门框处直接「穿模」了。因为门框边缘被平滑成了圆弧，路径规划以为那里能过。后来换成双边滤波，问题解决。
代码示例：
// 双边滤波
cv::Mat depth_filtered;
cv::bilateralFilter(depth_img, depth_filtered, 9, 75, 75);
// 或者用inpaint填补小空洞
cv::inpaint(depth_img, mask, depth_inpainted, 3, cv::INPAINT_TELEA);1.3 IMU：又爱又恨的「加速度计+陀螺仪」
IMU，惯性测量单元。它测量加速度和角速度。优点是不依赖外部信号，缺点嘛...你懂的，积分一次就漂移，积分两次就「上天」。
1.3.1 数据特点
- 高频：通常100Hz以上，比激光雷达（10Hz）和相机（30Hz）快得多。适合做短时间内的「插值」。
- 漂移严重：陀螺仪积分得到角度，几分钟就偏好几度。加速度计积分得到位置，几秒钟就飞了。
- 噪声大：尤其是低成本MEMS（微机电系统）IMU，比如MPU6050，静止时都能看到明显的跳动。
- 有偏置：静止时，陀螺仪输出不为0，加速度计输出不是[0,0,9.8]。这个偏置会随时间变化。
1.3.2 常见滤波方法
IMU滤波，我推荐两个经典方案：
- 低通滤波：对加速度计数据做低通，去掉高频振动。比如机器人走路时的震动。
- 互补滤波：融合加速度计和陀螺仪的姿态。加速度计低频准但高频噪，陀螺仪高频准但低频漂。互补滤波就是「取长补短」。
核心公式：互补滤波的权重通常取0.98（陀螺仪）和0.02（加速度计）。这个比例是我试出来的，效果不错。
代码示例：
// 互补滤波（简化版）
float alpha = 0.98;
float pitch = alpha * (pitch + gyro_y * dt) + (1 - alpha) * acc_pitch;
float roll = alpha * (roll + gyro_x * dt) + (1 - alpha) * acc_roll;我的习惯：在实际项目中，我会先用半小时的静态数据来标定IMU的偏置。静止时取平均值，然后减掉。这步不做，后面所有滤波都是白搭。
1.4 多传感器时间同步：一个容易被忽视的「大坑」
你想想看，激光雷达的数据是10Hz，IMU是100Hz，相机是30Hz。如果时间不同步，你拿激光雷达的位姿去对齐相机的图像，那误差能让你怀疑人生。
我常用的方法有两种：
- 硬件同步：用FPGA或MCU给所有传感器发同一个触发信号。这是最准的，但成本高。
- 软件同步：用ROS的TimeSynchronizer，或者自己写一个基于时间戳的插值。我一般用后者，因为灵活。
避坑指南：我曾经在ROS里直接用TimeSynchronizer，结果发现激光雷达和相机的时间戳差了50ms。查了半天，原来是两个传感器的时钟不同步。后来我改用NTP（网络时间协议）统一时钟，问题解决。
1.5 总结与建议
好了，这一章的内容就这些。我帮你理一下重点：
| 传感器 | 核心问题 | 推荐滤波 |
|---|---|---|
| 激光雷达 | 稀疏、噪点、离群点 | 直通→体素→统计 |
| 深度相机 | 精度下降、边缘效应、空洞 | 有效性检查→双边滤波 |
| IMU | 漂移、噪声、偏置 | 低通滤波→互补滤波 |
最后说一句：数据预处理占整个SLAM系统工作量的30%以上。别急着跑算法，先把数据洗干净。否则，你后面花再多时间调参，都是白费功夫。
下一章，咱们聊聊「坐标系与坐标变换」——这可是所有算法的数学基础。到时候见！