第2章 传感器数据获取与处理：激光雷达驱动配置、点云数据预处理、里程计数据融合
各位同学，欢迎来到第二章。上一章我们把机械臂的硬件平台搭起来了，也聊了聊ROS2的基本玩法。这一章，咱们要干点实在的——让机械臂“睁开眼睛”，学会感知周围的世界。
说白了，就是搞定两样东西：激光雷达（LiDAR）和里程计（Odometry）。一个负责看路，一个负责记路。两者一结合，你的机械臂才能知道自己在哪，周围有什么。
核心逻辑：激光雷达提供环境轮廓，里程计提供位姿变化。两者融合，才能构建出靠谱的地图。
2.1 激光雷达驱动配置——让雷达转起来
先说说激光雷达。市面上常见的单线雷达，比如思岚A1/A2、镭神N10，驱动方式大同小异。我个人习惯用slamtec_rplidar这个包，稳定，社区活跃。
安装很简单，一条命令搞定：
sudo apt install ros-humble-rplidar-ros然后启动驱动节点：
ros2 launch rplidar_ros rplidar_a1_launch.py嗯，这里要注意。如果你用的是USB转串口，记得给设备权限。我遇到过好几次，插上雷达没反应，一查是权限问题。
避坑指南：我曾经在调试时，雷达死活不出数据。折腾了半天，发现是USB口供电不足。雷达启动瞬间电流大，建议用带外接电源的USB Hub，或者直接接5V供电。
启动后，用ros2 topic echo /scan看看有没有数据。如果看到一堆浮点数，恭喜你，雷达活了。
2.2 点云数据预处理——去噪、降采样、分割
原始点云数据很“脏”。为什么？因为激光会打到玻璃、反光面、甚至灰尘上。直接拿来做地图，结果就是一团糟。
所以，预处理是必须的。我一般做三步：
- 直通滤波（PassThrough）——砍掉没用的远距离点。比如机械臂工作范围就3米，10米外的点留着干嘛？
- 体素滤波（VoxelGrid）——降采样。点云太密了，计算量扛不住。体素滤波相当于把空间切成小格子，每个格子只留一个代表点。
- 半径滤波（RadiusOutlierRemoval）——去掉孤立噪点。如果一个点周围邻居太少，八成是噪声，删掉。
代码示例，我用的是PCL库，在ROS2里直接调用：
#include <pcl/filters/passthrough.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/radius_outlier_removal.h>
// 1. 直通滤波
pcl::PassThrough<pcl::PointXYZ> pass;
pass.setInputCloud(cloud);
pass.setFilterFieldName("z");
pass.setFilterLimits(0.1, 3.0); // 只保留0.1米到3米之间的点
// 2. 体素滤波
pcl::VoxelGrid<pcl::PointXYZ> voxel;
voxel.setInputCloud(cloud_filtered);
voxel.setLeafSize(0.05f, 0.05f, 0.05f); // 5cm格子
// 3. 半径滤波
pcl::RadiusOutlierRemoval<pcl::PointXYZ> outrem;
outrem.setInputCloud(cloud_filtered);
outrem.setRadiusSearch(0.1);
outrem.setMinNeighborsInRadius(3);个人经验：体素滤波的格子大小很关键。设太大，细节全丢；设太小，计算量爆炸。我一般从0.05米开始调，根据实际场景微调。室内环境0.05米够用，室外空旷场景可以放到0.1米。
2.3 里程计数据融合——让机械臂知道自己走了多远
激光雷达告诉你“周围有什么”，但没告诉你“我在哪”。这时候就需要里程计了。
里程计数据来源有两种：
- 轮式编码器：通过轮子转了多少圈，推算位移。简单，但容易打滑。
- IMU（惯性测量单元）：通过加速度和角速度积分，推算位姿。不依赖轮子，但有累积误差。
我建议两者都用，做数据融合。说白了，就是取长补短。
融合的核心是时间戳同步和坐标变换。ROS2里用tf2库处理坐标变换，用message_filters做时间同步。
看代码：
# Python示例
import message_filters
from sensor_msgs.msg import LaserScan, Imu
from nav_msgs.msg import Odometry
def callback(scan, imu, odom):
# 三个数据的时间戳已经对齐
# 在这里做融合处理
pass
scan_sub = message_filters.Subscriber(node, LaserScan, '/scan')
imu_sub = message_filters.Subscriber(node, Imu, '/imu/data')
odom_sub = message_filters.Subscriber(node, Odometry, '/odom')
ts = message_filters.ApproximateTimeSynchronizer(
[scan_sub, imu_sub, odom_sub], queue_size=10, slop=0.1)
ts.registerCallback(callback)核心要点：时间同步的slop参数别设太大。0.1秒以内比较安全。设大了，数据错位，地图会飘。
融合后的数据，我一般发布成一个nav_msgs/Odometry话题，里面包含位置、姿态、速度信息。这样下游的建图节点直接订阅就行。
2.4 实战中的坑与技巧
最后，分享几个我踩过的坑：
- 雷达安装位置：别装在机械臂正下方。激光扫到机械臂本体，会产生大量无效点云。我一般装在机械臂上方30cm处，斜向下扫。
- IMU零偏校准：IMU上电后，先静止10秒，让算法自动校准零偏。否则积分出来的角度会一直漂。
- 数据频率匹配：激光雷达一般10Hz，IMU可能100Hz。融合时，以低频数据为准，高频数据做插值。别反过来，否则CPU扛不住。
好了，这一章的内容就到这里。传感器数据获取与处理，是建图与导航的基石。基础打牢了，后面的事情就顺了。