CIT自律ロボット研究室

千葉工業大学 先進工学部 未来ロボティクス学科 上田隆一研究室

お知らせ: 2024年度の3年生ガイダンスのページを公開中． /  第5回自動運転AIチャレンジ2022で最優秀賞． /  研究室紹介動画を撮影しました． /  上田が日本機械学会教育賞を受賞しました． /  本研究室所属の3年生チームがAWS Robot Delivery Challengeで優勝しました． / 

確率ロボティクス2016第4回

Tue Oct 11 09:27:53 JST 2016 (modified: Fri Nov 29 17:23:39 JST 2019)
views: 970, keywords:

  このエントリーをはてなブックマークに追加 
   

確率ロボティクス

第4回

上田 隆一

2016年10月10日@千葉工業大学

前回・前々回のおさらいと今回

  • 前々回
    • 制御出力（移動）に伴う雑音の扱い
    • 信念に反映させる方法
  •  前回
    • センサ入力に乗る雑音の扱い
    • 信念に反映させる方法
  • 今回
    •  制御とセンサ計測を繰り返していくと信念もその都度更新されなければならない
      • ベイズフィルタ

ベイズフィルタ（の前に）

  • 前々回の制御出力の式と前回のセンサ入力の式を並べて記号を整理
    • 以下のように定義しましょう
      • 制御出力[latex]\\boldsymbol{u}_t [/latex]後の信念:
        • [latex]\\hat{bel}_t(\\boldsymbol{x}) = bel(\\boldsymbol{x} |\\boldsymbol{u}_{1:t},\\boldsymbol{z}_{1:t-1},bel_0)[/latex]
      • センサ入力[latex]\\boldsymbol{z}_t[/latex]後の信念:
        • [latex]bel_t(\\boldsymbol{x}) = bel(\\boldsymbol{x} |\\boldsymbol{u}_{1:t},\\boldsymbol{z}_{1:t},bel_0)[/latex]
      • ここで
        • [latex]bel_0[/latex]: 最初にエージェントが持つ信念
        • [latex]\\boldsymbol{u}_{1:t}[/latex]: 時刻[latex]1[/latex]から[latex]t[/latex]までの制御出力のシーケンス
        • [latex]\\boldsymbol{z}_{1:t}[/latex]: 時刻[latex]1[/latex]から[latex]t[/latex]までのセンサ入力のシーケンス

ベイズフィルタ

  • 制御出力後の変更（motion update）
    • [latex]\\hat{bel}_t(\\boldsymbol{x}) = \\int_\\mathcal{X} p(\\boldsymbol{x} | \\boldsymbol{x}',\\boldsymbol{u}_t )bel_{t-1}(\\boldsymbol{x}') d\\boldsymbol{x}'[/latex]
      • 式の意味: 状態が動いた後の信念は、その動きの予測の密度関数に、もとの信念の密度関数をかけて積分したもの
  • センサ入力後の更新（sensor update）
    • [latex]bel_t(\\boldsymbol{x}) = \\eta \\ell(\\boldsymbol{x}|\\boldsymbol{z}_t)\\hat{bel}_t(\\boldsymbol{x})[/latex]
      • 式の意味: センサから情報が入った後の信念は、そのセンサ値が分かったときの尤度関数と、もとの信念の密度関数をベイズの定理でかけたもの

拡張カルマンフィルタ

  • ベイズフィルタをガウス分布に限定したもの
    • これも前回、前々回の式をまとめると構成できる
  • 制御出力後の変更（motion update）
    • 移動後の[latex]\\hat{bel}_t[/latex]:
      • 中心: [latex]\\hat{\\boldsymbol{x}}_t = f(\\boldsymbol{\\bar{x}}_{t-1},\\boldsymbol{u}_t)[/latex]
        • 雑音を考慮しない状態方程式で出力前の推定の中心を移動させたもの
        • [latex]\\bar{x}_{t-1}[/latex]: [latex]bel_{t-1}[/latex]の中心
      • 共分散: [latex]\\hat\\Sigma_t = F_t\\bar\\Sigma_{t-1}F_t^T + R_t[/latex]
        • [latex]\\bar\\Sigma_{t-1}[/latex]: [latex]bel_{t-1}[/latex]の共分散
        • [latex]F_t[/latex]:[latex]\\bar\\Sigma_{t-1}[/latex]まわりで状態遷移関数[latex]f[/latex]から作ったヤコビ行列
        • [latex]R_t[/latex]: 移動の雑音の共分散行列（事前に計測しておく）

  • センサ入力後の更新（sensor update）
    • センサ入力後の[latex]bel_t(\\boldsymbol{x})[/latex]:
      • 中心: [latex]\\bar{\\boldsymbol{x}}_t = \\boldsymbol{\\hat{x}}_t + K(\\boldsymbol{z}_t - h(\\boldsymbol{\\hat{x}}_t)) [/latex]
        •  [latex]h(\\boldsymbol{x})[/latex]: [latex]\\boldsymbol{x}[/latex]で得られる観測値
        • [latex]K = \\hat\\Sigma_t H_t (H_t \\bar\\Sigma_t H_t^T + Q_t )^{-1}[/latex]（カルマンゲイン）
          • [latex]H_t[/latex]: [latex]h[/latex]を[latex]\\boldsymbol{\\hat{x}}_t[/latex]まわりで偏微分して得られるヤコビ行列
        • 意味: 新しい分布の中心（推定状態）は、もとの推定状態に、実際のセンサ入力ともとの推定状態で得られるはずのセンサ入力の差にカルマンゲインをかけた分だけ足したものとなる
      • 共分散行列: [latex]\\Sigma_t = (I - KH_t) \\hat\\Sigma_t[/latex]
        • 新たな推定の共分散行列は、カルマンゲインにヤコビ行列をかけた分の割合だけ小さくなる

