综合实验：基于Gazebo的虚拟模块组装

学习目标

  • 掌握Gazebo仿真环境中传感器模型的SDF配置方法

  • 理解真实硬件参数到仿真参数的映射关系

  • 学会使用ROS工具订阅、可视化和分析传感器数据

  • 完成多传感器集成与坐标系标定

本章通过四个递进式实验，引导读者在Gazebo仿真环境中完成传感器的建模、配置和集成。每个实验以真实硬件规格为原型，帮助读者建立仿真与实物之间的对应关系。

实验一：单目相机仿真

实验目的与任务

实验目的

  • 理解单目相机的成像原理和核心参数（分辨率、帧率、视场角FOV）

  • 掌握Gazebo仿真环境中相机传感器的SDF配置方法

  • 学会将真实硬件规格书参数映射到仿真模型参数

  • 掌握在ROS中订阅图像话题并进行可视化的方法

实验任务：

  1. 阅读USB单目相机（DECXIN-OG2M-1888V1）规格书，提取关键参数

  2. 根据硬件参数创建Gazebo相机SDF模型文件

  3. 在Gazebo中加载相机模型并验证ROS话题输出

  4. 使用rqt_image_view和rviz可视化相机图像

  5. 完成参数调整实验，观察不同参数对图像的影响

实验预习

预备知识：

  1. 针孔相机模型：了解相机成像的基本原理，理解焦距、像素尺寸、视场角之间的关系

  2. ROS基础：熟悉ROS话题（Topic）的发布与订阅机制，了解sensor_msgs/Image消息类型

  3. SDF/URDF格式：了解Gazebo模型描述文件的基本结构

  4. Linux基础：熟悉基本的终端命令操作

课前思考题：

  1. 什么是相机的视场角（FOV）？对角线FOV与水平FOV如何换算？

  2. 相机的分辨率和帧率之间通常存在什么样的权衡关系？

  3. Gazebo中的相机插件如何将图像数据发布到ROS话题？

安全提示

本实验为纯软件仿真实验，无人身安全隐患。但请注意：高分辨率、高帧率的相机仿真会消耗大量GPU资源，可能导致系统卡顿。建议在仿真中使用降采样的分辨率（如640×480）以保证流畅运行。

实验原理

针孔相机成像模型

单目相机的成像过程可以用针孔相机模型（Pinhole Camera Model）描述。三维空间中的点 \(P(X, Y, Z)\) 通过光心 \(O\) 投影到像素平面上的点 \(p(u, v)\)，其投影关系为：

(34)\[\begin{split}\begin{bmatrix} u \\ v \\ 1 \end{bmatrix} = \frac{1}{Z} \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} X \\ Y \\ Z \end{bmatrix}.\end{split}\]

其中：

  • \((f_x, f_y)\) 是以像素为单位的焦距

  • \((c_x, c_y)\) 是光心在像素坐标系中的位置

  • \(Z\) 是点到相机的深度

视场角（FOV）计算

视场角（FOV, Field of View）决定了相机能够”看到”的范围。已知对角线FOV和图像宽高比，可计算水平FOV：

(35)\[\text{FOV}_{horizontal} = 2 \times \arctan\left(\frac{w}{\sqrt{w^2 + h^2}} \times \tan\left(\frac{\text{FOV}_{diagonal}}{2}\right)\right).\]

对于本实验使用的USB相机（1600×1300分辨率，92°对角线FOV）：

  • 宽高比：\(w:h = 1600:1300 \approx 1.23:1\)

  • 计算得水平FOV ≈ 77° ≈ 1.34 rad

硬件参数到仿真参数的映射

硬件参数

SDF参数

映射方法

分辨率1600×1300

<width> <height>

直接映射或降采样

帧率60fps

<update_rate>

直接映射

对角线FOV 92°

<horizontal_fov>

转换为水平FOV（弧度）

像素尺寸3.0μm

影响噪声模型

间接映射

Gazebo相机插件工作原理

Gazebo通过libgazebo_ros_camera.so插件将仿真相机的图像数据发布到ROS话题。插件的主要功能包括：

  1. 渲染虚拟场景生成图像

  2. 添加噪声模型模拟真实相机特性

  3. 将图像封装为ROS消息并发布

  4. 发布相机内参信息（camera_info话题）

实验内容

硬件原型参数表

