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

確率ロボティクス入門 第2回 千葉工業大学 上田隆一 2024年5月8日 @中部大学

前回のおさらい • ロボットと確率 • 機械学習の分野でも制御の分野でも確率・統計が重要に • 世の中は複雑で予測が難しい • ロボットが習うべき動物は統計的に生きている • 悪く言えばいい加減に生きている 今回は制御の延長線上で確率を考える （確率ロボティクス） 買うといいと思う 2023年5月8日 ロボットフロンティア@中部大学 2

その前に余談：自由エネルギー原理 • フリストンが2006年に提唱した脳の情報理論 • （大雑把に言うと）脳のやってることは確率の計算 • 認識: 変分推論 • 行動: サプライズ最小化 • サプライズ: ベイズの定理の分母の小ささ 今回やります 2023年5月8日 ロボットフロンティア@中部大学 3

確率ロボティクス • 世間では自己位置推定やSLAMのイメージ • 実際：状態方程式、観測方程式を次の式で扱う制御理論 • 𝑝𝑝は「確率分布」を表す • 状態𝒙𝒙𝑡𝑡 も確率分布で表される（1つに決めつけない） • 𝒙𝒙𝑡𝑡 ~𝑏𝑏𝑡𝑡 (𝒙𝒙) なんか難しそうなのでぼんやり理解していきましょう （これも確率ロボティクス的なアプローチ） 2023年5月8日 ロボットフロンティア@中部大学 4

確率分布 • 未来のこと、分からないことを占うもの • 起こることを1つに決めないで何が何割で起こるか（=確率）示す • 起こることが無限にある場合は密度で示す • 密度: 積分したら確率になる値 • 表記:𝑃𝑃𝐴𝐴 𝒙𝒙 , 𝑝𝑝𝐴𝐴 𝒙𝒙 , 𝑃𝑃 𝒙𝒙 , 𝑝𝑝(𝒙𝒙)などなど（関数になっていればなんでも可） • 「𝒙𝒙は分布𝑃𝑃にしたがう」→ 𝑥𝑥~𝑃𝑃 𝑥𝑥 と表記 確率 Y 1/6 2023年5月8日 このへん にいそう 高密度 1 2 3 4 5 6 （普通の）サイコロの例 𝑥𝑥 ロボットフロンティア@中部大学 低密度 X （向きを考えないときの） ロボットの位置の例 5

状態を確率分布で考えるメリット • 決めつけないでよい • センサが信頼できない/情報を送ってこない場合 →「ここらへん」と表現可能 • ロボットに「分からない」 を分からせることが可能 • 行動を思いとどまらせる • 分からないことも含めて「知識」 • 真の賢さ？ ゴール このまま下がる とまずい！ 自著「詳解確率ロボティクス」より転載 どうやって計算するの？→これが難しい 2023年5月8日 ロボットフロンティア@中部大学 6

確率的自己位置推定 • どうするか？: 下図のような操作を繰り返す Y このへん にいそう ロボット が移動 戻る X 動くと位置が 不明確に センシング センサ値から 考えるとこのへんが 尤もらしい 情報の統合  この操作: 確率の計算 2023年5月8日 ロボットフロンティア@中部大学 7

確率分布の演算 ― 乗法定理・加法定理 • ふたつのルールしかない • 乗法定理: 𝑃𝑃 𝑥𝑥, 𝑦𝑦 = 𝑃𝑃 𝑥𝑥 𝑦𝑦 𝑃𝑃(𝑦𝑦) • 𝑃𝑃 𝑥𝑥 𝑦𝑦 は𝑦𝑦 が分かっているときに𝑥𝑥 となる確率を表す「条件付き確率」 • 例: • 𝑥𝑥: 猫が寝ているかどうか（布団にいる場合9/10，その他1/2） • 𝑦𝑦: 猫の居場所（布団1/2，その他1/2） • 𝑃𝑃 寝, 布団 = ？ • 加法定理: 𝑝𝑝 𝑥𝑥 = ∫ 𝑝𝑝 𝑥𝑥, 𝑦𝑦 d𝑦𝑦 • サイコロのように𝑥𝑥の値が連続でない（離散的な）場合は𝑃𝑃 𝑥𝑥 = ∑𝑦𝑦 𝑃𝑃 𝑥𝑥, 𝑦𝑦 • 上の例で，猫が寝ている確率は？ 2023年5月8日 ロボットフロンティア@中部大学 8