ガウス分布に従わないときは？

  • 実際を表し、かつ計算ができる分布を頑張って見つける
    • 機械学習でよく行われる（共役事前分布）
      • ロボットの動きに関しては難しい場合が多い
  • 数値計算
    • 第２回に触れました
      • モンテカルロ法
      • 格子を使う方法

モンテカルロ法による[latex]bel[/latex]の計算

  • 発想: ロボットのシミュレーションをロボットの中で、一挙にたくさん並列で行う
  •  状態遷移のシミュレーション
    • 一つ時刻0の状態[latex]\\boldsymbol{x}_0[/latex]を選んで、制御出力に合わせて移動
      • 誤差を混入
    • 一つのシミュレーションの状態を、[latex]bel[/latex]からサンプリングされる標本（サンプル）とみなすことができる
      • 「粒子（パーティクル）」という言い方も
  • センサ入力の評価
    • 各パーティクルの姿勢とセンサ入力の妥当性をベイズの定理で評価

パーティクルの定義

  • [latex]\\Xi = \\{\\xi^{(i)} = (\\boldsymbol{x}^{(i)}, w^{(i)}) | i = 1,2,\\dots,N \\}[/latex]
  • パーティクルは次のように[latex]bel[/latex]を近似しなければならない
    • [latex]Bel(X) = \\int_{\\boldsymbol{x} \\in X} bel(\\boldsymbol{x}) d\\boldsymbol{x} \\approx \\sum_{i=1}^N \\delta(\\boldsymbol{x}^{(i)} \\in X) w^{(i)}[/latex]
      • [latex]Bel(X)[/latex]: [latex]X[/latex]の中に真の状態が存在する確率
        • 密度関数[latel]bel[/latex]を積分すると確率になる
      • [latex]\\forall X \\subset \\mathcal{X}[/latex]
      • [latex]\\delta[/latex]: カッコの中が真なら1、そうでなければ0

motion update

  • 各パーティクルを[latex]p(\\boldsymbol{x} | \\boldsymbol{x}_{t-1}, \\boldsymbol{u}_t)[/latex]に従って動かす
    • [latex]\\boldsymbol{x}^{(i)}_t \\sim p(\\boldsymbol{x} | \\boldsymbol{x}_{t-1}, \\boldsymbol{u}_t) \\qquad (i=1,2,\\dots,N)[/latex]
      • 時刻[latex]t[/latex]における[latex]i[/latex]番目のパーティクルの状態を、状態遷移の確率分布にしたがって一つランダムに選ぶ

sensor update

  • 各パーティクルの重みをベイズの定理で変える
    • [latex]w^{(i)}_t \\quad  \\texttt{*=} \\quad \\ell(\\boldsymbol{x}^{(i)}_t | \\boldsymbol{z}_t)  \\qquad (i=1,2,\\dots,N)[/latex]
      • もとの重みに尤度をかける
  • 重みの合計を1に正規化
    • [latex]\\eta = \\sum_{i=1}^N w^{(i)}_t [/latex]
    • [latex]w^{(i)}_t \\quad \\texttt{/=} \\quad \\eta  \\qquad (i=1,2,\\dots,N)[/latex]

その他の処理

  • パーティクルで[latex]bel[/latex]を近似しようとすると、いろいろ計算上の問題が出る
    • 特定のパーティクルだけ重みが1に近づいて他が0になると近似能力を喪失
      • [latex]\\rightarrow[/latex]リサンプリング
    • パーティクルの分布がずれて（外から見ていると）真の状態付近に戻ってこない
      • [latex]\\rightarrow[/latex]リセット

リサンプリング

  • パーティクルの重みをならす処理
    • 直感的な理解はこれでOK
      • 重みの大きいパーティクルを複数のパーティクルに
      • 重みの小さいパーティクルは消す
    • ただし、統計的になるべく正しく、なるべく速く
  • 方法

リセット

格子地図（離散状態）を用いる方法

  • 状態空間[latex]\\mathcal{X}[/latex]を離散化
    • [latex]\\mathcal{S} = \\{s_i | i=1,2,\\dots,N \\}[/latex]
      • [latex]i[/latex]: ここでは時刻ではなく離散状態の番号
      • [latex]\\forall s \\in \\mathcal{S}[/latex]は[latex]\\mathcal{X}[/latex]の部分空間
        • [latex]\\bigcup_{i=1}^N s_i = \\mathcal{X}[/latex]
        • [latex]s_i \\cap s_j = \\phi \\qquad \\forall i,j \\in \\{1,2,\\dots,N\\}[/latex]

  • 制御出力後の変更（motion update）
    • [latex]\\hat{Bel}_t(s_i) = \\sum_{j=1}^N P(s_i | s_j,\\boldsymbol{u}_t )Bel_{t-1}(s_j) [/latex]
  • センサ入力後の更新（sensor update）
    • [latex]Bel_t(s_i) = \\eta^{-1} L(s_i | \\boldsymbol{z}_t ) \\hat Bel_t(s_i) [/latex]
      • where [latex]\\eta = \\sum_{j=1}^N L(s_j | \\boldsymbol{z}_t ) \\hat Bel_t(s_j) [/latex]