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

上田隆一

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

モンテカルロ法による$bel$の計算

  • 発想: 前回の移動ロボットのシミュレーションをロボット(エージェント)が行う。
    • 一挙にたくさん並列で
    • 状態遷移のシミュレーション
  • 一つ時刻0の状態$\boldsymbol{x}_0$を選んで、制御出力に合わせて移動
    • 誤差を混入
  • 一つのシミュレーションの状態を、$bel$からサンプリングされる標本(サンプル)とみなすことができる
    • 「粒子(パーティクル)」という言い方も

モンテカルロ法のプログラミング

  • 前回の移動ロボットの動作をふまえて
    • 実際のロボットの動き(のシミュレーションを作る)
    • パーティクルの動きを作る
      • 数は100個にしましょう。

まず変数を準備

In [1]:
%matplotlib inline
import numpy as np
from copy import copy
import math, random
import matplotlib.pyplot as plt                   #   for plotting data
from matplotlib.patches import Ellipse      #  for drawing

### 変数 ###

# 実際の世界のシミュレーション
actual_x = np.array([0.0,0.0,0.0])   #ロボットの実際の姿勢
u = np.array([0.2,math.pi / 180.0 * 20]) #ロボットの移動

# モンテカルロ法のための変数
class Particle:
    def __init__(self,w):
        self.pose = np.array([0.0,0.0,0.0])
        self.weight = w
    
    def __repr__(self):
        return "pose: " + str(self.pose) + " weight: " + str(self.weight)
        
particles = [Particle(1.0/100) for i in range(100)]
In [2]:
# 一個プリントしてみましょう
print(particles[0])
pose: [ 0.  0.  0.] weight: 0.01

ロボットとパーティクルを動かす関数

前回のをコピペ

In [3]:
def f(x_old,u):
    pos_x, pos_y, pos_theta = x_old
    act_fw, act_rot = u
    
    act_fw = random.gauss(act_fw,act_fw/10)
    dir_error = random.gauss(0.0, math.pi / 180.0 * 3.0)
    act_rot = random.gauss(act_rot,act_rot/10)
    
    pos_x += act_fw * math.cos(pos_theta + dir_error)
    pos_y += act_fw * math.sin(pos_theta + dir_error)
    pos_theta += act_rot
    
    return np.array([pos_x,pos_y,pos_theta])

これで準備OK

動作の確認

In [4]:
import copy

path = [actual_x]
particle_path = [copy.deepcopy(particles)]
for i in range(10):
    actual_x = f(actual_x,u)
    path.append(actual_x)
    
    for p in particles:
        p.pose = f(p.pose,u)
    particle_path.append(copy.deepcopy(particles))

値の確認

In [5]:
print(path[0])
print(particle_path[0])

