多线激光雷达+imu的gazebo仿真

多线激光雷达+imu的gazebo仿真

前言:在做2D/3D slam的课题研究时,往往我们手里头没有昂贵的多线激光雷达和惯性传感器,ros提供了相应的传感器插件,通过插件可以在物理仿真环境下,模拟传感器运行时的效果。

1.搭建小车底盘模型

gazebo下xacro描述文件多加了碰撞标签和惯性矩阵,定义常见形状的惯性矩阵描述文件my_inertia.xacro,

<robot name="base" xmlns:xacro="http://wiki.ros.org/xacro">
    <!-- Macro for inertia matrix -->
    <xacro:macro name="sphere_inertial_matrix" params="m r">
        <inertial>
            <mass value="${m}" />
            <inertia ixx="${2*m*r*r/5}" ixy="0" ixz="0"
                iyy="${2*m*r*r/5}" iyz="0" 
                izz="${2*m*r*r/5}" />
        </inertial>
    </xacro:macro>

    <xacro:macro name="cylinder_inertial_matrix" params="m r h">
        <inertial>
            <mass value="${m}" />
            <inertia ixx="${m*(3*r*r+h*h)/12}" ixy = "0" ixz = "0"
                iyy="${m*(3*r*r+h*h)/12}" iyz = "0"
                izz="${m*r*r/2}" /> 
        </inertial>
    </xacro:macro>

    <xacro:macro name="Box_inertial_matrix" params="m l w h">
       <inertial>
               <mass value="${m}" />
               <inertia ixx="${m*(h*h + l*l)/12}" ixy = "0" ixz = "0"
                   iyy="${m*(w*w + l*l)/12}" iyz= "0"
                   izz="${m*(w*w + h*h)/12}" />
       </inertial>
   </xacro:macro>
</robot>

xacro文件格式对urdf文件格式进行了升级,提高了urdf格式代码的复用性。小车底盘的描述文件 my_car_Cylider.xacro如下:

<!--
    使用 xacro 优化 URDF 版的小车底盘实现:

    实现思路:
    1.将一些常量、变量封装为 xacro:property
      比如:PI 值、小车底盘半径、离地间距、车轮半径、宽度 ....
    2.使用 宏 封装驱动轮以及支撑轮实现,调用相关宏生成驱动轮与支撑轮

