移動ロボットの確率的な自己位置推定と地図構築

1

概要
参考文献：『移動ロボットのための確率的な自己位置推定と地図構築』（友納昌弘, ロボット学会誌, vol. 29, No. 5, pp. 423-426, 2011）など– 自己位置推定と地図構築は、移動ロボットにとってもっとも基本的な機能で、表裏一体をなす••

不確実性のあるセンサデータ ⇒ 確率に基づく手法
ベイズフィルタによる自己位置推定・地図構築が主流– 記法・仕様•••

車輪型移動ロボット、センサはロボットに搭載、GPSなし
T
ロボットの位置： rt = ( x, y, θ ) は3自由度で表現
ここで、 x, y は地図上の座標値、θ はロボットの向き
地図 m はランドマーク m j の集合
T
(
m
,
m
)
ここで、 m j の位置は二次元の点 j , x
j , y で表現 2

自己位置推定（1）– 地図が与えられている場合、自分の地図上の位置をセンサデータから推定
T
r
=
(
x
,
y
,
θ
)
• 自己位置推定の状態ベクトル： tttt•

位置追跡と大域的自己位置推定（ここでは前者）– 位置追跡（position tracking）••

初期値を外部から与え、それを始点に現在の位置を推定
センサの時系列データ z1:t = z1 , z2 ,!, zt 、ロボットの動作の時系列データ u1:t = u1 , u2 ,!, ut ときのロボットの位置の確率密度 p(rt | z1:t , u1:t , m) を推定

p(rt | z1:t , u1:t , m) = α p( zt | rt , m)× ∫ p(rt | rt −1 , ut ) p(rt −1 | z1:t −1 , u1:t −1 , m)drt −1(4)3

ロボット位置の確率密度

自己位置推定（2）p(rt | z1:t , u1:t , m) = α p( zt | rt , m)•••× ∫ p(rt | rt −1 , ut ) p(rt −1 | z1:t −1 , u1:t −1 , m)drt −1

ベイズ定理とマルコフ性を仮定して変形
漸化式：時刻 t − 1 のロボット位置の確率密度関数がわかれば、時刻 t の関数を求めることができる
重要：状態ベクトル rt は直接観測されず、観測可能なデータ z1:t と動作 u1:t から推定する動作モデル p(rt | rt −1 , ut ) と計測モデル p( zt | rt , m)がそれを担う

ベイズフィルタアルゴリズム参照

4参考：ベイズフィルタ（１）– ベイズフィルタアルゴリズム（BFA）••

信念を計算する一般的なアルゴリズム
時刻 t − 1における信念 bel ( xt −1 )から、時刻 t における信念 bel ( xt ) を計算

5

参考：ベイズフィルタ（２）– 3行目：制御更新、もしくは予測（cf. 全確率の定理）– 4行目：計測更新、正規化変数 η（積分しても1にならないので）
・3, 4行目が厳密に実行可能– 単純な推定問題にのみ適用可能 ・3行目の積分が有限回の加算になる状態空間
状態

信念

制御
計測

予測
時刻 t-1 の信念

計測更新

6

動作モデル7

動作モデル（概要）
•
•
•

T
u
=
(
v
,
ω
)
ロボットの速度 vt 、角速度 ωt とすると ttt
実際には誤差を含むため、uˆt = (vˆt , ωˆ t )T （ut = uˆt + ε t ）ロボットの位置 rt は以下のように計算できる⎛ xt −1 ⎞ ⎛ vt cosθ t −1 ⎞⎜⎟ ⎜⎟微小変位を累積
(5)
g
(
r
,
u
)
=
y
+
v
sin
θ
Δ
t
⎜
⎟
⎜
⎟
t
t
t
−
1
t
t
−
1
して位置推定⎜θ ⎟ ⎜ ω⎟t⎝ t −1 ⎠ ⎝⎠• 誤差が累積する可能性 ⇒ 計測モデルによる位置修
正が必要

ut

rt −1

rt
誤差

8

