gggggraziegrazie

graizegrazieさんのやったこと、学んだことを記録する雑記帳です

nodelet用に定義したクラスを使ってnodeを立ち上げる

ROSで処理を書く際、ノード間でのデータのやり取りの効率化のため、殆どnodeletとしてクラスを作ることが多いんじゃないかと思います。ただそうすると、そのクラスのメンバ関数をテストをしようとすると他のノードも立ち上がったりして面倒ですよね。じゃあnodeletをnodeとして立ち上げる方法はないのだろうか?と探したところ、下記の記事が参考になりました。

garaemon.github.io

記事曰く、image_procは上記node化の参考になるみたいです。実際にコードを見てみると、

nodelet::Loader manager(false); // Don't bring up the manager ROS API

の部分が重要なようです。loaderのコンストラクタの引数としてfalseが入っています。もしもtrueが入ると、LoaderROSクラスのオブジェクトが生成され、外部からこのloaderオブジェクトに対してnodeletのloadを要求できることなります。そのため、nodeletをnodeとして使いたい(外部のnodeletと繋ぎたくない)場合、loaderクラスのコンストラクタにfalseを入れ、LoaderROSオブジェクトを生成しないようにする必要があります。この後は通常のnodeletオブジェクとのハンドリングと代わりがないようです。

ご参考になりましたら幸いです。

マルコフ性、マルコフ過程、マルコフ報酬過程、マルコフ決定過程

 マルコフ決定過程(MDP)を勉強する上では、1つずつマルコフ過程から少しずつ変数を増やして理解していくのが分かりやすい様な気がします。[1]がそのパターンでしたので、[1]のほぼ流用の形でマルコフ決定過程についての紹介をしたいと思います。

マルコフ性(Markov Property)

 マルコフ性とは、次の状態は現在の状態のみに依存し、現在より前の状態(過去の状態)には依存しないという性質のことです。例えばサイコロを振る時、何回かサイコロを振っていたとしても、出る目は過去に依存しないですよね?そういうことを指します。

f:id:graziegrazie:20190601223856p:plain
Fig. 1 Markov Property[1]

 状態が複数ある場合、その遷移確率は行列で表現できます。行列で表現すると言うと小難しく聞こえますが、A -> B, B -> C, A -> C...というのを総当たり表の様に記載しただけです。なお"確率"と言うだけあって、状態Aから次の状態へ遷移する確率の和(状態遷移行列の行成分の和)は1になります。

f:id:graziegrazie:20190601225101p:plain
Fig. 2 状態の遷移確率[1]

-
マルコフ過程(Markov Process, MP)

 マルコフ過程とは、マルコフ連鎖とも呼ばれ、タプル\langle S, P \rangleで表現されます。ここで、{S}は状態の集合、{P}は状態遷移確率の行列をそれぞれ表します。例えばFig. 3のように状態遷移表と状態遷移行列で表現できます。確率については、計算式で求められることもあるのでしょうが、実験的に求めることが多いと思います。データを蓄積する過程で、状態遷移確率は適宜更新されます。行成分の和が1になることも確認してください。

f:id:graziegrazie:20190601230334p:plain
Fig. 3 マルコフ過程[1]

-
マルコフ報酬過程(Markov Reward Process, MRP)

 マルコフ過程に報酬という概念を加えたもので、タプル\langle S, P, \textbf{R}, \boldsymbol{\gamma} \rangleで表現されます。ここで{\textbf{R}}は報酬、{\boldsymbol{\gamma} \in \lbrack 0, 1\rbrack }は割引率と呼ばれます。最終的に最も高いreturnを稼げる(最も褒められる)状態遷移シーケンスを見つけることが目的です。予想されるreturnは、Fig. 4の様にして求められます。将来に得られるだろう報酬に関しては、割引率が掛けられていることがポイントです。未来の報酬というのは現時点では不確かなもので、得られない可能性があります。そのため確実に得られる直近の報酬はそのままで、将来的な報酬は先であればあるほど、その価値を低く(割り引いて)考えます。

f:id:graziegrazie:20190602003028p:plain
Fig. X Markov reward process[1]
f:id:graziegrazie:20190602041740p:plain
Fig. 4 Formula of return calculation[1]

報酬を考慮した時の状態遷移表は下記Fig. 5の様に表現されます。

f:id:graziegrazie:20190602041859p:plain
Fig. 5 Markov reward process[1]

-
マルコフ決定過程(Markov Decision Process, MDP)

 マルコフ報酬過程に対し、行動という概念を加えたものです。状態遷移という結果が勝手に起こる訳ないので、原因である行動(事象が正しい場合もあると思います)を考慮することは大変合点がいきます。どの行動をとるかについても、状態遷移と同様に確率で表現されます。ただし行動は現在の状態を元に決定されるため、その行動に関する確率は状態を使った条件付き確率で表現されます。この確率はPolicyと呼ばれます。Aさんはこういう時にこうする、というものを表現している訳なので、意味合いとしては納得しやすい様に感じます。

