amcl¶

2.2.1.1 diff¶

　diffモデルでは、オドメトリの並進速度に対する、各パーティクルへの回転速度・並進速度へのノイズ、オドメトリの回転速度に対する、各パーティクルへの回転速度・並進速度へのノイズが設定できます。前後にのみ移動するロボットに対するモデルになっているため、それ以外の方向にも移動できるロボットの場合はomniにモデルを使用したほうが良いです。

　具体的な計算方法は、以下のようになります。

1. 前回のamclでのヨー角に対する現在のオドメトリの移動方向の角度の差分を始点角度差分とする。
2. オドメトリの移動方向の角度と現在のオドメトリの回転速度の差分を終点角度差分とする。
3. 始点角度差分・終点角度差分・並進速度に対してガウシアンノイズを追加する。
4. 各パーティクルにおいて、パーティクルのヨー角に始点角度差分を足した角度を移動方向とする。
5. 移動方向に対して並進速度の移動量を足して、現在のパーティクルの位置姿勢を求める。

2.2.1.2 omni¶

　omniモデルは、オムニホイール等を使用した前後以外にも進むことのできるロボットに対応した運動モデルになります。diffで設定できるノイズの他にオドメトリの並進速度に対する、各パーティクルへの進行方向と垂直な成分のノイズが設定できます。

　具体的な計算方法は、以下のようになります。

1. 前回のamclでのヨー角に対する移動方向を求めるために、前回のamclでのヨー角に対する現在のオドメトリの移動方向の角度の差分を計算する。
2. 角度の差分を各パーティクルのヨー角に加算して、それぞれのパーティクルでの移動方向を推定する。
3. 現在のオドメトリの並進速度・回転速度から各ノイズの大きさをガウシアンフィルタを用いて計算する。
4. オドメトリの値・移動方向に対する並進のノイズ・回転のノイズによって、現在のパーティクルの位置姿勢を求める。

2.2.2.1 ビームモデルを用いたレーザー¶

　ビームモデルでは、「ガウシアンノイズ」と「障害物対策ノイズ」「センサの最大距離ノイズ」「ランダムノイズ」の組み合わせで使用しています。 　それぞれのパーティクルの位置で各レーザの方向に伸ばしていき、マップ上で壁に当たる距離を計算し、その距離と実際のレンジの値によって尤度を計算します。そのレーザーが返ってくると期待される値によって尤度を求めていきます。壁に当たる位置はレーザーごとに異なるため、「パーティクル数」×「レーザー数」分、マップ上で壁に当たる位置を計算する必要があります。

ガウシアンノイズ¶

　ガウシアンノイズは、マップ上で壁に当たる距離(レーザーで期待される値)の周辺では尤度が高くなるようになります。

障害物対策ノイズ¶

　地図上のレーザーが当たる地点より前に、地図にない障害物がありレンジの値が小さくなる場合があります。その際の尤度を大きくするため。障害物対策のノイズでは、想定されるレンジより小さな値には尤度を少し上げるようなノイズを追加します。

センサの最大距離ノイズ¶

　乱反射等によりレーザーがセンサに帰ってこなかった場合、レンジの距離はセンサの最大値が設定される場合があります。その際の尤度を大きくするため。センサの最大距離ノイズでは、センサの最大距離部分では尤度が高くなるようなノイズを追加します。

ランダムノイズ¶

　パーティクルフィルタで安定して動作させるため、ある程度外れてしまっているパーティクルがすべて削除されることがないように、想定されるレンジの値に関わらず尤度を持たせます。ランダムノイズでは、すべての距離で一律の尤度を持たせるようなノイズを追加します。

全ノイズの合成¶

　総合した尤度のモデルは以下のようになります。各レーザーごとに以下のモデルで尤度を計算していきます。

　各ノイズの重みは、以下のパラメータで設定できます。実行する状況に合わせてノイズの重みを変更することができます。各重みの和が1になるようにする必要があります。

• ガウシアンノイズの重み：laser_z_hit
• 障害物対策ノイズの重み：laser_z_short
• センサの最大距離ノイズの重み：laser_z_max
• ランダムノイズの重み：laser_z_rand

2.2.2.2 尤度モデルを用いたレーザー¶

