Localization / Self Localization 2

自己位置推定について説明します。motion modelの説明をします。

原著の英語の「Probabilistic Robotics」を買っておきましょう。
英語が苦手な人でもGoogle翻訳で戦えます。Google翻訳で厳しい時のみdeeplで戦えば、無料で戦い続けられると思います。

Probabilistic Roboticsの自己位置推定は2つのmodelを使います。
原理として必要なmodelです。なので、他の自己位置推定でも2つのmodelを使うと思います。

• motion model
• measurement model

Probabilistic Roboticsのmotion modelは2種類が定義されています。

• velocity motion model
• odometry motion model

ここではodometry motion modelの説明をします。

Bayes Filterの説明は後で別の記事でやります。ここではsource codeで理解します。
原著の英語の「Probabilistic Robotics」のodometry motion modelは読んでおいた方が理解は早いです。

AMCL 1 (motion modelのみの説明)

概要は説明しましたので、ここからはAMCLの実装を理解していくことにします。

下記の記事の設定は完了しているとします。

M2 MacBookでROS2を動かす

$ sudo apt install ros-humble-navigation2
$ sudo apt install ros-humble-nav2-bringup

理解のためAMCLを書き換えるのでbuildの確認をしておきます。

$ cd
$ mkdir ros
$ cd ros
$ mkdir robot_ws
$ cd robot_ws
$ mkdir src
$ cd src
$ git clone https://github.com/ros-navigation/navigation2.git
$ cd navigation2
$ git branch -a
$ git checkout humble
$ git branch -a
$ cd
$ cd ros/robot_ws
$ mkdir log2
$ colcon build --packages-select nav2_msgs 2>&1 | tee log2/colcon.log
$ colcon build --packages-select nav2_amcl 2>&1 | tee log2/colcon.log

以前はこちらでした。

$ git clone https://github.com/ros-planning/navigation2.git

.bashrcに. ~/ros/robot_ws/install/setup.bashが書かれているとして。
初めてnav2_amclをcolcon buildした後のみ必要です。

$ . ~/.bashrc

AMCL 2 (motion modelのみの説明)

移動します。

$ cd navigation2/nav2_amcl

ctagsしておきます。

$ ctags -R

$ vi src/amcl_node.cpp

main.cppはnodeを作っているだけなので読まなくて大丈夫です。

AMCL 3 (motion modelのみの説明)

下記を理解します。

src/amcl_node.cpp

/scan topicを受信したらlaserReceived()関数が呼び出されます。

motion modelのみの説明なので最初にgetOdomPose()関数を読みます。

commentにあるように「scanを受け取ったとき、robotはどこに居たか」です。

tf_bufferからframe_idで検索して引っ張り出して来ているだけです。

下記のように姿勢をfprintfして確認します。
ROS2はprintfだと出力されません。stdoutが表示されないようになっているようです。
これで自作log関数などが作りやすいです。macroでもclassでも。

fprintf( stderr, "getOdomPose: (%f, %f, %f)\n", pose.v[0], pose.v[1], pose.v[2] );

AMCL 4 (motion modelのみの説明)

shouldUpdateFilter()関数を読みます。
自分が読みやすいように書き換えています。

amcl_node.cpp (original)

/// p : pose
/// d : delta
bool AmclNode::shouldUpdateFilter(const pf_vector_t p, pf_vector_t & d)
{
  d.v[0] = p.v[0] - pf_odom_pose_.v[0];
  d.v[1] = p.v[1] - pf_odom_pose_.v[1];
  d.v[2] = angleutils::angle_diff(p.v[2], pf_odom_pose_.v[2]);

  double dt = d_thresh_;
  double at = a_thresh_;
  // See if we should update the filter
  bool update = fabs(d.v[0]) > dt || fabs(d.v[1]) > dt || fabs(d.v[2]) > at;
  update = update || force_update_;
  return update;
}

