第三讲：传感器仿真与数据读取——给机器人装上“眼睛”和“耳朵”

大家好，欢迎来到第三讲。

上一讲我们把机器人模型放进了Gazebo世界，它已经能动了。但说实话，一个没有传感器的机器人，就像蒙着眼睛走路——你根本不知道它周围是什么。今天我们就来解决这个问题：给机器人装上激光雷达和IMU，然后通过ROS2把数据读出来。

我个人习惯把传感器比作机器人的感官。激光雷达是“眼睛”，IMU是“内耳”——一个看外面，一个感知自身姿态。两者配合，导航才能稳。

3.1 激光雷达的仿真配置

先聊激光雷达。Gazebo里模拟激光雷达，其实就是在模型文件里加一个<sensor>标签。我最早做这个的时候，以为随便写写就行，结果雷达死活不出数据——后来发现是update_rate和range没配对。

来看一个典型的激光雷达SDF配置。嗯，这里我建议直接用SDF格式，比URDF更灵活：

<!-- 在 robot.sdf 中添加 -->
<sensor name="laser" type="ray">
  <pose>0.15 0 0.05 0 0 0</pose>
  <ray>
    <scan>
      <horizontal>
        <samples>360</samples>
        <resolution>1</resolution>
        <min_angle>-3.14159</min_angle>
        <max_angle>3.14159</max_angle>
      </horizontal>
    </scan>
    <range>
      <min>0.12</min>
      <max>3.5</max>
      <resolution>0.01</resolution>
    </range>
    <noise>
      <type>gaussian</type>
      <mean>0.0</mean>
      <stddev>0.01</stddev>
    </noise>
  </ray>
  <plugin name="laser_controller" filename="libgazebo_ros_ray_sensor.so">
    <ros>
      <namespace>/robot</namespace>
      <remapping>~/out:=scan</remapping>
    </ros>
    <output_type>sensor_msgs/LaserScan</output_type>
    <frame_name>laser_frame</frame_name>
  </plugin>
</sensor>

这里有几个参数我特别想强调：

• samples：360就是360度一圈，每度一个点。如果你想要更高精度，可以设720，但CPU负载会翻倍。
• min_angle / max_angle：-π到π就是全向扫描。有些机器人只需要前向180度，那就设-1.57到1.57。
• range.min：千万别设0！激光雷达有盲区，我见过有人设0.0，结果雷达自己身上的反射把数据全污染了。一般0.1~0.2比较合理。
• noise：加一点高斯噪声更真实。实际雷达都有噪声，完全干净的数据反而会让你在真机上翻车。

3.2 IMU的仿真配置

IMU相对简单一些。它输出角速度和线加速度，Gazebo里用imu传感器类型。我记得第一次做IMU仿真时，忘了加orientation的协方差矩阵，结果EKF融合时一直报错——嗯，这个坑我替你们踩过了。

<sensor name="imu" type="imu">
  <pose>0 0 0 0 0 0</pose>
  <imu>
    <angular_velocity>
      <noise>
        <type>gaussian</type>
        <mean>0.0</mean>
        <stddev>0.0002</stddev>
        <bias_mean>0.00001</bias_mean>
        <bias_stddev>0.00002</bias_stddev>
      </noise>
    </angular_velocity>
    <linear_acceleration>
      <noise>
        <type>gaussian</type>
        <mean>0.0</mean>
        <stddev>0.017</stddev>
        <bias_mean>0.01</bias_mean>
        <bias_stddev>0.02</bias_stddev>
      </noise>
    </linear_acceleration>
  </imu>
  <plugin name="imu_controller" filename="libgazebo_ros_imu_sensor.so">
    <ros>
      <namespace>/robot</namespace>
      <remapping>~/out:=imu/data</remapping>
    </ros>
    <frame_name>imu_frame</frame_name>
  </plugin>
</sensor>

注意看bias_mean和bias_stddev——这是模拟IMU零偏漂移的。实际IMU用久了温度升高，零偏会慢慢漂。Gazebo里加上这个，你的导航算法才会更鲁棒。

3.3 通过ROS2读取传感器数据

配置写好了，启动Gazebo，接下来就是读数据。我个人习惯先开三个终端：一个跑Gazebo，一个跑ros2 topic list，一个跑ros2 topic echo。

先确认话题存在：

ros2 topic list
# 应该能看到：
# /robot/scan
# /robot/imu/data
# /tf
# /tf_static

然后看激光数据：

ros2 topic echo /robot/scan

你会看到类似这样的输出：

---
header:
  stamp:
    sec: 1234
    nanosec: 567890123
  frame_id: laser_frame
angle_min: -3.14159
angle_max: 3.14159
angle_increment: 0.0174533
time_increment: 0.0
scan_time: 0.1
range_min: 0.12
range_max: 3.5
ranges: [3.5, 3.5, 3.5, ...]
---

如果ranges全是inf或NaN，别慌。大概率是雷达周围没物体，或者range_max设得太小。你可以在雷达前面放一堵墙试试。

再看IMU数据：

ros2 topic echo /robot/imu/data

---
header:
  stamp:
    sec: 1234
    nanosec: 567890123
  frame_id: imu_frame
orientation:
  x: 0.0
  y: 0.0
  z: 0.0
  w: 1.0
angular_velocity:
  x: 0.0
  y: 0.0
  z: 0.0
linear_acceleration:
  x: 0.0
  y: 0.0
  z: 9.81
---

注意linear_acceleration.z是9.81——这是重力加速度。如果IMU静止时z轴不是9.81，说明你的IMU坐标系和世界坐标系没对齐。我遇到过有人把IMU装反了，z轴朝下，结果加速度是-9.81，导航直接崩了。

3.4 用Rviz2可视化验证

光看数字不够直观。打开Rviz2，添加LaserScan和Imu显示：

ros2 run rviz2 rviz2

在Rviz2里：

1. Fixed Frame 设为 odom 或 base_link
2. Add → LaserScan → Topic选 /robot/scan
3. Add → Imu → Topic选 /robot/imu/data

你应该能看到激光点云围绕机器人一圈，IMU显示一个小坐标系在机器人中心。如果激光点云是乱的，检查一下frame_id是否和TF一致。

3.5 避坑指南——我踩过的那些坑

做传感器仿真这几年，我总结了几条血泪教训：

• 我曾经把激光雷达的min_angle和max_angle写反了，结果雷达扫描范围是负的，数据全是空的。后来我养成了习惯：写完配置先ros2 topic echo看一眼angle_min和angle_max。
• IMU的噪声参数别照搬真机手册。真机IMU的噪声模型很复杂，Gazebo的高斯模型只是近似。我一般先设小噪声跑通流程，再慢慢加大到接近真实值。
• 多个传感器共用一个frame_id会出大问题。激光雷达和IMU的frame必须不同，否则TF树会打架。我习惯用laser_frame和imu_frame区分。

好了，这一讲的内容就是这些。传感器配置看起来琐碎，但每一步都关系到后续导航的成败。你想想看，如果雷达数据都是错的，那路径规划再牛也没用，对吧？

去动手试试吧。把激光雷达和IMU加到你的机器人模型里，然后用ros2 topic echo看看数据。相信我，当你第一次看到激光点云在Rviz里亮起来的时候，那种成就感——嗯，很爽。