動的計画法0 フィボナッチ数

つくばチャレンジ本番まであとわずかですが,,,カメラとLidarのフュージョンをするにあたってせっかくなのでSGMとStixelをちゃんと理解しておこうと思い論文を読んでます.

SGMの論文
https://core.ac.uk/download/pdf/11134866.pdf

Stixelの論文
http://www.lelaps.de/papers/badino_dagm09.pdf

が,,,SGMもStixelも動的計画法がキーなんですが,これがいまいち理解できず...このあたり,やっぱり情報系の基礎・基本がなってないんだろうなあと思います.ということで,遠回りではあるんですが,一念発起して今週は動的計画法の勉強をすることにしました.

動的計画法の定義

で,積読してあったアルゴリズムの本を開いて定義から確認です.動的計画法は英語では Dynamic Programming というそうですが,この Programming という単語はプログラミングの意ではなく,【表】を意味するんだそうです.ということで,厳密な意味で言うと動的計画法

"計算結果を表に保存しておき,必要になったらその結果を利用することで計算の効率化を図る方法."

といえるのかなと思います.動的計画法のキモは,”【表】をどうやって作るか?”というところかなと理解しました.この定義だけ書いても意味不明だと思うので,最初の例題としてフィボナッチ数列をやってみます.

フィボナッチ数列

フィボナッチ数列はググればいろんな情報が出てきます.ひとまず,Wikipediaのリンクを載せておきます.
フィボナッチ数 - Wikipedia

で,実際のフィボナッチ数列の定義ですが,下記のようになります.
f:id:rkoichi2001:20180923011357j:plain

こいつをプログラムで求めるとすると,下記のようになります.

def fibonacci_recursive(n):

  print("fibonacci(" + str(n) + ")")

  if (n == 0):
    return 0
  elif (n == 1):
    return 1

  return fibonacci_recursive(n-2) + fibonacci_recursive(n-1) 

この再帰関数は,nが0か1になるまでスタックに関数が積まれていきます.試しにn=6として上記関数を実行すると,下記のように関数がコールされます.

fibonacci(6), fibonacci(4), fibonacci(2), fibonacci(0), fibonacci(1), fibonacci(3), fibonacci(1), fibonacci(2), fibonacci(0), fibonacci(1), fibonacci(5), fibonacci(3), fibonacci(1), fibonacci(2), fibonacci(0), fibonacci(1), fibonacci(4), fibonacci(2), fibonacci(0), fibonacci(1),
fibonacci(3), fibonacci(1), fibonacci(2), fibonacci(0), fibonacci(1)

たかだかフィボナッチ数列の6項目を求めるのために,かなりの回数関数が呼ばれていることがわかります.ひとつの関数が2つの関数を再帰的に呼び出すので,図にすると下記のように2分木になります.
f:id:rkoichi2001:20180923013759j:plain

で,上図を見てもらうとわかるんですが,一度計算している関数が何回も呼ばれていることがわかります.例えば,fib(4)とか.一度計算した値に関しては保存しておいて再利用すれば,関数を呼び出す回数はかなり削減できそうです.

メモ化再帰を用いたフィボナッチ数列の計算

ということで,一度計算済のフィボナッチ数をメモしながらフィボナッチ数を求めます.

def fibonacci_recurcive_memo(n):
  memo = np.zeros(n+1)
  memo.fill(-1)
  return fibonacci_recurcive_memo_int(n, memo)

def fibonacci_recurcive_memo_int(n, memo):

  print("fibonacci(" + str(n) + ")")

  if (memo[n] != -1):
    return memo[n]
  elif (n == 0):
    memo[n] = 0
    return memo[n]
  elif (n == 1):
    memo[n] = 1
    return memo[n]
  else:
    memo[n-2] = fibonacci_recurcive_memo_int(n-2, memo)
    memo[n-1] = fibonacci_recurcive_memo_int(n-1, memo)
    return memo[n-2] + memo[n-1]