pf_odom_pose_にはupdateFilter()関数が呼ばれたときだけ、そのときのposeで更新されます。
poseの方が新しくpf_odom_pose_の方が古いです。poseとpf_odom_pose_の値が同じときもあります。
必ず1つ古いというわけではなくて2つも3つも古いということもあり得ます。

1つでも閾値を超えていたら関数を抜けてlasers_update_[i]がtrueになってmotion modelのupdateとmeasurement modelのupdateが行われます。

移動がなくても自己位置推定が更新されるように変更します。

amcl_node.cpp (update every time)

/// p : pose
/// d : delta
bool AmclNode::shouldUpdateFilter(const pf_vector_t p, pf_vector_t & d)
{
  d.v[0] = p.v[0] - pf_odom_pose_.v[0];
  d.v[1] = p.v[1] - pf_odom_pose_.v[1];
  d.v[2] = angleutils::angle_diff(p.v[2], pf_odom_pose_.v[2]);

  // double dt = d_thresh_;
  // double at = a_thresh_;
  // See if we should update the filter
  // bool update = fabs(d.v[0]) > dt || fabs(d.v[1]) > dt || fabs(d.v[2]) > at;
  // update = update || force_update_;
  // return update;
  return true;
}

deltaの計算は必須です。関数呼び出しにするほどでもないと思いますので、laserReceived()関数に3行を書いてshouldUpdateFilter()関数は削除してしまいます。
動作確認済みです。ずれたinitialposeをpublishしても修正してくれるようになります。

motion modelの説明の記事に載せる動画ではない気がしますが、ここに掲載します。
measurement modelの方が説明すること満載なので、ごちゃごちゃさせたくない気持ちがあります。

before. original.

after.

緑の小さい点1つがparticle filter 1個です。←ROS2に慣れていない人向けの説明。

AMCL 5 (motion modelのみの説明)

odometryUpdate()関数を読みます。
自分が読みやすいように書き換えています。logicは変更してません。原型は留めてません。

下記のどちらかを読みます。

src/motion_model/differential_motion_model.cpp
src/motion_model/omni_motion_model.cpp

ほとんどの人はomni robotではなくdifferential robotに関心があると思いますのでdifferential_motion_model.cppを読みます。
differential_motion_model.cppはvelocity motion modelでなくodometry motion modelで実装されています。

// Implement sample_motion_odometry (Prob Rob p 136)
/// d : delta
/// h : hat
/// at2 : atan2
/// op : old pose
/// an_diff : angle diff
/// pf_gauss : particle filter random gaussian
/// a1 : alpha1
using std::min;
void
DifferentialMotionModel::odometryUpdate(
  pf_t * pf, const pf_vector_t & pose,
  const pf_vector_t & d)
{
  pf_sample_set_t * set;
  set = pf->sets + pf->current_set;

  pf_vector_t old_pose = pf_vector_sub(pose, d);

  double d_rot1, d_trans, d_rot2;
  double d_rot1_h, d_trans_h, d_rot2_h;
  double d_rot1_abs, d_rot2_abs;

  double a1 = alpha1_, a2 = alpha2_, a3 = alpha3_ , a4 = alpha4_;

  d_trans = sqrt( d.v[0] * d.v[0] + d.v[1] * d.v[1] );

  if ( 0.01 <= d_trans )
  { d_rot1 = an_diff( atan2(d.v[1], d.v[0]), old_pose.v[2] ); }
  else
  { d_rot1 = 0.0; }

  d_rot2 = an_diff( d.v[2], d_rot1 );

  d_rot1_abs = min( fabs(an_diff(d_rot1, 0.0)), fabs(an_diff(d_rot1, M_PI)) );
  d_rot2_abs = min( fabs(an_diff(d_rot2, 0.0)), fabs(an_diff(d_rot2, M_PI)) );

  /// update all particle filter's pose
  for (int i = 0; i < set->sample_count; i++) {
    pf_sample_t * sample = set->samples + i;

    d_rot1_h = an_diff( d_rot1, pf_gauss( sqrt( a1 * d_rot1_abs + a2 * d_trans)) );
    d_trans_h = d_trans - pf_gauss( sqrt( a3 * d_trans + a4 * d_rot1_abs + a4 * d_rot2_abs) );
    d_rot2_h = an_diff( d_rot2, pf_gauss( sqrt( a1 * d_rot2_abs + a2 * d_trans)) );

    sample->pose.v[0] += d_trans_h * cos( sample->pose.v[2] + d_rot1_h );
    sample->pose.v[1] += d_trans_h * sin( sample->pose.v[2] + d_rot1_h );
    sample->pose.v[2] += d_rot1_h + d_rot2_h;
  }
}

  d_trans = sqrt( d.v[0] * d.v[0] + d.v[1] * d.v[1] );