動作モデルと計測モデル（概要）– 動作モデル：状態遷移確率 p( xt | ut , xt −1 )– 計測モデル：計測確率 p( zt | xt )– 従来のロボット運動学 ⇒ 決定論的– 「確率ロボティクス」 ⇒ モデルが確率的– [重要！]
厳密な形状の確率モデルはそれほど重要ではない⇒ 不確かな動作結果への対策が重要
9事前準備– コンフィギュレーション：6変数で記述•

デカルト座標（三次元）、オイラー角（三次元；ロール、
ピッチ、ヨー）– 二次平面を動くロボットの姿勢（平面上に限定）：
x, y ：座標、θ ：方向のとき、ロボットの姿勢は以下
⎛x⎞
⎜ ⎟
⎜ y⎟
⎜θ ⎟
⎝ ⎠
10

確率的運動学– 条件付き確率密度関数– ロボットの運動学モデル：
xt −1• 最初の姿勢•分布（グレースケール）p( xt | ut , xt −1 )p( xt | ut , xt −1 )
濃いほど
尤度が高い

動作コマンド– 2種類の確率的動作モデル• 速度動作モデル： ut はモータに与えられる速度指令• オドメトリ動作モデル：ut は移動距離と回転速度（正確？）11速度動作モデル（１）– 解析的な計算とアルゴリズム• 制御 ut ：並進速度 vt （前進方向が正）、回転速度ωt（反時計回りが正）•⎛ vt ⎞
ut = ⎜⎜ ⎟⎟⎝ ωt ⎠

解析的な計算：– 入力：初期姿勢 xt −1 = ( x y θ )T 、制御 ut = (v ω )T
次の姿勢の仮説 xt = ( xʹ yʹ θ ʹ)T– 出力： p( xt | ut , xt −1 )– パラメータ： α1 ,…, α 6⌢ ⌢– 誤差がないと仮定した値： v 、ω– 関数 prob( x, b) ：原点を中心とする標準偏差 b12の分布に従う確率変数 x を計算

入力

速度動作モデル（２）

誤
差
が
な
い
も
の
と
し
て
計
算

出力

円弧を描いて移動
その中心と半径方向の変化量

並進速度
回転速度

制御は固定時間Δt で実行

回転方向の誤差のパラメータ

並進方向の誤差パラメータ13速度動作モデル（３）– 動作の誤差をモデル化 prob( x, b)••正規分布について確率密度を計算するアルゴリズム
三角分布についての確率密度を計算するアルゴリズム

確率変数aの確率密度を計算

標準偏差b（分散b2）の分布に従う14

速度動作モデル（４）– 異なる雑音パラメータの設定で得られる速度動作モデル（3例とも同じ並進速度・回転速度）(a) 誤差パラメータ α1 ,…, α 6 を中程度に設定(b) 回転方向の誤差パラメータ（α 3 , α 4 ）を小さく、並進方向の誤差パラメータ（ α1 , α 2）を大きく設定(c) 逆に、回転方向の誤差を大きく、並進方向の誤差を
小さく設定

15

オドメトリ動作モデル（１）
– 特徴：
•
•

オドメトリ：車輪のエンコーダの情報を積分して取得
オドメトリ動作モデルの特徴（速度動作モデルとの比較）：–––

オドメトリの方が情報はより正確
オドメトリは動作後でなければわからない
オドメトリはセンサ情報（通常は制御信号とみなす）
回転
並進

回転

16

オドメトリ動作モデル（２）– 解析的な計算：• 動作情報 ut は以下、ここで、バーは内部座標系で表現⎛ xt −1 ⎞⎟⎟グローバル座標系との対応は不明
ut = ⎜⎜⎝ xt ⎠
•

入力：

–
–
–

•

初期姿勢 xt −1
T
オドメトリから得られる姿勢の対 ut = ( xt −1 xt )終端姿勢の仮説 xt

出力：–p( xt | ut , xt −1 ) の数値
17

オドメトリ動作モデル（３）オドメトリから相対的な動作
パラメータ計算