本实验以深圳德创信USB摄像模组（型号：DECXIN-OG2M-1888V1）为原型：

参数

规格值

说明

感光芯片

1/2.9”

芯片尺寸

最高分辨率

1600×1300

约200万像素

支持帧率

60fps @ MJPEG

压缩格式下的最高帧率

视场角(FOV)

92°（对角线）

广角镜头

焦距

2.87mm

定焦镜头

光圈

F2.2

大光圈设计

像素尺寸

3.0μm × 3.0μm

单个像素物理尺寸

接口

USB2.0

即插即用

尺寸

38×38×18mm

紧凑设计

工作电流

80-120mA

低功耗

创建模型目录结构

# 步骤1：创建模型目录
mkdir -p ~/catkin_ws/src/my_sensors/models/mono_camera
cd ~/catkin_ws/src/my_sensors/models/mono_camera

创建model.config文件

<?xml version="1.0"?>
<model>
  <name>mono_camera</name>
  <version>1.0</version>
  <sdf version="1.6">model.sdf</sdf>
  <author>
    <name>Student</name>
    <email>student@example.com</email>
  </author>
  <description>
    基于DECXIN-OG2M-1888V1参数的单目相机模型
  </description>
</model>

创建model.sdf文件

<?xml version="1.0"?>
<sdf version="1.6">
  <model name="mono_camera">
    <!-- 相机外壳 -->
    <link name="camera_link">
      <pose>0 0 0 0 0 0</pose>

      <!-- 惯性参数 -->
      <inertial>
        <mass>0.050</mass>
        <inertia>
          <ixx>0.00001</ixx>
          <iyy>0.00001</iyy>
          <izz>0.00001</izz>
        </inertia>
      </inertial>

      <!-- 视觉外观 -->
      <visual name="camera_visual">
        <geometry>
          <box>
            <size>0.038 0.038 0.018</size>
          </box>
        </geometry>
        <material>
          <ambient>0.1 0.1 0.1 1</ambient>
          <diffuse>0.2 0.2 0.2 1</diffuse>
        </material>
      </visual>

      <!-- 碰撞体积 -->
      <collision name="camera_collision">
        <geometry>
          <box>
            <size>0.038 0.038 0.018</size>
          </box>
        </geometry>
      </collision>

      <!-- 相机传感器 -->
      <sensor name="mono_camera_sensor" type="camera">
        <pose>0.019 0 0 0 0 0</pose>
        <always_on>true</always_on>
        <update_rate>30</update_rate>

        <camera name="mono_cam">
          <horizontal_fov>1.34</horizontal_fov>
          <image>
            <width>640</width>
            <height>480</height>
            <format>R8G8B8</format>
          </image>
          <clip>
            <near>0.1</near>
            <far>100</far>
          </clip>
          <noise>
            <type>gaussian</type>
            <mean>0.0</mean>
            <stddev>0.007</stddev>
          </noise>
        </camera>

        <!-- ROS插件 -->
        <plugin name="camera_controller" filename="libgazebo_ros_camera.so">
          <alwaysOn>true</alwaysOn>
          <updateRate>30.0</updateRate>
          <cameraName>mono_camera</cameraName>
          <imageTopicName>image_raw</imageTopicName>
          <cameraInfoTopicName>camera_info</cameraInfoTopicName>
          <frameName>camera_link</frameName>
        </plugin>
      </sensor>
    </link>
  </model>
</sdf>

运行仿真

步骤1：启动Gazebo

roslaunch gazebo_ros empty_world.launch

步骤2：设置模型路径并加载模型

export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:~/catkin_ws/src/my_sensors/models
rosrun gazebo_ros spawn_model -sdf -file ~/catkin_ws/src/my_sensors/models/mono_camera/model.sdf -model mono_camera -x 0 -y 0 -z 1

步骤3：验证ROS话题

rostopic list | grep mono_camera
# 预期输出：
# /mono_camera/camera_info
# /mono_camera/image_raw

步骤4：可视化图像

rqt_image_view /mono_camera/image_raw

参数调整实验

修改SDF文件中的参数，重新加载模型，观察效果变化：

实验项

修改参数

原值

新值

观察内容

实验A

<horizontal_fov>

1.34

0.8

视野范围变窄

实验B

<update_rate>

30

10

图像更新变慢

实验C

<stddev>

0.007

0.05

图像噪点增加