ピタゴラスの定理そのもの。dはdelta。

d.v[0] : x’-x
d.v[1] : y’-y

ピタゴラスの定理より。
sqrt((x’-x)^2 + (y’-y)^2)

  if ( 0.01 <= d_trans )
  { d_rot1 = an_diff( atan2(d.v[1], d.v[0]), old_pose.v[2] ); }
  else
  { d_rot1 = 0.0; }

0.01(m) = 1(cm)

1cm以上移動しているときはinitial turnを計算。odometry motion modelの定義はProbabilistic Roboticsを読んでください。
1cm以上移動していない場合はinitial turnをゼロとする。

  d_rot2 = an_diff( d.v[2], d_rot1 );

final turn.
向きの変化量からinitial turnを引く。

  d_rot1_abs = min( fabs(an_diff(d_rot1, 0.0)), fabs(an_diff(d_rot1, M_PI)) );
  d_rot2_abs = min( fabs(an_diff(d_rot2, 0.0)), fabs(an_diff(d_rot2, M_PI)) );

絶対値の取得。
commentにはforwardとbackwardを統一的に扱うための処理と書いてある。

    pf_sample_t * sample = set->samples + i;

particle filterを1つ取り出しておきます。

    d_rot1_h = an_diff( d_rot1, pf_gauss( sqrt(a1 * d_rot1_abs + a2 * d_trans) ) );
    d_trans_h = d_trans - pf_gauss( sqrt(a3 * d_trans + a4 * d_rot1_abs + a4 * d_rot2_abs) );
    d_rot2_h = an_diff( d_rot2, pf_gauss( sqrt(a1 * d_rot2_abs + a2 * d_trans) ) );

nav2のprogramはProbabilistic Roboticsの通りではありません。このprogramはProbabilistic Roboticsの通りのつもりです。(あ、logic変えてました。)
pf_gaussへのinputはsigmaです。つまり標準偏差。
a1 * d_rot1_abs + a2 * d_transが分散です。Probabilistic Roboticsを読んでください。
sqrt(a1 * d_rot1_abs + a2 * d_trans)で標準偏差。
pf_gauss( sqrt(a1 * d_rot1_abs + a2 * d_trans) )が誤差。

alpha1とalpha2が回転に対するerror parameter。誤差を操る。つまり分布の広がりを操る。
alpha3とalpha4がlinear velocityでの移動に対するerror parameter。誤差を操る。つまり分布の広がりを操る。

alpha5はomni_motion_model.cppの方に出てきます。

    sample->pose.v[0] += d_trans_h * cos( sample->pose.v[2] + d_rot1_h );
    sample->pose.v[1] += d_trans_h * sin( sample->pose.v[2] + d_rot1_h );
    sample->pose.v[2] += d_rot1_h + d_rot2_h;

三角関数を使った当たり前の処理。
particle filterを進んだ分だけ誤差付きで移動させています。pf_gaussの中でrand()を使っているのでparticle filterは全て誤差が違います。

まとめ

上記の通りmotion modelは簡単です。

広告

IT開発関連書とビジネス書が豊富な翔泳社の通販『SEshop』
さくらのレンタルサーバ
ムームードメイン
Ｏｉｓｉｘ（おいしっくす）
らでぃっしゅぼーや
珈琲きゃろっと
エプソムソルト

« Localization / Self Localization 1     Localization / Self Localization 3 »