回転
移動
回転

姿勢xt-1 とxt から相対的な動作
パラメータ計算
二通りのパラメータの違いに関する発生確率計算各確率密度の積で結合確率密度が出力

各誤差要因が互いに18独立であること

オドメトリ動作モデル（４）– 誤差パラメータによる変化：(a) 典型例(b) 並進の誤差大(c) 回転の誤差大– 「速度動作モデル（３）」の図と比較•

計測時間の間隔が短い（信念が頻繁に更新される）ほど両者の差は小さくなる

19

動作と地図（１）– 地図 m が考慮された動作モデル– m が姿勢推定に意味のある情報を含む場合
p( xt | ut , xt −1 ) ≠ p( xt | ut , xt −1 , m)
– マップベースト動作モデル（上式の右辺）はより正確な推定結果をもたらす– しかし、解析的な計算は困難（経路中に障害物がなくロボットが通れることを計算する必要あり）– 近似する有効な方法：2つの成分に分解
p( xt | ut , xt −1 ) p( xt | m)p( xt | ut , xt −1 , m) = ηp( xt )

地図と矛盾のない姿勢xtのみに確率が割り当てられた確率分布を得る

20

動作と地図（２）–p( xt | ut , xt −1 , m) を計算するアルゴリズム• 障害物に占有された地図 m 中の空間にはロボットが存在できない処理を付加

地図が占有されているか？
21

動作と地図（３）– 速度動作と地図：•(a)地図を考慮しない場合、(b) 地図 m を考慮する場合– 青い枠の領域の尤度はゼロではないp( xt | m)と p( xt | ut , xt −1 ) のどちらもゼロではないから– ロボットは壁をすり抜けるのか？
•

ア

・あ

22

計測モデル

23

重要
•
•

計測モデルを厳密に書くには、ランドマークと計測データの対応づけ関数 ct として、 p( zt | rt , ct , m) となる計測モデルはセンサの能力に大きく依存する–•計測モデル（概要）

レーザスキャナの場合、対象物までの距離と方向が得られる

T
m
(
m
,
m
)
いまランドマーク j の位置 j , x j , y とすると、ロボットの位置 rt で得る計測データは以下となる
2

2

ランドマークからの距離⎛ (m j , x − xt ) + (m j , y − yt ) ⎞⎜⎟⎛ m j , y − yt ⎞⎟h(rt , m j ) = ⎜
(6)
−1 ⎜
⎟
− θt ⎟⎜⎜ tan ⎜⎟⎟ランドマーク
m
−
x
j
,
x
t
⎝
⎠
⎝
⎠
との角度
• 制約関係 zt = h(rt , m j ) は成り立つがランドマーク1個から
rt を一意に求めることができない ⇒ 動作モデルで補う

rt

zt

mj

誤差

24

計測モデル（１）– 計測モデル：センサ計測値の生成過程を記述– 確率ロボティクス：センサ計測値の雑音を明示的にモデルに組み込む– 計測モデル p( zt | xt , m)
• ここでは、 zt は距離センサ（ソナーレンジスキャン）
計測誤り

反射波が
返ってこない25

計測モデル（２）：確率モデルの特徴– 典型的なレーザレンジスキャン••

照射範囲が音波よりずっと絞られている（狭い）センサの応答特性 ⇒ 扱いたくない変数を含む– 確率ロボティクス：決定論的な関数 zt = f ( xt ) のp( zt | xt ) でモデル化
代わりに条件付き確率密度•

センサモデルの不確かさ ⇒ モデルの非決定論的な性
質で吸収できる
レーザレンジスキャンの典型例（炭坑内）

26

計測モデル（３）：定式化– 計測 zt ：計測値の数 K{

zt = zt1 , … , ztK}– 確率 p( zt | xt , m)は、個々の計測値に対する尤
度の積
Kp( zt | xt , m) = ∏ p( ztk | xt , m)k =1•

上の式が成り立つのは独立仮定（個々の計測ビー
ムに生じる雑音が独立）の場合