实验报告要求

  1. 模型配置记录：记录创建的SDF文件内容，标注各参数的含义

  2. 话题验证截图：提供rostopic list和rostopic hz命令的输出截图

  3. 图像可视化截图：提供rqt_image_view显示图像的截图

  4. 参数调整对比：完成参数调整实验，记录观察结果并分析原因

  5. 相机内参分析：使用rostopic echo /mono_camera/camera_info -n 1获取相机内参，解释K矩阵各元素的含义

思考题

  1. 为什么仿真中将分辨率从1600×1300降采样到640×480？这样做有什么优缺点？

  2. 相机的视场角（FOV）对无人机的避障应用有什么影响？FOV越大越好吗？

  3. 查阅资料，说明Gazebo相机噪声模型中的高斯噪声标准差应该如何根据真实相机的信噪比（SNR）来设置？

  4. 如果需要在仿真中模拟相机的径向畸变，应该如何修改SDF配置？

实验二：双目深度相机仿真

实验目的与任务

实验目的

  • 理解双目立体视觉的深度测量原理

  • 掌握Intel RealSense D435i深度相机的核心参数和工作特性

  • 学会配置Gazebo深度相机插件，获取RGB图像、深度图像和点云数据

  • 理解深度相机的测量范围与精度限制

实验任务：

  1. 学习D435i硬件规格，理解主动立体视觉深度测量原理

  2. 创建D435i的Gazebo仿真模型（含深度相机、RGB相机、IMU）

  3. 在Gazebo中加载模型并验证多个ROS话题的输出

  4. 使用rviz可视化深度图像和点云

  5. 编写Python脚本进行深度测量和点云分析

实验预习

预备知识：

  1. 立体视觉原理：理解视差（disparity）与深度的关系

  2. 点云数据结构：了解sensor_msgs/PointCloud2消息格式

  3. ROS图像处理：熟悉cv_bridge库的使用方法

  4. 坐标系变换：理解相机坐标系与世界坐标系的关系

课前思考题：

  1. 双目相机的基线（baseline）对深度测量精度有什么影响？

  2. D435i为什么要使用红外结构光投射器？纯双目相机有什么局限性？

  3. 深度相机在什么情况下会产生无效深度值？

安全提示

深度相机仿真涉及点云渲染，对GPU负载较高，建议关闭不必要的后台程序。真实D435i使用红外光，避免将相机对准眼睛长时间观看。

实验原理

双目立体视觉深度测量原理

D435i使用主动立体视觉技术测量深度。其基本原理是三角测量：

(36)\[Z = \frac{f \times B}{d}.\]

其中：

  • \(Z\) 是深度（目标到相机的距离）

  • \(f\) 是焦距（像素单位）

  • \(B\) 是基线距离（D435i为50mm）

  • \(d\) 是视差（左右图像中对应点的像素差）

深度精度分析

深度测量误差与距离的平方成正比：

(37)\[\Delta Z = \frac{Z^2}{f \times B} \Delta d.\]

这解释了为什么深度相机在远距离时精度下降。对于D435i（基线50mm），在不同距离的理论精度：

距离

理论精度

1m

±2mm

2m

±8mm

5m

±50mm

10m

±200mm

D435i传感器组成

D435i集成了多个传感器模块：

D435i内部结构
├── 左红外相机 ─┐
├── 右红外相机 ─┼─→ 深度计算引擎 → 深度图/点云
├── 红外投射器 ─┘
├── RGB彩色相机 → 彩色图像
└── IMU (BMI055) → 加速度/角速度

实验内容

D435i硬件规格

参数

规格值

说明

深度技术

主动立体视觉

红外结构光辅助

深度分辨率

最高1280×720

可配置

深度帧率

最高90fps

取决于分辨率

RGB分辨率

1920×1080

全高清

深度FOV

87°×58°

水平×垂直

测距范围

0.1m-10m

推荐0.3m-3m

基线

50mm

左右红外相机间距

IMU

BMI055

6轴惯性测量单元

接口

USB 3.0

高速数据传输

尺寸

90×25×25mm

紧凑设计

重量

72g

轻量化

创建D435i模型

创建model.sdf文件（深度相机部分）：

