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

Mon Sep 12 09:10:38 JST 2016 (modified: Sat Dec 19 14:45:21 JST 2020)
views: 868, keywords:

確率ロボティクス

第2回

上田 隆一

2016年9月28日@千葉工業大学

$\require{\amsmath}$ $\require{\amssymb}$

今日の内容

• 定式化の続き
• 制御と状態

前回

• 状態について考えた
  • 状態空間\(\\mathcal{X}\)
  • 状態\(\\boldsymbol{x} = (x_1,x_2,\\dots,x_n) \\in \\mathcal{X}\)
  • 状態変数\(x_1,x_2,\\dots,x_n\)
• 制御の上で区別すべきものを考慮して状態を定義すべき

今回

• 制御出力
• 確率的な表現
  • 状態は分からなくなる

状態を動かす

• 状態は変化（遷移）する
• 時刻の導入
  • ディジタルの計算機を使うときは基本、離散系で考えてよい
  • \(\\boldsymbol{x}_0,\\boldsymbol{x}_1,\\dots,\\boldsymbol{x}_t,\\dots\)（\(t\):時刻）
    • （この表記だと\(x\)が太字か細字かで添字の意味が違うので注意）
    • （わかりにくくてごめんなさい）
• 状態が変化する原因
  • 自分（行動決定する何か）が能動的に動かす
  • 自分でないものが動かす
• とりあえず前者について考えましょう

制御出力

• 状態を動かすもの
  • \(\\boldsymbol{u} = (u_1,u_2,\\dots,u_m)\)
  • \(u_1,u_2,\\dots,u_m\): 制御変数
• 制御出力空間
  • \(\\boldsymbol{u} \\in \\mathcal{U}\)
• 制御出力を与えるもの
  • 厳密に言うとロボットではない。ロボットは状態変数に含まれる
  • とりあえずエージェントと呼びましょう

状態遷移

• ある時刻の状態\(\\boldsymbol{x}_{t-1}\)でエージェントが制御出力\(\\boldsymbol{u}_t\)を 選んだら\(\\boldsymbol{x}_t\)に状態が変化
• 状態遷移の式（状態方程式）
  • \(\\boldsymbol{x}_t = f(\\boldsymbol{x}_{t-1},\\boldsymbol{u}_t)\)
• 状態遷移関数
  • \(f: \\mathcal{X} \\times \\mathcal{U} \\longrightarrow \\mathcal{X}\)

状態方程式を考える

• どれか一つ選んで定式化してみましょう。15分くらい。
  • 前回同様、問題の定義ははっきりさせません。定式化の上で何か分からないことがあれば聞いていただければ適当に設定します。
  • ロボット1
    • 入力は移動量（何度回転して、何度直進する、等）にしましょう
    • 満足いかない人はトルクで
  • ロボット2
    • 入力はステップモータの周波数\(\\propto\)角速度
    • ステップモータじゃなくてDCモータならどうなる？
  • 上の二つでなかったら前回の資料から好きなものを・・・

ロボットは状態方程式通りに・・・

• 動かない
• なぜ
  • 雑音
  • 状態方程式が甘い
  • そもそもエージェントが操作できない変数が存在

雑音の考慮

• いくつかの定式化
• 線形な状態方程式
  • \(\\boldsymbol{x}_t = A \\boldsymbol{x}_{t-1} + B\\boldsymbol{u}_t +\\boldsymbol{\\varepsilon}_t \)
• 非線形な方程式
  • \(\\boldsymbol{x}_t = f(\\boldsymbol{x}_{t-1},\\boldsymbol{u}_t) +\\boldsymbol{\\varepsilon}_t \)
• \(\\boldsymbol{x}_t\)が決定論的でなくなる
  • 上記のような数式では非決定論的なものが表現しづらい

確率を用いた表現

• 状態遷移の書き方
  • \( \\boldsymbol{x}_t \\sim p(\\boldsymbol{x} | \\boldsymbol{x}_{t-1},\\boldsymbol{u}_t) \)
    • 意味: \(\\boldsymbol{x}_{t-1}\)で\(\\boldsymbol{u}_t\)を選択したときの、遷移後の状態が 従う確率密度関数\(p(\\boldsymbol{x})\)に従って、\(\\boldsymbol{x}_t\)が決まる