27

計測モデル（４）：地図
地図 m = {m1 , m2 ,…, mN } （ N は物体の個数）–– 地図の構築方法：•

特徴ベース：–––•

インデックスは特徴と対応づけられる
物体位置を調節することが容易
センサ情報から地図を作成するロボット ⇒ 特徴ベース採用

位置ベース：––

インデックスは特定の位置と対応づけられる
ボリューメトリック（容積測定的） ⇒ 物体のない場所も記述

28

カルマンフィルタによる位置追跡29

カルマンフィルタとは？
参考文献：“An Introduction to the Kalman Filter”, G. Welch and G. Bishop）– 離散的な誤差のある観測から、時々刻々と時間変化する量（位置、速度など）を推定する••

応用：レーダー、コンピュータビジョンなど
特徴：目標物の時間変化を支配する法則を活用して、目標物の位置を推定（現在：t = t0 、未来：t < t0 、過去： t > t0–––

現在（フィルタ）：現在の情報に基づき、現在の状態を推定
未来（予測）：現在までの情報に基づき、未来の状態を推定
過去（内挿、平滑化）：現在までの情報に基づき、過去の状態を推定

現在の状態は必ずしも直接観測できない

推定
時刻アップデート（時間更新、予測）

補正
観測（測定）値のアップ
デート（観測更新）
30

カルマンフィルタとは？
– 離散的な誤差のある観測から、時々刻々と時間変化する量（位置、速度など）を推定する••

応用：レーダー、コンピュータビジョンなど
特徴：目標物の時間変化を支配する法則を活用して、目標物の位置を現在（フィルタ）、未来（予測）、過去（内挿あるいは平滑化）において推定可能

推定
時刻アップデート（時間更新、予測）

補正
観測（測定）値のアップ
デート（観測更新）

31

カルマンフィルタ（1）– カルマンフィルタの特徴：••

連続状態に対して信念に関する計算が実装される離散系やハイブリッドな状態空間には適用できない– カルマンフィルタにおいて、事後信念がガウス分布となる条件：
いま、時刻 t において信念は平均 µ と共分散 Σ で表現
1. 状態遷移確率 p( xt | ut , xt −1 ) がガウス雑音を足した線形関数である必要がある．ここで、At と Bt は行列ガウス分布に従う
xt = At xt −1 + Bt ut + ε t－(1)
乱数ベクトル
2. 計測確率 p( zt | xt ) も線形であり、雑音はガウス雑音
線形ガウス型

zt = Ct xt + δ t

－(2)
32

カルマンフィルタ（2）

平均

共分散

3. 初期信念 bel ( x0 ) は正規分布でなければならない．
1
−
−1
⎧ 1
⎫
T
2
bel ( x0 ) = p( x0 ) = det(2πΣ0 ) exp⎨− ( x0 − µ0 ) Σ0 ( x 0 − µ0 )⎬⎩ 2⎭
多変量正規分布－(3)33

カルマンフィルタ（3）– カルマンフィルタのアルゴリズム•

時刻 t の信念 bel( xt ) を平均µt と共分散 Σ t で表現

カルマン
ゲイン

t − 1の信念

信念

制御

計測

予測ステップ（不確かさの増大）

bel( xt )zt が反映される前の信念を計算（ut を反映）

信念

bel( xt )計測更新ステップ（不確かさの減少）

zt が反映される

t の信念

34

カルマンフィルタ（4）：詳細な処理
•
•
•
•
•

2行目：平均値 µ は式(1)の状態 xt −1をµt −1で置き換えて、決定論的な部分のみ計算
3行目：共分散 Σ は、事後状態がAt の作用で事前状
態から遷移することを反映して更新．Rt は乱数ベクト
ルの共分散
4行目：カルマンゲインK t は、計測を新たな状態推定にどの程度反映させるかを決定
5行目：平均値の修正（カルマンゲイン K t、実際の計
測値 zt と計算確率の式(2)によって予測された計測
値の差
6行目：事後信念の共分散を計算

