📚 课程目录第3章机器人传感器接入与数据采集

3. 机器人传感器接入与数据采集

机器人感知系统的第一道工程门槛，不是算法，而是“把传感器接进来，并且让数据在时间与空间上对得上”。在 Jetson 平台上，RGB 相机、深度相机、LiDAR、IMU 和编码器分属不同物理接口、不同驱动模型、不同数据速率，最终都要汇入 ROS 2 的 topic 体系。本节围绕“接入—驱动—时间—标定”四条主线，说明每类传感器的关键参数、接入方式与数据判读方法。

一、五类传感器的接口与数据特征

RGB 相机通常通过 MIPI CSI-2 或 USB 3.0 接入。Jetson Orin Nano 提供 2 路 MIPI CSI-2 接口，配合 nvarguscamerasrc 或 V4L2 可输出 1920×1080@30fps 的 YUV/RGB 数据。深度相机分两类：结构光/双目（如 Intel RealSense D435i）走 USB 3.0，输出深度图与 RGB 图；ToF 相机则直接给出每像素距离。LiDAR 以 2D 单线（如 RPLIDAR A1，10Hz、360 点/圈）和 3D 多线（如 Velodyne VLP-16，10Hz、约 28.8 万点/秒）为主，接口多为 USB 转串口或以太网 UDP。IMU 通过 I²C 或 SPI 接入，典型输出 100～1000Hz 的加速度、角速度，部分含磁力计。编码器则通过 GPIO 正交解码或 MCU 计数后经串口上报，用于轮式里程计。

这些差异决定了接入策略：CSI 相机延迟低但通道有限；USB 相机即插即用但带宽共享；LiDAR 点云数据量大，必须走 UDP 并注意 MTU；IMU 频率高，适合做时间基准的“高频参考”。

二、ROS 2 驱动接入的通用流程

无论哪类传感器，接入 ROS 2 都遵循同一套动作：安装驱动包 → 确认设备节点 → 启动节点 → 检查 topic 与 frame_id → 用可视化工具验证。以 RealSense D435i 为例：

# 安装驱动
sudo apt install ros-humble-realsense2-camera
# 启动节点，开启深度、彩色、IMU
ros2 launch realsense2_camera rs_launch.py \
  enable_depth:=true enable_color:=true enable_imu:=true
# 查看话题
ros2 topic list
# 预期看到 /camera/color/image_raw、/camera/depth/image_rect_raw、
# /camera/imu、/camera/color/camera_info
# 检查频率
ros2 topic hz /camera/color/image_raw   # 约 30Hz
ros2 topic hz /camera/imu               # 约 200Hz

关键判断点有三个：一是 frame_id 是否与 URDF 中的 link 名称一致，否则 TF 树断裂；二是 camera_info 是否携带内参，深度图与彩色图能否对齐；三是频率是否稳定，若 hz 波动超过 20%，需排查 USB 带宽或 CPU 占用。

三、时间同步：让数据“同时发生”

多传感器融合的前提是时间戳可比。ROS 2 默认使用系统时钟，但不同驱动的时间戳来源不同：相机用曝光时刻，IMU 用采样时刻，LiDAR 用扫描起始或结束时刻。若不做处理，融合时会出现“用 100ms 前的 IMU 去配当前图像”的错误。

工程上有三种做法。第一，硬件触发：用同一 GPIO 脉冲同时触发相机曝光和 IMU 采样，时间偏差可控制在微秒级。第二，PTP/NTP 对时：LiDAR 支持 PTP 时，将其与 Jetson 系统时钟同步，误差可到亚毫秒。第三，软件近似：用 message_filters 的 ApproximateTime 策略，允许时间戳差异在设定阈值内配对。示例：

import message_filters
from sensor_msgs.msg import Image, Imu

def callback(img, imu):
    # 配对后的融合处理
    pass

img_sub = message_filters.Subscriber(self, Image, '/camera/color/image_raw')
imu_sub = message_filters.Subscriber(self, Imu, '/camera/imu')
sync = message_filters.ApproximateTimeSynchronizer(
    [img_sub, imu_sub], queue_size=10, slop=0.02)  # 允许 20ms 偏差
sync.registerCallback(callback)

slop 的取值需要根据传感器频率权衡：IMU 200Hz、相机 30Hz 时，20ms 是合理起点；若 slop 过大，会引入错误配对，过小则大量丢帧。

四、标定基础：内参、外参与手眼关系

标定解决“像素坐标如何映射到机器人坐标”。相机内参（焦距 fx、fy，主点 cx、cy，畸变系数）通过棋盘格标定获得，ROS 2 提供 camera_calibration 包：

ros2 run camera_calibration cameracalibrator \
  --size 8x6 --square 0.025 \
  image:=/camera/color/image_raw camera:=/camera/color

采集 20～30 组不同位姿后点击 CALIBRATE，得到 ost.yaml，其中 fx≈600、cx≈320（以 640×480 为例）为典型值。外参标定则确定相机与 LiDAR、IMU 之间的旋转平移矩阵，常用 lidar_camera_calibration 或 kalibr。IMU 还需标定零偏与噪声密度，否则积分漂移会迅速累积。

标定结果的验证方法是重投影误差：将标定板角点投影回图像，误差应小于 0.5 像素；若超过 1 像素，说明采集位姿覆盖不足或标定板检测不准，需要重做。

五、数据采集的验证与常见问题

接入完成后，必须做一次“端到端验证”。以 LiDAR 为例：启动 ros2 launch rplidar_ros rplidar.launch.py 后，用 ros2 topic echo /scan --once 查看 ranges 数组长度是否为 360，angle_min 是否为 -π。若长度异常，说明串口波特率或扫描模式配置错误。IMU 则检查 linear_acceleration.z 静止时是否接近 9.8，若偏差超过 0.5，需重新标定零偏。

常见问题有三类：一是 frame_id 拼写不一致导致 TF 报错，解决方法是统一 URDF 与驱动参数；二是 USB 带宽不足导致图像丢帧，可将相机分辨率降至 640×480 或改用 MIPI 接口；三是时间戳跳变，多因系统时钟被 NTP 校正，建议在采集期间关闭自动对时，改用 PTP 或硬件触发。

建议在正式采集前，用 ros2 bag record 录制 30 秒全传感器数据，回放时检查各 topic 时间戳是否单调递增、TF 树是否完整。这是发现同步与标定问题最经济的手段。

传感器接入不是一次性配置，而是“接入—验证—标定—再验证”的循环。只有时间戳对齐、坐标变换正确、内参可信，后续的 SLAM、目标检测与路径规划才有可靠输入。下一阶段将在此基础上展开感知算法的部署与优化。