<?xml version="1.0"?>
<sdf version="1.6">
  <model name="d435i">
    <link name="d435i_link">
      <pose>0 0 0 0 0 0</pose>

      <inertial>
        <mass>0.072</mass>
        <inertia>
          <ixx>0.0001</ixx>
          <iyy>0.0001</iyy>
          <izz>0.0001</izz>
        </inertia>
      </inertial>

      <visual name="d435i_visual">
        <geometry>
          <box>
            <size>0.090 0.025 0.025</size>
          </box>
        </geometry>
        <material>
          <ambient>0.5 0.5 0.5 1</ambient>
        </material>
      </visual>

      <!-- 深度相机传感器 -->
      <sensor name="depth_camera" type="depth">
        <pose>0 0 0 0 0 0</pose>
        <always_on>true</always_on>
        <update_rate>30</update_rate>

        <camera name="depth_cam">
          <horizontal_fov>1.52</horizontal_fov>
          <image>
            <width>640</width>
            <height>480</height>
            <format>R8G8B8</format>
          </image>
          <clip>
            <near>0.1</near>
            <far>10</far>
          </clip>
        </camera>

        <plugin name="depth_camera_controller" filename="libgazebo_ros_openni_kinect.so">
          <alwaysOn>true</alwaysOn>
          <updateRate>30.0</updateRate>
          <cameraName>d435i</cameraName>
          <imageTopicName>color/image_raw</imageTopicName>
          <depthImageTopicName>depth/image_raw</depthImageTopicName>
          <pointCloudTopicName>depth/points</pointCloudTopicName>
          <cameraInfoTopicName>color/camera_info</cameraInfoTopicName>
          <depthImageCameraInfoTopicName>depth/camera_info</depthImageCameraInfoTopicName>
          <frameName>d435i_link</frameName>
          <pointCloudCutoff>0.1</pointCloudCutoff>
          <pointCloudCutoffMax>10.0</pointCloudCutoffMax>
        </plugin>
      </sensor>
    </link>
  </model>
</sdf>

深度数据分析脚本

创建depth_analysis.py：

#!/usr/bin/env python
# -*- coding: utf-8 -*-
# 文件名: depth_analysis.py

import rospy
import numpy as np
from sensor_msgs.msg import Image
from cv_bridge import CvBridge

class DepthAnalysis:
    def __init__(self):
        rospy.init_node('depth_analysis')
        self.bridge = CvBridge()
        self.sub = rospy.Subscriber('/d435i/depth/image_raw',
                                    Image, self.depth_callback)
        rospy.loginfo("深度分析节点已启动")

    def depth_callback(self, msg):
        # 将ROS图像消息转换为numpy数组
        depth_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding='32FC1')

        # 统计有效深度值
        valid_mask = np.isfinite(depth_image) & (depth_image > 0)
        valid_depths = depth_image[valid_mask]

        if len(valid_depths) > 0:
            rospy.loginfo("深度统计: 最小=%.2fm, 最大=%.2fm, 平均=%.2fm" %
                         (valid_depths.min(), valid_depths.max(), valid_depths.mean()))

if __name__ == '__main__':
    da = DepthAnalysis()
    rospy.spin()

实验报告要求

  1. 深度图可视化：使用rviz显示深度图像，调整颜色映射

  2. 点云可视化：在rviz中显示点云，尝试不同的点大小和颜色模式

  3. 深度精度测试：放置已知距离的物体，对比深度测量值与实际值

  4. 有效范围验证：测试不同距离下深度值的有效性

思考题

  1. D435i的基线为50mm，如果增大基线到100mm，对深度测量有什么影响？

  2. 深度相机在遇到透明物体（玻璃）、镜面反射、黑色物体时会出现什么问题？

  3. 比较D435i与TOF深度相机（如Azure Kinect）的优缺点。

实验三：激光雷达仿真

实验目的与任务

实验目的

  • 理解激光雷达的测距原理和扫描方式

  • 掌握Livox Mid-360的核心特性（非重复扫描、FOV覆盖）

  • 学会配置Gazebo激光雷达插件并获取点云数据

  • 理解点云数据的结构和处理方法

实验任务：

  1. 学习Mid-360硬件规格，理解其非重复扫描特性

  2. 创建Mid-360的Gazebo仿真模型

  3. 在Gazebo中验证点云输出和FOV覆盖

  4. 编写点云分析脚本

实验原理

激光雷达测距原理

激光雷达通过测量激光脉冲的飞行时间（TOF, Time of Flight）来计算距离：

(38)\[d = \frac{c \cdot \Delta t}{2}.\]

