ROS学习系列：第五讲 — SDF模型文件常用传感器模型

IMU、GPS、磁罗盘

IMU、GPS、磁罗盘的观测模型都可用下式描述：

• $x_m$为测量值，$x$为真值
• $b$为漂移误差
• $n$和$n_b$都是高斯噪声，其中$n$为定常误差，$n_b$为$b$对时间的梯度。

<plugin name='rotors_gazebo_imu_plugin' filename='libgazebo_imu_plugin.so'>
	<robotNamespace/>
	<linkName>/imu_link</linkName>
	<imuTopic>/imu</imuTopic>
	<gyroscopeNoiseDensity>0.0003394</gyroscopeNoiseDensity>
	<gyroscopeRandomWalk>3.8785e-05</gyroscopeRandomWalk>
	<gyroscopeBiasCorrelationTime>1000.0</gyroscopeBiasCorrelationTime>
	<gyroscopeTurnOnBiasSigma>0.0087</gyroscopeTurnOnBiasSigma>
	<accelerometerNoiseDensity>0.004</accelerometerNoiseDensity>
	<accelerometerRandomWalk>0.006</accelerometerRandomWalk>
	<accelerometerBiasCorrelationTime>300.0</accelerometerBiasCorrelationTime>
	<accelerometerTurnOnBiasSigma>0.196</accelerometerTurnOnBiasSigma>
</plugin>

<plugin name='gps_plugin' filename='libgazebo_gps_plugin.so'>
	<robotNamespace/>
	<gpsNoise>true</gpsNoise>
	<!--the paremeters of GPS noise is defined on gazebo_gps_plugin.h-->
</plugin>

<plugin name='magnetometer_plugin' filename='libgazebo_magnetometer_plugin.so'>
	<robotNamespace/>
	<pubRate>20</pubRate>
	<noiseDensity>0.0004</noiseDensity>
	<randomWalk>6.4e-06</randomWalk>
	<biasCorrelationTime>600</biasCorrelationTime>
	<magTopic>/mag</magTopic>
</plugin>

主要参数含义：

参数	描述
NoiseDensity	$n$的标准差
RandomWalk	$n_b$的标准差
BiasCorrelationTime	$b$的相关时间
TurnOnBiasSigma	$b$的初始标准差

立体相机

立体相机是SDF1.5中multicamera这个类型的sensor的实例化，配合libgazebo_ros_multicamera.so插件来实现将话题发布到ros中的功能，可配置参数结构如xml中所示：

 <sensor name="camera" type="multicamera">
        <always_on>1</always_on>
        <update_rate>30</update_rate>
        <visualize>false</visualize>
        <camera name="left">
          <pose>0 0.06 0 0 0 0</pose>
          <horizontal_fov>1.5708</horizontal_fov>
          <image>
            <width>752</width>
            <height>480</height>
            <format>R8G8B8</format>
          </image>
          <clip>
            <near>0.1</near>
            <far>300</far>
          </clip>
        </camera>
        <camera name="right">
          <pose>0 -0.06 0 0 0 0</pose>
          <horizontal_fov>1.5708</horizontal_fov>
          <image>
            <width>752</width>
            <height>480</height>
            <format>R8G8B8</format>
          </image>
          <clip>
            <near>0.1</near>
            <far>300</far>
          </clip>
        </camera>
        <plugin name="stereo_camera_controller" filename="libgazebo_ros_multicamera.so">
          <robotNamespace></robotNamespace>
          <alwaysOn>true</alwaysOn>
          <updateRate>30</updateRate>
          <cameraName>/stereo_camera</cameraName>
          <imageTopicName>image_raw</imageTopicName>
          <cameraInfoTopicName>camera_info</cameraInfoTopicName>
          <frameName>stereo_camera_frame</frameName>
          <hackBaseline>0.12</hackBaseline>
          <Fx>376.0</Fx>
          <Fy>376.0</Fy>
          <Cx>376.0</Cx>
          <Cy>240.0</Cy>
          <distortionK1>-0.1</distortionK1>
          <distortionK2>0.01</distortionK2>
          <distortionK3>0.0</distortionK3>
          <distortionT1>5e-5</distortionT1>
          <distortionT2>-1e-4</distortionT2>
        </plugin>
</sensor>

主要参数含义：

注意，上面的参数有些是相互影响的，比如图像宽高、内参与视场角，因此调节的时候需要保证一致性。

雷达

雷达是SDF1.5格式中的ray类型sensor的实例化，配合libgazebo_ros_laser.so插件来实现将话题发布到ros中的功能，所有可配置参数如xml中所示：

<sensor name="lidar" type="ray">
    <pose></pose>
    <ray>
        <scan>
            <horizontal>
                <samples></samples>
                <resolution></resolution>
                <min_angle></min_angle>
                <max_angle></max_angle>
            </horizontal>
            <vertical></vertical>
            
        </scan>
      <range>
            <min></min>
            <max></max>
            <resolution></resolution>
        </range>
        <noise>
            <type></type>
          <mean></mean>
            <stddev></stddev>
        </noise>
    </ray>
  <plugin name="lidar_ros_plugin" filename = "libgazebo_ros_laser.so">
    <topicName></topicName>
        <frameName></frameName> 
    </plugin>
    <always_on></always_on>
    <update_rate></update_rate>
    <visualize></visualize>
</sensor>

主要参数含义：

3D激光雷达

<sensor name='3d_lidar' type='ray'>
      <ray>
        <scan>
          <horizontal>
            <samples>512</samples>
            <resolution>1</resolution>
            <min_angle>-3.1415926535897931</min_angle>
            <max_angle>3.1415926535897931</max_angle>
          </horizontal>
          <vertical>
            <samples>32</samples>
            <min_angle>-0.2617993877991494365</min_angle>
            <max_angle>0.2617993877991494365</max_angle>
          </vertical>
        </scan>
        <range>
          <min>0.2</min>
          <max>30</max>
          <resolution>0.01</resolution>
        </range>
      </ray>
      <plugin name='3d_laser' filename='libgazebo_ros_velodyne_laser.so'>
        <topicName>/3dscan</topicName>
        <frameName>3d_laser</frameName>
        <gaussianNoise>0.008</gaussianNoise>
      </plugin>
      <always_on>1</always_on>
      <update_rate>7</update_rate>
      <visualize>0</visualize>
    </sensor>

libgazebo_ros_velodyne_laser.so需要引用velodyne_simulator功能包。可通过源码编译安装；

参考链接：https://www.yuque.com/xtdrone/manual_cn/sensor_model