-->
<!-- 根标签,必须声明 xmlns:xacro -->
<robot name="my_base" xmlns:xacro="http://www.ros.org/wiki/xacro">
    <!-- 封装变量、常量 -->
    <xacro:property name="PI" value="3.141"/>
    <!-- 宏:黑色设置 -->
    <material name="black">
        <color rgba="0.0 0.0 0.0 1.0" />
    </material>
    <!-- 底盘属性 -->
    <xacro:property name="base_footprint_radius" value="0.001" /> <!-- base_footprint 半径  -->
    <xacro:property name="base_link_radius" value="0.1" /> <!-- base_link 半径 -->
    <xacro:property name="base_link_length" value="0.08" /> <!-- base_link 长 -->
    <xacro:property name="earth_space" value="0.02" /> <!-- 离地间距 -->
    <xacro:property name="base_link_m" value="1.0" /> <!-- 质量  -->
    <!-- 底盘 -->
    <link name="base_footprint">
      <visual>
        <geometry>
          <sphere radius="${base_footprint_radius}" />
        </geometry>
      </visual>
    </link>

    <link name="base_link">
      <visual>
        <geometry>
          <cylinder radius="${base_link_radius}" length="${base_link_length}" />
        </geometry> 
        <origin xyz="0 0 0" rpy="0 0 0" />
        <material name="yellow">
          <color rgba="0.5 0.3 0.0 0.5" />
        </material>
      </visual>
            <collision>
        <geometry>
          <cylinder radius="${base_link_radius}" length="${base_link_length}" />
        </geometry>
        <origin xyz="0 0 0" rpy="0 0 0" />
      </collision>
      <xacro:cylinder_inertial_matrix m="${base_link_m}" r="${base_link_radius}" h="${base_link_length}" />
    </link>

    <joint name="base_link2base_footprint" type="fixed">
      <parent link="base_footprint" />
      <child link="base_link" />
      <origin xyz="0 0 ${earth_space + base_link_length / 2 }" />
    </joint>

    <gazebo reference="base_link">
        <material>Gazebo/Yellow</material>
    </gazebo>
    <!-- 驱动轮 -->
    <!-- 驱动轮属性 -->
    <xacro:property name="wheel_radius" value="0.0325" /><!-- 半径 -->
    <xacro:property name="wheel_length" value="0.015" /><!-- 宽度 -->
    <xacro:property name="wheel_m" value="0.05" /> <!-- 质量  -->
    <!-- 驱动轮宏实现 -->
    <xacro:macro name="add_wheels" params="name flag">
      <link name="${name}_wheel">
        <visual>
          <geometry>
            <cylinder radius="${wheel_radius}" length="${wheel_length}" />
          </geometry>
          <origin xyz="0.0 0.0 0.0" rpy="${PI / 2} 0.0 0.0" />
          <material name="black" />
        </visual>
        <collision>
          <geometry>
            <cylinder radius="${wheel_radius}" length="${wheel_length}" />
          </geometry>
          <origin xyz="0.0 0.0 0.0" rpy="${PI / 2} 0.0 0.0" />
        </collision>
        <xacro:cylinder_inertial_matrix m="${wheel_m}" r="${wheel_radius}" h="${wheel_length}" />
      </link>

      <joint name="${name}" type="continuous">
        <parent link="base_link" />
        <child link="${name}_wheel" />
        <origin xyz="0 ${flag * base_link_radius} ${-(earth_space + base_link_length / 2 - wheel_radius) }" />
        <axis xyz="0 1 0" />
      </joint>
      <gazebo reference="${name}_wheel">
        <material>Gazebo/White</material>
      </gazebo>
    </xacro:macro>
    
    <xacro:add_wheels name="base_l_wheel_joint" flag="1" /> 
    <xacro:add_wheels name="base_r_wheel_joint" flag="-1" />
    <!-- 支撑轮 -->
    <!-- 支撑轮属性 -->
    <xacro:property name="support_wheel_radius" value="0.01" /> <!-- 支撑轮半径 -->
    <xacro:property name="support_wheel_m" value="0.03" /> <!-- 质量  -->

    <!-- 支撑轮宏 -->
    <xacro:macro name="add_support_wheel" params="name flag" >
      <link name="${name}_wheel">
        <visual>
            <geometry>
                <sphere radius="${support_wheel_radius}" />
            </geometry>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <material name="black" />
        </visual>
        <collision>
            <geometry>
                <sphere radius="${support_wheel_radius}" />
            </geometry>
            <origin xyz="0 0 0" rpy="0 0 0" />
        </collision>
        <xacro:sphere_inertial_matrix m="${support_wheel_m}" r="${support_wheel_radius}" />
      </link>

      <joint name="${name}_wheel2base_link" type="continuous">
          <parent link="base_link" />
          <child link="${name}_wheel" />
          <origin xyz="${flag * (base_link_radius - support_wheel_radius)} 0 ${-(base_link_length / 2 + earth_space / 2)}" />
          <axis xyz="1 1 1" />
      </joint>
      <gazebo reference="${name}_wheel">
       <material>Gazebo/Black</material>
      </gazebo>
    </xacro:macro>

    <xacro:add_support_wheel name="front" flag="1" />
    <xacro:add_support_wheel name="back" flag="-1" />

</robot>

2.添加多线激光雷达

多线激光雷达的配置文件my_sensor_vodyne.xacro

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="velodyne">
    <!-- Gazebo requires the velodyne_gazebo_plugins package -->
    <gazebo reference="vodyne">
      <sensor type="ray" name="vodyne16">
        <pose>0 0 0 0 0 0</pose>
        <visualize>false</visualize>
        <update_rate>10</update_rate>
        <ray>
          <scan>
            <horizontal>
              <samples>1800</samples> <!-- Change this to 500 if gazebo keeps timing out -->
              <resolution>0.2</resolution>
              <min_angle>-${M_PI}</min_angle>
              <max_angle> ${M_PI}</max_angle>
            </horizontal>
            <vertical>
              <samples>16</samples>
              <resolution>2</resolution>
              <min_angle>-${15*M_PI/180.0}</min_angle>
              <max_angle> ${15*M_PI/180.0}</max_angle>
            </vertical>
          </scan>
          <range>
            <min>0.055</min>
            <max>100.0</max>
            <resolution>0.1</resolution>
          </range>
          <noise>
            <type>gaussian</type>
            <mean>0.0</mean>
            <stddev>0.0</stddev>
          </noise>
        </ray>
        <plugin name="gazebo_ros_laser_controller" filename="libgazebo_ros_velodyne_laser.so">
          <topicName>/velodyne_points</topicName>
          <frameName>vodyne</frameName>
          <min_range>0.9</min_range>
          <max_range>130.0</max_range>
          <gaussianNoise>0.0</gaussianNoise>
        </plugin>
      </sensor>
    </gazebo>
