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

Tue Sep 27 10:31:11 JST 2016 (modified: Fri Nov 29 17:24:59 JST 2019)
views: 882, 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]を尤度関数と呼ぶ
• これをエージェントが知っていると、エージェントが状態を推定できる
•  どうやってこれを知るか？
  • 事前実験
  • センサの特性や環境の特性の知識
  • 正解はない

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

• ガウス分布状にノイズが乗るセンサの尤度
  • 昨年の講義資料の第三回の11ページ
• 動的な障害物とセンサの関係
  • 昨年の講義資料の第三回の17-19ページ

 

センサ入力からの[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]

次回

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