给机器人仿真机器人添加传感器仿真

apingna

已于 2025-06-08 11:43:22 修改

阅读量1.3k

点赞数 29

CC 4.0 BY-SA版权

文章标签：机器人 gazebo rviz2 ROS2

于 2025-05-29 17:29:06 首次发布

本文链接：https://blog.csdn.net/apingna/article/details/148305981

前言

ROS2的使用——机器人建模_ros2的机器人包怎么打开-CSDN博客

 将机器人模型加入gazebo中进行仿真-CSDN博客

之前写的这两篇文章已经介绍了如何在ROS2中对机器人进行建模、添加到可视化工具rviz2中、在gazebo中创建并保存世界模型、将机器人加载到世界模型中以及添加插件通过键盘控制机器人移动。这篇文章将要介绍如何添加传感器插件从而在gazebo中对传感器进行仿真。

一、激光雷达仿真

添加传感器仿真的方法和上一篇文章添加两轮差速控制器插件的方法相同。在urdf/robot1/plugins文件下创建一个名叫gazebo_sensor_plugins.xacro的文件，雷达、imu以及深度相机等传感器的传感器配置都会在这个文件内进行。

在文件中粘贴以下代码

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
    <xacro:macro name="gazebo_sensor_plugins"> 
        <!-- 雷达仿真 -->
        <gazebo reference="laser_link">
            <sensor name="laserscan" type="ray">
                <plugin name="laserscan" filename="libgazebo_ros_ray_sensor.so">
                    <ros>
                        <namespace>/</namespace>
                        <remapping>~/out:=scan</remapping>
                    </ros>
                    <output_type>sensor_msgs/LaserScan</output_type>
                    <frame_name>laser_link</frame_name>
                </plugin>
                <always_on>true</always_on>
                <visualize>true</visualize>
                <update_rate>10</update_rate>
                <pose>0 0 0 0 0 0</pose>

                
                <ray>
                    <!-- 设置扫描范围 -->
                    <scan>
                        <horizontal>
                            <samples>360</samples>
                            <resolution>1.000000</resolution>
                            <min_angle>0.000000</min_angle>
                            <max_angle>6.280000</max_angle>
                        </horizontal>
                    </scan>

                    <!-- 设置扫描距离 -->
                    <range>
                        <min>0.120000</min>
                        <max>8.0</max>
                        <resolution>0.015000</resolution>
                    </range>
                    <!-- 设置噪声 -->
                    <noise>
                        <type>gaussian</type>
                        <mean>0.0</mean>
                        <stddev>0.01</stddev>
                    </noise>
                </ray>
            </sensor>
        </gazebo>
    </xacro:macro>
</robot>

逐一解释代码的内容

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

声明xm文档l的版本为1.0，<robot>是机器人定义的根标签，定义了一个xacro的命名空间，表示该文件将使用宏语言简化机器人的定义，宏的名字叫gazebo_sensor_plugins

        <gazebo reference="laser_link">
            <sensor name="laserscan" type="ray">
                <plugin name="laserscan" filename="libgazebo_ros_ray_sensor.so">

告诉gazebo标签以内的是给gazebo看的内容，告诉gazebo参考laser_link；传感器的名字laserscan,ray指定传感器类型为射线类检测。插件名为laserscan，动态连接库的文件名libgazebo_ros_ray_sensor.so

                    <ros>
                        <namespace>/</namespace>
                        <remapping>~/out:=scan</remapping>
                    </ros>

给ros看的内容，命名空间使用默认命名空间，将输出的话题重映射为scan。

                    <output_type>sensor_msgs/LaserScan</output_type>
                    <frame_name>laser_link</frame_name>

定义消息格式采用sensor_msgs下的LaserScan，定义激光雷达的坐标系名称为laser_link，注意这个名称要和laser.urdf.xacro中定义的雷达连杆的名称保持一致。

             <always_on>true</always_on>
                <visualize>true</visualize>
                <update_rate>10</update_rate>
                <pose>0 0 0 0 0 0</pose>