確率分布の演算 ― ベイズ定理 • 知識をアップデートするための定理 • 𝑃𝑃 𝑋𝑋 𝑌𝑌 𝑃𝑃(𝑌𝑌|𝑋𝑋)𝑃𝑃(𝑋𝑋) = 𝑃𝑃 𝑌𝑌 • 乗法定理𝑃𝑃 𝑋𝑋, 𝑌𝑌 = 𝑃𝑃 𝑋𝑋 𝑌𝑌 𝑃𝑃 𝑌𝑌 と𝑃𝑃 𝑋𝑋, 𝑌𝑌 = 𝑃𝑃 𝑌𝑌 𝑋𝑋 𝑃𝑃(𝑋𝑋)から • 知能を考える上でとても重要 • 何かに対して𝑃𝑃 𝑋𝑋 と考えていたら情報𝑌𝑌が得られて𝑃𝑃 𝑋𝑋|𝑌𝑌 に • 𝑃𝑃 𝑋𝑋 : 事前確率 • 𝑃𝑃 𝑋𝑋|𝑌𝑌 : 事後確率 日常でよくある 余談: 事後確率は決して事前確率と 同じ形に戻らない（記憶は消せない） 2023年5月8日 ロボットフロンティア@中部大学 9

確率分布の演算 ― 期待値 • 𝑥𝑥~𝑃𝑃 𝑥𝑥 のときの、ある関数𝑓𝑓(𝑥𝑥)の値の見積もり • < 𝑓𝑓(𝑥𝑥) >𝑝𝑝(𝑥𝑥) と表記 •例 • < 𝑓𝑓 𝑥𝑥 >𝑝𝑝 𝑥𝑥 = ∫ 𝑓𝑓 𝑥𝑥 𝑝𝑝(𝑥𝑥)𝑑𝑑𝑑𝑑 • < 𝑓𝑓 𝑥𝑥 >𝑃𝑃 𝑥𝑥 = ∑ 𝑓𝑓 𝑥𝑥 𝑃𝑃(𝑥𝑥) 離散・連続で同じ表記に なるので便利 • 例: 400円払ってさいころを1回振り、 出目の100倍の金額をもらえる賭けの期待値は？ • まず普通に計算して、そのあと上の式に当てはめてみましょう • 加法定理の変形 • 𝑃𝑃 𝑥𝑥 = ∫ 𝑝𝑝 𝑥𝑥, 𝑦𝑦 d𝑦𝑦 = ∫ 𝑝𝑝 𝑥𝑥 𝑦𝑦 𝑝𝑝(𝑦𝑦)d𝑦𝑦 =< 𝑝𝑝 𝑥𝑥 𝑦𝑦 >𝑝𝑝(𝑦𝑦) 2023年5月8日 ロボットフロンティア@中部大学 10

自己位置に対する知識の表現—初期化 • 「最初」の自己位置: 𝑝𝑝 𝒙𝒙 実世界 • 一様分布に • なにも知らない状態 • 多くの場合，ロボットは初期位置𝒙𝒙0 を教えられる • 𝑝𝑝 𝒙𝒙 |𝒙𝒙0 • このあと，動いたりセンシング したりで「 |」の右側に情報が 増えていく • 次ページ 2023年5月8日 p p ロボットフロンティア@中部大学 分布𝑝𝑝 𝒙𝒙 分布𝑝𝑝 𝒙𝒙 |𝒙𝒙0 X X X 11

