robot frontier lesson3

確率ロボティクス入門第3回

上田隆一

本日の話の流れ

• SLAM

– SLAMとは何か

• SLAM手法の例

–よく使われるものと最新のもの

• graph-based SLAM

–理論

May 24, 2017 ロボットフロンティア第3回@中部大学 2

SLAM


SLAM（simultaneous localization and mapping）

• こういう問題

– 自己位置推定の問題について、ランドマークの位置を未知に

– ロボットの初期姿勢𝒙0を原点に置く

– ロボットの姿勢とランドマークの位置（姿勢）を両方求めてください

• 可能なのか？→ 可能


人間（素人）の平面地図の作り方

• 作り方1

– 白い紙を用意

– 見えたものを地図に書く

– 歩く

– 見えたものを前に書いたものと矛盾なく書く

– 以後繰り返し

• 作り方2

– 散歩して見えたものを覚えておいて後からまとめて書く


そんなに難しくない

ロボットに地図を作らせる

• 課題

–見たものを矛盾なくつなぎ合わせる→大変

• どれくらい大変か

– ここ20年間、ロボティクス分野で一番数学のできる連中の扱う課題のひとつだった

–今は使う・理解するのはさほど難しくない

• 計算機、センサの進化

• 研究成果

• ROSを中心としたモジュール化


SLAMの手法の例


分類

• オンライン/オフライン

– ロボットの動作中に地図を作るか、後で作るか

• ランドマークベース/格子地図ベース

• 2D/3D: 平面か立体か

• 移動モデルへの依存度

• ・・・


FastSLAM [Montemerlo 2003]

• MCLのパーティクル一つ一つに地図を持たせる

– 地図の種類

• その1: 点ランドマークの位置リスト

• その2: 占有格子地図（動画）

– 2次元平面を格子状に区切り壁の有無を記録したもの

• アルゴリズム

– 移動でパーティクルを移動

– センサの値と観測モデル、地図を比較して重みを変更

– 地図をセンサの値に基づいて変更


最近のもの

• CNN-SLAM [Tateno 2017]

• 頭にVelodyneをつけて走る [Zhang 2017]

• rtabmap

–上田研でもよく動かしてます

• 傾向

– 3Dのものはだんだん移動モデルを使わなくなってきているようだ

• 画像処理重視に

– 2DのFastSLAMもまだまだ現役

• 結局、移動ロボットを動かすには十分な場合が多いMay 24, 2017 ロボットフロンティア第3回@中部大学 10

https://www.youtube.com/watch?v=z_NJxbkQnBU

https://www.youtube.com/watch?v=27BsTkaGcjU

http://wiki.ros.org/rtabmap

graph-based SLAM

• 情報フィルタを用いた最適化手法

• 他のものにも応用が利く

• 本日扱うアルゴリズム

– この解説 [Grisetti 2010]の数式を簡単な例に適用したもの

– サンプルコード• https://github.com/ryuichiueda/probrobo_practice/blob/master/gra

ph-based_SLAM/graph-based_slam.ipynb

– 数式を交えた解説も書きました• https://github.com/ryuichiueda/commentary_on_graph-

based_slam/blob/master/commentary_on_graph-based_slam.pdf


http://ieeexplore.ieee.org/document/5681215/

https://github.com/ryuichiueda/probrobo_practice/blob/master/graph-based_SLAM/graph-based_slam.ipynb

https://github.com/ryuichiueda/commentary_on_graph-based_slam/blob/master/commentary_on_graph-based_slam.pdf

GRAPH-BASED SLAM


問題• ロボットがパス（姿勢遷移） 𝒙0:𝑇で移動したと思っている

– 図の灰色の矢印

– 実際のパス（赤色の矢印）とずれている

• 各姿勢でランドマークを観測

– 得られる値のシーケンス: 𝒛0:𝑇（右図の桃色の線）

• 𝒛 = (𝑑, 𝜑, 𝜓)

– 𝑑: ロボットからの距離

– 𝜑:見える方角

– 𝜓: ランドマークの向き（問題を簡単にするため追加）


問題（続き）

• 問題:– 𝒙0:𝑇と𝒛0:𝑇と観測モデル𝑝 𝒛|𝒙 から真のパス𝒙0:𝑇

∗ を推定してみましょう

– つまり、右の図の桃色の線がランドマークのところに合うように、灰色の矢印の位置を修正する問題

– 𝒛0:𝑇に雑音が混入しているので完全には一致しない

– 最適化問題を適切に定義して解く


解き方の例

A) 各時刻の姿勢をグラフのノードに

B) 同じランドマークを見た2姿勢をエッジで結ぶ

