📚 课程目录第3章传感器仿真

3. 传感器仿真

传感器仿真，说白了就是给机器人装上一双“虚拟的眼睛和耳朵”。

我在做真机迁移时，踩过最大的坑就是——仿真里跑得飞起，一上真机就瞎了。为什么？因为传感器模型没配好。Gazebo里的传感器不是随便拖个插件就能用的，你得理解它的参数、噪声模型，还有数据怎么读出来。

这一章，我们就来搞定三样核心传感器：激光雷达、IMU、深度相机。我会把我在项目中遇到的坑和配置技巧，一并告诉你。

3.1 激光雷达（LiDAR）仿真配置

激光雷达是机器人感知环境的“眼睛”。在Gazebo里，我们通常用 gazebo_ros_ray_sensor 或 gazebo_ros_laser 插件来模拟。

3.1.1 基本配置

我个人习惯用 gazebo_ros_laser，因为它直接输出 sensor_msgs/LaserScan 消息，和真机一致。下面是一个典型的URDF配置片段：

<gazebo reference="laser_link">
  <sensor type="ray" name="head_laser">
    <pose>0 0 0 0 0 0</pose>
    <visualize>true</visualize>
    <update_rate>10</update_rate>
    <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.10</min>
        <max>30.0</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_laser.so">
      <topicName>/scan</topicName>
      <frameName>laser_link</frameName>
    </plugin>
  </sensor>
</gazebo>

这里有几个关键参数：

samples：扫描点数，360就是每度一个点。我建议真机迁移时用720点，精度更高。
min_angle / max_angle：扫描范围，-π到π就是360度全覆盖。
noise：高斯噪声模型。我曾经在仿真里把噪声设成0，结果真机上激光雷达一抖，定位直接崩了。所以，噪声一定要加！

注意： 如果你用的是3D激光雷达（比如Velodyne），要用 gazebo_ros_gpu_laser 插件，它支持GPU加速，性能更好。

3.1.2 数据读取

配置好之后，启动Gazebo，激光雷达数据就会发布到 /scan 话题。你可以用 rostopic echo /scan 查看原始数据，或者用Rviz可视化。

在代码里读取也很简单：

import rospy
from sensor_msgs.msg import LaserScan

def scan_callback(msg):
    # msg.ranges 就是距离数组
    # msg.angle_min, msg.angle_max 是角度范围
    print(f"收到 {len(msg.ranges)} 个扫描点")
    print(f"最近距离: {min(msg.ranges):.2f} 米")

rospy.init_node('scan_listener')
rospy.Subscriber('/scan', LaserScan, scan_callback)
rospy.spin()

嗯，这里要注意：msg.ranges 里的 inf 表示没有检测到物体，记得做过滤。

3.2 IMU仿真配置

IMU（惯性测量单元）提供加速度和角速度。在Gazebo里，我们用 gazebo_ros_imu 插件。

3.2.1 基本配置

<gazebo reference="imu_link">
  <sensor type="imu" name="imu_sensor">
    <pose>0 0 0 0 0 0</pose>
    <update_rate>100</update_rate>
    <plugin name="imu_controller" filename="libgazebo_ros_imu.so">
      <topicName>/imu/data</topicName>
      <frameName>imu_link</frameName>
      <bodyName>base_footprint</bodyName>
    </plugin>
  </sensor>
</gazebo>

IMU的更新率我一般设100Hz，因为真机IMU通常也是100-200Hz。太低的话，里程计融合会出问题。

小技巧： 如果你做VIO（视觉惯性里程计），IMU和相机的坐标系要对齐。我习惯把IMU放在相机光心附近，减少外参标定的麻烦。

3.2.2 噪声与漂移

Gazebo默认的IMU是理想模型，没有噪声。但真机IMU有零偏和随机游走。我建议加一个噪声模型：

<noise>
  <type>gaussian</type>
  <mean>0.0</mean>
  <stddev>0.001</stddev>  <!-- 加速度噪声 -->
  <stddev_angular>0.001</stddev_angular>  <!-- 角速度噪声 -->
</noise>

我曾经在仿真里用理想IMU，结果真机上EKF直接发散。后来加了噪声，仿真和真机的表现就非常接近了。

3.2.3 数据读取

from sensor_msgs.msg import Imu

def imu_callback(msg):
    # 线性加速度 (m/s^2)
    ax = msg.linear_acceleration.x
    ay = msg.linear_acceleration.y
    az = msg.linear_acceleration.z
    # 角速度 (rad/s)
    wx = msg.angular_velocity.x
    wy = msg.angular_velocity.y
    wz = msg.angular_velocity.z
    print(f"加速度: ({ax:.2f}, {ay:.2f}, {az:.2f})")
    print(f"角速度: ({wx:.2f}, {wy:.2f}, {wz:.2f})")