自己位置に対する知識の表現—演算 • 次の時刻にロボットが動く • ロボットが得る情報: 自分の出したモータの出力 𝒖𝒖1 • →分布: 𝑝𝑝 𝒙𝒙 |𝒙𝒙0 , 𝒖𝒖1 • 移動後，センシング • ロボットが得る情報:センサ情報 𝐳𝐳1 • →分布: 𝑝𝑝 𝒙𝒙 |𝒙𝒙0 , 𝒖𝒖1 , 𝐳𝐳1 • 𝑏𝑏という記号を与えましょう • 𝑏𝑏0 𝒙𝒙 = 𝑝𝑝 𝒙𝒙 |𝒙𝒙0 • 𝑏𝑏�𝑡𝑡 𝒙𝒙 = 𝑝𝑝 𝒙𝒙 |𝒙𝒙0 , 𝒖𝒖1:𝑡𝑡 , 𝐳𝐳1:𝑡𝑡−1 • 𝑏𝑏𝑡𝑡 𝒙𝒙 = 𝑝𝑝 𝒙𝒙 |𝒙𝒙0 , 𝒖𝒖1:𝑡𝑡 , 𝐳𝐳1:𝑡𝑡 • 𝑏𝑏: belief（信念）の頭文字 • 以後，繰り返し • 分布: 𝑝𝑝 𝒙𝒙 |𝒙𝒙0 , 𝒖𝒖1 , 𝐳𝐳1 , 𝒖𝒖2 , 𝐳𝐳2 , 𝒖𝒖3 , 𝐳𝐳3 , … • 略記: 𝑝𝑝 𝒙𝒙 |𝒙𝒙0 , 𝒖𝒖1:𝑡𝑡 , 𝐳𝐳1:𝑡𝑡 2023年5月8日 ロボットフロンティア@中部大学 注意: 単にどう表記するか という話しかしていない 12

移動に関する計算（の前に図解） • ロボットが移動すると分布も移動 • 下: 1次元の例 • 右: 2次元平面の例 移動 移動後の信念分布 X p 想定される動きの雑音 移動 にあわせて分布を拡大 X 2023年5月8日 移動前の信念分布 「詳解確率ロボティクス」[上田2019]より転載，改変 （細かい説明はここでは無視しましょう） ロボットフロンティア@中部大学 13

移動に関する計算 • 𝑏𝑏𝑡𝑡−1 𝒙𝒙 から𝑏𝑏�𝑡𝑡 𝒙𝒙 を導出 • 𝑏𝑏𝑡𝑡−1 𝒙𝒙 = 𝑝𝑝 𝒙𝒙 |𝒙𝒙0 , 𝒖𝒖1:𝑡𝑡−1 , 𝐳𝐳1:𝑡𝑡−1 • 𝑏𝑏�𝑡𝑡 𝒙𝒙 = 𝑝𝑝 𝒙𝒙 |𝒙𝒙0 , 𝒖𝒖1:𝑡𝑡 , 𝐳𝐳1:𝑡𝑡−1 • 次のように変形 • 𝑏𝑏�𝑡𝑡 𝒙𝒙 = 𝑝𝑝 𝒙𝒙 |𝒙𝒙0 , 𝒖𝒖1:𝑡𝑡 , 𝐳𝐳1:𝑡𝑡−1 = ∫ 𝑝𝑝 𝒙𝒙, 𝒙𝒙𝑡𝑡−1 |𝒙𝒙0 , 𝒖𝒖1:𝑡𝑡 , 𝐳𝐳1:𝑡𝑡−1 d𝒙𝒙𝑡𝑡−1 = ∫ 𝑝𝑝 𝒙𝒙|𝒙𝒙𝑡𝑡−1 , 𝒙𝒙0 , 𝒖𝒖1:𝑡𝑡 , 𝐳𝐳1:𝑡𝑡−1 𝑝𝑝 𝒙𝒙𝑡𝑡−1 |𝒙𝒙0 , 𝒖𝒖1:𝑡𝑡 , 𝐳𝐳1:𝑡𝑡−1 d𝒙𝒙𝑡𝑡−1 = ∫ 𝑝𝑝 𝒙𝒙 |𝒙𝒙𝑡𝑡−1 , 𝒖𝒖𝑡𝑡 𝑝𝑝 𝒙𝒙𝑡𝑡−1 |𝒙𝒙0 , 𝒖𝒖𝑡𝑡−1 , 𝐳𝐳1:𝑡𝑡−1 d𝒙𝒙𝑡𝑡−1 = ∫ 𝑝𝑝 𝒙𝒙 |𝒙𝒙𝑡𝑡−1 , 𝒖𝒖𝑡𝑡 𝑏𝑏𝑡𝑡−1 𝒙𝒙𝑡𝑡−1 d𝒙𝒙𝑡𝑡−1 = 𝑝𝑝 𝒙𝒙 |𝒙𝒙′ , 𝒖𝒖𝑡𝑡 𝑏𝑏𝑡𝑡−1 (𝒙𝒙′) • 計算に必要な情報: 状態遷移モデル 𝑝𝑝 𝒙𝒙 |𝒙𝒙′ , 𝒖𝒖 • 𝒙𝒙′ でモータの出力が𝒖𝒖だったら次の時刻にどこにいくかを示す確率分布 • 実験や経験から求めておく 2023年5月8日 ロボットフロンティア@中部大学 14

