ロボットトレーニング
会社で「自分もロボットやりたいっす！」って言ってくれた子がいまして，どうやって進めたらいいかな〜と悩み中です．とりあえず応募開始の４月頭まであと３ヶ月弱ありますが，ここまでにどんなもんか感触をつかめるとこまで持って行こうとしたら，，，下記の項目くらいができれば万々歳なのかなと．
０．環境設定
とりあえず，まずはここが終わらないと始まりませんが，なれないうちはこのあたりが本当に鬼門なんですよね．．．．
０．Ubuntu1604のインストール
１．GCC とか CMake とか使って，"Hello World!" 出力するとこまでやってみる．
２．エディタ（VSCODE）の導入．
１．ROSを使ってみる．
次に，ROSの一通りの把握ですかね．
０．ROS Kinetic のインストール
１．チュートリアルを実施する．
２．入門本を一冊読みきる．
- 作者: 表允〓,鄭黎〓,倉爪亮
- 出版社/メーカー: オーム社
- 発売日: 2018/03/16
- メディア: 単行本
- この商品を含むブログ (1件) を見る
２．プログラミングに慣れる．
最終的にはコード書いて作んないと面白くないと思うので，，，プログラミングの勉強が必要かと．
０．C++ の入門本を一冊読みきる．
- 作者: 高橋麻奈
- 出版社/メーカー: SBクリエイティブ
- 発売日: 2017/06/14
- メディア: 単行本
- この商品を含むブログを見る
標準講座C++―基礎からSTLを利用したプログラミングまで (Programmer’s SELECTION)
- 作者: ハーバートシルト,Herbert Schildt,柏原正三
- 出版社/メーカー: 翔泳社
- 発売日: 1999/03
- メディア: 単行本
- 購入: 1人 クリック: 2回
- この商品を含むブログ (2件) を見る
１．Github に写行コード（笑）をコミット．
３．ロボット（座学）
４．ロボット（実験）
．．．これはちょっと考えます．
Fast SLAM with OGM ~ パーティクルフィルタを用いた SLAM （実験編）
確率ロボティクス１３章の内容です．ついにここまできました(*ﾉωﾉ)
目的
前回の Monte Carlo Localization (下記リンクのエントリ) は，環境の地図をすでに持っている前提で，自分が地図のどこにいるか？を求める問題でした．で，ここで一つ疑問が出てきます．
daily-tech.hatenablog.com
「環境の地図をすでに持っているのはええけど，じゃあ，環境の地図はどうやって作るの？」
で，この問題を解くために使う方法論が SLAM になります．「環境の地図を作る」という行為がなんで難しいかというと，
１．正確な地図を作るためには，自分の位置が正確にわかっていないといけない．
２．自分の位置を正確に知るためには，地図がないといけない．
ということで，卵が先か，鶏が先か問題にぶち当たってしまいます．で，いろんな SLAM アルゴリズムがありますが，ベイズの理論をベースとしている SLAM アルゴリズムは結局下記の前提をもとに問題を解決しています．
１．オドメトリは距離の短い二点間の移動に対しては比較的正確だけど，距離が長くなってくると誤差が積みあがって使い物にならない．
２．計測（Lidar）は精度がとても高い．
上記の前提を使って，下記のフローで問題を解決します．
１．地点 xt-1 -> xt への移動（比較的短距離）を知るために，オドメトリを使う．
２．オドメトリから求めた xt の位置は精度がそれほど高くないので，現時点で最新（作りかけ）の MAP と計測を使って xt を微調整して精度を上げる．
３．微調整した xt をもとに，MAP を更新し，１に戻る．
オフライン SLAM の場合なんかは，一通りデータを取得した後に最適化計算を回したりするので，またちょっと流れが変わってくると思うのですが，これは２月にやってみます．
実験結果
問題設定のイメージですが，前回 Monte Carlo Localization で使った仮想環境と同じものを使います．上図がSLAMの対象となる環境で，下図が実際にロボットが通った真値のパスです．
で，この実験の前提としては，事前の地図を持っておらず，ロボットはノイズの乗っているオドメトリ情報と 2D Lidar の計測結果を使ってこの環境の地図を作ります．実験の手順としては，
１．真値のパスに沿ってロボットを走行させます．が，ロボット自身が取得できるオドメトリデータには白色ノイズを載せてます．（＝実際には真値のパスを通っているが，ロボットはその正確な情報を取得できない状況．）
２．0.25mロボットが進む毎にロボットに搭載されている 2D Lidar からのデータを取得できます．（＝2D Lidar の計測値は実際のパスからの計測値．だけど，若干誤差が乗っている．）
３．2D Lidar から取得したデータと，自分の推定位置と作りかけのMAPから得られると想定される 2D Lidar の値を比較します．
４．比較したスコア順にパーティクルに重みをつけ，重み比例する形でパーティクルをリサンプリング．
で，実験結果が下記のようになります．
youtu.be
ちょっと角度のずれが発生してしまって，その影響で地図が一部乱れてしまってます．パラメータをちょっと変えて実行するとうまくいくかもしれなかったんですが，実行にすごく時間がかかるので，あきらめました．．．
理論とか式展開とか．．．．
ほかのエントリと同じく，，，また時間ができたらアップします．ROS でデフォルトの SLAM アルゴリズムとして使われている GMapping はこのFAST Slam に少し改良を加えたものになりますが，本質的なところは同じなので，ここまでくれば GMapping もすんなり理解できると思います．
参考文献
毎度の確率ロボティクスです．
- 作者: Sebastian Thrun,Wolfram Burgard,Dieter Fox,上田隆一
- 出版社/メーカー: マイナビ出版
- 発売日: 2016/09/21
- メディア: 単行本
- この商品を含むブログ (1件) を見る
Monte Carlo Localization ~ パーティクルフィルタを用いた自己位置推定（実験編）
確率ロボティクス８章の内容です．
目的
「こんなこと，何に使うねん？」って話ですが，自律移動のロボットを作るときに，走行する環境の地図をあらかじめ持っている場合が結構あります．（つくばチャレンジ，学習後のお掃除ロボット等）この「あらかじめ持っている地図」を有効に活用して自律移動するためには，「地図の中で，実際に自分がいる位置はどこか？」をある程度正確に求めなくては行けません．この時，タイヤの回転数を積分して自己位置を推定する方法（デッドレコニング）だと誤差が蓄積してしまって走行距離が長くなると全然使い物にならなくなるので，通常は Lidar，カメラ，ソナーなどのセンサーの結果も同時に使用します．ここで，よく似たような話が出てきます．
自己位置を求めるために，
１．デッドレコニング
２．Lidar の入力
の二つがあるけど，どうやって統合する？という， EKF Localization の時にも話したようなよく似たような話が出てきます．今回のアルゴリズム（Monte Carlo Localization）も，デッドレコニングと Lidar のデータの両方を用いて地図の中で自分がどの位置にいるかを推定します．地図に対して自分がどこにいるかわかることで，次に進むべき方向とか位置がはっきりします．
実験結果
問題設定のイメージですが，前回の EKF Localization のエントリのように特徴点ベースの地図だと GMapping とか ROS AMCL と結びつかないので，OGMを使ってみました．自己位置推定するフィールドは仮想的につくったものですが，下記になります．上図が自己位置推定対象となる環境で，下図が実際にロボットが通った真値のパスです．
で，この実験の前提としては，上図の地図を予め知っているという前提で，このコースを走行するロボットが自己位置推定をします．仮想的に 2D Lidar を想定して実験をしてみました．実験の手順としては，
１．真値のパスに沿ってロボットを走行させます．が，ロボット自身が取得できるオドメトリデータには白色ノイズを載せてます．（＝実際には真値のパスを通っているが，ロボットはその正確な情報を取得できない状況．）
２．0.25mロボットが進む毎にロボットに搭載されている 2D Lidar からのデータを取得できます．（＝2D Lidar の計測値は実際のパスからの計測値．だけど，若干誤差が乗っている．）
３．2D Lidar から取得したデータと，自分の推定位置から得られると想定される 2D Lidar の値を比較します．
４．比較したスコア順にパーティクルに重みをつけ，重み比例する形でパーティクルをリサンプリング．
で，実験結果が下記のようになります．
youtu.be
どうも自分の Ray Casting の実装が下手くそなのか，Python だからある程度は仕方ないのか，結構実行に時間がかかります．上記のビデオ，ゴールにたどり着くまでに40分くらい時間がかかってます（笑）．この動画では，１０個のパーティクルを使ってますが，割とうまいこと自己位置推定ができています．下図は，推定した自己位置からスキャン結果を用いて作った OGM になります．
理論とか式典解とか．．．．
EKF Localization の時と同じく，，，また時間ができたらアップします．ただ，Monte Carlo Localization は Fast Slam や GMapping とも関連が非常に強いので，SLAM を勉強する上では必須の内容だと思います．Fast Slam の実験結果を次のエントリで書こうと思っていますが，Monte Carlo Localization ができてしまった後は，意外と簡単に作れました．
ソースコード
いつもの Github においてます．
github.com
上記のリポジトリを落として，
cd tests python3 monte_carlo_localization_example.py
とやれば動きます！が時間がかかります（笑）
参考文献
いつものことながら，確率ロボティクスです．
- 作者: Sebastian Thrun,Wolfram Burgard,Dieter Fox,上田隆一
- 出版社/メーカー: マイナビ出版
- 発売日: 2016/09/21
- メディア: 単行本
- この商品を含むブログ (1件) を見る
EKF Localization ~ 拡張カルマンフィルタを用いた自己位置推定（実験編）
ということで，先月末からかじりついている確率ロボティクスですが，７章の拡張カルマンフィルタを用いた自己位置推定を実装してみたので書き留めておきます．
実験結果
問題設定のイメージは，確率ロボティクスにも出ていた RoboCup にしてみました．下記の写真は aibo がサッカーをする Field の写真ですが，Field の端に 6 本の色付きのバーが立っていることがわかります．ロボットは事前に位置のわかっている 6 本のバーを計測することによって，自分がフィールド上でどこにいるかを推定してサッカーをします．
で，上記のサッカーフィールドで楕円の軌跡を描いて移動するロボットが自己位置推定をするイメージで EKF Localization のサンプルコードを作ってみました．
で，ビデオの説明ですが，黒の点が真値，青の点が入力（速度，角速度）を単純に積分した位置，赤の点が EKF による自己位置推定の結果です．入力を積分した結果ですが，どうも楕円ぽい軌道にならないのでここは自分の間違いがあるかもしれないです．ただ，結果的に EKF を使って正しく自己位置推定ができているようにみえるので，良しとします．計測されたポールは画面上で黄色で表示されます．
先日のエントリで書いたように，カルマンフィルタを用いると，異なる二つの推定結果を「いい感じ」に統合してくれます．
daily-tech.hatenablog.com
ただし，異なる二つの推定がどんな確率分布になっていても，無理やり一つの正規分布で表現します．ここでは，
１．入力を積分したロボットの存在確率分布
２．計測から求まるロボットの存在確率分布
の二つがあって，この異なる二つの正規分布を最適に統合して一つの正規分布を作っています．で，この統合後の正規分布の平均値が赤でプロットされた点になっています．
理論とか式展開とか．．．
上記の内容ですが，ほぼ確率ロボティックスに書いてある内容になります．
またどこかで時間があるときに，理論編のエントリも書きたいと思います．
ソースコード
代わりにソースコードを Github に置いておいたので，もしよければ使ってみてください．
github.com
上記のリポジトリを落として，
cd tests python3 ekf_localization_example.py
とやれば，うごくはず！
参考文献＆サイト
確率ロボティクス
- 作者: Sebastian Thrun,Wolfram Burgard,Dieter Fox,上田隆一
- 出版社/メーカー: マイナビ出版
- 発売日: 2016/09/21
- メディア: 単行本
- この商品を含むブログ (1件) を見る
困ったときはこのサイト！
myenigma.hatenablog.com
２０１９年１月やること！
１月も折り返してしまったんですが，年初のエントリで書いたように今月やりたいことをまとめておきます．年明けから粛々とやってきたんで１月はまずまずの成果になるかと思います．
今月のテーマ
２０１９年やりたいこと！で宣言した中で，今月のテーマは下記にしました (ﾟДﾟ)ﾉ
今月の課題図書
ということで，今月の課題図書は下記の３冊です！
確率ロボティクス
- 作者: Sebastian Thrun,Wolfram Burgard,Dieter Fox,上田隆一
- 出版社/メーカー: マイナビ出版
- 発売日: 2016/09/21
- メディア: 単行本
- この商品を含むブログ (1件) を見る
Clean Architecture
Clean Architecture 達人に学ぶソフトウェアの構造と設計
- 作者: Robert C.Martin,角征典,高木正弘
- 出版社/メーカー: KADOKAWA
- 発売日: 2018/07/27
- メディア: 単行本
- この商品を含むブログを見る
新訂版 More Effective C++ (AddisonーWesley professional co)
- 作者: スコット・メイヤーズ,安村通晃,伊賀聡一郎,飯田朱美,永田周一
- 出版社/メーカー: ピアソンエデュケーション
- 発売日: 2007/06/29
- メディア: 単行本（ソフトカバー）
- 購入: 8人 クリック: 129回
- この商品を含むブログ (44件) を見る
目指せ今年の読了３０冊！
離散ベイズフィルタ（カルマンフィルタ）
12月から GMapping の勉強をするベくコードを読んでます．で，SLAMのバイブルこと確率ロボティックスを読みながら理解していってるんですが，やっぱりちょっとサンプルコードを書いたほうが理解が深まるかと思い，確率ロボティックスを部分的に Python で実装して勉強してます．一応 OGM ベースの Fast Slam まで動くようになったので，今日から順次ブログを書いていきたいと思います．コードは下記の Git においてます．
github.com
このエントリで触れるのは一番最初の離散ベイズフィルタの内容で，上記リポジトリの "prob_robotics/tests/hist_filter_example.py" のコードになります．
１．離散ベイズフィルタ
で，「離散ベイズフィルタっていったい何よ？」って話なんですが，ロボットとか車載の検知アルゴリズム作っている人にとっては「離散ベイズフィルタ」よりも「カルマンフィルタ」のほうがより身近だと思います．確率ロボティックスを読んで初めて知ったんですが，カルマンフィルタもベイズフィルタの一種なんですね．
で，どういうときにこのフィルタを使うか？という話なんですが，下記はちょっと自分解釈になりますが．．．
１．直接観測できない状態を推定したい．
２．状態を推定する方法が複数ある．
３．一つの推定方法だけで状態推定するのではなく，複数の方法を用いて状態推定をして一つの方法の時よりも推定結果をベターにしたい．
というときかなと思います．文章で書くと「なんやそれ？」って話なんですが，工学的にこういう状況って結構あると思います．
１．カーナビ
カーナビが適切に自車位置を把握していないと経路案内ができないですが，車両の位置を知る方法は複数あります．
方法１：タイヤの回転数とハンドルの舵角を積分して，位置を求める．
方法２：GPSを使う．
２．部屋の温度調整
エアコンを使って部屋の温度を設定温度まで温めたいとします．この時も部屋の温度を把握してないと設定温度まで温度を上げられないですが，部屋の温度を知る方法は複数あります．
方法１：これから部屋に送る温風（熱量）を計算して，どの程度温度が上がるか計算する．
方法２：エアコンについている温度計を使って温度を測る．
３．ロボットの自己位置推定
ロボットを所定の位置まで動かすために，今いる位置を求める問題．
方法１：タイヤの回転数とヨーレートを積分して，位置を求める．
方法２：LIDARやカメラのセンシング結果を用いて，位置を求める．
上記の３例を考えてみるとわかると思うのですが，どの方法を使っても「完璧！」には求めたいものが求まりません．例えばカーナビのケースですと，タイヤの回転数とハンドル舵角の積分で位置を出しても，走行距離が長くなるにしたがって誤差が蓄積してしまいますし，GPSも電波状況によっては 5~10m オーダーの誤差が乗ってしまいます．で，こんな状況で離散ベイズフィルタを使えば何がうれしいのかというと，方法１で計算した値と，方法２で計算した値を少なくとも確率的には最適になるようにフュージョンしてくれます．
２．アルゴリズム
で，実際のベイズフィルタアルゴリズムとそのイメージです．ベイズフィルタでは，大きく”予測ステップ”と”修正ステップ”があります．それぞのステップでやっていることのイメージを図示してみました．
予測ステップでやっていること
イメージを簡単にするために，カーナビの例を考えてみます．この例では，予測ステップにタイヤの回転数を使います．
t-1 から t まで時間が進んで，この間にタイヤが一回転（一周1.5m）したとすると，ベイズフィルタの入力である ut は 1.5 になります．この時，誤差が全くなければ xt = xt-1 + 1.5 になると思いますが，誤差が全くないということはありえないので，xt は xt-1 + 1.5 を中心にひろがりをもつ確率分布になります．
上記の誤差分布をみてわかる通り，xt = xt-1 + u は xt-1 + 1.5 にピークを持つ確率分布になります．アルゴリズムに書かれている遷移確率 p(xt | ut, xt-1) はこれを表現しています．この遷移確率は 「xt-1にいるときに，utという入力を受けて，xtという場所に移動する確率」を表しているので，最終的に xt にいる確率を求めるためにはそもそも xt-1 にいた確率 bel(xt-1) をかけてあげないといけません．これが予測ステップの積分の対象となっている式です．で，次に，遷移確率に分布があるために，xの取りうる値すべてから xt に遷移する確率を求めてあげる必要があります．この積分を実行すると，p(xt | ut, xt-1) -> p(xt | ut) となります．
アルゴリズムに "for all xt do" という記載があるように，このステップによって xt の分布がわかります．
修正ステップでやっていること
カーナビの例なので，計測ステップで GPS 信号を使います． GPS からはダイレクトに車両の位置がわかります．ただし，GPS から求まる位置も誤差を含んでいるため，この値も確率分布になります．このステップでは，予測ステップと修正ステップで得られる二つの分布を使って「答え合わせ」をします．どのようにするかというと，二つの分布を掛け算してその結果を最終分布として用います．ちょうど下記のようなイメージです．
上記のステップを経て，方法１と方法２の結果を統合することができました．結局のところ，これがベイズフィルタ・カルマンフィルタのやっていることです．
３．思考実験
ということで，実際にサンプルコードを書いてみました．状況設定ちょっと変わってしまうので恐縮ですが，下記のような仮想的なケースを考えてみました．
１．歩行者Aが自由の女神像から直線距離80mのところにいます．
２．歩行者Aは自分の歩幅を数えることで自分の位置を計算します．
３．歩行者Aは自分の歩幅を大体わかっていますが，1m 歩く毎に 1σ 0.3m くらいの誤差が乗ります．
４．歩行者Aは受信機を持っていて，この受信機は10m歩くたびに自由の女神像までの距離を教えてくれます．が，こちらの受信機も1σ 2m くらいの誤差が乗ります．
上記の設定で，「１．歩行者が歩幅を数えて自由の女神像までの距離を求めた時」「２．歩行者が両方の値を使って自由の女神像までの距離を求めた時」のうち，２の結果がよくなることを見たいと思います．
参考文献
- 作者: Sebastian Thrun,Wolfram Burgard,Dieter Fox,上田隆一
- 出版社/メーカー: マイナビ出版
- 発売日: 2016/09/21
- メディア: 単行本
- この商品を含むブログ (1件) を見る
SI2018に参加してきました．
ということで，もはや１２月の恒例行事になりつつありますが，昨年の１２月１３日にSI2018に参加してきました．毎年開催地が変わっていて，
SI2015 名古屋，SI2016 札幌，SI2017 仙台
に続いて今年は大阪開催でした．毎年開催地が変わるので何気に楽しみにしているんですが，このまま出続けて，47都道府県全制覇したいと思います（笑）
で，このSIなんですが，参加登録の締め切りが8月中頃にあります．で，つくばチャレンジに向けてのスケジュール的に，8月中頃は「今年はマジ完走させるぞ(ﾟДﾟ)ﾉ」っていう勢いなので，毎回SIの参加申し込みをするんですが，本番が近づくにつれて「あかん．間に合わん(;´･ω･)」というモードになってきます．何が言いたいかというと，12月に入ってからいつものように「しゃべることがないけどどうしよう状態」になるわけです（笑）
特に今年はハードを作り直して，ROSのフレームワークをほぼそのまま使っただけだったので本当になにも言うことがなかったんですが，「旅の恥はかき捨て」ならぬ，「講演会の恥はかき捨て」ということで，しゃべってきました．まあ，気長に行くしかないですね！来年はもうちょっと濃いことをしゃべります！
www.slideshare.net
