CIT自律ロボット研究室

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

お知らせ: 本研究室所属の3年生チームがAWS Robot Delivery Challengeで優勝しました / 

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

Tue Sep 27 10:31:11 JST 2016 (modified: Fri Nov 29 17:24:59 JST 2019)
views: 172, keywords:

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

確率ロボティクス

第3回

上田 隆一

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

今日の内容

  • センサ値と状態、信念

前回

  • 制御出力[latex]\\boldsymbol{u}_t[/latex]で状態[latex]\\boldsymbol{x}_{t-1}[/latex]から[latex]\\boldsymbol{x}_t[/latex]に変化
  • 自律ロボット(エージェント)にとって、状態[latex]\\boldsymbol{x}_t[/latex]は、未知
  • エージェントは状態[latex]\\boldsymbol{x}_t[/latex]に対する認識である信念[latex]bel_t[/latex]を持つ
  • [latex]bel_t[/latex]には、制御出力に対する実際の動きの雑音が蓄積していく
    • ガウス分布に従う場合は共分散行列に雑音が足されていく

状態を(間接的に)観測する

  • センサの値には状態に対するヒント(情報)が隠れている
  • センサ入力の定義
    • [latex]\\boldsymbol{z} = (z_1,z_2,\\dots,z_m) \\in \\mathcal{Z}[/latex]
  • 多くの場合、情報は間接的
    • レーザレンジファインダーは移動ロボットの [latex]\\boldsymbol{x} = (x,y,\\theta)[/latex]を直接は教えてくれない
      • 変数が違う([latex]\\mathcal{X} \\neq \\mathcal{Z}[/latex])
      • 雑音もある

観測方程式

  • センサ入力と状態を結びつける式
    • 現代制御に出てくる
  • 線形な場合
    • [latex]\\boldsymbol{z} = C\\boldsymbol{x} + \\delta[/latex]
      • 時刻の添字[latex]t[/latex]は省略
      • [latex]\\delta[/latex]: センサ入力の値に混入する雑音
  • 非線形な場合
    • [latex]\\boldsymbol{z} = h(\\boldsymbol{x}) + \\delta[/latex]
  • 前回の制御のときと同様、このような定式化をしてしまうと 観測にまつわる不確かさの表現力に乏しい

観測にまつわる不確かさの表現

  • [latex]p(\\boldsymbol{z}|\\boldsymbol{x})[/latex]: 状態[latex]\\boldsymbol{x}[/latex]にいた場合に、[latex]\\boldsymbol{z}[/latex]というセンサ入力を得る確率の密度
  • [latex]\\ell(\\boldsymbol{x}|\\boldsymbol{z})[/latex](尤度):センサ入力[latex]\\boldsymbol{z}[/latex]を得た場合に、[latex]\\boldsymbol{x}[/latex]が真の状態でありそうな度合いを数値化したもの
    • [latex]p(\\boldsymbol{z}|\\boldsymbol{x})[/latex]と基本的には同じもので、因果をひっくり返したもの
    • ただし確率の性質を満たす必要はない
    • [latex]\\ell[/latex]を尤度関数と呼ぶ
  • これをエージェントが知っていると、エージェントが状態を推定できる
  •  どうやってこれを知るか?
    • 事前実験
    • センサの特性や環境の特性の知識
    • 正解はない

センサ入力に生じる雑音に対する考察

 