センサ情報（以後「観測」）に関する計算 • 計算: 𝑏𝑏�𝑡𝑡 𝒙𝒙 から𝑏𝑏𝑡𝑡 𝒙𝒙 を導出 • 𝑏𝑏�𝑡𝑡 𝒙𝒙 = 𝑝𝑝 𝒙𝒙 |𝒙𝒙0 , 𝒖𝒖1:𝑡𝑡 , 𝐳𝐳1:𝑡𝑡−1 • 𝑏𝑏𝑡𝑡 𝒙𝒙 = 𝑝𝑝 𝒙𝒙 |𝒙𝒙0 , 𝒖𝒖1:𝑡𝑡 , 𝐳𝐳1:𝑡𝑡 • 増えた情報𝐳𝐳𝑡𝑡 を利用して ベイズの定理を適用 𝑝𝑝 𝐳𝐳𝑡𝑡 |𝒙𝒙 𝑏𝑏� 𝑡𝑡 𝒙𝒙 � • 𝑏𝑏𝑡𝑡 𝒙𝒙 = 𝑏𝑏𝑡𝑡 𝒙𝒙|𝐳𝐳𝑡𝑡 = = η𝑝𝑝 𝐳𝐳𝑡𝑡 |𝒙𝒙 𝑏𝑏�𝑡𝑡 𝒙𝒙 [上田2019]より転載 （細かい説明はここでは無視しましょう） 𝑝𝑝 𝐳𝐳𝑡𝑡 • ηは𝑏𝑏𝑡𝑡 の積分を1にする定数と考えてよい（正規化定数） • しかし、 𝑝𝑝 𝐳𝐳𝑡𝑡 |𝒙𝒙 はどこから持ってくる？ 2023年5月8日 ロボットフロンティア@中部大学 15

𝑝𝑝 𝐳𝐳𝑡𝑡 |𝒙𝒙 の例 — LiDARの情報から作る尤度場 • 地図と計測結果を照合して点数（尤度）をつける ある瞬間のLiDARの計測値 ロボットの位置を 仮定して，黒2点， 灰1点で点数を つけてみる． 地図（黒: 壁の位置，灰: 壁周辺） 2023年5月10日 仮定2 仮定1 14点（黒: 6，灰: 2） 7点（黒: 1，灰: 5） 仮定1のほうが，仮定2より 2倍「尤もらしい」と考える． 𝒙𝒙が変数なので、 𝑝𝑝 𝐳𝐳𝑡𝑡 |𝒙𝒙 = 𝐿𝐿 𝒙𝒙|𝐳𝐳𝑡𝑡 と表記 ロボットフロンティア@中部大学 16

尤度𝐿𝐿(𝒙𝒙|𝐳𝐳) センサ情報から考えて，どこにロボットがいそうかを 数値化したもの • （適切に設計できれば）「センサの前を人などが横切る」， 「観測対象が欠けて小さく見える」などの，大きな誤差を考慮可能 先ほどのLiDARの計測 実は人がいた 人がいなくても尤度は さほど変わらない （14点と16点） • 従来の誤差の概念に縛られない • 「計測して誤差を考慮する」だけではなく、あらゆる情報を位置情報に変換可能 2023年5月10日 ロボットフロンティア@中部大学 17