35

カルマンフィルタ（5）：構成要素
At

Bt
Ct

εt
δt

tへ
n×nの正方行列．制御やノイズなしに、 t −から
1
状態 xt がいかに発展するかを記述
t − 1から において状態xをいtn×m行列．制御 uがttかに変化させるかを記述
k×n行列．状態 xtを計測 zt にどのように位置づける
かを記述
乱数ベクトル．共分散 Rt , Qtからは独立で、正規分布
すると仮定される、処理と計測の誤差を表現
n：状態ベクトル xt の次元、m：制御ベクトル ut の次元、
k：計測ベクトル zt の次元36

カルマンフィルタ（5）•一次元での位置推定の例（ロボットは横軸を移動）

37計測値を取得

初期信念

ロボットが右に動いた⇒不確かさ増大KF 4-6行の計算
初期信念＋計測値
ガウス分布の中心は(a)と(b)の分布の中心で、不確かさが減少
2回目の計測値を取得

KF 2-3行の計算
不確かさ増大KF 4-6行目の計算
1つ前の信念＋2回目の計測値

カルマンフィルタによる情報統合の性質38カルマンフィルタの予測ステップ

Prediction⎧ µt = at µt −1 + bt utbel ( xt ) = ⎨ 2222⎩σ t = at σ t + σ act ,t⎧ µ = At µt −1 + Bt utbel ( xt ) = ⎨ t
T⎩Σ t = At Σ t −1 At + Rt

39

カルマンフィルタの計測更新ステップ⎧ µ t = µ t + K t ( zt − µ t )σ t2bel ( xt ) = ⎨, Kt = 2
2
2
2
σ
=
(
1
−
K
)
σ
σ t + σ obs
t
t
t
⎩
,t
⎧µ = µt + K t ( zt − Ct µt )bel ( xt ) = ⎨ t, Kt = Σt CtT (Ct Σt CtT + Qt ) −1
⎩ Σt = ( I − Kt Ct )Σt

Correction40

カルマンフィルタ（7）：予測・計測更新サイクル
Prediction⎧ µ t = µ t + K t ( zt − µ t )σ t2bel( xt ) = ⎨ 2, Kt = 2 2
2
σ
=
(
1
−
K
)
σ
σ t + σ obs,t
t t⎩ t⎧ µt = at µt −1 + bt utbel ( xt ) = ⎨ 2
2 2
2
σ
=
a
σ
+
σ
t
t
act ,t⎩ t⎧µ = µ + K ( z − C µ )bel( xt ) = ⎨ t t t t t t , Kt = Σt CtT (Ct Σt CtT + Qt ) −1
⎩ Σt = ( I − Kt Ct )Σt⎧ µt = At µt −1 + Bt utbel ( xt ) = ⎨T⎩Σt = At Σt −1 At + Rt

Correction41

カルマンフィルタの動作例•

カルマンフィルタを用いた慣性計測ユニット（Inertial
Measurement Unit ）のデモ（比較：IMUの加速度計
からのデータを融合しただけの場合）

42

カルマンフィルタ（6）：まとめ••

カルマンフィルタは、センサデータを信念に反映する計
測更新ステップ（5～7行目）と、行動によって信念を変形させる予測ステップ（あるいは制御更新ステップ）を交互に実行する
計測更新ステップはロボットの信念の不確かさを減少させ、予測ステップは増加させる– 拡張カルマンフィルタとは？
•••カルマンフィルタは、計測が状態の線形関数で、状態が直前の状態の線形関数であることが不可欠
しかし、状態遷移や計測が線形であることは、現実にはほとんどない
拡張カルマンフィルタ（extended kalman filter, EKF）
43
は線形性の仮定を緩和する

拡張カルマンフィルタ

44拡張カルマンフィルタによる位置追跡（1）– 動作モデルと計測モデルをベイズフィルタで融合することにより、上記のそれぞれの短所を補完できる•