</robot>

在gazebo环境下添加多线激光雷达物理模型my_vodyne_gazebo.xacro

<!--
    小车底盘添加雷达
-->
<robot name="my_vodyne" xmlns:xacro="http://wiki.ros.org/xacro">
    <!-- 雷达属性 -->
    <xacro:property name="M_PI" value="3.1415926"/>
    <xacro:property name="laser_length" value="0.03" /> <!-- 雷达长度 -->
    <xacro:property name="laser_radius" value="0.03" /> <!-- 雷达半径 -->
    <xacro:property name="laser_x" value="0.0" /> <!-- 雷达安装的x坐标 -->
    <xacro:property name="laser_y" value="0.0" /> <!-- 雷达安装的y坐标 -->
    <xacro:property name="laser_z" value="0.055" /> <!-- 雷达安装的z坐标:支架高度 / 2 + 雷达高度 / 2  -->
    <xacro:property name="laser_m" value="0.1" /> <!-- 雷达质量 -->
    <!-- 雷达关节以及link -->
    <link name="vodyne">
        <visual>
            <geometry>
                <cylinder radius="${laser_radius}" length="${laser_length}" />
            </geometry>
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
            <material name="black" />
        </visual>
        <collision>
            <geometry>
                <cylinder radius="${laser_radius}" length="${laser_length}" />
            </geometry>
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
        </collision>
        <xacro:cylinder_inertial_matrix m="${laser_m}" r="${laser_radius}" h="${laser_length}" />
    </link>

    <joint name="vodynetoBaselink" type="fixed">
        <parent link="base_link" />
        <child link="vodyne" />
        <origin xyz="${laser_x} ${laser_y} ${laser_z}" />
    </joint>
    <gazebo reference="vodyne">
        <material>Gazebo/Blue</material>
    </gazebo>
</robot>

3.添加IMU

imu的配置文件my_sensor_imu.xacro

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro"> 
    <gazebo reference="imu">
        <gravity>true</gravity>
        <sensor name="imu_sensor" type="imu">
            <always_on>true</always_on>
            <update_rate>100</update_rate>
            <visualize>true</visualize>
            <topic>__default_topic__</topic>
            <plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
                <topicName>imu</topicName>
                <bodyName>imu</bodyName>
                <updateRateHZ>100.0</updateRateHZ>
                <gaussianNoise>0.0</gaussianNoise>
                <xyzOffset>0 0 0</xyzOffset>     
                <rpyOffset>0 0 0</rpyOffset>
                <frameName>imu</frameName>        
            </plugin>
            <pose>0 0 0 0 0 0</pose>
        </sensor>
    </gazebo>
</robot>

在gazebo环境下添加imu物理模型my_imu_gazebo.xacro

<robot name="imu" xmlns:xacro="http://wiki.ros.org/xacro">  
    <xacro:property name="imu_offset_x" value="0" />
    <xacro:property name="imu_offset_y" value="0" />
    <xacro:property name="imu_offset_z" value="0.02" />
    <xacro:property name="imu_size"     value="0.01" />
    <xacro:property name="imu_m" value="0.01" /> <!-- imu质量 -->
    <!-- imu -->
    <joint name="imutoVodyne" type="fixed">
        <origin xyz="${imu_offset_x} ${imu_offset_y} ${imu_offset_z}" rpy="0 0 0" />
        <parent link="vodyne"/>
        <child link="imu"/>
    </joint>
        
    <link name="imu">
        <visual>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                    <box size="${imu_size} ${imu_size} ${imu_size}"/>
            </geometry>                
            <material name= "red" >
              <color rgba="1.0 0.0 0.0 1.0" />
            </material>
        </visual>
        <collision>
            <geometry>
                <box size="${imu_size} ${imu_size} ${imu_size}" />
            </geometry>
            <origin xyz="0.0 0.0 0" rpy="0.0 0.0 0.0" />
        </collision>
        <xacro:Box_inertial_matrix m = "${imu_m}" l = "${imu_size}" w = "${imu_size}" h = "${imu_size}"/>
    </link>

    <gazebo reference="imu">
        <material>Gazebo/Red</material>
    </gazebo>