同じようにn=6として実行すると,下記のように関数が呼ばれていきます.

fibonacci(6), fibonacci(4), fibonacci(2), fibonacci(0), fibonacci(1), fibonacci(3), fibonacci(1), fibonacci(2), fibonacci(5), fibonacci(3), fibonacci(4)

一度計算した値を保存して再利用することで,関数の呼び出し回数がかなり減りました.再帰呼出しの中で途中結果を保存しておく方法を"メモ化再帰"と呼ぶようです.おそらく厳密な意味ではこれは動的計画法ではなく,”分割統治法・再帰法+メモ化再帰”なのだと思います.ただ,メモ化再帰ができるということは,表(=メモ)を作ることができます.

動的計画法を用いたフィボナッチ数列の計算

やっとこさ本題の動的計画法です.最初に説明したように,動的計画法で問題を解くには,”表”を作る必要があります.この問題の場合は,fib(0), fib(1), fib(2), ...というようにそれぞれのフィボナッチ数を表として作成する必要があります.で,ここでポイントなんですが,フィボナッチ数を求める際に,値が定義されているのは n=0とn=1だけです.あとは漸化式を解いて求める必要があります.つまり,表を計算するときにも,nが小さい方から計算していく必要があります.動的計画法(=表作成)は,この例のようにボトムから解ける問題に対して応用可能な技法になります.基本的には,問題を漸化式の形に落とし込めることができれば,動的計画法で問題が解ける可能性が大ということかと思います.

動的計画法(表化)を用いたフィボナッチ数の計算

下記を見ていただくとわかるように,関数の再帰呼出しで実現していた部分がforループに置き換わっています.これができるのは,フィボナッチ数列の漸化式がはっきりしているからです.

def fibonacci_recurrence_formula(n):

  memo = np.zeros(n)
  memo[0] = 0
  memo[1] = 1

  for i in range(2, n):
    memo[i] = memo[i-2] + memo[i-1]

  return memo[n-2] + memo[n-1]

ということで,動的計画法パート1でした.まだしばらく動的計画法をやります...

初自動走行

本日9/15日,第三回試走会の日でしたが,,,,あいにくの雨だったので参加をあきらめました.ロボットの防水がしっかりできておらず,壊れてしまうとつらいので...結局次回の試走会が10/13ということで,だいぶ後になってしまうのですが,確認走行のトライはそこまでおあずけです.試走会はあと五回...だんだん追い込まれてきました.ただ,当初の目標だった下記6項目は完了しました.

1. 確認走行コースのデータ取得:完了
2. 確認走行コースのマップ作成:完了
3. AMCLを使っての自己位置推定:完了
4. Yp-Spurを使ってロボット制御実施.制御パラメータ調整:完了
5. Teb-Local Planner と Yp-Spur を使ってロボット制御実施:完了
6. move_base をつかって,センシングしながらのロボット制御を実施:完了

コース(リビング?)の写真です.段ボールの真ん中を目指して,障害物をよけながら走ります.
f:id:rkoichi2001:20180916055930j:plain

youtu.be

ちょっとまだ Local Planner が使いこなせてない感じでロボットの軌跡がふらふらしてますが,ここはもう少しチューニングが必要です.ただ,Lidar + ROS の威力はすさまじいですね.ハードを作ればあれよあれよというまに自動走行まで来てしまいました.ここからは,カメラとLidarのFusionに取り組んでいきます!

ROS Teb Local Planner を使った位置制御

ということで第三回試走会前最後の週末,割と頑張りましたが ROS Navigation Stack の理解で思ったより時間がかかってしまい,,,自宅の地図を作ってセンシングしながらの自律走行まで完了させたかったんですが,Teb Local Planner を使ってオドメトリだけを見ながら移動するとこまでで終わってしまいました.金曜までに何とか6まで行ければいいのですが...

