4. 传感器数据基础：激光雷达点云、深度图与IMU

各位同学，欢迎来到第四章。前面几章我们搭好了MoveIt的骨架，今天开始给它装上「眼睛」和「耳朵」——传感器数据。

说实话，做机器人运动规划，最怕的就是「闭着眼睛走路」。我见过太多项目，规划算法写得天花乱坠，结果传感器数据一进来就崩了。为什么？因为没搞懂数据格式。

这一章，我们就来啃下这三块硬骨头：激光雷达点云、RGB-D深度图、IMU惯性数据。别怕，我会把我在项目里踩过的坑都告诉你。

核心观点：传感器数据是机器人感知的「原材料」。格式不对，后面全是白费。

传感器数据基础体系 激光雷达点云 sensor_msgs/PointCloud2 RGB-D 深度图 sensor_msgs/Image IMU 惯性数据 sensor_msgs/Imu xyz + intensity + ring 深度值 + 相机内参 角速度 + 加速度 + 朝向 ROS 消息类型统一管理，通过话题发布/订阅

4.1 激光雷达点云数据格式

先聊聊激光雷达。我最早接触激光雷达是在2018年，那时候用的还是VLP-16。嗯，就是那个16线的「小蘑菇」。当时我犯过一个低级错误——直接把点云数据塞进规划器，结果程序直接崩了。后来才发现，点云的数据结构远比我想象的复杂。

在ROS里，点云的标准消息类型是 sensor_msgs/PointCloud2。你想想看，它为什么不是 PointCloud？因为老版本确实有 PointCloud，但后来被废弃了。为什么？因为 PointCloud2 更灵活，支持自定义字段。

来看它的核心结构：

# sensor_msgs/PointCloud2
std_msgs/Header header          # 时间戳和坐标系
uint32 height                   # 点云高度（1表示无序点云）
uint32 width                    # 点云宽度（点数）
sensor_msgs/PointField[] fields # 每个点的字段定义
bool is_bigendian               # 字节序
uint32 point_step               # 每个点的字节数
uint32 row_step                 # 每行的字节数
uint8[] data                    # 原始点云数据
bool is_dense                   # 是否包含无效点

这里我要特别强调 fields 这个字段。它定义了每个点包含什么信息。常见的字段有：

  • x, y, z —— 三维坐标，这个不用多说
  • intensity —— 反射强度，对识别地面和障碍物很有用
  • ring —— 激光线束编号，多线雷达特有
  • time —— 相对于扫描起始的时间偏移

我的小技巧： 处理点云时，先用 pcl::fromROSMsg() 转成PCL格式再操作。直接操作原始字节数组？我试过，太容易出错了。

举个例子，如果你拿到一个Velodyne的点云，它的 fields 通常长这样：

fields: [
  {name: "x", offset: 0, datatype: FLOAT32, count: 1},
  {name: "y", offset: 4, datatype: FLOAT32, count: 1},
  {name: "z", offset: 8, datatype: FLOAT32, count: 1},
  {name: "intensity", offset: 12, datatype: FLOAT32, count: 1},
  {name: "ring", offset: 16, datatype: UINT16, count: 1}
]

你看，每个点占20个字节。如果你直接按 sizeof(float)*3 去读，那 intensity 和 ring 就全乱了。我曾经就在这上面浪费了一整天。

4.2 RGB-D相机深度图

接下来是RGB-D相机。说白了，就是同时给你一张彩色图和一张深度图。常见的传感器有Intel RealSense、Microsoft Kinect、Orbbec等。

深度图的消息类型是 sensor_msgs/Image，和普通图像一样。但区别在于编码格式：

编码格式 含义 典型值范围
16UC1 16位无符号整数，单通道 0 ~ 65535（单位mm）
32FC1 32位浮点数，单通道 0.0 ~ 10.0（单位m）
8UC3 8位彩色图（RGB） 0 ~ 255