　尤度モデルでは、「ガウシアンノイズ」と「ランダムノイズ」の組み合わせで使用しています。マップの座標ごとにガウシアンノイズが決定するため、ビームモデルと異なり各レーザーごと壁に当たる位置を判定する必要はなく、レーザーの当たっている位置を求めるだけでよくなります。

ガウシアンノイズ¶

　ガウシアンノイズは、レーザーモデルとは異なり、壁からの距離で求める。

　壁に近いほうが尤度が高くなる。レーザーが当たった座標の尤度を使用する。

ランダムノイズ¶

　ランダムノイズはビームモデルと同じで、パーティクルフィルタで安定して動作させるため、ある程度外れてしまっているパーティクルがすべて削除されることがないように、想定されるレンジの値に関わらず尤度を持たせます。ランダムノイズでは、すべての距離で一律の尤度を持たせるようなノイズを追加します。

　下記マップのレーザーの座標の尤度の和がパーティクルの尤度になる。尤度マップを計算した後は、各レーザーの尤度はパーティクルの位置姿勢とレーザーの値から求められる座標の尤度を参照するだけため、計算量がビームモデルに対して小さくなる。

　各ノイズの重みは、以下のパラメータで設定できます。実行する状況に合わせてノイズの重みを変更することができます。各重みの和が1になるようにする必要があります。

• ガウシアンノイズの重み：laser_z_hit
• ランダムノイズの重み：laser_z_rand

2.2.2.3 ビームモデルと尤度モデルの違い¶

　ビームモデルは各レーザーごとに計算を行い処理に時間がかかるため、レーザー数・パーティクル数が多い状態でリアルタイムに実行したい場合は尤度モデルを使用するのが良いです。また、尤度モデルでは、障害物対策ノイズ等が使用されないため、人が多い等でマップと異なる障害物が多い場合は、ビームモデルが良いと思われます。

2.2.3.1 Augmented_MCL¶

　Augmented MCLとは、 位置推定がうまくいっている「安定状態」と誘拐ロボット問題を含む位置を見失っている「誘拐状態」によってランダムにパーティクルを配置する割合を調整する処理です。

　ロボットの状態を認識するためには、各パーティクルの尤度の平均を使用します。位置推定がうまくいっている場合は、ロボットの付近にパーティクルが多く配置されて、尤度平均は大きくなります。ロボットが想定の位置にいない場合は、多くのパーティクルがロボットの付近にいなくなるため、尤度平均は小さくなります。

　Augmented MCLでは尤度平均が小さくなると、ランダムに配置するパーティクルの数を増やします。具体的には、前回より尤度平均が小さくなった段階で、2種類の平滑化係数(recovery_alpha_slow, recovery_alpha_fast)で指数移動平均を求め、その割合でランダムパーティクルの割合が決定します。指数移動平均とは、ひとつ前の重みに定数をかけることで、指数関数的に減少させる手法で、下記のように2種類の重み(weight_slow, weight_fast)を指数移動平均で求め、平滑化係数の違いによって収束速度が異なり、値の差によってランダムパーティクルの割合を決定します。

weight_slow = (1 - recovery_alpha_slow) * weight_slow + recovery_alpha_slow * (尤度平均)
weight_fast = (1 - recovery_alpha_fast) * weight_fast + recovery_alpha_fast * (尤度平均)
(ランダムパーティクルの割合) = 1 - (weight_fast / weight_slow)

　weight_fastがweight_slowより大きくなった段階で、重みをすべてリセットするためランダムパーティクルの割合が負の値になることはありません。平滑化係数は大きくするほど最初の値の影響は小さくなるため、尤度が下がった際のランダムパーティクル割合はrecovery_alpha_fast、recovery_alpha_slowの値によって制御できます。

2.2.3.2 KLD_Sampling_MCL¶

　KLD Sampling(Kullback-Leibler Divergence Sampling)とは，パーティクルの分布状況に合わせて，数を動的に決定するものです。分布が広範囲の場合パーティクル数を増やし、狭い範囲の場合パーティクル数を減らします。実際には、地図を格子状に分割して、パーティクルがある領域の数により全体のパーティクル数を決定します。ランダムなパーティクルが多くなるとパーティクルのある領域も増えてパーティクル数が多くなります。