Localization / Self Localization 6

膨張リセットをnav2に入れます。

EMCL2

日本でROS2を動かしてみたりrobotに興味がある人なら知っている上田隆一さんがご自身の論文の実装をgithubで公開してくれています。

emcl2

真面目にamclの代替ROSパッケージを作った

自分も、nav2のAMCLは、LGPLであることもあり、変数名が好きでない(率直に言うとわかりにくい)し、ご指摘の通りと思いますので、書き直した方が良いと思っていますが、かなり大変なので入れ込むことにします。

走行していなくてもLiDARのsensor値でparticle filterを更新する

下記でやってます。

Localization / Self Localization 2

膨張リセット

自分がまだemcl2の全部を読んでいないこともあり、すぐに出来る範囲の実装のみをします。動作確認しましたが、EMCL2より弱いです。強くしたら別の記事で書きます。
そして丸ごと使わせていただきます。

下記は重要です。

fprintf( stderr, "total: %f\n", total );

ここではtotalが4未満でresetとしてますが、turtlebot3のworldだからだと思います。
動かす場所によって変えるべきの値と思います。そのときに実際に走行させた時のtotalの値のlogが重要になります。大体は4よりも小さい値になると思います。

pf.c

// Update the filter with some new sensor observation
void pf_update_sensor(pf_t * pf, pf_sensor_model_fn_t sensor_fn, void * sensor_data)
{
  int i;
  pf_sample_set_t * set;
  pf_sample_t * sample;
  double total;

  set = pf->sets + pf->current_set;

  // Compute the sample weights
  total = (*sensor_fn)(sensor_data, set);
  fprintf( stderr, "total: %f\n", total );

  // -----------------------------------------------------------------------------
  // expantion reset

  if ( total < 4.0 )
  {
    fprintf( stderr, "=========== RESET !!! ==========\n" );
    for (i = 0; i < set->sample_count; i++)
    {
      double expansion_radius_position = 0.1;
      double expansion_radius_orientation = 0.2;
      double length = 2*((double)rand()/RAND_MAX - 0.5) * expansion_radius_position;
      double direction = 2*((double)rand()/RAND_MAX - 0.5) * M_PI;

      sample = set->samples + i;

      sample->pose.v[0] += length*cos(direction);
      sample->pose.v[1] += length*sin(direction);
      sample->pose.v[2] += 2*((double)rand()/RAND_MAX - 0.5) * expansion_radius_orientation;

      sample->weight = 1.0/set->sample_count;
      /* sample->weight = sample->weight * 0.8; */
    }
    total = (*sensor_fn)(sensor_data, set);
  }

  // -----------------------------------------------------------------------------

  if (total > 0.0) {
    // Normalize weights
    double w_avg = 0.0;
    /* for (i = 0; i < set->sample_count; i++) { */
    /*   sample = set->samples + i; */
    /*   w_avg += sample->weight; */
    /*   sample->weight /= total; */
    /* } */

    // Update running averages of likelihood of samples (Prob Rob p258)
    w_avg /= set->sample_count;
    if (pf->w_slow == 0.0) {
      pf->w_slow = w_avg;
    } else {
      pf->w_slow += pf->alpha_slow * (w_avg - pf->w_slow);
    }
    if (pf->w_fast == 0.0) {
      pf->w_fast = w_avg;
    } else {
      pf->w_fast += pf->alpha_fast * (w_avg - pf->w_fast);
    }
  } else {
    // Handle zero total
    for (i = 0; i < set->sample_count; i++) {
      sample = set->samples + i;
      sample->weight = 1.0 / set->sample_count;
    }
  }
}

膨張リセットは詳解確率ロボティクスに書かれてますので説明しません。実装も丸ごと盗んでます。
蛇足ですがAMCLも詳解確率ロボティクスで書かれてます。

EMCL2

走行していなくてもLiDARのsensor値でparticle filterを更新する

膨張リセット

広告