</robot>

4.测试仿真

将小车底盘、多线激光雷达、Imu组合到一起的xacro文件 my_car_vodyne_imu.xacro

<!-- 组合小车底盘与imu 激光雷达-->
<robot name="my_car_camera_laser" xmlns:xacro="http://wiki.ros.org/xacro" reference= "base_footprint_radius">

  <!--注意inliude的顺序!-->
    <xacro:include filename="my_inertial.xacro" />
    <xacro:include filename="my_car_Cylider.xacro" />
    <xacro:include filename="my_vodyne_gazebo.xacro" />
    <xacro:include filename="my_imu_gazebo.xacro" />

    <!--rplidar仿真的xacro文件-->
    <xacro:include filename="$(find myrobot_gazebo)/urdf/sensor/my_sensor_rplidar.xacro" />

    <!--摄像头仿真的xacro文件-->
    <!--xacro:include filename="$(find myrobot_gazebo)/urdf/sensor/my_sensor_camera.xacro" /-->

    <!--imu仿真的xacro文件-->
    <xacro:include filename="$(find myrobot_gazebo)/urdf/sensor/my_sensor_imu.xacro" />

    <!--vodyne仿真的xacro文件-->
    <xacro:include filename="$(find myrobot_gazebo)/urdf/sensor/my_sensor_vodyne.xacro" />

</robot>

编写launch文件 my_car_vodyne_imu.launch

<launch>
    <!-- 将 Urdf 文件的内容加载到参数服务器 -->
    <param name="robot_description" command="my_car_vodyne_imu.xacro" />
    <!-- 启动 gazebo -->
    <include file="$(find gazebo_ros)/launch/empty_world.launch">
    </include>

    <!-- 在 gazebo 中显示机器人模型 -->
    <node pkg="gazebo_ros" type="spawn_model" name="model" args="-urdf -model mycar -param robot_description"  />

    <!--发布机器人的状态-->
    <node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher" output="screen" />
    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen" />
</launch>

打开终端,启动launch文件,可以看到组合到一起的效果
在这里插入图片描述另开一个终端 输入

rostopic pub -r 10 /cmd_vel geometry_msgs/Twist '{linear: {x: 0.2, y: 0, z: 0}, angular: {x: 0, y: 0, z: 0.5}}'

gazebo下测试的效果
在这里插入图片描述

在这里插入图片描述

  • 25
    点赞
  • 166
    收藏
    觉得还不错? 一键收藏
  • 35
    评论
激光雷达IMU是常用的传感器组合,可以用于实现机器人的定位与环境感知。对于不同的激光雷达,我们可以通过以下几个方面来进行性能测估: 1. 测距精度:激光雷达的主要功能是测量距离,因此测距精度是其最基本的性能指标。可以通过在不同距离下测量标准物体的距离来评估激光雷达的测距精度。 2. 视场角度:激光雷达的视场角度决定了其能够覆盖的范围。可以通过观察激光雷达的扫描数据来评估其视场角度。 3. 分辨率:激光雷达的分辨率决定了其能够检测到的最小物体尺寸。可以通过在不同距离下检测不同大小的物体来评估激光雷达的分辨率。 4. 反射率:激光雷达的反射率决定了其能够检测到的物体类型。可以通过在不同距离下检测不同类型的物体来评估激光雷达的反射率。 对于IMU,常用的性能指标包括: 1. 姿态精度:IMU可以测量机器人的姿态,姿态精度是其最基本的性能指标。可以通过比较IMU输出的姿态与参考姿态的差异来评估其姿态精度。 2. 加速度计精度:IMU中的加速度计可以测量机器人的加速度,加速度计精度是其最基本的性能指标。可以通过将IMU放置在不同角度下进行测试来评估其加速度计精度。 3. 陀螺仪精度:IMU中的陀螺仪可以测量机器人的角速度,陀螺仪精度是其最基本的性能指标。可以通过将IMU放置在不同角度下进行测试来评估其陀螺仪精度。 通过对激光雷达IMU的性能测评,可以选择合适的传感器组合,以满足机器人的定位与环境感知需求。
评论 35
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值