其中 \(c\) 是光速（约3×10⁸ m/s），\(\Delta t\) 是激光往返时间。

Mid-360非重复扫描特性

与传统机械式激光雷达不同，Mid-360采用非重复扫描（Non-repetitive Scanning）技术：

  • 扫描路径在短时间内不重复

  • 随时间累积，FOV内覆盖率逐渐提高

  • 0.1秒覆盖率约80%，0.5秒接近100%

Mid-360技术规格

参数

规格值

说明

扫描方式

非重复扫描

Livox专利技术

水平FOV

360°

全向覆盖

垂直FOV

-7°~+52°

非对称设计

测距范围

0.1m-40m

@10%反射率

测距精度

<2cm

@10m

点云速率

200,000点/秒

高密度

激光波长

905nm

人眼安全

重量

265g

适合无人机

功耗

6.5W

典型值

实验内容

创建Mid-360模型

<?xml version="1.0"?>
<sdf version="1.6">
  <model name="mid360">
    <link name="mid360_link">
      <pose>0 0 0 0 0 0</pose>

      <inertial>
        <mass>0.265</mass>
        <inertia>
          <ixx>0.0005</ixx>
          <iyy>0.0005</iyy>
          <izz>0.0005</izz>
        </inertia>
      </inertial>

      <visual name="mid360_visual">
        <geometry>
          <cylinder>
            <radius>0.0325</radius>
            <length>0.060</length>
          </cylinder>
        </geometry>
        <material>
          <ambient>0.3 0.3 0.3 1</ambient>
        </material>
      </visual>

      <!-- 激光雷达传感器 -->
      <sensor name="lidar" type="gpu_ray">
        <pose>0 0 0.03 0 0 0</pose>
        <always_on>true</always_on>
        <update_rate>10</update_rate>
        <visualize>true</visualize>

        <ray>
          <scan>
            <horizontal>
              <samples>360</samples>
              <resolution>1</resolution>
              <min_angle>-3.14159</min_angle>
              <max_angle>3.14159</max_angle>
            </horizontal>
            <vertical>
              <samples>60</samples>
              <resolution>1</resolution>
              <min_angle>-0.122</min_angle>
              <max_angle>0.908</max_angle>
            </vertical>
          </scan>
          <range>
            <min>0.1</min>
            <max>40</max>
            <resolution>0.01</resolution>
          </range>
          <noise>
            <type>gaussian</type>
            <mean>0</mean>
            <stddev>0.02</stddev>
          </noise>
        </ray>

        <plugin name="lidar_controller" filename="libgazebo_ros_velodyne_gpu_laser.so">
          <topicName>/mid360/points</topicName>
          <frameName>mid360_link</frameName>
          <organize_cloud>false</organize_cloud>
          <min_range>0.1</min_range>
          <max_range>40.0</max_range>
        </plugin>
      </sensor>
    </link>
  </model>
</sdf>

点云分析脚本

创建lidar_analysis.py：

#!/usr/bin/env python
# -*- coding: utf-8 -*-
# 文件名: lidar_analysis.py

import rospy
import numpy as np
from sensor_msgs.msg import PointCloud2
import sensor_msgs.point_cloud2 as pc2

class LidarAnalysis:
    def __init__(self):
        rospy.init_node('lidar_analysis')
        self.sub = rospy.Subscriber('/mid360/points',
                                    PointCloud2, self.cloud_callback)
        rospy.loginfo("激光雷达分析节点已启动")

    def cloud_callback(self, msg):
        # 将PointCloud2转换为numpy数组
        points = np.array(list(pc2.read_points(msg,
                         field_names=("x", "y", "z"), skip_nans=True)))

        if len(points) > 0:
            rospy.loginfo("点云数量: %d" % len(points))
            rospy.loginfo("X范围: [%.1f, %.1f]" % (points[:,0].min(), points[:,0].max()))
            rospy.loginfo("Y范围: [%.1f, %.1f]" % (points[:,1].min(), points[:,1].max()))
            rospy.loginfo("Z范围: [%.1f, %.1f]" % (points[:,2].min(), points[:,2].max()))

if __name__ == '__main__':
    la = LidarAnalysis()
    rospy.spin()