これまでのまとめ • 導出は難しいが，以下を知っていると 自己位置推定の計算が可能 • 初期値𝑏𝑏0 𝒙𝒙 = 𝑝𝑝 𝒙𝒙 |𝒙𝒙0 を準備 • 𝒙𝒙0 がない場合もある 「ベイズフィルタ」 と呼ばれる手続き • 以下，繰り返し • ロボットが動く: 𝑏𝑏�𝑡𝑡 𝒙𝒙 = 𝑝𝑝 𝒙𝒙 |𝒙𝒙′ , 𝒖𝒖𝑡𝑡 𝑏𝑏𝑡𝑡−1 (𝒙𝒙′) • ロボットがセンシング: 𝑏𝑏𝑡𝑡 𝒙𝒙 = η𝐿𝐿(𝒙𝒙|𝐳𝐳𝑡𝑡 )𝑏𝑏�𝑡𝑡 𝒙𝒙 どうやってプログラムにするのか？ 2023年5月8日 ロボットフロンティア@中部大学 18

カルマンフィルタによる自己位置推定 • 多変量「ガウス分布」に確率分布を制限して ベイズフィルタを実装 • ガウス分布: 正規分布とも呼ばれる • 信念の表現 • 𝑏𝑏𝑡𝑡 𝒙𝒙 = ηexp[− 1 2 𝒙𝒙𝑡𝑡 − 𝝁𝝁𝑡𝑡 ⊤ Σ𝑡𝑡−1 𝒙𝒙𝑡𝑡 − 𝝁𝝁𝑡𝑡 ] • 適切な近似で指数部の計算だけに • 移動時: Σ� 𝑡𝑡 = 𝐹𝐹𝑡𝑡 Σ𝑡𝑡−1 𝐹𝐹𝑡𝑡⊤ + 𝐴𝐴𝑡𝑡 𝑀𝑀𝑡𝑡 𝐴𝐴⊤𝑡𝑡 • 観測時: • Σ𝑡𝑡 = (𝐼𝐼 − 𝐾𝐾𝑡𝑡 𝐻𝐻𝑡𝑡 )Σ� 𝑡𝑡 � 𝑡𝑡 � 𝑡𝑡 + 𝝁𝝁 • 𝝁𝝁𝑡𝑡 = 𝐾𝐾𝑡𝑡 𝒛𝒛𝑡𝑡 − 𝒉𝒉 𝝁𝝁 • ここで𝐾𝐾𝑡𝑡 = Σ� 𝑡𝑡 𝐻𝐻𝑡𝑡⊤ (𝐻𝐻𝑡𝑡 Σ� 𝑡𝑡 𝐻𝐻𝑡𝑡⊤ + 𝑄𝑄𝑡𝑡 )−1 2023年5月8日 ロボットフロンティア@中部大学 2次元のガウス分布 by Piotrg~commonswiki https://commons.wikimedia.org/w iki/File:Multivariate_Gaussian.png CC BY-SA 3.0 19

• https://commons.wikimedia.org/wiki/File:Multivariate_Gaussian.png

カルマンフィルタによる自己位置推定の例 • 左: 観測なし • 移動によって位置・向きの不確かさが増加していく • 右: ★の向きと距離を計測して信念分布に反映 カルマンフィルタはアポロ計画でも 使われた（あとで見ておいてください） https://www.youtube.com/watch?v=aNzGCMRnvXQ 2023年5月8日 ロボットフロンティア@中部大学 20

• https://www.youtube.com/watch?v=aNzGCMRnvXQ

カルマンフィルタの限界 • うまく実装すれば機能するが・・・ • 限界が存在 • 向きを持つロボットが移動する際には， 信念分布の形状はガウス分布にならない • 尤度関数もガウス分布と 同じような形状にならない • 「複数の候補」が表現できない • ロボットのいる環境は空中と違って複雑 2023年5月8日 ロボットフロンティア@中部大学 [上田2019]より [上田2019]より 21

分身を使った自己位置推定 • 右のようなシミュレーションでできる図を そのまま利用できないか？ • いくつもロボットの分身を動かす • 分身の分布（偏在具合）を 信念分布の近似とみなす • 移動に関する具体的な操作 • 初期の信念分布（位置・向きを教えてもらえる場合）: • 分身を教えてもらった位置・向きに配置 • 移動: 状態遷移モデル 𝑝𝑝 𝒙𝒙 |𝒙𝒙′ , 𝒖𝒖𝑡𝑡 にしたがって動かす 2023年5月8日 ロボットフロンティア@中部大学 22

