九州工業大学 CIR-KIT Blog

九工大自律移動ロボット製作プロジェクトCIR-KITの技術系ブログ

No.5-1:センサについて (センサノイズモデル 編)

gazebo_logo

検証日時

02/14/2015 (Sat)

概要

Gazeboのチュートリアル第五弾「Sensors」。
シミュレーションのセンサは環境を分析し、センサになるべくよく似たデータストリームを生成します。
チュートリアルでは、センサの使い方と修正方法について説明します。 今回はその「Sensor Noise Model」編です。
公式サイトを適当に翻訳しただけですので、あしからず。

レベル

BEGINNER

環境
PC : Lenovo ThinkPad X240
Prosessor : Intel Core i7-4600U (2.10GHz, 4MB, 1600MHz)
RAM : PC3-12800 DDR3L (8GB)
OS : Ubuntu 14.04 LTS 64bit
Kernel : 3.13.0-44-generic
Gazebo : Version 5.0.1
参考

GAZEBO Tutorial-Sensors

はじめに

Gazeboでは、たくさんの一般的なセンサを提供しています。現実世界では、センサにはノイズが存在し、実環境を完全に観測することは出来ません。デフォルトでは、Gazeboのセンサは環境を完全に観測します(IMUは含みません;続きを参照ください)。認識プログラムを試してみる様な、よりリアルな環境を提供するには、Gazeboのセンサが生成したデータにあえてノイズを付け加える必要があります。
この記事を書いている時点で、Gazeboは以下のタイプのセンサにノイズを付加することができます。

  • Ray (e.g., lasers)
  • Camera
  • IMU

Ray(Laser)ノイズ

rayセンサでは、それぞれのビームの距離値にガウシアンノイズを付加します。ノイズの値として付加されるガウシアン分布の平均値と標準偏差を設定することができます。ノイズの値はそれぞれのビームに独立して付加されます。ノイズを付加した後、距離はセンサの最小距離と最大距離の間で偽の値（ノイズが付加された値）になります。
rayノイズモデルをテストするには:

  1. モデルのディレクトリを作る
1

  mkdir -p ~/.gazebo/models/noisy_laser
  1. configファイルを生成する
1

  gedit ~/.gazebo/models/noisy_laser/model.config
  1. model.configファイルに以下をコピペする
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15

<?xml version="1.0"?>
<model>
  <name>Noisy laser</name>
  <version>1.0</version>
  <sdf version='1.4'>model.sdf</sdf>

  <author>
   <name>My Name</name>
   <email>me@my.email</email>
  </author>

  <description>
    My noisy laser.
  </description>
</model>
  1. ~/.gazebo/models/noisy_laser/model.sdfファイルを生成する
1

gedit ~/.gazebo/models/noisy_laser/model.sdf
  1. 以下をmodel.sdfファイルにコピペする。以下のモデルはスタンダードなHokuyoモデルにノイズを付加したものです。:
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45

<?xml version="1.0" ?>
<sdf version="1.4">
  <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>
  1. Gazeboを起動する:
1

gazebo

  1. ノイズを付加したレーザーセンサを挿入する : 画面左側の枠の中で、Insertタブを選び、Noisy laserをクリックする。その後、シミュレータにそれを設置する。

  2. ノイズを付加したレーザーセンサを観測する : Topis Selectorを表示するために、Window->Topic Visualazationをクリックする(もしくは、Ctrl-Tを押す)。

  3. /gazebo/default/hokuyo/link/laser/scanのような名前のトピックを探し、クリックしたら、Okayをクリックしてください。すると、レーザーデータが表示されたウィンドウが見えるはずです。

スキャンデータにノイズが付加されているのを確認できます。ノイズを調節するには、平均値と標準偏差をmodel.sdfにある以下のブロックでで変更するだけでよいです。なお、それらの要素はすべてメートル単位系です。:

1
2
3
4
5

<noise>
  <type>gaussian</type>
  <mean>0.0</mean>
  <stddev>0.01</stddev>
</noise>

Hokuyoレーザーにはピッタリの値があります。
尚、これらの手順は、以下の動画を参照してください。動画では、ノイズを見やすくするために、平均値を0、標準偏差を0.1としてあります。

