对于射线传感器,我们将高斯噪声添加到每个光束的范围内。您可以设置从中采样噪声值的高斯分布的平均值和标准偏差。对每个光束独立地采样噪声值。添加噪声后,将得到的范围钳位在传感器的最小和最大范围(包括该范围)之间。
mkdir -p ~/.gazebo/models/noisy_laser
gedit ~/.gazebo/models/noisy_laser/model.config
-
模型配置
<?xml version="1.0"?> <model> <name>Noisy laser</name> <version>1.0</version> <sdf version='1.6'>model.sdf</sdf> <author> <name>My Name</name> <email>me@my.email</email> </author> <description> My noisy laser. </description> </model>
-
模型sdf描述
gedit ~/.gazebo/models/noisy_laser/model.sdf
<?xml version="1.0" ?> <sdf version="1.6"> <model name="hokuyo"> <link name="link"> <gravity>false</gravity> <inertial> <mass>0.1</mass> </inertial> <visual name="visual"> <geometry> <mesh> <uri>model://hokuyo/meshes/hokuyo.dae</uri> </mesh> </geometry> </visual> <sensor name="laser" type="ray"> <pose>0.01 0 0.03 0 -0 0</pose> <ray> <scan> <horizontal> <samples>640</samples> <resolution>1</resolution> <min_angle>-2.26889</min_angle> <max_angle>2.268899</max_angle> </horizontal> </scan> <range> <min>0.08</min> <max>10</max> <resolution>0.01</resolution> </range> <!--##### 此处调整噪声,以米为单位,调整平均值和标准偏差值 #####--> <noise> <type>gaussian</type> <mean>0.0</mean> <stddev>0.01</stddev> </noise> <!--#########################################--> </ray> <plugin name="laser" filename="libRayPlugin.so" /> <always_on>1</always_on> <update_rate>30</update_rate> <visualize>true</visualize> </sensor> </link> </model> </sdf>
查看话题/gazebo/default/hokuyo/link/laser/scan
观察现象
Hokuyo雷达噪声参数如下:
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.01</stddev>
</noise>
相机噪声
对于相机传感器,我们对输出放大器噪声建模,该噪声会给每个像素独立增加一个高斯采样干扰。您可以设置从中采样噪声值的高斯分布的平均值和标准偏差。为每个像素独立采样一个噪声值,然后将该噪声值独立添加到该像素的每个颜色通道。在添加噪声之后,所得的颜色通道值将被钳位在0.0到1.0之间;否则,颜色通道值将被限制在0.0到1.0之间。该浮点颜色值将最终以无符号整数形式出现在图像中,通常在0到255之间(每个通道使用8位)。此噪声模型在GLSL着色器中实现,并且需要GPU运行。
-
模型配置
mkdir -p ~/.gazebo/models/noisy_camera gedit ~/.gazebo/models/noisy_camera/model.config
<?xml version="1.0"?> <model> <name>Noisy camera</name> <version>1.0</version> <sdf version='1.6'>model.sdf</sdf> <author> <name>My Name</name> <email>me@my.email</email> </author> <description> My noisy camera. </description> </model>
-
模型sdf描述
gedit ~/.gazebo/models/noisy_camera/model.sdf
<?xml version="1.0" ?> <sdf version="1.6"> <model name="camera"> <link name="link"> <gravity>false</gravity> <pose>0.05 0.05 0.05 0 0 0</pose> <inertial> <mass>0.1</mass> </inertial> <visual name="visual"> <geometry> <box> <size>0.1 0.1 0.1</size> </box> </geometry> </visual> <sensor name="camera" type="camera"> <camera> <horizontal_fov>1.047</horizontal_fov> <image> <width>1024</width> <height>1024</height> </image> <clip> <near>0.1</near> <far>100</far> </clip> <!--####### 无单位值,杂波将添加到[0.0,1.0]范围内每个颜色通道 #######--> <noise> <type>gaussian</type> <mean>0.0</mean> <stddev>0.07</stddev> </noise> <!--####################--> </camera> <always_on>1</always_on> <update_rate>30</update_rate> <visualize>true</visualize> </sensor> </link> </model> </sdf>
观察话题/gazebo/default/camera/link/camera/image
对于高质量的相机,以下值是合理的:
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.007</stddev>
</noise>
IMU噪声
对于IMU传感器,我们对角速度和线性加速度的两种干扰进行建模:噪声和偏置。分别考虑角速度和线性加速度,从而为该模型提供4组参数:速度噪声,速率偏差,加速度噪声和加速度偏差。IMU的方向数据不会受到任何干扰,IMU的方向数据会在世界范围内提取为理想值(将来会有所变化)。
噪声是可加的,是从高斯分布中采样的。您可以设置高斯分布的平均值和标准偏差(一个用于比率,一个用于加速度),并从中取样噪声值。对每个样本的每个分量(X,Y,Z)独立采样噪声值,并将其添加到该分量。
偏置也是可加的,但在仿真开始时会被采样一次。您可以设置高斯分布的平均值和标准偏差(一个用于比率,一个用于加速度),从中将采样偏差值。将根据提供的参数对偏差进行采样,然后以相等的概率取反;假设提供的均值表示偏差的大小,并且在两个方向上均可能存在偏差。此后,偏差是固定值,添加到每个样本的每个分量(X,Y,Z)。
注意:根据模拟的系统和物理引擎的配置,可能会发生模拟的IMU数据已经非常嘈杂的情况,因为系统并没有完全解决该问题。因此,根据您的应用,可能没有必要添加噪音。
-
模型配置文件:
mkdir -p ~/.gazebo/models/noisy_imu gedit ~/.gazebo/models/noisy_imu/model.config
<?xml version="1.0"?> <model> <name>Noisy IMU</name> <version>1.0</version> <sdf version='1.6'>model.sdf</sdf> <author> <name>My Name</name> <email>me@my.email</email> </author> <description> My noisy IMU. </description> </model>
-
模型sdf描述:
gedit ~/.gazebo/models/noisy_imu/model.sdf
- 1
<?xml version="1.0" ?> <sdf version="1.6"> <model name="imu"> <link name="link"> <inertial> <mass>0.1</mass> </inertial> <visual name="visual"> <geometry> <box> <size>0.1 0.1 0.1</size> </box> </geometry> </visual> <collision name="collision"> <geometry> <box> <size>0.1 0.1 0.1</size> </box> </geometry> </collision> <sensor name="imu" type="imu"> <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> <always_on>1</always_on> <update_rate>1000</update_rate> </sensor> </link> </model> </sdf>
要调整噪音,只需在中使用平均值和标准偏差值即可。速率噪声和速率偏差的单位为rad/s,加速度噪声和加速度偏差的单位为m/ s^2。
对于质量高IMU,以上数值都是合理的!
参考文献:
评论(0)
您还未登录,请登录后发表或查看评论