ロボットの位置、動作モデル、計測モデルをそれぞれ一つの正規分布で表す一変数スカラ関数の接線の傾きを、多変数– 動作モデル•••

ベクトル値関数に対して拡張、高次元化

関数 g は非線形なので、テーラー展開で線形化
Gt は rt に関する g のヤコビ行例、Vt は ut に関する gのヤコビ行例ût を動作指令値（オドメトリ値）とし、ut は正規分布
N (aˆt , M t ) に従う
変化の比率を符号付きで表現

rt = g (rt −1 , ut )≈ g (rt −1 , uˆt ) + Gt (rt −1 − rt −1 ) + Vt (ut − uˆt )(6)45

拡張カルマンフィルタによる位置追跡（2）– 計測モデル•••h も非線形関数なので、テーラー展開で線形化
H t は rt に関する h のヤコビ行例
β t は誤差で正規分布 N (0, Qt ) に従う

zt = h(rt , m j ) + β t≈ h(rt , m j ) + H t (rt − rt −1 ) + β t
変化の比率を符号付きで表現

(7)

誤差

46

拡張カルマンフィルタによる位置追跡（3）ロボットの位置の分布を N (rt , Σt ) とすると、拡張カル
マンフィルタにより次のように計算できる(1) 動作モデルによる rt の予測
K t はカルマンゲイン
動作モデルと計測モデルの~
r = g (r , uˆ )•精度に応じた重みをつけて(8)~
T
T 最適値を得る
Σt = Gt Σt −1Gt + Vt M tVt また、実測値をセンサ座標系
から地図座標系に変換する
t

t −1

t

(2) 計測モデルによる rt の更新~ T~ T
動作モデルの値
Kt = Σt H t ( H t Σt H t + Qt ) −1
r =~r + K ( z − h( ~r , m ))
t

t

t

t

~
Σt = ( I − Kt H t )Σt

t

j

実測値
(9)
計測モデルの値
47拡張カルマンフィルタによる位置追跡（4）••カルマンフィルタは、動作モデル rt と、実測値 zt と計測モデルの値 h( ~rt , m j ) の差とを K t で重みづけした線形和
カルマンゲイン K t は、動作モデルと計測モデルの精度に応じた重みをつけて最適値を得る働きをする

カルマンフィルタにより、複数のデータを融合して最適解を得ることができる

48

拡張カルマンフィルタ（1）：線形変換と非線形変換
線形変換

非線形変換

49

線形変換

確率変数 YN ( y; aµ + b, a 2σ 2 )カルマンフィルタにおける状態の更新に対応（式(1)で X = xt −1 , Y = xt とし雑音がない場合）

確率変数 XN ( x; µ , σ 2 )50

非線形変換

非線形関数 g
確率密度関数Y = g (X )この確率密度関数は数式では求められない

p(x) のサンプルを選び、関数 g で変換し、ヒストグラムを描いて確率密度関数を推定
確率変数 X
51

拡張カルマンフィルタの位置推定アルゴリズム（参考）

52

53

拡張カルマンフィルタ（2）：特徴– 拡張カルマンフィルタは、線形性の仮定を緩和
する•

状態遷移確率と計測確率が非線形関数 g, hに従うと仮定し、以下のようにモデル化する

xt = g (ut , xt −1 ) + ε tzt = h( xt ) + δ t
•
•
•

－(4)

xt = At xt −1 + Bt ut + ε t－(5)zt = Ct xt + δ t

上記の式は、式（1）（2）を一般化したもの
しかし、 g, h は任意であるため、信念はガウス分布に従わない
非線形関数 g, h に対して信念を正確に更新することは不可能 ⇒ ベイズフィルタは数式解を持たなくなる
54

拡張カルマンフィルタ（3）：近似– EKFでは推定対象の状態に対してガウス分布の近似が計算される•••

EKFも時刻 t の信念 bel( xt ) を平均 µt 、共分散 Σ t で表す ⇒ カルマンフィルタの基本的な方法を受け継ぐ
しかし、EKFの信念は単なる近似
複雑な分布形状の事後信念を厳密に計算しようとせず、平均と共分散を計算効率良く推定すること55

