第4章传感器驱动与数据采集

第4章：传感器驱动与数据采集

好，咱们进入实战环节了。这一章我带你搞定三个最常用的传感器：激光雷达、IMU和摄像头。说白了，这就是机器人的「眼睛」和「平衡感」。我在多个项目里都跟它们打过交道，踩过的坑不少，今天一并告诉你。

4.1 激光雷达（RPLIDAR）驱动配置与数据读取

先聊激光雷达。RPLIDAR是思岚科技的明星产品，A1、A2、S1我都用过。我个人习惯用A2，性价比高，室内建图完全够用。

4.1.1 硬件连接与权限设置

拿到雷达后，先接USB。注意，RPLIDAR是串口通信，Linux下需要给权限。我刚开始做的时候，直接插上就跑，结果死活读不到数据。后来才发现是权限问题。

# 查看设备
ls -l /dev/ttyUSB*

# 临时授权
sudo chmod 666 /dev/ttyUSB0

# 永久授权（推荐）
sudo usermod -a -G dialout $USER
# 然后注销重新登录

注意： 永久授权后一定要重启终端或注销。我曾经忘了这步，折腾了半小时。

4.1.2 安装RPLIDAR ROS驱动

官方驱动在GitHub上，直接拉下来编译就行。

cd ~/catkin_ws/src
git clone https://github.com/Slamtec/rplidar_ros.git
cd ..
catkin_make

嗯，这里要注意。如果你用的是ROS Noetic，记得检查一下依赖。我遇到过编译报错，原因是缺少serial库。

sudo apt-get install ros-noetic-serial

4.1.3 启动雷达并读取数据

启动文件很简单，但参数要调对。我一般这样写launch文件：

<launch>
  <node name="rplidar_node" pkg="rplidar_ros" type="rplidarNode" output="screen">
    <param name="serial_port" value="/dev/ttyUSB0"/>
    <param name="serial_baudrate" value="115200"/>
    <param name="frame_id" value="laser"/>
    <param name="inverted" value="false"/>
    <param name="angle_compensate" value="true"/>
  </node>
</launch>

启动后，用rostopic看数据：

roslaunch rplidar_ros rplidar.launch
rostopic echo /scan

你会看到一堆角度和距离数据。为什么会有角度补偿？因为雷达旋转时，电机转速会有微小波动。补偿后数据更平滑。

小技巧： 用rviz可视化雷达数据，能直观看到扫描结果。我调试时必开rviz。

4.2 IMU（MPU6050）驱动配置与数据读取

IMU这东西，说白了就是加速度计+陀螺仪。MPU6050是经典款，便宜又好用。我在做平衡小车时用过它，当时被零漂问题折磨得不轻。

4.2.1 I2C连接与检测

MPU6050走I2C协议。连接很简单：VCC接3.3V，GND接地，SCL和SDA分别接I2C时钟和数据线。在树莓派或Jetson上，通常是引脚3和5。

# 检测I2C设备
sudo i2cdetect -y 1

如果看到0x68，说明连接成功。我遇到过检测不到的情况，十有八九是接线问题。检查一下SCL和SDA有没有接反。

4.2.2 安装MPU6050 ROS驱动

推荐用imu_calib和mpu6050_driver。安装命令：

sudo apt-get install ros-noetic-imu-tools
cd ~/catkin_ws/src
git clone https://github.com/ros-drivers/mpu6050_driver.git
cd ..
catkin_make

4.2.3 数据读取与校准

启动节点后，数据会发布在/imu/data_raw话题上。但原始数据有零漂，必须校准。

roslaunch mpu6050_driver mpu6050.launch
rostopic echo /imu/data_raw

你会看到这样的数据：

orientation: 
  x: 0.0
  y: 0.0
  z: 0.0
  w: 0.0
angular_velocity: 
  x: 0.02
  y: -0.01
  z: 0.03
linear_acceleration: 
  x: 0.05
  y: 0.02
  z: 9.81

注意看，静止时角速度不为零。这就是零漂。我曾经用这个数据直接做积分，结果角度越偏越大。后来才学会用互补滤波或卡尔曼滤波。

  校准方法： 静止采集1000个样本，取平均值作为偏置。然后在代码中减去这个偏置。

4.3 摄像头（USB_Cam）驱动配置与图像采集

摄像头这块，我建议用USB摄像头起步。便宜、通用、调试方便。我在做视觉导航时，用的就是普通的罗技C270。

4.3.1 检查摄像头设备

插上摄像头后，先看看系统认不认：

ls /dev/video*
v4l2-ctl --list-devices

如果看到video0，说明驱动加载成功。我遇到过插上没反应的情况，通常是USB口供电不足。换个口试试，或者用带供电的USB Hub。

4.3.2 安装USB_Cam驱动

ROS官方有usb_cam包，直接装：

sudo apt-get install ros-noetic-usb-cam

或者从源码编译：

cd ~/catkin_ws/src
git clone https://github.com/ros-drivers/usb_cam.git
cd ..
catkin_make

4.3.3 配置与启动

usb_cam的launch文件可以自定义参数。我一般这样配：

<launch>
  <node name="usb_cam" pkg="usb_cam" type="usb_cam_node" output="screen">
    <param name="video_device" value="/dev/video0"/>
    <param name="image_width" value="640"/>
    <param name="image_height" value="480"/>
    <param name="pixel_format" value="yuyv"/>
    <param name="camera_frame_id" value="usb_cam"/>
    <param name="io_method" value="mmap"/>
  </node>
</launch>

启动后，图像数据在/usb_cam/image_raw话题上。用image_view看：

rosrun image_view image_view image:=/usb_cam/image_raw

经验之谈： 像素格式选yuyv兼容性最好。mjpeg虽然压缩率高，但有些摄像头不支持。

4.3.4 图像采集与保存

需要保存图像时，可以用image_saver：

rosrun image_view image_saver image:=/usb_cam/image_raw _filename_format:=/home/user/images/frame%04d.jpg _save_all_image:=true

或者自己写个节点订阅话题。我习惯用OpenCV处理：

#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <cv_bridge/cv_bridge.h>

void imageCallback(const sensor_msgs::Image::ConstPtr& msg)
{
    cv::Mat frame = cv_bridge::toCvCopy(msg, "bgr8")->image;
    cv::imshow("Camera", frame);
    cv::waitKey(1);
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "image_listener");
    ros::NodeHandle nh;
    ros::Subscriber sub = nh.subscribe("/usb_cam/image_raw", 10, imageCallback);
    ros::spin();
    return 0;
}

4.4 多传感器时间同步

最后说个关键问题。三个传感器各自发布数据，时间戳可能对不上。我做融合时，发现激光雷达和摄像头的时间差有50ms，导致建图出现重影。

解决办法是用message_filters做时间同步：

#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/Image.h>

message_filters::Subscriber<sensor_msgs::LaserScan> laser_sub(nh, "/scan", 10);
message_filters::Subscriber<sensor_msgs::Image> image_sub(nh, "/usb_cam/image_raw", 10);
message_filters::TimeSynchronizer<sensor_msgs::LaserScan, sensor_msgs::Image> sync(laser_sub, image_sub, 10);
sync.registerCallback(boost::bind(&callback, _1, _2));

避坑指南： 时间同步的容忍度默认是0.1秒。如果传感器频率差异太大，可以调整slop参数。我曾经因为没调这个参数，丢了一半的数据。

好了，这一章的内容就这些。三种传感器的驱动配置和数据读取，你都应该能上手了。下一章我们聊聊数据预处理，这可是融合前的关键一步。

📚 课程目录