センサ入力からの[latex]bel[/latex]の更新

  • センサ入力と尤度関数からベイズの定理を使って[latex]bel[/latex]を更新
  • [latex]bel(\\boldsymbol{x}|\\boldsymbol{z}) = \\frac{p(\\boldsymbol{z}|\\boldsymbol{x})bel(\\boldsymbol{x})}{\\int_{\\mathcal{x}}p(\\boldsymbol{z}|\\boldsymbol{x}')bel(\\boldsymbol{x}')d\\boldsymbol{x}'} \\\\ = \\eta p(\\boldsymbol{z}|\\boldsymbol{x})bel(\\boldsymbol{x}) \\\\ = \\eta \\ell(\\boldsymbol{x}|\\boldsymbol{z})bel(\\boldsymbol{x})[/latex]
  • 計算が終わったら[latex]bel(\\boldsymbol{x}|\\boldsymbol{z})[/latex]を[latex]bel(\\boldsymbol{x})[/latex]と表記
    • 本来[latex]bel_t[/latex]は過去のロボットの制御出力、センサ入力全てから得られる条件つき確率になっている

1次元、ガウス分布に雑音が従う場合の問題

  • 今、[latex]bel[/latex]が、次の式で表されているとします。
    • [latex]bel(x) = \\frac{1}{\\sqrt{2\\pi \\sigma^2}}\\exp \\{-\\frac{(x-\\hat{x})^2}{2\\sigma^2}\\}[/latex]
    • [latex]\\sigma[/latex]: 標準偏差
    • [latex]\\hat{x}[/latex]: 推定の中心(推定値)
  • センサ入力[latex]z[/latex]がありました。
    • [latex]z[/latex]の値は、[latex]x[/latex]をロボットの真の位置とするとき、対して次の分布に従います
      • [latex]p(z|x) = \\ell(x|z) = \\frac{1}{\\sqrt{2\\pi \\zeta^2}}\\exp \\{-\\frac{(z-x)^2}{2\\zeta^2}\\}[/latex]
  • さて[latex]bel[/latex]はどのように更新されるでしょうか?

計算

  • [latex]x[/latex]に関係ない項を[latex]\\eta[/latex]に放り込みながら整理
    • [latex]bel(x | z) = \\eta e^{-\\frac{(x-\\hat{x})^2}{2\\sigma^2}}e^{-\\frac{(z-x)^2}{2\\zeta^2}} \\\\ =\\eta e^{-\\frac{1}{2\\sigma^2}x^2 -\\frac{1}{2\\zeta^2}x^2 + \\frac{\\hat{x}}{\\sigma^2}x + \\frac{z}{\\zeta^2}x }  \\\\ = \\eta e^{-\\frac{\\sigma^2 + \\zeta^2}{2\\sigma^2\\zeta^2}\\left\\{ x^2 - 2(\\frac{\\zeta^2\\hat{x}}{\\sigma^2 + \\zeta^2} +\\frac{\\sigma^2 z}{\\sigma^2 + \\zeta^2})x \\right\\} } \\\\ = \\eta e^{-\\frac{\\sigma^2 + \\zeta^2}{2\\sigma^2\\zeta^2}\\left(x - \\frac{\\zeta^2\\hat{x} +\\sigma^2 z}{\\sigma^2 + \\zeta^2}  \\right)^2  }[/latex]

結果

  • [latex]bel(x | z) [/latex]は、次のパラメータを持つガウス分布に
    • 中心: [latex]\\frac{\\zeta^2\\hat{x} +\\sigma^2 z}{\\sigma^2 + \\zeta^2}[/latex]
      • 元の分布の中心[latex]\\hat{x}[/latex]とセンサ入力の示す値[latex]z[/latex]の 重み付き平均に推定の中心が移動
      • [latex]\\zeta[/latex]が小さい(センサ入力に自信がある)と[latex]z[/latex]寄りに
      • [latex]\\sigma[/latex]が小さい(元の推定に自信がある)と[latex]\\hat{x}[/latex]寄りに
    • 分散: [latex]\\frac{\\sigma^2\\zeta^2}{\\sigma^2 + \\zeta^2}[/latex]
      • 元の[latex]bel[/latex]の分散[latex]\\sigma^2[/latex]より[latex]\\frac{\\zeta^2}{\\sigma^2 + \\zeta^2}[/latex]倍だけ小さくなる

非線形・雑音が多変量ガウス分布に従う場合のbelの導出

  • 制御出力と同様に導出してみましょう。
  • 非線形な観測方程式
    • [latex]\\boldsymbol{z} = h(\\boldsymbol{x}) + \\delta[/latex]
    • [latex]\\delta[/latex]は共分散行列[latex]Q[/latex]、中心ゼロの多変量ガウス分布に従うとする
  • [latex]\\boldsymbol{\\hat{x}}[/latex]周りで線形化するとこうなる
    • [latex] h(\\boldsymbol{x}) = h(\\boldsymbol{\\hat{x}}) + \\frac{\\partial h(\\boldsymbol{x})}{\\partial\\boldsymbol{x}}\\big|_{\\boldsymbol{\\hat{x}}} (\\boldsymbol{x} - \\boldsymbol{\\hat{x}}) =h(\\boldsymbol{\\hat{x}}) + H (\\boldsymbol{x} - \\boldsymbol{\\hat{x}})[/latex]
      • [latex]H =\\frac{\\partial h(\\boldsymbol{x})}{\\partial\\boldsymbol{x}}\\big|_{\\boldsymbol{\\hat{x}}}[/latex]とする。
      • [latex]H[/latex]はヤコビ行列

尤度関数、[latex]bel[/latex]への代入

  • [latex]\\ell(\\boldsymbol{x} | \\boldsymbol{z}) = \\eta e^{-\\frac{1}{2} (\\boldsymbol{z} -h(\\boldsymbol{x}))^TQ^{-1}(\\boldsymbol{z} -h(\\boldsymbol{x}))} \\\\ =\\eta e^{-\\frac{1}{2} (\\boldsymbol{z} -h(\\boldsymbol{\\hat{x}}) - H(\\boldsymbol{x} - \\boldsymbol{\\hat{x}} ))^TQ^{-1}(\\boldsymbol{z} -h(\\boldsymbol{\\hat{x}}) - H(\\boldsymbol{x} - \\boldsymbol{\\hat{x}} )) }[/latex]
  • [latex]bel(\\boldsymbol{x}|\\boldsymbol{z}) = \\ell(\\boldsymbol{x} | \\boldsymbol{z})bel(\\boldsymbol{x}) \\\\ =\\eta e^{-\\frac{1}{2} (\\boldsymbol{x} - \\boldsymbol{\\hat{x}})^T\\Sigma^{-1}(\\boldsymbol{x} - \\boldsymbol{\\hat{x}})} e^{-\\frac{1}{2} (\\boldsymbol{z} -h(\\boldsymbol{\\hat{x}}) - H(\\boldsymbol{x} - \\boldsymbol{\\hat{x}} ))^TQ^{-1}(\\boldsymbol{z} -h(\\boldsymbol{\\hat{x}}) - H(\\boldsymbol{x} - \\boldsymbol{\\hat{x}} )) }[/latex]

 

計算(大変)は割愛しますが・・・

  • [latex]bel(\\boldsymbol{x}|\\boldsymbol{z})[/latex]は次のパラメータを持つ 多変量ガウス分布に
    • 分布の中心: [latex] \\boldsymbol{\\hat{x}} + K(\\boldsymbol{z} - h(\\boldsymbol{\\hat{x}})) [/latex]
    • 共分散行列: [latex](I - KH) \\Sigma[/latex]
    • ここで [latex]K = \\Sigma H (H \\Sigma H^T + Q )^{-1}[/latex] (確率ロボティクスの式(3.64)、間違ってます!!!!)

カルマンゲイン[latex]K[/latex]

  • 以下を表す
    • センサ入力でどれだけ状態推定の曖昧さを減らせるか
    • これまでの推定の状態を、センサ入力の教える状態にどれだけ近付けるか
  • [latex]K = \\Sigma H (H \\Sigma H^T + Q )^{-1}[/latex]
  • 先ほどの1次元の問題では、[latex]\\Sigma[/latex]が[latex]\\sigma^2[/latex]、[latex]Q[/latex]が[latex]\\zeta^2[/latex]、[latex]H[/latex]が単位行列に
    • 整理すると[latex]K = \\frac{\\sigma^2}{\\sigma^2 + \\zeta^2}[/latex]
    • [latex]I - KH =\\frac{\\zeta^2}{\\sigma^2 + \\zeta^2}[/latex]

次回

  • カルマンフィルタの整理
  • パーティクルフィルタの数理