拡張カルマンフィルタ（4）：線形化
線形化によって得られる
ガウス分布

g を線形近似
した直線

誤差

もとのガウス分布の中心における
g の接線

高精度のモンテカルロ推定から得られたガウス分布

g
確率変数 X
56

拡張カルマンフィルタ（5）：効率・手法– 線形近似の効率の良さ••

モンテカルロ推定：サンプルの変換を50万回繰り返し、平均と分散を計算
EKF：線形関数を選び、その関数からガウス分布の式を求めるだけ（ g が線形化されるとKFと同じ）計測関数 h に関するガウス分布の乗算にも適用可能– 線形化する手法••

テイラー展開：関数 g の値と傾きから、g の線形近似を∂g (ut , xt −1 )行うg ʹ(ut , xt −1 ) :=∂xt −1
最尤な状態を選ぶ ⇒ ガウス分布では事後信念の平均値 µt −1、計測関数 h では µt −157拡張カルマンフィルタ（6）：アルゴリズム– KFとの違い（KF ⇒ EKF） 線形予測 ⇒ 非線形予測••

2行目（予測）： At µt −1 + Bt ut ⇒ g (ut , µt −1 )
5行目（計測）： Ct µt ⇒ h( µt )
KFの

At

KFの

Ct

58

拡張カルマンフィルタ（7）：注意点– 不確かさの程度の違いで変わる近似性能同じ関数近似推定誤差大より不確かな
ガウス分布

推定誤差小

より確かな 59
ガウス分布

拡張カルマンフィルタ（8）：注意点– 関数 g の局所的非線形性に左右される近似性能
線形な部分
で近似
非線形な
部分で近似
推定誤差小
推定誤差大

同じ分布

60

拡張カルマンフィルタ位置推定

61

EKFによる位置推定（1）– 動作モデルと計測モデルをベイズフィルタで融合することにより、上記のそれぞれの短所を補完できる•

ロボットの位置、動作モデル、計測モデルをそれぞれ正規分布で表す
一変数スカラ関数の接線の傾きを、多変数– 動作モデル•••

ベクトル値関数に対して拡張、高次元化

関数 g は非線形なので、テーラー展開で線形化
Gt は xt に関する g のヤコビ行例、Vt は ut に関する gのヤコビ行例ût を動作指令値（オドメトリ値）とし、ut は正規分布⌢
N (ut , M t ) に従う
変化の比率を符号付きで表現

xt = g (ut , xt −1 )⌢≈ g (ut , xt −1 , ) + Gt ( xt −1 − xt −1 ) + Vt (ut − uˆt )(6)62

EKFによる位置推定（2）– 計測モデル•••h も非線形関数なので、テーラー展開で線形化
H t は xt に関する h のヤコビ行例
β t は誤差で正規分布 N (0, Qt ) に従う

zt = h( xt , m j ) + β t≈ h( xt , m j ) + H t ( xt − xt −1 ) + β t
変化の比率を符号付きで表現

(7)

誤差

63

EKFによる位置推定（3）ロボットの位置の分布を N ( xt , Σt ) とすると、拡張カル
マンフィルタにより次のように計算できる(1) 動作モデルによる xt の予測
K t はカルマンゲイン
動作モデルと計測モデルの~
x = g ( x , uˆ )•精度に応じた重みをつけて(8)~
T
T 最適値を得る
Σt = Gt Σt −1Gt + Vt M tVt また、実測値をセンサ座標系
から地図座標系に変換する
t

t −1

t

(2) 計測モデルによる xt の更新~ T~ T
動作モデルの値
Kt = Σt H t ( H t Σt H t + Qt ) −1x =~x + K ( z − h( ~x , m ))
t

t

t

t

~
Σt = ( I − Kt H t )Σt

t

j

実測値
(9)
計測モデルの値
64