– 移動履歴から計算される姿勢と観測から計算される姿勢に差

C) 差を解消するようにノードを初期値から移動

– エッジが互いに矛盾することも

– 観測値の信頼性で差に重み付けしなければならない


図: 拘束のグラフ（互いに歪み）

𝒙𝑡 𝒙𝑡′

𝑧𝑡 𝑧𝑡′

図: 2つの観測から2姿勢間の位置関係を計算

2方式で計算される姿勢の差

• 移動の履歴から計算される相対位置: 𝒙𝑡′ − 𝒙𝑡– ここでは移動の誤差は考えない

– 当然誤差がある

• 観測𝑧𝑡, 𝑧𝑡′から計算される相対位置: 𝝁𝑐,𝑡,𝑡′

– 𝑐はランドマークのID

– 誤差の考慮は後で

• 二つの情報のズレ

– 𝒆𝑐,𝑡,𝑡′ = 𝒙𝑡′ − 𝒙𝑡 − 𝝁𝑐,𝑡,𝑡′




𝒆𝑐,𝑡,𝑡′の計算

• 𝒆𝑐,𝑡,𝑡′ = 𝒙𝑡′ − 𝒙𝑡 − 𝝁𝑐,𝑡,𝑡′

=

𝑥𝑡′ − 𝑥𝑡 − 𝑑𝑐,𝑡cos 𝜃𝑡 + 𝜑𝑐,𝑡 +𝑑𝑐,𝑡′ cos 𝜃𝑡′ + 𝜑𝑐,𝑡′

𝑦𝑡′ − 𝑦𝑡 − 𝑑𝑐,𝑡sin 𝜃𝑡 + 𝜑𝑐,𝑡 +𝑑𝑐,𝑡′ sin 𝜃𝑡′ + 𝜑𝑐,𝑡′

𝜃𝑡′ − 𝜃𝑡 −𝜓𝑐,𝑡 + 𝜓𝑐,𝑡′


𝒙𝑡𝑡′を少し動かしたときの𝒆𝑐,𝑡,𝑡′の変化

• 後から計算で使います

• 𝒆𝑐,𝑡,𝑡′ 𝒙𝑡𝑡′ + ∆𝒙 = 𝒆𝑐,𝑡,𝑡′ 𝒙𝑡𝑡′ +𝝏𝒆

𝑐,𝑡,𝑡′

𝝏𝒙|𝒙=𝒙

𝑡𝑡′∆𝒙

•𝝏𝒆

𝑐,𝑡,𝑡′

𝝏𝒙|𝒙=𝒙

𝑡𝑡′= [𝐴𝑐,𝑡,𝑡′ 𝐵𝑐,𝑡,𝑡′]とすると

• 𝐴𝑐,𝑡,𝑡′ =

−1 0 𝑑𝑐,𝑡sin 𝜃𝑡 + 𝜑𝑐,𝑡

0 −1 −𝑑𝑐,𝑡cos 𝜃𝑡 + 𝜑𝑐,𝑡

0 0 −1

• 𝐵𝑐,𝑡,𝑡′ =

1 0 −𝑑𝑐,𝑡′sin(𝜃𝑡′ + 𝜑𝑐,𝑡′)

0 1 𝑑𝑐,𝑡′cos(𝜃𝑡′ + 𝜑𝑐,𝑡′)

0 0 1


エッジの歪み• 𝒆𝑐,𝑡,𝑡′の大きさだけでは評価できない

– ランドマークの計測値が信頼できる: 𝒆𝑐,𝑡,𝑡′が大きいと問題

– 信頼性がないなら𝒆𝑐,𝑡,𝑡′が大きくてもさほど問題でない

– マハラノビス距離 𝒆𝑐,𝑡,𝑡′𝑇 Ω𝑐,𝑡,𝑡′𝒆𝑐,𝑡,𝑡′ で評価

• 情報行列: Ω𝑐,𝑡,𝑡′

– 共分散行列の逆行列

– この場合の共分散行列: 𝛴𝑐,𝑡,𝑡′ = 𝑅𝑐,𝑡𝛴𝑐,𝑡𝑅𝑐,𝑡𝑇 + 𝑅𝑐,𝑡′𝛴𝑐,𝑡′𝑅𝑐,𝑡′

𝑇

• 𝑅は回転行列

• 計測値のばらつきに関する共分散行列を回転して和をとったもの




𝛴𝑐,𝑡𝛴𝑐,𝑡′

グラフ全体の歪み• 𝐹 𝒙0:𝑇 = 𝒆𝑐,𝑡,𝑡′∈ℰ