激光雷达保持一直开启，雷达射线可见，刷新频率10hz,位置姿态保持与laser_link一致。

<ray>
                    <!-- 设置扫描范围 -->
                    <scan>
                        <horizontal>
                            <samples>360</samples>
                            <resolution>1.000000</resolution>
                            <min_angle>0.000000</min_angle>
                            <max_angle>6.280000</max_angle>
                        </horizontal>
                    </scan>

射线的参数配置，扫码设置，水平方向，每一圈扫描产生360个点，分辨率1.0，最小角度0度，最大角度6.28（弧度制）即360度。

                  <!-- 设置扫描范围 -->
                    <range>
                        <min>0.120000</min>
                        <max>8.0</max>
                        <resolution>0.015000</resolution>
                    </range>

扫描范围设置，最小0.12米，最大8.0米，分别率0.015米。

            <!-- 设置噪声 -->
                     <noise>
                        <type>gaussian</type>
                        <mean>0.0</mean>
                        <stddev>0.01</stddev>
                     </noise>

设置噪声干扰，使用高斯噪声 ,噪声的均值为0，标准差为0.01

代码粘贴完成之后就可以在robot1.urdf.xacro中进行调用了，在robot1.urdf.xacro中添加以下两条代码

<xacro:include filename="$(find robot_sim)/urdf/robot1/plugins/gazebo_sensor_plugins.xacro"/>

这条代码表示文件插入了gazebo_sensor_plugins.xacro的内容。

<xacro:gazebo_sensor_plugins/>

调用gazebo_sensor_plugins宏

添加完这两行代码并保存文件后，在终端里跳转到工作空间下 colcon build 构建代码。构建完成后可以 source install/setup.base 配置一下环境，之后就可以来运行lanuch文件启动gazebo了。

ros2 launch robot_sim robot1_gazebo_sim.launch.py

gazebo启动后可以看到机器人顶部的雷达发出了蓝色的线。

接下来ctrl alt t打开一个新的终端，在终端输入rviz2启动rivz2。

启动后点击左下角的Add添加选择By topic 选择/scan点击ok，时你的屏幕上就会显示很多小红点，勾勒出墙体大概的形状，这就是模拟的激光雷达采集的数据了。

觉得点太小的可以更改点的尺寸，将默认的0.01改为0.1.

最后的效果呈现的效果大概就是这样

对应gazebo中的位置

二、imu(惯性测量单元)仿真

简单介绍一下imu,核心构成：三轴加速度计和三轴陀螺仪，三轴加速度计测量三个轴方向的线加速度，陀螺仪测量三轴方向的角加速度。这里可以用来进行短时运动轨迹的推算，和里程计相结合，比如说里程计是通过轮子转动的状态来计算的，如果轮子与地面打滑历程计就会不准确，这是时就可以通过imu的数据推算出机器人实际的状态，来补偿里程计了。此外imu还用于平衡车平衡、无人机飞行平稳等。