EKFによる位置推定（4）••カルマンフィルタは、動作モデル xt と、実測値 zt と計測モデルの値 h( ~xt , m j ) の差とを K t で重みづけした線形和
カルマンゲイン K t は、動作モデルと計測モデルの精度に応じた重みをつけて最適値を得る働きをする

カルマンフィルタにより、複数のデータを融合して最適解を得ることができる

65

EKFによる位置推定（5）：アルゴリズム

予
測
ス
テ
ッ
プ

66

計
測
更
新
ス
テ
ッ
プ

計測予測

推定更新

67

EKFによる位置推定（6）：詳細
1行目：ロボットの姿勢推定のガウス分布（ µt −1 , Σt −1）、制御 ut 、地図 m 、計測 zt と対応変数 ct
22行目：更新された推定 µt , Σt 、特徴観測の尤度 p z---------------------------------------------------------------3～4行目：動作モデルを線形化（ヤコビ行列計算）
5行目：制御から、動作に生じる雑音の共分散特定
6～7行目：動作更新（6行目：予測される姿勢 µt、7行目：それに対応する誤差楕円）
9～18行目：時刻 t に観測されたすべての特徴 iに対するループt

68

EKFによる位置推定（7）：詳細
10行目：計測ベクトル中における i 番目の特徴の対応変数 j に代入iẑ
12行目：予測される計測 t を計算
iH
13行目：計測モデルのヤコビ行列 t を計算iiẑS
14行目：予測される計測 t の不確かさ t を計算
16～17行目：推定が一つの特徴に対して一回ずつ更新
19～20行目：新たな推定姿勢の計算
21行目：計測の尤度を計算
69

地図構築

70

地図構築（1）– 地図構築は自己位置推定と分離してできず、
SLAM（Simultaneous Localization and
Mapping）という枠組みが提案された•••計測データはロボットからの相対値で取得 ⇒ 座標変
換し、地図座標系でのランドマーク位置を求め、地図を構築
しかし、ランドマークを載せた地図はこれから作られるゆえに、ロボット位置と地図は同時に推定しなければならない ⇒ SLAMを適用

71

地図構築（2）– SLAM•

ポイント：同じランドマークを複数回観察すること–•

これまで観測したランドマークを使ってロボットの位置推定⇒ そのロボットの位置に従って新たなランドマークを配置⇒ 地図を成長させる

SLAMにもベイズフィルタが使われる–

ロボット位置と地図の同時確率密度の推定問題 ⇒ 状態ベ
クトルの次元は非常に大きくなる

72カルマンフィルタを用いたSLAM
•

位置と地図
の同時推定

ロボット位置 rt と地図 m の同時確率密度を正規分布で表し、自己位置推定と同様に漸化式を構成する

p(rt , m | z1:t , u1:t ) = α p( zt | rt , m)× ∫ p(rt | rt −1 , ut ) p(rt −1 , m | z1:t −1 , u1:t −1 )drt −1•

カルマンフィルタアルゴリズム参照
いま ut = (rt , m1 ,…, mn ) とすると、求める同時確率はp(at | z1:t , u1:t ) となり、自己位置推定と同様に拡張カ
ルマンフィルタで実装できる–動作モデル は rt に対して計算するが、計測モデルは ut に対して計算する（次元が大きいので、計算量は増える）

73

フィルタアプローチの特徴– 自己位置推定のように状態ベクトルの次元が小さい場合 ⇒ ベイズフィルタは実用性が高く有効– 地図構築は状態ベクトルの次元が膨大（数千～数百万） ⇒ ベイズフィルタの適用は困難が伴う– 最適化アプローチによる地図構築では、動作モ
デルと計測モデルから評価関数を構成し、その最適解を求めることにより、ロボット位置とランド
マーク位置を同時に求める ⇒ 地図構築に有効

74

まとめ
– 移動ロボットの確率的な自己位置推定と地図構
築の方法
•
•

ベイズフィルタによるアプローチが有効
利点– データ融合を統一的に扱えること– ロボット位置などの不確実性を表せること

75

おわり

76