検索して何となく目に付いたweb記事を順に追うことで、ロボットの環境認識や自律移動の概略を何となく掴み、自分のロボットにも実装できるようにしよう、という取り組み第2回です。
一文で二回なんとなくという単語を使ったのは初めてです。
前回はNavigationの部分を試しに動かしてみたところでした。
ktd-prototype.hatenablog.com
今回はそれに引き続き、自己位置推定の要素を加えます。
この記事は自分用のメモです。現時点では間違っている可能性も大いにあり、自分で使ってみたり書籍を当たったりして知識を修正していくつもりです。間違っても参考になどしないようにお願いします。
自己位置推定とは
どういうものをそう呼ぶのか
文字通り自機の現在位置を推定するということなのだと思います。
INSやホイールオドメトリをベースにした場合など、誤差が蓄積する一方で補正する手段を持たないやり方はあまりそう呼ばない気がします。
これはそれぞれ人に例えれば
- 車に乗って目を瞑っていたけどなんとなく加速してた感があるからスタート地点より前方〇〇mの地点にいるだろう
- 目を瞑って△△歩あるいたから今はスタート地点より前方××ｍの地点にいるだろう
・・・ということになり、移動が長くなれば誤差は蓄積する一方です。
そうでないやり方として手っ取り早いのがGPSですが、逆にこれは銀の弾丸過ぎて、わざわざ自己位置推定とは呼ばないような気もします（そうでもない？
突き詰めればこれも簡単ではないのでしょうが・・・ところでGPS衛星自身の絶対位置みたいのはどうやって取っているんでしょうね。
結局のところ「ちょうど良く難しくて研究でも実用でもみんな取り組んでいる」みたいな部分を狭い意味で「自己位置推定」と呼んでいる印象です。
具体的には、空間中に固定された目印（事前地図やランドマーク）との相対位置をベースに自分の位置を推測するやつですね。
前回記事で、Rviz上で障害物との位置関係からロボットの初期位置はだいたいこの辺だったはず、と「僕が」推測していましたが、それをロボットにやらせるイメージでしょう。
前回記事の範囲では
前回は自己位置推定はしていない、とは言いましたが、それはおそらく狭義の自己位置推定はしていなかったということだと考えられます。
なにかしらやらなければ、2D Nav Goalで指定した目的地に移動させても、移動完了を判定することもできないはずです。
移動完了の判定を/move_baseノードで行っているとすれば、前回記事で指摘したように/move_baseノードへの入力情報が少なすぎる気がするのですが、そこはとりあえず無視します。
それに少なくとも、/imu/dataと/husky_velocity_controller/odomのメッセージから/ekf_localizationによって/odometry/filteredメッセージを出力する機能は持っていたようでした（どこにも使われていなかったけど）
web記事に従ってシミュレータを動かしてみる
参考にした記事
前回参考にした記事の続き、第3章の部分です。
qiita.com
使っている方式
AMCLという手法を使っているらしい。
wiki.ros.org
ROSwikiの概要によると、
2次元平面上のロボットの移動においてその位置を推定する適応モンテカルロ自己位置推定方式（AMCL：Adaptive Monte Carlo Localization）のことで、地図が既知の環境においてロボットの位置・姿勢を追跡するためにパーティクルフィルタを活用した方式（本稿著者訳）
ということらしい。
購読トピックは
- /scan
- /tf
- /initialpose
- /map
の4つだから、（大元の思想がどうかは知らないが）ROSパッケージとしての実装は少なくとも2D-LiDAR（次からLRFと書きます）を想定している様子。
2次元平面上の、と限定している部分と、想定している器材が対応しているのでしょう。
アルゴリズムは有名な書籍 Probabilistic Robotics（訳本邦題：確率ロボティクス）に載っているようだ。最近実装を学べる演習本も出たから、この部分を深堀するならちょうどいいかも知れない。
確率ロボティクス
books.rakuten.co.jp
シミュレータを起動してみる
記事に沿って実際にシミュレータを立ち上げてみた。
roscore、シミュレータ環境であるGazebo、可視化ツールであるRvizと順に起動していく。
記事中だとこの時点でワールド座標系の原点を/mapフレームとするとあるが、（Rvizの事前設定として登録でもされていない限り？）/mapはまだ選択できない。
amcl_demo.launchを立ち上げると/mapフレームをワールド座標系として設定できるし、実際にマップが表示されるようになる。
AMCLの説明で「既知のマップにおいて」とあったのがこれだろう。
実際左のトピックリストを見ると、前回記事では/mapメッセージがありませんとエラーを吐いていたStatic Mapの所にメッセージが来ている（赤枠）
/mapトピックをechoしてみると、-1が並ぶ謎の配列が表示された。都度更新されているわけではない（hzコマンドで調べても新しいメッセージが来ない）。既知と言うくらいだから当然か。
地図情報は画像ファイルとして外挿可能と聞いたことがある気がするので、もしかしたらピクセル毎に対応して-1 と1が格納されているのかもしれない。目がチカチカして1は見つけられなかったが・・・
シミュレータ上でロボットを動かしてみる
前回記事と同様の要領で2D Nav Goalによって目標位置をRviz上で指定しロボットを動かしてみた。
なぜかGlobal Costmapが薄く表示されるようになったが、調子よく動いてくれている。
既知のマップ（当然ただのマップなので動かない）とそれに固定された座標系（だと思う）mapフレームを固定座標系にしているので、それらは画面の中でも動かない。
ホイールオドメトリは誤差が蓄積するので、そこから逆算した/odomフーム原点は少しずつズレていく。意外と誤差が蓄積せず、わかりやすい画像を得るのに苦労したが、ズレは上のような感じだ。
既知のマップが与えられたからと言って、その中での自分の位置がわかっていなければ、LRFデータ（黄色点群）や、それに基づくコストマップは、既知のマップに対して相対的にズレていくはずだが、それは殆ど生起していない。
きちんと自己位置推定が働いているようだ。
トピックを見てみる
rqt_graphを見てみると以下のような感じになった（細かすぎて見えないかも知れないが）
自己位置推定というのだから、前回記事では不使用と断じていた/ekf_localizationによる/odometry/filteredが使われているのだと勝手に予想していたし、参考にしているwebサイトにもそのような図が掲載されているが、どうもそうではないらしい。。
自己位置推定を行っているのは右下の/amclノードだと考えられるが、このノードに流れ込んでいる主な情報は/mapと/scanのみで、事前地図情報と時点のLRFデータで自己位置推定を行うらしい。
/ekf_localizationは事前に少し予習していたが、確かにLRFデータを用いるような構成にはなっていなかった気がする。逆に先述したAMCLのros wiki記事は購読トピックとして/scanしか挙げられていなかった。不整合はないし、不可能でもないと思うけど・・・使ったほうが良い気もする。
自己位置推定結果のフィードバック？
自己位置推定結果を様々な情報に（global／local costmapとか）フィードバックする仕組みが必要だと思うのだが、少なくともrqt_graph上は現れていない。
/amclノードから出力された自己位置推定結果は/amcl_poseトピックに格納されているらしく、これが位置と姿勢を発行（移動中のみ）するトピックになっている。一方、rostopic infoしてもその購読者は誰もいないことになっており、このあたりがどういう仕組みなのかはちょっとわからなかった。
global／local costmapトピックと/map＆/scanトピックが結局、/amclにノードとは別系統で/move_baseノードを介して接続されているから、この辺りが上手く処理されている・・・？
この辺りを日本語で説明できないことが後々理解の深化を妨げる気がするが、とりあえず先に進むことにする。
AMCLの原理
プロのパワポエンジニアとしては原理にはあまり興味がないが、アマチュアの技術者としては頭に入っていた方が後々良いことがあるような気もして、迷いどころ。
特徴ベースと格子ベース
AMCL自体の話に入る前の予備知識。
自己位置推定において主流な方式は特徴ベースと格子ベースの２種に分類が可能。
特徴ベースとは、ロボットが既知のランドマークを予め知識と保持し、視野内のランドマークの見え方、サイズ感、他のランドマークとの位置関係等から自分の位置を推定する、というやり方。
これはステレオカメラの視差画像マッチングとかで出てくる「特徴点」とか、画像識別とかで出てくる「特徴量」というよりは、人が一般に使う意味での「特徴」な気がする。
逆にロボットがこの方法で自己位置推定をするというのは、思いつきはするけどあまり聞いたことは無い。
たまたま見つけたが、この辺がちょっと近いかも知れない・・・？
www.iaiai.org
一方で格子ベースは予め空間を格子（グリッド）で区切っておき、その格子内に障害物の有無を書き込んでいくことで格子地図（グリッドマップ）を作っていく。
これだけできても自己位置推定には繋がらないと思うが、グリッドの形状でマッチングでも取るのか？
自己位置推定に使うくらいだから、グリッドは空間に固定されたものと考えるのだろう。
カルマンフィルタとパーティクルフィルタ
AMCLはパーティクルフィルタというものを使っているらしい。カルマンフィルタともども、名前しか知らない。
（ここからは特に理解がかなりあやふやになるが）
カルマンフィルタは計測値、推定値の、ノイズ等による誤差が正規分布に従う（ガウス性を有する）と仮定する。
現在までの情報に基づく推定値（仮）と観測値それぞれにガウス性を仮定し、それらから最も尤度が高い値を新しく推定値（確）とする。「間を取ってちょうど良さそうな値に更新する」という解釈で良いのだろうか。
従って、例えば観測値に大きなノイズが乗った値が得られた場合（純粋なノイズだけでなく、想定外の環境の変化を含む）でも、推定値（仮）も、観測値もガウス性のノイズしか仮定していないから、観測値自体を異常値として棄却してしまったりする。あるいはどちらから見ても確率が低い部分を尤度最大としてしまい、「どっちつかず」となってしまうこともあるかも知れない。
それに対してパーティクルフィルタでは、例えば突然障害物が現れて測距センサがものすごく小さな値を出すとか、逆にノイズ等の影響で突然最大値を出すとか、そういった現象に対して一定の可能性を織り込んでおく・・・というイメージか？
壁までの距離を測距センサにより計測している際、急に壁との間に障害物が現れて実際の距離よりも小さな値がセンサから吐き出されたとすると、
こんな感じということか？
全くトンチンカンなことを言っているような気もする。
悔しいけれど今参考にしているweb記事の記述は全く理解できないので、この部分は多分最初から最後まで説明してもらえる参考書を読み込むしかなさそうだ。
AMCLの実際
参考にしているweb記事では応用編とされている部分。
qiita.com
事前推定
1フレーム前の自己位置推定結果に、ホイールオドメトリ等のデータによる移動量を加味して、現在位置を事前に推定しておく。これが前項で言う推定値（仮）だ。
ただ、この時点で、オドメトリにはノイズ、スリップ等による誤差が乗るため、厳密にその１点であることはありえない。ので、推定値（仮）としては、推定した点の周囲に複数「撒いて」おく。
点（Particle）を撒くから、パーティクルフィルタということだろうか。
もちろん、推定した点の周りにガウス分布的に推定値（仮）の候補点を撒くなら、それは実質的にカルマンフィルタであるということになるが、そうはならないということだろう。
ここは非常にわかりやすい。
観測に基づき自己位置の尤度を計算
僕も忘れていたが、今回は地図を既知のものとして予め持っているのであった。
従って、上記の各候補点それぞれに「自機が居ると仮定して」、そこからLRFで/scanデータを取った場合、どのような値が得られるかは予想が出来る。
どれが一番それっぽいか？を事前地図と観測値のマッチングにより推定し、最も尤度が高い候補点を現在の自己位置推定値として確定する ・・・という流れということか。
事前地図とマッチングを取る前の推定値（仮）の時点で、ガウス性を仮定しない（色々な場所に候補点をばら撒く）から、マッチする候補点が見つからないことを防げる、というのが1点目のメリットか。
LRFが出力するのは距離情報だから、この場合で説明するとすれば、「ある候補点（ロボットの位置・姿勢）においては既知の地図によるとこのような距離情報が出るはず、それにはノイズが乗るはず」として確率分布と、観測値とのマッチングによる尤度とを出力することになる。
ここで、事前地図には登録されていない障害物が出現し、それによって出力された距離情報が「このような距離情報（ノイズを加味しある程度幅をもたせた値）がでるはず」という範囲を超えてしまうと、その候補点自体が棄却されてしまう。
そこでこの「ノイズを加味しある程度幅を持たせる」という部分、このもたせ方を、ガウス分布を仮定するのではなく、上記のような極端な誤差も加味した分布としてやることがAMCLの（というかパーティクルフィルタの？）ポイントで、2点目のメリット。
この理解で合っていれば、前節で仮定した僕の理解（上のグラフみたいなやつ）も、概念としては間違っては居ないかも知れない。
の2段階でロバスト性を確保するイメージということになる？
その他
後は作り込みの問題と言うか、どのように候補点を撒くか（沢山、幅広く撒くほど正確・ロバストになるが、その分計算量が増す）という所が技巧的には問題になるようだ。
候補点の撒き方を適応的に変化させる手法もあるらしい。例えば以下のような感じ。
最終的には最も尤度が高い候補点を推定値（確）とするわけだが、この候補点は元々推定値（仮）に基づいて、その周囲（ガウス分布ほどは素直な周囲ではないにせよ）に重点的に撒かれているものだ。
従って、ロボットが強制的に移動させられたなど大きな位置誤差が乗った場合は、おそらく候補点群から、真の位置に最も近い端の点が尤度最高として選ばれることになり、候補点群の中央（推定値（仮）の付近）〜反対の端など、多くの点の尤度が低くなる、すなわち、全候補点の尤度の合計値が小さくなることが予想される。
そして、それでも尤度最高の候補点を新たな自己位置推定結果として算出せざるを得ないわけであるが、候補点群の端の方は、推定値（仮）の付近に比べれば疎であろうから、まだ誤差が大きいだろうと推定できる。
そこで、全候補点の尤度の合計値が小さくなった時には、自己位置推定精度が落ちているものとして、次回の候補点を撒く数を増やす、範囲を広げる等の適応的な対応ができる。
まとめ
とりあえず自己位置推定を動かしながらシミュレータ上でロボットが動いた。
AMCLの理屈はわかったような、全くわからないような・・・だが、一応なんとなくのイメージはできた。数式的に説明しろとか、自力で実装しろと言われたら全く無理だが、それはその必要が生じた時に考えるものとする。
ところで、ロボットが急に（例えば人によって）ひょいと位置を変えられてしまい自己位置推定が破綻するような状況になることを、参考したweb記事では「誘拐」と表現していた。
この記事でもそれを踏襲したが、多分英語論文でこのことを"kidnap"と表現するからだと思う。多分。
次回はSLAMをやります。
ktd-prototype.hatenablog.com