接下来添加imu惯性测量单元，在gazebo_sensor_plugins.xacro文件下的</gazebo>标签后面粘贴以下代码

   <!-- 惯性测量传感器仿真 -->
        <gazebo reference="imu_link">
            <sensor name="imu_sensor" type="imu">
            <plugin name="imu_plugin" filename="libgazebo_ros_imu_sensor.so">
                <ros>
                <namespace>/</namespace> 
                <remapping>~/out:=imu</remapping>
                 </ros>
                <initial_orientation_as_reference>false</initial_orientation_as_reference>
            </plugin>
            <update_rate>100</update_rate>
            <always_on>true</always_on>
            <!-- 六轴噪声设置 -->
            <imu>
                <angular_velocity>
                    <x>
                        <noise type="gaussian">
                            <mean>0.0</mean>
                            <stddev>2e-4</stddev>
                            <bias_mean>0.0000075</bias_mean>
                            <bias_stddev>0.0000008</bias_stddev>
                        </noise>
                    </x>
                    <y>
                        <noise type="gaussian">
                            <mean>0.0</mean>
                            <stddev>2e-4</stddev>
                            <bias_mean>0.0000075</bias_mean>
                            <bias_stddev>0.0000008</bias_stddev>
                        </noise>
                    </y>
                    <z>
                        <noise type="gaussian">
                            <mean>0.0</mean>
                            <stddev>2e-4</stddev>
                            <bias_mean>0.0000075</bias_mean>
                            <bias_stddev>0.0000008</bias_stddev>
                        </noise>
                    </z>
                </angular_velocity>
                <linear_acceleration>
                    <x>
                        <noise type="gaussian">
                            <mean>0.0</mean>
                            <stddev>1.7e-2</stddev>
                            <bias_mean>0.1</bias_mean>
                            <bias_stddev>0.001</bias_stddev>
                        </noise>
                    </x>
                    <y>
                        <noise type="gaussian">
                            <mean>0.0</mean>
                            <stddev>1.7e-2</stddev>
                            <bias_mean>0.1</bias_mean>
                            <bias_stddev>0.001</bias_stddev>
                        </noise>
                    </y>
                    <z>
                        <noise type="gaussian">
                            <mean>0.0</mean>
                            <stddev>1.7e-2</stddev>
                            <bias_mean>0.1</bias_mean>
                            <bias_stddev>0.001</bias_stddev>
                        </noise>
                    </z>
                </linear_acceleration>
            </imu>
            </sensor>
        </gazebo>

这里就不反复的讲重复的标签了，只讲和上面激光雷达不一样的地方。

<initial_orientation_as_reference>false</initial_orientation_as_reference>是否以传感器的初始位置作为参考基准，这里是false代表以gazebo的全局坐标系作为参考基准。

<angular_velocity>标签内配置了imu角加速度三个轴的噪声干扰，<linear_acceleration>标签内配置了imu线加速度三个轴的噪声干扰，这里使用的都是高斯噪声，感兴趣的同学可以去查一下传感器噪声干扰的相关的资料。

粘贴完代码保存后，重新在工作空间构建代码，构建完成启动launch文件。

ros2 launch robot_sim robot1_gazebo_sim.launch.py

启动后怎么查看是否添加imu成功了，打开一个新的终端，在终端中输入

ros2 topic list | grep imu

这段代码时查看当前有哪些活跃的话题并将这些话题传给grep筛选带imu的话题，如果添加成功应该返回 /imu，再输入一条指令

ros2 topic echo /imu --once

打印一次话题/imu的内容，仿真imu成功会输出类似以图片中的以下结果

收到的消息中：

orientation:
  x: 0.004820074118892088
  y: 0.010553178715121809
  z: -0.41511599404147026
  w: 0.909694513996571

四元数表示机器人的位姿

angular_velocity:
  x: 0.0004985544047274407
  y: 0.0006186699190864275
  z: -0.00022904889716530806

角加速度几乎都为零（噪声干扰），此时机器人静止

linear_acceleration:
  x: -0.10620080234785895
  y: 0.10272649195281167
  z: 9.669990803231647

线加速度水平方向xy都接近于零，z轴垂直方向9.669接近重力加速度9.81左右

三、深度相机仿真

简单介绍一下深度相机：核心输出内容：

- RGB色彩像素

- 深度图，与普通的图像不同，深度图没有丰富的颜色，它用颜色表示距离，物体离得越近颜色越深，离得越远颜色越浅。

- 点云，将深度图转换为三维空间中的点，格式为（x,y,z）坐标

由于深度相机的z轴默认是向前x轴向右y轴向下，而我们设置的相机坐标z轴是向上的，因此需要在camera.urdf.xacro文件下添加虚拟部件来调整xyz坐标的朝向。

        <link name="camera_optical_link"></link>
         <joint name="camera_optical_joint" type="fixed">
            <origin xyz="0.0 0.0 0.0" rpy="-1.57075 0.0 -1.57075"/>
            <parent link="camera_link"/>
            <child link="camera_optical_link"/>
         </joint>