• 依然\(\\boldsymbol{x}_t\)が実際には何なのかは分からない
  • 分かるのは真の状態がどれなのかという確率密度関数
• 確率密度関数（状態遷移確率）を求める式
  • \(bel_t(\\boldsymbol{x}) = \\int_\\mathcal{X} p(\\boldsymbol{x} | \\boldsymbol{x}',\\boldsymbol{u}_t )bel_{t-1}(\\boldsymbol{x}') d\\boldsymbol{x}'\)
  • \(bel\): 状態に関する確率密度関数

\(bel\)という表記

• \(bel\): belief、信念
  • エージェントが持っている、状態に対する理解
  • 何か正解があるわけではなく、 エージェントが自身の知覚と知識で作り上げるもの
• 知覚: 自身の制御出力、センサ入力
• 知識:制御出力、センサ入力と状態の関係
  • 制御出力の場合は状態方程式

\(bel\)の計算（ガウス分布）

• 問題
  • 状態は一次元: \(x \\in \\Re\)
  • 制御出力: 状態の変位\(\\Delta_t\)
    • \(x\)から\(x + \\Delta_t + \\epsilon\)に移動
      • 雑音\(\\epsilon\)は標準偏差\(\\delta_t\)でガウス分布に従う
• \(bel_t\)を以下から導出してみましょう（かなり難しい）
  • \(bel_{t-1}(x) = \\frac{1}{\\sqrt{2\\pi \\sigma_{t-1}^2}}\\exp \\{-\\frac{(x-\\hat{x}_{t-1})^2}{2\\sigma_{t-1}^2}\\}\)
    • \(\\hat{x}_{t-1}: t-1\)における分布の中心（状態の推定値）
    • \(\\sigma_{t-1}: t-1\)における分布の標準偏差
  • \(p(\\boldsymbol{x} | \\boldsymbol{x}_{t-1},\\Delta_t) =\\frac{1}{\\sqrt{2\\pi \\delta_t^2}}\\exp \\{-\\frac{(x-x_{t-1}-\\Delta_t)^2}{2\\delta_t^2}\\}\)

\(bel\)の式にガウス分布を代入

(bel_t(x) = \int_{-\infty}^{\infty} \frac{1}{\sqrt{2\pi\delta_t^2}}e^{-\frac{1}{2\delta_t^2}(x - x_{t-1} - \Delta_t)^2}\frac{1}{\sqrt{2\pi\sigma_{t-1}^2}}e^{-\frac{1}{2\sigma_{t-1}^2}(x_{t-1} - \hat{x_{t-1}})^2}dx_{t-1}\\= \eta\int_{-\infty}^{\infty} e^{\frac{-1}{2}L}dx_{t-1}\\)

• ここで
  • (\eta)は定数項
  • (L)は指数部の(-1/2)を除く部分

(L)から(x_{t-1})を分離

( L = \frac{1}{\delta_t^2}(x - x_{t-1} - \Delta_t)^2 + \frac{1}{\sigma_{t-1}^2}(x_{t-1} - \hat{x_{t-1}})^2 \\ = \frac{\sigma_{t-1}^2 + \delta_t^2}{\sigma_{t-1}^2\delta_t^2}\left\{ x_{t-1} - \frac{\sigma_{t-1}^2(x - \Delta_t) + \delta_t^2\hat{x_{t-1}}}{\sigma_{t-1}^2 + \delta_t^2}\right\}^2 + \frac{(x - \hat{x_{t-1}} - \Delta_t)^2}{\sigma_{t-1}^2 + \delta_t^2}\\)

左側と右側の項をそれぞれ(L(x_{t-1},x),L(x))とおきましょう

 

再び(bel_t)の式に代入

(bel_t(x) = \eta\int_{-\infty}^{\infty} e^{\frac{-1}{2}L}dx_{t-1}\\ =\eta\int_{-\infty}^{\infty} e^{- \frac{1}{2}L(x_{t-1},x) - \frac{1}{2} L(x)}dx_{t-1}\\ =\eta e^{-\frac{1}{2} L(x)}\int_{-\infty}^{\infty} e^{- \frac{1}{2}L(x_{t-1},x)}dx_{t-1}\\ =\eta e^{-\frac{1}{2}\frac{(x - \hat{x_{t-1}} - \Delta_t)^2}{\sigma_{t-1}^2 + \delta_t^2}}\int_{-\infty}^{\infty} e^{- \frac{1}{2}\frac{\sigma_{t-1}^2 + \delta_t^2}{\sigma_{t-1}^2\delta_t^2}\left\{ x_{t-1} - \frac{\sigma_{t-1}^2(x - \Delta_t) + \delta_t^2\hat{x_{t-1}}}{\sigma_{t-1}^2 + \delta_t^2}\right\}^2}dx_{t-1} \\)

積分の部分がガウス分布の積分の形になっており定数となる。(\eta)に入れてしまう

結局(bel_t)は・・・

(bel_t(x) = \eta \exp\left\{-\frac{(x - \hat{x_{t-1}} - \Delta_t)^2}{2(\sigma_{t-1}^2 + \delta_t^2)}\right\} = \mathcal{N}(\hat{x_{t-1}} + \Delta_t,\sigma_{t-1}^2 + \delta_t^2))

• (bel_t)は再びガウス分布となる
  • 分布の中心: 元の移動量に制御出力の値を加えた値
  • 分散: 移動の雑音の分散に元の（(bel_{t-1})の）分散を足した値

(bel)の計算（数値計算）

• ガウス分布のように計算前後が同じ分布になるものでない場合
  • どうすんの？
• 乱数を使った数値計算で近似的に解く
  • 式を解くよりはこっちの方が簡単なことが多い
  • 計算量や近似精度の問題がつきまとう

モンテカルロ法を使った(bel)の計算

• 手を動かしてみましょう
  •  先ほどと同じ問題で
    • 制御出力: (\Delta_t)
    • 誤差: (\delta_t)
• 以下をExcelかプログラミングで何度も繰り返してみる
  • (x = 0)に点を打つ
  • (\Delta = 1,\delta = 0.1)として乱数を一つ選んで移動量とし、(x)を移動
    • 簡易的なガウス乱数の作り方: 区間([0,1])から 12個の数字を一様乱数を選んで足し、6を引くと (\mathcal{N}(0,1))にほぼ従う乱数が得られる
  • 数ステップ(x)を動かしてみる（(t=1,2,3)と3ステップ程度）
• 1,2,3ステップ後の分布はガウス分布？分散は？

格子を使った(bel)の計算

• (bel)の表現
  • 状態空間を格子状に区切る
    • (s_1,s_2,s_3,\dots,s_N \subset \mathcal{X})
      • (\bigcap_{i=1}^N s_i = \phi)
      • (\bigcup_{i=1}^N s_i = \mathcal{X})
  • 各格子に確率を割り当て
    • P(s_i)
      • (\sum_{i=1}^N P(s_i) = 1)
• これも今までと同じ問題で手を動かしてみましょう
  • 次ページ

• (0.1)刻みで数直線上に区間を作る
  • どこかの区間の確率を1に。他を0に。
•  以下をExcelかプログラミングで何度も繰り返してみる
  • (\Delta = 1)だけ分布を動かす
    • 雑音(\delta = 0.1)の量だけ分布をぼかす
      • 方法は議論のこと
    • ある区間から移動した確率は二つ以上の区間にまたがる

2次元以上な系・かつ非線形な系

• 数式を解く場合
  • 行列、線形化
•  数値計算
  • 自然に拡張可能
  • 計算量が増える

移動ロボットにおける非線形な系

• この資料（昨年の講義の第2回資料）の4,5,15ページ
  • http://www.slideshare.net/ryuichiueda/ss-53672127
  • 普通の移動ロボットでも回転がある関係で非線形に
• 線形化は必要？数値計算でいいのでは？
  • （残念ながら）graph-based SLAMの実装等で出てくる

雑音がガウス分布に従う場合の(bel)の導出

• おさらい
  •  線形な状態方程式
    • (\boldsymbol{x}t = A \boldsymbol{x}{t-1} + B\boldsymbol{u}t +\boldsymbol{\varepsilon}t )
  • 非線形な方程式（行列で書けない）
    • (\boldsymbol{x}t = f(\boldsymbol{x}{t-1},\boldsymbol{u}t) +\boldsymbol{\varepsilon}t )

(f)を線形化

• 姿勢の推定値(\boldsymbol{\hat{x}}{t-1})まわりでテイラー展開
  • ( f(\boldsymbol{x},\boldsymbol{u}) = f(\boldsymbol{\hat{x}}{t-1},\boldsymbol{u}) + \frac{\partial f(\boldsymbol{x},\boldsymbol{u})}{\partial\boldsymbol{x}}\big|{\boldsymbol{\hat{x}}{t-1}} (\boldsymbol{x} - \boldsymbol{\hat{x}}{t-1}) )
  • この式は(\boldsymbol{x})の線形式になっている
  • (\boldsymbol{x}): (n)次元の状態ベクトル
  • (\frac{\partial f(\boldsymbol{x},\boldsymbol{u})}{\partial\boldsymbol{x}}): ヤコビ行列
    • (n\times n)行列

状態方程式の書き換え

• (F_t = \frac{\partial f(\boldsymbol{x},\boldsymbol{u})}{\partial\boldsymbol{x}}\big|{\boldsymbol{\hat{x}}{t-1},\boldsymbol{u}t})とする
  • 確率ロボティクスだと(G_t)になっている
• (\boldsymbol{x}t = f(\boldsymbol{x}{t-1},\boldsymbol{u}t) +\boldsymbol{\varepsilon}t )に前のページで得た(f)を代入
  • (\boldsymbol{x}t = f(\boldsymbol{\hat{x}}{t-1},\boldsymbol{u}t) + F_t(\boldsymbol{x}{t-1} - \boldsymbol{\hat{x}}{t-1}) +\boldsymbol{\varepsilon}t)

状態遷移確率

• (p(\boldsymbol{x} | \boldsymbol{x}{t-1},  \boldsymbol{u}t ) \approx \eta e^{ -\frac{1}{2} \left[\boldsymbol{x} -f(\boldsymbol{\hat{x}}{t-1},\boldsymbol{u}t) - F_t(\boldsymbol{x}{t-1} - \boldsymbol{\hat{x}}{t-1})  \right]^T  R_t^{-1}\left[\boldsymbol{x} - f(\boldsymbol{\hat{x}}{t-1},\boldsymbol{u}t) -F_t(\boldsymbol{x}{t-1} - \boldsymbol{\hat{x}}{t-1}) \right]  })
  • (\boldsymbol{x}): 移動後の状態の候補
  • (f(\boldsymbol{\hat{x}}{t-1},\boldsymbol{u}t)): 移動前の推定位置から予測される移動後の状態
  • (\boldsymbol{x}{t-1} - \boldsymbol{\hat{x}}{t-1}): 移動前の姿勢（左辺で指定）と移動前の推定姿勢の差
  • (R_t): (\boldsymbol{\varepsilon}t)に対応する共分散行列

導出される(bel_t)

• 移動前の(bel{t-1})が次のとき
  • 分布の形状: 多変量ガウス分布
  • 中心: (\boldsymbol{\hat{x}}{t-1})
  • 共分散: (\hat\Sigma{t-1})
• 移動後の(bel_t)は
  • 中心: (f(\boldsymbol{\hat{x}}{t-1},\boldsymbol{u}t) + F_t(\boldsymbol{\hat{x}}{t-1} - \boldsymbol{\hat{x}}{t-1}) = f(\boldsymbol{\hat{x}}_{t-1},\boldsymbol{u}t))
    • 移動前の平均値を素直に移動
  • 共分散: (F_t\hat\Sigma{t-1}F_t^T + R_t)
    • 共分散行列を回転して雑音を足しているだけ