環境認識とか自律走行をやりたかったのを忘れていました。
・・・が、新しいことを恐れる気持ちがあるのか知りませんが、どうも「まだブログにまとめてないし・・・」と先に進むのを躊躇する自分がいるので、あまり利点は多くないとは思いつつ勉強記録的にまとめます。
僕以外が後から追ってどうにかなるような記事にはなりません多分。
この記事は自分用のメモです。現時点では間違っている可能性も大いにあり、自分で使ってみたり書籍を当たったりして知識を修正していくつもりです。間違っても参考になどしないようにお願いします。
今までの取り組み
今まで自律ローバーを作ってきましたが、そもそもこれは「環境認識と自律走行をやりたいのに、車体制御だけで一杯々々の倒立振子には付き合ってられない」ということで作り始めたのでした。
ktd-prototype.hatenablog.com
・・・が、実際は車体製作まで終わった所で長いこと放置してしまいました。
ここで終わってしまっていたのは、自分の電子工作への興味が「モーターが回る所まで」でしかないからなのですが。
とは言っても車体開発だけで進歩しない自分を好きで居られる自信もないので、やはり環境認識とか自律移動もやりたい、と思い直した所です。
何を以て環境認識と自律移動なのか
あくまで自分なりの便宜的定義です。下手したらこの記事の中でですら定義が変わり得ますが。
前回記事でも書いたのですが、大域的な情報を取ろうとするか、それに基づいて移動するかというのが1つのポイントなのかなと思います。
ktd-prototype.hatenablog.com
電子工作のハウツー本に出てくるのは赤外線測距センサや超音波センサをつかって障害物検知なんぞをする所までだと思います。これは局所的な環境認識ってやつですね。
マッピング等によりセンサ覆域外の情報を保持・利用し始めたら・・・とか、次に右に行くとか左に行くとかではなく大局的な経路の計画を始めたら、といった所でしょうか。
ただ、局所的（あるいは即時的？）な情報であっても、物体認識とか三次元点群処理とかしていたら十分に環境認識と言って良いような気もしますし、ここはさじ加減ですね。
本題
ここから本題で、メモ書き的になります。
目に付いたサイトをとにかく追う
電子工作関連の勉強はずっとこのスタイル。目に付いたのはこのサイト。
qiita.com
ROSのNavigation Stackという枠組みの説明らしい。連載記事としては完結してないみたいだけど、今ある分でも十分。
1.導入
記事を見ると最初に図が出てくる。
ROSは殆ど理解していないつもりではあるけどわかる範囲で解釈すると、ナビゲーション全体を実現するための情報源と活用先しては以下のようになっているらしい。これはまあ直感とも合致する。
- 地図データ（/map）
- 大域地図（global_costmap）
- 局所情報 : 各種センサデータ（/LaserScan、/PointCloud）
- 大域地図と局所地図（local_costmap）の両方
- オドメトリ情報：オドメトリデータ（/Odometry）
- 局所プランナ（local_planner）
- センサ情報
- 何かに使うらしいが不明
それらを元に、大体以下の流れで動くということだと思われる。これもまあ、なんとなく何かを動かそうと思ったらこうなるし人間もこんな感じで動くだろうという気がして直感と合う。
- 大域地図と指定目的地（/goal or(and?) /PoseStamped）から大域プランナ（global_planner）によって経路（/Path）を算出
- 経路と局所地図（今見えている景色と考えて良いだろう）から、局所プランナによって次に動くべき方向を算出
- 次に動くべき方向から、ロボットの速度指令を算出し発行（/Twist）
- 各種センサデータ→局所地図→大域地図という流れで大域地図を適宜更新（？）
2.move_base（ナビゲーション）
move_baseのデモンストレーション
大域地図、自己位置情報は既知として、ナビゲーション（経路計画のことだと思っていいだろう）の部分を特出しで考えるらしい。
実際には地図生成、地図管理、自己位置推定、ナビゲーションとあるようで、その中の最後の１つだけを実行するということ。SLAMとか言うくらいだから、地図生成の中に自己位置推定も含まれているのであって、記事中に出てくるamclとやらは不要では？と思うがとりあえず置いておく。
記事中のガイドに従ってROSの各コマンドをターミナルに打ち込んでみた。
確かに動く。これは面白い。Rviz上で2D Nav Goalを選択した状態でマップ上の選択するとそこがゴールとして設定され、緑色のラインによって大域プランナによる経路、赤いラインによって局所プランナによる進行方向が示されている。（今の図だとオドメトリ情報の赤い矢印で潰れて見えていない）
大局地図と局所地図
global costmapとやらはセンサ覆域外の情報も保持できているようで、これは地図生成もしていると言えるのでは？と思ったが、下の図をよくよく見ると、global costmapが示す壁などの位置と、2D-LiDARが示す黄色い点群の位置がかなりズレている。（図の中央下あたり）
後者の黄色い点群が実際の障害物位置だろうから、global costmapは「mapのように見えるが信頼性が低く役に立たない情報」だと捉えるしか無いのだろう。
地図生成も自己位置推定もせずに移動すると誤差が蓄積するということだろう。ただ、ズレたglobal costmapも、また近くに寄ってセンサ覆域内に入れば更新自体はしてくれるようだ。
何の情報を元に動いている？
これが経路計画オンリーで動かした場合の限界ということか。時点のセンサデータ＋局所地図（local costmap）が主で、不正確な大域地図（global costmap）も使いながら、障害物を避けて目的地にたどり着いてはくれるということだろうか。
・・・ってよくよくrqt_graphを見ると、/move_baseノードに対してlocal/global_costmapの入力がないことから、経路計画と移動には局所地図も大域地図も使っていないことがわかる。
/odomデータは入力されているがそもそも発行がされていない（echoしてみて気付いた）し、/move_baseに入力されている/footprintはロボットの端点座標を示すだけ。
事実上/scanデータと目的地座標（/move_base_simple/goal）しか入力情報が無いようだ。ちなみにこの目的地座標は類似のメッセージが複数あるが、すべて2D Nav GoalをRviz上で指定するとメッセージが発行されていて、違いはよくわからない。
本当に「目標位置には向かいたいけど障害物にはぶつかりたくない」という要請を目標位置データと2D-LiDARの情報だけで実行しているらしい。
そして拡張カルマンフィルタを使った自己位置推定ノード（/ekf_localization）もあり、これにはホイールオドメトリ（/husky_velocity_controller/odom こちらは発行されている）とIMUデータ（/imu/data）の情報が流れ込んでいるが、この結果得られた自己位置推定結果（/odometry/filtered）は確かに今の所どこにも使われていなさそう。
コストマップ
コストマップは名前しか聞いたことなかったが、障害物そのものを示す領域（黄色）、ロボットの重心（的な点）が進入したら寸法的にロボットの端部が障害物にぶつかることを示す領域（水色）、ぶつかりはしないがそのリスクが高まる領域（濃い青）として、衝突リスクをコストとしてマッピングしたものらしい。よくできとる。
スタックからの脱出
移動中に袋小路に落ち込んだ場合、その時点のセンサ情報だけだと経路が見つからない可能性がある。
その場合に備えて、その場旋回して他の領域をセンサ覆域に含めることで改めて通れる場所を探す処理、コストマップのコスト判定閾値を下げることでより衝突リスクの高い経路にも進入できるようにする処理などが含まれているみたい（recovery_behaviors）
ホイールオドメトリの誤差
拡張カルマンフィルタ（/ekf_localization）を使っており、おそらくそこから出力される /odometry/filtered を自己位置として扱っているのだと思うが、情報源がホイールオドメトリ（/husky_velocity_controller/odom）とIMUデータ（/imu/data）しかないためドリフト補正の機能を持たない。
実際見てみると、固定座標系として据えた/odomフレームの原点（ロボット初期位置であり、GUI上の位置としても固定）に対して、障害物等との相対位置から（僕が）推定するロボットの真の初期位置はズレていくことがわかる。
特にClearpath Huskyのようなスキッドステア（左右のタイヤの回転数の差で曲がる操舵方式、いわゆる差動二輪は除く？）は方向転換時にオドメトリ誤差が乗りやすいため、このようになっているのだろう。
スキッドステアのために、方向転換だけは車輪の回転でなくジャイロやコンパスの情報で検出する方式もあり得るかも知れない。
初めて見た時は、/odomフレームがズレて動いていくべきなのか、そちらは固定座標としたので固定であり障害物等の検知結果がズレて動いていくべきなのか脳が混乱したが、後者のようだ。
障害物の位置は不変なのだから前者では？と思いもしたが、実際は「ズレていく/odomという移動座標系に固定した神（僕）の視点」がRvizの視野であるということだろう。
障害物の位置が認識するごとにズレていくので、（自分が気付いていないだけで）Rvizの視野範囲が真のワールド座標系に対して（/odomフレームに載って）動いているようなイメージを持っている。
まとめ
とりあえず地図作成も自己位置推定もなしで、シミュレータ上でロボットを動かしてみた。
誤差だらけでも良ければ大域マップを擬似的に作ること自体はできること、2D-LiDARによる局所地図によってだけでもナビゲーション自体は不可能ではないこと、でも誤差がたまることなどを理解した（つもり）
それにしてもこのシミュレータ面白い。
Clearpath Huskyも欲しくなる。何百万円とするのだろうけど・・・
clearpathrobotics.com
この会社、Boston Dynamics社のSpotも売っているようだし、Husky以外の他のモデルは別企業で見たことがあるような気がするので、車体は外注品なのかも知れない。