我个人习惯用 16UC1，因为精度够用，而且传输效率高。但要注意：深度值默认单位是毫米。如果你直接拿它做碰撞检测，记得除以1000转成米。

避坑指南： 我曾经在项目里直接用深度图的像素值做距离判断，结果规划出来的路径全在「地下」。查了半天才发现，深度值没转单位。嗯，从那以后我每次拿到深度图，第一件事就是检查编码格式和单位。

深度图还有一个关键参数——相机内参。它决定了如何把像素坐标映射到三维空间。内参矩阵长这样：

K = [fx  0  cx]
    [ 0  fy cy]
    [ 0   0  1]

其中 fx, fy 是焦距，cx, cy 是光心坐标。有了内参，你才能把深度图转成点云。ROS里用 sensor_msgs/CameraInfo 来传递内参。

你想想看，如果没有内参，你只知道「这个像素距离相机1米」，但不知道它在空间哪个方向。那怎么行？

4.3 IMU惯性测量单元数据

最后聊聊IMU。IMU这个东西，说简单也简单，说复杂也复杂。它通常包含三部分数据：

  • 角速度（gyroscope）—— 绕三个轴的旋转速度，单位 rad/s
  • 线加速度（accelerometer）—— 三个轴的加速度，单位 m/s²
  • 朝向（orientation）—— 用四元数表示，可选

在ROS里，IMU的消息类型是 sensor_msgs/Imu：

# sensor_msgs/Imu
std_msgs/Header header
geometry_msgs/Quaternion orientation
float64[9] orientation_covariance
geometry_msgs/Vector3 angular_velocity
float64[9] angular_velocity_covariance
geometry_msgs/Vector3 linear_acceleration
float64[9] linear_acceleration_covariance

注意看，每个数据都带了一个 协方差矩阵。这个矩阵告诉你数据的可信度。对角线元素越大，说明噪声越大。我见过很多新手直接忽略协方差，结果融合出来的姿态一塌糊涂。

重要提醒： IMU的加速度数据包含重力分量。静止时，z轴加速度应该是9.8 m/s²，不是0。如果你要做运动检测，记得先去除重力。

举个例子，假设你拿到一组IMU数据：

angular_velocity: 
  x: 0.01, y: -0.02, z: 0.15
linear_acceleration:
  x: 0.12, y: -0.08, z: 9.81
orientation:
  x: 0.0, y: 0.0, z: 0.0, w: 1.0

你看，z轴加速度是9.81，说明传感器是水平的。角速度z是0.15，说明它在缓慢旋转。如果你要做航位推算，这些数据就是输入。

但我要提醒你：IMU数据有漂移。纯靠积分算位置，几分钟就飘到天边去了。我做过一个测试，把IMU放在桌上不动，积分10分钟，位置漂了50多米。所以IMU通常要和视觉或激光雷达做融合。

4.4 传感器消息类型总结

好了，三种传感器都聊完了。我们来个快速总结：

传感器 ROS消息类型 关键字段 常见坑点
激光雷达 sensor_msgs/PointCloud2 fields, data, point_step 字段偏移量计算错误
RGB-D相机 sensor_msgs/Image encoding, data 深度单位搞错（mm vs m）
IMU sensor_msgs/Imu orientation, angular_velocity, linear_acceleration 忽略协方差、未去除重力

我个人建议，拿到任何传感器数据，第一件事就是 rostopic echo /topic_name 看看原始格式。别急着写代码，先搞清楚数据长什么样。这一步做好了，后面就顺了。

实用工具推荐： 用 rqt_image_view 看深度图，用 rviz 看点云，用 rostopic echo 看IMU数据。这三个工具能帮你快速验证数据是否正确。

嗯，这一章的内容就到这里。记住，传感器数据是机器人感知的「原材料」。格式不对，后面全是白费。下一章我们会把这些数据用起来，做传感器融合。到时候你就知道，今天打的基础有多重要了。