上田隆一
2017年10月11日@千葉工業大学
モンテカルロ近似された$bel$にセンサ情報を反映
現在主流はレーザレンジファインダーを使う方法ですが、シミュレーションが大変なので点ランドマークで。
%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_landmarks = [np.array([-0.5,0.0]),np.array([0.5,0.0]),np.array([0.0,0.5])]
def draw_landmarks(landmarks):
xs = [ e[0] for e in landmarks]
ys = [ e[1] for e in landmarks]
plt.scatter(xs,ys,s=300,marker="*",label="landmarks",color="orange")
draw_landmarks(actual_landmarks)
actual_x = np.array([0.3,0.2,math.pi*20.0/180]) #ロボットの実際の姿勢
def draw_robot(pose):
plt.quiver([pose[0]],[pose[1]],[math.cos(pose[2])],[math.sin(pose[2])],color="red",label="actual robot motion")
draw_robot(actual_x)
draw_landmarks(actual_landmarks)
まず、ロボットからランドマークがどの向き、距離に見えるか計算してみましょう。これは、ロボットが360度どこからでも誤差なくランドマークを観測できる時の観測モデルですが、後から制限します。
def relative_landmark_pos(pose,landmark):
x,y,theta = pose
lx,ly = landmark
distance = math.sqrt((x -lx)**2 + (y-ly)**2)
direction = math.atan2(ly-y, lx-x) - theta
return (distance, direction,lx,ly) # 実際の位置も一緒に返す
measurements = [ relative_landmark_pos(actual_x,e) for e in actual_landmarks]
print(measurements)
[(0.8246211251235323, -3.2456798408617948, -0.5, 0.0), (0.28284271247461906, -1.1344640137963142, 0.5, 0.0), (0.4242640687119285, 2.0071286397934789, 0.0, 0.5)]
描画する関数も作成しましょう。
def draw_observation(pose, measurement):
x,y,theta = pose
distance, direction,lx,ly = measurement
lx = distance*math.cos(theta + direction) + x
ly = distance*math.sin(theta + direction) + y
plt.plot([pose[0], lx],[pose[1], ly],color="pink")
def draw_observations(pose, measurements):
for m in measurements:
draw_observation(pose, m)
draw_landmarks(actual_landmarks)
draw_robot(actual_x)
draw_observations(actual_x,measurements)
観測モデルを作成します。
def observation(pose, landmark):
actual_distance,actual_direction,lx,ly = relative_landmark_pos(pose,landmark)
# 方向の制限(cosの値が正)
if (math.cos(actual_direction) < 0.0):
return None
measured_distance = random.gauss(actual_distance,actual_distance*0.1)
measured_direction = random.gauss(actual_direction,5.0/180.0*math.pi)
return (measured_distance, measured_direction,lx,ly)
def observations(pose,landmarks):
return filter(lambda x: x != None, [ observation(pose,e) for e in landmarks])
actual_x = np.array([0.3,0.2,math.pi*180.0/180]) #姿勢は変えること
measurements = observations(actual_x, actual_landmarks)
draw_landmarks(actual_landmarks)
draw_robot(actual_x)
draw_observations(actual_x, measurements)
先週作ったコードに今の観測モデルを載せる。
### クラスや関数のコピペ ###
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)
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])
### 描画関数は少し変更を ###
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])*e.weight for e in particles] #重みで長さを変えるようにしましょう
vys = [math.sin(e.pose[2])*e.weight 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")
### 行動もコピペベースで。ランドマークの観測と描画を入れる ###
actual_x = np.array([0.0,0.0,0.0]) #ロボットの実際の姿勢
particles = [Particle(1.0/100) for i in range(100)]
u = np.array([0.2,math.pi / 180.0 * 20]) #ロボットの移動
import copy
path = [actual_x]
particle_path = [copy.deepcopy(particles)]
measurementss = [observations(actual_x, actual_landmarks)]
for i in range(10):
actual_x = f(actual_x,u)
path.append(actual_x)
measurementss.append(observations(actual_x,actual_landmarks))
for p in particles:
p.pose = f(p.pose,u)
particle_path.append(copy.deepcopy(particles))
for i,p in enumerate(path):
draw(path[i],particle_path[i])
draw_landmarks(actual_landmarks)
draw_observations(path[i],measurementss[i])
### パーティクルの姿勢と観測結果を比較する関数 ###
from scipy.stats import norm # ガウス分布(正規分布)のオブジェクトをインポート
def likelihood(pose, measurement):
x,y,theta = pose
distance, direction,lx,ly = measurement
# パーティクルの姿勢から観測されるはずのランドマークの距離と向き
rel_distance, rel_direction, tmp_x,tmp_y = relative_landmark_pos(pose,(lx,ly))
# 誤差をガウスで評価
return norm.pdf(x = distance - rel_distance, loc = 0.0, scale = rel_distance / 10.0) \
* norm.pdf(x = direction - rel_direction, loc = 0.0, scale = 5.0/180.0 * math.pi)
### パーティクル群の重みを変更する関数 ###
def change_weights(particles, measurement):
for p in particles:
p.weight *= likelihood(p.pose, measurement)
# 重みの合計を1に保つ
ws = [ p.weight for p in particles ]
s = sum(ws)
for p in particles: p.weight = p.weight / s
関数を適用してみましょう。
actual_x = np.array([0.0,0.0,0.0]) #ロボットの実際の姿勢
particles = [Particle(1.0/100) for i in range(100)]
u = np.array([0.2,math.pi / 180.0 * 20]) #ロボットの移動
path = [actual_x]
particle_path = [copy.deepcopy(particles)]
measurementss = [observations(actual_x, actual_landmarks)]
for i in range(10):
actual_x = f(actual_x,u)
path.append(actual_x)
ms = observations(actual_x,actual_landmarks)
measurementss.append(ms)
for p in particles:
p.pose = f(p.pose,u)
for m in ms:
change_weights(particles, m)
particle_path.append(copy.deepcopy(particles))
for i,p in enumerate(path):
draw(path[i],particle_path[i])
draw_landmarks(actual_landmarks)
draw_observations(path[i],measurementss[i])
norm.pdf
を二回かけた時の計算に相当s
に相当