ROSのナビゲーションamclについて理解を深めてみる

ROSのロボットの動作計画のスタック(パッケージの集合)としてnavigationスタックが有名です。
そこで今回はamclを使いこなせるようになってみたいと思います。

amclを使う上での事前知識

  • パーティクルフィルタを用いた2次元平面での自己位置推定(x, y, yawの推定)
  • パーティクル数は、推定の精度によって動的に変動する
  • 2d LiDARの観測モデルにしたがって尤度を更新する

以下パラメータでは、LiDARの観測モデルに関連した変数が登場します。LiDARの 観測モデルについてはこちらのページにてざっくり紹介しています。

LiDARの計測モデルについて | MY ROBOTICS

amclのパラメータについて

まず、私が使用しているlaunchファイルでのパラメータを載せておきます。

<launch>
    <node pkg="amcl" type="amcl" name="amcl" output="screen">
        <param name="min_particles" value="300"/>
        <param name="max_particles" value="3000"/>
        <param name="kld_err" value="0.01"/>
        <param name="kld_z" value="0.99"/>
        <param name="update_min_d" value="0.02"/>
        <param name="update_min_a" value="0.02"/>
        <param name="resample_interval" value="1"/>
        <param name="transform_tolerance" value="0.2" />
        <param name="recovery_alpha_slow" value="0.0"/>
        <param name="recovery_alpha_fast" value="0.0"/>
        <param name="initial_pose_x" value="0.0"/>
        <param name="initial_pose_y" value="0.0"/>
        <param name="initial_pose_a" value="0.0"/>
        <param name="initial_cov_xx" value="0.03"/>
        <param name="initial_cov_yy" value="0.001"/>
        <param name="initial_cov_aa" value="0.12"/>
    </node>
</launch>

min_particles :
パーティクルフィルタで使用するパーティクル数の最小値。

max_particles :
パーティクルフィルタで使用するパーティクル数の最大値。

kid_err :
真値と推定値との誤差???真値とは???

kid_z :
1-p。1-kid_err???

update_min_d :
update_min_d[m]前進する毎にフィルタをアップデート。
(同じ姿勢で度々パーディクルの尤度が更新されることを防ぐため。)

update_min_a :
update_min_a[rad]回転する毎にフィルタをアップデート。
(同じ姿勢で度々パーディクルの尤度が更新されることを防ぐため。)

resample_interval :
パーティクルをリサンプルする間隔。
パーティクルフィルタでは、各パーティクルの尤度にある程度のばらつきが生じた時点でリサンプリングが行われる。(その際に、尤度が低いパーティクルが消去される。)

transform_tolerance :
センサデータやtfなどのtimestampと現在時刻とのズレの許容範囲。センサデータの更新が無い場合には処理が停止してしまうのではないかと思われる。[sec]

recoverry_alpha_slow :
ランダムパーティクルをばら撒くことでのリカバリを試みる。0.0で無効に。ROS Wikiでの推奨値は0.001。

recoverry_alpha_fast :
ランダムパーティクルをばら撒くことでのリカバリを試みる。0.0で無効に.ROS Wikiでの推奨値は0.1。

initial_pose_x, y, a :
ロボットの初期姿勢。x, y, yaw。

initial_cov_xx, yy, aa :
ロボットの初期姿勢の分散。値が大きくなるほどばらまくパーティクルの分布、角度が広がる。

gui_publish_rate :
rviz等で可視化するためにpublishされるトピックの周波数。[Hz]

save_pose_rate :
ガウシアン(平均(姿勢)と分散)をパラメータサーバに保存する周波数。[Hz] 保存された姿勢は後の走行でフィルタを初期化するために使用される???
-1.0で無効に。

use_map_topic :
サービス(ROSの機能)によって得られるマップを使用する代わりに、mapトピックをsubscribeする.

first_map_only :
このフラグを立てると、一番最初のmapのみを使用し、新しいmapをsubscribeしても無視する。

LiDARによる尤度更新に関するパラメータ

ここから先は測域センサのパラメータとなります。
LiDARの観測モデルについてざっと説明したページも参考にしてみてください。

LiDARの計測モデルについて | MY ROBOTICS

このレーザのパラメータの説明は正確ではありません.ただ,直感的に理解しやすいようにそれっぽい言葉で説明してあります.

例えばロボットが持っている地図情報から,ロボットの前方に障害物があることが分かっているとします.

ロボット正面を観測しているレーザ1本のデータが

laser_z_hit :
Laserが地図に記載されている障害物に正しくhitし、その距離を観測する確率。

laser_z_short :
地図にはない障害物を検出して、地図とロボットの現在姿勢から導き出される想定観測距離よりも短い計測値を観測する確率。

laser_z_max :
レーザの反射光を受信できない等の計測失敗により、計測値が計測可能レンジの最大値をとってしまう確率。

laser_z_rand :
原因不明のランダムな雑音が発生する確率。

レーザモデルがscanの場合、上記4つのパラメータをすべて使用する。
レーザモデルがlikelihood_field(_prob)の場合はlaser_z_hitとlaser_z_randのみが使用される。
(後に示すが、レーザのモデルを指定するパラメータがある。)

上記4つ(2つ)のパラメータの合計値は1とならなければいけない。

laser_sigma_hit :
レーザが正しい値を計測するとき、その計測値の分布をガウス分布で表現する。その時の分散の値。

laser_lambda_short :
レーザが地図に無い障害物などにより正しい値よりも低い値を取るとき、その確率の分布は指数関数を表す。(ロボットに近い物体を検出する確率が高く、ロボットから遠い物体を検出する確率は低い。)その指数関数の形状を決定する変数。

laser_likelihood_max_dist :
調査中.

ロボットの移動モデルについてのパラメータ

(x1, y1, yaw1)から(x2, y2, yaw2)へロボットが移動する時、その移動を
その場回転 → 直線移動 → その場回転
という動きをするものとみなし、それぞれの移動量についてそれぞれ誤差が生じるというモデルで考える。詳しくは確率ロボティクスを参照して欲しい。

以下alphaのパラメータは、その回転移動についての誤差と直線移動についての誤差になります。

odom_model_type :
オドメトリのデータタイプ。”diff”, “omni”, “diff-corrected” or “omni-corrected”のタイプがあるらしい。

odom_alpha1 :
ロボットの回転運動によるオドメトリの回転成分のズレ。

odom_alpha2 :
ロボットの回転運動によるオドメトリの並進成分のズレ。

odom_alpha3 :
ロボットの並進運動によるオドメトリの並進成分のズレ。

odom_alpha4 :
ロボットの並進運動によるオドメトリの回転成分のズレ。

odom_frame_id :
オドメトリのデータのトピック名の指定。

base_frame_id :
ロボットの足元のframe。

global_frame_id :
自己位置推定の結果がpublishされるframe。(map)

tf_broadcast :
[bool, default true] global_frameからodom_frameへのtfをpublishするかどうか。

関連事項として、move_baseについての記事も紹介しておきます。
ROSのナビゲーションmove_baseについて理解を深めてみる | MY ROBOTICS

以上、amclのパラメータや動作についてでした。