目次
- 導入
- move_base (ナビゲーション)
- ROSで遊んでみる
- ソフトウェア構成をみる ← いまココ
- amcl (自己位置推定)
- gmapping (地図生成)
- ROSで遊んでみる
- ソフトウェア構成をみる
- 原理をみる(応用編)
- 原理をみる(準備編 その1)
- 原理をみる(準備編 その2)
はじめに
Navigation Stack を理解する 2.1 move_base: ROSで遊んでみる の続きです．move_base ソフトウェアの構成とメッセージのやりとりを覗いてみます．
ROS Wiki の図中
導入部で示したパッケージの図中
環境
- OS: Ubuntu 14.04
- ROS: Indigo
- モデル：Husky
- パッケージ：husky_navigation
- チュートリアル：Husky Move Base Demo
rqt_graph をベースにソフトウェアの構成を見る
素のrqt_graph を見る
下記コマンドを実行するとメッセージのやりとりを含めたソフトウェア構成図が出てきます．
$ rqt_graph
ドーンって感じです．初めて使った時，ド素人の私にはもうわけが分からんという状態でした．こういう構成図をパッと理解できる世のGeekには，本当に頭が上がらないです…
簡易版rqt_graph を見る
というわけで，ひとまず全体像を理解する上で重要そうな最小限の構成要素を，自分なりにピックアップして作図し直したものが下図です．ただし，本当は変換が必要だったり，正確にメッセージを追うと経路が複雑になってしまうような詳細部分は，意図的に省略しました．
まだ少しごちゃついていますが，少しだけすっきりしたかな，というところです．各ノードの役割は図中に記載の通りです．目標姿勢が入力されたら，move_base を起点に反時計回りにループが形成されているようなイメージです．
目標姿勢は/move_base_simple/goalというトピックをsubscribe することで設定され，そこから内部的にはMoveBaseAction というactionlib が走ります．actionlib は非同期で実行されるので，ナビゲーションの途中状態においても，move_base は他の指令を受け取ることができます．例えば，まだ目標に到達していない状態でも，目標を上書き更新することが可能です．複数の目標を順番に指定して経路を生成するケースでは，好きなタイミングで目標を切り替えられる点で，使い勝手が良い仕様と言えます．
/odom フレーム？ /odom トピック？
ここで，念のため注意点です．オドメトリの原点を指すフレーム名と，オドメトリによる現時刻における積分値が格納されるトピック名が，まったく同じ/odom であることです．いや，分かっている人からしたら当たり前なことなのかもしれませんが，少なくとも素人でかつ頭の回転も早くない小生にとっては，初見で混乱したことを覚えております汗．
その上，よりにもよって片方は「オドメトリの原点」，もう片方は「オドメトリの先」を意味する情報であり，同一の名称を持つにも関わらず互いに反対の意味を持っていることで，（大変不甲斐ないことですが）私の脳内での混乱は激しさを増すばかりでした汗．
という経緯もありまして，敢えて図中の「トピック」と「フレーム」を異なる書式にすることで，異なる実体を指すことを強調しています．これにより，初見での混乱を少しでも軽減できればと思っています．
経路探索
ここまで，move_base と他のノードとのやり取りについてみました．その中を覗くと，更に細分化された機能に分解されます．ROS Wikiの図のうち，下図の4つの赤い円で囲ったところを見ていきます．
コストマップと経路計画
大別すると，コストマップを管理する機能(costmap)と，経路計画(planner)を行う機能です．前者は，センサ情報をもとに検出された障害物に近いほど値が大きくなるコスト情報で，後者はそのコスト情報を使って障害物を回避しながら経路計画を行います．
大域的探索と局所的探索
かつ，それぞれが大域(global)と局所(local)について実行する機能を備えています．大域的な処理については，事前に得られた地図などの情報を元に，予め壁や障害物があるところにコストを割り振り，経路を決定するものです．局所的な処理については，現在得られたセンサ情報を元に，事前知識では存在しない障害物（歩行中の人など）を検知し，リアルタイムに経路を補正する役割を担っています．
大域と局所の比較
上述した情報がどのように異なるのか，rvizのキャプチャを含む図で確認してみます．まず，rvizの表示項目のうち，Global Costmap, Global Costmap, Global Costmap, Global Costmapが該当するチェック項目です．見たい項目に応じて，チェックの有無を切り替えながら挙動を確認してみましょう．
下図がまとめです．左が大域的探索の場合，右が局所的探索の場合です．
画像内で青く色づいているところがコストマップです．左側ではこれまでの移動によって蓄積された情報に対してより広い範囲で，右側では現在センサ(Husky のモデルではLRF)で得られた情報を元にコストマップが生成されています．
コストマップの内訳は，ざっくり下記のとおりです．
- 黄色：障害物
- シアン：ロボットの領域から逆算して，侵入したら障害物と衝突する領域
- 青：ぶつからないかもしれないけど，すれすれを通るのも怖いのでできれば避けたい領域
シアンの領域については，/footprint という情報を元に算出します．これは，ロボットを外側から囲った領域を地面に投影したようなものです．事前に設定しておくものですが，その辺りの定義の仕方はcostmap_2d に譲ります．
さらに，特に青の領域は，障害物から離れる毎に徐々に小さくなるような関数となっているようです．これの詳細も，costmap_2d に記載されています．
そして，経路計画自体もrviz 上で確認できます．大域的経路計画はNavfnRos，局所的経路計画はDWSPlannerROSで行われています．それぞれの経路情報は，/move_base/NavfnRos/plan，/move_base/DWAPlannerROS/local_planに格納されています．図で言うと，ロボットの下に見える緑の線が大域的経路，赤い線が局所的経路です．緑の線に追従するように赤い線が逐次的に更新されるようなイメージです．ただし，大域的経路の周辺に障害物が現れたら，赤い線は一時的に緑の線からは遠のくことになりますが，障害物を回避した後は再び緑の線を追従するような挙動を示します．
これらの探索アルゴリズムについての直感的な説明は，原, "ROSの活用による屋外の歩行者空間に適応した自律移動ロボットの開発", ロボット工学セミナー, 2015. に分かりやすく記載されています．
DWAに関するサンプルと元論文の解説まで含めた詳細な情報は，Atsushi(ブログプロフィールより), "Dynamic Window ApproachのMATLAB サンプルプログラム", MyEnigma, 2014.に記載されています．このブログには他のロボット関連の技術に関するサンプルが豊富に掲載されており，非常に参考になります．
積み状態からの回復
さて，まだ下図の赤い楕円で囲ったrotate_recovery が残っています．これについて見てみます．
大域的探索においては少なくても事前情報の範囲で障害物にぶつからないように経路探索をすることは前述のとおりです．しかし，局所的探索においては，障害物の出現位置によっては，当初進めると思っていたのにそれ以上進めなくなってしまう状況が起こりえます．通せんぼ状態です．
そういう状態から，頑張って抜けだそうとするための仕組みが，rotate_recovery です．その場旋回が可能なディファレンシャルドライブ型，あるいはオムニドライブ型のロボットであることが前提です．
コストマップをクリアしたり，その場を一周してどこか通り道がないかを探します．コストマップをクリアしているのは，通せんぼをしている領域を少なくすることで，隙間を縫うような経路を見つけられるのではないか，という発想があると解釈できます．
上図は極端に書いたので気づくと思われますが，当然衝突のリスクが大きくなります．動かなくなるよりは，スレスレでも前に進めた方がずっとましということです．
さて，ROS Wikiの図を参考にすると，流れは次のとおりです．
- できれば避けたいと思っていたコストマップ領域を少し狭めて，その場で局所的探索を行い，隙間を通れるところが無いか確認する
- 1. で失敗したら，一周回りなが局所的探索を行い，隙間を通れるところが無いか確認する
- 2. で失敗したら，1. よりももっとコストマップ領域を狭めて，その場で局所的探索を行い，隙間を通れるところが無いか確認する
- 3. で失敗したら，2. と同様の局所的探索を行う
- 4. で失敗したら，潔く諦めます
上記フローのうち 1. ～4. のどこかで隙間を見つけられれば，再び元のナビゲーションに復帰します．
なお，rotate_recovery はあくまでリカバリ用プラグインの一つで，回復行動を自分で定義してプラグイン化すれば，任意の構成のロボットでも適用可能です．
オドメトリの問題点
さて，ここまででmove_base については，何となく分かった気になれたと思います．
冒頭でも記述したとおり，ここでのチュートリアルでは自己位置推定がオドメトリのみです．
※/world という記述は，私が便宜的にワールド座標系の原点を表すために用意したもので，実際にrviz等で確認されるものではありません．
つまり，ワールド座標系の原点と，オドメトリの原点がずっと同一であることを想定しています．
しかし，実際には様々な外乱の蓄積によって，ワールド座標系とオドメトリの原点はずれていくことになります．
そこで，センサ情報と地図情報のマッチングによって，ワールド座標系の原点とオドメトリの原点との間のずれを補正しようという発想に至ります．そのためのパッケージがamcl となります．本パッケージが何を行っているのか，次項以降で見てみることとします．
おわりに
以上が，当方が理解している範囲での，move_baseに関するメモです．私が初めてこれを使って，rqt_graphを打った時に大群に出くわしたような感覚で面を食らってしまったのを覚えており，もう少しシンプルにした記事があったらいいのにな，という思いを元と書き起こしました．
原理的な記述は他の文献に預けたので，次節ではamclで自己位置推定を行う部分の解説をします．
Next: 3.1 amcl: ROSで遊んでみる
Prev: 2.1 move_base: ROSで遊んでみる