3、传感器数据处理基础：激光雷达、IMU与里程计
做SLAM这么多年，我越来越觉得传感器数据是整条流水线的“原材料”。原材料不好，后面算法再牛也白搭。今天咱们就聊聊激光雷达、IMU和里程计这三样东西——怎么读数据、怎么看数据、怎么让它们不坑你。
3.1 激光雷达数据格式与可视化
激光雷达分两种：2D和3D。2D的扫一个平面，3D的扫一个球面。你想想看，一个像扫地机器人那样转圈，一个像自动驾驶车顶那个“花盆”一样转。
3.1.1 2D激光雷达：LaserScan
ROS里2D激光雷达的消息类型是 sensor_msgs/LaserScan。说白了就是一个数组，每个元素代表某个角度测到的距离。
# LaserScan 消息结构
std_msgs/Header header # 时间戳、坐标系
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对应最小角度，那地图就会歪掉。嗯，这里要注意——一定要用角度值去算坐标，别用索引号硬算。
核心转换公式：将极坐标转成笛卡尔坐标
x = range * cos(angle_min + i * angle_increment)
y = range * sin(angle_min + i * angle_increment)3.1.2 3D激光雷达：PointCloud2
3D雷达的数据量大得多。ROS里用 sensor_msgs/PointCloud2。它不像LaserScan那样规整，每个点可以带xyz坐标、强度、时间戳、甚至RGB颜色。
我个人习惯用 pcl_ros 库来处理点云。它能把ROS消息直接转成PCL的点云对象，方便做滤波、分割、配准。
// C++ 示例：订阅点云并转成PCL格式
void cloudCallback(const sensor_msgs::PointCloud2ConstPtr& msg)
{
pcl::PointCloud<pcl::PointXYZ> cloud;
pcl::fromROSMsg(*msg, cloud);
// 现在可以愉快地使用PCL算法了
}
小技巧：3D点云数据量大，别在回调函数里做复杂计算。我一般用 tf2_ros::Buffer 缓存一下，再开个单独线程处理。
3.2 IMU数据读取与标定
IMU这东西，说白了就是加速度计+陀螺仪。加速度计测重力方向，陀螺仪测角速度。但你要直接拿原始数据用，那肯定翻车——因为IMU有零偏、有噪声、有温漂。
3.2.1 IMU消息类型
ROS里IMU的消息是 sensor_msgs/Imu：
std_msgs/Header header
geometry_msgs/Quaternion orientation # 姿态（通常由滤波算法给出）
float64[9] orientation_covariance # 姿态协方差
geometry_msgs/Vector3 angular_velocity # 角速度（rad/s）
float64[9] angular_velocity_covariance
geometry_msgs/Vector3 linear_acceleration # 线加速度（m/s²）
float64[9] linear_acceleration_covariance
注意：orientation 字段很多IMU驱动不填，或者填的是原始四元数。我建议你拿到数据后先检查一下这个字段是不是真的可用。
3.2.2 IMU标定：别跳过这一步
我曾经偷懒没标定IMU，结果融合出来的姿态每10分钟漂了30度。后来老老实实做了标定，效果立竿见影。
标定主要做两件事：
- 零偏标定：静止状态下采集一段时间数据，取均值作为零偏
- 尺度与对齐：加速度计三轴是否正交，陀螺仪是否与重力方向对齐
常用的标定工具有：
imu_utils：基于Allan方差分析，能给出噪声密度和零偏稳定性imu_calib：简单易用，适合快速标定
避坑指南：标定时IMU要放在水平面上，远离震动源。我曾经把IMU放在桌子上，旁边有人敲键盘，结果标定出来的零偏全是错的。嗯，后来我学乖了，标定时把IMU放在泡沫垫上。
3.3 里程计（Odometry）消息类型
里程计是SLAM的核心输出之一。ROS里用 nav_msgs/Odometry：
std_msgs/Header header
string child_frame_id # 通常为 "base_footprint" 或 "base_link"
geometry_msgs/PoseWithCovariance pose # 位置+姿态+协方差
geometry_msgs/TwistWithCovariance twist # 线速度+角速度+协方差
这里有个容易搞混的点：header.frame_id 是父坐标系（比如 odom），child_frame_id 是子坐标系（比如 base_link）。说白了，这个消息告诉你“机器人相对于世界坐标系在哪，以及它现在运动得多快”。
协方差矩阵很重要。它告诉滤波器“这个里程计数据有多可信”。我见过有人直接把协方差设成单位矩阵，结果融合出来的轨迹抖得像筛子。
经验值参考：
| 传感器类型 | 位置协方差（m²） | 角度协方差（rad²） |
|---|---|---|
| 轮式里程计 | 0.01 ~ 0.1 | 0.001 ~ 0.01 |
| 激光里程计 | 0.001 ~ 0.01 | 0.0001 ~ 0.001 |
| 视觉里程计 | 0.01 ~ 0.5 | 0.001 ~ 0.1 |
3.4 使用RViz进行传感器数据可视化
RViz是ROS的“眼睛”。你写再多代码，不如在RViz里看一眼数据来得直观。
3.4.1 基本配置
启动RViz后，添加以下显示项：
- LaserScan：显示2D激光雷达数据，选好话题名和颜色
- PointCloud2：显示3D点云，可以按高度或强度着色
- Imu：显示IMU的加速度和角速度箭头
- Odometry：显示机器人位姿和路径
我个人习惯把 Fixed Frame 设为 map 或 odom，这样所有数据都在同一个坐标系下显示。
3.4.2 调试技巧
你想想看，如果激光雷达数据在RViz里显示的位置和实际对不上，那多半是TF树没配好。我一般先检查 tf_echo 看看坐标系变换对不对。
还有一个常见问题：点云显示出来全是黑的。那是因为点云没有颜色信息，或者强度值范围不对。把 Color Transformer 改成 AxisColor 或者 Intensity 就能看到东西了。
调试口诀：数据不对看话题，位置不对看TF，颜色不对看着色器。
3.5 本章知识体系
下面这张图总结了传感器数据处理的整体流程。从原始数据到可视化，每一步都有坑，但每一步也都有套路。
好了，这一章的内容就这些。传感器数据处理是SLAM的基石，别嫌它基础。你想想看，如果连数据都读不对、看不明，后面建图导航再花哨也是空中楼阁。
可可精品资料库