print(path[10])
print(particle_path[10])
[ 0.  0.  0.]
[pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01, pose: [ 0.  0.  0.] weight: 0.01]
[ 0.01614545  1.11655664  3.58506062]
[pose: [ 0.01734599  1.20518118  3.38961758] weight: 0.01, pose: [ 0.14067426  1.09540245  3.48434512] weight: 0.01, pose: [-0.03675291  1.01050254  3.63560435] weight: 0.01, pose: [-0.11952947  1.104785    3.56950413] weight: 0.01, pose: [ 0.03209341  1.10986851  3.45209625] weight: 0.01, pose: [ 0.09789036  1.20811542  3.41238955] weight: 0.01, pose: [-0.04804419  1.0742375   3.55197697] weight: 0.01, pose: [-0.13056078  1.12235485  3.51665937] weight: 0.01, pose: [-0.08359872  1.06479677  3.59629178] weight: 0.01, pose: [-0.0143955   1.14247446  3.40402246] weight: 0.01, pose: [ 0.15079481  1.17775277  3.2638102 ] weight: 0.01, pose: [ 0.04332535  1.18890611  3.47796401] weight: 0.01, pose: [ 0.0070267   1.00853635  3.51737211] weight: 0.01, pose: [ 0.00710169  1.12465871  3.48126149] weight: 0.01, pose: [ 0.12671985  1.26209876  3.4756379 ] weight: 0.01, pose: [-0.01486352  1.03847194  3.56298045] weight: 0.01, pose: [-0.0282506   0.99847902  3.64203359] weight: 0.01, pose: [-0.077858    1.14127602  3.54841263] weight: 0.01, pose: [-0.0664697   1.06792518  3.61256779] weight: 0.01, pose: [ 0.00922352  1.11377798  3.49758701] weight: 0.01, pose: [-0.09459638  1.13872486  3.48016417] weight: 0.01, pose: [-0.05171431  0.9413085   3.55444682] weight: 0.01, pose: [ 0.1484142   1.26004352  3.27679085] weight: 0.01, pose: [-0.04656941  1.13615364  3.40581222] weight: 0.01, pose: [-0.02464148  1.18969382  3.41803873] weight: 0.01, pose: [ 0.05720068  1.16731752  3.4368182 ] weight: 0.01, pose: [ 0.23441378  1.16301533  3.34262301] weight: 0.01, pose: [ 0.05725734  1.14356817  3.28522726] weight: 0.01, pose: [ 0.12428795  1.14703828  3.45743417] weight: 0.01, pose: [-0.01254756  1.08998504  3.6505922 ] weight: 0.01, pose: [ 0.04020105  1.17224547  3.33504312] weight: 0.01, pose: [-0.2604777   1.05467473  3.64033609] weight: 0.01, pose: [ 0.00572072  1.10535325  3.47412964] weight: 0.01, pose: [-0.00864797  1.16449334  3.46556097] weight: 0.01, pose: [ -9.07603286e-04   1.07481671e+00   3.70750228e+00] weight: 0.01, pose: [ 0.07480832  1.05916152  3.52981378] weight: 0.01, pose: [ 0.06722957  1.2218492   3.39132964] weight: 0.01, pose: [-0.03387826  1.12393226  3.55223523] weight: 0.01, pose: [-0.0174824   1.12660374  3.52692288] weight: 0.01, pose: [-0.05870051  1.0408486   3.6324189 ] weight: 0.01, pose: [ 0.08797761  1.16636222  3.40132655] weight: 0.01, pose: [ 0.0825788   1.12815353  3.43219542] weight: 0.01, pose: [ 0.06286047  1.19418831  3.38621449] weight: 0.01, pose: [-0.12570807  1.00598377  3.64162192] weight: 0.01, pose: [ 0.02436705  1.01954888  3.64516018] weight: 0.01, pose: [-0.16816711  1.14611102  3.54321306] weight: 0.01, pose: [-0.07882103  1.06863102  3.55946962] weight: 0.01, pose: [ 0.08570043  1.09476847  3.36808281] weight: 0.01, pose: [ 0.03080596  1.17446359  3.2874088 ] weight: 0.01, pose: [ 0.01906103  1.17370571  3.53835583] weight: 0.01, pose: [-0.04039951  1.14913523  3.5337639 ] weight: 0.01, pose: [-0.05202592  1.15015385  3.54717778] weight: 0.01, pose: [ 0.12142147  1.16730544  3.46323377] weight: 0.01, pose: [-0.03764508  1.13306468  3.52495051] weight: 0.01, pose: [ 0.14570327  1.20237252  3.45213784] weight: 0.01, pose: [ 0.04602506  1.06368099  3.51479391] weight: 0.01, pose: [-0.16225531  1.04383968  3.57096711] weight: 0.01, pose: [-0.03108543  1.16616758  3.47207338] weight: 0.01, pose: [-0.04768975  1.19465629  3.50537609] weight: 0.01, pose: [-0.0489686   1.09667159  3.4859566 ] weight: 0.01, pose: [-0.03564458  1.11614241  3.32290167] weight: 0.01, pose: [ 0.02173056  1.06339176  3.67430178] weight: 0.01, pose: [-0.13355162  1.00669949  3.7288648 ] weight: 0.01, pose: [ 0.05846703  1.07904908  3.65362641] weight: 0.01, pose: [ 0.02857776  1.205207    3.41763767] weight: 0.01, pose: [ 0.21340256  1.2461012   3.37405234] weight: 0.01, pose: [-0.09814257  1.12557724  3.48025132] weight: 0.01, pose: [-0.00594669  1.20940314  3.51234865] weight: 0.01, pose: [-0.0946811   1.07292629  3.63943214] weight: 0.01, pose: [-0.13120766  1.11856025  3.58455391] weight: 0.01, pose: [ 0.04556234  1.11747266  3.48304896] weight: 0.01, pose: [-0.01078967  1.13391111  3.44343668] weight: 0.01, pose: [ 0.02748639  1.28010546  3.38682889] weight: 0.01, pose: [ 0.08043834  1.15860471  3.38329211] weight: 0.01, pose: [ 0.07526524  1.10465433  3.4765806 ] weight: 0.01, pose: [-0.00736167  1.10547357  3.49457588] weight: 0.01, pose: [-0.07061461  1.12996688  3.58193182] weight: 0.01, pose: [-0.11947022  1.09319107  3.53328906] weight: 0.01, pose: [-0.04813927  1.12332923  3.54283598] weight: 0.01, pose: [ 0.14050502  1.23118722  3.39338234] weight: 0.01, pose: [-0.0256233   1.16561213  3.57678437] weight: 0.01, pose: [-0.07127631  0.94807556  3.70327368] weight: 0.01, pose: [ 0.0976791   1.18405202  3.37984228] weight: 0.01, pose: [ 0.03978661  1.2653564   3.44929203] weight: 0.01, pose: [ 0.09686846  1.19513706  3.35230642] weight: 0.01, pose: [-0.07058766  1.11026733  3.52443772] weight: 0.01, pose: [ 0.08259952  1.11230701  3.36883253] weight: 0.01, pose: [-0.01364817  1.03883002  3.55118194] weight: 0.01, pose: [ 0.00362118  1.11112611  3.52194495] weight: 0.01, pose: [ 0.07198805  1.17571155  3.42233676] weight: 0.01, pose: [-0.07167549  1.09892701  3.52359114] weight: 0.01, pose: [ 0.06740852  1.15074433  3.49684461] weight: 0.01, pose: [ 0.07599775  1.15525312  3.44088293] weight: 0.01, pose: [ 0.13875945  1.2644619   3.36819546] weight: 0.01, pose: [ 0.08270819  1.1609582   3.43980574] weight: 0.01, pose: [ 0.04604382  1.18831354  3.41534129] weight: 0.01, pose: [-0.11456185  1.00344149  3.56744868] weight: 0.01, pose: [ 0.00890828  1.09800655  3.38303898] weight: 0.01, pose: [-0.04861796  1.07882509  3.63858782] weight: 0.01, pose: [ 0.00546514  1.06600912  3.55540792] weight: 0.01]