𝒆𝑐,𝑡,𝑡′𝑇 Ω𝑐,𝑡,𝑡′𝒆𝑐,𝑡,𝑡′

– ℰ: 2姿勢間の誤差の集合

– 𝒙0:𝑇: 初期姿勢から最後の姿勢までの姿勢の集合

– 評価関数𝐹を最小にする𝒙0:𝑇を求める（ 𝒙0:𝑇 を最適な場所に動かす）

• どの方向に各𝒙𝑡を動かすのか？May 24, 2017 ロボットフロンティア第3回@中部大学 20

解法

• 𝐹 𝒙0:𝑇 + ∆𝒙0:𝑇 が最小になる変化量∆𝒙0:𝑇を求めると良いが、どうやって解くのか？

– 以下、証明は無しでアルゴリズムだけ示します

• 情報行列𝐻と情報ベクトル𝒃を準備

– 𝐻: 3𝑇 + 1 × 3𝑇 + 1

– 𝒃: (3𝑇 + 1)次元

– 3𝑇 + 1: 𝒙0:𝑇の全変数の数• ただしここでは観測のない時刻の姿勢を𝒙0:𝑇から除外

• ある手続きで𝐻, 𝒃を作ると∆𝒙0:𝑇が以下の式で求まる

– ∆𝒙0:𝑇 = 𝐻−1𝒃


𝐻, 𝒃の計算（1）

• 𝐻の 𝒙𝑡と𝒙𝑡′に対応する部分を𝐻[𝑡𝑡′]、

𝒃の𝒙𝑡に対応する部分を𝒃[𝑡]と表す

– 𝐻 =

𝐻[00] ⋯ 𝐻[0𝑇]

⋮ ⋱ ⋮𝐻[𝑇0] ⋯ 𝐻[𝑇𝑇]

, 𝒃 =

𝒃[0]⋮

𝒃[𝑇]

• 初期化

– 𝐻, 𝒃の全要素を0で初期化

– ただし𝐻[00]の部分だけ大きな数をかけた単位行列で初期化

• 座標を固定するため



• 各エッジの歪みの情報を足しこむ

– 𝐻[𝑡𝑡] += 𝐴𝑐,𝑡,𝑡′𝑇 Ω𝑐,𝑡,𝑡′𝐴𝑐,𝑡,𝑡′

– 𝐻[𝑡𝑡′] += 𝐴𝑐,𝑡,𝑡′𝑇 Ω𝑐,𝑡,𝑡′𝐵𝑐,𝑡,𝑡′

– 𝐻[𝑡′𝑡] += 𝐵𝑐,𝑡,𝑡′𝑇 Ω𝑐,𝑡,𝑡′𝐴𝑐,𝑡,𝑡′

– 𝐻[𝑡′𝑡′] += 𝐵𝑐,𝑡,𝑡′𝑇 Ω𝑐,𝑡,𝑡′𝐵𝑐,𝑡,𝑡′

– 𝒃[𝑡] += 𝐴𝑐,𝑡,𝑡′𝑇 Ω𝑐,𝑡,𝑡′𝒆𝑐,𝑡,𝑡′

– 𝒃[𝑡′] += 𝐵𝑐,𝑡,𝑡′𝑇 Ω𝑐,𝑡,𝑡′𝒆𝑐,𝑡,𝑡′

• 大雑把に考えると、各エッジについて、マハラノビス距離の変化量を足しているイメージ


𝐴𝑐,𝑡,𝑡′ , 𝐵𝑐,𝑡,𝑡′のおさらい:𝝏𝒆𝑐,𝑡,𝑡′

𝝏𝒙|𝒙=𝒙

𝑡𝑡′= [𝐴𝑐,𝑡,𝑡′ 𝐵𝑐,𝑡,𝑡′]


• 𝒙0:𝑇を∆𝒙0:𝑇だけ動かして更新

⦁ 𝒙0:𝑇 += 𝐻−1𝒃

• 以上の手続きを収束まで何回か繰り返す

– 線形化の際に誤差があるので1回で終わらない


実行例


初期値収束後

アニメーション

ロボットの動作と観測

• 青の点: 𝒆𝑐,𝑡,𝑡′から導かれる

𝒙𝑡から見たときの 𝒙𝑡′の姿勢• 楕円: 𝚺𝑐,𝑡,𝑡′

まとめ

• SLAM

–センサデータを貼り合わせて地図を作成

• 様々な確率的な手法、最適化手法

–高度な画像処理に基づくものが最近の研究のトレンド

• 基本は最適化問題を作って解くというものなので、根本の理屈を抑えることが重要です。

• graph-based SLAM

–情報フィルタを用いた最適化


robot frontier lesson3

Technology