カメラノイズ

カメラのノイズでは、output amplifier noiseを付加します。これは、各ピクセルに独立したガウシアンサンプルノイズを付加するものです。各ピクセルにでサンプリングされる値に、ガウシアン分布の平均値と標準偏差を設定することができます。ノイズの値は各ピクセルで独立しており、ノイズの値は各ピクセルの各色チャンネルにも独立して付加されます。ノイズを付加した後、色チャンネルの値は0.0から1.0の間で偽の値を取ります。;このfloat型のポイントの色の値はイメージの中では、unsigned int型として扱われます。普通は、0から255の間の値です。(8 [ビット/チャンネル] を使用します)
このノイズモデルはGLSLの中で、シェーダーやGPUを起動するときの必要要件として実行されます。
カメラノイズモデルをテストするには:

  1. モデルディレクトリをつくります:
1

mkdir -p ~/.gazebo/models/noisy_camera
  1. model.configファイルを生成する
1

gedit ~/.gazebo/models/noisy_camera/model.config
  1. model.configに以下の内容をコピペする
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15

<?xml version="1.0"?>
<model>
  <name>Noisy camera</name>
  <version>1.0</version>
  <sdf version='1.4'>model.sdf</sdf>

  <author>
   <name>My Name</name>
   <email>me@my.email</email>
  </author>

  <description>
    My noisy camera.
  </description>
</model>
  1. ~/.gazebo/models/noisy_camera/model.sdfファイルを生成する
1

gedit ~/.gazebo/models/noisy_camera/model.sdf
  1. 以下の内容をmodel.sdfにコピペする。これは、基本的なカメラモデルにノイズを付加したモデルです。:
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40

<?xml version="1.0" ?>
<sdf version="1.4">
  <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>
          <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>
  1. Gazeboを起動する:
1

gazebo

  1. ノイズが付加されたカメラを挿入する : 画面左側の枠の中で、Insertタブを選び、Noisy cameraをクリックする。その後、シミュレータにそれを設置する。

  2. ノイズを付加したカメラを観測する : Topis Selectorを表示するために、Window->Topic Visualazationをクリックする(もしくは、Ctrl-Tを押す)。

  3. /gazebo/default/camera/link/camera/imageのような名前のトピックを探し、クリックしたら、Okayをクリックしてください。すると、カメラのイメージが表示されたウィンドウが見えるはずです。

よく見ると、イメージにノイズが付加されているのが見えると思います。ノイズを調整するには、model.sdfファイルの中にある以下のようなセクションで、平均値と標準偏差を調節してやるだけで良いです。これらには単位系はありません。;各色チャンネルに0.0以上、1.0以下の値でノイズを付加します。
以下のレイは、<stddev>の値をとても大きくしています。この値を減らしてみてください。:

1
2
3
4
5

<noise>
  <type>gaussian</type>
  <mean>0.0</mean>
  <stddev>0.007</stddev>
</noise>

デジタルカメラには、最適なノイズの値があります。
尚、これらの手順は、以下の動画を参照してください。動画では、ノイズを見やすくするために、平均値を0、標準偏差を0.07としたものと、平均値を0、標準偏差を0.007としたものを表示しています。

IMUノイズ