f:id:graziegrazie:20190602002954p:plain
Fig. X Markov decision process
f:id:graziegrazie:20190602043129p:plain
Fig. X Policy[1]

いかがでしょうか。私はこれで概要をわかった気になっているので、ご質問があればいただけると幸いです。

インピーダンス制御とアドミッタンス制御

インピーダンス制御(位置入力 ・力出力)

 目標位置と目標インピーダンス(バネ・マス・ダンパのパラメータ)を決めた状態でエンドエフェクタに力が加わると、目標位置に留まるように関節の固さ(軟らかさ)各関節角度を調整する。アドミッタンス制御のように、力に流されるのではなく、寧ろ逆らう。この時の固さは目標インピーダンスによって決まる。

www.youtube.com

アドミッタンス制御(力入力 ・位置出力)

 エンドエフェクタにかかる力が目標力と同じになるように制御を行う。例えばアドミッタンス制御を開始すると、もしも初期状態が目標力と異なれば、目標力になるよう手先が勝手に移動する。例えば外部からエンドエフェクタに対して力が加えられた時、その力に馴染むようにエンドエフェクタを操作する。つまり、エンドエフェクタが上から押されれば、その力に沿ってエンドエフェクタが下に移動する。この時の動作速度はインピーダンス(バネ・マス・ダンパのパラメータ)によって決定される。インピーダンス制御と異なり、エンドエフェクタが特定の位置・姿勢になるようには制御されない。

www.youtube.com

Visual Odometry Part I: The First 30 Years and Fundamentals

[0]を読んで気になった所を抜粋して纏めたいと思います。

Introduction

 Visual Odometry(VO)は、エージェントのegomotion(自己運動)を推定するものです。VOという言葉は、[1]でNisterらが初めて作り出しました。VOを効率的に行うには、環境中の輝度が十分であり、テクスチャーが十分にある静的な環境(動くものがない)が都合がよい。そして連続する画像がそれぞれ、十分に重複領域があることが必要である。Wheel Odometryに対するVOのメリットは、スリップがないこと。GPSが使えない場合(水中など)では、VOは非常に重要である。

History of Visual Odometry

 VOはStrucrure from Motion(SfM)の一種である。SfMは、もっと一般的な話であり、かつ画像セットからStructureとカメラ姿勢(順序は関係ない)の3次元再構築問題に取り組んでいる。それに対しVOは、時系列順に並んだ3次元カメラ姿勢を新しい画像が届いた時にリアルタイムで推定することにフォーカスしている。Bundle Adjustmentは、局所的な軌跡をrefineするのに使える。
 初期のVOの研究は、NASAの火星探査プロジェクトでの惑星探査ローバーのために行われた。slider stereo(単カメラをレール上でスライドさせてステレオ視を行う)やCoarse-to-Fine戦略を用いていた。外れ値は、depthの不連続でないかを8つのステレオのペアで確認することで、除外していた。システムは、重み付き最小二乗法で方程式を解いていた。この時の重みは、3次元点の距離に反比例して設定していた。
 Stereo VOとは、特徴点の3次元相対位置が各ロボット位置において三角測量で直接的に計測されること、そして相対運動の導出に使われること、の2つに由来している。Stereo VOのデメリットは、モーションをスケールファクターまでしか回復できないことである。スケールファクターは、直接計測(シーン中の物体のサイズを計測する)や他のセンサの統合(IMUや気圧計、距離センサなど)により決められる。シーンへの距離がステレオのベースラインよりも非常に遠くなると
三角測量ができない。この時はステレオではなく単眼画像でVOする必要がある。

f:id:graziegrazie:20190519043720p:plain
Fig. 1 Coarse-to-Fine[2]

Stereo VO

 カメラ単体でegomotionの推定をすると、距離を進むにつれてエラーがsuperlinearに蓄積していく。しかし絶対姿勢を計測するセンサを使うことで、エラー蓄積をlinearに抑えられる。[1]の論文は、従来の研究と異なり、リアルタイムで長距離のVOを求められる、最初の事例である。従来と異なる点は、第一にハリスコーナーなどの特徴点を各フレーム独自で求め、それを特徴点間だけでマッチングを取るようにした。今までは、3次元点のマッチングを、ステレオペアの画像同士でしかできなかった。この手法を用いることで、相互相関ベースのトラッキングを行っている間、特徴点のドリフトを避けられる。第二に、従来の3D-to-3D point registrationではなく、3D-to-2D問題としてカメラ姿勢の推定問題を解いた。最後に、RANSACを外れ値除外に適用した。3次元三角測量点ではなく2D特徴点を使うことで、モーション推定の計算の精度を高められる。

f:id:graziegrazie:20190519052840p:plain
Fig. 2 Superliner vs linear[3]

参考文献