1. 確認走行コースのデータ取得:完了
2. 確認走行コースのマップ作成:完了
3. AMCLを使っての自己位置推定:完了
4. Yp-Spurを使ってロボット制御実施.制御パラメータ調整:完了
5. Teb-Local Planner と Yp-Spur を使ってロボット制御実施:完了
6. move_base をつかって,センシングしながらのロボット制御を実施:これから

下記,PS3のコントローラでロボットが動くようになりました.
www.youtube.com

下記,Teb Local Planner で指定した4点を順々に移動していきます.差動式のロボットだと位置を変えずに回転できるので,車型のロボットよりだいぶ有利ですね.
www.youtube.com

Teb Local Planner と Yp-Spurの制御がうまくかみ合わず,パラメータ調整にも結構時間がかかりました.部屋の中だけで動かしている状態なので,また外に持っていくといろいろと調整をしなくてはいけないんだろうなと思います.

Yp-Spur を使ったロボット制御

ロボットが,,,,走ったーーーー(∩´∀`)∩

9/15日の確認走行完了に向けて準備をしてます.今日明日で,ROSの基本パッケージを使って自動走行できるとこまでもっていかないといけないんですが,やらないといけないことは....

1. 確認走行コースのデータ取得:完了
2. 確認走行コースのマップ作成:完了
3. AMCLを使っての自己位置推定:完了
4. Yp-Spurを使ってロボット制御実施.制御パラメータ調整:<- さっき完了!
5. Teb-Local Planner と Yp-Spur を使ってロボット制御実施:これから
6. move_base をつかって,センシングしながらのロボット制御を実施:これから

Yp-Spur を使った制御ですが,推奨パラメータを使ってテストすると大きな音が鳴って全然動かず...焦りました.結果的には比例ゲインが大きすぎて不安定になっているだけでした.T-Frog の方が残してくれている下記の Wiki を見ながらパラメータ調整すれば,すんなり動くようになりました.

パラメータファイル [Robot Platform Project Wiki]

さすが研究者の方が設計してくれただけあって,動き方の安定感が半端ないですね.

youtu.be

ROS amcl を使った自己位置推定@つくば市役所

ということで,gmapping を使ったマップ生成が完了したので,amcl を用いて自己位置推定をしてみました.一応マップを作成したデータとは異なるデータなので,ここでうまく自己位置推定ができていれば,ある程度再現性はあるかと思います.

www.youtube.com

結果として,,,,ROSはすごいですね.一昨年・昨年と散々苦しんだ自己位置推定ですが,呆気無く完了しました.実際に会場でやるときは周辺環境の動的変化もあるのでここまでうまくは行かないと思うのですが,ひとまず手持ちのデータに関しては amcl を使って Localization 完了です.ということで,明日からは move_baseやplanner に入っていきます.

ROS slam_gmapping を使ったマップ作成@つくば市役所

オドメトリ作成を終え,ようやく gmapping を使うとこまで来ました!Lidarの精度はやっぱり強烈ですね...もともとオドメトリの精度がそこそこ出ていることもあって,かなり綺麗なマップができました.赤のラインはオドメトリラインです.

f:id:rkoichi2001:20180902175054p:plain

bag ファイルを再生後,下記のコマンドで pgm ファイルを生成します.

rosrun map_server map_saver

結果,生成されたマップが下記になります.少しゴミが入ってしまっているので,ここは手作業でクリーニングが必要ですかね.

f:id:rkoichi2001:20180902175920p:plain

ステップ3ではこのマップを使って,ロボットの自己位置推定をします.

複数の Input と ROS message_filter

前回のエントリでオドメトリの話をしましたが,自分のオドメトリを生成するために必要な Input は下記になります.

Input :

1. sensor_msgs/JointState.h : i-Cart Middleのモータドライバからの出力
2. kmsgs/Imu.h : 自作 IMU からのヨーレート出力

ここで,1の入力が5ms,2の入力が10msなんですが,普通にROSのcallbackを使うと1の入力が5ms,2の入力が10msごとに届いてその都度コールバックが呼ばれる形になると思います.オドメトリを計算するときには,バッファに対してロックをかけてヨーレート,車輪の回転角度をコピーしないといけないですが,このやり方だとInputの数が増えるに従ってロックをかける数が多くなってしまいます.最初はこの方法でやろうとしていたんですが,どうもロックのオーバーヘッドが大きすぎて,計算が回りきってないようだったので,message_filterを使いました.(自分のロックのかけ方が下手糞だったこともあります.(笑))

オドメトリモジュールの In/Out

f:id:rkoichi2001:20180901235217j:plain

いちいちLockをしてバッファコピーしていた時のオドメトリ結果(0.5倍速再生)

f:id:rkoichi2001:20180901233004p:plain

いちいちLockをしてバッファコピーしていた時のオドメトリ結果(3倍速再生)

f:id:rkoichi2001:20180901233108p:plain

上記の写真を比較するとわかると思うんですが,3倍速再生だとヨーレートの積分計算が十分に間に合ってなく,コーナーを迎えるたびに実際のヨー角と計算上のヨー角の開きが大きくなってしまいます.

Lockフリーオドメトリ作成

ということで,ROSのノード内でLockフリーにするにはどうすればよいか検討しました.調べてみた結果,message_filterを使えば所望の実装が簡単にできることがわかりました.
http://wiki.ros.org/message_filters

で,今回の自分の場合,最終的にオドメトリ計算に使用される入力の組み合わせイメージは下記のようになります.
f:id:rkoichi2001:20180902000133j:plain

結局 JointState が 5ms 毎に届いているのに,二回に一回は捨ててしまっていることになりますが,10ms毎に計算できれば十分かと.
結果的に3倍速で実行しても何とか回りきるようになりました.ちょっとヨー角が出すぎてしまってますが,,,まあ,こんなもんでしょう.
f:id:rkoichi2001:20180902000849p:plain

最終的な実装のイメージは下記のようになりました.

//
#include <sys/time.h>
#include <cstddef>

//
#include <nodelet/nodelet.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/JointState.h>
#include <string>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

//
#include "ros/ros.h"
#include "odometry.h"

// 3rd party include
#include "kmsgs/Imu.h"

namespace koichi_robotics_lib {

class OdometryNodelet : public nodelet::Nodelet {

public:

  OdometryNodelet();

  ~OdometryNodelet();

  virtual void onInit();

private:

  void publishTransform(const nav_msgs::Odometry &odomMsg, std::string frameName);

  void sensorCallback(const kmsgs::ImuConstPtr &imuMsg, const sensor_msgs::JointStateConstPtr &jointPos);

  void calcAndPublishOdom(const ros::Time &stamp);

  void readParams(OdomParams &odomParam);

  void readParam(std::string paramName, bool result);

  void parseJointStatesMsg(const sensor_msgs::JointStateConstPtr &jointPos, 
    double &rRad, double &lRad, double &rVel, double &lVel);

  void fillOdomMessage(nav_msgs::Odometry &odomMsg, std::string frameName, const ros::Time &stamp, 
    const OdomPos &odomPos, const OdomVel &odomVel);

private:

  // Topic Name
  std::string odomImuFrameName, odomWhlFrameName, baselinkFrameName;

  int maxSyncDiff;

  ros::NodeHandle nh, nh_ns;
  nav_msgs::Odometry curOdom;

  // ROS Subsceibers
  message_filters::Subscriber<kmsgs::Imu> *subImu;
  message_filters::Subscriber<sensor_msgs::JointState> *subWhl;

  // ROS Sync Policy
  typedef message_filters::sync_policies::ApproximateTime<
    kmsgs::Imu, sensor_msgs::JointState
  > OdomSyncPolicy;
  message_filters::Synchronizer<OdomSyncPolicy> *odomSync;

  // ROS Publisher
  ros::Publisher odomPubWhl, odomPubImu;
  tf::TransformBroadcaster *odomBroadCaster;

  // Class
  Odometry odometry;
};

}

using namespace std;
using namespace koichi_robotics_lib;

OdometryNodelet::OdometryNodelet() {
}

OdometryNodelet::~OdometryNodelet() {
  odometry.Finalize();
  delete this->odomBroadCaster;
  delete this->subImu;
  delete this->subWhl;
  delete this->odomSync;
}

void OdometryNodelet::onInit() {
  NODELET_DEBUG("Initializing Odometry Nodelet....");

  nh = getMTNodeHandle();
  nh_ns = getMTPrivateNodeHandle();
  odomBroadCaster = new tf::TransformBroadcaster();

  OdomParams params;
  readParams(params);
  odometry.Initialize(params);

  subImu = new message_filters::Subscriber<kmsgs::Imu>(nh, "imu", 100);
  subWhl = new message_filters::Subscriber<sensor_msgs::JointState>(nh, "joint_states", 100);
  
  odomSync = new message_filters::Synchronizer<OdomSyncPolicy>(OdomSyncPolicy(maxSyncDiff), *subImu, *subWhl);
  odomSync->registerCallback(boost::bind(&OdometryNodelet::sensorCallback, this, _1, _2));

  odomPubWhl = nh.advertise<nav_msgs::Odometry>("odom_whl", 100);
  odomPubImu = nh.advertise<nav_msgs::Odometry>("odom_imu", 100);
}

void OdometryNodelet::readParams(OdomParams &odomParams) {

  // Topic Name
  readParam("odometry_imu_frame_name", nh.param<std::string>("odometry_imu_frame_name", odomImuFrameName, ""));
  readParam("odometry_whl_frame_name", nh.param<std::string>("odometry_whl_frame_name", odomWhlFrameName, ""));
  readParam("base_link_frame_name", nh.param<std::string>("base_link_frame_name", baselinkFrameName, ""));

  readParam("gyro_1bit_in_rad", nh.param<double>("gyro_1bit_in_rad", odomParams.gyro1BitInRad, 0.0));

  double r_whl_dia, l_whl_dia;
  readParam("l_whl_diameter", nh.param<double>("l_whl_diameter", l_whl_dia, 0.0));
  odomParams.leftDistPerRad = l_whl_dia / 2.0;

  readParam("r_whl_diameter", nh.param<double>("r_whl_diameter", r_whl_dia, 0.0));
  odomParams.rightDistPerRad = r_whl_dia / 2.0;

  readParam("odometry/standstill_thresh", nh.param<double>("odometry/standstill_thresh", odomParams.standstillThresh, 0.0));
  readParam("wheel_base", nh.param<double>("wheel_base", odomParams.wheelBase, 1.0));
  readParam("odometry/use_left_pls", nh.param<bool>("odometry/use_left_pls", odomParams.useLeftPls, false));
  readParam("odometry/use_right_pls", nh.param<bool>("odometry/use_right_pls", odomParams.useRightPls, false));
  readParam("cycle_time", nh_ns.param<double>("cycle_time", odomParams.cycleTime, 0.0));
  readParam("max_sync_diff", nh_ns.param<int>("max_sync_diff", maxSyncDiff, 8));

  // Buffer Size
  readParam("imu_yaw_buffer", nh_ns.param<int>("imu_yaw_buffer", odomParams.imuYawBufferSize, 0.0));
  readParam("whl_yaw_buffer", nh_ns.param<int>("whl_yaw_buffer", odomParams.whlYawBufferSize, 0.0));
  readParam("dist_buffer", nh_ns.param<int>("dist_buffer", odomParams.distBufferSize, 0));

}

void OdometryNodelet::readParam(string paramName, bool result) {
  if (!result) {
    ROS_FATAL("Param read failure. : %s", paramName.c_str());
  }
}

void OdometryNodelet::publishTransform(const nav_msgs::Odometry &odomMsg, std::string odomFrameName) {

  tf::Transform odomTrans;
  tf::Pose tfPose;

  tf::poseMsgToTF(odomMsg.pose.pose, odomTrans);
  odomBroadCaster->sendTransform(tf::StampedTransform(odomTrans, ros::Time::now(), odomFrameName, baselinkFrameName));
}

void OdometryNodelet::sensorCallback(const kmsgs::ImuConstPtr &imuMsg, const sensor_msgs::JointStateConstPtr &jointPos) {

  double rRad, lRad, rVel, lVel;
  parseJointStatesMsg(jointPos, rRad, lRad, rVel, lVel);
  odometry.SetCurrentPosition(rRad, lRad, rVel, lVel);
  odometry.SetCurrentGyroZ(imuMsg->omegaZ);
  calcAndPublishOdom(imuMsg->header.stamp);
}

void OdometryNodelet::parseJointStatesMsg(const sensor_msgs::JointStateConstPtr &jointPos, double &rRad, double &lRad, double &rVel, double &lVel) {

  for (int i = 0; i < jointPos->name.size(); i++) {
    
    string name = jointPos->name[i];
    double jntPos = jointPos->position[i];
    double jntVel = jointPos->velocity[i];

    if (name == "right") {
      rRad = -jntPos;
      rVel = -jntVel;
    } else if (name == "left") {
      lRad = jntPos;
      lVel = jntVel;
    } else {
      ROS_ERROR("Unexpected Joint Name : %s", name);
    }
  }
}

void OdometryNodelet::calcAndPublishOdom(const ros::Time &stamp) {

  static double lastStamp = stamp.toSec();
  double newStamp = stamp.toSec();
  double dT = newStamp - lastStamp;
  lastStamp = newStamp;

  OdomPos odomWhl, odomImu;
  OdomVel velWhl, velImu;
  odometry.CalculateOdometry(odomWhl, odomImu, velWhl, velImu, dT);

  // Published IMU based odometry.
  {
    nav_msgs::Odometry odomImuMsg;
    fillOdomMessage(odomImuMsg, odomImuFrameName, stamp, odomImu, velImu);
    publishTransform(odomImuMsg, odomImuFrameName);
    odomPubImu.publish(odomImuMsg);
  }

  // Published Whl Based Odometry.
  {
    nav_msgs::Odometry odomWhlMsg;
    fillOdomMessage(odomWhlMsg, odomWhlFrameName, stamp, odomWhl, velWhl);
    publishTransform(odomWhlMsg, odomWhlFrameName);
    odomPubWhl.publish(odomWhlMsg);
  }

}

void OdometryNodelet::fillOdomMessage(nav_msgs::Odometry &odomMsg, std::string odomFrameName,
    const ros::Time &stamp, const OdomPos &odomPos, const OdomVel &odomVel) {

  // Header
  odomMsg.header.frame_id = odomFrameName;
  odomMsg.header.stamp = stamp;

  // Pose
  odomMsg.pose.pose.position.x = odomPos.x;
  odomMsg.pose.pose.position.y = odomPos.y;
  odomMsg.pose.pose.position.z = 0.0;
  odomMsg.pose.pose.orientation = tf::createQuaternionMsgFromYaw(odomPos.angz);

  odomMsg.pose.covariance[0] = odomPos.totDist;
  // Twist
  odomMsg.twist.twist.linear.x = odomVel.vx;
  odomMsg.twist.twist.linear.y = odomVel.vy;
  odomMsg.twist.twist.linear.z = 0;
  odomMsg.twist.twist.angular.x = 0;
  odomMsg.twist.twist.angular.y = 0;
  odomMsg.twist.twist.angular.z = odomVel.wz;

}

#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(koichi_robotics_lib::OdometryNodelet, nodelet::Nodelet)