思考题

  1. Gazebo中使用的激光雷达插件是基于传统多线扫描模型的，无法真正模拟Mid-360的非重复扫描特性。你认为应该如何改进仿真模型来更准确地模拟这一特性？

  2. Mid-360的垂直FOV为-7°~+52°，如果将其安装在无人机顶部，这个FOV分布对于不同应用场景（如室内导航、室外建图）有什么影响？

  3. 激光雷达在遇到玻璃、镜面、黑色物体时会出现什么问题？为什么？

  4. Mid-360的点云速率为200,000点/秒，帧率为10Hz，这意味着每帧平均有多少个点？如果用于实时SLAM算法，这个数据量是否需要进行下采样？

实验四：多传感器集成

实验目的与任务

实验目的

  • 掌握将多个传感器集成到无人机模型的方法

  • 理解传感器坐标系与机体坐标系的变换关系

  • 学会使用TF（Transform）管理多传感器坐标系

  • 实现多传感器数据的时间同步

实验任务：

  1. 设计传感器在无人机上的布局方案

  2. 创建集成多传感器的URDF/Xacro模型

  3. 验证TF坐标系关系的正确性

  4. 实现多传感器数据的同步接收

  5. 构建完整的智能无人机感知系统

实验原理

传感器坐标系定义

每个传感器都有自己的坐标系，需要通过TF变换统一到机体坐标系：

world (世界坐标系)
  └── base_link (机体坐标系)
        ├── mono_camera_link (单目相机)
        ├── d435i_link (深度相机)
        └── mid360_link (激光雷达)

坐标变换表示

从传感器坐标系到机体坐标系的变换包括：

  • 平移 (x, y, z)：传感器相对机体中心的位置

  • 旋转 (roll, pitch, yaw)：传感器的安装角度

变换矩阵表示为：

(39)\[\begin{split}T_{body}^{sensor} = \begin{bmatrix} R & t \\ 0 & 1 \end{bmatrix}.\end{split}\]

时间同步机制

ROS中使用message_filters实现多话题的时间同步：

  • ExactTime：要求时间戳完全相同

  • ApproximateTime：允许一定的时间差（slop）

传感器布局设计原则

原则

说明

FOV不遮挡

传感器之间不能互相遮挡视野

重心平衡

传感器布局应尽量保持无人机重心稳定

减少振动影响

IMU等敏感传感器应远离电机

便于维护

传感器应便于安装和拆卸

实验内容

传感器布局方案

P450无人机上的传感器布局设计：

传感器

位置

安装角度

用途

单目相机

机头前方15cm

向下倾斜15°

前视避障

D435i

机身下方8cm

向下倾斜30°

深度感知

Mid-360

机身顶部8cm

水平安装

360°环境感知

创建集成模型

创建p450_sensors.urdf.xacro文件：

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="p450_sensors">

  <!-- 单目相机 -->
  <link name="mono_camera_link">
    <visual>
      <geometry><box size="0.038 0.038 0.018"/></geometry>
      <material name="dark_grey"><color rgba="0.2 0.2 0.2 1"/></material>
    </visual>
    <collision>
      <geometry><box size="0.038 0.038 0.018"/></geometry>
    </collision>
    <inertial>
      <mass value="0.05"/>
      <inertia ixx="0.00001" ixy="0" ixz="0" iyy="0.00001" iyz="0" izz="0.00001"/>
    </inertial>
  </link>

  <joint name="mono_camera_joint" type="fixed">
    <parent link="base_link"/>
    <child link="mono_camera_link"/>
    <origin xyz="0.15 0 -0.05" rpy="0 0.26 0"/>
  </joint>

  <!-- D435i -->
  <link name="d435i_link">
    <visual>
      <geometry><box size="0.090 0.025 0.025"/></geometry>
      <material name="grey"><color rgba="0.7 0.7 0.7 1"/></material>
    </visual>
    <collision>
      <geometry><box size="0.090 0.025 0.025"/></geometry>
    </collision>
    <inertial>
      <mass value="0.072"/>
      <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
    </inertial>
  </link>

  <joint name="d435i_joint" type="fixed">
    <parent link="base_link"/>
    <child link="d435i_link"/>
    <origin xyz="0.05 0 -0.08" rpy="0 0.52 0"/>
  </joint>

  <!-- Mid-360 -->
  <link name="mid360_link">
    <visual>
      <geometry><cylinder radius="0.0325" length="0.06"/></geometry>
      <material name="dark_grey"><color rgba="0.3 0.3 0.3 1"/></material>
    </visual>
    <collision>
      <geometry><box size="0.065 0.065 0.060"/></geometry>
    </collision>
    <inertial>
      <mass value="0.265"/>
      <inertia ixx="0.0005" ixy="0" ixz="0" iyy="0.0005" iyz="0" izz="0.0005"/>
    </inertial>
  </link>

  <joint name="mid360_joint" type="fixed">
    <parent link="base_link"/>
    <child link="mid360_link"/>
    <origin xyz="0 0 0.08" rpy="0 0 0"/>
  </joint>