rospy.Subscriber('/imu/data', Imu, imu_callback)

3.3 深度相机仿真配置

深度相机（如Intel RealSense、Kinect）能同时提供RGB图像和深度图。Gazebo里用 gazebo_ros_openni_kinect 或 gazebo_ros_depth_camera 插件。

3.3.1 基本配置

<gazebo reference="camera_link">
  <sensor type="depth" name="depth_camera">
    <pose>0 0 0 0 0 0</pose>
    <update_rate>30</update_rate>
    <camera>
      <horizontal_fov>1.047</horizontal_fov>  <!-- 60度 -->
      <image>
        <width>640</width>
        <height>480</height>
        <format>R8G8B8</format>
      </image>
      <clip>
        <near>0.1</near>
        <far>10.0</far>
      </clip>
    </camera>
    <plugin name="camera_controller" filename="libgazebo_ros_depth_camera.so">
      <topicName>/camera/depth</topicName>
      <frameName>camera_link</frameName>
    </plugin>
  </sensor>
</gazebo>

这里 horizontal_fov 是水平视场角，我一般设60度，和RealSense D435一致。分辨率640x480够用，太高了仿真会卡。

注意： 深度相机的 near 和 far 决定了有效深度范围。真机RealSense的近端是0.1米，远端10米。仿真里设太大，反而会引入无效数据。

3.3.2 数据读取

深度相机发布两个话题：/camera/rgb/image_raw（彩色图）和 /camera/depth/image_raw（深度图）。读取深度图时，注意单位是毫米：

from sensor_msgs.msg import Image
import cv2
from cv_bridge import CvBridge

bridge = CvBridge()

def depth_callback(msg):
    # 深度图是32位浮点，单位毫米
    depth_img = bridge.imgmsg_to_cv2(msg, "32FC1")
    # 转成米
    depth_m = depth_img / 1000.0
    print(f"深度图尺寸: {depth_m.shape}")
    print(f"最近距离: {depth_m.min():.2f} 米")

rospy.Subscriber('/camera/depth/image_raw', Image, depth_callback)

我个人习惯把深度图转成点云，用 depth_image_proc 包里的 depth_to_pointcloud 节点，直接发布 sensor_msgs/PointCloud2，方便Rviz显示。

3.4 传感器数据同步与融合

在实际项目中，我们经常需要把激光雷达、IMU、深度相机的数据对齐时间戳。Gazebo仿真里，所有传感器的时间戳都是仿真时间，天然同步。但真机上，你需要用 message_filters 做时间同步。

下面是一个简单的同步示例：

import message_filters
from sensor_msgs.msg import LaserScan, Imu

def sync_callback(scan, imu):
    # scan和imu的时间戳已经对齐
    print(f"激光时间: {scan.header.stamp}")
    print(f"IMU时间: {imu.header.stamp}")

scan_sub = message_filters.Subscriber('/scan', LaserScan)
imu_sub = message_filters.Subscriber('/imu/data', Imu)
ts = message_filters.ApproximateTimeSynchronizer([scan_sub, imu_sub], 10, 0.1)
ts.registerCallback(sync_callback)

为什么要同步？因为SLAM算法（比如Cartographer）要求激光和IMU的时间差在10毫秒以内。我在真机上吃过这个亏，时间戳没对齐，地图直接扭曲了。

3.5 本章知识体系

下面这张图，帮你理清三种传感器的配置、数据流和关键参数：

3.6 避坑指南

最后，分享几个我踩过的坑：

激光雷达的帧率：仿真里设10Hz，真机可能是20Hz。迁移时记得改参数。
IMU的坐标系：Gazebo默认IMU的Z轴向上，但真机IMU可能不同。一定要检查 frame_id。
深度相机的噪声：仿真深度图很干净，真机有大量空洞。建议在仿真里加一些随机缺失点，模拟真实情况。

嗯，这一章的内容就到这里。传感器仿真配置好了，你的机器人就有了“感知能力”。下一章，我们会把这些传感器数据用起来，开始做SLAM仿真。

核心要点回顾：

激光雷达用 gazebo_ros_laser，注意加噪声
IMU用 gazebo_ros_imu，更新率设100Hz
深度相机用 gazebo_ros_depth_camera，注意近远裁剪面
时间同步用 message_filters，真机迁移必备

可可精品资料库