要約
- Pythonで動くMonte Carlo位置推定（MCL: Monte Carlo localization）とextended Kalman filter（EKF）を用いた位置推定を実装
- ついでに簡単なシミュレーションのプログラムも実装
- シンプルに使える単純な自己位置推定用の勉強プログラムを提供
注：私はPython初心者，かつ独学でしか勉強していないので，コーディングの仕方は参考にはしないでください...
プログラム作成のモチベーション
最近，自己位置推定に関する講演をする機会がちょくちょく増えて来て，その度に，数式のみを用いた理論的な説明だけでなく，実際にサンプルプログラムを用いて説明ができると良いなと思っていました．特に，C/C++よりも，最近良く使われているPythonを使ったサンプルがあると良いなと思っていました．しかし説明に使うためには，中身をしっかりと理解していないといけません．というわけで，自分の講演で説明用に使うことを目的として，実際に実装してみました．ついでにGitHubに公開しました．この記事では，そのプログラムの中身を簡単に紹介します．
自己位置推定の初学者のために，少しでも役に立てば幸いです．
robot_sim
ロボットの内界・外界センサをシミュレートするためのクラスを格納しています．後で示すMCLもEKFも，このファイルをインポートし，ロボットの動作をシミュレートしています．
update_pose()は並進速度$v$と角速度$w$を受け取り，ロボットの動きをシミュレートします．robot_sim.pyは真値（self.gt_）とシミュレーション（self.sim_）の2種類の姿勢を持っています．gtは受け取った速度通りに更新され，simはノイズを加えて更新されます．そのため，gtとsimは違う姿勢となります．位置推定によって得られた値とgtを比較すると，推定誤差が計算できます．
def update_pose(self, v, w):
# update the ground truth pose
delta_dist = v * self.sim_time_step
delta_yaw = w * self.sim_time_step
x = self.gt_x + delta_dist * math.cos(self.gt_yaw)
y = self.gt_y + delta_dist * math.sin(self.gt_yaw)
yaw = self.gt_yaw + delta_yaw
self.gt_x = x
self.gt_y = y
self.gt_yaw = self.mod_yaw(yaw)
# update the simulation pose and calculate the simulation velocities
delta_dist2 = delta_dist * delta_dist
delta_yaw2 = delta_yaw * delta_yaw
delta_dist_sim = delta_dist * 0.9 + normal(0.0, self.odom_noise1 * delta_dist2 + self.odom_noise2 * delta_yaw2)
delta_yaw_sim = delta_yaw * 0.9 + normal(0.0, self.odom_noise3 * delta_dist2 + self.odom_noise4 * delta_yaw2)
x = self.sim_x + delta_dist_sim * math.cos(self.sim_yaw)
y = self.sim_y + delta_dist_sim * math.sin(self.sim_yaw)
yaw = self.sim_yaw + delta_yaw_sim
self.sim_x = x
self.sim_y = y
self.sim_yaw = self.mod_yaw(yaw)
self.sim_v = delta_dist_sim / self.sim_time_step
self.sim_w = delta_yaw_sim / self.sim_time_step
get_sensor_measurements()をコールすると，gtの姿勢に従ってセンサ観測をシミュレートします．今回は点ランドマークを想定しているので，gtの姿勢から観測できる距離（self.max_measurement_range）に存在するランドマークすべての距離と角度を取得します．観測値には適当なノイズが加えられます．また，一定の割合（self.random_measurement_rate）で距離の計測値は完全にランダムになります．
def get_sensor_measurements(self):
measurements = []
for i in range(len(self.landmarks)):
dx = self.landmarks[i][0] - self.gt_x
dy = self.landmarks[i][1] - self.gt_y
dl = normal(math.sqrt(dx * dx + dy * dy), self.measurement_range_variance)
if dl <= self.max_measurement_range:
dyaw = normal(math.atan2(dy, dx) - self.gt_yaw, self.measurement_angle_variance)
dyaw = self.mod_yaw(dyaw)
# simulate random range measurement
if random() < self.random_measurement_rate:
dl = random() * self.max_measurement_range
measurements.append([dl, dyaw])
return measurements
MCL
MCLは，パーティクルフィルタを用いた自己位置推定です．
mcl.pyはMCLのクラスを格納しており，main.pyはrobot_sim.pyとmcl.pyをインポートし，実際にプログラムを動かします．robot_sim.pyをインポートするために，Pythonのパスを設定する必要があります．exec.sh内で，パスを設定とmain.py実行を行っています．
main.pyのメインループは以下の様になっています．まず適当な速度（今回は固定）を入力し，移動量（delta_dist，delta_yaw），およびセンサ観測（measurements）をシミュレートします．そして，MCLによる位置推定を実行します．
while True:
# simulate robot behaviors
robot_sim.update_pose(0.2, -0.2)
delta_dist = robot_sim.sim_v * robot_sim.sim_time_step
delta_yaw = robot_sim.sim_w * robot_sim.sim_time_step
measurements = robot_sim.get_sensor_measurements()
# mcl
mcl.update_particles(delta_dist, delta_yaw)
mcl.calculate_weights(measurements)
mcl.estimate_robot_pose()
mcl.resample_particles()
mcl.print_estimated_pose()
mcl.print_effective_sample_size_and_total_weight()
mcl.plot_mcl_world(measurements)
# sleep
time.sleep(robot_sim.sim_time_step)
update_particles()は移動量を受け取り，パーティクルの姿勢を更新します．姿勢を更新する際には，移動量に適当なノイズが加えられます．これにより，パーティクルは分布を持って広がることになります．この関数が「動作モデル」に相当します．また，更新されたパーティクル群が近似する確率分布が「予測分布」と呼ばれます．
def update_particles(self, delta_dist, delta_yaw):
delta_dist2 = delta_dist * delta_dist
delta_yaw2 = delta_yaw * delta_yaw
for i in range(self.particle_num):
del_dist = normal(delta_dist, self.odom_noise1 * delta_dist2 + self.odom_noise2 * delta_yaw2)
del_yaw = normal(delta_yaw, self.odom_noise3 * delta_dist2 + self.odom_noise4 * delta_yaw2)
x = self.particles[i][0] + del_dist * math.cos(self.particles[i][2])
y = self.particles[i][1] + del_dist * math.sin(self.particles[i][2])
yaw_ = self.particles[i][2] + del_yaw
yaw = self.mod_yaw(yaw_)
self.particles[i][0] = x
self.particles[i][1] = y
self.particles[i][2] = yaw
calculate_weights()は，センサ観測を受け取り，「観測モデル」を用いてパーティクルの重みを計算します．今回は観測モデルとして「尤度場モデル」を用いています（ただしmaxがないことに注意してください）．確率の掛け算はしばしばアンダーフローを起こすため，尤度場モデルにより計算された値のlogの総和を計算してます．この総和に対してexpを取った値が，複数の観測に対する同時分布から得られる確率の値となります．
全パーティクルの重みの計算を終えた後に，パーティクルの重みを正規化（総和が1になる操作）を実行し，かつ有効サンプル数（effective sample size）を計算します．有効サンプル数とは，パーティクル群の重みの相対値を考慮し，いくつのパーティクルが有効かを示す指標になります．
def calculate_weights(self, measurements):
self.total_weight = 0.0
norm_coef = 1.0 / (math.sqrt(2.0 * self.PI * self.measurement_variance))
for i in range(self.particle_num):
total_log_prob = 0.0
for j in range(len(measurements)):
myaw = self.particles[i][2] + measurements[j][1]
mx = measurements[j][0] * math.cos(myaw) + self.particles[i][0]
my = measurements[j][0] * math.sin(myaw) + self.particles[i][1]
min_dl = 0.0
for k in range(len(self.landmarks)):
dx = self.landmarks[k][0] - mx
dy = self.landmarks[k][1] - my
dl = math.sqrt(dx * dx + dy * dy)
if k == 0:
min_dl = dl
elif min_dl > dl:
min_dl = dl
prob = self.z_hit * norm_coef * math.exp(-0.5 * (min_dl * min_dl) / (2.0 * self.measurement_variance)) + self.z_rand * 10e-6
prob *= self.measurement_resolution
if prob > 1.0:
prob = 1.0
total_log_prob += math.log(prob)
prob = math.exp(total_log_prob)
weight = self.particles[i][3] * prob
self.particles[i][3] = weight
self.total_weight += weight
# normalize weights and calculate effective sample size
self.effective_sample_size = 0.0
for i in range(self.particle_num):
normalized_weight = self.particles[i][3] / self.total_weight
self.particles[i][3] = normalized_weight
normalized_weight2 = normalized_weight * normalized_weight
self.effective_sample_size += normalized_weight2
self.effective_sample_size = 1.0 / self.effective_sample_size
estimate_robot_pose()内で，パーティクルの姿勢に対する重み付き平均を計算します．この操作が，パーティクル群により近似された確率分布（事後分布）から期待値を取るという操作に相当します．なお角度成分に関しては計算の都合上，ある角度（tmp_yaw）からの角度差の重み付き平均を求め，その差を用いて推定角度とします．
def estimate_robot_pose(self):
x = 0.0
y = 0.0
yaw = 0.0
tmp_yaw = copy.copy(self.robot_pose_yaw)
for i in range(self.particle_num):
x += self.particles[i][0] * self.particles[i][3]
y += self.particles[i][1] * self.particles[i][3]
dyaw = tmp_yaw - self.particles[i][2]
dyaw = self.mod_yaw(dyaw)
yaw += dyaw * self.particles[i][3]
yaw_ = tmp_yaw - yaw
self.robot_pose_x = x
self.robot_pose_y = y
self.robot_pose_yaw = self.mod_yaw(yaw_)
最後にresample_particles()内で，パーティクルのリサンプリングを行います．なお誤収束を防ぐために，有効サンプル数が一定以下になった場合にのみにリサンプリングを行うようにしています．もしリサンプリングを過度に行うと，パーティクルが真値に存在しなくなるということが起こります．そのため，有効パーティクル数の数を見て，リサンプリングを行うかどうかを決定します．
def resample_particles(self):
if self.effective_sample_size > float(self.particle_num) * self.resample_threshold:
return
tmp_particles = copy.copy(self.particles)
wo = 1.0 / float(self.particle_num)
board = []
board.append(self.particles[0][3])
for i in range(1, self.particle_num):
board.append(board[i - 1] + self.particles[i][3])
for i in range(self.particle_num):
darts = random()
for j in range(self.particle_num):
if (darts < board[j]):
self.particles[i] = copy.copy(tmp_particles[j])
self.particles[i][3] = wo
break
以上の操作を繰り返すことで，MCLが実装できます．
EKF
カルマンフィルタは，正規分布で表現される確率分布により時系列の状態推定を行う手法です．しかしカルマンフィルタは，線形モデルにしか適用できません．拡張カルマンフィルタ（EKF）は，非線形のモデルにも適用できるようにしたものです．
ekf.pyがEKFのクラスを格納し，main.pyが同様に実行のためのプログラムとなっています．
MCL内のmain.pyと同様に，メインループではロボットの内界・外界センサの値をシミュレートし，その後にEKFを用いた位置推定を実行しています．
while True:
# simulate robot behaviors
robot_sim.update_pose(0.2, -0.2)
delta_dist = robot_sim.sim_v * robot_sim.sim_time_step
delta_yaw = robot_sim.sim_w * robot_sim.sim_time_step
measurements = robot_sim.get_sensor_measurements()
# ekf
ekf.predict(delta_dist, delta_yaw)
ekf.update(measurements)
ekf.print_estimated_pose()
ekf.print_estimated_covariance()
ekf.plot_ekf_world(measurements)
# sleep
time.sleep(robot_sim.sim_time_step)
predict()は，移動量を受け取り，姿勢（self.robot_pose_*）とその共分散行列（self.pose_cov）の予測を行います．これらによって表現される正規分布が「予測分布」と呼ばれます．
def predict(self, delta_dist, delta_yaw):
# update the robot pose
x = self.robot_pose_x + delta_dist * math.cos(self.robot_pose_yaw)
y = self.robot_pose_y + delta_dist * math.sin(self.robot_pose_yaw)
yaw = self.robot_pose_yaw + delta_yaw
self.robot_pose_x = x
self.robot_pose_y = y
self.robot_pose_yaw = self.mod_yaw(yaw)
# update the pose covariance
G = np.array([[1.0, 0.0, -delta_dist * math.sin(self.robot_pose_yaw)],
[0.0, 1.0, delta_dist * math.cos(self.robot_pose_yaw)],
[0.0, 0.0, 1.0]])
V = np.array([[math.cos(self.robot_pose_yaw), 0.0],
[math.sin(self.robot_pose_yaw), 0.0],
[0.0, 1.0]])
delta_dist2 = delta_dist * delta_dist
delta_yaw2 = delta_yaw * delta_yaw
M = np.array([[self.odom_noise1 * delta_dist2 + self.odom_noise2 * delta_yaw2, 0.0],
[0.0, self.odom_noise3 * delta_dist2 + self.odom_noise4 * delta_yaw2]])
pose_cov = np.dot(G, np.dot(self.pose_cov, G.transpose())) + np.dot(V, np.dot(M, V.transpose()))
self.pose_cov = copy.copy(pose_cov)
update()は，センサ観測を受け取り，姿勢とその共分散行列の更新を行います．MCLにおいて誤収束を防ぐために有効サンプル数を用いたのに比べ，今回は共分散行列のトレースの値をチェックしています．更新を連続して繰り返し，トレースが小さくなりすぎると，姿勢に関する不確かさが減少しすぎ，更新による修正が反映されなくなります．そのため，最小のトレースを設け，それ以下の値にならないようにしています．
今回はまず各センサ観測に対して，推定位置を基準として最も近いランドマークを選択します．その距離が一定以下の場合に，EKFによる更新を行うこととしています．
def update(self, measurements):
trace = np.trace(self.pose_cov)
if trace < self.min_trace:
return
for i in range(len(measurements)):
if measurements[i][0] < 0.0:
continue
myaw = self.robot_pose_yaw + measurements[i][1]
mx = measurements[i][0] * math.cos(myaw) + self.robot_pose_x
my = measurements[i][0] * math.sin(myaw) + self.robot_pose_y
min_dl = 0.0
lidx = 0
for j in range(len(measurements)):
dx = self.landmarks[j][0] - mx
dy = self.landmarks[j][1] - my
dl = math.sqrt(dx * dx + dy * dy)
if j == 0:
min_dl = dl
elif min_dl > dl:
min_dl = dl
lidx = j
if min_dl >= 1.0:
continue
dx = self.landmarks[lidx][0] - self.robot_pose_x
dy = self.landmarks[lidx][1] - self.robot_pose_y
q = dx * dx + dy * dy
dyaw = math.atan2(dy, dx) - self.robot_pose_yaw
dyaw = self.mod_yaw(dyaw)
dz = np.array([measurements[i][0] - math.sqrt(q), measurements[i][1] - dyaw])
dz[1] = self.mod_yaw(dz[1])
H = np.array([[-dx / math.sqrt(q), -dy / math.sqrt(q), 0.0],
[dy / q, dx / q, -1.0]])
S = np.dot(H, np.dot(self.pose_cov, H.transpose())) + self.Q
det_S = np.linalg.det(S)
if det_S <= 0.0:
continue
K = np.dot(self.pose_cov, np.dot(H.transpose(), np.linalg.inv(S)))
mu = np.dot(K, dz)
x = self.robot_pose_x + mu[0]
y = self.robot_pose_y + mu[1]
yaw = self.robot_pose_yaw + mu[2]
yaw = self.mod_yaw(yaw)
pose_cov = np.dot(np.eye(3) - np.dot(K, H), self.pose_cov)
self.robot_pose_x = x
self.robot_pose_y = y
self.robot_pose_yaw = yaw
self.pose_cov = pose_cov
trace = np.trace(self.pose_cov)
if trace < self.min_trace:
return
これを繰り返すと，EKFによる位置推定が行えます．
まとめ
Pythonを用いたMCLとEKFによる位置推定のプログラムを公開し，その中身を紹介しました．これはあくまで基本的なプログラムで，実環境で適用できる様なものではありません．実環境ではもっと複雑なことが起こるので，それに対処する必要がでてきます．ですが，基礎はこのプログラムと同様なので，これらを発展していけば，実環境でも適用できるものとすることができます．