IMUセンサでは、角度速度と並進加速度に対する2種類の外乱(ノイズとバイアス)を考えます。角速度と並進加速度は分かれているものとして、このモデルでは、速度ノイズ、速度バイアス、加速度ノイズ、加速度バイアスの4つのパラメータを導出します。世界座標系（将来的には変化する）の中で、完全な値として表現されるようなIMUの元データから応用するノイズはありません。
ノイズは付加的であり、ガウシアン分布からサンプリングされます。ノイズの値をサンプリングしてくる、ガウシアン分布（速度や、角速度の一つ一つに対するもの）の平均値と標準偏差を設定することができます。ノイズの値は、互いのサンプリング要素(X,Y,Z)では独立しており、それらの要素に付加されます。
バイアスもまた付加的でありますが、これは、シミュレーションがスタートした時の一度だけしかサンプリングされません。バイアスの値をサンプリングしてくる、ガウシアン分布（速度や、角速度の一つ一つに対するもの）の平均値と標準偏差を設定することができます。バイアスは設定されたパラメータに沿ってサンプリングされます;提供される平均値はバイアスの大きさを定義し、各方向にバイアスがかかっており、バイアスは修正された値で、各要素(X,Y,Z)に付加されていると仮定されます。
Note : シミュレートされているシステムと物理エンジンの構成に依存しているので、シミュレーションされたIMUのデータはすでにいくぶんかノイズがのっていることがあります。なぜなら、システムはすべての収斂の方法を解決しているわけではないからです。なので、アプリケーションに依存するという点では、ノイズを付加する必要はないかもしれません。
IMUノイズモデルをテストするには:

  1. モデルのディレクトリを作る:
1

mkdir -p ~/.gazebo/models/noisy_imu
  1. model.configファイルを生成する
1

gedit ~/.gazebo/models/noisy_imu/model.config
  1. model.configに以下の内容をコピペする
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15

<?xml version="1.0"?>
<model>
  <name>Noisy IMU</name>
  <version>1.0</version>
  <sdf version='1.4'>model.sdf</sdf>

  <author>
   <name>My Name</name>
   <email>me@my.email</email>
  </author>

  <description>
    My noisy IMU.
  </description>
</model>
  1. ~/.gazebo/models/noisy_imu/model.sdfファイルを生成する
1

gedit ~/.gazebo/models/noisy_imu/model.sdf
  1. model.sdfに以下の内容をコピペする:
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45

<?xml version="1.0" ?>
<sdf version="1.4">
  <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>
          <noise>
            <type>gaussian</type>
            <rate>
              <mean>0.0</mean>
              <stddev>2e-4</stddev>
              <bias_mean>0.0000075</bias_mean>
              <bias_stddev>0.0000008</bias_stddev>
            </rate>
            <accel>
              <mean>0.0</mean>
              <stddev>1.7e-2</stddev>
              <bias_mean>0.1</bias_mean>
              <bias_stddev>0.001</bias_stddev>
            </accel>
          </noise>
        </imu>
        <always_on>1</always_on>
        <update_rate>1000</update_rate>
      </sensor>Laser
    </link>
  </model>
</sdf>
  1. Gazeboを起動する:
1

gazebo

  1. ノイズを付加したIMUを挿入する : 画面左側の枠の中で、Insertタブを選び、Noisy IMUをクリックする。その後、シミュレータにそれを設置する。

  2. ノイズを付加したIMUを観測する : Topis Selectorを表示するために、Window->Topic Visualazationをクリックする(もしくは、Ctrl-Tを押す)。

  3. /gazebo/default/camera/link/imu/imuのような名前のトピックを探し、クリックしたら、Okayをクリックしてください。すると、IMUデータのテキストビューウィンドウが見えるはずです。

ノイズをIMUのような高レートのセンサに適用するのは難しくなりがちです。特に複雑なシステムにおいてはそれはなおさらです。ノイズでゼロではない大きな値の平均値をとる　かつ/もしくは バイアスパラメータをとるのが友好的であるとわかるでしょう。
ノイズを調整するには、model.sdfファイルの中にある以下のようなセクションで、平均値と標準偏差を調節してやるだけで良いです。角速度のノイズとバイアスの単位系は[rad/s]で加速度のノイズとバイアスの単位系は[m/s2]です。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15

<noise>
  <type>gaussian</type>
  <rate>
    <mean>0.0</mean>
    <stddev>2e-4</stddev>
    <bias_mean>0.0000075</bias_mean>
    <bias_stddev>0.0000008</bias_stddev>
  </rate>
  <accel>
    <mean>0.0</mean>
    <stddev>1.7e-2</stddev>
    <bias_mean>0.1</bias_mean>
    <bias_stddev>0.001</bias_stddev>
  </accel>
</noise>

高い品質のIMU装置には、最適なノイズの値があります。
尚、これらの手順は、以下の動画を参照してください。平均値や標準偏差などは、上記の設定のものを使用しています。