这段代码添加了一个没有主体的连杆，通过关节的旋转改变了相机xyz轴的朝向，使得 xyz轴的朝向符合深度相机的要求。

下面添加添加深度相机仿真的插件和参数配置，在gazebo_sensor_plugins.xacro文件下的imu的</gazebo>标签后面粘贴以下代码

 <!-- 深度相机配置 -->
        <gazebo reference="camera_link">
            <sensor type="depth" name="camera_sensor">
                <plugin name="depth_camera" filename="libgazebo_ros_camera.so">
                    <frame_name>camera_optical_link</frame_name>
                </plugin>
                <always_on>true</always_on>
                <update_rate>10</update_rate>
                <camera name="camera">
                    <horizontal_fov>1.5009831567</horizontal_fov>
                    <image>
                        <width>800</width>
                        <height>600</height>
                        <format>R8G8B8</format>

                    </image>
                    <distortion>
                        <k1>0.0</k1>
                        <k2>0.0</k2>
                        <k3>0.0</k3>
                        <p1>0.0</p1>
                        <p2>0.0</p2>
                        <center>0.5 0.5 </center>
                    </distortion>
                </camera>
            </sensor>
        </gazebo>

粘贴代码后需要保证 <frame_name>camera_optical_link</frame_name>与添加的虚拟连杆名称完全相同。

                    <horizontal_fov>1.5009831567</horizontal_fov>
                    <image>
                        <width>800</width>
                        <height>600</height>
                        <format>R8G8B8</format>

配置水平视角场角为1.50(弧度制)约八十几度，设置图像参数，图像宽800像素高600像素，格式为RGB，每个通道8位（24位真彩色）。

                    <distortion>
                        <k1>0.0</k1>
                        <k2>0.0</k2>
                        <k3>0.0</k3>
                        <p1>0.0</p1>
                        <p2>0.0</p2>
                        <center>0.5 0.5 </center>
                    </distortion>

‌参数‌	‌类型‌	‌说明‌
`<k1>`, `<k2>`, `<k3>`	径向畸变系数	修正镜头径向变形（如桶形/枕形畸变），通常 `k1` 影响最大。全为0表示无径向畸变。
`<p1>`, `<p2>`	切向畸变系数	修正镜头装配误差导致的切向变形。全为0表示无切向畸变。
`<center>`	畸变中心坐标	归一化坐标 `(0.5, 0.5)` 表示图像中心（假设图像宽高为1时的相对位置）。

保存好代码后，这里可以先安装一个可视化工具rqt

sudo apt install ros-humble-rqt-rf-tree -y

删除原有配置，重新扫描

rm -rf  ~/.config/ros.org/rqt_gui.ini

安装完成之后，在工作空间下重新构建代码，构建完成后运行launch文件

ros2 launch robot_sim robot1_gazebo_sim.launch.py

启动gazebo后再另一个终端中输入rviz2打开可视化工具，点击Add在By Tpoic下选择Point Cloud2点击ok,这时你的屏幕上就会出现深度图转换出来的点堆积出来的图像

对应的gazebo下

现在打开一个新的终端输入rqt，点击Visualization选择Image View

可以选择两个话题depth/image_raw是深度图，另一个是色彩图，如果没有可以点击刷新一下试试。

对应上面机器人位置的色彩图

深度图（离得越近颜色越深）

可以放点别物体试试效果

色彩图

深度图

总结

通过本次学习你已经学会了如何给仿真的机器人添加激光雷达、imu以及深度相机等传感器了，并且你还通过可视化工具看到了机器人在gazebo仿真中采集到的雷达数据、加速度数据、以及图像数据。你可以尝试控制机器人的同时查看机器人采集的图像，这样是不是就像你在用第一视角操控机器人呢，这样一想还挺有意思的，毕竟这是你一点一点搭建起来的。学习很枯燥，但是我们可以“学中作乐”，加油！