数学的な根拠 • 「ドローイング」 • 確率分布にしたがってひとつ値を取り出す行為 • 𝑥𝑥~𝑃𝑃(𝑥𝑥) • ドローイングを繰り返して集めたデータ（標本，サンプル）は， 元の分布の近似 • 例: さいころを何度も振って出目の統計をとると， 頻度の割合が1/6に近づく サンプルで元の分布を近似可能 頻度 𝑃𝑃 1/6 何度（ 𝑁𝑁 回としましょう） もさいころを振って各目の 出た回数を記録 1 2 3 4 5 6 2023年5月8日 𝑥𝑥 つまり𝑥𝑥~𝑃𝑃(𝑥𝑥)の繰り返し ロボットフロンティア@中部大学 𝑁𝑁/6 1 2 3 4 5 6 23 𝑥𝑥

尤度関数の反映 • 各分身に尤度の変数を持たせる • この場合の尤度: 各分身の 位置・向きの尤もらしさ • そのまま尤度関数の考え方を適用可能 • 演算 • ある分身の尤度を𝑤𝑤とする • 尤度ではあるが「重み」と表現 • センサ情報が入ると，次のように更新 • 𝑤𝑤 ⟵ 𝑤𝑤𝑤𝑤(𝒙𝒙|𝐳𝐳) 2023年5月8日 分身2 分身1 14点（黒: 6，灰: 2） 7点（黒: 1，灰: 5） ロボットフロンティア@中部大学 分身1のほうが，分身2より 2倍「尤もらしい」と考える． （前回の図とほぼ同じ） 24

観測を重ねることによる重みの変化 • 尤もらしい分身は，次の観測でも高い尤度になりやすい →ほとんどの分身の重みが相対的に0に 例: 矢印の長さが重み →1つの分身の重みだけ 大きくなってあとはゼロ 2023年5月8日 ロボットフロンティア@中部大学 25

リサンプリング • 重みの大きい分身を増やす • 方法 • パーティクルの重みを積み上げたリストを作成 • 1/𝑁𝑁ずつリストを進んで分身を𝑁𝑁個コピー • コピーで作った分身の重みを1/𝑁𝑁にして新たな分身の集団を作成 • 元の分身の集団は破棄 公平になるように，乱数で 開始位置を少しずらす 2023年5月8日 [上田2019]より ロボットフロンティア@中部大学 26

分身を使った自己位置推定の完成 • 別名: • パーティクルフィルタ，粒子フィルタ（粒子: 分身のこと） • MCL（Monte Carlo localization） https://www.youtube.com/watch?v=mGkNZUbasXo より．https://github.com/ryuichiueda/emclを使用 2023年5月8日 ロボットフロンティア@中部大学 27

• https://www.youtube.com/watch?v=mGkNZUbasXo
• https://github.com/ryuichiueda/emcl

分身を使った自己位置推定 →パーティクルフィルタ • 𝑁𝑁個あるうちの𝑖𝑖番目のパーティクルを次のように表現 (𝑖𝑖) (𝑖𝑖) 𝑖𝑖 • 𝜉𝜉𝑡𝑡 = (𝒙𝒙𝑡𝑡 , 𝑤𝑤𝑡𝑡 ) • 手続き （𝑖𝑖 = 0, 1, 2, … , 𝑁𝑁 − 1） • 移動時: 𝒙𝒙𝑡𝑡 ~𝑝𝑝 𝒙𝒙 |𝒙𝒙𝑡𝑡−1 , 𝒖𝒖𝑡𝑡 𝑖𝑖 𝑖𝑖 • 観測時: 𝑤𝑤𝑡𝑡 ⟵ 𝑤𝑤𝑡𝑡 𝐿𝐿(𝒙𝒙𝑡𝑡 |𝐳𝐳𝑡𝑡 ) • その後，移動直前にリサンプリング (𝑖𝑖) 2023年5月8日 (𝑖𝑖) 𝑖𝑖 ロボットフロンティア@中部大学 28

まとめ • 確率分布を各自の知識と考える • 人工知能（つまり主観を持つ何か）を考えるうえで必須と考えられる • ベイズの定理 • 知識のアップデート方法 • 尤度関数を使った自己位置推定（ベイズフィルタ） • 2種類の実装を紹介（いずれも近似計算） • カルマンフィルタ • パーティクルフィルタ 2023年5月8日 ロボットフロンティア@中部大学 29