描画

In [6]:
def draw(pose,particles):    
    fig = plt.figure(i,figsize=(8, 8))
    sp = fig.add_subplot(111, aspect='equal')
    sp.set_xlim(-1.0,1.0)
    sp.set_ylim(-0.5,1.5)
    
    xs = [e.pose[0] for e in particles]
    ys = [e.pose[1] for e in particles]
    vxs = [math.cos(e.pose[2]) for e in particles]
    vys = [math.sin(e.pose[2]) for e in particles]
    plt.quiver(xs,ys,vxs,vys,color="blue",label="particles")
    
    plt.quiver([pose[0]],[pose[1]],[math.cos(pose[2])],[math.sin(pose[2])],color="red",label="actual robot motion")
In [7]:
for i,p in enumerate(path):
    draw(path[i],particle_path[i])

数式での表現

パーティクルの定義

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

motion update

  • 先ほどのシミュレーション
  • 各パーティクルを$p(\boldsymbol{x} | \boldsymbol{x}_{t-1}, \boldsymbol{u}_t)$に従って動かす
    • $\boldsymbol{x}^{(i)}_t \sim p(\boldsymbol{x} | \boldsymbol{x}_{t-1}, \boldsymbol{u}_t) \quad (i=1,2,\dots,N)$
      • 時刻$t$における$i$番目のパーティクルの状態を、状態遷移の確率分布にしたがって一つランダムに選ぶ
      • 今のところ、全パーティクルをひとつずつ選んで動かすだけでよい

まとめ

  • $bel$をモンテカルロ法(パーティクル)で近似計算
  • ロボットの動きを雑音つきの状態方程式で表現してロボットに実装
  • ロボットの中でパーティクルの数だけロボットの動作のシミュレーション