</robot>

多传感器时间同步

创建sensor_sync.py：

#!/usr/bin/env python
# -*- coding: utf-8 -*-
# 文件名: sensor_sync.py

import rospy
import message_filters
from sensor_msgs.msg import Image, PointCloud2

class SensorSync:
    def __init__(self):
        rospy.init_node('sensor_sync')

        mono_sub = message_filters.Subscriber('/mono_camera/image_raw', Image)
        depth_sub = message_filters.Subscriber('/d435i/depth/image_raw', Image)
        lidar_sub = message_filters.Subscriber('/mid360/points', PointCloud2)

        ts = message_filters.ApproximateTimeSynchronizer(
            [mono_sub, depth_sub, lidar_sub],
            queue_size=10,
            slop=0.1
        )
        ts.registerCallback(self.callback)
        rospy.loginfo("传感器同步节点已启动")

    def callback(self, mono_img, depth_img, lidar_points):
        mono_time = mono_img.header.stamp.to_sec()
        depth_time = depth_img.header.stamp.to_sec()
        lidar_time = lidar_points.header.stamp.to_sec()

        time_diff = max(mono_time, depth_time, lidar_time) - min(mono_time, depth_time, lidar_time)
        rospy.loginfo("同步成功! 最大时间差: %.3fs" % time_diff)

if __name__ == '__main__':
    ss = SensorSync()
    rospy.spin()

TF坐标系验证

# 查看TF树
rosrun rqt_tf_tree rqt_tf_tree

# 检查特定变换
rosrun tf tf_echo base_link mono_camera_link
rosrun tf tf_echo base_link d435i_link
rosrun tf tf_echo base_link mid360_link

实验报告要求

  1. 传感器布局图：绘制无人机俯视图和侧视图，标注各传感器的安装位置和角度

  2. TF树截图：提供rqt_tf_tree显示的完整TF树截图

  3. 坐标变换验证：使用tf_echo验证各传感器到base_link的变换，记录输出结果

  4. 时间同步测试：运行sensor_sync.py，记录同步成功率和时间差统计

思考题

  1. 如果单目相机和D435i的视野有重叠区域，这是好事还是坏事？在什么应用场景下需要这种设计？

  2. Mid-360安装在机身顶部，会被无人机的桨叶遮挡一部分FOV。如何评估这种遮挡的影响？有什么解决方案？

  3. 时间同步中使用的slop=0.1意味着允许100ms的时间差。对于高速飞行的无人机（如10m/s），100ms内会移动多少距离？这个误差是否可以接受？

  4. 如果要将仿真中的传感器配置迁移到真实无人机上，需要额外考虑哪些因素？（提示：标定、EMI、供电等）

附录

附录A：硬件规格对比表

参数

USB单目相机

Intel D435i

Livox Mid-360

类型

2D视觉

深度视觉

3D激光

分辨率

1600×1300

1280×720 (深度)

200k点/秒

帧率

60fps

30fps

10Hz

FOV

92°(对角)

87°×58°

360°×59°

测量范围

N/A

0.1-10m

0.1-40m

精度

N/A

±2% @ 2m

<2cm @ 10m

接口

USB2.0

USB3.0

以太网

功耗

0.5W

2.5W

6.5W

重量

50g

72g

265g

价格参考

￥50-100

￥2000-3000

￥4000-5000

附录B：常见问题与解决方案

问题

可能原因

解决方案

Gazebo中看不到传感器可视化

visualize未开启

在SDF中设置<visualize>true</visualize>

点云话题无数据

插件未正确加载

检查plugin路径和名称

图像话题延迟高

分辨率/帧率过高

降低分辨率或帧率

TF坐标系错误

joint配置错误

使用tf_echo检查变换

仿真卡顿

GPU负载过高

减少同时运行的传感器数量

延伸阅读