[0]D. Scaramuzza, F. Fraundorfer, "Visual Odometry: Part I - The First 30 Years and Fundamentals," IEEE Robotics and Automation Magazine, Volume 18, issue 4, 2011.
[1]D. Nister, O. Naroditsky, and J. Bergen, “Visual odometry,” in Proc. Int. Conf. Computer Vision and Pattern Recognition, 2004, pp. 652.659
[2]https://www.researchgate.net/publication/228639372_Myocardial_Motion_Analysis_and_Visualization_from_Echocardiograms/figures?lo=1
[3]https://www.researchgate.net/publication/293654209_A_Promise_Theory_Approach_to_Understanding_Resilience_Faults_Errors_and_Tolerance_within_Systems/figures?lo=1

ROSにおけるodomフレームの役割

Navigaitonパッケージのamclを使うと、odomフレームがちょろちょろ動くのが確認できると思います。どうしてフレームが動くんだろうと不思議に思っていたので、調べてみました。推測も入っていますが、下記に調べたことなどを纏めます。

ROSの下記を見ると、

  • map : 自己位置推定の基準フレーム。
  • odom: オドメトリの基準フレーム。

となっています。map - odom間の位置関係は、特に何も無ければ一定です。ただし自己位置推定の結果としてロボットの座標が修正されると、その位置関係は変化します。なぜならば、odomフレームにおける軌跡(オドメトリ)の形状は一定となる必要があります。そのため、map上でのロボットの位置が変わる、つまりオドメトリの最新座標の場所が変化します。しかしオドメトリの形状が固定という制約があります。そのため、つじつま合わせにodomフレームが動きます(sendTransformでmap - base_linkの位置関係が更新させるのがトリガとなり、芋づる式にbase_link - odomが変化→map - odom間が変化となります)。

answers.ros.org
www.ros.org

Submap:部分地図(= local map:局所地図)

ここで取り上げるsubmapとは、ある一定の条件下で作成される、グローバル地図(全体地図)を構成する構成要素(部品)を指します。Submapの活用方法は、自己位置推定やloop closingです。今回はこのsubmapがどの様に構成されるのか、先行文献を調査した結果を記載します。文献のタイトル名を論文へのリンクにしていますので、ご興味あれば原著を読んでみてください。また参考になる文献を見つければ、適宜調査方法を変えてここに列挙したいと思います。

調査方法

  • 2017年以降のICRA/IROSを対象
  • 論文自体にsubmap又はlocal mapの構築方法の記述がある論文を対象としており、参考文献に構成方法を丸投げしているものは対象としない

An Online Multi-Robot SLAM System for 3D LiDARs

"The submap size is controlled by {m}, the number of previous scans and {d_{min}}, the minimum travelled distance between consecutive scans."とあるので、

  • 最低scanデータ数(submapを構成するセンサデータの数)をクリアしていること
  • 1つのsubmapを構成する上で必要な走行距離をクリアしていること(ただし始点と終点の距離が一定以上という意味であり、submap中でくるくる円を描いたとしても、円のサイズによっては目標クリアには届かないことを意味していると思います)

の2つが条件となります。これは一定の密度や大きさのsubmapを得られることを期待した処置と考えられます。

Visual Occupancy Grid Map for Submap-based Pose Graph SLAM and Planning in 3D Environments

  • {\Delta t}の間のscanデータを使って構築する
  • ただしsubmapの共分散が最大値を超えないように気を付ける
    • つまり、最大値を超えた場合は{\Delta t}を待たずして構築を止めると考えられる。

上記条件を定めた理由として考えられるのは、一定のデータ量を持った地図を作成したい、ただし自信のない地図はpose graphの最適化時に以降の座標推定を邪魔するため避けたい、ということだと思います。また本論文はnavigationを目的としているため、submapの共分散が上限を超えた場合はシステムをリスタートする等し、ロボットがおかしな方向に進まないようにするといった意図もあるかもしれません。

ProSLAM: Graph SLAM from a Programmer’s Perspective

  • 一定の距離毎に生成する

オドメトリの運動モデル

SLAMの研究で有名なFreiburg大のBurgard先生の授業資料を見ると、主に下記の4種類があるようです。

  1. Differential drive (AmigoBot, Pioneer 2-DX)
  2. Car drive (Ackerman steering):自動車の基本的なステアリング機構
  3. Synchronous drive (B21):タイミングベルトを使って、各車輪の動きを同期する機構
  4. XR4000 (はっきりと書いてないですが、4輪駆動ということだと思います)
  5. Mecanumwheels

運動モデルの考え方や数式は下記の資料に纏まっていますので、下記を参照ください。なお途中で出てくるHolonomicsとは、そのモデルの拘束条件が一般化座標と時間にのみ依存する性質を指します[2][3]。上記の1. のdifferentialモデルは、横に動けないといった構造上の制約があるため、Honolomicではない拘束を受けます。つまり、非Holonomic拘束を受けます。正しくはないですが理解の出助けとして、自由に動けるのがHolonomic拘束、そうでないのが非Holonomic拘束と考えるとイメージしやすいと思います。